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

// -----------------------------------------------------------
//> Interplanetary transfers (Tisserand graph) 
//> The plot shows the relationship between fly-by conditions of a 
//> planet by a spacecraft and some heliocentric orbit characteristics. 
//> 
//> The fly-by conditions are described by: 
//> - Vinf: excess velocity of spacecraft (positive)
//> - Theta: angle beween the velocity vector of the planet and the 
//>   excess velocity vector of the spacecraft (between 0 and 180 deg). 
//> 
//> The heliocentric orbit characteristics are: 
//> - Perihelion and aphelion radii (aphelion radius is undefined for an hyperbola)
//> - Energy of orbit (negative for an ellipse)
//> - Semi major axis
//> - Eccentricity (> 1 for a hyperbola). 
//> 
//> The hypotheses considered are: 
//> - Motions are Keplerian
//> - Planets' orbits are supposed circular (radius = semi-major axis on 1/1/2000)
//> - The heliocentric velocity vector is the sum of the velocity vector
//>   of the planet + the excess velocity vector of the spacecraft. 
//>
//> Notes: 
//> - The plotted semi-major axis is negative for hyperbolic orbits. 
//> - Plot in periapsis/apoapsis is limited to apoapsis < 10 AU. 
//> - Resonnant orbits are computed for an eccentricity <= 0.9. 
//> - The symbols indicate how much the helocentric trajectory can be changed
//>   by a swing-by of a planet. For a given excess velocity (vinf), the symbols correspond
//>   to evenly spaced theta values (step = turn angle). 
//> - The arrows on the "orbit" plot represent the relative velocity (excess velocity vector). 
//> 
//
// Author: A. Lamy
// -----------------------------------------------------------

// Excess velocity values (m/s)
vinf = (2:2:10) * 1000; 

// Planet numbers (see below for meaning)
bodynums = [2,3,4]; 

// Minimum fly-by periapsis altitude (m)
hpmin = 1000.e3; 

// Plot option 
opt = 1; 

// Body nums for resonnant orbits 
res_bodynums = []; 

desc_param = list(..
   CL_defParam("Hyperbolic excess velocity values", vinf, units=['m/s', 'km/s'], dim=[1,20], valid='$x >= 0'), ..
   CL_defParam("Planet numbers (2=Venus, 3=Earth, 4=Mars, 5=Jupiter)", bodynums, dim=[1,4], accv=2:5, ), ..
   CL_defParam("Minimum fly-by periapsis altitude", hpmin, units=['m', 'km'], valid='$x >= 0'), ..
   CL_defParam("Plot: 1=rp/energy, 2=sma/ecc, 3=rp/ra", opt, accv=1:3), ..
   CL_defParam("Plot resonnant orbits for planet numbers ...", res_bodynums, dim=[0,4], accv=2:5) ..
);

[vinf, bodynums, hpmin, opt, res_bodynums] = CL_inputParam(desc_param);

BodyNames = ["Venus", "Earth", "Mars", "Jupiter"]; 
BodyNums = [2, 3, 4, 5]; 

bodynames = BodyNames(dsearch(bodynums, BodyNums, "d")); 
res_bodynames = BodyNames(dsearch(res_bodynums, BodyNums, "d")); 


// ===================================
// Internal functions
// ===================================

// vinf and theta: same size (can be NxP)
// sma0 = sma planet
function [res] = compute(sma0, vinf, theta, muSun)

  vel0 = sqrt(muSun / sma0); 

  all_vinf = matrix(vinf(:), 1, -1); 
  all_theta = matrix(theta(:), 1, -1); 

  vt = vel0 + all_vinf .* cos(all_theta); 
  vn = all_vinf .* sin(all_theta); 

  r = sma0
  v = sqrt(vt.^2 + vn.^2); 

  energy = v.^2 / 2 - muSun ./ r; 
  
  // eliminate hyperbola case
  I = find(energy == 0);
  energy(I) = %nan; 
  
  // sma < 0 for hyperbolas ! 
  sma = - muSun ./ (2 * energy); 
  ecc = sqrt(1 - (r .* vt).^2 ./ (muSun * sma)); 
  
  // rp: valid for ellipses or hyperbolas
  rp = sma .* (1 - ecc); 

  // Ellipses only (otherwise: %nan) 
  ra = %nan * ones(sma); 
  per = %nan * ones(sma); 
  
  I = find(energy < 0)
  if (I <> [])
    ra(I) = sma(I) .* (1 + ecc(I)); 
    per(I) = sqrt(muSun ./ sma(I).^3); 
  end
  
  I = find(ra > 10 * %CL_au)
  ra(I) = %nan; 
  
  res = struct(); 
  res.vinf = vinf; 
  res.theta = theta; 
  // ellipse or hyperbola
  res.energy = matrix(energy, size(vinf)); 
  res.sma = matrix(sma, size(vinf)); 
  res.ecc = matrix(ecc, size(vinf)); 
  res.rp = matrix(rp, size(vinf)); 
  // ellipse only
  res.ra = matrix(ra, size(vinf)); 
  res.per = matrix(per, size(vinf)); 
  
endfunction


// vinf, turnang: 1xN
function [Vinf, Theta] = grid_markers(vinf, turnang)
  turnang = turnang; 
  I = find(turnang == 0); 
  turnang(I) = %nan; 
  
  N = ceil(%pi / min(turnang)); 
  x = (-N-0.5 : N+0.5)

  [Vinf, X] = ndgrid(vinf, x); 
  Theta = %pi/2 + CL_dMult(X, turnang'); 
  I = find(Theta < 0 | Theta > %pi);
  Theta(I) = %nan; 
endfunction

// resonnant orbits
// sma0: 1x1
// N: number of body rev (1xN)
// M: number of spacecraft rev. (1xN)
// results: 1 column for each (N,M) pair
function [res] = compute_resonnant(sma0, N, M, muSun)

  n_body = sqrt(muSun / sma0^3); 
  per_body = 2*%pi / n_body; 
 
  // N * per_body = M * per 
  per = per_body * (N ./ M); 
  n = 2*%pi ./ per; 
  sma = (muSun ./ n.^2).^(1/3); 
  ecc = linspace(0, 0.9, 51)'; 
    
  res = struct(); 
  res.N = ones(ecc) * N; 
  res.M = ones(ecc) * M; 
  res.ecc = ecc * ones(sma); 
  res.sma = ones(ecc) * sma;
  res.energy = -muSun * ones(ecc) * (1 ./ (2 * sma)); 
  res.rp = (1-ecc) * sma;  
  res.ra = (1+ecc) * sma; 
  res.per = ones(ecc) * per;  
  
endfunction


// --------------------------------------------
// event handler
// --------------------------------------------
function myHandler(win, xpix, ypix, but)

  // sub-function to avoid copying userdata
  // App = f.user_data
  function Handle(f, App)
  
  scf(f);
  sca(f.children(1)); // axes 

  [xdef, ydef, rect] = xchange(xpix, ypix, "i2f"); 
  
  // rect: in pixels
  in_bounds = %t; 
  if (~(xpix>=rect(1) & xpix<=rect(1)+rect(3) & ypix>=rect(2) & ypix<=rect(2)+rect(4)))
    in_bounds = %f; 
  end
  
  // try results
  [res_xpix, res_ypix] = xchange(App.x, App.y, "f2i"); 
  [dist,index] = min(sqrt((res_xpix-xpix).^2+(res_ypix-ypix).^2));
  
  fly_by = %f; 
  if (dist < 10); fly_by = %t; end

  // try resonnance orbits (if no fly-by selected) 
  reso = %f; 
  if (~fly_by & App.ro.x <> [])
    [resr_xpix, resr_ypix] = xchange(App.ro.x, App.ro.y, "f2i"); 
    [distr,indexr] = min(sqrt((resr_xpix-xpix).^2+(resr_ypix-ypix).^2));
    if (distr < 10); reso = %t; end
  end

  if (fly_by)
    index = index(1); 
    if (index == []); return; end
  
    ibody = App.ibody(index); 
    bname = App.body_names(ibody); 
	bsma = App.body_sma(ibody); 
    bvel = App.body_vel(ibody); 

    x = App.x(index); 
    y = App.y(index); 
    i = App.ij(1,index); 
    j = App.ij(2,index);
	vinf = App.all(ibody).vinf(i,j); 
    theta = App.all(ibody).theta(i,j); 
  end
  
  if (reso)
    indexr = indexr(1); 
    if (index == []); return; end
    reso_ibody = App.ro.ibody(indexr); 
    reso_bname = App.body_names(reso_ibody); 
	reso_N = App.ro.N(indexr); 
	reso_M = App.ro.M(indexr); 
  end

  
  // convert data to pixels
  if (~ in_bounds)
    f.info_message = "(Out of bounds)";

  elseif (reso & but < 0) // mouse move + distance = 10 pixels
    text1 = msprintf("Resonnant orbit %d:%d", reso_N, reso_M); 
	text2 = msprintf("X = %.2f,  Y = %.2f", xdef, ydef); 
    msg = msprintf("%-8s  %-35s   %-30s", reso_bname + ":", text1, text2); 
	f.info_message = msg;

  elseif (fly_by & but < 0) // mouse move + distance = 10 pixels
    text1 = msprintf("Vinf = %.1f km/s, Theta = %d deg", vinf/1000, round(theta * 180/%pi)); 
	text2 = msprintf("X = %.2f,  Y = %.2f", x, y); 
    msg = msprintf("%-8s  %-35s   %-30s", bname + ":", text1, text2); 
	f.info_message = msg;
	
  elseif (but < 0)
  	text2 = msprintf("X = %.2f,  Y = %.2f", xdef, ydef); 
    msg = msprintf("%-45s   %-30s", "(No selection)", text2); 
    f.info_message = msg;
	
  elseif (but == 3) // left click 
	mprintf("\n----------------------------\n"); 
	if (fly_by)
      turnang = App.all(ibody).turnang(i,j); 
      energy = App.all(ibody).energy(i,j); 
      sma = App.all(ibody).sma(i,j); 
      ecc = App.all(ibody).ecc(i,j); 
      rp = App.all(ibody).rp(i,j); 
      ra = App.all(ibody).ra(i,j); 
      I = find(isnan(ra)); 
      ra(I) = %inf; 

      mprintf("Planet: %s\n", bname); 
      mprintf("Radius (AU [km])):            %6.2f  [%.2e]\n", bsma/App.AU, bsma/1000); 
      mprintf("Speed (/Earth vel [km/s])):   %6.2f  [%.2f]\n", bvel/App.V0, bvel/1000); 
      mprintf("---\n"); 
      mprintf("Vinf (km/s):                  %6.2f\n", vinf/1000); 
      mprintf("Turn angle (deg):             %6.2f\n", turnang * 180/%pi); 
	  mprintf("Theta (deg):                  %6.2f\n", theta * 180 / %pi); 
	  mprintf("---\n");
	  mprintf("Heliocentric orbit: \n"); 
	  mprintf("Semi-major axis (AU [km]):    %6.2f  [%.2e]\n", abs(sma) / App.AU, abs(sma)/1000); 
	  mprintf("Eccentricity:                 %6.2f\n", ecc); 
      mprintf("Energy (/|Earth en.| [kJ]):   %6.2f  [%.2e]\n", energy/App.E0, energy/1000); 
      mprintf("Perihelion radius (AU [km])): %6.2f  [%.2e]\n", rp/App.AU, rp/1000); 
      mprintf("Aphelion radius (AU [km])):   %6.2f  [%.2e]\n", ra/App.AU, ra/1000); 
	  mprintf("---\n");
	  mprintf("All fly-bys: \n"); 
	  
	  [p1, dv1, p2, dv2] = App.fct.orb_inters(sma, ecc, App.body_sma, mu=App.MUSUN); 

	  for k = 1 : size(dv1,2)
	    if (isnan(CL_norm(dv1(:,k)))); continue; end
	    mprintf("Vinf (km/s):                  %6.2f  (%s)\n", CL_norm(dv1(:,k))/1000, App.body_names(k)); 
	  end

	  if (is_handle_valid(App.fig))
	    set(App.fig, "immediate_drawing", "off");
	    App.fct.plot_orbit(App.fig, sma / App.AU, ecc, "black", 2, 1); 
	    App.fct.plot_vinf(App.fig, [p1, p2] / App.AU, [dv1, dv2]/1000, "grey80", 1.5, 3); 
	    App.fct.plot_vinf(App.fig, [p1(:,ibody), p2(:,ibody)] / App.AU, [dv1(:,ibody), dv2(:,ibody)]/1000, "black", 1.5, 4); 
	    set(App.fig, "immediate_drawing", "on");
      end
	  
    else
      mprintf("No fly-by selected\n"); 
	end

  end  
  
  endfunction
  
  f = get_figure_handle(win);
  if isempty(f); return; end

  Handle(f, f.user_data); 
  
endfunction 


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

// -----------------------------------
// Data preparation
// -----------------------------------

muSun = %CL_body.Sun.mu; 
sma = zeros(bodynames); 
mu = zeros(bodynames); 
rad = zeros(bodynames); 

// ref dat for planet sma
cjd_ref = CL_dat_cal2cjd(2000,1,1); 

for k = 1 : size(bodynames, "*")
  name = bodynames(k); 
  [pos, vel] = CL_eph_planet(name, cjd_ref); 
  kep = CL_oe_car2kep(pos, vel, mu=muSun); 
  sma(k) = mean(kep(1,:)); 
  mu(k) = %CL_body(name).mu; 
  rad(k) = %CL_body(name).eqRad;; 
end


// -----------------------------------
// Computation
// -----------------------------------

theta = linspace(0, 180, 181) * %CL_deg2rad;

[Theta, Vinf] = ndgrid(theta, vinf); 

list_res = list(); 
list_resM = list(); 
list_resR = list(); // resonnant orbits


for k = 1 : size(bodynames, 2)
  list_res($+1) = compute(sma(k), Vinf, Theta, muSun);
  turnang = CL_ip_flybyParams("vinf", vinf, "rp", rad(k) + hpmin, "turnang", mu=mu(k)); 

  [Vinf_mark, Theta_mark] = grid_markers(vinf, turnang); 
  list_resM($+1) = compute(sma(k), Vinf_mark, Theta_mark, muSun);
  
  turnang = CL_ip_flybyParams("vinf", matrix(Vinf,1,-1), "rp", rad(k) + hpmin, "turnang", mu=mu(k)); 
  list_res($).turnang = matrix(turnang, size(Vinf)); 
  
  // resonnant orbits
  list_resR($+1) = compute_resonnant(sma(k), [2,3,4], [1,1,1], muSun);
end


// -----------------------------------
// Plot preparation
// -----------------------------------
function [x, y] = gen_xy(res, opt, muSun)
  if (opt == 1)
    x = res.rp / %CL_au; 
    y = res.energy / (muSun / (2*%CL_au)); 
  elseif (opt == 2)
    x = %CL_au ./ res.sma; 
    y = res.ecc; 
  elseif (opt == 3)
    x = res.rp / %CL_au; 
    y = res.ra / %CL_au; 
  end
endfunction

function [x_title, y_title] = gen_xy_titles(opt)
  if (opt == 1)
    x_title = "Perihelion radius (AU)"; 
    y_title = "Energy of heliocentric orbit (relative to |energy| of Earth orbit)"
  elseif (opt == 2)
    x_title = "Inverse of semi-major axis of heliocentric orbit (1/AU)"; 
    y_title = "Eccentricity of heliocentric orbit"
  elseif (opt == 3)
    x_title = "Perihelion radius (AU)"; 
    y_title = "Aphelion radius (AU)"; 
  end
endfunction

// sma and ecc: 1x1
// sma0: 1xN
function [p1, dv1, p2, dv2] = orb_inters(sma, ecc, sma0, mu)
 
  // If there are solutions => +abs(anv) and -abs(anv)
  anv = CL_gm_intersectCoplanOrb(abs(sma), ecc, 0, abs(sma0), 0, 0);
 
  // NB: t and t0  = time from periapsis (s). Same size as anv
  t0 = abs(anv) .* sqrt(abs(sma0).^3 ./ mu);
  t = CL_kp_v2M(ecc, abs(anv)) .* sqrt(abs(sma).^3 ./ mu);
  
  // sol 1
  p1 = %nan * ones(2, length(anv));
  dv1 = %nan * ones(2, length(anv));
  p2 = p1; 
  dv2 = dv1; 
  
  I = find(~isnan(anv)); 
  if (I <> [])
    [pos, vel] = CL_oe_kep2car(CL_ex_kepler(0, [abs(sma); ecc; 0; 0; 0; 0]*ones(I), t(I)/86400, mu=mu), mu=mu); 
    [pos0, vel0] = CL_oe_kep2car(CL_ex_kepler(0, [abs(sma0(I)); zeros(5, length(I))], t0(I)/86400, mu=mu), mu=mu); 
    p1(:,I) = pos(1:2,:); 
    dv1(:,I) = vel(1:2,:) - vel0(1:2,:); 

    [pos, vel] = CL_oe_kep2car(CL_ex_kepler(0, [abs(sma); ecc; 0; 0; 0; 0]*ones(I), -t(I)/86400, mu=mu), mu=mu); 
    [pos0, vel0] = CL_oe_kep2car(CL_ex_kepler(0, [abs(sma0(I)); zeros(5, length(I))], -t0(I)/86400, mu=mu), mu=mu); 
    p2(:,I) = pos(1:2,:); 
    dv2(:,I) = vel(1:2,:) - vel0(1:2,:); 
  end
endfunction


function plot_orbit(f, sma, ecc, colorname, thickness, tag)
  scf(f); 
  a = gca(); 
  db = a.data_bounds; 
  
  if (tag <> 0)
    h = CL_g_select(a, "Polyline", tag); 
    CL_g_delete(h); 
  end
  
  anvinf = %pi; 
  if (ecc > 1); anvinf = acos(-1 / ecc); end
  anv = linspace(-anvinf+1.e-6, anvinf-1.e-6, 101); 

  r = (sma .* (1 - ecc.^2)) ./ (1 + ecc .* cos(anv)); 
  rmax = sqrt(db(2,1)^2 + db(2,2)^2); 

  I = find(abs(r) > 2 * rmax); 
  r(I) = %nan; 
  anv(I) = %nan; 
  
  if (r <> [])
    plot(r .* cos(anv), r .* sin(anv), "thickness", thickness); 
    e = CL_g_select(gce(), "Polyline"); 
    e.foreground = color(colorname); 
  
    CL_g_tag(a, tag); 
    a.data_bounds = db; 
  end
  
endfunction


function plot_vinf(f, p, dv, colorname, thickness, tag)
  scf(f); 
  a = gca(); 
  db = a.data_bounds; 
  fact = db(2,2) / 40; 
  
  if (tag <> 0)
    h = CL_g_select(a, "Polyline", tag); 
    CL_g_delete(h); 
  end
  
  for k = 1 : size(p,2)
    plot([p(1,k), p(1,k)+fact*dv(1,k)], [p(2,k), p(2,k)+fact*dv(2,k)], "thickness", thickness); 
    e = CL_g_select(gce(), "Polyline"); 
    e.foreground = color(colorname); 
	e.polyline_style = 4; // arrow
  end
  
  CL_g_tag(a, tag); 
  a.data_bounds = db; 
endfunction




// -----------------------------------
// Plot 
// -----------------------------------

f = scf();
f.visible = "on"; 
f.immediate_drawing = "off"; 

BodyColors = ["seagreen", "blue", "red", "brown"]; 
colornames = BodyColors(dsearch(bodynums, BodyNums, "d")); 

legend_entities = []; 

App = struct(); 
// constants (Earth orbit)
App.MUSUN = muSun; 
App.AU = %CL_au; 
App.E0 = muSun / (2*%CL_au); 
App.V0 = sqrt(muSun / %CL_au); 

App.body_names = bodynames; 
App.body_sma = sma; 
App.body_vel = sqrt(muSun ./ sma); 
App.x = []; 
App.y = []; 
App.ibody = []; 
App.ij = []; 
App.all = list_res; 

App.ro = struct(); // resonnance data
App.ro.x = [];  
App.ro.y = [];  
App.ro.ibody = [];  
App.ro.N = [];  
App.ro.M = [];  

for k = 1 : lstsize(list_res)
  [x, y] = gen_xy(list_res(k), opt, muSun); 
  plot(x, y, "-", "thickness", 1); 
  e = CL_g_select(gce(), "Polyline"); 
  e.foreground = color(colornames(k)); 
  legend_entities = [legend_entities, e(1)]; 
  
  [xm, ym] = gen_xy(list_resM(k), opt, muSun); 
  plot(xm, ym, "o"); 
  e = CL_g_select(gce(), "Polyline"); 
  e.mark_size_unit = "point"; 
  e.mark_size = 4; 
  e.mark_foreground = color(colornames(k)); 
  e.mark_background = e.mark_foreground; 
  
  App.x = [App.x, matrix(x, 1, -1)]; 
  App.y = [App.y, matrix(y, 1, -1)]; 
  App.ibody = [App.ibody, k * ones(matrix(x, 1, -1))]; 
  [i,j] = ind2sub(size(x), 1:length(x)); 
  App.ij = [App.ij, [i; j]]; 
  
  // resonnant orbits
  if (intersect(bodynames(k), res_bodynames) <> [])
    [xr, yr] = gen_xy(list_resR(k), opt, muSun); 
    plot(xr, yr, ":"); 
    e = CL_g_select(gce(), "Polyline"); 
    e.foreground = color(colornames(k)); 
    e.line_style = 3; 

    App.ro.x = [App.ro.x, matrix(xr, 1, -1)]; 
    App.ro.y = [App.ro.y, matrix(yr, 1, -1)]; 
    App.ro.ibody = [App.ro.ibody, k * ones(matrix(xr, 1, -1))]; 
    App.ro.N = [App.ro.N, matrix(list_resR(k).N, 1, -1)]; 
    App.ro.M = [App.ro.M, matrix(list_resR(k).M, 1, -1)]; 
  end
  
end

CL_g_legend(legend_entities, bodynames); 
CL_g_stdaxes(); 

a = gca(); 
a.grid = color("grey70") * [1, 1]; 

a.title.text = "Interplanetary transfers (Tisserand graph)"; 
[a.x_label.text, a.y_label.text] = gen_xy_titles(opt); 


// second plot(orbits)
f2 = scf();
f2.immediate_drawing = "off"; 
f2.axes_size = [400, 400]; 
f2.figure_position = [10, 10]; 
f2.visible = "on"; 

// Sun
plot(0,0, "o"); 
e = CL_g_select(gce(), "Polyline"); 
e.mark_size_unit = "point"; 
e.mark_size = 10; 
e.mark_background = color("yellow"); 
e.mark_foreground = color("gold"); 
a = gca(); 
a.data_bounds = 1.4 * [-1, -1; 1, 1] * (max(sma) / %CL_au); 


// Planet's orbits
for k = 1 : size(sma, "*")
  plot_orbit(f2, sma(k) / %CL_au, 0, colornames(k), 2, 0); 
end

a.isoview = "on"; 
a.tight_limits = "on"; 
CL_g_stdaxes(a); 
a.title.text = "Orbits"; 
a.x_label.text = "x coordinate (AU)"; 
a.y_label.text = "y coordinate (AU)"; 
a.sub_ticks = [0,0]; 
a.grid = color("grey70") * [1, 1]; 
a.grid_position = "background"; 
a.x_ticks = a.y_ticks; 

f2.immediate_drawing = "on"; 

App.fig = f2; 
App.fct.orb_inters = orb_inters; 
App.fct.plot_orbit = plot_orbit; 
App.fct.plot_vinf = plot_vinf; 

App.eventHandler = myHandler; 
// f.event_handler = "myHandler"; // uncomment if necessary

set(f, "user_data", App); 

f.event_handler_enable = "on";
f.immediate_drawing = "on"; 
f.visible = "on";