//  Copyright (c) CNES  2008
//
//  This software is part of CelestLab, a CNES toolbox for Scilab
//
//  This software is governed by the CeCILL  license under French law and
//  abiding by the rules of distribution of free software.  You can  use,
//  modify and/ or redistribute the software under the terms of the CeCILL
//  license as circulated by CEA, CNRS and INRIA at the following URL
//  'http://www.cecill.info'.

function [psoa, psob, numsol] = CL_gm_intersectCoplanOrb(sma1, ecc1, pom1, sma2, ecc2, pom2)
// Intersection of 2 coplanar orbits (ellipses or hyperbolas)
//
// Calling Sequence
// [psoa, psob, numsol] = CL_gm_intersectCoplanOrb(sma1, ecc1, pom1, sma2, ecc2, pom2)
//
// Description
// <itemizedlist><listitem>
// <p>Computes the argument of latitude (= true anomaly + arg of periapsis) of the intersection 
// of 2 coplanar orbits. </p> 
// <p></p>
// <p>The orbit are defined by 3 parameters: </p>
// <p><b>sma</b>: Semi major axis (positive), </p>
// <p><b>ecc</b>: Excentricity, </p>
// <p><b>pom</b>: Argument of periapsis = angle between some arbitrary direction in the orbit plane 
// and the periapsis. </p>
// <p></p>
// <p>The number of solutions (0, 1 or 2) is returned in <b>numsol</b>. </p>
// <p></p></listitem>
// <listitem>
// <p><b>Notes</b>:</p>
// <p> - The orbits can be of any type (elliptical or hyperbolic).  </p>
// <p> - If the number of intersections is infinite, no solution is returned (numsol == 0).  </p>
// <p> - If there are no solutions, both psoa and psob are set to %nan. </p>
// <p> - If there is 1 solution, it is returned in psoa, and psob is set to %nan. </p>
// <p> - If there are 2 solutions, psoa contains the one with the smaller radius. </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// sma1: Semi major axis of orbit 1 [m] (1xN or 1x1)
// ecc1:  Excentricity of orbit 1 (1xN or 1x1)
// pom1: Argument of periapsis of orbit 1 [rad] (1xN or 1x1)
// sma2: Semi major axis of orbit 2 [m] (1xN or 1x1)
// ecc2:  Excentricity of orbit 2 (1xN or 1x1)
// pom2: Argument of periapsis of orbit 2 [rad] (1xN or 1x1)
// psoa: Argument of latitude of 1st intersection (or %nan) [rad] (1xN)
// psob: Argument of latitude of 2nd intersection (or %nan) [rad] (1xN)
// numsol: Number of intersections (1xN)
//
// Authors
// CNES - DCT/SB
//
// Examples
// // Define 2 elliptical orbits with 2 intersections
// [sma1, ecc1] = CL_op_rarp2ae(10.e6,  8.e6); 
// [sma2, ecc2] = CL_op_rarp2ae(11.e6,  7.e6); // lower periapsis, higher apoapsis)
// pom1 = 0; // rad
// pom2 = 1; // rad
// [psoa, psob, numsol] = CL_gm_intersectCoplanOrb(sma1, ecc1, pom1, sma2, ecc2, pom2); 
// 
// // Check result
// res1 = CL_kp_characteristics(sma1, ecc1, [psoa, psob] - pom1); 
// res2 = CL_kp_characteristics(sma2, ecc2, [psoa, psob] - pom2); 
// res1.r - res2.r  // => 0
//

// Declarations:


// Code:
// margin on acos() 
EPS = 1.e-12; 

// checks arguments sizes are OK / resizes
[sma1, ecc1, pom1, sma2, ecc2, pom2] = CL__checkInputs(sma1,1, ecc1,1, pom1,1, sma2,1, ecc2,1, pom2,1); 

// check no input argument is [], or all are
Nr = size([sma1; ecc1; pom1; sma2; ecc2; pom2], 1); 

if (Nr <> 0 & Nr <> 6)
  CL__error("Invalid arguments (possibly empty inputs)"); 
end

if (Nr == 0)  
  psoa = []; 
  psob = []; 
  numsol = [];
  return;   // <= RETURN
end

I = find(sma1 <= 0 | sma2 <= 0 | ecc1 < 0 | ecc2 < 0); 
if (I <> [])
  CL__error("Invalid arguments (incorrect values for sma or ecc)"); 
end

// Orbit parameters: p = sma * (1 - ecc.^2); 
// Values are > 0 for hyperbolas and ellipses => absolute values
// (For hyperbolas, formula: sma*(1-ecc^2)/(1+ecc*cos(anv)) is valid if sma < 0)
// %nan for parabolic orbits (ecc == 1)
// 
p1 = abs(sma1 .* (1 - ecc1 .^2)); 
p2 = abs(sma2 .* (1 - ecc2 .^2)); 

I = find(p1 <= 0 | p2 <= 0); 
p1(I) = %nan; 
p2(I) = %nan; 


// Equation to be solved: 
// A * cos(pso) + B * sin(pso) = C
A = p2 .* ecc1 .* cos(pom1) - p1 .* ecc2 .* cos(pom2); 
B = p2 .* ecc1 .* sin(pom1) - p1 .* ecc2 .* sin(pom2); 
C = p1 - p2; 

// Solution
D = sqrt(A.^2 + B.^2); 
I = find(D == 0 | abs(C) > D .* (1 + EPS)); 
D(I) = %nan; 

theta = real(acos(C ./ D)); 
phi = atan(B, A); 

// because bug Scilab in versions <= 5.5.1
I = find(isnan(C + D)); 
theta(I) = %nan; 


psoa = CL_rMod(phi - theta, -%pi, %pi); 
psob = CL_rMod(phi + theta, -%pi, %pi); 

// theta close to 0 or %pi => only one solution kept
I = find(abs(C) > D .* (1 - EPS)); 
psob(I) = %nan; 


// check that true anomalies are acceptable (if hyperbolic orbits)
// true anomalies must be in [-anvinf, +anvinf] with anvinf = acos(-1/ecc)
// <=> 1 + e * cos(anv) > 0

den1a = 1 + ecc1 .* cos(psoa - pom1); 
den2a = 1 + ecc2 .* cos(psoa - pom2); 
den1b = 1 + ecc1 .* cos(psob - pom1); 
den2b = 1 + ecc2 .* cos(psob - pom2); 

I = find(den1a <= 0 | den2a <= 0); 
psoa(I) = %nan; 
I = find(den1b <= 0 | den2b <= 0); 
psob(I) = %nan; 


// Sort solutions: 
// Not %nan first and minimum radius first
I = find(isnan(psoa)); 
psoa(I) = psob(I); 
psob(I) = %nan; 

// Sort: minimum radius first
// radii for solution 1 (= radii for solution 2)
// minimum radius first 
ra = p1 ./ den1a; 
rb = p1 ./ den1b; 
I = find(ra > rb);
if (I <> [])
  [psoa(I), psob(I)] = (psob(I), psoa(I)); // swap values
  [ra(I), rb(I)] = (rb(I), ra(I)); // swap values... in case
end

// Number of solutions
numsol = zeros(psoa); 
I = find(~isnan(psoa) | ~isnan(psob)); 
numsol(I) = 1; // in fact means : at least 1
I = find(~isnan(psoa) & ~isnan(psob)); 
numsol(I) = 2; 

endfunction