//  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 [] = CL_plot_swath(pos, win_id, style, color_id, data_bounds)
// Simple swath 2D plot
//
// Calling Sequence
// CL_plot_swath(pos [, win_id, style, color_id, data_bounds])
//
// Description
// <itemizedlist><listitem>
// <p>Plots a 2D longitude/latitude swath.</p>
// <p></p>
// <p>The swath is defined by several "trajectories", from leftmost to rightmost or vice versa. For instance: </p>
// <p>- For 2 trajectories: trajectory1 = left side, trajectory2 = right side. </p>
// <p>- For 3 trajectories: trajectory1 = left side, trajectory2 = middle, trajectory3 = right side. </p>
// <p></p>
// <p>The trajectories (<b>pos</b>) can be given in 2 ways: </p>
// <p>- list: list(pos1, pos2,... ), where posk is a trajectory in cartesian coordinates (3xN).  </p>
// <p>- hypermat(3,:,:): pos(:,:,k) is then the kth trajectory (size = 3xN).  </p>
// <p></p>
// <p>The swath can be plotted using the following styles: </p>
// <p>- style = "f" ("fill"): polygons made from positions at index k and k+1 are filled with color k.  </p> 
// <p>- style = "c" ("contour"): polygons made from positions at index k and k+1 are plotted with color k.  </p> 
// <p>- style = "s" ("segment"): segments made from positions at index k are plotted with color k.  </p> 
// <p></p>
// </listitem>
// <listitem>
// <p><b>Notes</b>:</p>
// <p>- If style == "f" or "c", the last color index is not used. </p>
// <p>- The function implements a simple algorithm using the longitudes only to detect if a polygon contains a pole. </p>
// <p> The algorithm works if the positions define the points of a
// "real" swath whose width on Earth is less than 180 deg, 
// Otherwise, results are unpredictable. </p> 
// <p>- The plotted longitudes are spherical longitudes. </p>
// <p>- Polygons or segments that contain %nan are ignored. </p>
// <p>- The function creates only one graphical entity. Properties can be changed after the call, for instance by: </p>
// <p> h = CL_g_select(gce(), "Polyline"); h.line_mode = "on"; h.foreground = color("black"); </p>
// <p></p>
// </listitem>
// </itemizedlist>
//
// Parameters
// pos: Points describing the swath in cartesian coordinates. (3xNxP) or list(3xN). See above for details. 
// win_id: (integer or window handle, optional) Figure Id. Id of current window if omitted.
// style: (string, optional) The style used to draw the swath: "f", "c" or "s". Default is "c". (1x1)
// color_id: (integer, optional) Vector of color indices used to draw the swath. Default: black color (1xN or 1x1)
// data_bounds: (optional) View area: [longmin, latmin; longmax, latmax] in degrees. Default is [-180, -90; 180, 90].
//
// Authors
// CNES - DCT/SB
//
// Examples
// // Generate orbit
// t0 = 0; // initial time
// kep0 = [7200.e3; 0; 1.4; 0; 0; 0]; // orbit parameters
// t = t0 + (0 : 1 : 95) / 1440; // days
// kep = CL_ex_kepler(t0, kep0, t); // orbit propagation
// [pos, vel] = CL_oe_kep2car(kep); // inertial position and velocity
//
// // Generate swath points (arbitrary radius)
// ang = 0.3; // "center angle" (rad)
// p1 = CL_fr_qsw2inertial(pos, vel, [cos(ang); 0; sin(ang)]); 
// p2 = pos; 
// p3 = CL_fr_qsw2inertial(pos, vel, [cos(ang); 0; -sin(ang)]); 
//
// // Plot (filled, continuous colors)
// f = scf();
// f.color_map = jetcolormap(101); 
// colorbar(t(1)*1440, t($)*1440, colminmax = [1, 101]);
// CL_plot_swath(list(p1, p2, p3), style = "f", color_id = 1+ round(100*t/t($))); 


// ================================================
// Auxiliary functions
// ================================================
  
  function [lon, lat, colid] = plsw_genPoly(pos, style, color_id, db)
  // --------------------------------------------------
  // Generates polygons or segments (lon/lat) from points in cartesian coordinates
  // pos: swath points in cartesian coordinates. (3xNxP)
  // color_id: associated color indices (1xN)
  // lon: longitude of vertices (QxM) (rad) - no %nan
  // lat: latitude of vertices (QxM) (rad) - no %nan
  // colid: associated colors (1xM) (rad)
  // --------------------------------------------------

  // For polygons: last point coincides with the first one lon(5,:) = lon(1,:)
  // The longitudes are such that given the first point of one polygon (of longitude lon1)
  // the following points of the same polygon are "close" in longitude to the first one:
  // lon2, lon3, lon4,... in [lon1 - %pi, lon1 + %pi]

    pos_sph = zeros(pos); 

    n = size(pos, 2); 
    N = size(pos, 3); 
    for (k = 1 : N)
      pos_sph(:,:,k) = CL_co_car2sph(pos(:,:,k)); 
    end 
    
    if (style == "f" | style == "c")
      // Polygons
      lon = zeros(2*N+1, n-1); 
      lat = zeros(2*N+1, n-1); 
      
      lon(1,:) = pos_sph(1,1:n-1,1); 
      lon($,:) = lon(1,:); 
      lat(1,:) = pos_sph(2,1:n-1,1); 
      lat($,:) = lat(1,:); 

      for (k = 1 : N)
        lon(k,:) = pos_sph(1,1:n-1,k); 
        lon(N+k,:) = pos_sph(1,2:n,N-k+1); 
        lat(k,:) = pos_sph(2,1:n-1,k); 
        lat(N+k,:) = pos_sph(2,2:n,N-k+1); 
      end
      
      lon($,:) = lon(1,:); 
      lat($,:) = lat(1,:); 
       
    else
      // segments
      lon = zeros(N, n); 
      lat = zeros(N, n); 

      for (k = 1 : N)
        lon(k,:) = pos_sph(1,1:n,k); 
        lat(k,:) = pos_sph(2,1:n,k); 
      end

    end
    
    // Forcing longitudes in correct bounds ([bmin, bmax])
    // bmin = middle longitude - %pi; bmax = middle longitude + %pi
    bmin = mean(db(:,1)) - %pi;  
    bmax = bmin + 2*%pi; 
    lon(1:$,:) = CL_rMod(lon(1:$,:), bmin, bmax);
    
    // Points of each polygon are forced to be close to its first point   
    N = size(lon, 1);    
    lon(2:$,:) = CL_rMod(lon(2:$,:), repmat(lon(1,:),N-1,1) - %pi, repmat(lon(1,:),N-1,1) + %pi);
    
    // one color per segment or polygon
    colid = color_id(1:size(lon,2)); 

    // eliminates polygons that contains %nan
    I = find(isnan(sum(lon+lat, "r"))); 
    lon(I) = []; 
    lat(I) = []; 
    colid(I) = []; 
          
  endfunction
  
  
  function [I] = plsw_detectPoles(lon, lat)
  // --------------------------------------------------
  // Indices of polygons that contain the pole (N or S)
  // works if lon and lat are []. 
  // lon: longitudes of vertices (PxN) (rad)
  // lat: latitudes of vertices (PxN) (rad)
  // I: indices of polygons containing the pole
  // --------------------------------------------------
     
  // Hypothesis for pole detection.
  
  // 1) The polygone is considered to be the swath of a real instrument, so that less
  // then 180 deg in longitudes are seen at once
  // 2) The Earth is spherical (?)
  
  // Let's say lon1, 1on2, lon3, lon4 the longitudes of the four
  // vertices of one polygon, with (lon1 <= lon2 <= lon3 <= lon4 )
  // Under hypothesis 1) and 2) the condition for the pole to contain one of the poles  
  // abs(lon4 - lon1) > %pi
  
  // In order to say that the detected pole is the southern or northern
  // we consider the mean latitude of the polygon's vertices lat_m
  // If lat_m > 0 then the pole encompassed by the polygon is the northern pole
  
    // Difference between maximum and minimum longitudes (for each column = each polygon) 
    dif = (max(lon, "r") - min(lon, "r"));
    // 
    // Indices contenants le pole
    I = find(abs(dif) > %pi);
           
  endfunction 
  
  
  function [lon, lat, colid] = plsw_genptsPole(lon, lat, colid, db)
  // --------------------------------------------------
  // Prepares data for plot (for polygons that contain a pole (N or S)) 
  // => closes the polygon with additional points 
  // lon/lat: vertices longitudes/latitudes (PxN) (rad)
  // colid: color indices (1xN)
  // db: data bounds = [lon_min, lat_min; lon_max, lat_max] (rad) (2x2)
  // --------------------------------------------------
    
    // determines which pole : pole = -1 => South, 1 => North
    // (uses sign of mean latitude)
    pole = 1 * ones(lon(1,:)); 
    I = find(mean(lat, "r") < 0); 
    pole(I) = -1; 
      
    // Longitudes are sorted in increasing order
    // and latitudes accordingly
    bmin = mean(db(:,1)) - %pi; 
    bmax = bmin + 2*%pi; 

    lon = CL_rMod(lon, bmin, bmax); 
    [lon, i] = gsort(lon, "r", "i");

    sz = size(i);
    I = sub2ind(sz, i, repmat(1:sz(2), sz(1), 1));
    lat = matrix(lat(I), sz(1), sz(2));
  
    // Adding points to include pole
    lat_add = %pi/2 * 1.1 * (pole .* ones(lon(1,:))); 
    lon = [ lon; lon(1,:)+2*%pi; lon(1,:)+2*%pi; lon($,:)-2*%pi; lon($,:)-2*%pi ]; 
    lat = [ lat; lat(1,:); lat_add; lat_add; lat($,:)]; 
    
    // colid: unchanged
    
  endfunction

  
  function [Kn, KI] = plsw_insertAfterInd(n, I, nb); 
  // --------------------------------------------------
  // array insertion utility for inserting nb elmements 
  // after each element of index I.
  // I = set of indices in 1:n 
  // (increasing order, unique values, length(I) <= n)
  // nb = number of elements to insert after each index
  // Kn: indices where to insert "old" elements in new array
  // KI: indices of elements "I" in new array
  // (insertion positions in new array are: 
  // KI+1, KI+2 .. KI+nb
  // NB: new array has size = n + nb * length(I)
  // --------------------------------------------------
    // note1: n == 0 => Kn = [] 
    // note2: (0:min(0,n-1)) == 0 if n >= 1, else []
    d = zeros(1, n);  
    d(I) = nb; 
    Kn = (1:n) + cumsum([(0:min(0,n-1)), d(1:n-1)]);
    KI = (0 : length(I)-1) * nb + I; 
  endfunction

  
  function [lon, lat, colid] = plsw_genptsNotPole(lon, lat, colid, db)
  // --------------------------------------------------
  // Prepares data for plot (for polygons that do not contain the pole) 
  // => detect if polygons is only partly inside the bounds
  // => duplicates the polygon (add + or - 2*%pi to the longitudes) 
  // lon/lat: vertices longitudes/latitudes (PxN) (rad)
  // colid: color indices (1xN)
  // db: data bounds = [lon_min, lat_min; lon_max, lat_max] (rad) (2x2)
  // --------------------------------------------------

    lon_min = min(lon, "r"); 
    lon_max = max(lon, "r"); 
    
    bmin = mean(db(:,1)) - %pi; 
    bmax = bmin + 2*%pi; 

    I1 = find(lon_min <= bmin & lon_max > bmin); 
    I2 = find(lon_min <= bmax & lon_max > bmax); 
    
    if (length(I1) + length(I2) == 0); return; end
    
    // generate points 
    M = [[I1; -1 * ones(I1)],  [I2; +1 * ones(I2)]]; 
    M = CL_sortMat(M, M(1,:)); 
    I = M(1,:); 
    bound = M(2,:); 
    
    // added polygons
    lon_add = lon(:,I) - repmat(bound * 2*%pi, size(lon,1), 1); 
    lat_add = lat(:,I); 
    colid_add = colid(I); 
    
    // initial arguments
    lon1 = lon; 
    lat1 = lat; 
    colid1 = colid; 
    
    // insert polygons
    n = size(lon,2); 
    nb = length(I); 
    [Kn, KI] = plsw_insertAfterInd(n, I, 1);
    
    m = size(lon,1); 
    lon = %nan * ones(m, n+nb); 
    lat = %nan * ones(m, n+nb); 
    colid = %nan * ones(1, n+nb); 
    
    lon(:,Kn) = lon1; 
    lat(:,Kn) = lat1; 
    colid(Kn) = colid1; 

    lon(:,KI+1) = lon_add; 
    lat(:,KI+1) = lat_add; 
    colid(KI+1) = colid_add; 
    
  endfunction

    
  function [e] = plsw_plotPoly(ipol, lon, lat, style, color_id, db)
  // --------------------------------------------------
  // Plot polygon according to style ("f", "c" or "s")
  // ipol: 1 if polygon contains a pole, otherwise: 0
  // lon/lat: vertices longitudes/latitudes (PxN) (rad)
  // color_id: color indices (1xN)
  // db: data bounds = [lon_min, lat_min; lon_max, lat_max] (rad) (2x2)
  // e: created graphical entity. May be empty
  // --------------------------------------------------
  
    e = []; 
    
    if (lon == []); return; end
    
    // bounds in longitude
    bmin = mean(db(:,1)) - %pi; 
    bmax = bmin + 2*%pi; 
    
    if (ipol == 0); 
      [lon, lat, color_id] = plsw_genptsNotPole(lon, lat, color_id, db); 
    else
      [lon, lat, color_id] = plsw_genptsPole(lon, lat, color_id, db); 
    end
     
    // remove polygons that are not visible (outside bounds)
    I = find(max(lon, "r") < db(1,1) | min(lon, "r") > db(2,1) | ..
             max(lat, "r") < db(1,2) | min(lat, "r") > db(2,2)); 
    lon(:,I) = [];  
    lat(:,I) = [];  
    color_id(I) = []; 

    if (lon == []); return; end

    if (style == "f")
      // Fill polygons
      xfpolys(lon * 180/%pi, lat * 180/%pi, -color_id);

    elseif(style == "c" | style == "s")  
      // Do not fill
      xpolys(lon * 180/%pi, lat * 180/%pi, color_id);    
      
    end
    
    e = gce();

    // h = CL_g_select(e, "Polyline"); 
    // h.line_mode = "off"; // in case user wants to change later 
    
  endfunction
  
  
// ================================================
// MAIN
// ================================================

// Argument Checking
if (exists("win_id","local")) then   
  f = scf(win_id)
else
  f = gcf();
end

if (~exists("style", "local")); style = "c"; end;  
if (~exists("color_id", "local")); color_id = color("black"); end;  
if (~exists("data_bounds","local")); data_bounds = [-180, -90; 180, 90]; end  

// Error checking
// case list => convert to hypermat
if (typeof(pos) == "list")
  lpos = pos; 
  if (lstsize(lpos) < 2);  CL__error("Invalid size for pos"); end
    pos = lpos(1); 
  if (size(pos,1) <> 3); CL__error("Invalid size for pos"); end
    for (k = 2 : lstsize(lpos))
      if (size(lpos(k),1) <> 3 | size(lpos(k),2) <> size(pos,2))
        CL__error("Invalid size for pos"); 
      end
      pos(:,:,k) = lpos(k); 
    end
end

if (size(pos, 1) <> 3)
  CL__error("Invalid size for pos");
end
N = size(pos,2); 

if (style <> "f" & style <> "c" & style <> "s")
  CL__error("Invalid style");
end

if (size(color_id, 1) <> 1 | (size(color_id, 2) <> 1 & size(color_id, 2) <> N))
  CL__error("Invalid size for color_id");
end

// if only 1 point for each trajectory => duplicates 
if (N == 1)
  pos(:,2,:) = pos(:,1,:); 
  color_id(1,2) = color_id(1,1); 
end


if (size(data_bounds,1) <> 2 | size(data_bounds,2) <> 2)
  CL__error("Invalid size for data_bounds");
end
if (data_bounds(2,1) <= data_bounds(1,1) | data_bounds(2,1) > data_bounds(1,1)+360)
  CL__error("Invalid data bounds in longitude");
end 
if (data_bounds(2,2) <= data_bounds(1,2) | data_bounds(1,2) < -90 | data_bounds(2,2) > 90)
  CL__error("Invalid data bounds in latitude");
end 

// data bounds in degrees
db = data_bounds * %pi / 180; 

// color indices: size N
color_id = color_id .* ones(1:N); 

// Generate polygons (+ associated colors)
[lon, lat, colid] = plsw_genPoly(pos, style, color_id, db);


// plot
immediate_drawing_save = f.immediate_drawing; // store mode
f.immediate_drawing = "off";

a = gca();
a.clip_state = "clipgrf";
   
// array of created graphical entities   
e = []; 
 
// indices of polygons containing the poles (0 added for convenience)
Ip = [0, plsw_detectPoles(lon, lat)];

// for each polygon containing a pole: plot pole and previous data
for k = 2 : length(Ip)
  I = Ip(k-1)+1 : Ip(k)-1; // Ip(k-1): always defined
  // not pole
  e = [e, plsw_plotPoly(0, lon(:,I), lat(:,I), style, colid(I), db)]; 
  // pole
  e = [e, plsw_plotPoly(1, lon(:,Ip(k)), lat(:,Ip(k)), style, colid(Ip(k)), db)]; 
end

// last part (after last pole; OK if no pole detected)
I = Ip($) + 1 : size(lon,2); 
e = [e, plsw_plotPoly(0, lon(:,I), lat(:,I), style, colid(I), db)]; 

// if nothing plotted => creates invisible plot
if (e == [])
  plot(data_bounds(1,1), data_bounds(1,2));  // lonmin, latmin (deg)
  e = gce(); 
end

// group all created entities
glue(e); 

a.data_bounds = data_bounds;
a.tight_limits = "on"; 
a.axes_visible = ["on", "on", "on"]; 
CL_g_stdaxes(a);  

f.immediate_drawing = immediate_drawing_save; // restore mode
  
endfunction