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

% Modelo 1  - Caso 4    ( o [] ) distanciados 4883.5
%---------------------------------------------------------------------
function [coord_loc_robot] = LOCALIZACAO_CASO1_4(coord_pilares,posicao_pilares)

[r1, r2] = DIST_ROBOT2PILAR(coord_pilares); % calcula da distancia do robot aos pilares
angulo_1=atan2(coord_pilares(1,2),coord_pilares(1,1));
angulo_2=atan2(coord_pilares(2,2),coord_pilares(2,1));

   
if and( angulo_1 > angulo_2 , posicao_pilares == 0 ) % onde se define de que lado se encontra o robot (isto pq se sabe que a 1ª linha do coord_pilares é um pilar circular) 
   
    % lado direito
    %x2_coord=3300;
    %y2_coord=23300;    
    %x1_coord=0;
    %y1_coord=19800;
       
    coord_loc_robot(1,1) =[ 1650+33/462800*r1^2-33/462800*r2^2+7/92560*(-r1^4+46280000*r1^2+2*r2^2*r1^2-535459600000000+46280000*r2^2-r2^4)^(1/2)];
    coord_loc_robot(1,2) =[ 7/92560*r1^2+21550-7/92560*r2^2-33/462800*(-r1^4+46280000*r1^2+2*r2^2*r1^2-535459600000000+46280000*r2^2-r2^4)^(1/2)];
else
    
    % lado esquerdo
    %x2_coord=-3300;
    %y2_coord=23300;
    %x1_coord=0;
    %y1_coord=19800;
       
    coord_loc_robot(1,1) =[ -1650-33/462800*r1^2+33/462800*r2^2-7/92560*(-r1^4+46280000*r1^2+2*r2^2*r1^2-535459600000000+46280000*r2^2-r2^4)^(1/2)];
    coord_loc_robot(1,2) =[ 7/92560*r1^2+21550-7/92560*r2^2-33/462800*(-r1^4+46280000*r1^2+2*r2^2*r1^2-535459600000000+46280000*r2^2-r2^4)^(1/2)];
end

    

   
