//  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 angribution of free software.  You can  use,
//  modify and/ or reangribute 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 [posr,incid] = CL_gm_reflectionPtSph(pos1,pos2,srad,pos2_inf);
// Reflection point on a sphere
// 
// Calling Sequence
// [posr,incid] = CL_gm_reflectionPtSph(pos1,pos2,srad,pos2_inf);
//
// Description
// <itemizedlist><listitem>
// <p>Computes the (specular) reflection point on a sphere whose
// centre is the origin of the reference frame. </p>
// <p> Let P1 and P2 be 2 positions outside the sphere. </p>
// <p> The reflection point Pr is defined by identical
// incident and reflected angles: theta1 = theta2, </p>
// <p> where: theta1 = angle(P1-Pr, Pr) </p>
// <p> and: theta2 = angle(P2-Pr, Pr) </p>
// <p> Note that Pr is the same as Pr-[0;0;0] because 
// the centre of the sphere is assumed to be the origin of 
// the reference frame. </p>
// <p></p>
// <p> If P2 is considered to be "at infinity" (using optional pos2_inf), then
// theta2 is defined by: </p>
// <p> theta2 = angle(P2, Pr) </p>
// <p> which means that P2 is considered as the direction 
// of object 2 rather than its position. </p>
// <p><inlinemediaobject><imageobject><imagedata fileref="reflection_pt_sph.gif"/></imageobject></inlinemediaobject></p>
// </listitem>
// <listitem>
// <p><b>Note</b> :</p>
// <p> The incidence of the reflexion points can have any value in the range [0, pi]. </p>
// <p> If only points with incidence less than %pi/2 degrees are desired, it is necessary to filter out the results like this : </p>
// <p> [posr,incid] = CL_gm_reflectionPtSph(...); I = find(incid > %pi/2); posr(:,I) = %nan;  (or posr(:,I) = []) </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// pos1: Position of object 1 in cartesian coordinates (3xN or 3x1) 
// pos2: Position of object 2 in cartesian coordinates (relative to the same frame as object 1) (3xN or 3x1)
// srad: Sphere radius (1xN or 1x1)
// pos2_inf: (optional, boolean) Computation option: %t if object 2 is considered to be at infinity, %f otherwise. Default is %f
// posr: Position of reflection point (3xN)
// incid: Incidence at reflection point. NB: can be more than 90 degrees (1xN)
//
// Authors
// CNES - DCT/SB
//
// Examples
// alpha = linspace(-%pi/2, %pi/2, 11); 
// pos1 = 2 * [1;0;0]; 
// pos2 = 2 * [cos(alpha); sin(alpha); zeros(alpha)]; 
// [posr, incid] = CL_gm_reflectionPtSph(pos1, pos2, 1); 
// theta1 = CL_vectAngle(pos1*ones(alpha)-posr, posr);
// theta2 = CL_vectAngle(pos2-posr, posr);
// theta2 - theta1 // should be 0 
// theta1 - incid  // should be 0 


// ---------------------------------------------------
// internal function 
// same interface as main function
// ---------------------------------------------------
function [posr,incid] = reflection_point(pos1,pos2,srad,pos2_inf)
  
  // ---------------------------------------------------
  // y = difference of incidence angles at reflection point
  // alpha1 = (signed) centre angle = (reflection point, pos1)  
  // Note: "inc1 = %pi/2 - atan(cos(abs(alpha1))-q1, sin(abs(alpha1)))"
  // is identical to: "inc1 = CL_gm_visiParams(1, q1, 'cen', abs(alpha1), ['incid'])"

  function [y] = fct1(alpha1, I, args)
    inc1 = %pi/2 - atan(cos(abs(alpha1))-args.q1(I), sin(abs(alpha1))); 
    alpha2 = args.ang(I) - alpha1;  
    inc2 = %pi/2 - atan(cos(abs(alpha2))-args.q2(I), sin(abs(alpha2))); 
    y = inc1 .* sign(alpha1) - inc2 .* sign(alpha2); 
  endfunction

  // ---------------------------------------------------
  // y = difference of incidence angles at reflection point
  // alpha1 = (signed) centre angle = (reflection point, pos1) 
  // case : pos2 at infinity

  function [y] = fct2(alpha1, I, args)
    inc1 = %pi/2 - atan(cos(abs(alpha1))-args.q1(I), sin(abs(alpha1))); 
    alpha2 = args.ang(I) - alpha1;  // alpha2 == inc2
    y = inc1 .* sign(alpha1) - alpha2; 
  endfunction

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

  // initialization of geometrical data
  args = struct();
  args.ang = CL_vectAngle(pos1, pos2);
  args.q1 = srad ./ CL_norm(pos1); // supposed <= 1 
  args.q2 = srad ./ CL_norm(pos2); 

  // preparation for CL_fsolveb
  if (pos2_inf == %t)
    fct = fct2; 
  else 
    fct = fct1; 
  end
  tol = 1.e-10; // tolerance on fct(x); 

  //[alpha1] = Fsolve(zeros(ang), ang, fct, tol); 
  alpha1 = CL_fsolveb(fct, zeros(args.ang), args.ang, args, ytol=tol, meth="ds");

  // u1: // pos1
  // v1: perpendicular to u1 and towards pos2
  u1 = CL_unitVector(pos1);
  v1 = CL_unitVector(CL_cross(CL_cross(pos1, pos2), pos1));

  // v1 = %nan if pos1 and pos2 are aligned and in opposite 
  // directions (=> posr = nan) 
  // v1 = 0 if pos1 and pos2 are aligned and in same direction
  // (valid case) => posr = srad * u1  
  I = find(isnan(CL_dot(v1)) & CL_dot(pos1,pos2) >= 0); 
  v1(:,I) = 0; 

  posr = CL_dMult(srad .* cos(alpha1), u1) + ..
         CL_dMult(srad .* sin(alpha1), v1);  

  incid = CL_gm_visiParams(1, args.q1, 'cen', alpha1, ['incid']);
  
endfunction


// ---------------------------------------------------
// Main 
// ---------------------------------------------------

if (~exists("pos2_inf", "local")); pos2_inf = %f; end

s = [ size(pos1) ; size(pos2) ];
smin = min(s, "r"); // min nb of rows, min nb of columns
smax = max(s, "r"); // max nb of rows, max nb of columns
N = smax(2); // max number of columns
  
// check rows (must be 3)
if (smin(1) <> 3 | smax(1) <> 3) 
  CL__error("Invalid argument sizes (number of rows)");
end

// check columns (must be 1 or N)
I = find(s(:,2) <> 1 & s(:,2) <> N);
if (I <> [])
  CL__error("Invalid argument sizes (number of columns)");
end

// size of srad (no need to resize) 
if (size(srad,1) <> 1 | (size(srad,2) <> 1 & size(srad,2) <> N))
  CL__error("Invalid argument size (srad)");
end

if (find(srad <= 0))
  CL__error("Invalid sphere radius"); 
end

if (s(1,2) < N); pos1 = pos1 * ones(1,N); end 
if (s(2,2) < N); pos2 = pos2 * ones(1,N); end

if (  find(CL_norm(pos1) < srad) <> [] | ..
     (find(CL_norm(pos2) < srad) <> [] & pos2_inf == %f) | ..
     (find(CL_norm(pos2) <= 0)   <> [] & pos2_inf == %t) )
  CL__error("Invalid objects positions"); 
end

[posr, incid] = reflection_point(pos1, pos2, srad, pos2_inf); 

endfunction