function manga1_v5
%Versao 5

warning off;
clear;
global ao ao1 dio_out dio_in;
global img1 img2;
global yy1_xM yy1_xm yy2_xm yy2_xM ganho Vmotor lap lap_tmp;
initHardware;

setSpeed( 0 );
disp('Ligado');
pause;

%Determinaçao inicial do tamanho da imagem em uso
rgb_=vfm('grab',1);
[alturaImg larguraImg] = size(rgb_);

%-------------------------------
Vmotor=2;
ganho=0.75;
comp=-15;

lap_tmp=0;

setSpeed( Vmotor );

yy1_xm =41+comp;
yy1_xM =55+comp;
yy2_xm =36+comp;%40 vaL anterior
yy2_xM =48+comp;%53  "     "

%----------------------------------

manga_1=1;

while manga_1==1
% tic
% for n = 1:100


%Vmotor=2.5;  %%%ISTO ha'-de ser mudado!!!!!!
%setSpeed( Vmotor);  %% ISTO CONSOME MUITO TEMPO (0.03 segundos!!!!)    

%[passadeira1, manga_1]=test_passadeira(dio_in);

%%==============Aquisicao e binarizacao imagens

[xx1, xx2,dif1,dif2]=Calc_img;

%****************************

if xx1 < (0.5 * alturaImg)  %Teste a 60% do topo da image
    continue
end
%=======================
%Ha' pixels Brancos abaixo dos 60 % da imagem

%[passadeira2, manga_1]=test_passadeira(dio_in);

if calc_traj1(xx1,dif1) == 1    %Correu bem!
    continue
end

%Correu mal. Ver 2ª imagem



%%%%%%%%%%%% TESTE DA 2ª imagem (Esquerda)

if xx2 < 0.5 * alturaImg 
    continue % Se nao houver brancos abaixo dos 60% na img esq continua o ciclo
end           
%[passadeira3, manga_1]=test_passadeira(dio_in);           

if calc_traj2(xx2,dif2) == 1    %Correu bem!
    continue
end

   
          
end % do ciclo while


%---------------------------------------

%   ENCERRAR COM OS CANAIS A ZERO.

setSpeed(0);

putvalue(dio_out, [0 0 0 0 0]);
stop(dio_out);
disp('concluido');

return

%================================

%   AUXILIARY FUNCTIONS

%==================================================
function [emcima,manga_1]=test_passadeira(io_add)
global ao Vmotor lap lap_tmp;
emcima = getvalue(io_add); 
if emcima == 1
    lap_tmp=lap_tmp+1;
    lap=(lap_tmp)/3;
    disp('linha');
    if lap==3
       setSpeed(0);          
       manga_1=0
       return
    else manga_1=1;
       return 
    pause(2);  
    end    
else manga_1=1;
end 
%======================================
function setSpeed( speed )
global ao;
    putdata(ao, [speed]');
    start(ao);
    stop(ao);

end
%======================================
function setDirection( dir )

global ao1;
    putdata(ao1, [dir]');
    start(ao1); 
    stop(ao1);

end
%==================================
function initHardware()

global ao ao1 dio_out dio_in;

ao=analogoutput('nidaq');
addchannel(ao, 0);
ch0=ao.Channel;

ao1=analogoutput('nidaq');
addchannel(ao1, 1);
ch1=ao1.Channel;

sampleRate=1000;

set(ao,'SampleRate',sampleRate);
set(ao1,'SampleRate',sampleRate);

set(ao, 'TriggerType', 'Immediate');
set(ao, 'RepeatOutput',0);

set(ao1, 'TriggerType', 'Immediate');
set(ao1, 'RepeatOutput', 0);

dio_out = digitalio('nidaq', 1);
addline(dio_out, 0:4, 0, 'Out');
putvalue(dio_out, [1 1 1 1 0]);

dio_in = digitalio('nidaq', 1);
addline(dio_in, 0, 1, 'In');
end


%************************************
function [xx1,xx2,dif1,dif2]=Calc_img()
global img1 img2;
rgb_=vfm('grab',1);
rgb_1=(rgb_(:,:,1) >250);
rgb_2=(rgb_(:,:,2) >250);
% rgb_3=(rgb_(:,:,3) >250);
rgb_4 = rgb_1 & rgb_2;

img1=rgb_4(:,1:end/2);  %Imagem direita
img2=rgb_4(:,end/2:end); %Imagem esquerda

%===== INICIO ALGORITMO de GUIAMENTO

[x1 y1]=find(img1);
xx1=max(x1);  %ponto banco mais baixo da imagem direita
xx1_min=min(x1);
dif1=xx1-xx1_min;

[x2 y2]=find(img2);
xx2=max(x2);  %idem esquerda
xx2_min=min(x2);
dif2=xx2-xx2_min;
end

%************************************
function [ret,Vref]=calc_traj1(xx1,dif1)
global img1 img2;
global yy1_xM ganho Vmotor;
%----- Determinar a largura da linha a 20 pixels acima do limite do branco
xx_M1=img1(xx1-20,:);
[x1_M y1_M]=find(xx_M1);

[yy1_m]=min(y1_M);
[yy1_M]=max(y1_M);

%************************
%% teste para ver se nao e' passadeira
if (yy1_M >= 1 & yy1_m >=1 ) & length(y1_M)<=15 %& dif1>=38     
   Vref=(yy1_M-yy1_xM)*(-ganho/(yy1_xM-1));
   setDirection( Vref);
   ret=1;
   return;
    
elseif yy1_M~=[] & yy1_m~=[] %& dif1>=40 %27 deste modo o robot mantem a trajectoria
    yy1_M=yy1_m+15;                                          % que tinha ao entrar na interseccao e           
    Vref=(yy1_M-yy1_xM)*(-ganho/(yy1_xM-1));                 % ignora a linha que vai encontrar!!!
    setDirection( Vref);
    ret=1;
    return;
%     
% elseif  length(y1_M)>15 & dif1<60 
%     setSpeed(Vmotor-0.65);
%     ret=1;
%     return;
    
end
ret = 0;

%***************************************
function [ret2,Vref]=calc_traj2(xx2,dif2)
global img1 img2;
global yy2_xM yy2_xm ganho Vmotor;
%----- Determinar a largura da linha a 20 pixels acima do limite do branco
xx_M2=img2(xx2-20,:);
[x2_M y2_M]=find(xx_M2);

[yy2_m]=min(y2_M);
[yy2_M]=max(y2_M);


%----------------------------
%Teste para ver se nao e' passadeira
if yy2_m>=1 & yy2_M>=1 & length(y2_M)<=15 %& dif2>=38
    Vref=(yy2_m-yy2_xm)*(-ganho/(yy2_xm-1));
    setDirection( Vref );
    ret2=1;
    return;
    
elseif yy2_M~=[] & yy2_m~=[]  %& dif2>=40% 27 se e' passdeira (ou outra coisa estranha)
    yy2_m=yy2_M-15;
    Vref=(yy2_m-yy2_xm)*(-ganho/(yy2_xm-1));
    setDirection( Vref);
    ret2=1;
    return;
    

% elseif  length(y2_M)>15 & dif2<60 
%     setSpeed(Vmotor-0.65);
%     ret=1;
%     return;
end 