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

% funçao para calcular o ponto de intercepçao de duas rectas 
%----------------------------------------------------------

function [ang_1_lado, ang_2_lado]=BISSECTRIZeORIENT(cart_data, segmento, indexofconsider, k5, param_rectas)

        % Calculo da bissectriz do angulo entre as duas rectas para se saber a ORIENTAÇAO do pilar 
        angrect1_e=atan2(cart_data(segmento(indexofconsider(k5),1),2)-cart_data(segmento(indexofconsider(k5),2),2),...
        cart_data(segmento(indexofconsider(k5),1),1)-cart_data(segmento(indexofconsider(k5),2),1))*180/pi;
        angrect2_e=atan2(cart_data(segmento(indexofconsider(k5)+1,2),2)-cart_data(segmento(indexofconsider(k5)+1,1),2),...
        cart_data(segmento(indexofconsider(k5)+1,2),1)-cart_data(segmento(indexofconsider(k5)+1,1),1))*180/pi;
        
        angrect1_r=atan(param_rectas(1,1))*180/pi;
        angrect2_r=atan(param_rectas(2,1))*180/pi; 
    
        angrect1=atan(param_rectas(1,1));
        angrect2=atan(param_rectas(2,1));
                               
        % forma de resolver o problema da funçao TAN (1º e 4º quadrante)
        if and(angrect1_r < angrect1_e + 30 , angrect1_r > angrect1_e - 30)
            % angulo ok
        else
            angrect1=angrect1+pi;
        end
    
        if and(angrect2_r < angrect2_e + 30 , angrect2_r > angrect2_e - 30)
            % angulo ok
        else
            angrect2=angrect2+pi;
        end

%         %Robot do lado ESQUERDO----------------------
        ang_1_lado=angrect1;
%         %calculos para representaçao grafica do pilar rectangular detectado
%         ang_2_lado=ang_1_lado+90*pi/180;
%     
%         x_1=x_int+500*cos(ang_1_lado);
%         y_1=y_int+500*sin(ang_1_lado);
%         x_2=x_int+200*cos(ang_2_lado);
%         y_2=y_int+200*sin(ang_2_lado);
%         x_3=x_2+500*cos(ang_1_lado);
%         y_3=y_2+500*sin(ang_1_lado);
%     
%     
%         hold on
%         plot([x_int x_1 x_3 x_2 x_int],[y_int y_1 y_3 y_2 y_int],'g')
%         %-------------------------------------------

%         %Robot do lado DIREITO----------------------
        ang_2_lado=angrect2;
%         %calculos para representaçao grafica do pilar rectangular detectado
%         ang_1_lado=ang_2_lado-90*pi/180;
%         
%         x_1=x_int+200*cos(ang_1_lado);
%         y_1=y_int+200*sin(ang_1_lado);
%         x_2=x_int+500*cos(ang_2_lado);
%         y_2=y_int+500*sin(ang_2_lado);
%         x_3=x_2+200*cos(ang_1_lado);
%         y_3=y_2+200*sin(ang_1_lado);
% 
%         hold on
%         plot([x_int x_1 x_3 x_2 x_int],[y_int y_1 y_3 y_2 y_int],'g')