//  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 [y, ydot] = CL_interpLagrange(xref,yref,x, n,opt,nder)
// Lagrange interpolation
//
// Calling Sequence
// [y, ydot] = CL_interpLagrange(xref,yref,x [,n,opt,nder])
//
// Description
// <itemizedlist><listitem>
// <p>Interpolates using Lagrange method.</p>
// <p>Given reference abscissae <b>(xref)</b> sorted in strictly increasing order and corresponding ordinates <b>(yref)</b>, 
// the function computes interpolated ordinates for the given abscissae <b>(x)</b>.</p>
// <p></p>
// <p>The number of points to be used for the interpolation <b>(n)</b> should be an even whole number in the range [2,16].</p> 
// <p>The behavior of the function for the allowed range of values for <b>x</b> can be changed via the argument <b>opt</b>:</p>
// <p>- opt = 0 : Default mode. The allowed range for <b>x</b> is [xref(1), xref($)]</p>
// <p>- opt = 1 : Strict (= optimal) mode. The allowed range for <b>x</b> is [xref(n/2), xref($-n/2+1)]</p>
// <p></p>
// <p>The derivatives of the interpolating polynomials are computed if <b>nder</b> == 1 and if the output argument is present. </p>
// <p></p></listitem>
// <listitem>
// <p>Notes:</p>  
// <p> - The degree of the polynomial is n-1. </p>
// <p> - y = %nan for values of x that are outside the allowed range</p>
// </listitem>
// </itemizedlist>
//
// Parameters
// xref: Reference abscissae, sorted in strictly increasing order (1xN)
// yref: Corresponding ordinates (PxN)
// x: Abscissae, where to interpolate (1xM)
// y: Interpolated values (PxM)
// n: (integer, optional) Number of points for the interpolation (1x1). Must be even, >= 2 et <= 16. Default is 8. (1x1)
// opt: (integer, optional) Option for accuracy control (0 or 1). Default is 0. (1x1)
// nder: (integer, optional) Option for the computation of the derivatives (1=yes, 0=no). Default is 1. (1x1)
// y: Values of interpolating polynomials (PxN)
// ydot: Values of interpolating polynomials derivatives. [] if not computed (PxN)
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_interpLin
//
// Examples
// // Example 1: Exact interpolation 
// xref = 1:6; 
// yref = [xref; xref.^2; xref.^3]; 
// x = 0:0.5:4; 
//
// [y] = CL_interpLagrange(xref, yref, x, n=4); 
// y - [x; x.^2; x.^3]
// 
// [y] = CL_interpLagrange(xref, yref, x, n=4, opt=1); 
// y - [x; x.^2; x.^3]
//
// // Example 2: Interpolation of sin and cos
// xref = linspace(0,1,101); 
// yref = [sin(2*%pi*xref); cos(2*%pi*xref)];  
// x = linspace(0,1,1001); 
// 
// [y] = CL_interpLagrange(xref, yref, x, opt=0); 
// scf();
// plot(x, y - [sin(2*%pi*x); cos(2*%pi*x)]); 
// 
// [y] = CL_interpLagrange(xref, yref, x, opt=1); 
// scf();
// plot(x, y - [sin(2*%pi*x); cos(2*%pi*x)]); 



// ---------------------------
// Internal function 
// pre-computations that only depend on abscissae. 
// Ind, Coefs => dimension = nxN 
// (N: number of columns in x) 
// row "i" de Ind,Coefs <=> ith point for interpolation (1 to n)
// for a given vector "yref" (same size as xref), 
// the interpolated value is: 
// y = sum_I(yref_I * C_I)
// C_I = prod_K((x - xref_K)/(xref_I - xref_K), K <> I)
// I : indices of interpolation points (n values)
// I -> column of "Ind" matrix
// C_I ->  column of "Coefs" matrix
//
// interpolation points (indices I, here 1 to n) 
// are chosen so that:
// xref_1 < xref_2 < xref_n/2 <= x <= xref_n/2+1 < ... xref_n
// (except near the edges)
// 
// der = 1 => compute derivative of interpolating polynomial. 
//
// NB: no check of arguments. 
// ---------------------------
function [Ind, Coefs, Coefsd] = interpLagrange_mat(xref, x, n, nder)

  N = size(x,2); 
  Nref = size(xref,2); 

  // note: dsearch(x,X) returns k if x belongs to ]X(k), X(k+1)]
  // extension of "xref" interval to interpolate beyong the edges. 
  // => K == -1 si outside interval
  // (K row vector)
  K = dsearch(x, [xref(1) - (xref(2) - xref(1)), xref, xref(Nref) + (xref(Nref) - xref(Nref-1))]) - 1; 

  // Kmin = min indices of "points" used for interpolation
  // (K is index n/2)
  Kmin = K - n/2 + 1; 

  // the n indices (Kmin .. Kmin+n-1) are forced into [1, Nref] 
  I = find(Kmin < 1); 
  Kmin(I) = 1; 
  I = find(Kmin + n - 1 > Nref); 
  Kmin(I) = Nref - n + 1; 

  // Ind: each column <=> indices used for interpolation
  Ind = zeros(n, N); 
  for i = 1 : n
    Ind(i,:) = Kmin + i - 1; 
  end
  
  // Values of xref for indices Ind
  Xref = matrix(xref(Ind), size(Ind)); 

  // No result if abscissa is out of bounds 
  I = find(K == -1); 
  Xref(:,I) = %nan; 

  Coefs = zeros(n, N); 
  Coefsd = []; 
  if (nder == 1); Coefsd = zeros(n, N); end

  if (n == 2)

    // special case for n=2 (especially for derivative)
    for i = 1 : n
      j = n-i+1; 
      Coefs(i,:) = (x - Xref(j,:)) ./ (Xref(i,:) - Xref(j,:));
      if (nder == 1)
        Coefsd(i,:) = 1 ./ (Xref(i,:) - Xref(j,:));
      end
    end

  else

    C1 = ones(n-1, 1); 
    C2 = ones(n-2, 1); 

    for i = 1 : n
      I = [1:i-1, i+1:n]; 
      den = prod(C1 * Xref(i,:) - Xref(I,:), "r"); 
      Coefs(i,:) = prod(C1 * x - Xref(I,:), "r") ./ den;

      // derivative
      // sum {i=I} (prod {j=I,j<>i} (x-xj)) / den

      if (nder == 1)
        for j = 1 : n-1
          J = [I(1:j-1), I(j+1:n-1)]
          Coefsd(i,:) = Coefsd(i,:) + prod(C2 * x - Xref(J,:), "r");
        end
        Coefsd(i,:) = Coefsd(i,:) ./ den;
      end
    end

  end

endfunction


// ---------------------------
// MAIN 
// ---------------------------

if (~exists("n", "local")); n = 8; end 
if (~exists("opt", "local")); opt = 0; end 
if (~exists("nder", "local")); nder = 1; end 

// Temporary compatibility check
if (exists("cder","local")); CL__error("Invalid argument: cder"); end;

if (opt <> 0 & opt <> 1) 
  CL__error("Invalid value for opt"); 
end

if (n < 2 | n > 16 | modulo(n,2) <> 0) 
  CL__error("Invalid value for n"); 
end

if (nder <> 0 & nder <> 1) 
  CL__error("Invalid value for nder"); 
end

N = size(x,2); 
Nref = size(xref,2); 
P = size(yref,1); 

if (size(xref,1) <> 1 | size(yref,2) <> Nref)
  CL__error("Wrong input argument size (xref or yref)"); 
end

// default outputs
y = []; 
ydot = []; 

if (N == 0) 
  return; // <== RETURN
end

if (Nref < n)
  CL__error("Insufficient number of values in xref (should be >= n)"); 
end

if (find(xref(2:$)-xref(1:$-1) <= 0) <> [])
  CL__error("Abscissae not strictly increasing"); 
end

// Do not compute derivative if there is only one output argument
// (optimization). 
if (argn(1) == 1); nder = 0; end

// Indices and coefficients for interpolation
[Ind, Coefs, Coefsd] = interpLagrange_mat(xref, x, n, nder); 

// Interpolation for each row of yref
y = zeros(P,N); 
if (nder == 1); ydot = zeros(P,N); end 

for k = 1 : P

  // -- readable version: 
  // yrefk = yref(k,:); 
  // Yrefk = matrix(yrefk(Ind), size(Ind)); 
  // y(k,:) = sum(Yrefk .* Coefs, "r");

  // -- more compact version: 
  y(k,:) = sum(matrix(yref(k,Ind), size(Ind)) .* Coefs, "r"); 

  if (nder == 1)
    ydot(k,:) = sum(matrix(yref(k,Ind), size(Ind)) .* Coefsd, "r");
  end 

end

// depending on "opt", some results are set to %nan (invalid)
// if abscissa is out of bounds. 

// n1/n2 : min/max indices for which interpolation is OK 
// (not considering margins)

if (opt == 0)
  n1 = 1; 
  n2 = Nref;
else
  n1 = n/2; 
  n2 = Nref - n/2 + 1;
end

// eps1/eps2 : margins min/max (> 0)

m1 = max(n1,2); 
m2 = min(n2,Nref-1); 
eps1 = (xref(m1) - xref(m1-1))/100; 
eps2 = (xref(m2+1) - xref(m2))/100; 

I = find(x < xref(n1) - eps1 | x > xref(n2) + eps2); 
y(:,I) = %nan; 
if (nder == 1)
  ydot(:,I) = %nan; 
end

endfunction