%MINQMEX Find at most N local extrema values of the profile(AQ,QQ)
%
% This is the MATLAB equivalent implementation of libmeq/minQ.c.
% A more detailed help is available in MINQMEX.
%
% [+MEQ MatlabEQuilibrium Toolbox+] Swiss Plasma Center EPFL Lausanne 2022. All rights reserved.
function [aqmin,qmin] = minQmexm(aQ,qQ,sq,n,NF)

if nargin<5, NF=NaN; end % default
if nargin<4, n=1; end % default

ismin = sq > 0;

[aqmin,qmin] = deal(repmat(NF,n,1));

nQ =  numel(qQ);
imin = 1; % counter for number of points found

% outer point
if ( ismin && (qQ(nQ) - qQ(nQ-1)) < 0) || ...
   (~ismin && (qQ(nQ) - qQ(nQ-1) > 0))
  aqmin(imin) = aQ(nQ);
  qmin(imin) = qQ(nQ);
  if n==1; return; 
  else, imin = imin+1;
  end
end

% inner points
for kQ = (nQ-1):-1:2
  if ( ismin && (qQ(kQ+1) - qQ(kQ) > 0) && (qQ(kQ) - qQ(kQ-1) <= 0)) || ...
     (~ismin && (qQ(kQ+1) - qQ(kQ) < 0) && (qQ(kQ) - qQ(kQ-1) >= 0)) % derivative sign change
    % quadratic fit
    x32 = aQ(kQ+1) - aQ(kQ);
    x12 = aQ(kQ-1) - aQ(kQ);
    y32 = qQ(kQ+1) - qQ(kQ);
    y12 = qQ(kQ-1) - qQ(kQ);
    
    % lazy mode:
    %       A = [x32.*x32 x32; 
    %            x12.*x12 x12];
    %       b = [y32;y12];
    %       p=A\b; p1=p(1);p2=p(2);
    
    % fast way, explicit inverse of 2x2 matrix:
    iD = 1./(x32*x12*(x32-x12));
    p1 = ( x12*y32     - x32*y12     );
    p2 = ( x32*x32*y12 - x12*x12*y32 );
    
    % (y-y0) = (p(1)(x-x0)^2 + p(2)(x-x0))*iD
    % with x0=aQ(kQ), y(0)=qQ(kQ);
    
    xmin = -p2/(2*p1);
    ymin = xmin*p2*iD/2;
    
    aqmin(imin) = aQ(kQ)+xmin;
    qmin(imin)  = qQ(kQ)+ymin;
    
    imin = imin+1;
    if imin>n, return; end % maximum number of extremum points fount
  end
end

% if still counting, then first point is a minimum
qmin(imin) = qQ(1);
aqmin(imin) = aQ(1);

end
