/* ======================================================================== *\
!
! *
! * 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 "fHiGainSignal" and "fLoGainSignal"
//   which means the FADC value subtracted by the clock-noise corrected pedestal.
//
//   The coefficients "y2a" get immediately divided 6. and are called here 
//   "fHiGainSecondDeriv" and "fLoGainSecondDeriv" 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:
// 
//   y =    a*fHiGainSignal[klo] + b*fHiGainSignal[khi] 
//       + (a*a*a-a)*fHiGainSecondDeriv[klo] + (b*b*b-b)*fHiGainSecondDeriv[khi]
//
//   with:
//   a = (khi - x)
//   b = (x - klo)
//
//   and "klo" being the lower bin edge FADC index and "khi" the upper bin edge FADC index.
//   fHiGainSignal[klo] and fHiGainSignal[khi] are the FADC values at "klo" and "khi".
//
//   An analogues formula is used for the low-gain values.
//
//   The coefficients fHiGainSecondDeriv and fLoGainSecondDeriv are calculated with the 
//   following simplified algorithm:
//
//   for (Int_t i=1;i<range-1;i++) {
//       pp                   = fHiGainSecondDeriv[i-1] + 4.;
//       fHiGainFirstDeriv[i] = fHiGainSignal[i+1] - 2.*fHiGainSignal[i] + fHiGainSignal[i-1]
//       fHiGainFirstDeriv[i] = (6.0*fHiGainFirstDeriv[i]-fHiGainFirstDeriv[i-1])/pp;
//   }
// 
//   for (Int_t k=range-2;k>=0;k--)
//       fHiGainSecondDeriv[k] = (fHiGainSecondDeriv[k]*fHiGainSecondDeriv[k+1] + fHiGainFirstDeriv[k])/6.;
// 
//
//   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 "fHiGainFirstDeriv" are not real first derivative coefficients.)
//
//   The algorithm to search the time proceeds as follows:
//
//   1) Calculate all fHiGainSignal from fHiGainFirst to fHiGainLast 
//      (note that an "overlap" to the low-gain arrays is possible: i.e. fHiGainLast>14 in the case of 
//      the MAGIC FADCs).
//   2) Remember the position of the slice with the highest content "fAbMax" at "fAbMaxPos".
//   3) If one or more slices are saturated or fAbMaxPos is less than 2 slices from fHiGainFirst, 
//      return fAbMaxPos as time and fAbMax as charge (note that the pedestal is subtracted here).
//   4) Calculate all fHiGainSecondDeriv from the fHiGainSignal array
//   5) Search for the maximum, starting in interval fAbMaxPos-1 in steps of 0.2 till fAbMaxPos-0.2.
//      If no maximum is found, go to interval fAbMaxPos+1. 
//      --> 4 function evaluations
//   6) Search for the absolute maximum from fAbMaxPos to fAbMaxPos+1 in steps of 0.2 
//      --> 4 function  evaluations
//   7) Try a better precision searching from new max. position fAbMaxPos-0.2 to fAbMaxPos+0.2
//      in steps of 0.025 (83 psec. in the case of the MAGIC FADCs).
//      --> 14 function evaluations
//   8) If Time Extraction Type kMaximum has been chosen, the position of the found maximum is 
//      returned, else:
//   9) The Half Maximum is calculated. 
//  10) fHiGainSignal is called beginning from fAbMaxPos-1 backwards until a value smaller than fHalfMax
//      is found at "klo". 
//  11) Then, the spline value between "klo" and "klo"+1 is halfed by means of bisection as long as 
//      the difference between fHalfMax and spline evaluation is less than fResolution (default: 0.01).
//      --> maximum 12 interations.
//   
//  The algorithm to search the charge proceeds as follows:
//
//  1) If Charge Type: kAmplitude was chosen, return the Maximum of the spline, found during the 
//     time search.
//  2) If Charge Type: kIntegral was chosen, sum the fHiGainSignal between:
//     (Int_t)(fAbMaxPos - fRiseTimeHiGain) and 
//     (Int_t)(fAbMaxPos + fFallTimeHiGain)
//     (default: fRiseTime: 1.5, fFallTime: 4.5)
//                                           sum the fLoGainSignal between:
//     (Int_t)(fAbMaxPos - fRiseTimeHiGain*fLoGainStretch) and 
//     (Int_t)(fAbMaxPos + fFallTimeHiGain*fLoGainStretch)
//     (default: fLoGainStretch: 1.5)
//       
//  The values: fNumHiGainSamples and fNumLoGainSamples are set to:
//  1) If Charge Type: kAmplitude was chosen: 1.
//  2) If Charge Type: kIntegral was chosen: fRiseTimeHiGain + fFallTimeHiGain
//                 or: fNumHiGainSamples*fLoGainStretch in the case of the low-gain
//
//  Call: SetRange(fHiGainFirst, fHiGainLast, fLoGainFirst, fLoGainLast) 
//        to modify the ranges.
// 
//        Defaults: 
//        fHiGainFirst =  2 
//        fHiGainLast  =  14
//        fLoGainFirst =  2 
//        fLoGainLast  =  14
//
//  Call: SetResolution() to define the resolution of the half-maximum search.
//        Default: 0.01
//
//  Call: SetRiseTime() and SetFallTime() to define the integration ranges 
//        for the case, the extraction type kIntegral has been chosen.
//
//  Call: - SetChargeType(MExtractTimeAndChargeSpline::kAmplitude) for the 
//          computation of the amplitude at the maximum (default) and extraction 
//          the position of the maximum (default)
//          --> no further function evaluation needed
//        - SetChargeType(MExtractTimeAndChargeSpline::kIntegral) for the 
//          computation of the integral beneith the spline between fRiseTimeHiGain
//          from the position of the maximum to fFallTimeHiGain after the position of 
//          the maximum. The Low Gain is computed with half a slice more at the rising
//          edge and half a slice more at the falling edge.
//          The time of the half maximum is returned.
//          --> needs one function evaluations but is more precise
//        
//////////////////////////////////////////////////////////////////////////////
#include "MExtralgoSpline.h"

using namespace std;

void MExtralgoSpline::InitDerivatives() const
{
    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.;
}

Float_t MExtralgoSpline::CalcIntegral(Float_t start, Float_t range) 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 step  = 0.2;
    const Float_t width = fRiseTime+fFallTime;
    const Float_t max   = range-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++)
    {
        const Float_t x = start+i*step;
        const Int_t klo = (Int_t)TMath::Floor(x);
        // 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 += Eval(x, klo);

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

Float_t MExtralgoSpline::ExtractNoise(Int_t iter)
{
    const Float_t nsx = iter * fResolution;

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

void MExtralgoSpline::Extract(Byte_t sat, Int_t maxpos)
{
    fSignal    =  0;
    fTime      =  0;
    fSignalDev = -1;
    fTimeDev   = -1;

    //
    // Allow no saturated slice and
    // Don't start if the maxpos is too close to the limits.
    //
    const Bool_t limlo = maxpos <      TMath::Ceil(fRiseTime);
    const Bool_t limup = maxpos > fNum-TMath::Ceil(fFallTime)-1;
    if (sat || limlo || limup)
    {
        fTimeDev = 1.0;
        if (fExtractionType == kAmplitude)
        {
            fSignal    = fVal[maxpos];
            fTime      = maxpos;
            fSignalDev = 0;  // means: is valid
            return;
        }

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

    fTimeDev = fResolution;

    //
    // 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 = maxpos-1;
    Float_t fAbMaxPos = maxpos;//! Current position of the maximum of the spline
    Float_t fAbMax = fVal[maxpos];//! 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(x, klo);
        if (y > fAbMax)
        {
            fAbMax    = y;
            fAbMaxPos = x;
        }
    }

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

        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(x, klo);
            if (y > fAbMax)
            {
                fAbMax    = y;
                fAbMaxPos = x;
            }
        }
    }

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

    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(x, klo);
        if (y > fAbMax)
        {
            fAbMax    = y;
            fAbMaxPos = 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(x, klo);
        if (y > fAbMax)
        {
            fAbMax    = y;
            fAbMaxPos = x;
        }
    }

    if (fExtractionType == kAmplitude)
    {
        fTime      = fAbMaxPos;
        fSignal    = fAbMax;
        fSignalDev = 0;  // means: is valid
        return;
    }

    Float_t fHalfMax = fAbMax/2.;//! Current half maximum of the spline

    //
    // Now, loop from the maximum bin leftward down in order to find the
    // position of the half maximum. First, find the right FADC slice:
    //
    klo = maxpos;
    while (klo > 0)
    {
        if (fVal[--klo] < fHalfMax)
            break;
    }

    //
    // Loop from the beginning of the slice upwards to reach the fHalfMax:
    // With means of bisection:
    //
    step = 0.5;

    Int_t maxcnt = 20;
    Int_t cnt    = 0;

    Float_t y    = Eval(klo, klo); // FIXME: IS THIS CORRECT???????
    Float_t x    = klo;
    Bool_t back  = kFALSE;

    while (TMath::Abs(y-fHalfMax) > fResolution)
    {
        x += back ? -step : +step;

        const Float_t y = Eval(x, klo);

        back = y > fHalfMax;

        if (++cnt > maxcnt)
            break;

        step /= 2.;
    }

    //
    // Now integrate the whole thing!
    //
    fTime      = x;
    fSignal    = CalcIntegral(fAbMaxPos - fRiseTime, fNum);
    fSignalDev = 0;  // means: is valid
}