
%
% samasc.m
%
% code for `reproducing' figure 3 in Samsonovich & Ascoli, 2005,
% Learnin and Memory, 12:193-208
%
% this is an altered copy of newrob1.m from
%
%  http://krasnow.gmu.edu/L-Neuron/ascoli/sa_lm05_som/newrob1.m
%
% I was unable to find maze.mat, getany.m and randn2.m and so
% rolled my own
%
% I found inmaze1.m, bump.m, and moveto.m in the directory containing 
% newrob1.m
% They appear below as subfunctions
%

clear all
close all

%nwalks=0;
nwalks=10;
ntrial=50;
%load maze
maze = round(2*rand(5));
[n,m]=size(maze);
y=zeros(n,m);
y = zeros(1,2);
J=zeros(2*n,2*m);
tstamp=zeros(n,m);
weight=zeros(n,m);
t=0;

%--------------drawing the maze

hfig=figure('position',[0    50   900   900]);
hold on;
ha=get(hfig,'currentaxes');
set(ha,'color',[0.8 0.8 0.8]);
myaxis=[0 n 0 n];
axis('manual');
axis(myaxis);
shading flat;
grid;
for i=1:n,
for j=1:m,
    if maze(i,j),
       rr=[i-1 j-1 1 1];
       rectangle('position',rr,'facecolor','w','linestyle',':','edgecolor','k');
    end;
end;
end;

%--------------setting the starting point / re-initializing

radius=n.*0.01;
[xy,flag]=getany;
if flag,
    break;
elseif not(all(xy<=[n m]) & all(xy>[0 0]) & inmaze1(xy, maze, radius)),
	load handel
	sound(y,Fs);
    fprintf('one more chance\n');
    [xy,flag]=getany;
end;
if flag | not(all(xy<=[n m]) & all(xy>[0 0]) & inmaze1(xy, maze, radius)),
    close;
    break;
end;
h=rectangle('position',[xy-radius,[2 2]*radius],'curvature',1,'facecolor','r','edgecolor','w');
loc=get(h,'position'); %--current location
x=loc(1:2);
goal=[n,m];
nmov=100;

%---------------random walk

itmax=10000;
itm=1000;
itm0=10;
nmov=3;
hl=[];
alp=1-0.125;
sig=0.5;
v=[0 0];
%if exist('robdata.mat')~=2,

for walk=1:nwalks,
    total=0;
	for it=1:itmax,
        try
            for mm=1:40,
                ksi=randn2(sig);
                y=x+alp*v+ksi;
                if inmaze1(y,maze,radius),
                    error('ok');
                end;
                if mm==20,
                    v=[0 0];
                end;
            end;
            fprintf('couldn''t find a ksi\n');
            break;
        catch

            %-----------updating the kinematic state of the body

                ksi=randn2(sig);

            b=bump(x,y,maze,radius);
            if any(b),
                y=b;
                v=[0 0];
            else
                v=alp*v+ksi;
            end;
            [len,hl1]=moveto(y,h,radius);
            hl=[hl;hl1];
            total=total+len;

            %---------------learning kernel begin

            ix=ceil(x);
            iy=ceil(y);
            k=0;
            t=t+1;
            tstamp(ix(1),ix(2))=t;
            while any(ceil(y)~=ix),
                k=k+1;
                dij=sign(ceil(y)-ix);
                iy=ix+dij;
                t=t+1;
                tstamp(iy(1),iy(2))=t;
                if dij(1)==dij(2),
                    ij=2*min(ix,iy);
                else
                    ij=ix+iy;
                end;
                J(ij(1),ij(2))=1;
                ix=iy;
            end;

            %---------------learning kernel end

            x=y;

            if iy==goal,
                fprintf('goal reached\n');
                break;
            end;

        end;  % try
	end;  % for it

    fprintf('total trajectory length = %.2f\n', total);

end;   % for walks

if nwalks,
    save robdata J tstamp
else
    load robdata
end;

%---------------------computing weights

maxstamp=max(max(tstamp));
%weight=-log(maxstamp+1-tstamp);
%weight=maxstamp-tstamp;

for i=1:n,
for j=1:m,
        weight(i,j)=max(weight(i,j), 1./(maxstamp+1-tstamp(i,j)));
end;
end;

%---------------------drawing model data

figure(2);
subplot(2,2,1);
pcolor(tstamp');
shading flat
subplot(2,2,3);
pcolor(J');
shading flat
figure(1);

%delete(hl);
hl=[];
% alp=0;
% sig=1;
itmax=1000;
for trial=1:ntrial,
	%--------------setting new starting point
	[xy,flag]=getany;
	if flag,
        break;
	elseif not(all(xy<=[n m]) & all(xy>[0 0]) & inmaze1(xy, maze, radius)),
		load handel
		sound(y,Fs);
        fprintf('one more chance\n');
        [xy,flag]=getany;
	end;
	if flag | not(all(xy<=[n m]) & all(xy>[0 0]) & inmaze1(xy, maze, radius)),
        close;
        break;
	end;
    if exist('hl'),
        delete(hl);
        hl=[];
    end;
	loc(1:2)=xy-[1 1].*radius; %--new location
	x=loc(1:2);
	set(h,'position',loc);
    total=0;
	%---------------goal search
	v=[0 0];
	for it=1:itmax,
        %--------------search for a best move
        gain=[];
        ksi=[];
        for k=1:10,
            for mm=1:40,
                ksi(k,:)=randn2(sig);
                y=x+alp*v+ksi(k,:);
                if inmaze1(y,maze,radius),
                    break;
                end;
                if mm==20,
                    v=[0 0];
                end;
            end;
            if mm==40,
                fprintf('couldn''t find a ksi\n');
                break;
            end;
            %------------------------- kernel begin
            ix=ceil(x);
            iy=ceil(y);
            gain(k)=weight(iy(1),iy(2))-weight(ix(1),ix(2));
            %------------------------- kernel end
        end;
        [g,k]=max(gain);
        y=x+alp*v+ksi(k,:);
        %-----------updating the kinematic state of the body
        b=bump(x,y,maze,radius);
        if any(b),
            y=b;
            v=[0 0];
        else
            v=alp*v+ksi(k,:);
        end;
        [len,hl1]=moveto(y,h,radius,'red');
        total=total+len;
        hl=[hl;hl1];
        %---------------learning kernel begin
        ix=ceil(x);
        iy=ceil(y);
        k=0;
        t=t+1;
        tstamp(ix(1),ix(2))=t;
        while any(ceil(y)~=ix),
            k=k+1;
            dij=sign(ceil(y)-ix);
            iy=ix+dij;
            t=t+1;
            tstamp(iy(1),iy(2))=t;
            if dij(1)==dij(2),
                ij=2*min(ix,iy);
            else
                ij=ix+iy;
            end;
            J(ij(1),ij(2))=1;
            ix=iy;
        end;
		%---------------learning kernel end    
        x=y;
        if iy==goal,
%            fprintf('goal reached\n');
            break;
        end
	end

    fprintf('total trajectory length = %.0f\n', total);

end

