clear all; close all; clc;
xv=[0 4 2 2 4];yv=[1 2 5 8 1];
U= [xv;yv];
[Fx,Fy]=gradient(U);Fx=-Fx; Fy=-Fy;

%Representaçãoda direcção da força
%de atracção em cada ponto

quiver(xv,yv,Fx,Fy);
%% mudançao de fundo
close all;clear all;clc;
% This creates the 'background' axes
ha = axes('units','normalized', ...
'position',[0 0 1 1]);

% Move the background axes to the bottom
uistack(ha,'bottom');

% Load in a background image and display it using the correct colors
% The image used below, is in the Image Processing Toolbox. If you do not have %access to this toolbox, you can use another image file instead.
I=imread('mapa_reitoria_2_small.bmp');
hi = imagesc(I);
colormap gray

% Turn the handlevisibility off so that we don't inadvertently plot into the axes again
% Also, make the axes invisible
set(ha,'handlevisibility','off', ...
'visible','off')

% Now we can use the figure, as required.
% For example, we can put a plot in an axes
axes('position',[0.3,0.35,0.4,0.4])
%% 2nd round
close all; clear all; clc;
% replace with an image of your choice
img = imread('mapa_reitoria_2_small.bmp');
 
% set the range of the axes
% The image will be stretched to this.
min_x = 0;
max_x = 251;
min_y = 0;
max_y = 252;

% make data to plot - just a line.
x = min_x:max_x;
y = (6/8)*x;

plot(x,y,'b-*','linewidth',1.5);

img(:,:,1)=fliplr(img(:,:,1)); %soluçao para a inverao de imagem que ocorre
img(:,:,2)=fliplr(img(:,:,2));
img(:,:,3)=fliplr(img(:,:,3)); 
figure;
imagesc(img);
xlabel('Raster Column');
ylabel('Raster Row');
colormap(gray);

% Flip the image upside down before showing it
% imagesc([min_x max_x], [min_y max_y], flipud(img));
hold on;
plot(x,y,'b-*','linewidth',1.5);
 
% set the y-axis back to normal.
set(gca,'ydir','normal');

%% GRID %%
close all; clear all; clc;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   colocação de uma imagem numa grelha
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%DEFINE THE 2-D MAP ARRAY
MAX_X=660;
MAX_Y=600;

%This array stores the coordinates of the map and the 
%Objects in each coordinate
MAP=2*(ones(MAX_X,MAX_Y));

% Obtain Obstacle, Target and Robot Position
% Initialize the MAP with input values
% Obstacle=-1,Target = 0,Robot=1,Space=2
j=0;
x_val = 1;
y_val = 1;
axis([1 MAX_X-1 1 MAX_Y-1])
hold on;
grid on;
% replace with an image of your choice
img = imread('mapa_reitoria_2.bmp');
% set the range of the axes , The image will be stretched to this.
min_x = 0;
max_x = 660;
min_y = 0;
max_y = 600;
 %soluçao para a inverao de imagem que ocorre
img(:,:,1)=fliplr(img(:,:,1));
img(:,:,2)=fliplr(img(:,:,2));
img(:,:,3)=fliplr(img(:,:,3)); 
imagesc(img);
xlabel('XX');
ylabel('YY');
%grid placement
% for xx=10:10:max_x%linspace(1,max_x,30)
%    for yy=10:10:max_y%linspace(1,max_y,30)
%        plot(xx,yy,'k+')
%    end
% end
% set the y-axis back to normal.
set(gca,'ydir','normal');

plot(100,200,'*r');
%% exemplo completo
close all; clear all; clc;
%%%% space =2   %%%% Robot =1   %%%% Target =0  %%%% obstacle =-1
MAX_X=6;MAX_Y=6;
MAP=2*(ones(MAX_X,MAX_Y));
% Target
xTarget=5;yTarget=2;
MAP(xTarget,yTarget)=0;
%obstacles
obs_points=[ 4 4;
    1 1; 2 1; 3 1; 4 1; 5 1 ;6 1;
    1 2; 1 3; 1 4; 1 5];
for n=1:size(obs_points,1)
    MAP(obs_points(n,1),obs_points(n,2))=-1;
end
%starting point
xStart=2;yStart=2;
MAP(xStart,yStart)=1;
xCurrent=xStart;
yCurrent=yStart;

hFig_1 = figure(1);
set(hFig_1, 'Position', [200 400 350 250])
axis([0 MAX_X 0 MAX_Y])
hold on;grid on;
h_robot=plot(xStart,yStart,'ob','MarkerSize',20); %robot
plot(xTarget,yTarget,'dr','MarkerSize',20) %target
% obstacles
for n=1:size(obs_points,1)
    plot(obs_points(n,1),obs_points(n,2),'ks','MarkerSize',20) 
end
legend('robot','target','obstacles','Location','North')
title('representação espacial')



%%%%%%% CALCULOS   %%%%%%%  
% calculo do potencial atractor de cada celula
while(1)
E=0.1; %constante atractor
Pot_atra=zeros(size(MAP));
for xx=1:size(MAP,2)
    for yy=1:size(MAP,1)
        dist=sqrt((abs(xx-xTarget))^2+(abs(yy-yTarget))^2);
        Pot_atra(xx,yy)=0.5*E*dist;
    end
end 

%calculo do potencial repulsor da cada obstaculos
p=3; %dist. min de repulsão
N=10; %constante de repuslao
Pot_rep=zeros(size(MAP));
for nn=1:sum(sum(MAP==-1))
    % comaprar distancias
       dist=sqrt((abs(obs_points(nn,1)-xCurrent))^2+(abs(obs_points(nn,2)-yCurrent))^2);
%     if dist<=p
%         Pot_rep(obs_points(nn,1),obs_points(nn,2))=0.5*N*((1/dist)*(1/p))^2;
%     else
%         Pot_rep(obs_points(nn,1),obs_points(nn,2))=0;
%     end
     Pot_rep(obs_points(nn,1),obs_points(nn,2))=10^2;

end

% força atractor
% [Fatr_x Fatr_y]=gradient(Pot_atra);
% Fatr_x=-Fatr_x;
% Fatr_y=-Fatr_y;

%força repulsor
% Frep_x=zeros(size(MAP));
% Frep_y=zeros(size(MAP));
% for nn=1:sum(sum(MAP==-1))
%     % comaprar distancias
%     dist=sqrt((abs(obs_points(nn,2)-xCurrent))^2+(abs(obs_points(nn,1)-yCurrent))^2);
%     if dist<=p
%         [Frep_x Frep_y]=gradient(Pot_rep);
%         Frep_x=-Frep_x;
%         Frep_y=-Frep_y;
%     else
%         Frep_x(obs_points(nn,1),obs_points(nn,2))=0;
%         Frep_y(obs_points(nn,1),obs_points(nn,2))=0;
%     end
% end
%%% somatorio dos potenciais
Pot_tot=Pot_atra+Pot_rep;

% Ftot_x=zeros(size(MAP));
% Ftot_y=zeros(size(MAP));
% for nn=1:sum(sum(MAP==-1))
    % comaprar distancias
%     dist=sqrt((abs(obs_points(nn,2)-xCurrent))^2+(abs(obs_points(nn,1)-yCurrent))^2);
%     if dist<=p
        [Ftot_x Ftot_y]=gradient(Pot_tot);
        Ftot_x=-Ftot_x;
        Ftot_y=-Ftot_y;
        Ftot_x(1,1)=100;
%     else
%         Frep_x(obs_points(nn,1),obs_points(nn,2))=0;
%         Frep_y(obs_points(nn,1),obs_points(nn,2))=0;
%     end
% end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%representação dos valores de potencial 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
hFig_2 = figure(2);
set(hFig_2, 'Position', [550 400 350 250])
axis([0 MAX_X 0 MAX_Y])
hold on;grid on;
title('potencial atractor');
for xx=1:MAX_X
    for yy=1:MAX_Y
        %atractor
        str=sprintf('%0.2f', Pot_atra(xx,yy));
       text(xx-0.25,yy,str,'Color',[0 0 1]);
    end
end
clear str;
% potencial repulsor
hFig_3 = figure(3);
set(hFig_3, 'Position', [900 400 350 250])
axis([0 MAX_X 0 MAX_Y])
hold on;grid on;
title('potencial repulsor');
for xx=1:MAX_X
    for yy=1:MAX_Y
        str=sprintf('%0.2f', Pot_rep(xx,yy));
       text(xx-0.25,yy,str,'Color',[1 0 0]);       
    end
end
clear str;

%força atractor
% hFig_4 = figure(4);
% set(hFig_4, 'Position', [550 100 350 250])
% axis([0 MAX_X 0 MAX_Y])
% hold on;grid on;
% title('força atractor');
% for xx=1:MAX_X
%     for yy=1:MAX_Y
%         str=sprintf('%0.2f', (Fatr_x(xx,yy)+Fatr_y(xx,yy)));
%        text(xx-0.25,yy,str,'Color',[0 0 1]);       
%     end
% end
% clear str;

% força repulsor
% hFig_5 = figure(5);
% set(hFig_5, 'Position', [900 100 350 250])
% axis([0 MAX_X 0 MAX_Y])
% hold on;grid on;
% title('força repulsor');
% for xx=1:MAX_X
%     for yy=1:MAX_Y
%         str=sprintf('%0.2f', (Frep_x (xx,yy)+Frep_y(xx,yy)));
%        text(xx-0.25,yy,str,'Color',[1 0 0]);       
%     end
% end
% clear str;

% somatorio das forças 
hFig_6 = figure(6);
set(hFig_6, 'Position', [200 100 350 250])
axis([0 MAX_X 0 MAX_Y])
hold on;grid on;
title('somatorio das forças');
Ftot=Ftot_x+Ftot_y;
for xx=1:MAX_X
    for yy=1:MAX_Y
        str=sprintf('%0.2f', (Ftot(xx,yy)));
       text(xx-0.25,yy,str,'Color',[0 0 1]);
    end
end
%%% verificação na vizinhança mais proxima
%%% para saber proxima direcção
viz=zeros(3);
n=0;
for i=-1:1
    for j=-1:1
        if xCurrent+i>0 && xCurrent<=MAX_X+1 && yCurrent+j>=0 && yCurrent<=MAX_Y+1
        viz(4-(i+2),4-(j+2))=Ftot((4-(yCurrent+j)),xCurrent+i);
        end
    end
end
viz
[r c] = find(viz==min(min(viz)));
xCurrent=xCurrent+c-2;
switch r
    case 1
        r=1;
    case 2
        r = 0;
    case 3
        r= -1;
end
yCurrent=yCurrent+r;
figure(1)
plot(xCurrent,yCurrent,'g*')
pause();
set(h_robot,'XData',xCurrent,'YData',yCurrent);

if xCurrent==xTarget && yCurrent==yTarget
    break
end
 figure(hFig_2);figure(hFig_3);figure(hFig_6);
end
%representação dos pontos
% quiver(MAP, MAP,yTarget,Fatr_x,Fatr_y);