//  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_gaussEquin(equin,frame,mu)
// Gauss equations for equinoctial orbital elements
//
// Calling Sequence
// [M, N] = CL__op_gaussEquin(equin,frame,mu)
//
// Description
// <itemizedlist><listitem>
// <p>Computes Gauss equations for equinoctial orbital elements in the QSW or TNW frame.</p>
// <p>The function returns M and N such that d(equin)/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 orbits with an inclination of pi, most of the parameters cannot be computed 
//  (dex/dt, dey/dt, dix/dt, diy/dt, dalpha/dt). They are set to %nan</p>
// <p></p></listitem>
// </itemizedlist>
//
// Parameters
// equin: Equinoctial orbital elements (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_gaussKep 
// CL__op_gaussCir
// CL__op_gaussEquin
//
// Examples
// // M and N in qsw frame
// equin = [7000.e3;0.01;1e-3;-2e-2;0.0008;0.3];
// [M,N] = CL__op_gaussEquin(equin,"qsw");
//
// // d(equin)/dt due to an acceleration:
// acc = [1e-3 ; 1e-4; -2e-3];
// dequindt = M*acc + N;


// Declarations:

// Code:

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

N = size(equin,2);

a = equin(1,:); 
ex = equin(2,:);  // e*cos(pom+gom)
ey = equin(3,:);  // e*sin(pom+gom)
hx = equin(4,:);  // tan(i/2)*cos(gom)
hy = equin(5,:);  // tan(i/2)*sin(gom)
// equin(6,:)     // L = pom+gom+anm

// Kepler's equation: F = pom+gom+u with u = eccentric anomaly
F = CL_kp_M2Ecir(ex,ey,equin(6,:));

// Intermediate values
e2 = ex.^2 + ey.^2
eta = sqrt(1 - e2);
nu = ex .* cos(F) + ey .* sin(F); // = ecosE
ksi = ex .* sin(F) - ey .* cos(F) // = esinE
n = sqrt(mu ./ a.^3);
cosLv = (cos(F)-ex) ./ (1-nu) + 1 ./ (1+eta) .* ey .* ksi ./ (1-nu);  // cos(w+gom+v)
sinLv = (sin(F)-ey) ./ (1-nu) - 1 ./ (1+eta) .* ex .* ksi ./ (1-nu);  // sin(w+gom+v)
ecosv = ex .* cosLv + ey .* sinLv;
esinv = ex .* sinLv - ey .* cosLv;
 



if (frame == "tnw" | frame == "qsw")
  
  M = zeros(6,3,N);
  
  M(1,1,:) = 2 ./ (n .* eta) .* esinv;        // da/dt (component Q)
  M(1,2,:) = 2 ./ (n .* eta) .* (1 + ecosv);  // da/dt (component Q)
  M(1,3,:) = 0;                               // da/dt (component W)

  M(2,1,:) = eta ./ (n .* a) .* sinLv;                                               // dex/dt (component Q)
  M(2,2,:) = eta ./ (n .* a) .* (ex + cosLv .* (2 + ecosv)) ./ (1 + ecosv);          // dex/dt (component S)
  M(2,3,:) = -eta ./ (n .* a) .* (ey .* (hx .* sinLv - hy .* cosLv) ./ (1 + ecosv));  // dex/dt (component W)

  M(3,1,:) = - eta ./ (n .* a) .* cosLv;                                               // dey/dt (component Q)
  M(3,2,:) = eta ./ (n .* a) .* (ey + sinLv .* (2 + ecosv)) ./ (1 + ecosv);          // dey/dt (component S)
  M(3,3,:) = eta ./ (n .* a) .* (ex .* (hx .* sinLv - hy .* cosLv) ./ (1 + ecosv));  // dey/dt (component W)
  
  M(4,1,:) = 0;                                                                // dhx/dt (component Q)
  M(4,2,:) = 0;                                                                // dhx/dt (component S)
  M(4,3,:) = eta ./ (2 * n .* a) .* (1 + hx.^2 + hy.^2) .* cosLv ./ (1 + ecosv);   // dhx/dt (component W)

  M(5,1,:) = 0;                                                                // dhy/dt (component Q)
  M(5,2,:) = 0;                                                                // dhy/dt (component S)
  M(5,3,:) = eta ./ (2 * n .* a) .* (1 + hx.^2 + hy.^2) .* sinLv ./ (1 + ecosv);   // dhy/dt (component W)

  M(6,1,:) = -2 * (1-nu) ./ (n .* a) - eta ./ (n .* a) .* (nu - e2) ./ ((1+eta).*(1-nu)); // dL/dt (component Q)
  M(6,2,:) = ksi .* (2 - nu - e2) ./ (n .* a .* (1-nu) .* (1+eta));                       // dL/dt (component S)
  M(6,3,:) = 1 ./ (n .* a .* eta) .* (hx .* (sin(F)-ey) - hy .* (cos(F)-ex) - 1 ./ (1+eta) .* (ex .* hx + ey .* hy) .* ksi);  // dL/dt (component W)

end


if (frame == "tnw")
  // Direct formula : TODO?

  // Deduce TNW results from QSW : 
  // (simpler but requires more computation and memory)
  
  P_tnw_qsw = zeros(3,3,N)
  P_tnw_qsw(1,1,:) =  1 ./ sqrt((1 - nu.^2)) .* ksi;
  P_tnw_qsw(1,2,:) = 1 ./ sqrt((1 - nu.^2)) .* eta;
  P_tnw_qsw(2,1,:) = -1 ./ sqrt((1 - nu.^2)) .* eta;
  P_tnw_qsw(2,2,:) = 1 ./ sqrt((1 - nu.^2)) .* ksi;
  P_tnw_qsw(3,3,:) = 1;
  
  // NB: at this point, M is equal to M_qsw
  M = M * (P_tnw_qsw');
end

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

endfunction