%--------------------------------------------------------------------------------------------
%
%                   Projecto de automaç~ao HEXAPODE
%
%--------------------------------------------------------------------------------------------





clear all
clc


channel1=ddeinit('MelDDE','hexapode')
a='n';

ligado=0;
frente=0;tras=0;esquerda=0;direita=0;emergencia=2;
data=0;

nivel=200;

I = vfm('grab',1);

vfm('preview',1);

pause(1)


while a~='0'
   
   clc
   % I=imread('seta.bmp');
   % I=imresize(I,0.2,'nearest');
   Ia = vfm('grab',1);
   %I2 = rgb2gray(I);
   %I3 = im2bw(I2);
   %I3 =~ I3;
   % I3 = imrotate(I3,5,'nearest');
   Ia=imresize(Ia,0.5,'nearest');    
   
   deteta_stop_ar = sum(sum(Ia(:,:,1)));
   deteta_stop_ag = sum(sum(Ia(:,:,2)));
   deteta_stop_ab = sum(sum(Ia(:,:,3)));
   
   
   if channel1~=0
      
      if and(0.8*deteta_stop_ar>deteta_stop_ag,0.8*deteta_stop_ar>deteta_stop_ab);
         emergencia=1
         rc=ddepoke(channel1,'M502',1);
         emergencia=0;
      end
   end
   
   if and(0.85*deteta_stop_ag>deteta_stop_ar,0.85*deteta_stop_ag>deteta_stop_ab);          
      emergencia=2
   end
   if and(0.8*deteta_stop_ab>deteta_stop_ar,0.8*deteta_stop_ab>deteta_stop_ag);
      a='0';
      emergencia=1;
   end
   
   Ib=(double(Ia(:,:,1))>nivel).*(double(Ia(:,:,2))>nivel).*(double(Ia(:,:,3))>nivel);
   Ib=~Ib;
   Icentro_1 = bwmorph(Ib,'shrink',inf);
   Icentro_2 = bwmorph(Ib,'erode',5);
   Icentro_21 = bwmorph(Icentro_2,'shrink',inf);
   %I_final = double(Icentro_21)+double(Icentro_1);
   [ponto1_y,ponto1_x] = find(Icentro_21);
   [ponto2_y,ponto2_x] = find(Icentro_1);
   
   
   if and(length(ponto1_x)==1,length(ponto2_x)==1)
      direcao = atan2(ponto2_y-ponto1_y,ponto1_x-ponto2_x);
      if abs(direcao) < pi/4 ;
         direita=1
      end
      if and(direcao >= pi/4 ,direcao <3*pi/4) ;
         frente=1
      end
      if abs(direcao) >=3*pi/4 ;
         esquerda=1
      end
      if and(direcao > -3*pi/4 ,direcao < -pi/4) ;
         tras=1
      end
   end
   
   
   % comunicaç~ao com o automato através do MelDDE
   if channel1~=0;
      if frente==1;
         data1=ddereq(channel1,'M102');
         if and(data1==0,channel1~=0);
            rc=ddepoke(channel1,'X1',1);
            frente=0;
         end
      end
      if tras==1;
         data2=ddereq(channel1,'M202');
         if data2==0;
            rc=ddepoke(channel1,'X2',1);
            tras=0;
         end
      end
      if esquerda==1;
         data3=ddereq(channel1,'M302');
         if data3==0;
            rc=ddepoke(channel1,'X4',1);
            esquerda=0;
         end
      end
      if direita==1;
         data4=ddereq(channel1,'M402');
         if data4==0;
            rc=ddepoke(channel1,'X5',1);
            direita=0;
         end          
      end
      if emergencia==1;
         data=ddereq(channel1,'M502');
         if data==0;
            rc=ddepoke(channel1,'M502',1);
                                   
            emergencia=0;
         end
      end
      if emergencia==2;
         data=ddereq(channel1,'M502');
         if data==1;
            
            rc=ddepoke(channel1,'M505',1);
            while ddereq(channel1,'M505')==1
               pause(0.5)
            end

            rc=ddepoke(channel1,'M502',0);
            emergencia=0;
         end
      end
   end
end%while

if ddereq(channel1,'M102')==1;
   rc=ddepoke(channel1,'X1',0);
   while ddereq(channel1,'M102')==1;
      pause(0.5)
   end
end

if ddereq(channel1,'M202')==1;
   rc=ddepoke(channel1,'X2',0);
   while ddereq(channel1,'M102')==1;
      pause(0.5)
   end
end

while ddereq(channel1,'M302')==1;
   pause(0.5)   
end

while ddereq(channel1,'M402')==1
   pause(0.5)
end

rc=ddepoke(channel1,'M502',0);

rc=ddeterm(channel1);   