//  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_op_latSwath(sma, inc, angc, lat, res, er, mu, j2, rotr_pla)
// Swath at given latitude (circular orbits) 
//
// Calling Sequence
// [lona, lond, info] = CL_op_latSwath(sma, inc, angc, lat [, res="li"] [, er, mu, j2, rotr_pla])
// [lona, lond, ta, td, info] = CL_op_latSwath(sma, inc, angc, lat, res="lti" [, er, mu, j2, rotr_pla])
//
// Description
// <itemizedlist><listitem>
// <p>Computes the intersection of an instrument's swath with a parallel (small circle at a specified 
// latitude) for a circular orbit and a spherical planet. </p>
// <p></p>
// <p>The swath instrument outlines are formed by two beams in the plane perpendicular to the inertial
// velocity. Each beam is defined by a "center angle" (<b>angc</b>): angle from the planet center between 
// the radius vector and the intersection of the beam with the planet. The angle is signed: negative 
// to the left (direction of the angular momentum vector), and positive to the right. </p>
// <inlinemediaobject><imageobject><imagedata fileref="latswath.png"/></imageobject></inlinemediaobject>
// <p></p>
// <p>Two longitude intervals (<b>lona</b> and <b>lond</b>) are computed for the ascending and 
// descending pass for one orbit (ascending node to the next). Times intervals (<b>ta</b> and <b>td</b>) when the swath 
// crosses the longitude intervals are optionally computed. By convention, the spacecraft is supposed to
// be at longitude 0 and going from South to North at time 0. </p>
// <p></p>
// <p>Additional information (<b>info</b>) is also returned: </p>
// <p>- nodper: The orbit's nodal period (s)</p>
// <p>- lgap: Longitude gap between two consecutive orbit tracks (rad) </p>
// <p>- ncase: situation encountered: </p>
// <p>  0 = latitude not visible, </p>
// <p>  1 = only one beam intersects the small circle at the specified latitude, </p>
// <p>  2 = two beams intersect the small circle at the specified latitude, </p>
// <p>  3 = latitude is visible from any orbit position in the orbit (polar case), </p>
// <p>  4 = latitude is visible from any orbit position in the orbit (equatorial case) </p>
// <p></p>
// </listitem>
// <listitem>
// <p><b>Notes:</b></p>
// <p> - Secular effect of J2 is taken into account. </p>
// <p> - Right ascension (rather than longitude) intervals can be computed by setting the rotation
// rate to 0. </p>
// <p></p>
// </listitem>
// </itemizedlist>
//
// Parameters
// sma: Semi major axis [m] (1xN or 1x1)
// inc: Inclination [rad] (1xN or 1x1)
// angc: Instrument's center angles [rad] (1xN or 1x1 or 2xN or 2x1)
// lat: Latitude [rad] (1xN or 1x1)
// res: type of output: "li" or "lti"
// er: (optional) Equatorial radius [m] (default is %CL_eqRad)
// mu: (optional) Gravitational constant [m^3/s^2] (default value is %CL_mu)
// j2: (optional) Zonal coefficient (second zonal harmonic) (default is %CL_j1jn(2))
// rotr_pla: (optional) Rotation rate of the planet [rad/s] (default is %CL_rotrBody)
// lona: Longitude interval for ascending pass [rad] (2xN)
// lond: Longitude interval for descending pass [rad] (2xN)
// ta: Time interval for ascending pass [s] (1xN) 
// td: Time interval for descending pass [s] (1xN) 
// info: (struct) Additional information - see above
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_gm_visiParams
// CL_op_equatorialSwath
//
// Examples
// // Orbit (sma + inc) 
// sma = 7.e6; // m
// inc = 60 * %pi/180; // rad
//
// // instrument center angles
// angc = [-1; 2] * %pi/180;
//
// // Latitude of interest
// lat = 40 * %pi/180;
//
// // Longitude intervals 
// [lona, lond] = CL_op_latSwath(sma, inc, angc, lat)
// 
// // Longitude and intervals and additional info
// [lona, lond, ta, td, info] = CL_op_latSwath(sma, inc, angc, lat, res="lti")
//

// Declarations:
 
// -----------------------------------
// Auxiliary functions
// -----------------------------------
  
// ------------------------------
// Solve the spherical triangle
//
// inc: orbital inclination (rad) 1xN
// lat: latitude (rad) >= 0 1xN
// alpha: center angle defining the beam direction wrt to the angular momentum direction (>=0) 2xN
// A: dhiedral angle: (angular momentum, pole, meridian) 3x1
// B: dhiedral angle: (pole, angular momentum, point at latitude lat) 3xN
// NB: A and B: [side1; side2; tangent point]
// ------------------------------
function [A, B] = latSwath_sphTriang(inc, lat, alpha)

  // 1) computes the point where the swath is tangent to the small circle of latitude lat
  // nan if does not belong to [alpha(1), alpha(2)] or if not computable
  cosinc = cos(inc); 
  sinlat = sin(lat); 
  
  alpha0 = %nan * ones(alpha(1,:)); 
  I = find(abs(cosinc) <= abs(sinlat) & abs(sinlat) > 0); 
  alpha0(I) = real(acos(cosinc(I) ./ sinlat(I))); 
  I = find(alpha0 > alpha(2,:) | alpha0 < alpha(1,:)); 
  alpha0(I) = %nan; 
  
  // 2) computes A and B for [alpha, alpha0] = [side1; side2; tangent point]

  inc = repmat(inc, 3, 1); 
  lat = repmat(lat, 3, 1); 
  alpha = [alpha; alpha0]; 
  A = %nan * ones(alpha); 
  B = %nan * ones(alpha); 

  // compute A
  den = sin(inc) .* cos(lat);
  I = find(den <= 0); 
  den(I) = %nan; 
  cosA = (cos(alpha) - cos(inc) .* sin(lat)) ./ den;
  I = find(abs(cosA) <= 1); // %nan not included
  A(I) = real(acos(cosA(I)));
  
  // compute B
  den = sin(inc) .* sin(alpha); 
  I = find(den <= 0);
  den(I) = %nan; 
  cosB = (sin(lat) - cos(inc) .* cos(alpha)) ./ den;
  I = find(abs(cosB) <= 1); // %nan not included 
  B(I) = real(acos(cosB(I)));
    
endfunction

// ---------------------------
// Computes gaps in longitude and date with respect to the node
// ---------------------------
function [dt, dlon, nodper, lgap] = latSwath_intervLong(sma, inc, dgom, dpso, er, mu, j2, rotr_pla)    
  // effect of J2
  ecc = 0; 
  [pomdot, gomdot, anmdot] = CL_op_driftJ2(sma, ecc, inc, er, mu, j2); // rad/s
  
  // PSO drift rate
  psodot = (pomdot + anmdot); // rad/s
  
  // longitude drift rate
  londot = gomdot - rotr_pla; // rad/s
  
  // time from ascending node  
  dt = CL_dMult(dpso, 1 ./ psodot);   

  // longitude from longitude at ascending node 
  dlon = [dgom + CL_dMult(dt, londot)];
  
  // nodal period 
  nodper = (2*%pi) ./ psodot; // s
  
  // longitude gap between 2 consecutive orbits 
  lgap = londot .* nodper; // rad

endfunction

// -----------------------------------
// Argument Checking
// -----------------------------------

if (~exists("res", "local")); res = "li"; end;
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

if (res <> "li" & res <> "lti")
  CL__error("Invalid valud for argument res"); 
end

nbargout = argn(1); 
if ((res == "li" & nbargout > 3) | (res == "lti" & nbargout > 5))
  CL__error("Too many output arguments"); 
end   
    
if (find (sma <= 0) <> []) 
  CL__error("Semi major axis out of range"); 
end

if (find (inc < 0 | inc > %pi + %eps) <> []) 
  CL__error("Inclination out of range"); 
end

if (size(angc, 1) == 1)
  angc = [-angc; angc]; 
end 

// Check/resize arguments 
[sma, inc, lat, angc, N] = CL__checkInputs(sma, 1, inc, 1, lat, 1, angc, 2); 
  
if (find(angc(1,:) > angc(2,:)) <> [])
  CL__error("Invalid value for angc"); 
end


// -----------------------------------
// Main Code
// -----------------------------------

// number of instrument's beams (= number of rows of angc)
Nb = 2;
 
// Computation for positive latitude. Results for negative latitudes are obtained by symmetry. 
// invlat == 1 if lat < 0
invlat = zeros(lat); 
I = find(lat < 0);
if (I <> [])
  lat(I) = -lat(I);
  angc(:,I) = [-angc(2, I); -angc(1, I)];
  invlat(I) = 1; 
end

// signed angles defining small circle and instrument (wrt angular momentum vector)
lambda =  [lat + inc - %pi/2; -lat + inc + %pi/2];  
alpha = %pi/2 + angc;


// sgn: 1 if inclination is <= 90 deg, -1 otherwise
sgn = ones(inc); 
I = find(inc > %pi/2); 
sgn(I) = -1; 
  
// Solve spherical triangles
// each line of A,B corresponds to a distinct value of angc
[A, B] = latSwath_sphTriang(inc, lat, alpha);

// initialization
// res(1:4, :) = [dgom; dpso] => ascending pass
// res(5:8, :) = [dgom; dpso] => descending pass
dgom = %nan * ones(6, N); 
dpso = %nan * ones(6, N);
ncase = %nan * ones(1,N); 

// Case 1) no intersections exist- latitude not seen
I = find(alpha(1,:) >= lambda(2,:) | alpha(2,:) <= lambda(1,:));
if (I <> [])
  ncase(I) = 0;
end

// Case 1b) no intersections exist - latitude seen at any PSO - inc <= 90
I = find(alpha(1,:) <= lambda(2,:) & alpha(2,:) >= lambda(2,:) & ..
         alpha(1,:) <= -lambda(1,:) & alpha(2,:) >= -lambda(1,:) & ..
         isnan(ncase));
if (I <> [])
  ncase(I) = 4;
  dgom(1:2,I) = (%pi/2) * [-1; 1] * ones(I);     
  dpso(1:2,I) = (%pi/2) * [-1; 1] * ones(I);     
end

// Case 1c) no intersections exist - latitude seen at any PSO - inc >= 90
I = find(alpha(1,:) <= lambda(1,:) & alpha(2,:) >= lambda(1,:) & ..
         alpha(1,:) <= lambda(2,:)-%pi & alpha(2,:) >= lambda(2,:)-%pi & ..
         isnan(ncase));
if (I <> [])
  ncase(I) = 4;
  dgom(1:2,I) = (%pi/2) * [1; -1] * ones(I);     
  dpso(1:2,I) = (%pi/2) * [-1; 1] * ones(I);     
end

// Case 2) small circle is entirely visible 
I = find(alpha(1,:) <= lambda(1,:) & alpha(2,:) >= lambda(2,:) & isnan(ncase));  
if (I <> [])
  ncase(I) = 3;
  dgom(1:2,I) = (%pi/2) * [-1; 1] * ones(I);     
  dpso(1:2,I) = (%pi/2) * [1; 1] * ones(I);     
end 
  
// Case 3) only beam no. 1 intersects
// interval is centered on max value of ra for beam no. 1
I = find(alpha(2,:) >= lambda(2,:) & alpha(1,:) >= lambda(1,:) & isnan(ncase));  
if (I <> [])
  ncase(I) = 1;
  dgom(1:2,I) = [A(1,I) - %pi/2; (%pi/2) * ones(I)];
  dpso(1:2,I) = [%pi/2 - B(1,I); (%pi/2) * ones(I)];
end

// Case 4) only beam no. 2 intersects
// interval is centered on max value of ra for beam no. 2  
I = find(alpha(1,:) <= lambda(1,:) & alpha(2,:) <= lambda(2,:) & isnan(ncase)); 
if (I <> [])
  ncase(I) = 1;
  dgom(1:2,I) = [-(%pi/2) * ones(I); A(2,I) - %pi/2];
  dpso(1:2,I) = [(%pi/2) * ones(I); %pi/2 - B(2,I)];
end
  
// Case 5) 2 intersections (ascending and descending pass)
I = find(alpha(1,:) >= lambda(1,:) & alpha(2,:) <= lambda(2,:) & isnan(ncase));  
if (I <> [])
  ncase(I) = 2;
  dgom(1:2,I) = [A(1,I) - %pi/2; A(2,I) - %pi/2];
  dpso(1:2,I) = [%pi/2 - B(1,I); %pi/2 - B(2,I)];
end

// Add tangent point
dgom(3,:) = A(3,:) - %pi/2;
dpso(3,:) = %pi/2 - B(3,:);


// Add intervals for descending pass : symmetrical / point where PSO = %pi/2
dgom(4:6,:) = [sgn * %pi - dgom(2,:);  sgn .* %pi - dgom(1,:); %nan * ones(dgom(3,:))];
dpso(4:6,:) = [%pi - dpso(2,:);  %pi - dpso(1,:); %pi - dpso(3,:)]; 

               
// lat < 0 => symmetrical with respect to ascending node (for both RAAN and PSO) 
I = find(invlat == 1);
if (I <> [])
  dgom(:, I) = 2*%pi + [-dgom([2,1,3], I); -dgom([5,4,6], I)];
  dpso(:, I) = 2*%pi + [-dpso([2,1,3], I); -dpso([5,4,6], I)];
end 
 
// Compute gaps in longitude and date (ascending/descending passes)
[dt, dlon, nodper, lgap] = latSwath_intervLong(sma, inc, dgom, dpso, .. 
                                               er, mu, j2, rotr_pla);

// longitude interval 
// NB: min/max because order reversed in some cases  
lona = [min(dlon(1:2,:), "r"); max(dlon(1:2,:), "r")]; 
lond = [min(dlon(4:5,:), "r"); max(dlon(4:5,:), "r")];

// time interval   
// => force in [0, T] 
// NB: if %nan: no effect on min or max
ta = [min(dt(1:3,:), "r"); max(dt(1:3,:), "r")]; 
td = [min(dt(4:6,:), "r"); max(dt(4:6,:), "r")];  


// Build structure containing additional information: 
// nodal period, longitude gap, computation case
info = struct();
info.nodper = nodper; 
info.lgap = lgap;
info.ncase = ncase;

if (res == "li")
  varargout = list(lona, lond, info); 
else
  varargout = list(lona, lond, ta, td, info); 
end
 
endfunction