//  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 [Xstab,corrnum,Xtraj,ttraj,omegatraj,deltaY] = CL__3b_stabHaloNewton(X0,omegacorr,t0,step,MU)
// This function stabilises the halo initial conditions (Richardson's method)
// Using Newton Method, the function assures the convergence of half an orbit arcs.
//
// Inputs: 
// X0: Halo Initial position+velocity (6x1)
// omegacorr: Halo pulsation corrected
// t0: initial time
// step: output step
// MU: mass ratio
//
// Outputs: 
// Xstab: Initial conditions of the input orbit stabilise: vect (columns of 6 dimension (position+velocity))
// corrnum: numeric corrections of the stabilisation process: vect colums of 2: dx dvy
// Xtraj: Every point of the stabilise trajectory: matrix 6 lines (pos+vel) and n columns (dates)
// ttraj: dates of each point of the trajectoire: vect n lines
// deltaY: difference between the norms of the last 2 vectors Y of the Newton's method
//
// NB: if no stabilization: all output arguments are [] 
//
// Author:
// B. MEYSSIGNAC (CNES DCT/SB/MO)
// AL: adaptations (convergence control)

// Declarations:


// Code:

// default outputs

Xstab = []; 
corrnum = []; 
Xtraj = []; 
ttraj = []; 
omegatraj = []; 
deltaY = []; 


// Initialisation of Newton's method
Yn = [X0(1); X0(5); %pi/omegacorr];
Xn = [Yn(1); X0(2); X0(3); X0(4); Yn(2); X0(6)];
dn = Yn(3); // propagation duration
dYn = [];

[Xf,Mn] = CL__3b_monodromy(Xn, dn, MU);
iter = 1; 
maxiter = 50; 
tol = 3.e-14; 

// Newton's method
while (abs(Xf(2)) + abs(Xf(4)) + abs(Xf(6)) > tol & iter <= maxiter)

  r0f = sqrt((Xf(1)+MU)^2+Xf(2)^2+Xf(3)^2);
  r1f = sqrt((Xf(1)-1+MU)^2+Xf(2)^2+Xf(3)^2);

  DNewton = [Mn(2,1), Mn(2,5),  Xf(5);
             Mn(4,1), Mn(4,5),  2*Xf(5)+Xf(1)-(1-MU)*(Xf(1)+MU)/(r0f^3)-MU*(Xf(1)-1+MU)/(r1f^3);
             Mn(6,1), Mn(6,5), -(1-MU)*Xf(3)/(r0f^3)-MU*Xf(3)/(r1f^3)];

  dYn = inv(DNewton)*[Xf(2);Xf(4);Xf(6)];

  dY = norm(dYn);
  if (dY <= %eps); break; end

  Yn = Yn - dYn;
  Xn = [Yn(1);X0(2);X0(3);X0(4);Yn(2);X0(6)];
  dn = Yn(3);

  [Xf,Mn] = CL__3b_monodromy(Xn,dn,MU);
  iter = iter + 1; 
  
end

// no convergence ! 
if (dY <= %eps | iter > maxiter)
  return;  // <== RETURN 
end


// integration results
deltaY = dY; 

Xstab = Xn;
corrnum = [Xn(1)-X0(1);Xn(5)-X0(5)];

ttraj = [(t0 : step : t0+dn-step/2), t0+dn];
Z0 = [Xn; matrix(eye(6,6),36,1)];
Ztraj = CL__3b_integratorFix(Z0, t0, ttraj, CL__3b_RHSDP, MU);
Xtraj = Ztraj(1:6,:);  

omegatraj = %pi / dn;

endfunction