//  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 [M] = CL_op_orbGapLofMat(type_oe, oe, loc_frame, meth, motion, form, mu)
// Orbital elements to pos/vel in local frame (Jacobian)
//
// Calling Sequence
// M = CL_op_orbGapLofMat(type_oe, oe, loc_frame, meth [, motion, form, mu])
//
// Description
// <itemizedlist>
// <listitem>
// <p>Computes the jacobian of the transformation from orbital elements to position and (inertial) velocity in 
// the specified local orbital frame. </p>
// <p></p>
// <p>The jacobian matrix can be normalized (form = "n") or unnormalized (form = "un"). The normalized form 
// corresponds to the transformation (for elliptical orbits only): </p>
// <p>[dpos/sma; dvel/(n*sma)] = M * [dsma/sma; ...], with n = mean motion. </p>  
// <p></p>
// <p>The available calculation methods are: </p>
// <p> - <b>c</b>: The jacobian is computed numerically (no explicit formulas), </p>
// <p> - <b>f</b>: The jacobian is given by explicit formulas (no approximation), </p>
// <p> - <b>f1</b>: Same as "f", but the formulas are developped to order 1 in eccentricity, </p>
// <p> - <b>f0</b>: Same as "f", but the formulas are developped to order 0 in eccentricity. </p>
// <p></p>
// <p><b>Notes</b>: </p>
// <p> - Methods f0, f1, f are only available for elliptical orbits. </p>
// <p> - Methods f0 and f1 are only available for "kep" or "cir" orbital elements and "tnw" or "qsw" local frames. </p>
// <p> - Relative motion is only available for "f0" methods. </p>
// <p></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)
// loc_frame: (string) Name of local orbital frame (1x1)
// meth: (string, optional) Method used: "f", "f1", "f0", "c". Default is "c" 
// motion: (optional, string) "abs" = absolute motion, "rel" = relative motion. Default is "abs"
// form: (optional) Matrix option:  "n" = normalized, "un" = unnormalized. Default is "n"
// mu : (optional) Gravitational constant [m^3/s^2]. Default value is %CL_mu
// M: Jacobian in normalized or unnormalized form (6x6xN) 
//
// 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_orbGapLof
// CL_fr_locOrbMat
// 
// Examples
// // Reference orbital elements (circular type)
// cir = [7000.e3; 1.e-5; 1.e-3; 1; 2; 3]; 
// // Normalized matrices
// M0 = CL_op_orbGapLofMat("cir", cir, "qsw", "f0") // order 0
// M  = CL_op_orbGapLofMat("cir", cir, "qsw") // reference


// Declarations:

// ----------------------------------------------
// Normalize / unnormalize matrix (elliptical orbits only) 
// form = "n" => norm, form = "un" => unnorm
// Normalization = following successive operations: 
// => rows 1-3: divided by sma (semi major axis)
// => rows 4-6: divided by n*a (mean velocity)
// => col 1: multipled by sma
// M: 6x6xN
// ----------------------------------------------
function [M] = orbGapLofM_conv(M, form, sma, mu)
  if (form <> "n" & form <> "un")
    CL__error("invalid value for form"); 
  end
  
  // mean velocity
  na = sqrt(mu ./ sma); 
  
  if (form == "n")
    sma = 1 ./ sma; 
	na = 1 ./ na; 
  end
  
  // NB: only diagonal terms in J are <> 0
  J = zeros(M);
  J(1,1,:) = 1 ./ sma; 
  for (k = 2:6)
    J(k,k,:) = 1; 
  end
  
  M = M * J; 
  
  // reuse J to save memory
  for (k=1:3)
    J(k,k,:) = sma; 
    J(k+3,k+3,:) = na;
  end

  M = J * M;

endfunction
  

// ----------------------------------------------
// Normalized jacobian
// General formula for elliptical orbits (kep, qsw) - No expansion in ecc 
// Formula from "Casotto" paper
// kep: 6xN
// M: 6x6xN
// NB: deg0: unused (added to have same interface as other functions)
// ----------------------------------------------
function [M] = orbGapLofM_kepqsw(kep, deg0)   
  N = size(kep, 2); 
  M = zeros(6,6,N); 

  // true anomaly
  anv = CL_kp_M2v(kep(2,:), kep(6,:)); 
  cosAnv = cos(anv);
  sinAnv = sin(anv);

  // true arg of latitude
  psov = anv + kep(4,:); 
  cosPso = cos(psov); 
  sinPso = sin(psov);
    
  ecc = kep(2,:); 
  pom = kep(4,:); 

  cosInc = cos(kep(3,:)); 
  sinInc = sin(kep(3,:)); 
  cospom = cos(pom); 
  sinpom = sin(pom); 
    
  q = sqrt(1 - kep(2,:).^2);
    
  // Norm of radius vector and dsma divided by sma
  Rsma = (1 - ecc.^2) ./ (1 + ecc .* cosAnv);
    
  // Delta position / sma (rows: 1-3)
  
  M(1,1,:) = Rsma; 
  M(1,2,:) = -cosAnv; 
  M(1,6,:) = (ecc ./ q) .* sinAnv; 
  
  M(2,2,:) = (1 + Rsma ./ (q.^2)) .* sinAnv; 
  M(2,4,:) = Rsma; 
  M(2,5,:) = Rsma .* cosInc; 
  M(2,6,:) = q ./ Rsma; 

  M(3,3,:) = Rsma .* sinPso; 
  M(3,5,:) = -Rsma .* sinInc .* cosPso; 

  // Delta velocity / na (rows: 4-6)

  M(4,1,:) = - ecc .* sinAnv ./ (2 * q); 
  M(4,2,:) = - sinAnv ./ (q .* Rsma); 
  M(4,4,:) = - (q ./ Rsma); 
  M(4,5,:) = - (q ./ Rsma) .* cosInc; 
  M(4,6,:) =  - (1 ./ (Rsma.^2)); 
  
  M(5,1,:) = - q ./ (2 * Rsma); 
  M(5,2,:) = (ecc + cosAnv) ./ (q.^3); 
  M(5,4,:) = (ecc .* sinAnv) ./ q; 
  M(5,5,:) = (ecc .* sinAnv .* cosInc) ./ q; 
    
  M(6,3,:) = (cosPso + ecc .* cospom) ./ q; 
  M(6,5,:) = ((sinPso + ecc .* sinpom) .* sinInc) ./ q; 
 
endfunction


// ----------------------------------------------
// Normalized jacobian
// approx formula for elliptical orbits (kep, qsw) 
// 1st order in ecc for all terms 
// deg0: %t for degree 0 expansion
// kep: 6xN
// M: 6x6xN
// ----------------------------------------------
function [M] = orbGapLofM_kepqsw1(kep, deg0)
  N = size(kep, 2); 
  M = zeros(6,6,N); 

  ecc = kep(2,:);
  pom = kep(4,:); 
  
  cosInc = cos(kep(3,:)); 
  sinInc = sin(kep(3,:)); 
  cosAnm = cos(kep(6,:));
  sinAnm = sin(kep(6,:));
  cosPso = cos(kep(6,:) + kep(4,:));
  sinPso = sin(kep(6,:) + kep(4,:));
      
  eps = ecc .* cosAnm; 
  delta = ecc .* sinAnm; 
  ex = ecc .* cos(pom); 
  ey = ecc .* sin(pom); 
    
  if (deg0)
    eps = 0; 
	delta = 0; 
	ecc = 0; 
	ex = 0; 
	ey = 0; 
  end
  
  // Delta position / sma (rows: 1-3)
  
  M(1,1,:) = 1 - eps; 
  M(1,2,:) = 2 * ecc - (1 + 2 * eps) .* cosAnm; 
  M(1,6,:) = delta; 
  
  M(2,2,:) = (2 + 3 * eps) .* sinAnm; 
  M(2,4,:) = 1 - eps; 
  M(2,5,:) = (1 - eps) .* cosInc; 
  M(2,6,:) = 1 + eps; 

  M(3,3,:) = -2 * ey + (1 + eps) .* sinPso; 
  M(3,5,:) = (2 * ex  - (1 + eps) .* cosPso) .* sinInc; 
                    
  // Delta velocity / na (rows: 4-6)
  
  M(4,1,:) = -delta / 2; 
  M(4,2,:) = -(1 + 3 * eps) .* sinAnm; 
  M(4,4,:) = -(1 + eps); 
  M(4,5,:) = -(1 + eps) .* cosInc; 
  M(4,6,:) = -(1 + 2 * eps); 
  
  M(5,1,:) = -(1 + eps) / 2; 
  M(5,2,:) = -ecc + (1 + 2 * eps) .* cosAnm; 
  M(5,4,:) = delta; 
  M(5,5,:) = delta .* cosInc; 
    
  M(6,3,:) = -ex + (1 + 2 * eps) .* cosPso; 
  M(6,5,:) = sinInc .* (-ey + (1 + 2 * eps) .* sinPso); 
 
endfunction
  

// ----------------------------------------------
// Normalized jacobian
// approx formula for elliptical orbits (kep, tnw) 
// 1st order in ecc for all terms 
// deg0: %t for degree 0 expansion
// kep: 6xN
// M: 6x6xN
// ----------------------------------------------
function [M] = orbGapLofM_keptnw1(kep, deg0)
  N = size(kep, 2); 
  M = zeros(6,6,N); 

  ecc = kep(2,:);
  pom = kep(4,:); 
  
  cosInc = cos(kep(3,:)); 
  sinInc = sin(kep(3,:)); 
  cosAnm = cos(kep(6,:));
  sinAnm = sin(kep(6,:));
  cosPso = cos(kep(6,:) + kep(4,:));
  sinPso = sin(kep(6,:) + kep(4,:));
      
  eps = ecc .* cosAnm; 
  delta = ecc .* sinAnm; 
  ex = ecc .* cos(pom); 
  ey = ecc .* sin(pom); 
    
  if (deg0)
    eps = 0; 
	delta = 0; 
	ecc = 0; 
	ex = 0; 
	ey = 0; 
  end

  // Delta position / sma (rows: 1-3)
  
  M(1,1,:) = delta; 
  M(1,2,:) = 2 * (1 + eps) .* sinAnm; 
  M(1,4,:) = 1 - eps; 
  M(1,5,:) = (1 - eps) .* cosInc; 
  M(1,6,:) = 1 + eps; 
  
  M(2,1,:) = -1 + eps; 
  M(2,2,:) = cosAnm; 
  M(2,4,:) = delta; 
  M(2,5,:) = delta .* cosInc; 

  M(3,3,:) = -2 * ey + (1 + eps) .* sinPso; 
  M(3,5,:) = (2 * ex  - (1 + eps) .* cosPso) .* sinInc; 
                    
  // Delta velocity / na (rows: 4-6)
  
  M(4,1,:) = -(1 + eps) / 2; 
  M(4,2,:) = -2 * ecc + (1 + 3 * eps) .* cosAnm; 
  M(4,6,:) = -delta; 
  
  M(5,1,:) = 0; 
  M(5,2,:) = (1 + 4 * eps) .* sinAnm; 
  M(5,4,:) = 1 + eps; 
  M(5,5,:) = (1 + eps) .* cosInc; 
  M(5,6,:) = 1 + 2 * eps; 
  
  M(6,3,:) = -ex + (1 + 2 * eps) .* cosPso; 
  M(6,5,:) = sinInc .* (-ey + (1 + 2 * eps) .* sinPso); 
 
endfunction


// ----------------------------------------------
// Normalized jacobian
// approx formula for elliptical orbits (cir, qsw) 
// 1st order in ex/ey for all terms 
// deg0: %t for degree 0 expansion
// cir: 6xN
// M: 6x6xN
// ----------------------------------------------
function [M] = orbGapLofM_cirqsw1(cir, deg0)   
  N = size(cir, 2); 
  M = zeros(6,6,N); 

  ex = cir(2,:); 
  ey = cir(3,:); 
  cosPso = cos(cir(6,:)); 
  sinPso = sin(cir(6,:)); 
  cosInc = cos(cir(4,:)); 
  sinInc = sin(cir(4,:)); 

  eps = ex .* cosPso + ey .* sinPso; 
  delta = ex .* sinPso - ey .* cosPso; 

  if (deg0)
    eps = 0; 
	delta = 0; 
	ex = 0; 
	ey = 0; 
  end
  
  // Delta position / sma (rows: 1-3)
  
  M(1,1,:) = 1 - eps; 
  M(1,2,:) = 2 * ex - (1 + 2 * eps) .* cosPso;
  M(1,3,:) = 2 * ey - (1 + 2 * eps) .* sinPso;  
  M(1,6,:) = delta; 
  
  M(2,2,:) = (2 + 3 * eps) .* sinPso - 5 * ey / 2; 
  M(2,3,:) = -(2 + 3 * eps) .* cosPso + 5 * ex / 2; 
  M(2,5,:) = (1 - eps) .* cosInc; 
  M(2,6,:) = 1 + eps; 

  M(3,4,:) = -2 * ey + (1 + eps) .* sinPso; 
  M(3,5,:) = (2 * ex - (1 + eps) .* cosPso) .* sinInc; 

  // Delta velocity / na (rows: 4-6)

  M(4,1,:) = - delta / 2; 
  M(4,2,:) = - (1 + 3 * eps) .* sinPso + ey / 2; 
  M(4,3,:) = (1 + 3 * eps) .* cosPso - ex / 2; 
  M(4,5,:) = - (1 + eps) .* cosInc; 
  M(4,6,:) =  - (1 + 2 * eps); 
  
  M(5,1,:) = - (1 + eps) / 2; 
  M(5,2,:) = -ex + (1 + 2 * eps) .* cosPso; 
  M(5,3,:) = -ey + (1 + 2 * eps) .* sinPso; 
  M(5,5,:) = delta .* cosInc; 
    
  M(6,4,:) = -ex + (1 + 2 * eps) .* cosPso; 
  M(6,5,:) = (-ey + (1 + 2 * eps) .* sinPso) .* sinInc; 
 
endfunction


// ----------------------------------------------
// Normalized jacobian
// approx formula for elliptical orbits (cir, tnw) 
// 1st order in ex/ey for all terms 
// deg0: %t for degree 0 expansion
// cir: 6xN
// M: 6x6xN
// ----------------------------------------------
function [M] = orbGapLofM_cirtnw1(cir, deg0)   
  N = size(cir, 2); 
  M = zeros(6,6,N); 

  ex = cir(2,:); 
  ey = cir(3,:); 
  ecc = sqrt(ex.^2 + ey.^2); 
  cosPso = cos(cir(6,:)); 
  sinPso = sin(cir(6,:)); 
  cosInc = cos(cir(4,:)); 
  sinInc = sin(cir(4,:)); 

  eps = ex .* cosPso + ey .* sinPso; 
  delta = ex .* sinPso - ey .* cosPso; 

  if (deg0)
    eps = 0; 
	delta = 0; 
	ex = 0; 
	ey = 0; 
  end
    
  // Delta position / sma (rows: 1-3)
  
  M(1,1,:) = delta; 
  M(1,2,:) = (2 + 2 * eps) .* sinPso - 3 * ey / 2;
  M(1,3,:) = -(2 + 2 * eps) .* cosPso + 3 * ex / 2;  
  M(1,5,:) = (1 - eps) .* cosInc;  
  M(1,6,:) = 1 + eps; 
  
  M(2,1,:) = -(1 - eps); 
  M(2,2,:) = cosPso; 
  M(2,3,:) = sinPso; 
  M(2,5,:) = delta .* cosInc; 

  M(3,4,:) = -2 * ey + (1 + eps) .* sinPso; 
  M(3,5,:) = (2 * ex - (1 + eps) .* cosPso) .* sinInc; 

  // Delta velocity / na (rows: 4-6)

  M(4,1,:) = - (1 + eps) / 2; 
  M(4,2,:) = (1 + 3 * eps) .* cosPso - 2 * ex; 
  M(4,3,:) = (1 + 3 * eps) .* sinPso - 2 * ey; 
  M(4,6,:) =  -delta; 
  
  M(5,2,:) = (1 + 4 * eps) .* sinPso - 3 * ey / 2; 
  M(5,3,:) = -(1 + 4 * eps) .* cosPso + 3 * ex / 2; 
  M(5,5,:) = (1 + eps) .* cosInc; 
  M(5,6,:) = 1 + 2 * eps; 
    
  M(6,4,:) = -ex + (1 + 2 * eps) .* cosPso; 
  M(6,5,:) = (-ey + (1 + 2 * eps) .* sinPso) .* sinInc; 
 
endfunction


// ----------------------------------------------
// Apply any formula
// oe: orbital elements 6xN
// M: 6x6xN
// form: "n" (normalized or "un" (unnormalized)
// motion = "rel" or "abs" (only if deg0)
// ----------------------------------------------
function [M] = orbGapLofM_f(type_oe, oe, loc_frame, meth, motion, form, mu) 
  Meths  = ["f",   "f1",  "f1",  "f1",  "f1"]; 
  Types  = ["kep", "kep", "kep", "cir", "cir"]; 
  Frames = ["qsw", "qsw", "tnw", "qsw", "tnw"]; 
  Funs   = list(orbGapLofM_kepqsw, orbGapLofM_kepqsw1, orbGapLofM_keptnw1, ...
                orbGapLofM_cirqsw1, orbGapLofM_cirtnw1); 

  // if meth = "f0" => call "f1" with deg0 = %t
  deg0 = %f; 
  if (meth == "f0")
    meth = "f1"; 
	deg0 = %t; 
  end
			 
  k = find(Types == type_oe & Meths == meth & Frames == loc_frame); 
  
  if (k == [])
    CL__error("Invalid arguments (unavailable combination of meth, type_oe and loc_frame)"); 
  end

  M = Funs(k)(oe, deg0); 
  
  // if deg0 => relative motion remove velocity due to rotation of frame
  // Vr = Va - [0;0;n] ^ dx => V = V + [dy; -dx; 0]
  // NB: formula only valid for degree 0 expansion
  if (deg0 & motion == "rel")
    M(4,:) = M(4,:) + M(2,:); 
	M(5,:) = M(5,:) - M(1,:); 
  end
  
  if (form == "un")
    // unnormalize
    M = orbGapLofM_conv(M, "un", oe(1,:), mu); 
  end 
     
endfunction


// ----------------------------------------------
// Numerical computation
// oe: orbital elements 6xN
// M: 6x6xN
// form: "n" (normalized or "un" (unnormalized)
// ----------------------------------------------
function [M] = orbGapLofM_c(type_oe, oe, loc_frame, form, mu)   
  N = size(oe, 2); 
  [pv, M] = CL_oe_convert(type_oe, "pv", oe, mu); 

  // Jacobian to local frame
  J = zeros(6,6,N);
  Mloc = CL_fr_locOrbMat(pv(1:3,:), pv(4:6,:), loc_frame); 
  J(1:3,1:3,1:N) = Mloc;
  J(4:6,4:6,1:N) = Mloc;

  M = J * M; 
  // at this point, M is in "unnormalized" form
  
  // Normalize matrix (only for elliptical orbits)
  if (form == "n")
    energy = CL_dot(pv(4:6,:))/2 - mu ./ CL_norm(pv(1:3,:)); 
    sma = -mu ./ (2 * energy);
    M = orbGapLofM_conv(M, "n", sma, mu); 
  end
   
endfunction


// ==============================================
// MAIN
// ==============================================

// check arguments are present
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("form", "local")); form = "n"; end;
if (~exists("mu", "local")); mu = CL__dataGetEnv("mu"); end

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

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

if (~or(motion == ["abs", "rel"]))
  CL__error("Invalid value for argument motion"); 
end

if (~or(form == ["n", "un"]))
  CL__error("Invalid value for argument form"); 
end

if (motion == "rel" & meth <> "f0")
  CL__error("Invalid value for argument motion (relative motion not available)"); 
end

// Check size of orbital elements (number of rows)
[oe, N] = CL__checkInputs(oe, 6); 

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

// Check validity of orbital elements
// if not elliptical orbit => meth must be "c" and form must be "un"
[valid, orbtype] = CL__oe_isValid(type_oe, oe, mu); 
if (~valid) 
  CL__error("Invalid orbital elements"); 
end
if (find(orbtype <> 1) <> [] & meth <> "c" & form <> "un")
  CL__error("Invalid arguents or orbital elements"); 
end


if (meth == "f" | meth == "f1" | meth == "f0")
  // use one of the formulas
  M = orbGapLofM_f(type_oe, oe, loc_frame, meth, motion, form, mu); 
  
else // meth == "c"
  // compute numerically
  M = orbGapLofM_c(type_oe, oe, loc_frame, form, mu);

end

endfunction