//  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_gaussCir(cir,frame,mu)
// Gauss equations for circular orbital elements
//
// Calling Sequence
// [M, N] = CL__op_gaussCir(cir,frame,mu)
//
// Description
// <itemizedlist><listitem>
// <p>Computes Gauss equations for orbital elements adapted to near-circular orbits in the QSW or TNW frame.</p>
// <p>The function returns M and N such that d(cir)/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 equatorial orbits (i=0 or i=pi), some parameters cannot be computed 
//  (dex/dt, dey/dt, dgom/dt, dalpha/dt). They are set to %nan</p>
// <p></p></listitem>
// </itemizedlist>
//
// Parameters
// cir: Orbital elements adapted to near circular orbits (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) Les Equations du Mouvement Orbital Perturbe - Cours de l'Ecole GRGS 2002 - Pierre Exertier, Florent Deleflie
//
// See also
// CL__op_gaussKep 
// CL__op_gaussCireq
// CL__op_gaussEquin
//
// Examples
// // M and N in qsw frame
// cir = [7000.e3;0.01;0.005;1.8;0.2;0.3];
// [M,N] = CL__op_gaussCir(cir,"qsw");
//
// // d(cir)/dt due to an acceleration:
// acc = [1e-3 ; 1e-4; -2e-3];
// dcirdt = M*acc + N;


// Declarations:

// Code:

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

N = size(cir,2);

// NB: The equations come from :
// Les Equations du Mouvement Orbital Perturbe - Cours de l'Ecole GRGS 2002 - Pierre Exertier, Florent Deleflie

a = cir(1,:); 
ex = cir(2,:);   // e*cos(pom)
ey = cir(3,:);   // e*sin(pom)
inc = cir(4,:); 
gom = cir(5,:);
// cir(6,:)      // alpha = pom + anm

// Kepler's equation : psi = pom+u with u = eccentric anomaly - page 32
psi = CL_kp_M2Ecir(ex,ey,cir(6,:));

// Intermediate values
e2 = (ex.^2+ey.^2);
eta = sqrt(1-e2); // page 19
nu = ex.*cos(psi) + ey.*sin(psi);    // page 32
ksi = ex.*sin(psi) - ey.*cos(psi);   // page 32
cosp = (cos(psi)-ex) ./ (1-nu) + 1 ./ (1+eta) .* ey .* ksi ./ (1-nu);  // cos(w+v) - page 32 - eq 90
sinp = (sin(psi)-ey) ./ (1-nu) - 1 ./ (1+eta) .* ex .* ksi ./ (1-nu);  // sin(w+v) - page 32 - eq 91
n = sqrt(mu ./ a.^3);

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


// QSW frame
if (frame == "qsw")
  // page 33 - eq 92
  
  M = zeros(6,3,N);
  
  M(1,1,:) = 2 * ksi ./ (n .* (1-nu));                      // da/dt (component Q)
  M(1,2,:) = 2 ./ (n .* eta) .* (1 + (nu - e2) ./ (1-nu));  // da/dt (component S)
  M(1,3,:) = 0;                                             // da/dt (component W)
  
  M(2,1,:) = eta .* sinp ./ (n .* a);                                          // dex/dt (component Q)
  M(2,2,:) = 1 ./ (n .* a) .* (eta .* cosp + cos(psi) - ex .* nu ./ (1+eta));  // dex/dt (component S)
  M(2,3,:) = ey .* (1-nu) .* cos(inc) .* sinp ./ (n .* a .* eta .* sin(inc));  // dex/dt (component W)

  M(3,1,:) = -eta .* cosp ./ (n .* a);                                          // dey/dt (component Q)
  M(3,2,:) = 1 ./ (n .* a) .* (eta .* sinp + sin(psi) - ey .* nu ./ (1+eta));   // dey/dt (component S)
  M(3,3,:) = -ex .* (1-nu) .* cos(inc) .* sinp ./ (n .* a .* eta .* sin(inc));  // dey/dt (component W)
  
  M(4,1,:) = 0;                                   // di/dt (component Q)
  M(4,2,:) = 0;                                   // di/dt (component S)
  M(4,3,:) = (1-nu) .* cosp ./ (n .* a .* eta);   // di/dt (component W)

  M(5,1,:) = 0;                                              // dgom/dt (component Q)
  M(5,2,:) = 0;                                              // dgom/dt (component S)
  M(5,3,:) = (1-nu) .* sinp ./ (n .* a .* eta .* sin(inc));  // dgom/dt (component W)

  M(6,1,:) = -2 * (1-nu) ./ (n .* a) - eta ./ (n .* a) .* (nu - e2) ./ ((1+eta).*(1-nu)); // dalpha/dt (component Q)
  M(6,2,:) = ksi .* (2 - nu - e2) ./ (n .* a .* (1-nu) .* (1+eta));                       // dalpha/dt (component S)
  M(6,3,:) = - (1-nu) .* cos(inc) .* sinp ./ (n .* a .* eta .* sin(inc));                 // dalpha/dt (component W)
  
  // Deduce QSW results from TNW : eq 93 page 33 
  // (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;
  // M = res_qsw * P_tnw_qsw;
  
  
elseif (frame == "tnw")
  // page 34 - eq 94
  
  M = zeros(6,3,N);
  
  M(1,1,:) = 2 ./ n .* sqrt((1+nu)./(1-nu));   // da/dt (component T)
  M(1,2,:) = 0;                                // da/dt (component N)
  M(1,3,:) = 0;                                // da/dt (component W)

  M(2,1,:) = 1 ./ (n .* a .* sqrt(1-nu.^2)) .* (ksi .* eta .* sinp + eta .* (eta .* cosp + cos(psi) - ex .* nu ./ (1+eta))); // dex/dt (component T)
  M(2,2,:) = 1 ./ (n .* a .* sqrt(1-nu.^2)) .* (-eta.^2 .* sinp + ksi .* (eta .* cosp + cos(psi) - ex .* nu ./ (1+eta)));    // dex/dt (component N)
  M(2,3,:) = ey .* (1-nu) .* cos(inc) .* sinp ./ (n .* a .* eta .* sin(inc));                                                // dex/dt (component W)

  M(3,1,:) = 1 ./ (n .* a .* sqrt(1-nu.^2)) .* (-ksi .* eta .* cosp + eta .* (eta .* sinp + sin(psi) - ey .* nu ./ (1+eta))); // dey/dt (component T)
  M(3,2,:) = 1 ./ (n .* a .* sqrt(1-nu.^2)) .* (eta.^2 .* cosp + ksi .* (eta .* sinp + sin(psi) - ey .* nu ./ (1+eta)));      // dey/dt (component N)
  M(3,3,:) = -ex .* (1-nu) .* cos(inc) .* sinp ./ (n .* a .* eta .* sin(inc));                                                // dey/dt (component W)
  
  M(4,1,:) = 0;                                   // di/dt (component T)
  M(4,2,:) = 0;                                   // di/dt (component N)
  M(4,3,:) = (1-nu) .* cosp ./ (n .* a .* eta);   // di/dt (component W)

  M(5,1,:) = 0;                                              // dgom/dt (component T)
  M(5,2,:) = 0;                                              // dgom/dt (component N)
  M(5,3,:) = (1-nu) .* sinp ./ (n .* a .* eta .* sin(inc));  // dgom/dt (component W)

  M(6,1,:) = 2 * ksi .* (-1 + nu + eta ./ (1+eta)) ./ (n .* a .* sqrt(1-nu.^2));            // dalpha/dt (component T)
  M(6,2,:) = 1 ./ (n .* a .* sqrt(1-nu.^2)) .* (2 * eta .* (1-nu) + 1 ./ ((1+eta).*(1-nu)) .* (eta.^2 .* (nu - e2) + ksi.^2 .* (2 - nu - e2)));   // dalpha/dt (component N)
  M(6,3,:) = - (1-nu) .* cos(inc) .* sinp ./ (n .* a .* eta .* sin(inc));                   // dalpha/dt (component W)

end

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

endfunction