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

% CASOS PARTICULARES DO CANTO
% tem de se testar dois casos espeficificos que podem acontecer no caso de se ter detectado 3 pilares 
% (ver exemplo scan 750) 
%-----------------------------------------------------------
function [coord_loc_robot] = CASO_PARTICULAR_CANTO(combinacoes, coord_pilares, coord_loc_robot)

percentagem_error_dist=0.07; % =7%
tolerancia=8;

    linha_combinacoes=find(combinacoes(:,3)==0); % procura a combinaçao que nao verifica as condiçoes
    par_Q_falhou=[coord_pilares(combinacoes(linha_combinacoes,1),:) ; coord_pilares(combinacoes(linha_combinacoes,2),:)]; % isolar o par que falhou e voltar a testar este com novos parametros
    distBWpilares_par=sqrt((par_Q_falhou(2,1)-par_Q_falhou(1,1))^2+(par_Q_falhou(2,2)-par_Q_falhou(1,2))^2);
    angulo_alinha=atan2(par_Q_falhou(2,2)-par_Q_falhou(1,2),par_Q_falhou(2,1)-par_Q_falhou(1,1))*180/pi;
    if angulo_alinha < -90 % funçao para resolver o problemas dos angulos
        angulo_alinha= angulo_alinha+360;
    end
        
    switch sum(par_Q_falhou(:,4))
        case 0 % caso quandos os dois pilares sao ambos circulares
            %disp('3 pilares (KO rejeitar pois nada se pode concluir (redundante))')
            comb_OK_KO=0;
            caso_localiz=0;
            posicao_pilares=0;
            
        case 1
            %--------------------------------------------------------------
            % Detectou UM pilar RECTANGULAR e UM pilar CIRCULAR ( [] o ) ou ( o [] )   % (ver exemplo scan 750)
            [valor_x ind_x]=max( par_Q_falhou(:,1) );
            if par_Q_falhou(ind_x,4)==1
                configuracao=0; % ( o [] )
            else
                configuracao=1; % ( [] o )
            end        
            if abs(distBWpilares_par - 6900) < 6900*percentagem_error_dist % teste da distancia
                ang_teorico=0.8303;
                linha_rect=find(par_Q_falhou(:,4)==1);
                if abs(abs(angulo_alinha-par_Q_falhou(linha_rect,3)) - ang_teorico ) < tolerancia % teste do angulo
                    caso_localiz=9;
                    if  configuracao == 0
                       disp('o []')
                       comb_OK_KO=1;
                       %disp('3 pilares (OK tres pertencem (canto 1))')
                       posicao_pilares=0;
                   else
                       disp('[] o')
                       comb_OK_KO=1;
                       %disp('3 pilares (OK tres pertencem (canto 2))')
                       posicao_pilares=1;
                   end            
                else
                    comb_OK_KO=0;
                    %disp('3 pilares (KO rejeitar pois nada se pode concluir (redundante))')
                    caso_localiz=0;
                    posicao_pilares=0;
                end
            else
                comb_OK_KO=0;
                %disp('3 pilares (KO rejeitar pois nada se pode concluir (redundante))')
                caso_localiz=0;
                posicao_pilares=0;
            end            
            %--------------------------------------------------------------
           
        case 2
            %--------------------------------------------------------------
            % Detectou DOIS pilares RECTANGULARES ( [] [] )   % (ver exemplo scan 755)                    
            if abs(distBWpilares_par - 4669) < 4669*percentagem_error_dist % teste da distancia
                ang_teorico=133.26-90;
                caso_localiz=10;
                if abs(abs(angulo_alinha-par_Q_falhou(2,3)) - ang_teorico ) < tolerancia % teste do angulo
                    disp('[] []')
                    comb_OK_KO=1;
                    %disp('3 pilares (OK tres pertencem (canto 3))')
                    posicao_pilares=2;
                else
                    comb_OK_KO=0;
                    %disp('3 pilares (KO rejeitar pois nada se pode concluir (redundante))')
                    caso_localiz=0;
                    posicao_pilares=0;
                end
            else
                 ang_teorico= 0;
                 comb_OK_KO=0;
                 %disp('3 pilares (KO rejeitar pois nada se pode concluir (redundante))')
                 caso_localiz=0;
                 posicao_pilares=0;
             end
             %--------------------------------------------------------------
    end
   
%-------------------------------------------------------------------------------    
 combinacoes(linha_combinacoes,3:5)=[comb_OK_KO posicao_pilares caso_localiz];    
 if comb_OK_KO == 1 % se for verdade significa que os tres efectivamente pertencem (CASO 3 ONDE TODOS OS PILARES PERTENCEM E TEM DE SE ELIMINAR O MAIS AFASTADO) 
    distancia_pil(1,1)=sqrt(coord_pilares(1,1)^2+coord_pilares(1,2)^2);
    distancia_pil(1,2)=sqrt(coord_pilares(2,1)^2+coord_pilares(2,2)^2);
    distancia_pil(1,3)=sqrt(coord_pilares(3,1)^2+coord_pilares(3,2)^2);
    [distancia_pil_max, index_distancia_pil_max] = max(distancia_pil);
    coord_pilares(index_distancia_pil_max,:)=[];% eliminaçao do pilar mais distante

    matrix_test=combinacoes(:,1:2)==[index_distancia_pil_max index_distancia_pil_max;index_distancia_pil_max index_distancia_pil_max;index_distancia_pil_max index_distancia_pil_max];
    matrix_test_sum=sum(matrix_test,2);
    linha_pilares_ok=find(matrix_test_sum == 0);

    coord_loc_robot = LOCALIZACAO_CASOS(coord_pilares, combinacoes(linha_pilares_ok,5), combinacoes(linha_pilares_ok,4)) 
end
%-------------------------------------------------------------------------------    
