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

% funçao que quantifica o numero de pilares para fazer a localizaçao
%-----------------------------------------------------------
function [coord_loc_robot] = QUANT_PILARES4LOCALIZACAO(coord_pilares, coord_loc_robot)

quant_pilares=size(coord_pilares); % quantifica o numero de pilares detectados no scan actual 
switch quant_pilares (1,1)
    
    case 0
        %disp('0 pilar (KO nao e possivel localizar-se)')
    case 1
        %disp('1 pilar (KO nao e possivel localizar-se)')
    case 2
        % ------------caso de 2 pilares-------------
        [comb_OK_KO, posicao_pilares, caso_localiz] = VALIDA_2PILARES(coord_pilares); % validaçao dos pilares aos pares
        if comb_OK_KO==1 % se a combinaçao for OK  
            [coord_loc_robot] = LOCALIZACAO_CASOS(coord_pilares, caso_localiz, posicao_pilares); % ver a que caso de localizaçao pertence
        end
        % ------------caso de 2 pilares-------------         
        
    case 3
        % ------------caso de 3 pilares-------------
        combinacoes=[1 2; 1 3; 2 3]; % combinaçoes possiveis duas a duas que vao ser testadas 
        for n11=1:length(combinacoes(:,1))% ciclo  para testar as combinaçoes dos pilares
             [comb_OK_KO, posicao_pilares, caso_localiz] = VALIDA_2PILARES( [coord_pilares(combinacoes(n11,1),:) ; coord_pilares(combinacoes(n11,2),:) ]);
             combinacoes(n11,3:5)=[comb_OK_KO posicao_pilares caso_localiz] ; % terceira coluna classifica se o par e correcto (1) ou nao (0)
        end     
        switch sum(combinacoes(:,3)) % teste das combinaçoes para verificar quantas é que foram consideradas validas
            
            case 0     
                %disp('3 pilares (KO nenhum pertence)'
            case 1
                %disp('3 pilares (OK dois pertencem)')
                linha_pilares_ok=find(combinacoes(:,3) == 1);
                [coord_loc_robot] = LOCALIZACAO_CASOS([coord_pilares(combinacoes(linha_pilares_ok,1),:); coord_pilares(combinacoes(linha_pilares_ok,2),:)], combinacoes(linha_pilares_ok,5), combinacoes(linha_pilares_ok,4));                  
            case 2
                [coord_loc_robot] = CASO_PARTICULAR_CANTO(combinacoes, coord_pilares, coord_loc_robot);
            case 3
                %disp('3 pilares (OK tres pertencem)')
                % calculo das distancias para verificar qual e o pilar mais afastado para posterior eliminaçao
                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
        % ------------caso de 3 pilares-------------
        
    otherwise
        % ----------caso de + de 3 pilares----------
        %disp('+ de 4 pilares (KO)')
        for w=1:quant_pilares(1,1)
            coord_pilares(w,5)=sqrt(coord_pilares(w,1)^2+coord_pilares(w,2)^2);
        end
        while quant_pilares(1,1) ~= 3
            [distancia_pil_max, index_distancia_pil_max] = max(coord_pilares(:,5));
            % eliminaçao do pilar mais distante para passarmos somente a ter tres pilares
            coord_pilares(index_distancia_pil_max,:)=[];
            quant_pilares=size(coord_pilares);
        end
        
        [coord_loc_robot] = QUANT_PILARES4LOCALIZACAO(coord_pilares, coord_loc_robot);
        % ----------caso de + de 3 pilares----------
    end
