clear
clf

NX = 1.5*50; %1st index  = row index
NY = 1.5*70; %2nd index  = column index in matrix below

d  = 5; % grid spacing in meters
dx = d; % keep dx=dy for simplicity
dy = d;

LX=NX*dx;
LY=NY*dy;

%set initial conditions
correct_count = 0;
total_iterations = 0;

rng(2) % Seed for random value. Used for debuggin
numberOfTrials = 1; % Change to run multiple trials
for i = 1:numberOfTrials
    i
Z = zeros(NX,NY);
for i=10:10:NX
    for j = 10:10:NY
       if (rand() < 0.75) 
       Z = add_hill(Z, NX,NY,i,j,30,rand(1)/80);
       end
    end
end
% for i=1:4
%    Z = initial_conditions_add_hill(Z,NX,NY,rand()*NX,rand()*NY,rand()*NX/5,rand()*30);
% end

Zmin = min(min(Z));
Zmax = max(max(Z));

fprintf('Minimum elevation      %f\n',Zmin);
fprintf('Maximum elevation      %f\n',Zmax);

x = 1:NX;
y = 1:NY;
[X,Y] = meshgrid(y,x); %strange the y goes first !!!

subplot(2,2,1);
       
pcolor(X,Y,Z);
surf(X,Y,Z);
axis([0 NY 0 NX Zmin Zmax])
title('Surface Relief');
xlabel('x');
ylabel('y');
view([+34.5 14]);

subplot(2,2,2);

hold off
pcolor(X,Y,Z)
shading interp
hold on
contour(X,Y,Z,30,'k');  
title('Contour');

drawnow; 

robotPositionX = ceil(rand()*NX);
robotPositionY = ceil(rand()*NY);
normalize = Zmax - Zmin;

iteration = 1;
% Choose a Tinitial 
Tinitial = 0.08;
k = 1;
f = 0;
while true
%    T = Tinitial / iteration
   T = Tinitial / log(iteration);
%     T = Tinitial /iteration;
    
    if T <= 3e-4 % Temperature is low enough
        break;
    end
    Xpositions(iteration) = robotPositionX;
    Ypositions(iteration) = robotPositionY;
    iteration = iteration + 1;
    leftX = max(robotPositionX - 1, 1);
    rightX = min(robotPositionX + 1, NX);
    upY = min(robotPositionY + 1, NY);
    downY = max(robotPositionY - 1, 1);
    robotElevation = Z(robotPositionX, robotPositionY);
    leftElevation = Z(leftX, robotPositionY);
    rightElevation = Z(rightX, robotPositionY);
    upElevation = Z(robotPositionX, upY);
    downElevation = Z(robotPositionX, downY);
    randomNeighbor = ceil(rand() * 4);
    %Use the following code in order to take pictures
    if mod(iteration, 5000) == 0
        subplot(2, 2, 4);
%         plot(Ypositions(:), Xpositions(:)); % Plot entire path
        plot(Ypositions(max(iteration-10000+ 1, 1):iteration - 1), Xpositions(max(iteration + 1 - 10000, 1): iteration -1));
        title('Robot Path');
        axis([0, NY, 0, NX])
        saveas(gcf, ['sim_',sprintf('%03d',f),'.png'])
        
        xlabel('x');
        ylabel('y');
        f = f +1;
        subplot(2, 2, 3)
        plot(iteration, T, 'xr')
        title('Tempurature');
        hold on
        drawnow
    end
    if randomNeighbor == 1
        energy = (leftElevation - robotElevation)/normalize;
        if energy > 0
            robotPositionX = leftX;
            continue;
        elseif rand() < exp(energy/T)
            robotPositionX = leftX;
            continue;
        end
    end
    if randomNeighbor == 2
        energy = (rightElevation - robotElevation)/normalize;
        if energy > 0
            robotPositionX = rightX;
            continue;
        elseif rand() < exp(energy/T)
            robotPositionX = rightX;
            continue;
        end
    end
    if randomNeighbor == 3
        energy = (upElevation - robotElevation)/normalize;
        if energy > 0
            robotPositionY = upY;
            continue;
        elseif rand() < exp(energy/T)
            robotPositionY = upY;
            continue;
        end
    end
    if randomNeighbor == 4
        energy = (downElevation - robotElevation)/normalize;
        if energy > 0
            robotPositionY = downY;
            continue;
        elseif rand() < exp(energy/T)
            robotPositionY = downY;
            continue;
        end
    end


end
    endX = Xpositions(iteration -1);
    endY = Ypositions(iteration -1);
    if Z(endX, endY) == Zmax % Check if robot found max height
       correct_count = correct_count + 1;
    end
    total_iterations = total_iterations + iteration;
end
subplot(2, 2, 4);
endX = Xpositions(iteration -1);
endY = Ypositions(iteration -1);
plot(endY, endX, 'xr', Ypositions, Xpositions);
axis([0, NY, 0, NX])
title('Total Path');
saveas(gcf, ['sim_',sprintf('%03d',f),'.png'])

% hold on
% plot(endX, endY, 'xr');
hold off
drawnow
Zmax
Z(endX, endY)
averageIterations = total_iterations/numberOfTrials
percentCorrect = correct_count / numberOfTrials







