//  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 [M,omega,pos2,vel2,jacob] = CL__fr_convert(frame1, frame2, jd, pos1, vel1, opt, args)
// Conversion from a frame to another frame (internal function)
//
// Calling Sequence
// [M,omega,pos2,vel2,jacob] = CL__fr_convert(frame1, frame2, jd, pos1, vel1, opt, args)
//
// Description
// <itemizedlist><listitem>
// <p>This function is meant to be called by higher level functions (CL_fr_convert and CL_fr_convertMat)</p>
// <p>Depending on the value of opt, outputs are optionaly computed (if not computed: empty): 
// <p>  - if opt="mat": Only M is computed</p>
// <p>  - if opt="mat_omega": M and omega are computed</p>
// <p>  - if opt="pos": pos2 is computed </p>
// <p>  - if opt="pos_vel": pos2 and vel2 are computed</p>
// <p>  - if opt="pos_vel_jac": pos2, vel2 and jacob are computed</p>
// <p>Notes on interface: </p>
// <p> - The values that are not computed are returned as []</p>
// <p></p></listitem>
// <p>Available frames are : </p>
// <p>   - TIRS: Terrestrial Intermediate Reference System</p>
// <p>   - ITRS: International Terrestrial Reference System</p>
// <p>   - PEF: Pseudo Earth Fixed</p>
// <p>   - CIRS: Celestial Intermediate Reference System</p>
// <p>   - TEME: True Equator Mean Equinox</p>
// <p>   - TOD: True of Date</p>
// <p>   - Veis: G50 Veis</p>
// <p>   - GCRS: Geocentric Celestial Reference System</p>
// <p>   - EME2000: Earth Mean Equator at J2000.0</p>
// <p>   - MOD: Mean of Date </p>
// <p>   - EOD: Ecliptic of Date</p>
// <p>   - ECF: Earth Centered (Earth) Fixed</p>
// <p>   - ECI: Earth Centered Inertial</p>
// <p></p>
// <p>See <link linkend="Reference frames">Reference frames</link> for more details on the definition of reference frames.</p>
// <p></p>
// <p>The ECI frame can be CIRS or Veis depending on the configuration (preferences) of CelestLab. (See <link linkend="Configuration">Configuration</link>)</p>
// <p>The ECF frame is defined accordingly to the ECI frame. TIRS if ECI=CIRS and PEF if ECI=Veis.</p>
// <p></p></listitem>
// <listitem>
// <p>Notes : </p>
// <p>- The choice of the reference time scale (TREF) used for "cjd" is up to the user: </p>
// <p>** For maximum accuracy, the user needs to provide "tt_tref" and "ut1_tref" according 
//      to the reference time scale (TREF) used.</p>
// <p>- The frame names are case sensitive</p>
// <p>- ICRS (identical to GCRS in CelestLab) is also accepted as a frame name.</p>
// </listitem>
// </itemizedlist>
//
// Parameters
// frame1: (string) Initial frame (1x1)
// frame2: (string) Final frame (1x1)
// jd: Two-part julian day (Time scale: TT) (2xN)
// pos1: Position vector in initial frame [m] (3xN) or empty
// vel1: Velocity vector in initial frame [m/s] (3xN) or empty
// opt: (string) Type of computation: "pos", "pos_vel", "pos_vel_jac", "mat", "mat_omega"
// args: Structure containing fields used by CL__fr_xxx functions. See each function for more details.
// M: Transformation matrix from initial frame to final frame [m] (3x3xN)
// omega: Angular velocity vector final/initial, coord. in initial frame [rad/s] (3xN)
// pos2: Position vector in final frame [m] (3xN)
// vel2: Velocity vector in final frame [m/s] (3xN)
// jacob: Jacobian of the transformation (6x6xN)
//
// Authors
// CNES - DCT/SB
//

  
// --------------------------------------------------
// Find the hub associated to a frame.
// frame = (string) frame name (1x1)
// hub = (string) hub name = "nearest" major frame
// --------------------------------------------------
function [hub] = find_hub(frame)

  // 1st = frame, 2nd = hub
  // size = Nx2
  frame_hubs = [ ...
     "TIRS",    "TIRS";  
     "PEF",     "TIRS";  
     "ITRS",    "TIRS";  
     "CIRS",    "CIRS";
     "TEME",    "CIRS";
     "TOD",     "CIRS";
     "Veis",    "CIRS";
     "GCRS",    "GCRS";
     "EOD",     "GCRS";
     "EME2000", "GCRS";
     "MOD",     "GCRS" ]; 
     
  I = find(frame == frame_hubs(:,1));
  if (I == [])
    CL__error("Unknown reference frame: " + frame);
  end

  hub = frame_hubs(I,2);

endfunction
  

// --------------------------------------------------
// Find the function associated to a frame transformation.
// frame1, frame2 = (string) frame name (1x1)
// fun = function frame1->frame2 or frame2->frame1
// direct: 1: OK, -1: inverted direction
// --------------------------------------------------
function [fun, direct] = find_fct(frame1, frame2)

  // List of frames that are linked to one another:
  // (hub -> frame or hub -> hub)
  // (first row = initial frame, second row = final frame)
  // size = Nx2
  frame_links = [ ...
     "TIRS", "ITRS";
     "TIRS", "PEF";  
     "CIRS", "TIRS";  // hub -> hub
     "CIRS", "TOD";
     "CIRS", "TEME";
     "CIRS", "Veis";
     "GCRS", "CIRS";  // hub -> hub
     "GCRS", "MOD";
     "GCRS", "EOD";
     "GCRS", "EME2000";
     "GCRS", "TIRS"   // hub -> hub
     ]; 

  // associated functions 
  fun_list = list( ...
     CL__fr_tirs2itrs, ...
     CL__fr_tirs2pef, ...
     CL__fr_cirs2tirs, ...
     CL__fr_cirs2tod, ...
     CL__fr_cirs2teme, ...
     CL__fr_cirs2veis, ...
     CL__fr_gcrs2cirs, ...
     CL__fr_gcrs2mod, ...
     CL__fr_gcrs2eod, ...
     CL__fr_gcrs2eme2000, ...
     CL__fr_gcrs2tirs); 
      
  // looks for the frame link (1->2)
  direct = 1;
  I = find(frame1 == frame_links(:,1) & ...
           frame2 == frame_links(:,2));

  // if not found => looks for: 2->1
  if (I == [])
    direct = -1;
    I = find(frame1 == frame_links(:,2) & ...
             frame2 == frame_links(:,1));

    if (I == [])
      CL__error("Unknown transformation from " + frame1 + " to " + frame2);
    end
  end

  fun = fun_list(I);

endfunction


// --------------------------------------------------
// Frame transformation matrix and angular velocity 
// vector from frame1 to frame2
// frame1 = (string) frame name 1
// frame2 = (string) frame name 2
// compute_omega = (boolean) %t if omega should be computed
// M = Transformation matrix from frame1 to frame2
// omega = Angular velocity vector of frame2 relative to frame1, with coordinates in frame1
//
// Note : Only works for "linked" frames 
// --------------------------------------------------
function [M, omega] = transf_matrix(frame1, frame2, jd, compute_omega, args)

  // direct == -1 if fun corresponds to frame2->frame1
  // and not frame1->frame2
  [fun, direct] = find_fct(frame1, frame2); 
  
  if (compute_omega)
    [M, omega] = fun(jd, args);
  else
    M = fun(jd, args);
    omega = [];
  end
    
  if (direct == -1)
    // We have: M(Ra->Rb) and omega(Rb/Ra) with coord. in Ra
    // We want: M(Rb->Ra) and omega(Ra/Rb) with coord. in Rb
    omega = -M * omega; // if omega empty => remains empty
    M = M';
  end

endfunction
  

// ----------------------------------
// MAIN
// ----------------------------------
  
if (argn(2) < 3)
  CL__error('Not enough input arguments');
end

if (typeof(frame1) <> "string" | typeof(frame2) <> "string")
  CL__error("Wrong type for input arguments"); 
end

// Replace ECI by the name of the frame from CelestLab configuration
// Replace ECF by the corresponding rotation frame (TIRS or PEF) 
ECI_frame = CL_configGet("ECI_FRAME");
if (ECI_frame == "CIRS"); ECF_frame = "TIRS"; end;
if (ECI_frame == "Veis"); ECF_frame = "PEF"; end;

if (frame1 == "ECI"); frame1 = ECI_frame; end
if (frame2 == "ECI"); frame2 = ECI_frame; end
if (frame1 == "ECF"); frame1 = ECF_frame; end
if (frame2 == "ECF"); frame2 = ECF_frame; end

// Substitute ICRS by GCRS
if (frame1 == "ICRS") frame1 = "GCRS"; end;
if (frame2 == "ICRS") frame2 = "GCRS"; end;


// compute_pos_vel == %t if result is: pos (+vel, +jacob) 
// and not: mat (+omega)
compute_pos_vel = %f; 
if (opt == "pos" | opt == "pos_vel" | opt == "pos_vel_jac")
  compute_pos_vel = %t; 
end

// compute_omega == %t if omega needed in computations
// (needed if explicitly wanted or for velocity or jacobian)
compute_omega = %f; 
if (opt == "mat_omega" | opt == "pos_vel" | opt == "pos_vel_jac")
  compute_omega = %t; 
end

// compute_jacob == %t if jacobian should be computed
compute_jacob = %f; 
if (opt == "pos_vel_jac")
  compute_jacob = %t; 
end


// frames sequence: frame1 -> hub1 -> hub2 -> frame2
hub1 = find_hub(frame1);
hub2 = find_hub(frame2);
frames_seq = [frame1, hub1, hub2, frame2]; 

M = []; 
omega = []; 
jacob = []; 
pos_tmp = [];
vel_tmp = [];

if (compute_pos_vel)
  pos_tmp = pos1;
  vel_tmp = vel1;
end

for (i = 1 : 3)
  F1 = frames_seq(i); 
  F2 = frames_seq(i+1); 

  // does nothing if frames are identical
  if (F1 == F2); continue; end

  // M_tmp = matrix from F1 to F2 (X_F2 = M_tmp * X_F1)
  // omega_tmp = omega(F2/F1), coordinates in F1

  [M_tmp, omega_tmp] = transf_matrix(F1, F2, jd, compute_omega, args);

  if (compute_pos_vel)
    // NB: velocity computed if omega not empty
    [pos_tmp, vel_tmp, jacob_tmp] = CL_rot_pvConvert(pos_tmp, vel_tmp, M_tmp, omega_tmp, cjac=compute_jacob);
    if (jacob == [])
      jacob = jacob_tmp; 
    else
      jacob = jacob_tmp * jacob;
    end

  else
    if (M == [])
      omega = omega_tmp; 
      M = M_tmp;  
    else
      // omega => coordinates in frame1
      // (convert omega_tmp to frame1 and add omega)
      omega = omega + M' * omega_tmp; 
      // M => matrix from frame1 to current frame (=F2)
      M = M_tmp * M;  
    end
                
  end

end


// NB: if frame1 == frame2: pos2 = pos1 and vel2 = vel1 
if (compute_pos_vel)
  pos2 = pos_tmp;
  vel2 = vel_tmp;
end
  

// check matrices (if []) 
// NB: matrices are [] if frame1 == frame2; 
N = size(jd,2); 

// "mat" wanted but empty
if ((opt == "mat" | opt == "mat_omega") & M == [])
  M = repmat(eye(3,3),1,1,N); // identity matrix
end

// "omega" wanted but empty
if (opt == "mat_omega" & omega == []); 
  omega = zeros(3,N);
end

// "jacobian" wanted but empty
if (opt == "pos_vel_jac" & jacob == []); 
  jacob = repmat(eye(6,6),1,1,N); // identity matrix
end


endfunction