%
% samasc.m
%
% code for `reproducing' figure 3 in Samsonovich & Ascoli, 2005,
% Learning and Memory, 12:193-208
%
% usage   plen = samasc(ntrials,paw)
%
%            ntrials = number of goal directed journeys
%            paw = display parameter, pause duration during walk
%
%            plen = row of pathlenghts of each trial
%
% example  plen = samasc(1,.01);
%

%
% 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. For getany I have substituted ginput
%
% I found inmaze1.m, bump.m, and moveto.m in the directory containing 
% newrob1.m
% They appear below as subfunctions
%

function plen = samasc(ntrials,paw)

close all

maze = [1 1 0 1 0
        0 1 1 1 1
        1 1 0 1 0
        0 1 0 1 1
        1 1 0 0 1]';

%maze = [1 1 0;0 1 0;0 1 1]; 	% an aggie maze

[n,m] = size(maze);

%  draw the maze

hfig = figure;
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;

% initialize the rat

radius=n.*0.01;
disp(' ')
disp(' Phase 1: Explore')
disp(' Please use the mouse to start the rat')
xy = ginput(1);
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];	% goal is upper right element

itmax = 10000;
alp = 1-0.125;
sig = 0.5;
v = [0 0];
tstamp = zeros(n,m);
weight = zeros(n,m);
t = 0;

total = 0;			% initial path length
set(h,'position',loc);		% initial position

for it=1:itmax,

    try     % try random moves until one lands in maze

        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');
        return;

    catch   % accept the move and record the time

        % update the kinematic state of the rat

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

        % record the time stamp at cell ix
        % and stamp cells between ix and iy with subsequent times

        ix = ceil(x);
        iy = ceil(y);
        t = t + 1;
        tstamp(ix(1),ix(2)) = t;   

        while any(ceil(y) ~= ix),     % stamp intermediate cells

            iy = ix + sign(ceil(y)-ix);
            t = t + 1;
            tstamp(iy(1),iy(2)) = t;
            ix = iy;

        end;

        x = y;	% start at last end

        if iy==goal,
            % fprintf('goal reached\n');
            x = xy;   % return to starting position
            loc(1:2) = x;
            break;
        end

    end  % try

end  % for it

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

% set and display the weights

maxstamp = max(max(tstamp));

weight = max(weight,1./(maxstamp+1-tstamp));

%disp(' weight =')
%disp(weight)

figure(2)
for i=1:m,
for j=1:n,
    xc = [j-1 j j j-1];
    yc = [i-1 i-1 i i];
    cc = (weight(j,i))^(.2)*maze(j,i)*[1 0 0];
    fill(xc,yc,cc);
    hold on
end
end

title('The Weight Matrix','fontsize',18)

figure(1)

%%%%%%%%%%%%% Now for the Goal %%%%%%%%%%%%%%%%%%%%%

disp(' ')
disp(' Phase 2: Reach the Goal')

itmax=1000;

plen = zeros(1,ntrials);

for trial=1:ntrials,

    disp(' Please use the mouse to start the rat')
    xy = ginput(1);

    loc(1:2)=xy;
    x=loc(1:2);
    set(h,'position',loc);

    v=[0 0];

    for it=1:itmax,

        gain=zeros(10,1);
        ksi=zeros(10,2);

        for k=1:10,     % move in 10 random directions

            for mm=1:40,   % iterate till we stay in maze

                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('could not find a ksi\n');
               return
            end

            %  kernel begin

            ix = ceil(x);
            iy = ceil(y);

            gain(k) = weight(iy(1),iy(2)) - weight(ix(1),ix(2));

            %  kernel end

        end;  % for k

        [g,kbest] = max(gain);         	% find best direction

        y = x + alp*v + ksi(kbest,:);    % move in direction of best gain

        % update the kinematic state of the rat

        b = bump(x,y,maze,radius);
        if any(b),
            y = b;
            v = [0 0];
        else
            v = alp*v + ksi(k,:);
        end;
        len = moveto(y,h,radius,'red');
        pause(paw)
        plen(trial) = plen(trial) + len;

        x=y;
        iy = ceil(y);
        if iy==goal,
            % fprintf('goal reached\n');
            break;
        end

    end   % for it

    fprintf('   path length = %.2f\n', plen(trial));

end   % for trials

disp(' ')

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  subfunctions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function result = inmaze1(xy,maze,r);
r1=0.8*r;
xy=xy+r1;
[n,m]=size(maze);
if all(xy>r) & all(xy<size(maze)-r),
	result = maze(ceil(xy(1)+r1),ceil(xy(2)+r1))...
	*maze(ceil(xy(1)-r1),ceil(xy(2)+r1))...
	*maze(ceil(xy(1)+r1),ceil(xy(2)-r1))...
	*maze(ceil(xy(1)-r1),ceil(xy(2)-r1));
else
    result=0;
end;

%

function r = bump(x,y,maze,rad);
if not(inmaze1(x,maze,rad)),
    r=x;
    return;
end;
d=y-x;
nmov=ceil(sqrt(d*d'))./rad;
d=d/nmov;
for k=1:nmov,
    r=x+k*d;
    if not(inmaze1(r,maze,rad)),
        r=r-d./5;
	while not(inmaze1(r,maze,rad)),
            r=r-d./5;
        end;
        return;
    end;
end;
r=[0 0];

%

function len = moveto(xy, h, rad, col);
loc=get(h,'position');
line([loc(1)+rad, xy(1)+rad],[loc(2)+rad, xy(2)+rad],'color',col);
len=xy-loc(1:2);
len = norm(len);
loc(1:2)=xy;
set(h,'position',loc);

%

function val = randn2(sig);
val = randn(1,2)*sig;