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

// -----------------------------------------------------------
//> The geoid is an equipotential surface in a frame fixed with
//> respect to the (rotating) Earth. 
//> The equipotential value is determined knowing the height of
//> the geoid at the location given by: longitude = 0 and latitude = 0. 
//> The reference height has been obtained at: 
//> http://earth-info.nga.mil/GandG/wgs84/gravitymod/egm96/intpt.html
//> The plots show the heights of the geoid with respect to the
//> reference ellipsoid, and/or the variations of the potential 
//> on the ellipsoid (0 being the value at the reference position). 
//> 
//> Notes: 
//> - It is assumed that the computation of the potential using spherical 
//> harmonics is valid on the reference ellipsoid and slightly 
//> below. 
//> - For comparison purposes, another map can be found at: 
//> http://en.wikipedia.org/wiki/EGM96
//
// Author: A. Lamy
// -----------------------------------------------------------


// ===========================================================
// Miscellaneous functions
// ===========================================================

// ------------------------------------
// Potential: Gravity + centrifugal force
// pos: cartesian coordinates
// ------------------------------------
function [pot] = potential(pos, nmax)
  // Rotation rate of the Earth (WGS84)
  omega = 0.00007292115; 

  pot = CL_fo_sphHarmPot(pos, [nmax,nmax], inc00=%t) + ..
        CL_fo_centrifugPot(pos, [0;0;omega]);
endfunction 


// ------------------------------------
// radius on ellipsoid at latitude phi (=geocentric latitude)
// a = eq. radius, f = oblateness
// ------------------------------------
function [r] = radius_ell(phi, a, f)
  alpha = f * (2-f) / (1-f)^2;
  r = a ./ sqrt(1 + alpha * sin(phi).^2);
endfunction


// ------------------------------------
// Computes geoid heights
// Such that potential = some reference value
// delta_h: difference of (geodetic) altitude with "pos" 
// ------------------------------------
function [delta_h] = geoid_heights(pos, nmax, potref)

  pos_ell0 = CL_co_car2ell(pos); 

  args = struct(); 
  args.pos_ell0 = pos_ell0; 
  args.nmax = nmax; 
  args.potref = potref; 

  // h and I: same size (h: geodetic)
  function [dpot] = fct(h, I, args)
    pos_ell = args.pos_ell0(:,I); 
    pos_ell(3,:) = pos_ell(3,:) + h; 
    pos = CL_co_ell2car(pos_ell); 
    dpot = potential(pos, args.nmax) - args.potref; 
  endfunction
  
  // altitude range assumed: [-500m, 500m]
  h1 = pos_ell0(3,:) - 500;
  h2 = pos_ell0(3,:) + 500; 

  h = CL_fsolveb(fct, h1, h2, args, dxtol=0.1, meth="s2"); 
  delta_h = h - pos_ell0(3,:); 
    
endfunction


// ------------------------------------
// Plots potential
// lambda, phi: (geocentric) longitude, altitude (rad)
// pot(i,j) = value for lambda(i) and phi(j) 
// ------------------------------------
function plot_potential(lambda, phi, pot, text); 

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

  colorbar(min(pot), max(pot));

  // adjust colorbar labels
  h = CL_g_select(f, "Compound"); 
  h.parent.y_ticks.labels = string(strtod(h.parent.y_ticks.labels));

  Sgrayplot(lambda*%CL_rad2deg, phi*%CL_rad2deg, pot); 

  CL_plot_earthMap(color_id=-1);

  a = gca();
  a.title.text = text; 
  a.x_label.text = "Geocentric longitude"; 
  a.y_label.text = "Geocentric latitude"; 

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

endfunction


// ------------------------------------
// Plots geoid 2d
// lambda, phi: (geocentric) longitude, latitude (rad) 
// alt(i,j) = altitude value for lambda(i) and phi(j) 
// ------------------------------------

function plot_geoid_2d(lambda, phi, height, text); 

  f = scf(); 
  f.visible = "on"; 
  f.immediate_drawing = "off"; 
  f.color_map = jetcolormap(100); 
  a = gca();

  colorbar(min(height), max(height));

  // adjust colorbar labels
  h = CL_g_select(f, "Compound"); 
  h.parent.y_ticks.labels = string(strtod(h.parent.y_ticks.labels));

  Sgrayplot(lambda*%CL_rad2deg, phi*%CL_rad2deg, height); 

  CL_plot_earthMap(color_id=-1);

  a = gca();
  a.title.text = text; 
  a.x_label.text = "Geocentric longitude"; 
  a.y_label.text = "Geocentric latitude"; 
 
  f.immediate_drawing = "on"; 
  f.visible = "on"; 

endfunction


// ------------------------------------
// Plots geoid 3d
// lambda, phi: (geocentric) longitude, latitude (rad) 
// alt(i,j) = altitude value for lambda(i) and phi(j) 
// ------------------------------------

function plot_geoid_3d(lambda, phi, delta_h, titre); 

// function for interpolation on sphere
C = splin2d(lambda, phi, matrix(delta_h, nx, ny), "periodic");

function [dh] = interpole_h(lon,lat)
 dh = interp2d(lon.*ones(lat), lat.*ones(lon), lambda, phi, C, "C0");
endfunction

function [r] = rapp(phi)
  r = radius_ell(phi, 1., 0.); 
endfunction

f = scf();
f.visible = "on"; 
f.immediate_drawing = "off"; 
 
nbcol = 201; 
f.color_map = jetcolormap(nbcol); 

[Lambda, Phi] = ndgrid(lambda, phi); 
pos_sph = [Lambda(:)'; Phi(:)'; rapp(Phi(:)')]; 
pos = CL_co_sph2car(pos_sph); 

x = matrix(pos(1,:), size(Lambda));
y = matrix(pos(2,:), size(Lambda));
z = matrix(pos(3,:), size(Lambda));

plot3d2(x, y, z); 

e = gce();

// reverse facet orientation
e.data.x(1:4,:) = e.data.x(4:-1:1,:); 
e.data.y(1:4,:) = e.data.y(4:-1:1,:); 
e.data.z(1:4,:) = e.data.z(4:-1:1,:); 

d = e.data;

V = (2+d.x(:)') + 10 * (2+d.y(:)') + 100 * (2+d.z(:)');
V1 = (2+x(:)') + 10 * (2+y(:)') + 100 * (2+z(:)');
[V1u, k1] = unique(V1); 

K1 = dsearch(V1, V1u); 
K = dsearch(V, V1u, "d");

Z = delta_h(k1(K));

colorbar(min(Z), max(Z));
h = CL_g_select(f, "Compound"); 
h.parent.y_ticks.labels = string(strtod(h.parent.y_ticks.labels));

col = 1 + (nbcol-1)* (Z - min(Z)) / (max(Z) - min(Z)); 
col = matrix(col, size(d.x)); 
delta = matrix(Z, size(d.x)); 

// F: inverse of amplification factor
F = 500; 

d.x = d.x .* (1 + delta / F); 
d.y = d.y .* (1 + delta / F); 
d.z = d.z .* (1 + delta / F); 

tl = tlist(["3d" "x" "y" "z" "color"],d.x,d.y,d.z,col);
e.data = tl;
e.color_flag = 3; // interpolated
e.color_mode = -1;

pts = CL_getEarthMap(res = "high", er=1); 
pts(3,:) = pts(3,:) * 1.005 .* (1 + interpole_h(pts(1,:),pts(2,:))/F) .* rapp(pts(2,:)); 

pos = CL_co_sph2car(pts); 
param3d(pos(1,:), pos(2,:), pos(3,:));  

h = CL_g_select(gce(), "Polyline"); 
h.foreground = color("black"); 

function Plot_line(lon,lat)
  r = 1.005 * (1 + interpole_h(lon,lat)/F) .* rapp(lat); 
  pos = CL_co_sph2car([lon; lat; r]); 
  param3d(pos(1,:), pos(2,:), pos(3,:));  
  h = CL_g_select(gce(), "Polyline"); 
  h.foreground = color("grey50"); 
endfunction

// grille lon/lat
for lon = (-180:15:180-15) * %CL_deg2rad
  lat = linspace(-%pi/2, %pi/2, 100); 
  Plot_line(lon*ones(lat),lat); 
end

for lat = (-90+15:15:90-15) * %CL_deg2rad
  lon = linspace(-%pi, %pi, 100); 
  Plot_line(lon,lat*ones(lon)); 
end

a = gca();
CL_g_stdaxes(a); 

a.axes_visible = ["off", "off", "off"];
a.grid = [-1,-1,-1]; 
a.box = "off"; 
a.x_label.text = ""; 
a.y_label.text = ""; 
a.z_label.text = ""; 
a.isoview = "on";
a.tight_limits = "on"; 
a.margins([1,2,4]) = 0.05; // left, top, bottom
a.rotation_angles = [90,25];
a.title.text = titre;

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

endfunction


// ===========================================================
// MAIN
// ===========================================================
deff("[n] = NMAX()", "n = max(size(%CL_cs1nm))"); 

// max degree/order of potential
nmax = 30; 

// height of geoid / ref ellipsoid at position
// longitude = 0 and latitude = 0
dh0 = 17.16; 

// option for computation / plot (see meaning below)
iplot = 2; 

desc_param = list(..
   CL_defParam("Max degree and order of potential (>=2 and <=" + msprintf("%d",NMAX()) + ")", nmax, accv=2:NMAX()),..
   CL_defParam("Geoid height at location: longitude = 0 and latitude = 0", dh0, units=['m'], valid='$x >= -500 & $x <= 500'), ..
   CL_defParam("Plot: 1=geoid heights (2D), 2=geoid heights (3D), 3=potential variations", iplot, accv = 1:3) ..
);

[nmax, dh0, iplot] = CL_inputParam(desc_param);


// ------------------------------------
// Computations / plot
// ------------------------------------

// Value of equipotential
// knowing height = dh0 above ellipsoid at (lon=0,lat=0)
potref = potential([%CL_eqRad+dh0; 0; 0], nmax);

// longitude/latitude mesh
nx = 81; 
ny = 41;
lambda = linspace(-%pi, %pi, nx); 
phi = linspace(-%pi/2, %pi/2, ny); 
[Lambda, Phi] = ndgrid(lambda, phi); 
Rad = radius_ell(Phi, %CL_eqRad, %CL_obla); 

pos_sph = [Lambda(:)'; Phi(:)'; Rad(:)']; 
pos = CL_co_sph2car(pos_sph); 



// geoid 2D
if (iplot == 1)
  delta_h = geoid_heights(pos, nmax, potref); 
  nmax_str = sprintf("%d", nmax); 
  text = "Geoid heights / reference ellipsoid (m) - EGM96s ("+nmax_str+"x"+nmax_str+")"; 
  plot_geoid_2d(lambda, phi, matrix(delta_h, nx, ny), text); 
end

// geoid 3D
if (iplot == 2)
  delta_h = geoid_heights(pos, nmax, potref); 
  nmax_str = sprintf("%d", nmax); 
  text = "Geoid heights / reference ellipsoid (m) - EGM96s ("+nmax_str+"x"+nmax_str+")"; 
  plot_geoid_3d(lambda, phi, matrix(delta_h, nx, ny), text); 
end

// potential variations on ellipsoid
if (iplot == 3)
  pot = potential(pos, nmax); 
  nmax_str = sprintf("%d", nmax); 
  text = "Potential variations on ref. ellipsoid (m2/s2) - EGM96s ("+nmax_str+"x"+nmax_str+")"; 
  plot_potential(lambda, phi, matrix(pot, nx, ny) - potref, text); 
end