//  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 [Xt,norm_F] = CL__3b_shootingEach(X0,t,MU)
// Multiple shooting method to determine the orbit from reduce initial conditions
// This function is just part of the loop of the multiple shooting method, describe on CL__3b_shooting
// Inputs: 
// X0: Halo Initial conditions (6x1) (position+velocity))
// t: discretization times for output orbit.
// MU: mass ratio (see env structure)
//
// Output: 
// Xt: Stabilized Trajectory (pv + t)
// norm_F: Values of the corrections applied for each point
//
// NB: There are several ways to perform this computation here we have presented one posibility but others are proposed in order to solve DZ
//
// Author:
// A. BLAZQUEZ (CNES DCT/SB/MO)

// L'objectif est de pouvoir diminuer le nombre de points par orbite
// Avec une autre methode pour eviter l'inversion directe
// regarde le rapport final du "development of a software library for libration point mission analysis"

//  First part A(6,6,i)
//  -----------------
// initialisations

// Declarations:


// Code:

taille = size(X0,2);
A = [];
F = [];
Xt = []
F_X0 = []

// loop to determine A, F, F_ligne
for i = 1 : taille-1
  [F_X0(:,i),tmp] = CL__3b_monodromy(X0(1:6,i),t(i+1)-t(i), MU);
  A(:,:,i) = tmp;
  F(:,i) = F_X0(:,i)-X0(1:6,i+1);
  F_ligne(6*i-5:6*i) = F(:,i);
end

DF = zeros(6*(taille-1),6*(taille));
for i = 1 : taille-1
  DF(6*i-5:6*i,6*i-5:6*i) = A(:,:,i);
  DF(6*i-5:6*i,6*i+1:6*i+6) = -eye(6,6);
end

// The object is to compute DQ
//   Direct method
//   DQ=-inv(DF)*F_ligne;
//   This method has problems with the direct inversion so we must use another method
// It could be also interesting using a cholesky factorisation.
// This system can be resolve by a cholesky factorisation by D and L


// Decomposition of DF in order to avoid a direct inversion

tmp = DF * DF';
tmp1 = -DF' * inv(tmp);
DQ = tmp1 * F_ligne;

// Addition of DQ to the initial condition
for i = 1 : taille
  Xt(1:6,i) = X0(1:6,i)+DQ(6*i-5:6*i);
end

// To estimate the absolute error
for j = 1 : size(F,2)
  norm_F(j) = norm(F(:,j));
end

endfunction