//  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_integrate(x,ydotn, yini,meth,n)
// Integration of a function defined by a data series
//
// Calling Sequence
// [y,ydot] = CL_integrate(x,ydotn [,yini, meth, n])
//
// Description
// <itemizedlist><listitem>
// <p>Integrates a function of one variable defined by a data series.</p>
// <p></p>
// <p>The nth integral is evaluated by using derivatives (<b>ydotn</b>) and initial conditions (<b>yini</b>). </p>
// <p>- For a simple integration (<b>n</b>=1), ydotn is the first derivative of y at abscissae x, and yini gives the 
// value of y at first abscissa. The function returns the values of y at abscissae x. </p>
// <p>- For a double integration (<b>n</b>=2), ydotn is the second derivative of y at abscissae x, and yini gives the 
// value of y and ydot at first abscissa. The function returns the values of y and its first derivative ydot 
// at abscissae x. </p>
// <p></p>
// <p>The methods used to integrate are:</p>
// <p></p> 
// <p>- <b>step_l</b> (l as lower bound): The result is the integral of a step function 
// (constant between x(k) and x(k+1)). The constant value between x(k) and x(k+1) is ydotn(k).</p>
// <inlinemediaobject><imageobject><imagedata fileref="integrate_step_l.gif"/></imageobject></inlinemediaobject>
// <p></p> 
// <p>- <b>step_m</b> (m as middle): The result is the integral of  step function (constant between x(k) and x(k+1)). 
// The constant value between x(k) and x(k+1) is (ydotn(k)+ydotn(k+1)/2. </p>
// <inlinemediaobject><imageobject><imagedata fileref="integrate_step_m.gif"/></imageobject></inlinemediaobject>
// <p></p> 
// <p>- <b>lin</b> (linear): The result is the integral of a linear function on each interval (polynomial (P) 
// of degree 1). P(xk)=ydotn(k) and P(xk+1)=ydotn(k+1). </p>
// <inlinemediaobject><imageobject><imagedata fileref="integrate_lin.gif"/></imageobject></inlinemediaobject>
// <p></p> 
// <p>- <b>sl4</b> : Simpson (1 intermediate point) + Lagrange interpolation using 4 points. 
// %nan is returned if there are not enough input data. </p>
// <p></p> 
// <p>- <b>sspl</b> : Simpson (1 intermediate point) + interpolation using splines. </p>
// <p></p> 
// <p>- <b>bl6</b> : Boole (3 intermediate points) + Lagrange interpolation using 6 points. 
// %nan is returned if there are not enough input data. </p>
// <p></p></listitem>
// </itemizedlist>
//
// Parameters
// x: Abscissae (in strictly increasing order) (1xN or PxN)
// ydotn: Values of nth derivative at abscissae x (1xN or PxN)
// yini: (optional) Initial conditions for y (y0 or [y0, ydot0]). Default is 0. (1xn or Pxn)
// meth: (string, optional) Integration method: "step_l", "step_m", "lin", "sl4", "sspl", "bl6". Default is "lin". (1x1)
// n: (integer, optional) Integration order (= number of successive integrations): 1 or 2. Default is 1. (1x1)
// y: Integrated values (after n successive integrations) at abscissae x (PxN)
// ydot: First derivatives of y at abscissae x ([] if n == 1) (PxM)
//
// Authors
// CNES - DCT/SB
//
// Examples
// // Example 1: Simple integration 
// x = linspace(0,2*%pi,100);
// ydot = cos(x);
// yini = 0;
// y1 = CL_integrate(x, ydot, yini, meth="lin");
// disp(max(abs(y1-sin(x)))); 
// y2 = CL_integrate(x, ydot, yini, meth="sl4");
// disp(max(abs(y2-sin(x)))); 
// y3 = CL_integrate(x, ydot, yini, meth="sspl");
// disp(max(abs(y3-sin(x)))); 
// y4 = CL_integrate(x, ydot, yini, meth="bl6");
// disp(max(abs(y4-sin(x)))); 
//
// // Example 2: Double integration (step method)
// x = linspace(0,2*%pi,100);
// ydot2 = cos(x);
// yini = [1, 2];
// [y, ydot] = CL_integrate(x, ydot2, yini, meth="step_m", n=2);
// ydotref = sin(x) + 2; 
// disp(max(abs(ydot-ydotref))); 
// yref = -cos(x) + 2*x + 2; 
// disp(max(abs(y-yref))); 


// Integration method (example for order 2): 
// we have x_0, x_1, ... x_N : abscissae
// and:    D_1, D_2, D_N : constant values on each interval
// (<=> second derivative = constant)
//
// we compute : I(k-1,k) = integral(k-1, k), k = 1..N
// I(k-1,k) = D_k * (x_k - x_k-1) = ydot_k - ydot_k-1, k=1..N
// => ydot_1 = ydot_0 + I(0,1)
// => ydot_2 = ydot_1 + I(1,2) = ydot_0 + I(0,1) + I(1,2) 
// => ... 
//
// Second integral: 
// I2(k-1,k) = 1/2 * D_k * (x_k - x_k-1)^2 + 
//              ydot_k-1 * (x_k - x_k-1) 
//           = y_k - y_k-1, k=1..N
// => y_k = ... , k=1..N, knowing y_0


// Single integration of constant values on intervals
// x = abscissae (PxN)
// D = constant values of first derivative on each interval. (Px(N-1))
//            D1       D2       D3      DN-1
//        |--------|--------|--------|--------|
//       x1       x2       x3       x4        xN
// yini = Initial value of y at first abscissa (Px1)
function y = evalInt1(x,D,yini)
  dx = x(:,2:$) - x(:,1:$-1);  // dim N-1
  dy = D .* dx;                // dim N-1
  y = cumsum([yini, dy],"c");  // dim N
endfunction
  
// Double integration of constant values on intervals
// x = abscissae (PxN)
// D = constant values of second derivative on each interval (Px(N-1))
// yini = Initial value of y at first abscissa (Px1)
// ydotini = Initial value of first derivative of y at first abscissa (Px1)
function [y,ydot] = evalInt2(x,D,yini,ydotini)
  dx = x(:,2:$) - x(:,1:$-1);                 // dim N-1
  dydot = D .* dx;                            // dim N-1
  ydot = cumsum([ydotini, dydot],"c");        // dim N
  dy = 0.5 * D .* dx.^2 + ydot(:,1:$-1) .* dx;  // dim N-1
  y = cumsum([yini, dy],"c");                 // dim N
endfunction
  
// Triple integration of constant values on intervals
// x = abscissae (PxN)
// D = constant values of 3rd derivative on each interval (Px(N-1))
// yini = Initial value of y at first abscissa (Px1)
// ydotini = Initial value of first derivative of y at first abscissa (Px1)
// ydot2ini = Initial value of second derivative of y at first abscissa (Px1)
function [y,ydot,ydot2] = evalInt3(x,D,yini,ydotini,ydot2ini)
  dx = x(:,2:$) - x(:,1:$-1);                       // dim N-1
  dydot2 = D .* dx;                                 // dim N-1
  ydot2 = cumsum([ydot2ini, dydot2],"c");           // dim N
  dydot = 0.5 * D .* dx.^2 + ydot2(:,1:$-1) .* dx;    // dim N-1
  ydot = cumsum([ydotini, dydot],"c");              // dim N
  dy = (1/6) * D .* dx.^3 + 0.5 * ydot2(:,1:$-1) .* dx.^2 + ydot(:,1:$-1) .* dx;  // dim N-1 
  y = cumsum([yini, dy],"c");                       // dim N
endfunction

// Simpson (1 intermediate point = midpoint) 
// + lagrange interpolation on 4 points (degree 3)
// => %nan if less than 4 points
// x and y: same size
function [y] = evalInt_sl4(x, ydot, yini)
  if (size(x,2) < 4)
    y = %nan * ones(ydot); 
    return; // <= RETURN
  end

  // step, dim = N-1
  dx = x(:,2:$) - x(:,1:$-1);   
  // mid points abscissae, dim = N-1
  xm = x(:,1:$-1) + dx/2; 
  // mid points ordinates, dim = N-1
  // interpolation for each line as x may not be the same
  ydotm = zeros(xm); 
  for i = 1 : size(ydotm,1)
    ydotm(i,:) = CL_interpLagrange(x(i,:), ydot(i,:), xm(i,:), n=4);
  end

  y = cumsum([yini, dx .* (ydot(:,1:$-1) + ydot(:,2:$) + 4 * ydotm)/6], "c"); 
endfunction

// Simpson (1 intermediate point = midpoint) 
// + spline interpolation 
// x and y: same size
function [y] = evalInt_sspl(x, ydot, yini)
  // step, dim = N-1
  dx = x(:,2:$) - x(:,1:$-1);   
  // mid points abscissae, dim = N-1
  xm = x(:,1:$-1) + dx/2; 
  // mid points ordinates, dim = N-1
  // interpolation for each line as x may not be the same
  ydotm = zeros(xm); 
  for i = 1 : size(ydotm,1)
    S = splin(x(i,:), ydot(i,:));  
    ydotm(i,:) = interp(xm(i,:), x(i,:), ydot(i,:), S);
  end

  y = cumsum([yini, dx .* (ydot(:,1:$-1) + ydot(:,2:$) + 4 * ydotm)/6], "c"); 
endfunction
  
// Boole (3 intermediate points = 1/4, 1/2, 3/4) 
// + lagrange interpolation on 6 points (degree 5)
// => %nan if less than 6 points
// x and y: same size
function [y] = evalInt_bl6(x, ydot, yini)
  if (size(x,2) < 6)
    y = %nan * ones(ydot); 
    return; // <= RETURN
  end

  // step, dim = N-1
  dx = x(:,2:$) - x(:,1:$-1);   
  // intermediate points abscissae
  x1 = x(:,1:$-1); 
  xk = [x1 + dx/4, x1 + dx/2, x1 + 3*dx/4]; 
  // interpolation for each line as x may not be the same
  ydotk = zeros(xk); 
  for i = 1 : size(ydotk,1)
    ydotk(i,:) = CL_interpLagrange(x(i,:), ydot(i,:), xk(i,:), n=6);
  end

  m = size(x1,2); 
  y = cumsum([yini, dx .* (7*ydot(:,1:$-1) + 32*ydotk(:,1:m) + 12*ydotk(:,m+1:2*m) + .. 
                           32*ydotk(:,2*m+1:3*m) + 7*ydot(:,2:$))/90], "c"); 
endfunction

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

if (~exists("n", "local")); n = 1; end
if (n <> 1 & n <> 2); CL__error("Wrong value for n"); end
if (~exists("yini", "local")); yini = zeros(1,n); end
if (~exists("meth", "local")); meth = "lin"; end
 

P = max(size(x,1), size(ydotn,1), size(yini,1));
 
// Check and resize arguments if necessary
[x, ydotn, N] = CL__checkInputs(x,[1,P], ydotn,[1,P]);
if (size(x,1) == 1); x = ones(P,1) * x; end
if (size(ydotn,1) == 1); ydotn = ones(P,1) * ydotn; end 
if (size(yini,1) == 1); yini = ones(P,1) * yini; end 

// Check for other possible errors
if (size(yini,1) <> P | size(yini,2) <> n); CL__error("Wrong size for yini"); end
if (typeof(meth) <> "string"); CL__error("Wrong type for meth. String expected"); end;
if (size(meth,"*") <> 1); CL__error("Wrong type for meth. String expected"); end;

// Check that abscissae are in strictly increasing order 
if (find(x(:,2:$) - x(:,1:$-1) <= 0))
  error("Abscissae not in strictly increasing order");
end

ydot = [];

// Special cases (N=0 or N=1)
if (N == 0)
  y = [];
  ydot = [];
  return;  // <-- RETURN !
end

if (N == 1)
  y = yini(:,1);
  if (n==2)
    ydot = yini(:,2);
  end
  return;  // <-- RETURN !
end


if (meth == "step_m" | meth == "step_l")
  if (meth == "step_l")
    // The derivative is constant in each interval.
    // The constant is the beginning values.
    D = ydotn(:,1:$-1);
  elseif (meth == "step_m")
    // The derivative is constant in each interval.
    // The constant is the mean of beginning/end values.
    D = (ydotn(:,2:$) + ydotn(:,1:$-1))/2;
  end
    
  if (n == 1)
    y = evalInt1(x, D, yini);
  elseif (n == 2)
    [y, ydot] = evalInt2(x, D, yini(:,1), yini(:,2));
  end
    
elseif (meth == "lin")
  // The integrated function is linear (a*x+b) in each interval.
  // And we integrate n+1 times.

  D = (ydotn(:,2:$) - ydotn(:,1:$-1)) ./ (x(:,2:$) - x(:,1:$-1));
    
  if (n == 1)
    y = evalInt2(x, D, yini(:,1), ydotn(:,1));
  elseif (n == 2)
    [y, ydot] = evalInt3(x, D, yini(:,1), yini(:,2), ydotn(:,1));
  end
  
elseif (meth == "sl4")
  // Simpson + Lagrange interpolation (4 points)
  if (n == 1)
    y = evalInt_sl4(x, ydotn, yini(:,1)); 
  elseif (n == 2) 
    ydot = evalInt_sl4(x, ydotn, yini(:,2)); 
    y = evalInt_sl4(x, ydot, yini(:,1)); 
  end 

elseif (meth == "sspl")
  // Simpson + spline
  if (n == 1)
    y = evalInt_sspl(x, ydotn, yini(:,1)); 
  elseif (n == 2) 
    ydot = evalInt_sspl(x, ydotn, yini(:,2)); 
    y = evalInt_sspl(x, ydot, yini(:,1)); 
  end 

elseif (meth == "bl6")
  // Boole + Lagrange interpolation (6 points)
  if (n == 1)
    y = evalInt_bl6(x, ydotn, yini(:,1)); 
  elseif (n == 2) 
    ydot = evalInt_bl6(x, ydotn, yini(:,2)); 
    y = evalInt_bl6(x, ydot, yini(:,1)); 
  end 

else
  CL__error("Unknown integration method");

end


endfunction