//  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 [vel1,vel2] = CL_man_lambert(pos1,pos2,delta_t,direction,mu)
// Lambert's problem
//
// Calling Sequence
// [vel1,vel2] = CL_man_lambert(pos1,pos2,delta_t [, direction ,mu])
//
// Description
// <itemizedlist><listitem>
// <p>This function solves Lambert's problem in the case the transfer consists 
// in less than one revolution.</p> 
// <p>It Computes the velocity vectors <b>vel1</b> and <b>vel2</b>, 
// given the position vectors <b>pos1</b> and <b>pos2</b>, 
// the time of flight <b>delta_t</b>, and the direction of motion <b>direction</b>.</p>
// </listitem>
// </itemizedlist>
//
// Parameters
// pos1 : Initial position vector [m] (3xN or 3x1)
// pos2 : Final position vector [m] (3xN or 3x1)
// delta_t : Time of flight from pos1 to pos2 [s] (1xN or 1x1)
// direction : (string, optional) 'pro' if the transfer orbit is prograde, 'retro' if the transfer orbit is retrograde (default is 'pro')
// mu : (optional) Gravitational constant. Default is %CL_mu [m^3/s^2]
// vel1 : Initial velocity vector [m/s] (3xN)
// vel2 : Final velocity vector [m/s] (3xN)
//
// Authors
// CNES - DCT/SB
//
// Bibliography
// 1) Orbital Mechanics for Engineering Students, H.D. Curtis, Section 5.3 and Appendix D.11 (algorithm 5.2)
// 2) Modern astrodynamics Fundamentals and perturbation methods, V Bond and M Allman, Chapter 6.2
//
// Examples
// dt = 1000; // seconds
// kep1 = [7000.e3; 0.1; 1; 0; 0; 0]; 
// kep2 = CL_ex_kepler(0, kep1, dt/86400); 
// [pos1, vel1] = CL_oe_kep2car(kep1); 
// [pos2, vel2] = CL_oe_kep2car(kep2);
// [vel1b, vel2b] = CL_man_lambert(pos1, pos2, dt); 
// CL_norm(vel1-vel1b) + CL_norm(vel2-vel2b)


// Declarations:

function [val] = lamb_Y(z,u)
  val = real(1 + u .* (z .* CL_stumpS(z) - 1) ./ sqrt(CL_stumpC(z)));
endfunction

// lamb_F: function such that lamb_F(z,u) == v
// der: derivative - not computed if there is only one output argument
function [val, der] = lamb_F(z,u)
  c = CL_stumpC(z); 
  s = CL_stumpS(z); 
  // y = lamb_Y(z,u); 
  y = real(1 + u .* (z .* s - 1) ./ sqrt(c));
  val = real((y ./ c).^1.5 .* s + u .* sqrt(y));

  // no derivative if one output argument
  if (argn(1) < 2); return; end

  I = find(z == 0); 
  z(I) = %nan; 
  der = real( (y./c).^1.5 .* ((0.5 ./ z) .* (c - 1.5 * s ./ c) + 0.75 * s.^2 ./ c) + ...
             (u/ 8) .* (3*(s./c).*sqrt(y) + u.*sqrt(c./y)) );
  // re-assign value for z=0                  
  if (I <> [])
    der(I) = real( (sqrt(2) ./ 40) * y(I).^1.5 + ...
                 (u(I)/8) .* (sqrt(y(I)) + u(I) .* sqrt(0.5 ./ y(I))) );  
  end   

endfunction
  
// finds z / F(z,u) = v
function [z] = lamb_solve(u,v)
  // Modification of initial algorithm : 
  // Dichotomy to find the initial guess 

  // The solution to F(z,u,v) = 0 is looked for in [zmin, zmax] 
  z_min = -150 ;
  z_max = (2*%pi)^2 - 3.e-6 ; // such that CL_stumpC(z) > %eps
  
  a = z_min * ones(u); // lower bound of dichotomy
  b = z_max * ones(u); // upper bound of dichotomy
  fa = lamb_F(a,u); 
  fb = lamb_F(b,u); 
  c = zeros(a); 
  fc = c; 

  // find initial interval (dichotomy)
  // if u > 0: a should be such that F(a) > 0 (in practice > s/10)
  // if u <= 0: no need to limit the interval (in practice: length < 1)
  // not iterations if v == 0
  
  K = find(v > 0 & (fb-v).*(fa-v) < 0); 
  maxiter = 15; 
  iter = 1; 
  
  while (K <> [] & iter <= maxiter)  
    c(K) = (a(K) + b(K))/2; 
    fc(K) = lamb_F(c(K), u(K)); 
    I = K(find ((fc(K)-v(K)) .* (fa(K)-v(K)) >= 0)); 
    a(I) = c(I); 
    fa(I) = fc(I);  
    I = K(find ((fc(K)-v(K)) .* (fb(K)-v(K)) >= 0)); 
    b(I) = c(I); 
    fb(I) = fc(I);
    K = find((b-a) > 1 | (u >= 0 & fa < v/10)); 
    iter = iter + 1; 
  end
 
  // Newton resolution : Iterate on Equation 5.45 until z is determined to within
  // the error tolerance (tol)
  // NB: The algorithm is assumed to converge given the accuracy of the initial guess  
  tol = 1.e-8;   // error tolerance on z variation
  maxiter = 15;  // limit on the number of iterations
  z = %nan * ones(u); 
  
  // only indices for which dichotomy converged
  K = setdiff(find(v > 0), K); 
  z(K) = (a(K)+b(K))/2;   // initial guess
  dz = %inf * ones(z);
  iter = 1;

  while (K <> [] & iter <= maxiter)
    [valK, derK] = lamb_F(z(K),u(K)); 
    dz(K) = -(valK-v(K)) ./ derK;
    I = K(find(dz(K) < a(K) - z(K))); 
    if (I <> [])
      dz(I) = (a(I)-z(I))*0.9; 
    end
    I = K(find(dz(K) > b(K)-z(K))); 
    if (I <> [])
      dz(I) = (b(I)-z(I))*0.9; 
    end
    z(K) = z(K) + dz(K);
    K = find(abs(dz) > tol);
    iter = iter + 1;
  end

  // %nan if maximum number of iterations reached (K not empty)
  z(K) = %nan; 

  // check for inconsistencies
  I = find(abs(lamb_F(z,u) - v) > 1.e-6); 
  z(I) = %nan; 

endfunction


// Code:

if (~exists("direction","local")); direction = "pro"; end;
if (~exists("mu", "local")); mu = CL__dataGetEnv("mu"); end;

if (find(delta_t < 0) <> [])
  CL__error("Invalid value for delta_t"); 
end

if (direction <> "pro" & direction <> "retro")
  CL__error("Invalid value for direction (pro or retro expected)"); 
end

// Consistency of inputs :
[pos1, pos2, delta_t] = CL__checkInputs(pos1, 3, pos2, 3, delta_t, 1); 

// Magnitudes of pos1 and pos2
r1 = CL_norm(pos1);
r2 = CL_norm(pos2);

if (find(r1.*r2 == 0) <> [])
  CL__error("Invalid value for pos1 or pos2 (norm is 0)"); 
end

c12 = CL_cross(pos1,pos2);
d12 = CL_dot(pos1,pos2); 

// sign of sin(theta)
sgn = ones(r1); 

if (direction == "pro")
  I = find(c12(3,:) < 0);
  sgn(I) = -1;
else
  I = find(c12(3,:) > 0);
  sgn(I) = -1;
end

// Equation 5.35: 
// A = sin_theta .* sqrt(r1.*r2 ./ (1 - cos_theta));
// Now: Normalized variables (u = A/(r1+r2)) 
// Note: -sqrt(2)/2 <= u <= sqrt(2)/2
u = sgn .* real(sqrt(r1.*r2 + d12)) ./ (r1+r2); 
v = sqrt(mu ./ (r1+r2).^3) .* delta_t; 

// find z such that lamb_F(u)== v
z = lamb_solve(u,v); 
y = lamb_Y(z,u); 

// check
I = find(y < 0);
y(I) = %nan; 

// protection against division by 0 (see g = ...)
I = find(u .* y == 0);
y(I) = %nan; 

// Equation 5.46a:
f = ones(3,1) * (1 - y .* (r1+r2) ./ r1);

// Equation 5.46b:
g = ones(3,1) * (u .* sqrt(y .* ((r1+r2).^3 ./ mu)));

// Equation 5.46d:
gdot = ones(3,1) * (1 - y .* (r1+r2) ./ r2);

// Equation 5.28:
vel1 = (pos2 - f .* pos1) ./ g;

// Equation 5.29:
vel2 = (gdot .* pos2 - pos1) ./ g;

endfunction