%% Potencial Fields
%% Incio - definição de pontos
close all; clear all; clc;
%%%% space =2   %%%% Robot =1   %%%% Target =0  %%%% obstacle =-1
% MAX_X=251;MAX_Y=252;
MAX_X=6;MAX_Y=6;
MAP=2*(ones(MAX_X,MAX_Y));
%starting point
xStart=1;yStart=3;
% Target
xTarget=6;yTarget=3;
MAP(xTarget,yTarget)=0;
%obstacles
obs_points=[ 
    2 3; 3 4; 1 6;
              3 2; 3 3;
              4 2;  4 3
               ];
for n=1:size(obs_points,1)
    MAP(obs_points(n,1),obs_points(n,2))=-1;
end
MAP(xStart,yStart)=1;
xCurrent=xStart;
yCurrent=yStart;
%% representação inicial
% replace with an image of your choice
%img = imread('mapa_reitoria_2_small.bmp');
% img(:,:,1)=fliplr(img(:,:,1)); %soluçao para a inverao de imagem que ocorre
% img(:,:,2)=fliplr(img(:,:,2));
% img(:,:,3)=fliplr(img(:,:,3)); 
 hFig_1 = figure(1);
% imagesc(img);
% colormap(gr ay);
hold on;
set(gca,'ydir','normal');
set(hFig_1, 'Position', [200 400 350 250])
grid on;
axis([0 MAX_X 0 MAX_Y])
h_robot=plot(xStart,yStart,'ob','MarkerSize',10, 'MarkerFaceColor',[0 0 1]); %robot
plot(xTarget,yTarget,'dr','MarkerSize',10, 'MarkerFaceColor',[1 0 0]) %target

% obstacles
for n=1:size(obs_points,1)
    plot(obs_points(n,1),obs_points(n,2),'ks','MarkerSize',10, 'MarkerFaceColor',[0 0 0]) 
end
legend('robot','target','obstacles','Location','North')
title('representação espacial')
%% %%%%% CALCULOS   %%%%%%%  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  calculo do potencial atractor de cada celula  %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
E=2; %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=1.45; %dist. min de repulsã
N=10; %constante de repuslao
Pot_rep=zeros(size(MAP));
for xx=1:MAX_X
    for yy=1:MAX_Y
        for nn=1:sum(sum(MAP==-1))
            Pot_rep_temp=zeros(size(MAP));
            if xx==obs_points(nn,1) && yy==obs_points(nn,2)
                Pot_rep_temp(xx,yy)=10^3;  %valor que substitui INFINITO
            else 
                dist=sqrt((obs_points(nn,1)-xx)^2+(obs_points(nn,2)-yy)^2);
                if dist<=p
                    Pot_rep_temp(xx,yy)=0.5*N*((1/dist)*(1/p))^2;
                end
            end
            Pot_rep=Pot_rep+Pot_rep_temp;
        end
    end
end

%%% somatorio dos potenciais
Pot_tot=Pot_atra+Pot_rep;

[Ftot_x Ftot_y]=gradient(Pot_tot);
Ftot_x=-Ftot_x;
Ftot_y=-Ftot_y;
Ftot_x(1,1)=100;
%% 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

% 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

% potencia total
hFig_4 = figure(4);
set(hFig_4, 'Position', [550 100 350 250])
axis([0 MAX_X 0 MAX_Y])
hold on;grid on;
title('potencial total');
for xx=1:MAX_X
    for yy=1:MAX_Y
        str=sprintf('%0.2f', (Pot_tot(xx,yy)));
        text(xx-0.25,yy,str,'Color',[0 0 1]);       
    end
end

% 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
    
%% ciclo até chegar ao TARGET %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while(1)
    if exist('plot_tra','var')==1
        set(plot_tra,'Color',[0 0 1])
    end
    %%% verificação na vizinhança mais proxima
    %%% para saber proxima direcção
    viz=10*ones(3);
    if xCurrent-1>0 
         viz(2,1)=Pot_tot(xCurrent-1,yCurrent);
         if yCurrent-1>0
             viz(3,1)=Pot_tot(xCurrent-1,yCurrent-1);
         end
         if yCurrent+1<=MAX_Y
             viz(1,1)=Pot_tot(xCurrent-1,yCurrent+1);
         end
    end
     if xCurrent+1<=MAX_X 
          viz(2,3)=Pot_tot(xCurrent+1,yCurrent);    
          if yCurrent+1<MAX_Y
              viz(1,3)=Pot_tot(xCurrent+1,yCurrent+1);   
          end
          if yCurrent-1>0
              viz(3,3)=Pot_tot(xCurrent+1,yCurrent-1);   
          end
     end    
     if yCurrent-1>0
        viz(3,2)=Pot_tot(xCurrent,yCurrent-1);
     end
     if yCurrent+1<=MAX_Y
        viz(1,2)=Pot_tot(xCurrent,yCurrent+1);
     end
     viz(2,2)=Pot_tot(xCurrent,yCurrent);
     viz(2,2)=20;
     Pot_tot(xCurrent,yCurrent)=10^3;
     viz
     [r c] = find(viz==min(min(viz)));
     if size(r,1)>1 || size(c,1)>1
        r=r(1);
        c=c(1);
     end
    
    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_tra=plot(xCurrent,yCurrent,'g*');
    pause();
    set(h_robot,'XData',xCurrent,'YData',yCurrent);
      
if xCurrent==xTarget && yCurrent==yTarget
    figure(1)
    str=sprintf('%s','TARGET REACHED!' );
    text(0,MAX_Y/2,str,'Color',[1 0 0],'FontSize',20);
    break
end
end
%representação dos pontos
% quiver(MAP, MAP,yTarget,Fatr_x,Fatr_y);