//  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 [varargout] = CL_man_dvSmaEccv(smai, ecci, pomi, smaf, eccf, pomf, res, mu)
// Change of semi major axis and excentricity vector (eccentric orbits)
//
// Calling Sequence
// [deltav, dv, pso] = CL_man_dvSmaEccv(smai, ecci, pomi, smaf, eccf, pomf [, res="opt"] [, mu])
// [dva, psoa, dvb, psob, numsol] = CL_man_dvSmaEccv(smai, ecci, pomi, smaf, eccf, pomf, res="all", [, mu])
// [man] = CL_man_dvSmaEccv(smai, ecci, pomi, smaf, eccf, pomf, res="s", [, mu])
//
// Description
// <itemizedlist><listitem>
// <p>Computes the maneuver that simultaneously changes the semi-major axis and the eccentricity 
// vector (eccentricity and argument of periapsis). </p>
// <p></p>
// <p>The orbits 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>There can be 0, 1 or 2 solutions. </p>
// <p>If the initial and final orbits don't intersect, no solution is returned. </p> 
// <p>If <b>res</b> is equal to "opt", the solution returned is the one with the smallest deltav norm. </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, all the results are set to %nan (except numsol). </p>
// <p> - If there is 1 solution, the results for the second maneuver (solution "b") are set to %nan. </p>
// <p> - If there are 2 solutions, solution "a" is the one with the smallest |deltav|. </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// smai: Semi major axis of initial orbit [m] (1xN or 1x1)
// ecci:  Excentricity of initial orbit (1xN or 1x1)
// pomi: Argument of periapsis of initial orbit [rad] (1xN or 1x1)
// smaf: Semi major axis of final orbit [m] (1xN or 1x1)
// eccf:  Excentricity of final orbit (1xN or 1x1)
// pomf: Argument of periapsis of final orbit [rad] (1xN or 1x1)
// res: (string, optional) Type of output: "opt", "all" or "s" 
// mu : (optional) Gravitational constant. [m^3/s^2] (default value is %CL_mu)
// deltav: Norm of velocity increment (minimum cost) [m/s] (1xN)
// dv, dva: Velocity increment (cartesian coordinates) in "qsw" local frame [m/s] - solution "a" (3xN)
// pso, psoa: True argument of latitude [rad] - solution "a" (1xN)
// dvb: Velocity increment (cartesian coordinates) in "qsw" local frame [m/s] - solution "b" (3xN)
// psob: True argument of latitude [rad] - solution b (1xN)
// numsol: Number of solutions: 0, 1, or 2 (1xN)
//
// Authors
// CNES - DCT/SB
// 
// See also
// CL_gm_intersectCoplanOrb
//
// Examples
// // Define 2 elliptical orbits that intersect
// [smai, ecci] = CL_op_rarp2ae(7378.e3, 6878.e3); 
// [smaf, eccf] = CL_op_rarp2ae(7378.e3, 6868.e3); 
// pomi = 0; 
// pomf = CL_deg2rad(1); 
// 
// // Compute maneuver (minimal cost)
// [deltav, dv, pso] = CL_man_dvSmaEccv(smai, ecci, pomi, smaf, eccf, pomf)
// 
// // Check result
// [posi, veli] = CL_oe_kep2car([smai; ecci; 0; pomi; 0; CL_kp_v2M(ecci, pso-pomi)]); 
// [posf, velf] = CL_oe_kep2car([smaf; eccf; 0; pomf; 0; CL_kp_v2M(eccf, pso-pomf)]); 
// posf - posi // => 0
// dv - (CL_fr_inertial2qsw(posi,veli,velf) - CL_fr_inertial2qsw(posi,veli,veli)) // => 0  
//

// Declarations:

// Velocity vector in "qsw" local frame
// anv = true anomaly
// all arguments (sma, ecc, anv): same size
// Excentric orbits only. 
function [vel_qsw] = dvSmaEccv_velqsw(sma, ecc, anv, mu)
  // orbit parameter 
  p = sma .* (1 - ecc.^2);
  // norm of radius vector
  r = p ./ (1 + ecc .* cos(anv));  
  // constant of areas
  C = sqrt(p * mu); 
  // velocity * cos(slope), velocity * sin(slope) 
  Vcosgamma = C ./ r; 
  Vsingamma = ecc .* sin(anv) .* sqrt(mu ./ p); 
  // velocity vector in qsw local frame
  vel_qsw = [Vsingamma; Vcosgamma; zeros(Vsingamma)];
  I = find(isnan(r)); 
  vel_qsw(:,I) = %nan; 
endfunction

    
// Code
if (~exists("mu", "local")); mu = CL__dataGetEnv("mu"); end
if (~exists("res","local")); res = "opt"; end

if (typeof(res) <> "string" | size(res,"*") <> 1) 
  CL__error("Invalid type or size for argument res"); 
end

// check "res" and number of output arguments
nbargsout = argn(1); 
RES = ["opt", "all", "s"]; 
// max number of output arguments 
NBARGS = [3, 5, 1]; 
I = find(res == RES); 
if (I == [])
  CL__error("Invalid value for argument res"); 
end
if (nbargsout > NBARGS(I)) 
  CL__error("Invalid number of output arguments"); 
end

// checks arguments sizes are OK / resizes
[smai, ecci, pomi, smaf, eccf, pomf, N] = CL__checkInputs(smai,1, ecci,1, pomi,1, smaf,1, eccf,1, pomf,1); 

// check no input argument is [], or all are
Nr = size([smai; ecci; pomi; smaf; eccf; pomf], 1); 

if (~(Nr == 0 | Nr == 6))
  CL__error("Invalid arguments sizes (possibly empty inputs)"); 
end

// check validity of arguments
if (find(smai <= 0 | smaf <= 0 | ecci < 0 | eccf < 0) <> [])
  CL__error("Invalid arguments (incorrect values for sma or ecc)"); 
end

if (Nr == 0)  
  deltav = []; 
  dva = [];
  dvb = [];
  psoa = []; 
  psob = []; 
  numsol = 0;

else
  // Find the intersection between the orbits
  // %nan if solution (psoa and/or psob) does not exist
  [psoa, psob, numsol] = CL_gm_intersectCoplanOrb(smai, ecci, pomi, smaf, eccf, pomf); 
  
  // invalidate solutions if orbits are not eccentric
  I = find(ecci >= 1 | eccf >= 1)
  pso(I) = %nan; 
  psob(I) = %nan; 
  numsol(I) = 0; 

  // solution a (may be %nan)
  // dva = velocity increment in "qsw" of initial orbit
  [velqswi] = dvSmaEccv_velqsw(smai, ecci, psoa - pomi, mu); 
  [velqswf] = dvSmaEccv_velqsw(smaf, eccf, psoa - pomf, mu); 
  dva = velqswf - velqswi; 
  
  // solution b (may be %nan) 
  // dvb = velocity increment in "qsw" of initial orbit
  [velqswi] = dvSmaEccv_velqsw(smai, ecci, psob - pomi, mu); 
  [velqswf] = dvSmaEccv_velqsw(smaf, eccf, psob - pomf, mu); 
  dvb = velqswf - velqswi; 

  [deltav, k] = min(CL_norm(dva), CL_norm(dvb)); 
  
  I = find(k == 2); 
  if (I <> [])
    // swap solutions a and b
    [psoa(I), psob(I)] = (psob(I), psoa(I)); 
    [dva(:,I), dvb(:,I)] = (dvb(:,I), dva(:,I)); 
  end
end

if (res == "opt")
  varargout = list(deltav, dva, psoa); 
elseif (res == "all")
  varargout = list(dva, psoa, dvb, psob, numsol); 
else
  varargout = list(struct("deltav", deltav, "dva", dva, "psoa", psoa, ..
                          "dvb", dvb, "psob", psob, "numsol", numsol)); 
end

endfunction