/* ======================================================================== *\
!
! *
! * This file is part of MARS, the MAGIC Analysis and Reconstruction
! * Software. It is distributed to you in the hope that it can be a useful
! * and timesaving tool in analyzing Data of imaging Cerenkov telescopes.
! * It is distributed WITHOUT ANY WARRANTY.
! *
! * Permission to use, copy, modify and distribute this software and its
! * documentation for any purpose is hereby granted without fee,
! * provided that the above copyright notice appear in all copies and
! * that both that copyright notice and this permission notice appear
! * in supporting documentation. It is provided "as is" without express
! * or implied warranty.
! *
!
!   Author(s): Thomas Bretz <mailto:tbretz@astro.uni-wuerzbrug.de>
!   Author(s): Markus Gaug 09/2004 <mailto:markus@ifae.es>
!
!   Copyright: MAGIC Software Development, 2002-2006
!
!
\* ======================================================================== */

//////////////////////////////////////////////////////////////////////////////
//
//   MExtralgoSpline
//
//   Fast Spline extractor using a cubic spline algorithm, adapted from 
//   Numerical Recipes in C++, 2nd edition, pp. 116-119.
//   
//   The coefficients "ya" are here denoted as "fVal" corresponding to
//   the FADC value subtracted by the clock-noise corrected pedestal.
//
//   The coefficients "y2a" get immediately divided 6. and are called here 
//   fDer2 although they are now not exactly the second derivative
//   coefficients any more.
// 
//   The calculation of the cubic-spline interpolated value "y" on a point 
//   "x" along the FADC-slices axis becomes: EvalAt(x)
//
//   The coefficients fDer2 are calculated with the simplified
//   algorithm in InitDerivatives.
//
//   This algorithm takes advantage of the fact that the x-values are all
//   separated by exactly 1 which simplifies the Numerical Recipes algorithm.
//   (Note that the variables fDer are not real first derivative coefficients.)
//
//////////////////////////////////////////////////////////////////////////////
#include "MExtralgoSpline.h"

#include <TRandom.h>

#include "../mbase/MMath.h"

using namespace std;

// --------------------------------------------------------------------------
//
// Calculate the first and second derivative for the splie.
//
// The coefficients are calculated such that
//   1) fVal[i] = Eval(i, 0)
//   2) Eval(i-1, 1)==Eval(i, 0)
//
// In other words: The values with the index i describe the spline
// between fVal[i] and fVal[i+1]
//
void MExtralgoSpline::InitDerivatives() const
{
    if (fNum<2)
        return;

    fDer1[0] = 0.;
    fDer2[0] = 0.;

    for (Int_t i=1; i<fNum-1; i++)
    {
        const Float_t pp = fDer2[i-1] + 4.;

        fDer2[i] = -1.0/pp;

        const Float_t d1 = fVal[i+1] - 2*fVal[i] + fVal[i-1];
        fDer1[i] = (6.0*d1-fDer1[i-1])/pp;
    }

    fDer2[fNum-1] = 0.;

    for (Int_t k=fNum-2; k>=0; k--)
        fDer2[k] = fDer2[k]*fDer2[k+1] + fDer1[k];

    for (Int_t k=fNum-2; k>=0; k--)
        fDer2[k] /= 6.;
}

// --------------------------------------------------------------------------
//
// Returns the highest x value in [min;max[ at which the spline in
// the bin i is equal to y
//
// min and max are defined to be [0;1]
//
// The default for min is 0, the default for max is 1
// The defaule for y is 0
//
Double_t MExtralgoSpline::FindY(Int_t i, Double_t y, Double_t min, Double_t max) const
{
    // y = a*x^3 + b*x^2 + c*x + d'
    // 0 = a*x^3 + b*x^2 + c*x + d' - y

    // Calculate coefficients
    const Double_t a = fDer2[i+1]-fDer2[i];
    const Double_t b = 3*fDer2[i];
    const Double_t c = fVal[i+1]-fVal[i] -2*fDer2[i]-fDer2[i+1];
    const Double_t d = fVal[i] - y;

    Double_t x1, x2, x3;
    const Int_t rc = MMath::SolvePol3(a, b, c, d, x1, x2, x3);

    Double_t x = -1;
    if (rc>0 && x1>=min && x1<max && x1>x)
        x = x1;
    if (rc>1 && x2>=min && x2<max && x2>x)
        x = x2;
    if (rc>2 && x3>=min && x3<max && x3>x)
        x = x3;

    return x<0 ? -2 : x+i;
}

// --------------------------------------------------------------------------
//
// Search analytically downward for the value y of the spline, starting
// at x, until x==0. If y is not found -2 is returned.
//
Double_t MExtralgoSpline::SearchY(Float_t x, Float_t y) const
{
    if (x>=fNum-1)
        x = fNum-1.0001;

    Int_t i = TMath::FloorNint(x);
    Double_t rc = FindY(i, y, 0, x-i);
    while (--i>=0 && rc<0)
        rc = FindY(i, y);

    return rc;
}

Double_t MExtralgoSpline::SearchYup(Float_t x, Float_t y) const
{
    if (x<0)
        x = 0.0001;

    Int_t i = TMath::FloorNint(x);
    Double_t rc = FindY(i, y, x-i, 1.);
    while (i++<fNum-1 && rc<0)
        rc = FindY(i, y);

    return rc;
}

// --------------------------------------------------------------------------
//
// Do a range check an then calculate the integral from start-fRiseTime
// to start+fFallTime. An extrapolation of 0.5 slices is allowed.
//
Float_t MExtralgoSpline::CalcIntegral(Float_t pos) const
{
/*
    // The number of steps is calculated directly from the integration
    // window. This is the only way to ensure we are not dealing with
    // numerical rounding uncertanties, because we always get the same
    // value under the same conditions -- it might still be different on
    // other machines!
    const Float_t start = pos-fRiseTime;
    const Float_t step  = 0.2;
    const Float_t width = fRiseTime+fFallTime;
    const Float_t max   = fNum-1 - (width+step);
    const Int_t   num   = TMath::Nint(width/step);

    // The order is important. In some cases (limlo-/limup-check) it can
    // happen that max<0. In this case we start at 0
    if (start > max)
        start = max;
    if (start < 0)
        start = 0;

    start += step/2;

    Double_t sum = 0.;
    for (Int_t i=0; i<num; i++)
    {
        // Note: if x is close to one integer number (= a FADC sample)
        // we get the same result by using that sample as klo, and the
        // next one as khi, or using the sample as khi and the previous
        // one as klo (the spline is of course continuous). So we do not
        // expect problems from rounding issues in the argument of
        // Floor() above (we have noticed differences in roundings
        // depending on the compilation options).

        sum += EvalAt(start + i*step);

        // FIXME? Perhaps the integral should be done analitically
        // between every two FADC slices, instead of numerically
    }
    sum *= step; // Transform sum in integral

    return sum;
    */

    // In the future we will calculate the intgeral analytically.
    // It has been tested that it gives identical results within
    // acceptable differences.

    // We allow extrapolation of 1/2 slice.
    const Float_t min = fRiseTime;        //-0.5+fRiseTime;
    const Float_t max = fNum-1-fFallTime; //fNum-0.5+fFallTime;

    if (pos<min)
        pos = min;
    if (pos>max)
        pos = max;

    return EvalInteg(pos-fRiseTime, pos+fFallTime);
}

Float_t MExtralgoSpline::ExtractNoise(/*Int_t iter*/)
{
    if (fNum<5)
        return 0;

    // FIXME: Shell we keep the extraction inside one slice
    // or randomize it along the extraction window?
    const Float_t nsx = gRandom->Uniform(); //iter * fResolution;

    if (fExtractionType == kAmplitude)
        return Eval(2, nsx);
    else
        return CalcIntegral(2 + nsx);
}

void MExtralgoSpline::Extract(Byte_t sat, Int_t maxbin, Bool_t width)
{
    fSignal    =  0;
    fTime      =  0;
    fWidth     =  0;
    fSignalDev = -1;
    fTimeDev   = -1;
    fWidthDev  = -1;

    if (fNum<2)
        return;
/*
    //
    // Allow no saturated slice and
    // Don't start if the maxpos is too close to the limits.
    //

    const Bool_t limlo = maxbin <      TMath::Ceil(fRiseTime);
    const Bool_t limup = maxbin > fNum-TMath::Ceil(fFallTime)-1;
    if (sat || limlo || limup)
    {
        fTimeDev = 1.0;
        if (fExtractionType == kAmplitude)
        {
            fSignal    = fVal[maxbin];
            fTime      = maxbin;
            fSignalDev = 0;  // means: is valid
            return;
        }

        fSignal    = CalcIntegral(limlo ? 0 : fNum);
        fTime      = maxbin - 1;
        fSignalDev = 0;  // means: is valid
        return;
    }

    //
    // Now find the maximum
    //

    Float_t step = 0.2; // start with step size of 1ns and loop again with the smaller one

    Int_t klo = maxbin-1;

    Float_t maxpos = maxbin;//! Current position of the maximum of the spline
    Float_t max = fVal[maxbin];//! Current maximum of the spline

    //
    // Search for the maximum, starting in interval maxpos-1 in steps of 0.2 till maxpos-0.2.
    // If no maximum is found, go to interval maxpos+1.
    //
    for (Int_t i=0; i<TMath::Nint(TMath::Ceil((1-0.3)/step)); i++)
    {
        const Float_t x = klo + step*(i+1);
        //const Float_t y = Eval(klo, x);
        const Float_t y = Eval(klo, x-klo);
        if (y > max)
        {
            max    = y;
            maxpos = x;
        }
    }

    //
    // Search for the absolute maximum from maxpos to maxpos+1 in steps of 0.2
    //
    if (maxpos > maxbin - 0.1)
    {
        klo = maxbin;

        for (Int_t i=0; i<TMath::Nint(TMath::Ceil((1-0.3)/step)); i++)
        {
            const Float_t x = klo + step*(i+1);
            //const Float_t y = Eval(klo, x);
            const Float_t y = Eval(klo, x-klo);
            if (y > max)
            {
                max    = y;
                maxpos = x;
            }
        }
    }

    //
    // Now, the time, abmax and khicont and klocont are set correctly within the previous precision.
    // Try a better precision.
    //
    const Float_t up = maxpos+step - 3.0*fResolution;
    const Float_t lo = maxpos-step + 3.0*fResolution;
    const Float_t abmaxpos = maxpos;

    step  = 2.*fResolution; // step size of 0.1 FADC slices

    for (int i=0; i<TMath::Nint(TMath::Ceil((up-abmaxpos)/step)); i++)
    {
        const Float_t x = abmaxpos + (i+1)*step;
        //const Float_t y = Eval(klo, x);
        const Float_t y = Eval(klo, x-klo);
        if (y > max)
        {
            max    = y;
            maxpos = x;
        }
    }

    //
    // Second, try from time down to time-0.2 in steps of fResolution.
    //

    //
    // Test the possibility that the absolute maximum has not been found between
    // maxpos and maxpos+0.05, then we have to look between maxpos-0.05 and maxpos
    // which requires new setting of klocont and khicont
    //
    if (abmaxpos < klo + fResolution)
        klo--;

    for (int i=TMath::Nint(TMath::Ceil((abmaxpos-lo)/step))-1; i>=0; i--)
    {
        const Float_t x = abmaxpos - (i+1)*step;
        //const Float_t y = Eval(klo, x);
        const Float_t y = Eval(klo, x-klo);
        if (y > max)
        {
            max    = y;
            maxpos = x;
        }
    }

    fTime      = maxpos;
    fTimeDev   = fResolution;
    fSignal    = CalcIntegral(maxpos);
    fSignalDev = 0;  // means: is valid

    return;
*/
    // --- Start NEW ---

    // This block extracts values very similar to the old algorithm...
    // for max>10
    /* Most accurate (old identical) version:

    Float_t xmax=maxpos, ymax=Eval(maxpos-1, 1);
    Int_t rc = GetMaxPos(maxpos-1, xmax, ymax);
    if (xmax==maxpos)
    {
        GetMaxPos(maxpos, xmax, ymax);

        Float_t y = Eval(maxpos, 1);
        if (y>ymax)
        {
            ymax = y;
            xmax = maxpos+1;
        }
    }*/

    Float_t maxpos;
    // FIXME: Check the default if no maximum found!!!
    GetMaxAroundI(maxbin, maxpos, fHeight);

    // --- End NEW ---

    if (fExtractionType == kAmplitude)
    {
        fTime      = maxpos;
        fTimeDev   = 0;
        fSignal    = fHeight;
        fSignalDev = 0;  // means: is valid
        return;
    }

    const Float_t h = fExtractionType==kIntegralAbs ? fHeightTm : fHeight*fHeightTm;

    // Search downwards for fHeight/2
    // By doing also a search upwards we could extract the pulse width
    fTime      = SearchY(maxpos, h);
    fTimeDev   = 0;
    if (width)
    {
        fWidth    = SearchYup(maxpos, h)-fTime;
        fWidthDev = 0;
    }
    fSignal    = CalcIntegral(maxpos);
    fSignalDev = 0;  // means: is valid

    //
    // Loop from the beginning of the slice upwards to reach the maxhalf:
    // With means of bisection:
    //
    /*
    static const Float_t sqrt2 = TMath::Sqrt(2.);

    step = sqrt2*3*0.061;//fRiseTime;
    Float_t x = maxpos-0.86-3*0.061;//fRiseTime*1.25;

//    step = sqrt2*0.5;//fRiseTime;
//    Float_t x = maxpos-1.25;//fRiseTime*1.25;

    Int_t  cnt  =0;
    while (cnt++<30)
    {
        const Float_t y=EvalAt(x);

        if (TMath::Abs(y-maxval/2)<fResolution)
            break;

        step /= sqrt2; // /2
        x += y>maxval/2 ? -step : +step;
    }
    */

    //
    // Now integrate the whole thing!
    //
    //   fTime      = cnt==31 ? -1 : x;
    //   fTimeDev   = fResolution;
    //   fSignal    = cnt==31 ? CalcIntegral(x) : CalcIntegral(maxpos);
    //   fSignalDev = 0;  // means: is valid
}