//  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 [dpos, dvel] = CL_op_orbGapLof(type_oe, oe, doe, loc_frame, meth, motion, mu)
// Gaps in local orbital frame
//
// Calling Sequence
// [dpos, dvel] = CL_op_orbGapLof(type_oe, oe, doe, loc_frame [, meth, motion, mu])
//
// Description
// <itemizedlist>
// <listitem>
// <p>Transforms gaps in orbital elements into gaps in position and velocity in 
// the specified local orbital frame. </p>
// <p>The available calculation methods are: </p>
// <p> - <b>f</b>, <b>f0</b>, <b>f1</b>: use of linearized expressions
//(See <link linkend="CL_op_orbGapLofMat">CL_op_orbGapLofMat</link> for details) </p>
// <p> - <b>c</b>: Numerical computation (no explicit formulas), valid for any orbit type and any local frame </p>
// <p></p>
// <p><b>Notes</b>: </p>
// <p> - Relative motion is not available yet. </p>
// </listitem>
// <listitem>
// <p>See <link linkend="Local frames">Local frames</link> for more details on the definition of local frames.</p>
// <p>See <link linkend="Orbital elements">Orbital elements</link> for more details on orbital elements.</p>
// </listitem>
// </itemizedlist>
//
// Parameters
// type_oe: (string) Type of orbital elements (1x1)
// oe: Reference orbital elements (6xN or 6x1)
// doe: Gaps in orbital elements (6xN or 6x1)
// loc_frame: (string) Name of local orbital frame (1x1)
// meth: (string, optional) Method used: "f", "f1", "f0" or "c". Default is "c"
// motion: (optional, string) "abs" = absolute motion, "rel" = relative motion. Default is "abs"
// mu : (optional) Gravitational constant [m^3/s^2]. Default value is %CL_mu
// dpos: Gaps in position, components in local frame. [m] (3xN) 
// dvel: Gaps in (inertial) velocity, components in local frame. [m/s] (3xN)
//
// Authors
// CNES - DCT/SB
//  
// Bibliography
// 1) Casotto S, Position and velocity perturbation in the orbital frame in terms of classical elements perturbation, 1991.
// 
// See also
// CL_op_orbGapLofMat
// CL_fr_locOrbMat
// 
// Examples
// // Reference orbital elements (circular type)
// cir = [7000.e3; 1.e-5; 1.e-3; 1; 2; 3]; 
// // Increments in orbital elements 
// dcir = [100; 5.e-6; 8.e-6; 2.e-5; 5.e-5; 8.e-5]; 
//
// // Formula, order 0
// [dpos, dvel] = CL_op_orbGapLof("cir", cir, dcir, "qsw", meth = "f0") 
// // Numerical
// [dpos, dvel] = CL_op_orbGapLof("cir", cir, dcir, "qsw") 

// Declarations:

// ----------------------------------------------
// linearized - use of jacobian
// oe and doe: 6xN
// type_oe, loc_frame, meth: string (1x1)
// ----------------------------------------------
function [dpos, dvel] = orbGapLof_f(type_oe, oe, doe, loc_frame, meth, mu)   

  // validity check done by called function 
  // matrix in "unnormalized" form
  M = CL_op_orbGapLofMat(type_oe, oe, loc_frame, meth, "abs", "un", mu);
  
  // gaps in position/velocity
  dpv = M * doe; 
  dpos = dpv(1:3,:);  
  dvel = dpv(4:6,:);  
  
endfunction

// ----------------------------------------------
// direct computation - any frame - any type of orbital element
// oe and doe: 6xN
// type_oe, loc_frame: string (1x1)
// ----------------------------------------------
function [dpos, dvel] = orbGapLof_c(type_oe, oe, doe, loc_frame, mu)   
  // check validity of orbital elements 
  if (~CL__oe_isValid(type_oe, oe, mu)) 
    CL__error("Invalid orbital elements"); 	
  end

  pv   = CL_oe_convert(type_oe, "pv", oe, mu); 
  pv2  = CL_oe_convert(type_oe, "pv", oe + doe, mu);
  
  M = CL_fr_locOrbMat(pv(1:3,:), pv(4:6,:), loc_frame);
  
  dpos = M * (pv2(1:3,:) - pv(1:3,:)); 
  dvel = M * (pv2(4:6,:) - pv(4:6,:)); 
  
endfunction


// ==============================================
// Argument Checking
// ==============================================

// optional (or mandatory) arguments
if (~exists("loc_frame", "local")); CL__error("Missing argument: loc_frame"); end;
if (~exists("meth", "local")); meth = "c"; end;
if (~exists("motion", "local")); motion = "abs"; end;
if (~exists("mu", "local")); mu = CL__dataGetEnv("mu"); end

// Check sizes and types
for (arg = [meth, type_oe, loc_frame, motion])
  if (size(arg, "*") <> 1 | typeof(arg) <> "string")
    CL__error("Invalid type or size for argument " + arg); 
  end
end

// only "abs" for now (later: "abs" and "rel")
if (~or(motion == ["abs"]))
  CL__error("Invalid value for argument motion (relative motion not available)"); 
end

// check method value (other arguments checked elsewhere)
if (~or(meth == ["f", "f0", "f1", "c"]))
  CL__error("Invalid value for argument meth"); 
end

// Check/resize arguments 
[oe, doe, N] = CL__checkInputs(oe, 6, doe, 6); 

// special case: empty matrices
// Note: values of some arguments (type_oe, frame): not checked ! 
if (N == 0)
  dpos = []; 
  dvel = []; 
  return; 
end


// ==============================================
// Main code
// ==============================================

if (meth == "f" | meth == "f1" | meth == "f0")
  // analytical
  [dpos, dvel] = orbGapLof_f(type_oe, oe, doe, loc_frame, meth, mu); 

else // meth == c
  // numerical
  [dpos, dvel] = orbGapLof_c(type_oe, oe, doe, loc_frame, mu);

end

endfunction