//  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 [v1,v2] = CL_man_lambert2(pos1,pos2,tf,direction,m,leftbranch,mu)
// Lambert's problem
//
// Calling Sequence
// [v1,v2] = CL_man_lambert2(pos1,pos2,tf [,direction,m,leftbranch,mu])
//
// Description
// <itemizedlist><listitem>
// <p>Computes the velocity vectors <b>v1</b> and <b>v2</b>, 
// given the position vectors <b>r1</b> and <b>r2</b>, 
// the time of flight <b>tf</b>, and the direction of motion <b>direction</b>.</p>
// <p>Set <b>direction</b> to 'pro' for a prograde orbit. 
// That is a counter-clockwise rotation around the Z axis. (this is the prograde direction for the solar system) </p>
// <p>Set <b>direction</b> to 'retro' for a retrograde orbit. 
// That is a clockwise rotation around the Z axis. (this is the retrograde direction for the solar system)</p>
// <p>The number of revolutions <b>m</b> can optionaly be set to a value superior to 0,
// and in that case there are two solutions which can be selected using <b>leftbranch</b> </p>
// <inlinemediaobject><imageobject><imagedata fileref="lambert_m0.gif"/></imageobject></inlinemediaobject>
// <inlinemediaobject><imageobject><imagedata fileref="lambert_m1.gif"/></imageobject></inlinemediaobject>
// </listitem>
// </itemizedlist>
//
// Parameters
// pos1 : Initial position vector [m] (3xN)
// pos2 : Final position vector [m] (3xN)
// tf : Time of flight from r1 to r2 [s] (1xN or 1x1)
// direction : (optional) 'pro' for prograde, 'retro' for retrograde (default is 'pro') (1x1)
// m : (optional) number of revolution  (default is 0) (1x1)
// leftbranch : (optional) (boolean) when m > 0, chooses between left branch or right branch (default is %f) (1x1)
// mu : (optional) Gravitational constant (default is %CL_mu) [m^3/s^2]
// v1 : Initial velocity vector [m/s] (3xN)
// v2 : Final velocity vector [m/s] (3xN)
//
// Authors
// CNES - DCT/SB
//
// Bibliography
// 1) Izzo, D. ESA Advanced Concepts team. Code used available in MGA.M, on http://www.esa.int/gsp/ACT/inf/op/globopt.htm. Last retreived Nov, 2009.
//
// See also
//
// Examples
// dt = 1000; // seconds
// kep1 = [7000.e3; 0.1; 1; 0; 0; 0]; 
// kep2 = CL_ex_kepler(0, kep1, dt/86400); 
// [p1, v1] = CL_oe_kep2car(kep1); 
// [p2, v2] = CL_oe_kep2car(kep2);
// [v1b, v2b] = CL_man_lambert2(p1, p2, dt); 
// CL_norm(v1-v1b) + CL_norm(v2-v2b)



// original documentation:
// -----------------------
//
// This routine implements a new algorithm that solves Lambert's problem. The
// algorithm has two major characteristics that makes it favorable to other
// existing ones.
//
// 1) It describes the generic orbit solution of the boundary condition
// problem through the variable X=log(1+cos(alpha/2)). By doing so the
// graph of the time of flight become defined in the entire real axis and
// resembles a straight line. Convergence is granted within few iterations
// for all the possible geometries (except, of course, when the transfer
// angle is zero). When multiple revolutions are considered the variable is
// X=tan(cos(alpha/2)*%pi/2).
//
// 2) Once the orbit has been determined in the plane, this routine
// evaluates the velocity vectors at the two points in a way that is not
// singular for the transfer angle approaching to %pi (Lagrange coefficient
// based methods are numerically not well suited for this purpose).
//
// As a result Lambert's problem is solved (with multiple revolutions
// being accounted for) with the same computational effort for all
// possible geometries. The case of near 180 transfers is also solved
// efficiently.
//
//  We note here that even when the transfer angle is exactly equal to %pi
// the algorithm does solve the problem in the plane (it finds X), but it
// is not able to evaluate the plane in which the orbit lies. A solution
// to this would be to provide the direction of the plane containing the
// transfer orbit from outside. This has not been implemented in this
// routine since such a direction would depend on which application the
// transfer is going to be used in.
//
// please report bugs to dario.izzo@esa.int    
//
// adjusted documentation:
// ----------------------
//
// By default, the short-way solution is computed. The long way solution
// may be requested by giving a negative value to the corresponding
// time-of-flight [tf].
//
// For problems with |m| > 0, there are generally two solutions. By
// default, the right branch solution will be returned. The left branch
// may be requested by giving a negative value to the corresponding
// number of complete revolutions [m].

// Authors
// -------
// Name       : Dr. Dario Izzo
// E-mail     : dario.izzo@esa.int
// Affiliation: ESA / Advanced Concepts Team (ACT)
// Made readible and optimized for speed by Rody P.S. Oldenhuis
// Code available in MGA.M on   http://www.esa.int/gsp/ACT/inf/op/globopt.htm

// Declarations:
  
// Code
  if (~exists('direction','local')); direction = 'pro'; end;
  if (~exists('m','local')); m = 0; end;
  if (~exists('leftbranch','local')); leftbranch = %f; end;
  if (~exists("mu", "local")); mu = CL__dataGetEnv("mu"); end
  
  N = size(pos1,2);

  left_branch = 0;
  // Decide whether to use the left or right branch (for multi-revolution
    // problems)
  if(leftbranch) then left_branch = -1; else left_branch = 1; end;
  
  if(find(m < 0)~=[] ) then CL__error("m must be >= 0"); end;
  if(find(tf <= 0)~=[]) then CL__error("tf must be > 0"); end;
  
  // Consistency of inputs :
  Npos2 = size(pos2,2);
  Ntf = size(tf,2);
  if( Ntf == 1) then tf = tf * ones(1,N); end;
  Ntf = size(tf,2);
  if( Npos2 ~= N) then CL__error('pos1 and pos2 must be of same size'); end;
  if( Ntf ~= N) then CL__error('tf must be of size 1 or the same size as pos1'); end;
  
    // initial values        
    tol = 1e-12;    bad = %f; 
    // work with non-dimensional units
    [pos1,r1] = CL_unitVector(pos1);   
    V  = sqrt(mu ./ r1);   pos2 = CL_dMult(1 ./ r1 , pos2 );
    T  = r1 ./ V;          tf   = tf ./ T; // also transform to seconds

    // relevant geometry parameters (non dimensional)
  // unit vector for normalized [pos2]
    [r2n,mr2vec] = CL_unitVector(pos2);
    // make sure that (-1 <= dth <= +1)
    dth = acos( max(-1, min(1, (CL_dot(pos1,pos2)) ./ mr2vec)) );          
    c12 = CL_cross(pos1,pos2);

  // Determine whether to take the long way or short way whether the orbit is prograde or retrograde:
  long_way = ones(1,N);  // 1 = short way ; -1 = long way
  if( direction == 'pro')
    ii = find( c12(3,:) <0 );
    dth(ii) = 2*%pi - dth(ii);
    long_way(ii) = -1;
  elseif(  direction == 'retro')
    ii = find( c12(3,:) >= 0 );
    dth(ii) = 2*%pi - dth(ii);
    long_way(ii) = -1
  else
    CL__error('direction has to be ''pro'' or ''retro'' ');
  end
  

    // derived quantities        
    c      = sqrt(1 + mr2vec.^2 - 2*mr2vec.*cos(dth)); // non-dimensional chord
    s      = (1 + mr2vec + c)/2;                     // non-dimensional semi-perimeter
    a_min  = s/2;                                    // minimum energy ellipse semi major axis
    Lambda = sqrt(mr2vec).*cos(dth/2) ./ s;              // lambda parameter (from BATTIN's book)
    [nrmunit,mcr] = CL_unitVector(c12);           // unit vector and magnitude
    
    // Initial values
    //   
  
    logt = log(tf); // avoid re-computing the same value
    
    // single revolution (1 solution)
    if (m == 0)

        // initial values        
        inn1 = -0.5233*ones(1,N);      // first initial guess
        inn2 = +0.5233*ones(1,N);      // second initial guess

        x1   = log(1 + inn1);// transformed first initial guess
        x2   = log(1 + inn2);// transformed first second guess

        // multiple revolutions (0, 1 or 2 solutions)
        // the returned solution depends on the sign of [m]
    else            
        // select initial values
        if (left_branch < 0)
            inn1 = -0.5234*ones(1,N); // first initial guess, left branch
            inn2 = -0.2234*ones(1,N); // second initial guess, left branch
        else
            inn1 = +0.7234*ones(1,N); // first initial guess, right branch
            inn2 = +0.5234*ones(1,N); // second initial guess, right branch
        end        
        x1 = tan(inn1*%pi/2);// transformed first initial guess
        x2 = tan(inn2*%pi/2);// transformed first second guess
    end
  
    // since (inn1, inn2) < 0, initial estimate is always ellipse
    xx   = [inn1; inn2];  aa = CL_dMult(a_min ,1 ./ (1 - xx.^2));
    bbeta = CL_dMult(long_way , 2*asin(sqrt(CL_dMult((s-c)/2 , 1 ./ aa))));
    // make sure that (-1 <= xx <= +1)
    aalfa = 2*acos(  max(-1, min(1, xx)) );
    // evaluate the time of flight via Lagrange expression
    y12  = aa.*sqrt(aa).*((aalfa - sin(aalfa)) - (bbeta-sin(bbeta)) + 2*%pi*m);

    // initial estimates for y
    if m == 0
        y1 = log(y12(1,:)) - logt;
        y2 = log(y12(2,:)) - logt;
    else
        y1 = y12(1,:) - tf;
        y2 = y12(2,:) - tf;
    end

    // Solve for x
    //
    //
    // Newton-Raphson iterations
    // NOTE - the number of iterations will go to infinity in case
    // m > 0  and there is no solution. Start the other routine in 
    // that case
  I = 1 : N;  xnew = zeros(1,N);
    iterations = 0;  
    while (I ~= [])
    
        // increment number of iterations
        iterations = iterations + 1;
        // new x
        xnew(I) = (x1(I).*y2(I) - y1(I).*x2(I)) ./ (y2(I)-y1(I));
        // copy-pasted code (for performance)
        if m == 0, x = exp(xnew(I)) - 1; else x = atan(xnew(I))*2/%pi; end
        a = a_min(I) ./ (1 - x.^2);
      
    Ie = find(x < 1); // ellipse
    bbeta = zeros(x);
    alfa = zeros(x);
    if(Ie ~= [])
            bbeta(Ie) = CL_dMult(long_way(I(Ie)) , 2*asin(sqrt(CL_dMult((s(I(Ie))-c(I(Ie)))/2 , 1 ./ a(Ie)))) );
            alfa(Ie) = 2*acos( max(-1, min(1, x(Ie))) ); // make sure that (-1 <= xx <= +1)
    end    
        Ih = find(x >= 1 );  // hyperbola
    if(Ih ~= [])
            alfa(Ih) = 2*acosh(x(Ih));
            bbeta(Ih) = CL_dMult(long_way(I(Ih)) , 2*asinh(sqrt(CL_dMult((s(I(Ih))-c(I(Ih)))/2 , -1 ./ a(Ih)))) );
    end
        // evaluate the time of flight via Lagrange expression
    Ia = find(a > 0);
    tof = zeros(a);
    if(Ia ~= [])
            tof(Ia) = a(Ia).*sqrt(a(Ia)).*((alfa(Ia) - sin(alfa(Ia))) - (bbeta(Ia)-sin(bbeta(Ia))) + 2*%pi*m);
    end
        Ia2 = find(a <= 0);
    if(Ia2 ~= [])
            tof(Ia2) = -a(Ia2).*sqrt(-a(Ia2)).*((sinh(alfa(Ia2)) - alfa(Ia2)) - (sinh(bbeta(Ia2)) - bbeta(Ia2)));
    end
        // new value of y
        if m ==0, ynew = log(tof) - logt(I); else ynew = tof - tf(I); end
        // save previous and current values for the next iterarion
        // (prevents getting stuck between two values)
        x1(I) = x2(I);  x2(I) = xnew(I);
        y1(I) = y2(I);  y2(I) = ynew;
        // update error
        err = abs(x1 - xnew);
    I = find(err > tol);
        // escape clause
        if (iterations > 15), bad = %t; break; end
    end
   
    // convert converged value of x
    if m==0, x = exp(xnew) - 1; else x = atan(xnew)*2/%pi; end
    
    //
    //      The solution has been evaluated in terms of log(x+1) or tan(x*%pi/2), we
    //      now need the conic. As for transfer angles near to %pi the Lagrange-
    //      coefficients technique goes singular (dg approaches a zero/zero that is
    //     numerically bad) we here use a different technique for those cases. When
    //      the transfer angle is exactly equal to %pi, then the ih unit vector is not
    //      determined. The remaining equations, though, are still valid.
    //}

    // Solution for the semi-major axis
    a = a_min ./ (1-x.^2);

    // Calculate psi
  Ie = find(x < 1); // ellipse
  bbeta = zeros(x);
  alfa = zeros(x);
  psi = zeros(x);
  eta2 = zeros(x);
  eta = zeros(x);
    if (Ie ~= [])
    bbeta(Ie) = CL_dMult(long_way(Ie) , 2*asin(sqrt(CL_dMult((s(Ie)-c(Ie))/2 , 1 ./ a(Ie)))) );
        // make sure that (-1 <= xx <= +1)
        alfa(Ie) = 2*acos( max(-1, min(1, x(Ie))) );
        psi(Ie)  = (alfa(Ie)-bbeta(Ie))/2;
        eta2(Ie) = 2*a(Ie).*sin(psi(Ie)).^2 ./ s(Ie);
        eta(Ie)  = sqrt(eta2(Ie));
  end
  Ih = find(x >= 1); // hyperbola
  if(Ih ~= [])
    bbeta(Ih) = CL_dMult(long_way(Ih) , 2*asinh(sqrt(CL_dMult((s(Ih)-c(Ih))/2 , -1 ./ a(Ih)))) );
        alfa(Ih) = 2*acosh(x(Ih));
        psi(Ih)  = (alfa(Ih)-bbeta(Ih))/2;
        eta2(Ih) = -2*a(Ih).*sinh(psi(Ih)).^2 ./ s(Ih);
        eta(Ih)  = sqrt(eta2(Ih));
    end
  
    // unit of the normalized normal vector
    ih = CL_dMult(long_way , nrmunit);

    // cross-products
    crsprd1 = CL_cross(ih,pos1);
    crsprd2 = CL_cross(ih,r2n);

    // radial and tangential directions for departure velocity
    Vr1 = 1 ./ eta ./ sqrt(a_min) .* (2*Lambda.*a_min - Lambda - x.*eta);
    Vt1 = sqrt(mr2vec ./ a_min ./ eta2 .* sin(dth/2).^2);

    // radial and tangential directions for arrival velocity
    Vt2 = Vt1 ./ mr2vec;
    Vr2 = (Vt1 - Vt2) ./ tan(dth/2) - Vr1;
    
    // terminal velocities
    v1 = CL_dMult( V  ,  (CL_dMult(Vr1,pos1) + CL_dMult(Vt1,crsprd1))  );
    v2 = CL_dMult( V  ,  (CL_dMult(Vr2,r2n) + CL_dMult(Vt2,crsprd2))  );
    
    // If the Newton-Raphson scheme failed, return %nan
    if bad
        v1(:,I) = %nan;
    v2(:,I) = %nan;
    end
    
    // also compute minimum distance to central body
    // NOTE: use un-transformed vectors again!
    //extremal_distances = ...
    //    minmax_distances(pos1*r1, r1, pos2*r1, mr2vec*r1, dth, a*r1, v1, v2, m, mu);
   
endfunction