//  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'.

// -----------------------------------------------------------
//> Relative motion of satellite2 relative to satellite1 in
//> "qsw" or "lvlh" local frame. 
//> Colors (models): 
//> - Blue: Kepler
//> - Pulple: Clohessy-Whiltshire
//>
//> Abscissa: mean anomaly of satellite1. 
//> Clohessy-Whiltshire is initialized using the relative keplerian motion. 
//> Warning: when changing the argument of periapsis: the separation between 
//> the satellite changes if the mean anomaly is not modified accordingly. 
//
// Author: A. Lamy
// -----------------------------------------------------------

kep = [7000.e3; 0.; %pi/2; %pi/2; 0; 0]; 
dkep = [0; 1.e-4; 0; 0; 0.01*%CL_deg2rad; 0]; 
 
norb = 2; 
iframe = 1; 
imodel = [1, 0]; 
iplot = [1, 0, 1]; 


desc_param = list(..
   CL_defParam("sat1: Semi-major axis", kep(1), id = '$sma', units = ["m", "km"], valid='$x > 0'),..
   CL_defParam("sat1: Eccentricity", kep(2), valid='$x >= 0 & $x < 1'),..
   CL_defParam("sat1: Inclination", kep(3), units = ["rad", "deg"], valid='$x >= 0 & $x <= 180'),..
   CL_defParam("sat1: Argument of perigee", kep(4), units = ["rad", "deg"]),..
   CL_defParam("sat1: Right ascension of asc. node", kep(5), units = ["rad", "deg"]),..
   CL_defParam("sat1: Mean anomaly", kep(6), units = ["rad", "deg"]),..
   CL_defParam("sat2/1: Delta semi-major axis", dkep(1), units = ["m", "km"], valid='$x > -$sma'),..
   CL_defParam("sat2/1: Delta Eccentricity", dkep(2), valid='$x >= 0 & $x < 1'),..
   CL_defParam("sat2/1: Delta Inclination", dkep(3), units = ["rad", "deg"], valid='$x >= -180 & $x <= 180'),..
   CL_defParam("sat2/1: Delta Argument of perigee", dkep(4), units = ["rad", "deg"], valid='$x >= -180 & $x <= 180'),..
   CL_defParam("sat2/1: Delta Right ascension of asc. node", dkep(5), units = ["rad", "deg"], valid='$x >= -180 & $x <= 180'),..
   CL_defParam("sat2/1: Delta Mean anomaly", dkep(6), units = ["rad", "deg"], valid='$x >= -180 & $x <= 180'),..
   CL_defParam("Number of orbits for propagation", norb, valid='$x>0'),..
   CL_defParam("Local frame (1=qsw, 2=lvlh)", iframe, accv=[1,2]),..
   CL_defParam("Model: Kepler, C-W (1=yes, 0=no)", imodel, dim=2, accv=[0,1]),..
   CL_defParam("Plot: components, norms, 3d (1=yes, 0=no)", iplot, dim=3, accv=[0,1])..
); 

[kep(1),kep(2),kep(3),kep(4),kep(5),kep(6),dkep(1),dkep(2),dkep(3),dkep(4),dkep(5),dkep(6),norb,iframe,imodel,iplot] = CL_inputParam(desc_param);


// first orbit
kepref0 = kep; 
kep0 = kep + dkep;  

T = CL_kp_params(['per'], kepref0(1)); // seconds 

N = 200; // points per orbit  

dt = linspace(0, norb*T, N*norb+1); ; 
t0 = 0; 
t = t0 + dt / 86400; 

// warning: only frames based on "qsw" are possible here
frames_list = ["qsw", "lvlh"];  
frame = frames_list(iframe); 


// -----------------------------------------------------------
// position, velocity, acceleration - relative to qsw
// anmref = mean anomaly of reference (= target) satellite
// -----------------------------------------------------------
function [prel, vrel, arel, anmref] = extrap_rel_kepler(t0, kepref0, kep0, t, frame)

  if (frame == "lvlh"); frame = "sWQ"; end  // same ! 

  kepref = CL_ex_kepler(t0, kepref0, t); 
  kep = CL_ex_kepler(t0, kep0, t); 

  [pref, vref] = CL_oe_kep2car(kepref); 
  [p, v] = CL_oe_kep2car(kep); 

  // omega = angular rotation vector
  omega = CL_dMult(1 ./ CL_dot(pref), CL_cross(pref, vref));
  domegadt = -2 * CL_dMult(CL_dot(pref,vref) ./ CL_dot(pref), omega); 
  
  Mloc = CL_fr_locOrbMat(pref, vref, frame);

  // a = inertial acceleration
  aref = CL_dMult(-%CL_mu ./ CL_norm(pref).^3, pref); 
  a = CL_dMult(-%CL_mu ./ CL_norm(p).^3, p); 

  ve = CL_cross(omega, p-pref); 
  vr = v - vref - ve; 
  ar = a - aref - CL_cross(omega,ve) - CL_cross(domegadt,p-pref) - 2 * CL_cross(omega, vr);

  // relative position, velocity, acceleration
  prel = Mloc * (p - pref); 
  vrel = Mloc * vr; 
  arel = Mloc * ar; 

  anmref = kepref(6,:); 

endfunction


// -----------------------------------------------------------
// position, velocity, acceleration - relative to qsw
// anmref = mean anomaly of reference (= target) satellite
// -----------------------------------------------------------
function [prel, vrel, arel, anmref] = extrap_rel_cw(t0, kepref0, kep0, t, frame)

  if (frame == "lvlh"); frame = "sWQ"; end  // same ! 

  // initial conditions
  [pref, vref] = CL_oe_kep2car(kepref0); 
  [p, v] = CL_oe_kep2car(kep0); 

  omega = CL_dMult(1 ./ CL_dot(pref), CL_cross(pref, vref));

  Mloc = CL_fr_locOrbMat(pref, vref, frame);
  Mlvlh = CL_fr_lvlhMat(pref, vref); 

  prel =  Mlvlh * (p - pref); 
  vrel = Mlvlh * (v - vref - CL_cross(omega, p-pref));

  dt = (t - t0) * 86400; // seconds
  alt = CL_norm(pref)*ones(dt) - %CL_eqRad; 

  M_CW = CL_cw_Mmatrix(alt, dt); 

  pv = M_CW * [prel; vrel];

  prel =  Mloc * (Mlvlh' * pv(1:3,:)); 
  vrel =  Mloc * (Mlvlh' * pv(4:6,:)); 

  // acceleration computed numerically 
  dt = (t(3:$)-t(1:$-2)) * 86400; // sec
  arel = [%nan*ones(3,1), CL_dMult(vrel(:,3:$)-vrel(:,1:$-2), 1 ./ dt), %nan*ones(3,1)]; 

  kepref = CL_ex_kepler(t0, kepref0, t); 
  anmref = kepref(6,:); 

endfunction


// -----------------------------------------------------------
// Plot components - low level
// -----------------------------------------------------------
function plotpv_2d(ang, p, v, a, icol, thick)

  tab_y = [p(3,:); v(3,:); a(3,:); p(2,:); v(2,:); a(2,:); p(1,:); v(1,:); a(1,:)]; 
  tab_titles = ["pos-Z (km)", "vel-Z (m/s)", "acc-Z (mm/s2)", "pos-Y (km)", "vel-Y (m/s)", "acc-Y (mm/s2)", "pos-X (km)", "vel-X (m/s)", "acc-X (mm/s2)"]; 

  e = []; 

  for i = 1:9
    subplot(3,3,i); 
    plot(ang*%CL_rad2deg, tab_y(i,:)); 
    e = [e, gce()];  
    a=gca(); 
    a.title.text = tab_titles(i); 
    a.margins = [0.2,0.15,0.2,0.2]; 
    CL_g_stdaxes(a, ft=2);
  end

  h = CL_g_select(e, "Polyline"); 
  h.foreground = icol; 
  h.thickness = thick; 

endfunction 


// -----------------------------------------------------------
// Plot norms - low level
// -----------------------------------------------------------
function plotpv_2d_norm(ang, p, v, a, icol, thick)

  tab_y = [CL_norm(a); CL_norm(v); CL_norm(p)]; 
  tab_titles = ["|acc| (mm/s2)", "|vel| (m/s)", "|pos| (km)"]; 

  e = []; 

  for i = 1:3
    subplot(3,1,i); 
    plot(ang*%CL_rad2deg, tab_y(i,:)); 
    e = [e, gce()];  
    a=gca(); 
    a.title.text = tab_titles(i); 
    a.margins = [0.2,0.15,0.2,0.2]; 
    CL_g_stdaxes(a, ft=2);
  end

  h = CL_g_select(e, "Polyline"); 
  h.foreground = icol; 
  h.thickness = thick; 

endfunction 


// -----------------------------------------------------------
// Plot 3d view - low level
// -----------------------------------------------------------
function plotp_3d(p, icol, thick)

  x = p(1,:); 
  y = p(2,:); 
  z = p(3,:); 

  param3d(x, y, z); 
  h = CL_g_select(gce(), "Polyline"); 
  h.foreground = icol; 
  h.thickness = thick; 

endfunction


// -----------------------------------------------------------
// plot components - main
// -----------------------------------------------------------
function [f] = Plot_components()
  f = scf(); 
  f.visible = "on"; 
  f.immediate_drawing = "off"; 

  if (imodel(2) == 1)
    plotpv_2d(anmref_cw, prel_cw, vrel_cw, arel_cw, 24, 2);
  end
  if (imodel(1) == 1)
    plotpv_2d(anmref_k, prel_k, vrel_k, arel_k, 2, 2);
  end

  f.immediate_drawing = "on"; 
  // f.visible = "on"; => done later

endfunction


// -----------------------------------------------------------
// plot norms - main
// -----------------------------------------------------------
function [f] = Plot_norms()
  f = scf(); 
  f.visible = "on"; 
  f.immediate_drawing = "off"; 

  if (imodel(2) == 1) 
    plotpv_2d_norm(anmref_cw, prel_cw, vrel_cw, arel_cw, 24, 2);
  end
  if (imodel(1) == 1)
    plotpv_2d_norm(anmref_k, prel_k, vrel_k, arel_k, 2, 2);
  end

  f.immediate_drawing = "on"; 
  // f.visible = "on"; => done later

endfunction


// -----------------------------------------------------------
// 3D plot - main
// -----------------------------------------------------------
function [f] = Plot_3d()
  f = scf(); 
  f.visible = "on"; 
  f.immediate_drawing = "off"; 

  x = []; 
  y = []; 
  z = []; 

  if (imodel(2) == 1) 
    plotp_3d(prel_cw, 24, 2); 
    x = [x, prel_cw(1,:)]; 
    y = [y, prel_cw(2,:)]; 
    z = [z, prel_cw(3,:)]; 
  end

  if (imodel(1) == 1)
    plotp_3d(prel_k, 2, 2); 
    x = [x, prel_k(1,:)]; 
    y = [y, prel_k(2,:)]; 
    z = [z, prel_k(3,:)]; 
  end

  dim = (max(max(x)-min(x), max(y)-min(y), max(z)-min(z))/2) * 1.05; 
 
  xmin = (max(x) + min(x))/2 - dim;  
  xmax = (max(x) + min(x))/2 + dim;
  ymin = (max(y) + min(y))/2 - dim;  
  ymax = (max(y) + min(y))/2 + dim;
  zmin = (max(z) + min(z))/2 - dim;  
  zmax = (max(z) + min(z))/2 + dim;


  a=gca(); 
  a.data_bounds = [xmin, ymin, zmin; xmax, ymax, zmax]; 
  a.tight_limits = "on"; 
  a.rotation_angles = [70, 225]; 
  a.isoview = "on"; 

  a.title.text = "3D view (km) - " + frame; 
  CL_g_stdaxes(a);
  col = addcolor(0.8 * [1,1,1]); 
  a.grid = [col, col, col]; 

  f.immediate_drawing = "on"; 
  // f.visible = "on"; => done later

endfunction; 


// ===========================================
// MAIN: Computation / plot 
// ===========================================

if (imodel(1) == 1)
  [prel_k, vrel_k, arel_k, anmref_k] = extrap_rel_kepler(t0, kepref0, kep0, t, frame);
  prel_k = prel_k / 1000;  // => km
  arel_k = arel_k * 1000;  // => mm/s2
end

if (imodel(2) == 1)
  [prel_cw, vrel_cw, arel_cw, anmref_cw] = extrap_rel_cw(t0, kepref0, kep0, t, frame);
  prel_cw = prel_cw / 1000; 
  arel_cw = arel_cw * 1000;  // => mm/s2
end

// figures are made visble at the end to avoid interferences
// (pointer that may select wrong window)

f = []; // set of figures

if (iplot(1) == 1); f = [f, Plot_components()]; end

if (iplot(2) == 1); f = [f, Plot_norms()]; end

if (iplot(3) == 1); f = [f, Plot_3d()]; end


// makes everything visible
if (f <> []); f.visible = "on"; end