close all
clear all
clc

rows=10;
columns=10;
sizex=1;
sizey=1;

obstacles=[3 10;4 10;5 10;1 7;1 8;2 7;2 8;10 4;10 5;3 1;3 2;3 3;3 4;3 5;5 7;6 7;7 7;8 7;5 8;6 8;7 8;8 8;7 3;8 3;7 4;8 4];
%Goal deve essere 2x2 per via del secondo robot
goals=[9,9; 1,1];
goalsrow=[9,9 1,1];


movements=[0 0;1 0;-1 0;0 1;0 -1];
mov_costs=[0.1;1;1;1;1];


tot_mov=[];
tot_cost=[];
for i=1:5
    for j=1:5
        tot_mov=[tot_mov;movements(i,:) movements(j,:)];
        tot_cost=[tot_cost;mov_costs(i)+mov_costs(j)];
    end
end


figure

%Questa parte crea la figura con la matrice
for i=1:rows
    for j=1:columns
        Poly{i,j}=Polyhedron('A',[eye(2);-eye(2)],'b',[sizex*i;sizey*j;-sizex*(i-1);-sizey*(j-1)]);
        if ismember([i j],obstacles,'rows')
            plot(Poly{i,j},'Color','k');
        else %siccome non e' un ostacolo allora aumento l'indice
            [llog,poss]=ismember([i j],goals,'rows');
            if  llog && poss==1
                plot(Poly{i,j},'Color','g');
            elseif llog && poss==2 
                plot(Poly{i,j},'Color','b');
            else
                plot(Poly{i,j},'Color','w');
            end
        end
        hold on
        
    end
end

           
               
                


idx=0;
for i1=1:rows
    for j1=1:columns
        for i2=1:rows
            for j2=1:columns

                if ismember([i1 j1],obstacles,'rows')||ismember([i2 j2],obstacles,'rows')||ismember([i1 j1],[i2 j2],'rows')
                    ;
                else
                    idx=idx+1;
                    good(idx,:)=[i1 j1 i2 j2];

                    if ismember([i1 j1 i2 j2],goalsrow,'rows')   
                        final(idx)=0;
                    else
                        final(idx)=1e15; %sarebbe + infinito
                    end
                end
            end
        end
    end
end




initial=[1 10  10 1];
img=imread('asimo.jpeg');
img2=imread('robot.jpg');
im_p=image([initial(1)*sizex-0.05 (initial(1)-1)*sizex+0.05 ],[ initial(2)*sizex-0.05 (initial(2)-1)*sizex+0.05],img);
im_p2=image([initial(3)*sizex-0.05 (initial(3)-1)*sizex+0.05 ],[ initial(4)*sizex-0.05 (initial(4)-1)*sizex+0.05],img2);

horizon=15;
cost(horizon+1,1:idx)=final;
for ii=horizon:-1:1
    for jj=1:idx
        %         if ii==99 && jj==72
        %         keyboard
        %         end
        for ll=1:size(tot_mov,1)
            if not(ismember(good(jj,:),goalsrow,'rows'))&&ll==1                
                cost_temp(ll)=1e20;
            else
                temp(ll,:)=good(jj,:)+tot_mov(ll,:);
                [log,id]=ismember(temp(ll,:),good,'rows');
                if log
                    cost_temp(ll)=cost(ii+1,id)+tot_cost(ll);
                else
                    cost_temp(ll)=1e15;
                end
            end
        end
        [cm,idmm]=min(cost_temp);
        action(ii,jj)=idmm;
        cost(ii,jj)=cm;
    end
end
figure(1)
disp('Press a key !')  % Press a key here.You can see the message 'Paused: Press any key' in        % the lower left corner of MATLAB window.
pause;

pos(1,:)=initial;
for ii=1:horizon
    [~,jj]=ismember(pos(ii,:),good,'rows');
    pos(ii+1,:)=pos(ii,:)+tot_mov(action(ii,jj),:);
    
    im_p.XData=im_p.XData+tot_mov(action(ii,jj),1);
    im_p.YData=im_p.YData+tot_mov(action(ii,jj),2);
    im_p2.XData=im_p2.XData+tot_mov(action(ii,jj),3);
    im_p2.YData=im_p2.YData+tot_mov(action(ii,jj),4);

    figure(1)
    pause(1)
end