//  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 [xres, yres, indres] = CL__detectLocExtr(x, y, fct, args, dytol, n)
// Local extrema for zero detection (only!) 
//
// Calling Sequence
// [xres, yres, indres] = CL__detectLocExtr(x, y, fct, args, dytol, n) 
//
// Description
// <itemizedlist><listitem>
// <p>Local extrema of <b>fct</b> are computed using second order polynomial interpolation.</p>
// <p>The interface of <b>fct</b> must be y = fct(x, ind, args). ind is unused ([]).</p>
// <p>x and y must be 1xN and not empty. x should contain increasing values (not checked)</p>
// <p><b>n</b>: number of points for computation of local extrema (>= 3). </p>
// <p>IMPORTANT: To be used for zero detection only (not all local extrema detected, 
// accuracy is optimized). </p>
// </listitem></itemizedlist>
//
// Parameters 
// x: Abscissas (1xN)
// y: Ordinates (1xN)
// fct: (function) Function with interface: y = f(x, ind, args) 
// args: (any type) Argument passed to "fct" function. 
// dytol: Tolerance on variation of y. (1x1)
// n: (integer) Number of points used to discretize the intervals containing the extrema. (1x1)
// xres: Abscissae of local extrema (same order as in x). (1xP)
// yres: Ordinates of local extrema. (1xP)
// indres: indices in initial arrays where data can be replaced. (1xP)
//

// ----------------------------------------------
// Auxiliary functions
// ----------------------------------------------

// -----------------------------------------------
// Evaluates local maximum in each interval
// interv: intervals where local extrema are located (2xN)
// intent: 1xN: 1 => look for max, -1 => look for min
// fct: called by: fct(x, [], args)
// n: number of points for computation of maximum
// xres: Abscissae of local extrema. (1xN)
// yres: fct(xres). (1xN)
// NB: 
// - n should be >= 3
// - only one value returned per input interval
// -----------------------------------------------
function [interv, xres, yres] = detectLocExtr_eval(interv, intent, fct, args, n)
  
  N = size(interv, 2); 
    
  // discretize intervals containing local extrema (n pts)
  // X and Y: size = n x N
  X = CL_intervLinspace(interv, n); 
  x = matrix(X, 1, -1); 
  y = fct(x, [], args); 
  Y = matrix(y, n, -1); 
  
  // computes maxima or minima of Y in each column
  // forces indices in [2, n-1]
  ind = zeros(intent); 
  I = find(intent > 0); 
  if (I <> []); [temp, ind(I)] = max(Y(:,I), "r"); end
  I = find(intent < 0); 
  if (I <> []); [temp, ind(I)] = min(Y(:,I), "r"); end
  
  ind = max(2, min(n-1, ind)); 
  
  K1 = sub2ind([n,N], ind-1, 1:N); 
  K2 = sub2ind([n,N], ind, 1:N); 
  K3 = sub2ind([n,N], ind+1, 1:N); 

  // intervals of abscissas containing the local extrema
  interv = [x(K1); x(K3)];
  
  // p(x) = c + bx + ax^2
  // [coefs(1,:); coefs(2,:); coefs(3,:)] = [c; b; a]
  // note abscissas are offset by -x(K1) for better numerical results
  p = CL_lagrangeFitPoly([zeros(K1); x(K2)-x(K1); x(K3)-x(K1)], [y(K1); y(K2); y(K3)]); 
  coefs = matrix(coeff(p')', 3, -1); // 3xN <=> [coefs deg0; coefs deg1; coefs deg2]

  // final solution
  xres = zeros(1:N); 
  
  // case 1-a: polynomial of deg 1 or convex 
  I = find(coefs(3,:) >= 0 & y(K1) >= y(K3)); 
  xres(I) = x(K1(I));

  // case 1-b: polynomial of deg 1 or convex
  I = find(coefs(3,:) >= 0 & y(K1) < y(K3)); 
  xres(I) = x(K3(I));

  // case 2: polynomial of deg 2 : "-b / 2a"
  // extremum <=> -coef1 / (2*coef2)
  
  // NB: should be consistent with intent 
  // intent == 1 => concave, intent == -1 => convex => intent * coef2 <= 0
  if (find(intent .* coefs(3,:) > 0))
    CL__error("Anomaly in extremum computation"); 
  end
    
  I = find(coefs(3,:) <> 0); 
  if (I <> [])
    // force xres in [x(K1); x(K3)]
    xres(I) = min(x(K3(I)), max(x(K1(I)), x(K1(I)) - coefs(2, I) ./ (2 * coefs(3, I)))); 
  end
 
  yres = fct(xres, [], args); 

endfunction

// -----------------------------------------------
// (Iterative) search of local extrema
// interv: 2xN
// intent: 1xN: 1 => look for max, -1 => look for min
// -----------------------------------------------
function [xres, yres] = detectLocExtr_iterate(interv, intent, fct, args, dytol, n)
  MAXITER = 20;
  interv = interv; // local variable
  intent = intent; // local variable

  // K: set of indices not yet "converged"
  K = 1 : size(interv,2);  
  xres = %nan * ones(K); 
  yres = %nan * ones(K); 
  yres_prev = %inf * ones(K);
  
  iter = 1;
 
  while (K <> [] & iter <= MAXITER)
    [interv(:,K), xres(K), yres(K)] = detectLocExtr_eval(interv(:, K), intent(K), fct, args, n);
	  delta = yres - yres_prev; 
	
    K  = find(abs(delta) > dytol & ((intent > 0 & yres < 0) | (intent < 0 & yres > 0)));
	
    yres_prev = yres;  
    iter = iter + 1;
  end
  
  if (iter > MAXITER)
    CL__error("Too many iterations"); 
  end  

endfunction


// ----------------------------------------------
// Main Code
// ----------------------------------------------
xres = [];
yres = [];
indres = []; 

if (argn(2) <> 6)
  CL__error("Invalid number of input arguments (6 expected)"); 
end

// limited number of checks
if (n <> round(n) & n < 3)
  CL__error("Invalid value for argument n"); 
end

N = size(y, 2); 

// No local extrema
if (N < 3)
  return; // <= RETURN
end

// Selection of local extrema 
// If found: it is between indices I and I+2
// Select: local maxima if max < 0 or local minima if min > 0 

// 3 successive y not identical
// local min with y_middle > 0
// local max with y_middle < 0
cond_neq = (y(2:$-1) <> y(1:$-2) | y(2:$-1) <> y(3:$)); 
cond_min = (y(2:$-1) <= y(1:$-2) & y(2:$-1) <= y(3:$) & y(2:$-1) > 0) & cond_neq; 
cond_max = (y(2:$-1) >= y(1:$-2) & y(2:$-1) >= y(3:$) & y(2:$-1) < 0) & cond_neq; 
cond_all = cond_min | cond_max; 

// removes successive intervals (=> would normally lead to the same solution)
// case: y = 1 2 2 followed by: 2 2 1  
I = find(cond_min(1:$-1) & cond_min(2:$)); 
cond_min(I) = %f; 
I = find(cond_max(1:$-1) & cond_max(2:$)); 
cond_max(I) = %f; 

intent = zeros(cond_all); 
intent(cond_min) = -1; // means: look for minimum until converged or y <= 0
intent(cond_max) =  1; // means: look for maximum until converged or y >= 0

// NB: Extremum between I and I+2
I = find(cond_all); 
intent = intent(I); 

if (I <> [])
  [xres, yres] = detectLocExtr_iterate([x(I); x(I+2)], intent, fct, args, dytol, n);
  indres = I+1; 
end

if (indres <> [])
  // only select the abscissae that are not already in x
  I = find(xres > x(indres-1) & xres < x(indres+1) & xres <> x(indres)); 
  xres = xres(I); 
  yres = yres(I); 
  indres = indres(I); 
end
  
endfunction