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

% teste para poder ser candidato a pilar rectangular 
%----------------------------------------------------------

function [coord_rect]=TESTCANDIDATOPILARRECT(RECT_feature_bit_control, feature_pontos,data, cart_data, coord_rect)

    % (1)start ---- define ----
    min_pts_featureR=4; % valor minimo de pontos que devera ter uma feature (PorExe: se min_pts_featureR = 4 implica que a feature tenha 4+1 pontos, ou seja 5 pontos) 
    factor_dist_extremos=1.7; % factor de distancia toleravel entre os 4 pontos extremos de uma feature
    min_pontos_segm=2; % numero minimo de pontos para que um segmento seja levado em conta, ou seja de forma a nao ser rejeitado nos diferentes testes
    int_angbwrecta=[82 101]; % intervalo para o valor dos angulos admissiveis para poderem ser considerados como possiveis pilares rectangulares 
    segm_maior=[423 550]; % intervalo de tolerancia que podemos levar em conta no segmento de MAIOR dimensao
    segm_menor=[148 214]; % intervalo de tolerancia que podemos levar em conta no segmento de MENOR dimensao
    % (1)end ------------------

    % (2)start -------------------Teste para candidato a pilar rectangular------------------------------
    if RECT_feature_bit_control ~= 1 % condiçao necessaria para o caso de nao serem identificados nenhuma feature (se for =1 salta fora nao existem features a analisar)
                
        feature_pontos(:,3)=feature_pontos(:,2)-feature_pontos(:,1); % calculo da dimensao da feature (em termos de pontos)
        feature_pontos(find(feature_pontos(:,3) < min_pts_featureR),:)=[];% todas as features que tiverem menos de min_pts_featureR+1 pontos sao eliminadas
        
        % (2.1)start ----------Teste de verificaçao de deteccao de pilares rectangulares----------
        if isempty(feature_pontos) 
            % caso onde nao é detectado nenhum pilar rectangular
        else % (2.1) quando se detecta um ou mais pilares rectangulares
            
            [feature_pontos]=EXTREME_PROBLEM(feature_pontos, factor_dist_extremos, cart_data); % Funçao para resolver o problema de indefiniçao dos pontos extremos (problema de integaçao da luz reflectida)
            feature_pontos(:,3)=feature_pontos(:,2)-feature_pontos(:,1); % processo de identificaçao de pilares rect (processo ja realizado anteriormente mas repetido devido às alteraçoes que podem advir das operaçoes anteriores)               
       
            % (2.1.1)start ------ Local de analise efectiva de um dos candidatos a pilar rectangular para as diferentes features ------
            for i5=1:size(feature_pontos(:,1)) % a analise de cada feature é feita individualmente
               
                line_segment=1; %variavel auxiliar para ordenar os segmentos encontrados
                segmento=zeros(20,3); % matriz de tamanho fixo para identificaçao de todos os segmentos de uma feature (1ºcol-inicio; 2ºcol-final; 3ºcol-comprimento)
                [segmento,line_segment] = SEGMENTSFINDER(cart_data,feature_pontos(i5,:),segmento,line_segment); %analisa uma feature de cada vez
               
                % Analise dos segmentos encontrados
                segmento(find(segmento(:,3)==0),:)=[]; % para procurar o primeiro elemento zero e depois eliminar tudo o resto
                segmento(find(segmento(:,3)==min_pontos_segm-1),:)=[]; % Eliminaçao/rejeiçao de todos os segmentos que sao somente definidos por dois pontos, ou seja, pelos seus extremos garantindo assim que um segmento tenha pelo menos "min_pontos_segm"
                                        
                tamanho=size(segmento);
                if tamanho(1) >= 2 % se tiver 2 ou mais segmentos ENTRA senao salta fora
                                   
                    [segmrecta,angbwrectas_r]=SEGMENT_PARAMETERS(cart_data,tamanho,segmento);
                    % teste para encontrar os segmentos de recta que tenham um angulo entre eles dentro do intervalo int_angbwrecta
                    indexofconsider=find(and(angbwrectas_r > int_angbwrecta(1,1) ,angbwrectas_r < int_angbwrecta(1,2)));
                    tam=size(indexofconsider);

                    for k5=1:tam(2)
      
                        [segm1_compr, segm2_compr]=COMPRI_SEGM(cart_data, segmento, indexofconsider, k5); % funçao que calcula o comprimento dos segmentos de recta                        
                        % (2.1.1.1)Start---------19.07----------------------------
                        % Local de selecçao da ORIENTAÇAO do robot na passadeira (se os pilares estao do lado esquerdo ou direito)
                        if and( and(segm1_compr > segm_maior(1,1), segm1_compr < segm_maior(1,2)) , and(segm2_compr > segm_menor(1,1),segm2_compr < segm_menor(1,2)) ) % Robot do lado ESQUERDO 
                            
                            [distancia]=DIST4CONV(cart_data, segmento, indexofconsider, k5); % funçao para o calculo da distancias entre pontos para classificaçao da feature em relaçao à sua convexidade            
                            if distancia < 0 % Verdadeiro CONVEXO / Falso CONCAVO
                                %~~~~~~~~~~~Start 19.08~~~~~~~~~~
                                [x_int, y_int, param_rectas]=PT_INTERCEP2RECTAS(segmrecta, indexofconsider, k5); % Definiçao do ponto de intercepçao entre as rectas obtidas por regressao linear 
                                [ang_1_lado, ang_2_lado]=BISSECTRIZeORIENT(cart_data, segmento, indexofconsider, k5, param_rectas); % Calculo da orientaçao do pilar
                                coord_rect(i5,:)=[x_int y_int (ang_1_lado)*180/pi 1];
                                %~~~~~~~~~~~~~End 19.08~~~~~~~~~~                                               
                            else
                                 
                            end
                                                                                    
                        else
                            
                            if and( and(segm1_compr > 148,segm1_compr < 214) , and(segm2_compr > 423,segm2_compr < 550) )% Robot do lado DIREITO 

                                [distancia]=DIST4CONV(cart_data, segmento, indexofconsider, k5); % funçao para o calculo da distancias entre pontos para classificaçao da feature em relaçao à sua convexidade
                                if distancia < 0 % Verdadeiro CONVEXO / Falso CONCAVO
                                    %~~~~~~~~~~~Start 19.08~~~~~~~~~~
                                    [x_int, y_int, param_rectas]=PT_INTERCEP2RECTAS(segmrecta, indexofconsider, k5); % Definiçao do ponto de intercepçao entre as rectas obtidas por regressao linear 
                                    [ang_1_lado, ang_2_lado]=BISSECTRIZeORIENT(cart_data, segmento, indexofconsider, k5, param_rectas); % Calculo da orientaçao do pilar
                                    coord_rect(i5,:)=[x_int y_int (ang_2_lado)*180/pi 1];                    
                                    %~~~~~~~~~~~~~End 19.08~~~~~~~~~~                                               
                                end
                                               
                            else
                                % Não é PILAR RECTANGULAR
                            end
                            
                        end                   
                        %(2.1.1.1)end ---------19.07----------------------------
                    end % for k5=1:tam(2)
                end % if tamanho(1) >= 2
            end
            % (2.1.1)end --------------------------------------------------------------------
        end
        % (2.1)end --------------------------------------------------------------------------------
    end
    % (2)end -----------------------------------------------------------------------------------------
    
