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

% Modelo 1  - Caso 7    ( o [] ) distanciados 6328.5
%---------------------------------------------------------------------
function [coord_loc_robot] = LOCALIZACAO_CASO1_7(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=0;
    %x1_coord=0;
    %y1_coord=5400;
       
    coord_loc_robot(1,1) =[ 1650-11/267000*r2^2+11/267000*r1^2-3/44500*(-r2^4+2*r2^2*r1^2+80100000*r2^2-r1^4+80100000*r1^2-1604002500000000)^(1/2)];
    coord_loc_robot(1,2) =[ 3/44500*r2^2-3/44500*r1^2+2700-11/267000*(-r2^4+2*r2^2*r1^2+80100000*r2^2-r1^4+80100000*r1^2-1604002500000000)^(1/2)];
else
    
    % lado esquerdo
    %x2_coord=-3300;
    %y2_coord=0;    
    %x1_coord=0;
    %y1_coord=5400;
       
    coord_loc_robot(1,1) =[ -1650+11/267000*r2^2-11/267000*r1^2+3/44500*(-r2^4+2*r2^2*r1^2+80100000*r2^2-r1^4+80100000*r1^2-1604002500000000)^(1/2)];
    coord_loc_robot(1,2) =[ 3/44500*r2^2-3/44500*r1^2+2700-11/267000*(-r2^4+2*r2^2*r1^2+80100000*r2^2-r1^4+80100000*r1^2-1604002500000000)^(1/2)];
end