//  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 [varargout] = CL_ip_flybyVectors(type_in, vect1, vect2, output, mu)
// Conversion between various fly-by vectors (hyperbolic orbits)
//
// Calling Sequence
// [par_out1, par_out2, ...] = CL_ip_flybyVectors(type_in, vect1, vect2, output [, mu]) 
// [par_out1, par_out2, ...] = CL_ip_flybyVectors("pv", pos, vel, output [, mu])
// [par_out1, par_out2, ...] = CL_ip_flybyVectors("pvinfa", pinfva, vinfva, output [, mu])
// [par_out1, par_out2, ...] = CL_ip_flybyVectors("pvinfd", pinfvd, vinfvd, output [, mu])
// [par_out1, par_out2, ...] = CL_ip_flybyVectors("vvinf", vinfva, vinfvd, output [, mu])
// 
// Description
// <itemizedlist><listitem>
// <p>Given two vectors characterizing a fly-by, computes other ones. </p>
// <p></p>
// <p>The argument <b>type_in</b> describes the nature of the input vectors: </p>
// <p>- <b>pv</b>: Position and velocity on the hyperbola (pos and vel) - any anomaly </p>
// <p>- <b>pvinfa</b>: Arrival impact and excess velocity vectors (pinfva and vinfva)</p>
// <p>- <b>pvinfd</b>: Departure impact and excess velocity vectors (pinfvd and vinfvd)</p>
// <p>- <b>vvinf</b>: Arrival and departure excess velocity vectors (vinfva and vinfvd)</p>
// <p></p>
// <p>The quantities that can be computed are given in <b>output</b> and can be: </p>
// <p>- <b>posp</b>: Position vector at periapsis </p>
// <p>- <b>velp</b>: Velocity vector at periapsis </p>
// <p>- <b>vinfva</b>: Arrival excess velocity vector </p>
// <p>- <b>vinfvd</b>: Departure excess velocity vector </p>
// <p>- <b>pinfva</b>: Arrival impact vector (perpendicular to excess velocity vector)</p>
// <p>- <b>pinfvd</b>: Departure impact vector (perpendicular to excess velocity vector)</p>
// <p></p>
// <p>The output vectors are given in the same frame as the input vectors. </p>
// <p></p>
// <p><b>Notes</b>:</p>
// <p>1) Input impact vector (case type_in = pvinf): </p>
// <p>The impact vector pinfva does not need to be perpendicular to the excess velocity vector (vinfva). 
// pinfva can be any vector from the body center to a point M on the arrival aymptote. </p>
// <p>2) Input departure excess velocity vector (case type_in = vvinf): </p>
// <p>It is not checked whether the norm of vinfva is identical to the norm of the departure excess 
// velocity vector (vinfvd). The norm of vinfvd is actually not used. </p>
// </listitem></itemizedlist>
//
// Parameters
// type_in: (string) Nature of input vectors: "pv", "pvinf" or "vvinf". (1x1)
// pos, vel: Position [m] and velocity [m/s] vectors. (3xN or 3x1)
// pinfva, vinfva: Arrival impact [m] and excess velocity [m/s] vectors. (3xN or 3x1) 
// pinfvd, vinfvd: Departure impact [m] and excess velocity [m/s] vectors. (3xN or 3x1) 
// vinfva, vindvd: Arrival and departure excess velocity vectors [m/s]. (3xN or 3x1)
// output: (string) Name of outputs: "posp", "velp", "vinfva", "vinfvd", "pinfva" or "pinfvd". (1xP)
// mu: (optional) Gravitational constant [m^3/s^2]. Default value is %CL_mu.
// par_out1, par_out2, ...: Output vectors [m, m/s]. (3xN) 
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_ip_flybyParams
//
// Examples
// // Earth fly-by - Arrival impact and excess velocity vectors (m and m/s)
// // Note: the vectors are chosen perpendicular to each other. 
// pinfva = [15000.e3; 0; 0]; 
// vinfva = [0; 6.e3; 3.e3]; 
//
// // Position and velocity vectors at periapsis (m and m/s) 
// [posp, velp] = CL_ip_flybyVectors("pvinfa", pinfva, vinfva, ["posp", "velp"])
//
// // Departure excess velocity vector from position/velocity vectors (m/s)
// [vinfvd] = CL_ip_flybyVectors("pv", posp, velp, ["vinfvd"])
//
// // Impact vector from excess velocity vectors
// // Note: computed perpendicular to excess velocity vector
// CL_ip_flybyVectors("vvinf", vinfva, vinfvd, ["pinfva"]) - pinfva // => 0
// 

// Declarations:

// ---------------------------------------------------------------
// Computes position and velocity vectors at periapsis from arrival 
// or departure impact and excess velocity vectors. 
// arrdep: "a" for arrival, "d" for departure
// All vectors: 3xN
// Returns %nan if inputs are ill-defined
//
// NB: pinfv is any point on the asymptote. It may not be 
// perpendicular to vinfv
// ---------------------------------------------------------------
function [posp, velp] = flybyVect_pvInf2pvP(pinfv, vinfv, arrdep, mu)
  // NB: vinfv_u = %nan if norm(vinfv) is 0
  [vinfv_u, vinf]  = CL_unitVector(vinfv); 

  // Distance from the body center to the asymptote (orthogonal projection)
  dinf = CL_norm(CL_cross(vinfv_u, pinfv));
 
  // Periapsis radius, flyby turn-angle and velocity at periapsis
  // (%nan returned if dinf or vinf == 0) 
  [rp, turnang, vp] = CL_ip_flybyParams("vinf", vinf, "dinf", dinf, ["rp", "turnang", "vp"], mu = mu);

  // Define the "impact" frame:
  // Y axis: // vinfva
  // Z axis: // pinfva ^ vinfva
  [M, Inok] = CL_rot_defFrameVec(vinfv, pinfv, 2, 1);

  // Outputs in initial frame
  // sign of theta: + for arrival, - for departure
  theta = turnang / 2; 
  if (arrdep == "d"); theta = -theta; end
  posp = M' * CL_dMult([ cos(theta); sin(theta); zeros(theta)], rp); 
  velp = M' * CL_dMult([-sin(theta); cos(theta); zeros(theta)], vp); 

  // Check impact frame is correct defined
  if (Inok <> [])
    posp(:,Inok) = %nan;
    velp(:,Inok) = %nan;
  end

endfunction

// ---------------------------------------------------------------
// Computes position and velocity vectors at periapsis from arrival 
// and departure excess velocity vectors. 
// All vectors: 3xN
// Returns %nan if inputs are ill-defined
// 
// NB: The norm of vinfva has no impact (provided it is not 0). 
// Only its direction has. 
// ---------------------------------------------------------------
function [posp, velp] = flybyVect_vvInf2pvP(vinfva, vinfvd, mu)
  // NB: vinfva_u = %nan vector if norm(vinfva) is 0
  [vinfva_u, vinf]  = CL_unitVector(vinfva); 

  // Flyby turn angle in [0, %pi]
  turnang = CL_vectAngle(vinfva, vinfvd);

  // Periapsis radius, flyby turn-angle and velocity at periapsis
  // (%nan returned if dinf or vinf == 0) 
  [rp, dinf, vp] = CL_ip_flybyParams("vinf", vinf, "turnang", turnang, ["rp", "dinf", "vp"], mu = mu);

  // Define the "impact" frame:
  // Y axis: // vinfva
  // Z axis: // vinfva ^ vinfvd
  [M,Inok] = CL_rot_defFrameVec(vinfva, -vinfvd, 2, 1);

  // Outputs in initial frame
  theta = turnang / 2; 
  posp = M' * CL_dMult([ cos(theta); sin(theta); zeros(theta)], rp); 
  velp = M' * CL_dMult([-sin(theta); cos(theta); zeros(theta)], vp); 

  // Check impact frame is correct defined
  if (Inok <> [])
    posp(:,Inok) = %nan;
    velp(:,Inok) = %nan;
  end

endfunction

// ---------------------------------------------------------------
// Computes position and velocity vectors at periapsis from position 
// and velocity (any anomaly) 
//
// All vectors: 1xN
// Error if orbit not hyperbolic
// ---------------------------------------------------------------
function [posp, velp] = flybyVect_posvel2pvP(pos, vel, mu)
  kep = CL_oe_car2kep(pos, vel, mu = mu); 

  [isvalid, orbit_type] = CL__oe_isValid("kep", kep, mu);
  
  if (orbit_type <> 2 | ~isvalid) 
    CL__error("Invalid orbital elements (orbit may not be hyperbolic)"); 	
  end
 
  // Position and velocity at periapsis (v = M = 0)
  kep(6,:) = 0; 
  [posp, velp] = CL_oe_kep2car(kep, mu = mu);

endfunction


// ==================
// MAIN
// ==================

// ------------------
// Argument Checking
// ------------------
if (~exists("mu", "local")); mu = CL__dataGetEnv("mu"); end

// number of inputs (lhs) and output (rhs) arguments
[lhs, rhs] = argn();

if (rhs <= 3)
  CL__error("Invalid number of input arguments (at least 3 expected)");
end

if (typeof(type_in) <> "string" | size(type_in, "*") <> 1)
  CL__error("Invalid type or size for type_in");
end

InputNames = ["pv", "pvinfa", "pvinfd", "vvinf"];
if (setdiff(type_in, InputNames) <> [])
  CL__error("Invalid names for type_in");
end

if (typeof(output) <> "string" | size(output,1) <> 1)
  CL__error("Invalid type or size for output");
end

if (size(output,2) <> lhs)
  CL__error("Invalid number of output arguments");
end

OutputNames = ["posp", "velp", "vinfva", "vinfvd", "pinfva", "pinfvd"];
if (setdiff(output, OutputNames) <> [])
  CL__error("Invalid names for outputs");
end

// Check sizes / resize
[vect1, vect2] = CL__checkInputs(vect1, 3, vect2, 3);

// Check zero values in input vectors => %nan
I = find(CL_dot(vect1) == 0 | CL_dot(vect2) == 0); 
vect1(:,I) = %nan; 
vect2(:,I) = %nan; 


// ------------------
// Computations
// ------------------

// -- STEP1 --
// Compute position and velocity at periapsis from inputs

if (type_in == "pvinfa")
  [posp, velp] = flybyVect_pvInf2pvP(vect1, vect2, "a", mu); 

elseif (type_in == "pvinfd")
  [posp, velp] = flybyVect_pvInf2pvP(vect1, vect2, "d", mu); 
  
elseif (type_in == "vvinf")
  [posp, velp] = flybyVect_vvInf2pvP(vect1, vect2, mu); 

else // if (type_in == "pv")
  [posp, velp] = flybyVect_posvel2pvP(vect1, vect2, mu); 

end

// -- STEP2 --
// Compute requested outputs from posp and velp
// Optimization: avoids needless computation if only posp / velp are requested. 
// Initializes potentially used variables although this would not be necessary. 
M = []; 
Mip = []; 
Pinf = []; 
Vinf = []; 

if (setdiff(output, ["posp", "velp"]) <> [])

  // Norm of velocity at infinity: V^2 / 2 - mu / r = Vinf^2 / 2
  // NB: Orbit should be hyperbolic ! 
  vinf = real(sqrt(CL_dot(velp) - 2 * mu ./ CL_norm(posp)));

  // Impact distance and turn angle
  [dinf, turnang] = CL_ip_flybyParams("vinf", vinf, "rp", CL_norm(posp), ["dinf", "turnang"], mu = mu);

  // Define transformation matrix from initial frame to frame at perigee: 
  // X axis: // posp
  // Y axis: // velp
  // Z axis: // posp ^ velp 
  M = CL_rot_defFrameVec(posp, velp, 1, 2);

  // Define transformation matrix from arrival impact frame to frame at perigee
  //      = transformation matrix from frame at perigee to departure impact frame
  // Y axis: // vinfv
  // Z axis: // dinfv ^ vinfv
  Mip = CL_rot_angles2matrix(3, turnang / 2); 

  // Impact and excess velocity vectors in "impact" frame
  Pinf = [dinf; zeros(dinf); zeros(dinf)]; 
  Vinf = [zeros(vinf); vinf; zeros(vinf)]; 

end
  
// Compute outputs (in initial frame)

for k = 1 : size(output, 2); 
  name = output(k); 
  
  if (name == "posp")
    vec = posp; 
	
  elseif (name == "velp")
    vec = velp; 

  elseif (name == "vinfva")
    vec = M' * (Mip *  Vinf); 
	
  elseif (name == "vinfvd")
    vec = M' * (Mip' * Vinf); 
	
  elseif (name == "pinfva")
    vec = M' * (Mip *  Pinf); 
	
  else // if (name == "pinfvd")
    vec = M' * (Mip' * Pinf); 
	
  end
  
  varargout(k) = vec; 
end

endfunction