//  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_cw_lambert(p1,p2,delta_t,alt,acc,er,mu)
// Lambert's problem using Clohessy-Whiltshire model (including constant acceleration)
//
// Calling Sequence
// [v1,v2] = CL_cw_lambert(p1,p2,delta_t,alt [,acc,er,mu])
//
// Description
// <itemizedlist><listitem>
// <p>Computes the initial and final velocities of a transfer knowing the initial and final positions.</p>
// <p></p>
// <p>The reference frame used is the target's LVLH local frame (origin = target). </p>
// <p>The reference altitude <b>alt</b> is the altitude of the target = semi major axis 
// of target's (circular) orbit minus equatorial radius. </p>
// <p></p>
// <p><b>acc</b> is the differential inertial acceleration (corresponding to a physical force) 
// with components in the reference frame. </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// p1 : Initial (relative) position vector, components in reference frame [m] (3xN or 3x1) 
// p2 : Final (relative) position vector, components in reference frame [m] (3xN or 3x1) 
// delta_t : Transfer time [s] (1xN or 1x1)
// alt: Reference altitude (= altitude of target) [m] (1x1)
// acc: (optional) Differential acceleration, components in reference frame (m/s^2). Default is []. (3xN or 3x1 or [])
// er: (optional) Equatorial radius [m]. Default is %CL_eqRad
// mu: (optional) Gravitational constant [m^3/s^2]. Default value is %CL_mu
// v1: Initial (relative) velocity vector in reference frame [m/s] (3xN) 
// v2: Final (relative) velocity vector in reference frame [m/s] (3xN) 
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_cw_propagate
//
// Bibliography
// 1) Mecanique spatiale, CNES - Cepadues 1995, Tome II, 16.3
// 2) Orbital Mechanics for engineering students, H D Curtis, Chapter 7 (section 7.5)
//
// Examples
// p1 = [10e3;10.e3;100.e3];
// p2 = [-10e3;100.e3;20.e3];
// delta_t = 1000; // sec
// alt = 450e3;
// [v1,v2] = CL_cw_lambert(p1,p2,delta_t,alt)
// // Check
// t1 = 0; 
// t2 = delta_t/86400; // days
// [p2;v2] - CL_cw_propagate(t1,[p1;v1],t2,alt) // => 0

// Declarations:

// Code:
if (~exists('acc','local')) then acc=[]; end
if (~exists("er", "local")); er = CL__dataGetEnv("eqRad"); end
if (~exists("mu", "local")); mu = CL__dataGetEnv("mu"); end

// Note: if acc is [], it remains [] after  CL__checkInputs
[p1,p2,delta_t,alt,acc,N] = CL__checkInputs(p1,3,p2,3,delta_t,1,alt,1,acc,3);

v1 = zeros(3,N);
v2 = zeros(3,N);

matM = CL_cw_Mmatrix(alt,delta_t,er,mu);
matN = []; 

// if no acceleration => optimize computation
if (acc <> [])
  matN = CL_cw_Nmatrix(alt,delta_t,er,mu);
end

// loop as "inv" is not vectorized"
for (k = 1 : N)
  matM_pp = matM(1:3,1:3,k);
  matM_pv = matM(1:3,4:6,k);
  matM_vp = matM(4:6,1:3,k);
  matM_vv = matM(4:6,4:6,k);
  
  if (acc <> [])
    matN_p  = matN(1:3,1:3,k);
    matN_v  = matN(4:6,1:3,k);
    pa = matN_p * acc(:,k); 
    va = matN_v * acc(:,k);
  else
    pa = zeros(3,1); 
    va = zeros(3,1); 
  end
  
  v1(:,k) = inv(matM_pv) * (p2(:,k) - matM_pp * p1(:,k) - pa); 
  v2(:,k) = matM_vp * p1(:,k) + matM_vv * v1(:,k) + va; 
  
end


endfunction