//  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_gaussCireq(cireq,frame,mu)
// Gauss equations for circular equatorial orbital elements
//
// Calling Sequence
// [M, N] = CL__op_gaussCireq(cireq,frame,mu)
//
// Description
// <itemizedlist><listitem>
// <p>Computes Gauss equations for circular equatorial orbital elements in the QSW or TNW frame.</p>
// <p>The function returns M and N such that d(cireq)/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
// cireq: Circular equatorial 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
// cireq = [7000.e3;0.01;1e-3;-2e-2;0.0008;0.3];
// [M,N] = CL__op_gaussCireq(cireq,"qsw");
//
// // d(cireq)/dt due to an acceleration:
// acc = [1e-3 ; 1e-4; -2e-3];
// dcireqdt = M*acc + N;


// Declarations:

// Code:

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

N = size(cireq,2);

a = cireq(1,:); 
ex = cireq(2,:);  // e*cos(pom+gom)
ey = cireq(3,:);  // e*sin(pom+gom)
ix = cireq(4,:);  // sin(i/2)*cos(gom)
iy = cireq(5,:);  // sin(i/2)*sin(gom)
// cireq(6,:)     // L = pom+gom+anm

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

// Intermediate values
eta = sqrt(1 - ex.^2 - ey.^2);
cosF = cos(F);
sinF = sin(F);
ecosu = ex .* cosF + ey .* sinF;
esinu = ex .* sinF - ey .* cosF
n = sqrt(mu ./ a.^3);

// cosi2 = cos(inc/2)
cosi2 = sqrt(1 - ix.^2 - iy.^2);

// cosi2 is set to %nan if it is zero (happens when inc==%pi)
// (not computable results)
cosi2(find(cosi2 < %eps)) = %nan;


if (frame == "tnw" | frame == "qsw")
  
  M = zeros(6,3,N);
  
  M(1,1,:) = 2 ./ n .* sqrt((1 + ecosu)./(1-ecosu));   // da/dt (component T)
  M(1,2,:) = 0;                                        // da/dt (component N)
  M(1,3,:) = 0;                                        // da/dt (component W)

  M(2,1,:) = 2 ./ (n .* a) .* eta ./ sqrt(1-ecosu.^2) .* (eta .* cosF - ey ./ (1+eta) .* esinu);      // dex/dt (component T)
  M(2,2,:) = -1 ./ (n .* a) .* sqrt((1 - ecosu)./(1+ecosu)) .* (ey + sinF - ex ./ (1+eta) .* esinu);  // dex/dt (component N)
  M(2,3,:) = -1 ./ (n .* a .* cosi2 .* eta) .* ey .* (iy .* (ex - cosF) - ix .* (ey - sinF) - esinu ./ (1+eta) .* (ex.*ix + ey.*iy)); // dex/dt (component W)

  M(3,1,:) = 2 ./ (n .* a) .* eta ./ sqrt(1-ecosu.^2) .* (eta .* sinF + ex ./ (1+eta) .* esinu);     // dey/dt (component T)
  M(3,2,:) = 1 ./ (n .* a) .* sqrt((1 - ecosu)./(1+ecosu)) .* (ex + cosF + ey ./ (1+eta) .* esinu);  // dey/dt (component N)
  M(3,3,:) = 1 ./ (n .* a .* cosi2 .* eta) .* ex .* (iy .* (ex - cosF) - ix .* (ey - sinF) - esinu ./ (1+eta) .* (ex.*ix + ey.*iy)); // dey/dt (component W)

  
  M(4,1,:) = 0;                                                                                  // dix/dt (component T)
  M(4,2,:) = 0;                                                                                  // dix/dt (component N)
  M(4,3,:) = 1 ./ (2 * n .* a .* eta .* cosi2) .* ((1-ix.^2).*(cosF-ex) - ...
                        ix .* iy .* (sinF - ey) + esinu ./ (1+eta) .* (ix .* iy .* ex + (1-ix.^2) .* ey));   // dix/dt (component W)

  M(5,1,:) = 0;                                              // diy/dt (component T)
  M(5,2,:) = 0;                                              // diy/dt (component N)
  M(5,3,:) = 1 ./ (2 * n .* a .* eta .* cosi2) .* ((1-iy.^2).*(sinF-ey) - ...
                        ix .* iy .* (cosF - ex) - esinu ./ (1+eta) .* (ix .* iy .* ey + (1-iy.^2) .* ex));   // diy/dt (component W)

  M(6,1,:) = -2 ./ (n .* a) .* esinu ./ sqrt(1 - ecosu .^2) .* (1 - eta ./ (1 + eta) - ecosu);  // dalpha/dt (component T)
  M(6,2,:) = 1 ./ (n .* a) .* sqrt((1-ecosu) ./ (1+ecosu)) .* (1 + eta + ecosu ./ (1 + eta));   // dalpha/dt (component N)
  M(6,3,:) = 1 ./ (n .* a .* eta .* cosi2) .* (ix .* (sinF-ey) - iy .* (cosF-ex) - 1 ./ (1+eta) .* (ex .* ix + ey .* iy) .* esinu);  // dalpha/dt (component W)

end


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

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

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

endfunction