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

% funçao para encontrar segmentos de recta
%----------------------------------------------------------
function [segmento,line_segment]=SEGMENTSFINDER(cart_data,first_last,segmento,line_segment)
     
   [indice,distancia_indice,line_segment,segmento]=DIST2SEGMENT(cart_data,first_last,segmento,line_segment); % funçao que calcula a distancia de cada ponto ao segmento e verifica se estes pertencem todos ao mesmo segmento ( se sim incrementa 1 a line_segment senao nao faz nada ao line_segment)
   
   % (1)start--------------------------------------------------------------
   if not(isempty(indice))
       
       % (2.1)start--------------------------------------------------------------
       % Verificaçao se o ponto extremo é ou nao o ponto maximo ou minimo em relacao ao robot de entre um conjunto de + ou - 1 pontos 
       dist_i=sqrt(cart_data(indice,1)^2+cart_data(indice,2)^2); % calculo da distancia entre o ponto e o robot
       dist_i1=sqrt(cart_data(indice+1,1)^2+cart_data(indice+1,2)^2); % calculo da distancia entre o ponto vizinho + 1 e o robot
       dist_i_1=sqrt(cart_data(indice-1,1)^2+cart_data(indice-1,2)^2); % calculo da distancia entre o ponto vizinho - 1 e o robot
       distancias_i=[dist_i_1 dist_i dist_i1]; % matriz das distancias dos pontos ao robot
       
       % Agora é necessario verificar se estamos a ver um objecto convexo ou concavo (todos os pilares Rectangulares sao CONVEXOS)
       
       % TESTE DE CONVEXIDADE - O parametro distancia_indice é a distancia entre a recta definida entre os extremos e o ponto mais afastado
       %    - se este valor for negativo implica que o poligono em causa é concavo
       %    - se este valor for positivo implica que o poligono em causa é convexo
       if distancia_indice < 0
            %se for CONVEXO deve-se excolher o extremo com o valor minimo (mais perto)
            [valo indi]=min(distancias_i);
            
            % para garantir que os pontos em consideraçao pertencem feature à em causa
            % (indi pode tomar o valor [1 2 3] correspondendo aos pontos [original-1 original original+1])
            if and((indice+indi-2) <= first_last(2)-1, (indice+indi-2) >= first_last(1)+1)
                 indice=indice+indi-2;
            end
       else
            %se for CONCAVO deve-se escolher o extremo com o valor maximo (mais afastado)
            [valo indi]=max(distancias_i);
            
            % para garantir que os pontos em consideraçao pertencem feature à em causa
            if and((indice+indi-2) <= first_last(2)-1, (indice+indi-2) >= first_last(1)+1)
                indice=indice+indi-2;
            end
       end
       % (2.1)end-----------------------------------------------------------------
   
       % (2.2)start---------------------------------------------------------------
       for k2=1:2 %ciclo para testar os dois segmentos resultantes da separaçao de 1
           
           %local onde se define o primeiro e o ultimo ponto do proximo segmento a testar
           first=first_last(k2);
           last=indice;
           if first > last
               last=first_last(k2);
               first=indice;  
           end
           
           % condiçao para verificar se existem pontos intermedios entre os dois pontos extremos do segmento, se nao existir nenhum ou
           % seja a recta e definido simplesmente pelos extremos salta fora do ciclo
           if last-first == 1
               segmento(line_segment,:)=[first last last-first];
               line_segment=line_segment+1;
           else
               [segmento,line_segment]=SEGMENTSFINDER(cart_data,[first last last-first],segmento,line_segment);
           end
           
       end            
       % (2.2)end-----------------------------------------------------------------
 
   end
   % (1)end----------------------------------------------------------------