%--------------------------------------------------------------------------------------------
%
%                   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=0;
data=0;

nivel=200;

I = vfm('grab',1);
figure

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(I,0.8,'nearest');    
    
    deteta_stop_ar = sum(sum(Ia(:,:,1)));
    deteta_stop_ag = sum(sum(Ia(:,:,2)));
    deteta_stop_ab = sum(sum(Ia(:,:,3)));
    
    
    if and(deteta_stop_ar>deteta_stop_ag,deteta_stop_ar>deteta_stop_ab);
       emergencia=1
       if channel1~=0
          rc=ddepoke(channel1,'M502',1);
          emergencia=0;
       end
       
    else
       emergencia=2
    end
    
    
    Ib=(double(Ia(:,:,1))>nivel).*(double(Ia(:,:,2))>nivel).*(double(Ia(:,:,3))>nivel);
    imshow(Ib)
    pause
    Ib=~Ib;
    imshow(Ib)
    pause
    Icentro_1 = bwmorph(Ib,'shrink',inf);
    imshow(Icentro_1)
    pause
    Icentro_2 = bwmorph(Ib,'erode',3);
    imshow(Icentro_2)
	 pause 	
    Icentro_21 = bwmorph(Icentro_2,'shrink',inf);
    %I_final = double(Icentro_21)+double(Icentro_1);
    imshow(Icentro_21)
    pause
    [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(ponto1_y-ponto2_y,ponto2_x-ponto1_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
    
    
    %     a = input('ordem?','s');
    %     switch a
    %     case 'f' ,frente=1
    %     case 't' ,tras=1
    %     case 'e' ,esquerda=1
    %     case 'd' ,direita=1
    %     case 's' ,emergencia=1
    %     case 'r' ,emergencia=2
    %     end
    
    
    
    % comunicaç~ao com o automato através do MelDDE
    if channel1~=0;
       if frente==1;
          rc=ddepoke(channel1,'M102',1);
          rc=ddepoke(channel1,'M202',0);
          rc=ddepoke(channel1,'M300',0);
          rc=ddepoke(channel1,'M400',0);
          frente=0;
       end
       if tras==1;
          rc=ddepoke(channel1,'M202',1);
          rc=ddepoke(channel1,'M102',0);
          rc=ddepoke(channel1,'M300',0);
          rc=ddepoke(channel1,'M400',0);
          tras=0;
       end
       if esquerda==1;
          rc=ddepoke(channel1,'M300',1);
          rc=ddepoke(channel1,'M102',0);
          rc=ddepoke(channel1,'M202',0);
          rc=ddepoke(channel1,'M400',0);
          esquerda=0;
       end
       if direita==1;
          rc=ddepoke(channel1,'M400',1);
          rc=ddepoke(channel1,'M102',0);
          rc=ddepoke(channel1,'M202',0);
          rc=ddepoke(channel1,'M300',0);
          direita=0;
       end
       if emergencia==1;
          rc=ddepoke(channel1,'M502',1);
          emergencia=0;
       end
       if emergencia==2;
          rc=ddepoke(channel1,'M502',0);
          emergencia=0;
       end
    end
 end
 
 rc=ddeterm(channel1);   
 
 
 
 