//  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 [ang2] = CL_op_equatorialSwath(cmd,sma,inc,ang1, er,mu,j2,rotr_pla)
// Intersection of swath with equator (circular orbit and field of view)
//
// Calling Sequence
// ang2 = CL_op_equatorialSwath(cmd,sma,inc,ang1 [,er,mu,j2,rotr_pla])
// longitude_span = CL_op_equatorialSwath("cen2dlon",sma,inc,center_angle [,er,mu,j2,rotr_pla])
// center_angle = CL_op_equatorialSwath("dlon2cen",sma,inc,longitude_span [,er,mu,j2,rotr_pla])
//
// Description
// <itemizedlist><listitem>
// <p>Computes the (half) length of the portion of the equator seen by the satellite  
// as the satellite crosses the equator.</p>
// <p>There are 2 possibilities.</p>
// <p>- The field of view (defined by a center angle) is known: the span in longitude 
// is then computed 
// (cmd should be "cen2dlon").</p>
// <p>- The span in longitude is known: the field of view (defined by a center angle) 
// is then computed (cmd should be "dlon2cen").</p>
// <p></p></listitem>
// <listitem>
// <p><b>Notes:</b></p> 
// <p>- The planet is assumed spherical, and the orbit circular. </p> 
// <p>- The effects of J2 and Earth rotation are taken into account. </p>
// <p>- The input center angle and span in longitude should be in [0,pi] and [0, 2*pi] respectively. </p> 
// <p></p></listitem>
// <listitem>
// <p><b>Warning:</b></p>
// <p>There are two temporary limitations: </p>
// <p>- Center angle must be less than pi/2. </p>
// <p>- Semi major axis and inclination must be such that there is less
// than one revolution of the satellite per (orbital) day. </p>
// <p> Otherwise, the value %nan is returned. </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// cmd : (string) Action required: "cen2dlon" or "dlon2cen" 
// sma : Semi major axis [m] (1xN or 1x1)
// inc : Inclination [rad] (1xN or 1x1)
// ang1 : Input angle: center angle or half longitude span [rad] (1xN or 1x1)
// er : (optional) Equatorial radius [m] (default is %CL_eqRad)
// mu : (optional) Gravitational constant [m^3/s^2] (default value is %CL_mu)
// j2 : (optional) Second zonal harmonic (default is %CL_j1jn(2))
// rotr_pla : (optional) Rotation rate of the planet (default is %CL_rotrBody)
// ang2 : Output angle (half longitude span or center angle) [rad] (1xN)
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_gm_visiParams
//
// Examples
// sma = 10000.e3;
// inc = 0.5; // rad
// dlon = CL_op_equatorialSwath("cen2dlon", sma, inc, 0.3)
// cen = CL_op_equatorialSwath("dlon2cen", sma, inc, dlon)


// --------------------------------------
// Find initial guess for function findmin.
// The function starts looking at (xmin+xmax)/2 and extends towards xmin and xmax,
// until it finds an abscissa such that the 2nd derivative is positive.
// --------------------------------------
function xc = findmin_init(N, fct, args, xmin, xmax, nb_step)
  
  // K = index for which initial guess not yet found
  K = 1:N;
  C = zeros(K);
  
  step = 0.5*(xmin-xmax) / nb_step;
  
  // n_tab = [0,1,-1,2,-2,... ,nb_step,-nb_step]
  n_tab = [[0:nb_step];[0:-1:-nb_step]];
  n_tab = n_tab(:)';
  n_tab = n_tab(2:$);
  
  for n = n_tab
    if (K == []); break; end;
    K = find(C <= 0);
    xc(K) = (xmin(K)+xmax(K))/2 + n*step(K); 
    [fopt, gopt, C(K)] = fct(xc(K), K, args);
  end
  
  // no initial guess found --> %nan
  xc(K) = %nan;
endfunction

// --------------------------------------
// specific minimum search based on dichotomy and using derivatives
// N: size of vectors
// [f, fdot, fdotdot] = fct(x,K,args): function 
// xmin, xmax: bounds (1xN)
// dxtol: tolerance on solution (1x1 or 1xN)
// IMPORTANT: 
// The function 2nd derivative is supposed to change sign only once
// The algorithm is then : 
// - look for the value x such that fct(x) is minimum and the 2nd derivative 
//   has the same size as fct((xmin+xmax)/2) 
// - compare the fct(x) with fct(xmin) and fct(xmax)
// --------------------------------------
function [fopt, xopt] = findmin(N, fct, args, xmin, xmax, dxtol)

  if (~exists("dxtol", "local")); dxtol = -1; end

  itermax = 100; 
  K = 1:N; 

  xopt = zeros(K); 
  fopt = zeros(K);
  gopt = zeros(K);  // 1st derivative
  hopt = zeros(K);  // 2nd derivative

  // Initial guess : 
  xc = findmin_init(N, fct, args, xmin, xmax, nb_step=5); 
  
  [fopt, gopt, C] = fct(xc, K, args);  

  xmin0 = xmin; 
  xmax0 = xmax; 
  
  iter = 1; 
  while (iter <= itermax & K <> []) 

    xopt(K) = (xmin(K)+xmax(K))/2; 

    [fopt(K), gopt(K), hopt(K)] = fct(xopt(K), K, args);  

    cond1 = (hopt(K) .* C(K) < 0);  
    I = find(cond1);
    if (I <> [])
      I = K(I); 
      i = find(xopt(I) >= xc(I)); 
      xmax(I(i)) = xopt(I(i)); 
      i = find(xopt(I) < xc(I)); 
      xmin(I(i)) = xopt(I(i)); 
    end
   
    I = find(gopt(K) <= 0 & ~cond1); 
    xmin(K(I)) = xopt(K(I)); 

    I = find(gopt(K) > 0 & ~cond1); 
    xmax(K(I)) = xopt(K(I)); 
  
    K = find(abs(xmax-xmin) > dxtol); 
    iter = iter + 1; 
  end
  
  // Here K = indice that have not converged
  // no convergence => %nan
  xopt(K) = %nan; 
  fopt(K) = %nan; 
  
  // Compare solution with fct(xmin) and fct(xmax) if converged
  // Here K = indice that have converged
  K = setdiff(1:N, K); 

  x = xmin0(K); 
  [fx] = fct(x, K, args);  
  I = find(fx < fopt(K)); 
  fopt(K(I)) = fx(I);
  xopt(K(I)) = x(I);
 
  x = xmax0(K); 
  [fx] = fct(x, K, args);  
  I = find(fx < fopt(K)); 
  fopt(K(I)) = fx(I);
  xopt(K(I)) = x(I);
  
endfunction


// --------------------------------------
// Sec = 1/cos 
// NB: may return %inf
// --------------------------------------
function [y] = Sec(x)
  cosx = cos(x); 
  I = find(cosx == 0); 
  cosx(I) = %nan; 
  y = 1 ./ cosx; 
  y(I) = %inf; 
endfunction

// --------------------------------------
// longitude of intersection of field of view 
// with equator (right side) as function of center angle
// (longitude of satellite when at ascending node = 0)
// v: argument of latitude
// r: center angle
// L, Ldot, Ldotdot: longitude of intersection + 1st and 2nd derivatives
// NB: v should be such that |declination| <= r
// (not checked in function) 
// --------------------------------------
function [L, Ldot, Ldotdot] = calcul_L(v, inc, coef, r)

  alpha1 = atan(sin(v).*cos(inc), cos(v));
  delta = asin(sin(inc) .* sin(v)); 
  alpha2 = real(acos(cos(r) .* Sec(delta))); 

  alpha1dot = cos(alpha1).^2 .* cos(inc) .* Sec(v).^2; 
  deltadot = sin(inc) .* cos(v) .* Sec(delta); 
  alpha2dot = -cos(r) .* sin(delta) .* deltadot .* CL__csc(alpha2) .* Sec(delta).^2; 

  alpha1dotdot = (sin(2*v)-sin(2*alpha1).*cos(inc)) .* alpha1dot .* Sec(v).^2; 
  deltadotdot = (-sin(inc).*sin(v)+sin(delta).*deltadot.^2) .* Sec(delta); 
  alpha2dotdot = (-(deltadot.^2).*(Sec(delta).^2) - tan(delta).*deltadotdot - (alpha2dot.^2).*(Sec(alpha2).^2)) .* cos(alpha2) .* CL__csc(alpha2); 

  L = alpha1 + alpha2 - coef .* v; 
  Ldot = alpha1dot + alpha2dot - coef; 
  Ldotdot = alpha1dotdot + alpha2dotdot; 

endfunction


// --------------------------------------
// center angle as function of longitude of intersection of 
// field of view with equator (right side)
// (longitude of satellite when at ascending node = 0)
// v: argument of latitude
// L: longitude of intersection
// r, rdot, rdotdot: center angle + 1st and 2nd derivatives
// NB: r and rdot = %nan if they do not exist 
// --------------------------------------
function [r, rdot, rdotdot] = calcul_r(v, inc, coef, L)
  alpha = L + coef .* v; 

  cosr = cos(alpha).*cos(v) + sin(alpha).*sin(v).*cos(inc);
  r = real(acos(cosr)); 
  rdot = (sin(alpha).*cos(v) .* (coef - cos(inc)) + cos(alpha).*sin(v) .* (1 - coef.*cos(inc))) .* CL__csc(r); 
  rdotdot = (cos(alpha).*cos(v) .* (1 + coef .* (coef-2*cos(inc))) - sin(alpha).*sin(v) .* (2*coef - cos(inc).*(1+coef.^2)) - cos(r).*rdot.^2) .* CL__csc(r); 

  I = find(abs(cosr) > 1); 
  r(I) = %nan;
  rdot(I) = %nan;   
  rdotdot(I) = %nan;   
  
endfunction


// --------------------------------------
// utility: make structure with additional quantities 
// --------------------------------------
function [args] = make_args(inc, coef, y, signe)
  args = struct();
  args.inc = inc; 
  args.coef = coef; 
  args.y = y; 
  args.signe = signe; 
endfunction


// --------------------------------------
// interface of calcul_L for optimization function 
// v = pso (argument of latitude)
// K = indices computed (same size as v)
// args: additional arguments structure
// --------------------------------------
function [L, Ldot, Ldotdot] = fct_L(v, K, args); 
  [L, Ldot, Ldotdot] = calcul_L(v, args.inc(K), args.coef(K), args.y(K)); 
  L = L * args.signe; 
  Ldot = Ldot * args.signe; 
  Ldotdot = Ldotdot * args.signe; 
endfunction


// --------------------------------------
// interface of calcul_r for optimization function 
// v = pso (argument of latitude)
// K = indices computed (same size as v)
// args: additional arguments structure
// --------------------------------------
function [r, rdot, rdotdot] = fct_r(v, K, args); 
  [r, rdot, rdotdot] = calcul_r(v, args.inc(K), args.coef(K), args.y(K)); 
  r = r * args.signe; 
  rdot = rdot * args.signe; 
  rdotdot =rdotdot * args.signe; 
endfunction





// -------------------
// MAIN FUNCTION
// -------------------

// Declarations:

// Code:
if (~exists("er", "local")); er = CL__dataGetEnv("eqRad"); end
if (~exists("mu", "local")); mu = CL__dataGetEnv("mu"); end
if (~exists("j2", "local")); j2 = CL__dataGetEnv("j1jn", 2); end
if (~exists("rotr_pla", "local")); rotr_pla = CL__dataGetEnv("rotrBody"); end

// checks / resizes inputs
[sma, inc, ang1] = CL__checkInputs(sma, 1, inc, 1, ang1, 1); 

// returns [] if ang1 is []; 
if (sma == [] | inc == [] | ang1 == []); 
  ang2 = []; 
  return; // <== RETURN
end


// checks arguments: sma and inc
if (find(sma <= 0 | inc < 0 | inc > %pi) <> []) 
  CL__error("Input arguments out of range"); 
end

// check value of "cmd"
if (cmd <> "cen2dlon" & cmd <> "dlon2cen")
  CL__error("Invalid argument (''cen2dlon'' or ''dlon2cen'' expected"); 
end

// Check value of "ang1":
// center angle less than pi
// longitude span less than 2*pi
if (find(ang1 < 0 | (cmd == "cen2dlon" & ang1 > %pi) | (cmd == "dlon2cen" & ang1 > 2*%pi))  <> []) 
  CL__error("Input argument (ang1) out of range"); 
end


// computes coefficient relating pso and longitude
ecc = 0;
[pomdot, gomdot, anmdot] = CL_op_driftJ2(sma, ecc, inc, ...
                           er=er, mu=mu, j2=j2);
coef = (rotr_pla - gomdot) ./ (pomdot + anmdot); 


if (cmd == "cen2dlon")
  // pso bounds value for optimization: -vmax -> +vmax
  vmax = real(asin(sin(ang1) .* CL__csc(inc))) - 2*%eps; 
  I = find(sin(inc) <= %eps); 
  vmax(I) = %pi/2 - 2*%eps; 

  // args = arguments passed to function
  // NB: change sign of function to find minimum
  args = make_args(inc, coef, ang1, -1); 
  dxtol = 1.e-8; 
  N = length(ang1); 

  // maximize longitude span at equator (minimize -fct)
  [fopt, vopt] = findmin(N, fct_L, args, -vmax, vmax, dxtol=dxtol); 

  ang2 = -fopt; // ang2 = longitude span
  
  // limit output value to 2*pi
  I = find(ang2 > 2*%pi); 
  ang2(I) = 2*%pi; 
  
  // Temporary limitation: 
  // results are considered as invalid if ... 
  I = find(abs(coef) > 1 | ang1 >= %pi/2); 
  ang2(I) = %nan; 
  

else // cmd == "dlon2cen" 
  // bounds for optimization: WILL HAVE TO BE IMPROVED 
  vmax = (%pi/2 - 2*%eps) * ones(ang1); 
  vmin = -vmax; 

  // case for which we are sure the minimum is obtained for an anomaly <= 0
  // because the function fct_r is increasing
  I = find(coef >= 0 & inc >= %pi/2); 
  vmax(I) = 0; 
  
  // args = arguments passed to function
  args = make_args(inc, coef, ang1, 1); 
  dxtol = 1.e-8; 
  N = length(ang1); 
  
  // minimize center angle (i.e. sensor's field of view)
  [fopt, vopt] = findmin(N, fct_r, args, vmin, vmax, dxtol=dxtol); 
  
  ang2 = fopt; // ang2 = center angle
  
  // check value : recompute longitude span from the result
  // (because there is no theoretical proof that the result is correct in 100% of the cases)
  L = calcul_L(vopt, inc, coef, ang2); 
  eps = 1.e-10; // margin for checking (rad)
  I = find(abs(L-ang1) > eps); 
  r(I) = %nan; 
    
  // Temporary limitation: 
  // results are considered as invalid if ... 
  I = find(abs(coef) > 1 | ang2 >= %pi/2); 
  ang2(I) = %nan; 

end


endfunction