function acoli_path(initpos, foodfn, rttp, ttrp, runlength)
% acoli_path(initpos, foodfn, rttp, ttrp, runlength)
%
% Try: acoli_path([0 0], 1, 0.1, .1, 10)
%
% Displays in real time one trial in the experiment described in acoli_hist.
%
% Behavior: the bacterium either performs a run -- advancing two units in
% the current direction -- or a tumble -- rotating the current direction by
% a random angle between 20 and 90 degrees and advancing one unit.
% Switching between the two modes is controlled by the two probabilities
% rttp and ttrp. In addition, if the bacterium senses that it has moved in
% the direction of increasing food concentration, then the random angle
% during a tumble is between 10 and 45 degrees, i.e. the bacterium
% does not turn much if it goes toward food.
%
% Inputs
%
% initpos = initial position of the bacterium, e.g., [0 0]
% rttp = run-to-tumble probability, e.g., 0.1
% ttrp = tumble-to-run probability, e.g., 0.3
% runlength = number of steps
%

sp=0.1;
xmin=-2; xmax = 2;
ymin=-2; ymax = 2;
[X,Y] = meshgrid(xmin:sp:xmax, ymin:sp:ymax);

foodx=0; foody=0;
showfigs=1;
if foodfn==2
    Food.func = @foodfunc2;
elseif foodfn == 1
    Food.func = @foodfunc1;
else
   error('foodfn must be either 1 or 2.') 
end

Food.pts = [0.75, 0.75; -0.5 -0.5];

bloc.pos = initpos; % this is the bacterium [x,y; vx,vy]
bloc.oldpos = bloc.pos;
bloc.vec = [-1,1];
bloc.fgrad=0;
bloc.mode = 0; % 0 - run; 1 -- tumble
bloc.hist = bloc.pos; % history of position

dt = sp;

% bacterium frames = 300
% gradient ascent frames = 40
for i=1:runlength
    bloc = update_bacterium(bloc,Food,dt);
    %    bloc = update_gradascent(bloc,Food,dt);

    if showfigs~=0
        display_status(X,Y,Food, bloc);
    end
end

% if showfigs~=0
%     figure(2)
%     [C,h]  = contour(X,Y,Food.func(X,Y));
%     set(h,'ShowText','on','LabelSpacing',144*5);
%     colormap(0.7*[1 1 1])
%     hold on
%     display_trajectory(bloc);
%     plot(Food.pts(1,2), Food.pts(1,2),'*')
%     hold off
% end
%%
    function c=foodfunc1(x,y)
        s=.5;
        c = exp(-s*((x-Food.pts(1,1)).^2 + (y-Food.pts(1,2)).^2));
        
    end
%%
    function c=foodfunc2(x,y)
        s=5;
        c = exp(-s*((x-Food.pts(1,1)).^2 + (y-Food.pts(1,2)).^2)) +...
            exp(-s*((x-Food.pts(2,1)).^2 + (y-Food.pts(2,2)).^2));
        
    end

%%
    function nv=rotatevec(v,a)
        % rotate vector v by angle a
       rotM = [
           cos(a) sin(a)
           -sin(a) cos(a)
           ];
       
       nv = (rotM * v')';
    end

%% update_bacterium(bloc,Food, dt)
    function newbloc=update_bacterium(bloc,Food, dt)
        runtotumble_prob = 1-rttp; 0.1;%*2;
        tumbletorun_prob = 1-ttrp; 0.3;
        sec1 = 1; % # of steps corresponding to 1 sec
        sec3 = 1; sec1*3; % # of steps corresponding to 3 secs
        
        fdist = Food.func; % food distribution function
        
        % compute concentration gradient
        fgrad = fdist(bloc.pos(1), bloc.pos(2)) ...
            - fdist(bloc.oldpos(1), bloc.oldpos(2));

        newvec = bloc.vec;
        newpos = bloc.pos;
        newbloc = bloc;
        
        actualruntotumble_prob = runtotumble_prob;
        rotupgrad = 1; % power for tumble rotation
        
        if length(bloc.hist) >= sec3+sec1 | 1;
           %cvals1=Food.func(bloc.hist(end-sec1+1:end,1), bloc.hist(end-sec1+1:end,1)); 
           %cvals_prev3=Food.func(bloc.hist(end-sec1-sec3+1:end-sec1,1), bloc.hist(end-sec1-sec3+1:end-sec1,1));

           %temporal_diff = mean(cvals1) - mean(cvals_prev3);
           
           tdiff = Food.func(bloc.pos(1),bloc.pos(2)) - Food.func(bloc.oldpos(1),bloc.oldpos(2));
           %if temporal_diff > 0
           if tdiff > 0
               actualruntotumble_prob = runtotumble_prob; %/(1e1);  %*temporal_diff^2);
               rotupgrad =  2; 1.2;
           else
               actualruntotumble_prob = runtotumble_prob;
           end
        end
        
        % ignoring grad measurement
        %actualruntotumble_prob = runtotumble_prob; rotupgrad = 1;

        if bloc.mode == 0 % run
            if actualruntotumble_prob < rand()
                newbloc.mode = 1; % change to tumble mode
            end
                       
            vec = bloc.vec / norm(bloc.vec);
%            newvec = 2*rotatevec(vec, rand()*8 / (2*pi)); % two units
            newvec = 2*vec;%2*rotatevec(vec, 0*rand()*8 / (2*pi)); % two units
        
        end

        if bloc.mode == 1 % tumble
           if tumbletorun_prob < rand()
               newbloc.mode = 0; % change to run mode
           end
           
           %%%%
           %newbloc.mode = 0; % change to run mode
           %%%%

           vec = bloc.vec / norm(bloc.vec);
           rnum = rand();
           sgn = 1;sign(rand() - 0.5);
           newvec = 1*rotatevec(vec, sgn*(20*(1-rnum) + rnum*90)/rotupgrad / 360*(2*pi)); % one unit
        end
        
%         if fgrad < 0.0 || fgrad*(0.1*rand()) > 0.1
%             angle = pi/4 * (rand());% * (1-abs(fgrad));
%             newvec=rotatevec(bloc.vec,angle);                        
%         end
        
        newpos = bloc.pos + dt*newvec;
                
        newbloc.oldpos = bloc.pos;
        newbloc.pos = newpos;
        newbloc.vec = newvec;
        newbloc.fgrad = fgrad;
        
        newbloc.hist = [bloc.hist; newpos];
    end

%%
    function newbloc=update_bacterium___(bloc,Food, dt)
        runtotumble_prob = rttp; %0.1*2;
        tumbletorun_prob = ttrp; %0.3;
        sec1 = 2; % # of steps corresponding to 1 sec
        sec3 = sec1*3; % # of steps corresponding to 3 secs
        
        fdist = Food.func; % food distribution function
        
        % compute concentration gradient
        fgrad = fdist(bloc.pos(1), bloc.pos(2)) ...
            - fdist(bloc.oldpos(1), bloc.oldpos(2));

        newvec = bloc.vec;
        newpos = bloc.pos;
        newbloc = bloc;
        
        actualruntotumble_prob = runtotumble_prob;
        
        if length(bloc.hist) > sec3+sec1;
           cvals1=Food.func(bloc.hist(end-sec1+1:end,1), bloc.hist(end-sec1+1:end,1)); 
           cvals_prev3=Food.func(bloc.hist(end-sec1-sec3+1:end-sec1,1), bloc.hist(end-sec1-sec3+1:end-sec1,1));

           temporal_diff = mean(cvals1) - mean(cvals_prev3);
           if temporal_diff > 0
               actualruntotumble_prob = runtotumble_prob/(1000);%*temporal_diff^2);
           else
               actualruntotumble_prob = runtotumble_prob;
           end
        end
        
        if bloc.mode == 0 % run
            if rand() < actualruntotumble_prob
                newbloc.mode = 1; % change to tumble mode
            end
            vec = bloc.vec / norm(bloc.vec);
            newvec = 2*rotatevec(vec, rand()*8 / (2*pi)); % two units
        
        end

        if bloc.mode == 1 % tumble
           if rand() < tumbletorun_prob
               newbloc.mode = 0; % change to run mode
           end

           vec = bloc.vec / norm(bloc.vec);
           newvec = 1*rotatevec(vec, rand()*80 / (2*pi)); % one unit
        end
        
%         if fgrad < 0.0 || fgrad*(0.1*rand()) > 0.1
%             angle = pi/4 * (rand());% * (1-abs(fgrad));
%             newvec=rotatevec(bloc.vec,angle);                        
%         end
        
        newpos = bloc.pos + dt*newvec;
                
        newbloc.oldpos = bloc.pos;
        newbloc.pos = newpos;
        newbloc.vec = newvec;
        newbloc.fgrad = fgrad;
        newbloc.hist = [bloc.hist; newpos];
        
    end
%%
    function newbloc=update_bacterium_old(bloc,Food, dt)
        fdist = Food.func; % food distribution function
        
        % compute concentration gradient
        fgrad = fdist(bloc.pos(1), bloc.pos(2)) - fdist(bloc.oldpos(1), bloc.oldpos(2));

        newvec = bloc.vec;
        newpos = bloc.pos;
        
        % rotate rule
        if fgrad < 0.0 || fgrad*(0.1*rand()) > 0.1
            angle = pi/4 * (rand());% * (1-abs(fgrad));
            newvec=rotatevec(bloc.vec,angle);                        
        end
        
        newpos = bloc.pos +dt*newvec;
        
        newbloc = bloc;
        newbloc.oldpos = bloc.pos;
        newbloc.pos = newpos;
        newbloc.vec = newvec;
        newbloc.fgrad = fgrad;
        newbloc.hist = [bloc.hist; newpos];
        
    end

%%
    function display_status(X,Y,Food, bloc)
        figure(1);

        %subplot(1,2,1)
       
        % draw bacterium
        quiver(bloc.pos(1),bloc.pos(2),bloc.vec(1),bloc.vec(2),sp/2);
    
        hold on
        % draw food locations
        %plot(Food.pts(:,1), Food.pts(:,2), '*')
               
        display_trajectory(bloc);
        
        hold off
        axis equal tight
        axis([xmin xmax ymin ymax])
        
%         subplot(1,2,2)
%         mesh(X,Y,Food.func(X,Y));
%         hidden off
%         hold on
%         quiver3(bloc.pos(1), bloc.pos(2), Food.func(bloc.pos(1), bloc.pos(2)),...
%             bloc.vec(1), bloc.vec(2),0,sp)
%         
%         plot3(bloc.pos(1), bloc.pos(2), Food.func(bloc.pos(1), bloc.pos(2)),'-w*')
%         plot3(bloc.pos(1), bloc.pos(2), Food.func(bloc.pos(1), bloc.pos(2)),'-ko')
%         hold off
%         
%         axis off
    end
%% display_trajectory(bloc)
    function display_trajectory(bloc)
        % draw trajectory
        plot(bloc.hist(:,1), bloc.hist(:,2), ':');

        % draw end location
        plot(bloc.pos(1),bloc.pos(2), 'ko','MarkerFaceColor','r')

        % draw initial location
        plot(bloc.hist(1,1),bloc.hist(1,2), 'ks','MarkerFaceColor','k')

        axis equal tight
        axis([xmin xmax ymin ymax])

    end
end