//  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,N] = CL__op_gaussKep(kep,frame,mu)
// Gauss equations for Keplerian orbital elements
//
// Calling Sequence
// [M, N] = CL__op_gaussKep(kep,frame,mu)
//
// Description
// <itemizedlist><listitem>
// <p>Computes Gauss equations in the Keplerian form in the QSW or TNW frame.</p>
// <p>The function returns M and N such that d(kep)/dt = M*acc + N, with N = mean motion and
//    acc = acceleration expressed in the QSW or TNW frame.</p>
// <p></p>
// <p>See <link linkend="Orbital elements">Orbital elements</link> for the definition of orbital elements</p>
// <p>Notes: </p>
// <p>- For circular orbits (e=0) and equatorial orbits (i=0 or i=pi), some parameters cannot be computed 
//  (dw/dt, dM/dt, dgom/dt). They are set to %nan</p>
// <p></p></listitem>
// </itemizedlist>
//
// Parameters
// kep: Orbital elements in the classical (Keplerian) form (6xN)
// frame: (string) Local orbital frame: "qsw" or "tnw". (1x1)
// mu: Gravitational constant [m^3/s^2]
// M: Matrix of the effects of an acceleration on orbital elements (6x3xN)
// N: Mean motion (6xN)
//
// Authors
// CNES - DCT/SB
//
// Bibliography
// 1) 
//
// See also
// CL__op_gaussCir
// CL__op_gaussEquin
// CL__op_gaussEquin
//
// Examples
// // M and N in qsw frame
// kep = [7000.e3;0.01;1.8;0.1;0.2;0.3];
// [M,N] = CL__op_gaussKep(kep,"qsw");
//
// // d(kep)/dt due to an acceleration
// acc = [1e-3 ; 1e-4; -2e-3];
// dkepdt = M*acc + N;


// Declarations:

// Code:

// NB: Validity of input arguments checked in a higher level function

N = size(kep,2);

a = kep(1,:); 
e = kep(2,:); 
inc = kep(3,:); 
pom = kep(4,:); 
gom = kep(5,:); 
v = CL_kp_M2v(kep(2,:),kep(6,:));

// Intermediate values
n = sqrt(mu ./ a.^3); 
sinv = sin(v);
cosv = cos(v);
eta = sqrt(1 - e.^2);

// inc is set to %nan if inc==0 or inc==%pi
// (not computable results)
inc(find(sin(inc) < %eps)) = %nan;

if (frame == "qsw")
  
  // nae is set to %nan if e==0 (not computable results)
  nae = n .* a .* e;
  nae(find(e < %eps)) = %nan;
  
  // M = M
  M = zeros(6,3,N);
  M(1,1,:) = 2 ./ (n .* eta) .* (e .* sinv);      // da/dt (component Q)
  M(1,2,:) = 2 ./ (n .* eta) .* (1 + e .* cosv);  // da/dt (component S)
  M(1,3,:) = 0;                                   // da/dt (component W)
  
  M(2,1,:) = eta ./ (n .* a) .* (sinv);                                  // de/dt (component Q)
  M(2,2,:) = eta ./ (n .* a) .* ((cosv + e) ./ (1 + e .* cosv) + cosv);  // de/dt (component S)
  M(2,3,:) = 0;                                                          // de/dt (component W)

  M(3,1,:) = 0;                                                     // di/dt (component Q)
  M(3,2,:) = 0;                                                     // di/dt (component S)
  M(3,3,:) = eta .* cos(pom+v) ./ (n .* a .* (1 + e .* cos(v)));    // di/dt (component W)

  M(4,1,:) = eta ./ nae .* (-cosv);                                                 // dpom/dt (component Q)
  M(4,2,:) = eta ./ nae .* sinv .* (1 + 1 ./ (1 + e .* cos(v)));                    // dpom/dt (component S)
  M(4,3,:) = - eta ./ (n .* a) .* cos(inc)  .* sin(pom + v) ./ ((1 + e .* cos(v)) .* sin(inc));   // dpom/dt (component W)

  M(5,1,:) = 0;                                                                 // dgom/dt (component Q)
  M(5,2,:) = 0;                                                                 // dgom/dt (component S)
  M(5,3,:) = eta .* sin(pom+v) ./ (n .* a .* (1 + e .* cos(v)) .* sin(inc));    // dgom/dt (component W)

  M(6,1,:) = eta.^2 ./ nae .* (cosv - 2*e ./ (1 + e .* cos(v)));       // dM/dt (component Q)
  M(6,2,:) = - eta.^2 ./ nae .* sinv .* (1 + 1 ./ (1 + e .* cos(v)));  // dM/dt (component S)
  M(6,3,:) = 0;                                                        // dM/dt (component W)
  
  
elseif (frame == "tnw")

  q = 1 ;
  r = a .* (1-e.^2) ./ (1 + e.*cosv);
  V = sqrt( mu * (2 ./ r - 1 ./ a ));
  // eV is set to %nan if e==0 (not computable results)
  eV = e .* V;
  eV(find(e < %eps)) = %nan;
  
  
  // M = M
  M = zeros(6,3,N);
  M(1,1,:) = 2 ./ (n.^2 .* a) .* V;      // da/dt (component T)
  M(1,2,:) = 0;                          // da/dt (component N)
  M(1,3,:) = 0;                          // da/dt (component W)

  M(2,1,:) = 2 ./ V .* (e + cosv);         // de/dt (component T)
  M(2,2,:) = -1 ./ V .* (r ./ a .* sinv);  // de/dt (component N)
  M(2,3,:) = 0;                            // de/dt (component W)

  M(3,1,:) = 0;                                     // di/dt (component T)
  M(3,2,:) = 0;                                     // di/dt (component N)
  M(3,3,:) = r .* cos(pom+v) ./ (n .* a.^2 .* eta); // di/dt (component W)

  M(4,1,:) = 1 ./ eV .* (2 * sinv);                                     // dpom/dt (component T)
  M(4,2,:) = 1 ./ eV .* (2*e + cosv + e.^2 .* cosv) ./ (1 + e.*cosv);   // dpom/dt (component N)
  M(4,3,:) = -r .* sin(pom+v) .* cotg(inc) ./ (n .* a.^2 .* eta);       // dpom/dt (component W)

  M(5,1,:) = 0;                                                  // dgom/dt (component T)
  M(5,2,:) = 0;                                                  // dgom/dt (component N)
  M(5,3,:) = r .* sin(pom+v) ./ (n .* a.^2 .* eta .* sin(inc));  // dgom/dt (component W)

  M(6,1,:) = -eta ./ eV .* (2*sinv .*(1 + e .* cosv + e.^2) ./ (1 + e.*cosv));    // dM/dt (component T)
  M(6,2,:) = -eta ./ eV .* (eta.^2 .* cosv ./ (1 + e.*cosv));                     // dM/dt (component N)
  M(6,3,:) = 0;                                                                   // dM/dt (component W)
end

// N = mean motion
N = [zeros(5,N);n];


endfunction