context: /*****************************************************************************
   *   Copyright (C) 2004-2015 The PaGMO development team,                     *
   *   Advanced Concepts Team (ACT), European Space Agency (ESA)               *
   *                                                                           *
   *   https://github.com/esa/pagmo                                            *
   *                                                                           *
   *   act@esa.int                                                             *
   *                                                                           *
   *   This program is free software; you can redistribute it and/or modify    *
   *   it under the terms of the GNU General Public License as published by    *
   *   the Free Software Foundation; either version 2 of the License, or       *
   *   (at your option) any later version.                                     *
   *                                                                           *
   *   This program is distributed in the hope that it will be useful,         *
   *   but WITHOUT ANY WARRANTY; without even the implied warranty of          *
   *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           *
   *   GNU General Public License for more details.                            *
   *                                                                           *
   *   You should have received a copy of the GNU General Public License       *
   *   along with this program; if not, write to the                           *
   *   Free Software Foundation, Inc.,                                         *
   *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.               *
   *****************************************************************************/
  
  /*
     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=Math.Log(1+cos(alpha/2)). By doing so the
     graphs 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.
  
  Inputs:
  r1=Position vector at departure (column)
  r2=Position vector at arrival (column, same units as r1)
  t=Transfer time (scalar)
  mu=gravitational parameter (scalar, units have to be
  consistent with r1,t units)
  lw=1 if long way is chosen
  
  
  Outputs:
  v1=Velocity at departure        (consistent units)
  v2=Velocity at arrival
  a=semi major axis of the solution
  p=semi latus rectum of the solution
  theta=transfer angle in rad
  iter=number of iteration made by the newton solver (usually 6)
  */
  
  using System.Diagnostics;
  using System;
  class Pagmo
  { 
      double Acosh(double x)
      {
          return Math.Log(x + Math.Sqrt(x * x - 1));
      }
      double Asinh(double x)
      {
          return Math.Log(x + Math.Sqrt(x * x + 1));
      }
      //FIXME
      double x2tof(double a, double b, double c, bool d)
      {
          return a + b;
      }
  void LambertI (ref double[] r1_in, ref double[] r2_in, double t, double mu, //INPUT
  		ref bool lw, //INPUT
  		ref double[] v1, ref double[] v2, ref double a, ref double p, ref double theta, ref int iter)//OUTPUT
  {
  	double V,T,
  		r2_mod = 0.0,    // R2 module
  		dot_prod = 0.0, // dot product
  		c,        // non-dimensional chord
  		s,        // non dimensional semi-perimeter
  		am,        // minimum energy ellipse semi major axis
  		lambda,        //lambda parameter defined in Battin's Book
  		x,x1,x2,y1,y2,x_new=0,y_new,err,alfa,beta,psi,eta,eta2,sigma1,vr1,vt1,vt2,vr2,R=0.0;
  	int i_count, i;
  	double tolerance = 1e-11;
      double[] r1 = new double[3];
      double[] r2 = new double[3];
      double[] r2_vers = new double[3];
  	double[] ih_dum = new double[3];
      double[] ih = new double[3];
      double[] dum = new double[3];
  
  	// Increasing the tolerance does not bring any advantage as the
  	// precision is usually greater anyway (due to the rectification of the tof
  	// graph) except near particular cases such as parabolas in which cases a
  	// lower precision allow for usual convergence.
  
  	if (t <= 0)
  	{
  		//pagmo_throw(value_error,"ERROR in Lambert Solver: Negative Time in input.");
  		Debug.Assert(false, "error in lambert solver: negative time in input");
  	}
  
  	for (i = 0; i < 3; i++)
  	{
  		r1[i] = r1_in[i];
  		r2[i] = r2_in[i];
  		R += r1[i]*r1[i];
  	}
  
  	R = Math.Sqrt(R);
  	V = Math.Sqrt(mu/R);
  	T = R/V;
  
  	// working with non-dimensional radii and time-of-flight
  	t /= T;
  	for (i = 0;i <3;i++)  // r1 dimension is 3
  	{
  		r1[i] /= R;
  		r2[i] /= R;
  		r2_mod += r2[i]*r2[i];
  	}
  
  	// Evaluation of the relevant geometry parameters in non dimensional units
  	r2_mod = Math.Sqrt(r2_mod);
  
  	for (i = 0;i < 3;i++)
  		dot_prod += (r1[i] * r2[i]);
  
  	theta = Math.Acos(dot_prod/r2_mod);
  
  	if (lw)
  		theta=2*Math.Acos(-1.0)-theta;
  
  	c = Math.Sqrt(1 + r2_mod*(r2_mod - 2.0 * Math.Cos(theta)));
  	s = (1 + r2_mod + c)/2.0;
  	am = s/2.0;
  	lambda = Math.Sqrt (r2_mod) * Math.Cos (theta/2.0)/s;
  
  	// We start finding the Math.Log(x+1) value of the solution conic:
  	// NO MULTI REV --> (1 SOL)
  	//inn1=-.5233;    //first guess point
  	//  inn2=.5233;     //second guess point
  	x1=Math.Log(0.4767);
  	x2=Math.Log(1.5233);
  	y1=Math.Log(x2tof(-.5233,s,c,lw))-Math.Log(t);
  	y2=Math.Log(x2tof(.5233,s,c,lw))-Math.Log(t);
  
  	// Regula-falsi iterations
  	err=1;
  	i_count=0;
  	while ((err>tolerance) && (y1 != y2))
  	{
  		i_count++;
  		x_new=(x1*y2-y1*x2)/(y2-y1);
  		y_new=Math.Log(x2tof(Math.Exp(x_new)-1,s,c,lw))-Math.Log(t);
  		x1=x2;
  		y1=y2;
  		x2=x_new;
  		y2=y_new;
  		err = Math.Abs(x1-x_new);
  	}
  	iter = i_count;
  	x = Math.Exp(x_new)-1;
  
  	// The solution has been evaluated in terms of Math.Log(x+1) or tan(x*pi/2), we
  	// now need the conic. As for transfer angles near to pi the lagrange
  	// coefficient 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.
  
  	a = am/(1 - x*x);
  
  	// psi evaluation
  	if (x < 1)  // ellipse
  	{
  		beta = 2 * Math.Asin (Math.Sqrt( (s-c)/(2*a) ));
  		if (lw) beta = -beta;
  		alfa=2*Math.Acos(x);
  		psi=(alfa-beta)/2;
  		eta2=2*a*Math.Pow(Math.Sin(psi),2)/s;
  		eta=Math.Sqrt(eta2);
  	}
  	else       // hyperbola
  	{
  		beta = 2*Asinh(Math.Sqrt((c-s)/(2*a)));
  		if (lw) beta = -beta;
  		alfa = 2*Acosh(x);
  		psi = (alfa-beta)/2;
  		eta2 = -2 * a * Math.Pow(Math.Sinh(psi),2)/s;
  
gt: 		eta = Math.Sqrt(eta2);