%-----------------------------------------------------------
% Carlos Cabral 2003 - Dep.UA

% funçao para validar dois pilares de acordo com os modelos iniciais
%-----------------------------------------------------------
function [comb_OK_KO, posicao_pilares, caso_localiz] = VALIDA_2PILARES(coord_pilares)

% recebe (coord_pilares) somente as coordenadas de dois pilares 
% se comb_OK_KO  = 1 combinaçao verifica os parametros
%                  0 combinaçao falhou               
percentagem_error_dist=0.07; % =7 %percentagem de erro admissivel no calculo das distancias para que o conjunto seja considerado

distBWpilares=sqrt((coord_pilares(2,1)-coord_pilares(1,1))^2+(coord_pilares(2,2)-coord_pilares(1,2))^2); % calculo das distancias euclidianas
angulo_alinha=atan2(coord_pilares(2,2)-coord_pilares(1,2),coord_pilares(2,1)-coord_pilares(1,1))*180/pi; % angulo da recta que une dois pontos (especificos) de dois pilares
% se for pilar circular o ponto os pontos sao sempre os centros das circunferencias, se for pilar rectangular os pontos sao os cantos mais proximos do robot

if angulo_alinha < -90 % funçao para resolver o problemas dos angulos
    angulo_alinha= angulo_alinha+360;
end   
       
soma_classif=coord_pilares(1,4)+coord_pilares(2,4); % como na ultima coluna 0->circular e 1->rectangular a soma identifica o numero de pilares de cada categoria       
switch soma_classif 
    
    case 0 % Detectou DOIS pilares CIRCULARES  ( o o )
        [comb_OK_KO, posicao_pilares, caso_localiz] = MODS2CIRC(distBWpilares,percentagem_error_dist); 
    case 1 % Detectou UM pilar RECTANGULAR e UM pilar CIRCULAR ( [] o ) ou ( o [] )
        [comb_OK_KO, posicao_pilares, caso_localiz] = MODS1CIRC1RECT(distBWpilares, angulo_alinha, coord_pilares, percentagem_error_dist); 
    case 2 % Detectou DOIS pilares RECTANGULARES ( [] [] ) 
        [comb_OK_KO, posicao_pilares, caso_localiz] = MODS2RECT(distBWpilares,percentagem_error_dist, coord_pilares, angulo_alinha);
        
end

   
   