/* ======================================================================== *\
!
! *
! * 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): Markus Gaug       09/2004 <mailto:markus@ifae.es> 
!
!   Copyright: MAGIC Software Development, 2002-2004
!
!
\* ======================================================================== */
//////////////////////////////////////////////////////////////////////////////
//
//   MExtractTimeAndChargeSpline
//
//   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 "MExtractTimeAndChargeSpline.h"

#include "MPedestalPix.h"

#include "MLog.h"
#include "MLogManip.h"

ClassImp(MExtractTimeAndChargeSpline);

using namespace std;

const Byte_t  MExtractTimeAndChargeSpline::fgHiGainFirst    = 2;
const Byte_t  MExtractTimeAndChargeSpline::fgHiGainLast     = 14;
const Byte_t  MExtractTimeAndChargeSpline::fgLoGainFirst    = 2;
const Byte_t  MExtractTimeAndChargeSpline::fgLoGainLast     = 14;
const Float_t MExtractTimeAndChargeSpline::fgResolution     = 0.05;
const Float_t MExtractTimeAndChargeSpline::fgRiseTimeHiGain = 1.5;
const Float_t MExtractTimeAndChargeSpline::fgFallTimeHiGain = 4.5;
const Float_t MExtractTimeAndChargeSpline::fgLoGainStretch  = 1.5;
const Float_t MExtractTimeAndChargeSpline::fgOffsetLoGain   = 1.7;   // 5 ns
// --------------------------------------------------------------------------
//
// Default constructor.
//
// Calls: 
// - SetRange(fgHiGainFirst, fgHiGainLast, fgLoGainFirst, fgLoGainLast)
// 
// Initializes:
// - fResolution     to fgResolution
// - fRiseTimeHiGain to fgRiseTimeHiGain
// - fFallTimeHiGain to fgFallTimeHiGain
// - Charge Extraction Type to kAmplitude
// - fLoGainStretch  to fgLoGainStretch
//
MExtractTimeAndChargeSpline::MExtractTimeAndChargeSpline(const char *name, const char *title) 
    : fAbMax(0.), fAbMaxPos(0.), fHalfMax(0.), 
      fRiseTimeHiGain(fgRiseTimeHiGain), fFallTimeHiGain(fgFallTimeHiGain),
      fRandomIter(0)
{

  fName  = name  ? name  : "MExtractTimeAndChargeSpline";
  fTitle = title ? title : "Calculate photons arrival time using a fast spline";

  SetResolution();
  SetLoGainStretch();
  SetOffsetLoGain(fgOffsetLoGain);

  SetChargeType();
  SetRange(fgHiGainFirst, fgHiGainLast, fgLoGainFirst, fgLoGainLast);
}


//-------------------------------------------------------------------
//
// Set the ranges
// In order to set the fNum...Samples variables correctly for the case, 
// the integral is computed, have to overwrite this function and make an 
// explicit call to SetChargeType().
//
void MExtractTimeAndChargeSpline::SetRange(Byte_t hifirst, Byte_t hilast, Byte_t lofirst, Byte_t lolast)
{

  MExtractor::SetRange(hifirst, hilast, lofirst, lolast);

  if (IsExtractionType(kIntegral))
    SetChargeType(kIntegral);
  if (IsExtractionType(kAmplitude))
    SetChargeType(kAmplitude);
  
}

//-------------------------------------------------------------------
//
// Set the Charge Extraction type. Possible are:
// - kAmplitude: Search the value of the spline at the maximum
// - kIntegral:  Integral the spline from fHiGainFirst to fHiGainLast,   
//               by counting the edge bins only half and setting the 
//               second derivative to zero, there.
//
void MExtractTimeAndChargeSpline::SetChargeType( ExtractionType_t typ )
{

  CLRBIT(fFlags,kAmplitude);
  CLRBIT(fFlags,kIntegral );

  SETBIT(fFlags,typ);

  if (IsExtractionType(kAmplitude))
    {
      fNumHiGainSamples = 1.;
      fNumLoGainSamples = fLoGainLast ? 1. : 0.; 
      fSqrtHiGainSamples = 1.;
      fSqrtLoGainSamples = 1.;
      fWindowSizeHiGain  = 1;
      fWindowSizeLoGain  = 1;
      fRiseTimeHiGain    = 0.5;
      
      return;
    }

  if (IsExtractionType(kIntegral))
    {

      fNumHiGainSamples  = fRiseTimeHiGain + fFallTimeHiGain;
      fNumLoGainSamples  = fLoGainLast ? fRiseTimeLoGain + fFallTimeLoGain : 0.;
      //      fNumLoGainSamples  *= 0.75;      

      fSqrtHiGainSamples = TMath::Sqrt(fNumHiGainSamples);
      fSqrtLoGainSamples = TMath::Sqrt(fNumLoGainSamples);
      fWindowSizeHiGain  = (Int_t)(fRiseTimeHiGain + fFallTimeHiGain);
      fWindowSizeLoGain  = (Int_t)(fRiseTimeLoGain + fFallTimeLoGain);
      //      fNumLoGainSamples  *= 0.75;      
    }
}

// --------------------------------------------------------------------------
//
// InitArrays
//
// Gets called in the ReInit() and initialized the arrays
//
Bool_t MExtractTimeAndChargeSpline::InitArrays()
{

  Int_t range = fHiGainLast - fHiGainFirst + 1 + fHiLoLast;

  fHiGainSignal     .Set(range);
  fHiGainFirstDeriv .Set(range);
  fHiGainSecondDeriv.Set(range);

  range = fLoGainLast - fLoGainFirst + 1;

  fLoGainSignal     .Set(range);
  fLoGainFirstDeriv .Set(range);
  fLoGainSecondDeriv.Set(range);

  fHiGainSignal     .Reset();
  fHiGainFirstDeriv .Reset();
  fHiGainSecondDeriv.Reset();

  fLoGainSignal     .Reset();
  fLoGainFirstDeriv .Reset();
  fLoGainSecondDeriv.Reset();
  
  if (IsExtractionType(kAmplitude))
    {
      fNumHiGainSamples = 1.;
      fNumLoGainSamples = fLoGainLast ? 1. : 0.; 
      fSqrtHiGainSamples = 1.;
      fSqrtLoGainSamples = 1.;
      fWindowSizeHiGain  = 1;
      fWindowSizeLoGain  = 1;
      fRiseTimeHiGain    = 0.5;
    }

  fRiseTimeLoGain    = fRiseTimeHiGain * fLoGainStretch;
  fFallTimeLoGain    = fFallTimeHiGain * fLoGainStretch;      

  if (IsExtractionType(kIntegral))
    {

      fNumHiGainSamples  = fRiseTimeHiGain + fFallTimeHiGain;
      fNumLoGainSamples  = fLoGainLast ? fRiseTimeLoGain + fFallTimeLoGain : 0.;
      //      fNumLoGainSamples  *= 0.75;      

      fSqrtHiGainSamples = TMath::Sqrt(fNumHiGainSamples);
      fSqrtLoGainSamples = TMath::Sqrt(fNumLoGainSamples);
      fWindowSizeHiGain  = (Int_t)(fRiseTimeHiGain + fFallTimeHiGain);
      fWindowSizeLoGain  = (Int_t)(fRiseTimeLoGain + fFallTimeLoGain);
    }

  return kTRUE;

}

// --------------------------------------------------------------------------
//
// Calculates the arrival time and charge for each pixel 
//
void MExtractTimeAndChargeSpline::FindTimeAndChargeHiGain(Byte_t *first, Byte_t *logain, Float_t &sum, Float_t &dsum, 
                                                          Float_t &time, Float_t &dtime, 
                                                          Byte_t &sat, const MPedestalPix &ped, const Bool_t abflag)
{
  
  Int_t range = fHiGainLast - fHiGainFirst + 1;
  const Byte_t *end = first + range;
  Byte_t       *p  = first;

  sat = 0;

  const Float_t pedes  = ped.GetPedestal();
  const Float_t ABoffs = ped.GetPedestalABoffset();

  const Float_t pedmean[2] = { pedes + ABoffs, pedes - ABoffs };

  fAbMax         = 0.;
  fAbMaxPos      = 0.;
  fHalfMax       = 0.;
  fMaxBinContent = 0;
  Int_t  maxpos  = 0;

  //
  // Check for saturation in all other slices
  //
  Int_t ids = fHiGainFirst;
  Float_t *sample = fHiGainSignal.GetArray();
  while (p<end)
    {

      *sample++ = (Float_t)*p - pedmean[(ids++ + abflag) & 0x1];

      if (*p > fMaxBinContent)
        {
          maxpos = ids-fHiGainFirst-1;
          fMaxBinContent = *p;
        }

      if (*p++ >= fSaturationLimit)
        if (!sat)
          sat = ids-2;
      
    }
  
  if (fHiLoLast != 0)
    {

      end = logain + fHiLoLast;

      while (logain<end)
        {

          *sample++ = (Float_t)*logain - pedmean[(ids++ + abflag) & 0x1];

          if (*logain > fMaxBinContent)
            { 
              maxpos = ids-fHiGainFirst-1;
              fMaxBinContent = *logain;
            }
          
          if (*logain++ >= fSaturationLimit)
            if (!sat)
              sat = ids-2;

          range++;
        }
    }
  
  fAbMax = fHiGainSignal[maxpos];

  Float_t pp;

  fHiGainSecondDeriv[0] = 0.;
  fHiGainFirstDeriv[0]  = 0.;

  for (Int_t i=1;i<range-1;i++)
    {
      pp = fHiGainSecondDeriv[i-1] + 4.;
      fHiGainSecondDeriv[i] = -1.0/pp;
      fHiGainFirstDeriv [i] = fHiGainSignal[i+1] - fHiGainSignal[i] - fHiGainSignal[i] + fHiGainSignal[i-1];
      fHiGainFirstDeriv [i] = (6.0*fHiGainFirstDeriv[i]-fHiGainFirstDeriv[i-1])/pp;
    }

  fHiGainSecondDeriv[range-1] = 0.;

  for (Int_t k=range-2;k>=0;k--)
    fHiGainSecondDeriv[k] = fHiGainSecondDeriv[k]*fHiGainSecondDeriv[k+1] + fHiGainFirstDeriv[k];
  for (Int_t k=range-2;k>=0;k--)
    fHiGainSecondDeriv[k] /= 6.;
  
  if (IsNoiseCalculation())
    {

      if (fRandomIter == int(1./fResolution))
        fRandomIter = 0;
      
      const Float_t nsx = fRandomIter * fResolution;

      if (IsExtractionType(kAmplitude))
        {
          const Float_t b = nsx;
          const Float_t a = 1. - nsx;
          
          sum = a*fHiGainSignal[1]
            + b*fHiGainSignal[2]
            + (a*a*a-a)*fHiGainSecondDeriv[1] 
            + (b*b*b-b)*fHiGainSecondDeriv[2];
        }
      else
        {
          Float_t start = 2. + nsx;
          Float_t last  = start + fRiseTimeHiGain + fFallTimeHiGain;
      
          if (int(last) > range)
            {
              const Int_t diff = range - int(last);
              last  -= diff;
              start -= diff;
            }
          
          CalcIntegralHiGain(sum, start, last);
        }
      fRandomIter++;
      return;
    }

  //
  // Allow no saturated slice 
  // and 
  // Don't start if the maxpos is too close to the limits.
  //
  if (sat || maxpos < TMath::Ceil(fRiseTimeHiGain) || maxpos > range-2)
    {

      dtime = 1.0;
      if (IsExtractionType(kAmplitude))
        {
          sum  = fAbMax;
          time = (Float_t)(fHiGainFirst + maxpos);
          return;
        }
      
      if (maxpos > range - 2)
        CalcIntegralHiGain(sum, (Float_t)range - fRiseTimeHiGain - fFallTimeHiGain, (Float_t)range - 0.001);
      else
        CalcIntegralHiGain(sum, 0.001, fRiseTimeHiGain + fFallTimeHiGain);

      time =  (Float_t)(fHiGainFirst + maxpos - 1);
      return;
    }
      
  dtime = fResolution;

  //
  // Now find the maximum  
  //
  Float_t step    = 0.2; // start with step size of 1ns and loop again with the smaller one
  Float_t lower   = -1. + maxpos;
  Float_t upper   = (Float_t)maxpos;
  fAbMaxPos       = upper;
  Float_t x       = lower;
  Float_t y       = 0.;
  Float_t a       = 1.;
  Float_t b       = 0.;
  Int_t   klo     = maxpos-1;
  Int_t   khi     = maxpos;

  //
  // 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.
  //
  while ( x < upper - 0.3 )
    {

      x += step;
      a -= step;
      b += step;

      y = a*fHiGainSignal[klo]
        + b*fHiGainSignal[khi]
        + (a*a*a-a)*fHiGainSecondDeriv[klo] 
        + (b*b*b-b)*fHiGainSecondDeriv[khi];

      if (y > fAbMax)
        {
          fAbMax    = y;
          fAbMaxPos = x;
        }

    }

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

      upper   = 1. + maxpos;
      lower   = (Float_t)maxpos;
      x       = lower;
      a       = 1.;
      b       = 0.;
      khi     = maxpos+1;
      klo     = maxpos;

      while (x<upper-0.3)
        {

          x += step;
          a -= step;
          b += step;
          
          y = a*fHiGainSignal[klo]
            + b*fHiGainSignal[khi]
            + (a*a*a-a)*fHiGainSecondDeriv[klo] 
            + (b*b*b-b)*fHiGainSecondDeriv[khi];

          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 maxpossave = fAbMaxPos;
  
  x     = fAbMaxPos;
  a     = upper - x;
  b     = x - lower;
 
  step  = 2.*fResolution; // step size of 0.1 FADC slices
 
  while (x<up)
    {

      x += step;
      a -= step;
      b += step;
      
      y = a*fHiGainSignal[klo]
        + b*fHiGainSignal[khi]
        + (a*a*a-a)*fHiGainSecondDeriv[klo] 
        + (b*b*b-b)*fHiGainSecondDeriv[khi];
      
      if (y > fAbMax)
        {
          fAbMax    = y;
          fAbMaxPos = x;
        }
    }

  //
  // Second, try from time down to time-0.2 in steps of fResolution.
  //
  x     = maxpossave;

  //
  // 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 (x < lower + fResolution)
    {
      klo--;
      khi--;
      upper -= 1.;
      lower -= 1.;
    }

  a     = upper - x;
  b     = x - lower;
  
  while (x>lo)
    {

      x -= step;
      a += step;
      b -= step;
      
      y = a*fHiGainSignal[klo]
        + b*fHiGainSignal[khi]
        + (a*a*a-a)*fHiGainSecondDeriv[klo] 
        + (b*b*b-b)*fHiGainSecondDeriv[khi];
      
      if (y > fAbMax)
        {
          fAbMax    = y;
          fAbMaxPos = x;
        }
    }

  if (IsExtractionType(kAmplitude))
    {
      time  = fAbMaxPos + (Int_t)fHiGainFirst;
      sum   = fAbMax;
      return;
    }

  fHalfMax = fAbMax/2.;
  
  //
  // 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)
    {
      klo--;
      if (fHiGainSignal[klo] < fHalfMax)
        break;
    }
  
  khi = klo+1;
  //
  // Loop from the beginning of the slice upwards to reach the fHalfMax:
  // With means of bisection:
  // 
  x     = (Float_t)klo;
  a     = 1.;
  b     = 0.;
  
  step = 0.5;
  Bool_t back = kFALSE;
  
  Int_t maxcnt = 20;
  Int_t cnt    = 0;
  
  while (TMath::Abs(y-fHalfMax) > fResolution)
    {
      
      if (back)
        {
          x -= step;
          a += step;
          b -= step;
        }
      else
        {
          x += step;
          a -= step;
          b += step;
        }
      
      y = a*fHiGainSignal[klo]
        + b*fHiGainSignal[khi]
        + (a*a*a-a)*fHiGainSecondDeriv[klo] 
        + (b*b*b-b)*fHiGainSecondDeriv[khi];
      
      if (y > fHalfMax)
        back = kTRUE;
      else
        back = kFALSE;
      
      if (++cnt > maxcnt)
        break;
      
      step /= 2.;
    }
  
  time  = (Float_t)fHiGainFirst + x;
  //
  // Now integrate the whole thing!
  // 
  
  Float_t start = fAbMaxPos - fRiseTimeHiGain;
  Float_t last  = fAbMaxPos + fFallTimeHiGain;
  
  const Int_t diff = int(last) - range;
  
  if (diff > 0)
    {
      last  -= diff;
      start -= diff;
    }
  
  CalcIntegralHiGain(sum, start, last);
}


// --------------------------------------------------------------------------
//
// Calculates the arrival time and charge for each pixel 
//
void MExtractTimeAndChargeSpline::FindTimeAndChargeLoGain(Byte_t *first, Float_t &sum, Float_t &dsum, 
                                                          Float_t &time, Float_t &dtime, 
                                                          Byte_t &sat, const MPedestalPix &ped, const Bool_t abflag) 
{
  
  Int_t range = fLoGainLast - fLoGainFirst + 1;
  const Byte_t *end = first + range;
  Byte_t       *p  = first;

  const Float_t pedes  = ped.GetPedestal();
  const Float_t ABoffs = ped.GetPedestalABoffset();

  const Float_t pedmean[2] = { pedes + ABoffs, pedes - ABoffs };

  fAbMax        = 0.;
  fAbMaxPos     = 0.;
  Int_t  maxpos = 0;
  Int_t  max    = 0;

  //
  // Check for saturation in all other slices
  //
  Int_t    ids    = fLoGainFirst;
  Float_t *sample = fLoGainSignal.GetArray();
  while (p<end)
    {

      *sample++ = (Float_t)*p - pedmean[(ids++ + abflag) & 0x1];

      if (*p > max)
        {
          maxpos = ids-fLoGainFirst-1;
          max    = *p;
        }

      if (*p++ >= fSaturationLimit)
        sat++;
    }
  
  fAbMax = fLoGainSignal[maxpos];
  
  Float_t pp;

  fLoGainSecondDeriv[0] = 0.;
  fLoGainFirstDeriv[0]  = 0.;

  for (Int_t i=1;i<range-1;i++)
    {
      pp = fLoGainSecondDeriv[i-1] + 4.;
      fLoGainSecondDeriv[i] = -1.0/pp;
      fLoGainFirstDeriv [i] = fLoGainSignal[i+1] - fLoGainSignal[i] - fLoGainSignal[i] + fLoGainSignal[i-1];
      fLoGainFirstDeriv [i] = (6.0*fLoGainFirstDeriv[i]-fLoGainFirstDeriv[i-1])/pp;
    }

  fLoGainSecondDeriv[range-1] = 0.;

  for (Int_t k=range-2;k>=0;k--)
    fLoGainSecondDeriv[k] = fLoGainSecondDeriv[k]*fLoGainSecondDeriv[k+1] + fLoGainFirstDeriv[k];
  for (Int_t k=range-2;k>=0;k--)
    fLoGainSecondDeriv[k] /= 6.;
  
  if (IsNoiseCalculation())
    {
      if (fRandomIter == int(1./fResolution))
        fRandomIter = 0;
      
      const Float_t nsx = fRandomIter * fResolution;

      if (IsExtractionType(kAmplitude))
        {
          const Float_t b = nsx;
          const Float_t a = 1. - nsx;
          
          sum = a*fLoGainSignal[1]
            + b*fLoGainSignal[2]
            + (a*a*a-a)*fLoGainSecondDeriv[1] 
            + (b*b*b-b)*fLoGainSecondDeriv[2];
        }
      else
        {
          Float_t start = 2. + nsx;
          Float_t last  = start + fRiseTimeLoGain + fFallTimeLoGain;
      
          if (int(last) > range)
            {
              const Int_t diff = range - int(last);
              last  -= diff;
              start -= diff;
            }
          
          CalcIntegralLoGain(sum, start, last);
        }
      fRandomIter++;
      return;
    }
  //
  // Allow no saturated slice 
  // and 
  // Don't start if the maxpos is too close to the limits.
  //
  if (sat || maxpos < TMath::Ceil(fRiseTimeLoGain) || maxpos > range-2)
    {

      dtime = 1.0;
      if (IsExtractionType(kAmplitude))
        {
          time = (Float_t)(fLoGainFirst + maxpos);
          sum = fAbMax;
          return;
        }
      
      if (maxpos > range-2)
        CalcIntegralLoGain(sum, (Float_t)range - fRiseTimeLoGain - fFallTimeLoGain -1., (Float_t)range - 0.001);
      else
        CalcIntegralLoGain(sum, 0.001, fRiseTimeLoGain + fFallTimeLoGain + 1.);

      time = (Float_t)(fLoGainFirst + maxpos - 1);
      return;
    }

  dtime = fResolution;

  //
  // Now find the maximum  
  //
  Float_t step    = 0.2; // start with step size of 1ns and loop again with the smaller one
  Float_t lower   = -1. + maxpos;
  Float_t upper   = (Float_t)maxpos;
  fAbMaxPos       = upper;
  Float_t x       = lower;
  Float_t y       = 0.;
  Float_t a       = 1.;
  Float_t b       = 0.;
  Int_t   klo     = maxpos-1;
  Int_t   khi     = maxpos;

  //
  // 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.
  //
  while ( x < upper - 0.3 )
    {

      x += step;
      a -= step;
      b += step;

      y = a*fLoGainSignal[klo]
        + b*fLoGainSignal[khi]
        + (a*a*a-a)*fLoGainSecondDeriv[klo] 
        + (b*b*b-b)*fLoGainSecondDeriv[khi];

      if (y > fAbMax)
        {
          fAbMax    = y;
          fAbMaxPos = x;
        }

    }

  //
  // Test the possibility that the absolute maximum has not been found before the 
  // maxpos and search from maxpos to maxpos+1 in steps of 0.2
  //
  if (fAbMaxPos > upper-0.1)
    {

      upper   = 1. + maxpos;
      lower   = (Float_t)maxpos;
      x       = lower;
      a       = 1.;
      b       = 0.;
      khi     = maxpos+1;
      klo     = maxpos;

      while (x<upper-0.3)
        {

          x += step;
          a -= step;
          b += step;
          
          y = a*fLoGainSignal[klo]
            + b*fLoGainSignal[khi]
            + (a*a*a-a)*fLoGainSecondDeriv[klo] 
            + (b*b*b-b)*fLoGainSecondDeriv[khi];

          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 maxpossave = fAbMaxPos;
  
  x     = fAbMaxPos;
  a     = upper - x;
  b     = x - lower;
 
  step  = 2.*fResolution; // step size of 0.1 FADC slice
 
  while (x<up)
    {

      x += step;
      a -= step;
      b += step;
      
      y = a*fLoGainSignal[klo]
        + b*fLoGainSignal[khi]
        + (a*a*a-a)*fLoGainSecondDeriv[klo] 
        + (b*b*b-b)*fLoGainSecondDeriv[khi];
      
      if (y > fAbMax)
        {
          fAbMax    = y;
          fAbMaxPos = x;
        }
    }

  //
  // Second, try from time down to time-0.2 in steps of 0.025.
  //
  x     = maxpossave;

  //
  // 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 (x < lower + fResolution)
    {
      klo--;
      khi--;
      upper -= 1.;
      lower -= 1.;
    }

  a     = upper - x;
  b     = x - lower;
  
  while (x>lo)
    {

      x -= step;
      a += step;
      b -= step;
      
      y = a*fLoGainSignal[klo]
        + b*fLoGainSignal[khi]
        + (a*a*a-a)*fLoGainSecondDeriv[klo] 
        + (b*b*b-b)*fLoGainSecondDeriv[khi];
      
      if (y > fAbMax)
        {
          fAbMax    = y;
          fAbMaxPos = x;
        }
    }

  if (IsExtractionType(kAmplitude))
    {
      time = fAbMaxPos + (Int_t)fLoGainFirst;
      sum  = fAbMax;
      return;
    }
  
  fHalfMax = fAbMax/2.;
  
  //
  // 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)
    {
      klo--;
      if (fLoGainSignal[klo] < fHalfMax)
        break;
    }
  
  khi = klo+1;
  //
  // Loop from the beginning of the slice upwards to reach the fHalfMax:
  // With means of bisection:
  // 
  x     = (Float_t)klo;
  a     = 1.;
  b     = 0.;
  
  step = 0.5;
  Bool_t back = kFALSE;
  
  Int_t maxcnt = 20;
  Int_t cnt    = 0;
  
  while (TMath::Abs(y-fHalfMax) > fResolution)
    {
      
      if (back)
        {
          x -= step;
          a += step;
          b -= step;
        }
      else
        {
          x += step;
          a -= step;
          b += step;
        }
      
      y = a*fLoGainSignal[klo]
        + b*fLoGainSignal[khi]
        + (a*a*a-a)*fLoGainSecondDeriv[klo] 
        + (b*b*b-b)*fLoGainSecondDeriv[khi];
      
      if (y > fHalfMax)
        back = kTRUE;
      else
        back = kFALSE;
      
      if (++cnt > maxcnt)
        break;
      
      step /= 2.;
    }
  
  time  = x + (Int_t)fLoGainFirst;

  //
  // Now integrate the whole thing!
  // 
  Float_t start = fAbMaxPos - fRiseTimeLoGain;
  Float_t last  = fAbMaxPos + fFallTimeLoGain;
  
  const Int_t diff = int(last) - range;
  
  if (diff > 0)
    {
      last  -= diff;
      start -= diff;
    }
  CalcIntegralLoGain(sum, start, last);
}

void MExtractTimeAndChargeSpline::CalcIntegralHiGain(Float_t &sum, Float_t start, Float_t last)
{

  const Float_t step = 0.1;

  if (start < 0)
    {
      last -= start;
      start = 0.;
    }
  
  Int_t klo = int(start);
  Int_t khi = klo+1;

  Float_t lo = TMath::Floor(start);
  Float_t up = lo + 1.;

  const Int_t m = int((start-klo)/step);
  start = step*m + klo; // Correct start for the digitization due to resolution

  Float_t x = start;
  Float_t a = up-start;
  Float_t b = start-lo;

  while (1)
    {
      
      while (x<up)
        {
          x += step;

          if (x > last)
            {
              sum *= step;
              return;
            }
          
          a -= step;
          b += step;
          
          sum += a*fHiGainSignal[klo]
          + b*fHiGainSignal[khi]
            + (a*a*a-a)*fHiGainSecondDeriv[klo] 
            + (b*b*b-b)*fHiGainSecondDeriv[khi];
        }

      up += 1.;
      lo += 1.;
      klo++;
      khi++;
      start += 1.;
      a = 1.;
      b = 0.;
    }
  
}
void MExtractTimeAndChargeSpline::CalcIntegralLoGain(Float_t &sum, Float_t start, Float_t last)
{

  const Float_t step = 0.1;

  if (start < 0)
    {
      last -= start;
      start = 0.;
    }
  
  Int_t klo = int(start);
  Int_t khi = klo+1;

  Float_t lo = TMath::Floor(start);
  Float_t up = lo + 1.;

  const Int_t m = int((start-klo)/step);
  start = step*m + klo; // Correct start for the digitization due to resolution

  Float_t x = start;
  Float_t a = up-start;
  Float_t b = start-lo;

  while (1)
    {
      
      while (x<up)
        {
          x += step;
          
          if (x > last)
            {
              sum *= step;
              return;
            }
          
          a -= step;
          b += step;
          
          sum += a*fLoGainSignal[klo]
          + b*fLoGainSignal[khi]
            + (a*a*a-a)*fLoGainSecondDeriv[klo] 
            + (b*b*b-b)*fLoGainSecondDeriv[khi];
          
        }

      up += 1.;
      lo += 1.;
      klo++;
      khi++;
      start += 1.;
      a = 1.;
      b = 0.;
    }
  
}




// --------------------------------------------------------------------------
//
// In addition to the resources of the base-class MExtractor:
//   MJPedestal.MExtractor.WindowSizeHiGain: 6
//   MJPedestal.MExtractor.WindowSizeLoGain: 6
//
Int_t MExtractTimeAndChargeSpline::ReadEnv(const TEnv &env, TString prefix, Bool_t print)
{

  Bool_t rc = kFALSE;
  
  if (IsEnvDefined(env, prefix, "Resolution", print))
    {
      SetResolution(GetEnvValue(env, prefix, "Resolution",fResolution));
      rc  = kTRUE;
    }
  if (IsEnvDefined(env, prefix, "RiseTimeHiGain", print))
    {
      SetRiseTimeHiGain(GetEnvValue(env, prefix, "RiseTimeHiGain", fRiseTimeHiGain));
      rc = kTRUE;
    }
  if (IsEnvDefined(env, prefix, "FallTimeHiGain", print))
    {
      SetFallTimeHiGain(GetEnvValue(env, prefix, "FallTimeHiGain", fFallTimeHiGain));
      rc = kTRUE;
    }
  if (IsEnvDefined(env, prefix, "LoGainStretch", print))
    {
      SetLoGainStretch(GetEnvValue(env, prefix, "LoGainStretch", fLoGainStretch));
      rc = kTRUE;
    }
  
  Bool_t b = kFALSE;

  if (IsEnvDefined(env, prefix, "Amplitude", print))
    {
      b = GetEnvValue(env, prefix, "Amplitude", IsExtractionType(kAmplitude));
      if (b) 
        SetChargeType(kAmplitude);
      rc = kTRUE;
    }
  if (IsEnvDefined(env, prefix, "Integral", print))
    {
      b = GetEnvValue(env, prefix, "Integral", IsExtractionType(kIntegral));
      if (b) 
        SetChargeType(kIntegral);
      rc = kTRUE;
    }

  return MExtractTimeAndCharge::ReadEnv(env, prefix, print) ? kTRUE : rc;

}


