//  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 [x, y, info] = CL_fsolveb(fct, x1, x2, args, ytol, dxtol, meth, y1, y2)
// Zero of function knowing bounds
// 
// Calling Sequence
// [x, y, info] = CL_fsolveb(fct, x1, x2 [, args, ytol, dxtol, meth, y1, y2]); 
//
// Description
// <itemizedlist><listitem>
// <p>Solves fct(x) = 0 by the secant method or variants. </p>
// <p>The function <b>fct</b> must have the following interface: </p>
// <p>y = <b>fct</b>(x, ind, args)</p>
// <p>where x is a vector of size (1xP), ind is a vector of indices of the same 
// size as x, and args is a variable of any type (with a constant value). 
// ind contains the indices currently evalued (corresponding to the values in x). </p>
// <p>An interval (<b>x1</b>, <b>x2</b>) containing the solution must be known (x1 
// may not be smaller than x2). </p>
// <p>The solution is found if the 2 following conditions are met (if they 
// are applicable): |y| is less than <b>ytol</b> and |dx| is less than 
// <b>dxtol</b> where y=fct(x), and dx is the change in the value of 
// x in the last iteration. If ytol or dxtol are not specified, the 
// corresponding condition is automatically met. If none of them is specified, 
// ytol is set to 0, and dxtol is not set. </p>
// <p>Variants of algorithm are possible by using the <b>meth</b> parameter: </p>
// <p>- meth = "s" : usual secant method.</p>
// <p>- meth = "d" : dichotomy.</p>
// <p>- meth = "ds" : dichotomy + secant method at each iteration. Convergence may be faster but at the price 
// of one additional function call per iteration. </p>
// <p>- meth = "s2" : variant of secant method. The algorithm makes the size of the current 
// interval shrink as much as possible at each iteration (so that it may not necessarily 
// contain the solution). This method can be faster than the other ones, 
// but the convergence is not guaranteed.  </p>
// <p>The values <b>y1</b>=fct(x1) and <b>y2</b>=fct(x2) may be given. 
// If not, they are automatically computed by the algorithm. </p>
// <p>The output argument <b>info</b> contains additional information: </p>
// <p>info = 0: OK </p>
// <p>info = 1: fct(x1) and fct(x2) have the same sign or x1 == x2 (for some indices). 
// The corresponding results are set to %nan.  </p>
// <p>info = 2: No convergence (for some indices). The corresponding results 
// are set to %nan.  </p>
// <p>info = 3: Corresponds to: info == 1 & info == 2.  </p>
// <p></p>
// </listitem>
// <listitem>
// <p><b>Notes</b> :</p>
// <p>- if x1, x2, y1, or y2 contains %nan (for some indices), the corresponding 
// outputs are set to %nan. </p>
// <p>- if fct returns %nan (for some indices), the corresponding outputs are set to %nan. </p>
// <p>- fct should be continuous and monotonous in [x1, x2]. </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// fct: (function) Function with interface: y = f(x, ind, args)
// x1, x2: Interval containing the solution. (1xN) 
// args: (optional, any type) Argument passed to "fct" function. Default is [].
// ytol: (optional) Tolerance on fct(x). (1x1)
// dxtol: (optional) Tolerance on change in x. (1x1)
// meth: (optional) Option of algorithm ("s", "d", "ds", "s2"). Default is "s".
// y1: (optional) Value of fct at x1. (1xN)
// y2: (optional) Value of fct at x2. (1xN)
// x: value such that fct(x) = 0. (1xN) 
// y: Exact value of fct at x. (1xN)
// info: (integer) Additional information. 0 means "OK".   
//
// Authors
// CNES - DCT/SB
//
// Examples
// // Example 1: Solves: sin(x) = 0.1
// function [y] = fct(x, ind, args) 
//   y = sin(x) - 0.1; 
// endfunction 
// [x, y] = CL_fsolveb(fct, -1, 1); 
//
// // Example 2: Solves: sin(x) = [0.1, 0.2, 0.3, 0.4, 0.5]
// clear fct;
// function [y] = fct(x, ind, args) 
//   y = sin(x) - args(ind); 
// endfunction 
// n = 5; 
// args = (1:n) / 10; 
// x1 = -1 * ones(1,n);
// x2 =  1 * ones(1,n);
// [x, y, info] = CL_fsolveb(fct, x1, x2, args, ytol=1.e-12); 


// -------------------------------
// Internal function - secant
// -------------------------------

// Warning: uses variables from upper level (args, fct)
// NB: x1, y1, x2, y2, x, y: same size (current size)
// K: "absolute" indices concerned (K <> [])
// xmin, xmax: not used

function [x1, y1, x2, y2, x, y] = fsolve_s(x1, y1, x2, y2, K, xmin, xmax)

  // abscissa of intersection with y=0
  x = (y2 .* x1 - y1 .* x2) ./ (y2 - y1); 
  y = fct(x, K, args); 

  // y and y2 have same sign (on same side)
  // => x2 replaced by x

  I = find(y .* y2 >= 0);
  if (I <> [])
    x2(I) = x(I); 
    y2(I) = y(I); 
  end

  // y and y1 have same sign (on same side)
  // => x1 replaced by x

  I = find(y .* y1 > 0);
  if (I <> [])
    x1(I) = x(I);
    y1(I) = y(I);
  end

endfunction


// -------------------------------
// Internal function - dichotomy
// -------------------------------

// Warning: uses variables from upper level (args, fct)
// NB: x1, y1, x2, y2, x, y: same size (current size)
// K: "absolute" indices concerned (K <> [])
// xmin, xmax: not used

function [x1, y1, x2, y2, x, y] = fsolve_d(x1, y1, x2, y2, K, xmin, xmax)

  x = (x1+x2)/2; 
  y = fct(x, K, args); 

  // y and y2 have same sign (on same side)
  // => x2 replaced by x

  I = find(y .* y2 >= 0);
  if (I <> [])
    x2(I) = x(I); 
    y2(I) = y(I); 
  end

  // y and y1 have same sign (on same side)
  // => x1 replaced by x

  I = find(y .* y1 > 0);
  if (I <> [])
    x1(I) = x(I);
    y1(I) = y(I);
  end

endfunction


// -------------------------------
// Internal function - dichotomy + secant
// -------------------------------

// Dichotomy + Secant method
// Warning: uses variables from upper level (args, fct)
// NB: x1, y1, x2, y2, x, y: same size (current size)
// K: "absolute" indices concerned (K <> [])
// xmin, xmax: not used

function [x1, y1, x2, y2, x, y] = fsolve_ds(x1, y1, x2, y2, K, xmin, xmax)

  // --- step 1: dichotomy to reduce interval

  x = (x1+x2)/2; 
  y = fct(x, K, args); 

  // y and y2 have same sign (on same side)
  // => x2 replaced by x

  I = find(y .* y2 >= 0);
  if (I <> [])
    x2(I) = x(I); 
    y2(I) = y(I); 
  end

  // y and y1 have same sign (on same side)
  // => x1 replaced by x

  I = find(y .* y1 > 0);
  if (I <> [])
    x1(I) = x(I);
    y1(I) = y(I);
  end

  // --- step 2: secant (method 0)
  // only if y <> 0 & y1 <> y2

  I = find(abs(y) <> 0 & abs(y1-y2) <> 0);

  if (I <> [])
  
    [x1(I), y1(I), x2(I), y2(I), x(I), y(I)] = .. 
       fsolve_s(x1(I), y1(I), x2(I), y2(I), K(I), xmin(I), xmax(I))

  end

endfunction


// -------------------------------
// Internal function - adapted secant
// -------------------------------

// Max reduction of interval size
// Warning: uses variables from upper level (args, fct)
// NB: x1, y1, x2, y2, x, y: same size (current size)
// K: "absolute" indices concerned (K <> [])

function [x1, y1, x2, y2, x, y] = fsolve_s2(x1, y1, x2, y2, K, xmin, xmax)

  // abscissa of intersection with y=0
  x = (y2 .* x1 - y1 .* x2) ./ (y2 - y1); 

  // force into [xmin, xmax] as may not be the case
  I = find(x <= xmin); 
  x(I) = xmin(I); 

  I = find(x >= xmax); 
  x(I) = xmax(I); 

  y = fct(x, K, args); 

  // x2 replaced by x if |x-x1| < |x-x2]
  // x1 replaced by x if |x-x2| < |x-x1]
  Cond = abs(x-x1) < abs(x-x2); 

  I = find(Cond); 
  x2(I) = x(I);
  y2(I) = y(I);

  I = find(~Cond); 
  x1(I) = x(I);
  y1(I) = y(I);

endfunction


// -------------------------------
// Internal function - adapted secant
// -------------------------------

// (x1, x2) becomes (x2, x)
// Warning: uses variables from upper level (args, fct)
// NB: x1, y1, x2, y2, x, y: same size (current size)
// K: "absolute" indices concerned (K <> [])

function [x1, y1, x2, y2, x, y] = fsolve_s3(x1, y1, x2, y2, K, xmin, xmax)

  // abscissa of intersection with 0
  x = (y2 .* x1 - y1 .* x2) ./ (y2 - y1); 

  // force into [xmin, xmax] as may not be the case
  I = find(x <= xmin); 
  x(I) = xmin(I); 

  I = find(x >= xmax); 
  x(I) = xmax(I); 

  y = fct(x, K, args); 

  x1 = x2; 
  y1 = y2; 
  x2 = x; 
  y2 = y; 

endfunction


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

if (~exists("args", "local")); args = []; end
if (~exists("ytol", "local")); ytol = []; end
if (~exists("dxtol", "local")); dxtol = []; end
if (~exists("meth", "local")); meth = "s"; end
if (~exists("y1", "local")); y1 = []; end
if (~exists("y2", "local")); y2 = []; end

if (exists("opt", "local"))
  CL__error("Invalid argument: opt"); 
end

// available methods
// NB: "s3" is voluntarily not documented and for internal use only
METH = ["s", "d", "ds", "s2", "s3"]; 
FMETH = list(fsolve_s, fsolve_d, fsolve_ds, fsolve_s2, fsolve_s3); 

imeth = find(meth == METH); 
if (imeth == []); CL__error("Invalid method"); end
fmeth = FMETH(imeth); 

// check sizes are compatible (or [])
CL__checkInputs(x1, 1, x2, 1, y1, 1, y2, 1, ytol, 1, dxtol, 1); 

if (x1 == [] | x2 == [])
  if (x1 <> x2)
    CL__error("Invalid size for ''x1'' or ''x2''"); 
  end
  x = [];
  y = [];
  info = 0; 
  return; // <== RETURN
end

// tolerance
if (ytol == []); ytol = %inf; end
if (dxtol == []); dxtol = %inf; end
I = find(isinf(ytol) & isinf(dxtol)); 
ytol(I) = 0; 

// initialize y1, y2
if (y1 == []); y1 = fct(x1, 1:length(x1), args); end
if (y2 == []); y2 = fct(x2, 1:length(x2), args); end

// initial interval stored: 
// xmin = min(x1,x2), xmax = max(x1,x2) 
xmin = x1;
xmax = x2; 
I = find(x1 > x2); 
if (I <> [])
  xmin(I) = x2(I);
  xmax(I) = x1(I); 
end

// initialize outputs
// if y1 or y2 == 0 => y = 0
info = 0; 
x = x1; 
y = y1; 
I = find(abs(y2) < abs(y1)); 
x(I) = x2(I); 
y(I) = y2(I); 

I = find(isnan(x1) | isnan(x2) | isnan(y1) | isnan(y2)); 
x(I) = %nan; 
y(I) = %nan; 

// if y1 and y2 not with opposite signs (or x1 == x2) 
// => result is %nan
I = find(y1 .* y2 > 0 | (x1 == x2 & abs(y1) > 0));
if (I <> [])
  x(I) = %nan;
  y(I) = %nan; 
  info = 1; 
end


// convergence loop
// K: indices to examine (if y == 0: finished)

K = find(abs(y) > 0); 
iter = 1; 
maxiter = 30;

while (iter <= maxiter & K <> []) 

  // store previous value of x and update
  xprev = x; 

  [x1(K), y1(K), x2(K), y2(K), x(K), y(K)] = fmeth(x1(K), y1(K), x2(K), y2(K), K, xmin(K), xmax(K)); 

  //printf("--------\n"); 
  //disp(x(K)-xprev(K)); 
  //disp(x(K)); 
  //disp(y(K)); 

  // if %nan found => set x to nan and ignore
  I = find(isnan(y(K))); 
  if (I <> [])
    x(K(I)) = %nan; 
    y(K(I)) = %nan; 
  end

  // Remaining indices (not yet converged)
  // NB: 
  // - not OK if x == xmin or x == xmax as already checked
  // - If cond_nok is true (not converged) and y1 == y2
  //   => %nan (silently)
  
  cond_nok = abs(y) > 0 & (abs(y) > ytol | abs(x-xprev) > dxtol | ..
            x == xmin | x == xmax); 
  
  // not converged but y1 == y2 => %nan
  K = find(cond_nok & y1 == y2);
  x(K) = %nan;
  y(K) = %nan; 
  
  // not converged => continue
  K = find (cond_nok & y1 <> y2); 
  
  iter = iter + 1; 
end


// check convergence (update status)
if (K <> [])
  x(K) = %nan; 
  y(K) = %nan; 
  info = info + 2; 
end


endfunction