function manga1_v6_vs
%Versao 5

warning off;
clear;
global ao ao1 dio_out dio_in;
global img1 img2;
global yy1_xM yy1_xm yy2_xm yy2_xM;
global manga_1 ganho Vmotor lap time_out2;
global linhaPassadeira linhaSW1 linhaSW2 linhaSW3 linhaSW4;

linhaPassadeira = 1;
linhaSW1 = 2; linhaSW2 = 3; linhaSW3 = 4; linhaSW4 = 5; 

TEMPO_VALIDAR_PASSAGENS = 10;   %10 segundo...
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.7;
comp=-15;
% setSpeed( Vmotor );

yy1_xm =40+comp;
yy1_xM =53+comp;
yy2_xm =30+comp;
yy2_xM =43+comp;

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

manga_1=1;

while 1
   
  
%************************************************
while 1
if getvalue( dio_in.Line(linhaSW2)) & ~getvalue( dio_in.Line(linhaSW1)) %& ~getvalue( dio_in.Line(linhaSW3)) &~getvalue( dio_in.Line(linhaSW4));
      
      TURBO1=getvalue( dio_in.Line(linhaSW3));
      TURBO2=getvalue( dio_in.Line(linhaSW4));
      
      if TURBO1 &~TURBO2
        Vmotor=1.5;
        ganho=0.7;
        comp=-15;
        disp('Vmotor = 1.5');
        setSpeed( Vmotor );         % Selecao da velocidade
        break
      end

      if TURBO2 &~TURBO1
        Vmotor=2.5;
        ganho=0.75;
        comp=-15;
        disp('Vmotor = 2.5');
        setSpeed( Vmotor );
        break
      end

      if TURBO1 & TURBO2
        Vmotor=2;
        ganho=0.7;
        comp=-15;
        disp('Vmotor = 2');
        setSpeed( Vmotor );
        break
      end
end
end
%********************************************** 
  
  
%**********************************************
%Ciclo de espera pela ordem de arranque
%
while 1
  if  getvalue( dio_in.Line(linhaSW1) ) 
       break
   end
end
%*********************************************

disp('Ja comecei...')
putvalue(dio_out, [1 1 1 1 0]);



pause(0.5);
total_passagens_passadeira = 0;
tic;
tempo_ultima_passagem = toc;


while 1 
%%%%%
reset_ciclo=getvalue(dio_in.Line(linhaSW1));    
reset_voltas=getvalue(dio_in.Line(linhaSW2));

if reset_voltas & ~reset_ciclo      %*************************************
    total_passagens_passadeira=0;   %*Começar a contar de novo as voltas
end                                 %*


if reset_ciclo & ~reset_voltas  %***************************************************
    break                       %* Começar de novo!!
end                             %*

%%%% Testar e actualizar passagens na passadeira
em_cima=test_passadeira(dio_in);
if em_cima
    tmp_passadeira = toc;
    if tmp_passadeira - tempo_ultima_passagem > TEMPO_VALIDAR_PASSAGENS
        total_passagens_passadeira = total_passagens_passadeira+1
        tempo_ultima_passagem = tmp_passadeira;
    end
    if total_passagens_passadeira >= 5
        break
    end
end
%%%%
        

%%==============Aquisicao e binarizacao imagens
%% Isto leva cerca de 0.08 segundos

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

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

%Testa passadeira outra vez...
em_cima=test_passadeira(dio_in);
if em_cima
    tmp_passadeira = toc;
    if tmp_passadeira - tempo_ultima_passagem > TEMPO_VALIDAR_PASSAGENS
        total_passagens_passadeira = total_passagens_passadeira+1
        tempo_ultima_passagem = tmp_passadeira;
    end
    if total_passagens_passadeira >= 5
        setSpeed(0);
        break
    end
end

%%%%%%%%%%%


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


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 50% na img esq continua o ciclo
end 


if calc_traj2(xx2,dif2) == 1    %Correu bem!
    continue
end
   
          
end % do 2ciclo while


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

%   ENCERRAR COM OS CANAIS A ZERO.

setSpeed(0);

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

end %1ciclo while

return

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

%   AUXILIARY FUNCTIONS

%==================================================
function [emcima]=test_passadeira(io_add)
    global linhaPassadeira linhaSW1 linhaSW2 linhaSW3 linhaSW4;
    emcima = getvalue(io_add.Line(linhaPassadeira));
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:7, 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 