/* ======================================================================== *\
!
! *
! * 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       05/2004 <mailto:markus@ifae.es> 
!
!   Copyright: MAGIC Software Development, 2002-2004
!
!
\* ======================================================================== */

//////////////////////////////////////////////////////////////////////////////
//
//   MExtractTimeAndChargeSpline
//
//   Fast Spline extractor using a cubic spline algorithm of Numerical Recipes. 
//   It returns the integral below the interpolating spline. 
// 
//   Call: SetRange(fHiGainFirst, fHiGainLast, fLoGainFirst, fLoGainLast) 
//         to modify the ranges. Ranges have to be an even number. In case of odd 
//         ranges, the last slice will be reduced by one.
//
//  Defaults are: 
// 
//   fHiGainFirst =  fgHiGainFirst =  3 
//   fHiGainLast  =  fgHiGainLast  =  14
//   fLoGainFirst =  fgLoGainFirst =  3 
//   fLoGainLast  =  fgLoGainLast  =  14
//
//////////////////////////////////////////////////////////////////////////////
#include "MExtractTimeAndChargeSpline.h"

#include "MPedestalPix.h"
#include "MPedestalCam.h"

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

#include "MParList.h"
#include "MRawEvtPixelIter.h"

ClassImp(MExtractTimeAndChargeSpline);

using namespace std;

const Byte_t  MExtractTimeAndChargeSpline::fgHiGainFirst  = 2;
const Byte_t  MExtractTimeAndChargeSpline::fgHiGainLast   = 14;
const Byte_t  MExtractTimeAndChargeSpline::fgLoGainFirst  = 3;
const Byte_t  MExtractTimeAndChargeSpline::fgLoGainLast   = 14;
const Float_t MExtractTimeAndChargeSpline::fgResolution    = 0.001;
const Float_t MExtractTimeAndChargeSpline::fgRatioMax2Fall = 0.05;  
// --------------------------------------------------------------------------
//
// Default constructor.
//
// Calls: 
// - SetRange(fgHiGainFirst, fgHiGainLast, fgLoGainFirst, fgLoGainLast)
// 
// Initializes:
// - fResolution    to fgResolution
// - fRatioMax2Fall to fgRatioMax2Fall
// - fExtractCharges to kFALSE
//
MExtractTimeAndChargeSpline::MExtractTimeAndChargeSpline(const char *name, const char *title) 
    : fHiGainSignal(NULL), fLoGainSignal(NULL),
      fHiGainFirstDeriv(NULL), fLoGainFirstDeriv(NULL),
      fHiGainSecondDeriv(NULL), fLoGainSecondDeriv(NULL), 
      fAbMax(0.), fAbMaxPos(0.), fHalfMax(0.)
{

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

  SetResolution();
  SetRatioMax2Fall();
  SetRange(fgHiGainFirst, fgHiGainLast, fgLoGainFirst, fgLoGainLast);


  fNumHiGainSamples = 1.;
  fNumLoGainSamples = 1.;
  fSqrtHiGainSamples = 1.;
  fSqrtLoGainSamples = 1.;
  
}

MExtractTimeAndChargeSpline::~MExtractTimeAndChargeSpline()
{
  
  if (fHiGainSignal)
    delete fHiGainSignal;
  if (fLoGainSignal)
    delete fLoGainSignal;
  if (fHiGainFirstDeriv)
    delete fHiGainFirstDeriv;
  if (fLoGainFirstDeriv)
    delete fLoGainFirstDeriv;
  if (fHiGainSecondDeriv)
    delete fHiGainSecondDeriv;
  if (fLoGainSecondDeriv)
    delete fLoGainSecondDeriv;
  
}

// --------------------------------------------------------------------------
//
// The PreProcess searches for the following input containers:
//  - MRawEvtData
//  - MRawRunHeader
//  - MPedestalCam
//
// The following output containers are also searched and created if
// they were not found:
//
//  - MArrivalTimeCam
//
// If the flag fExtractCharges is set, also following containers are searched:
//
//  - MExtractedSignalCam
//
Int_t MExtractTimeAndChargeSpline::PreProcess(MParList *pList)
{

  if (!MExtractTimeAndCharge::PreProcess(pList))
    return kFALSE;
  
  return kTRUE;
}

// --------------------------------------------------------------------------
//
// ReInit
//
// Calls:
// - MExtractTimeAndCharge::ReInit(pList);
// - Deletes all arrays, if not NULL
// - Creates new arrays according to the extraction range
//
Bool_t MExtractTimeAndChargeSpline::ReInit(MParList *pList)
{

  if (!MExtractTimeAndCharge::ReInit(pList))
    return kFALSE;

  if (fHiGainSignal)
    delete fHiGainSignal;
  if (fLoGainSignal)
    delete fLoGainSignal;
  if (fHiGainFirstDeriv)
    delete fHiGainFirstDeriv;
  if (fLoGainFirstDeriv)
    delete fLoGainFirstDeriv;
  if (fHiGainSecondDeriv)
    delete fHiGainSecondDeriv;
  if (fLoGainSecondDeriv)
    delete fLoGainSecondDeriv;
  
  Int_t range = fHiGainLast - fHiGainFirst + 1 + fHiLoLast;

  fHiGainSignal = new Float_t[range];
  memset(fHiGainSignal,0,range*sizeof(Float_t));
  fHiGainFirstDeriv = new Float_t[range];
  memset(fHiGainFirstDeriv,0,range*sizeof(Float_t));
  fHiGainSecondDeriv = new Float_t[range];
  memset(fHiGainSecondDeriv,0,range*sizeof(Float_t));

  range = fLoGainLast - fLoGainFirst + 1;

  fLoGainSignal = new Float_t[range];
  memset(fLoGainSignal,0,range*sizeof(Float_t));
  fLoGainFirstDeriv = new Float_t[range];
  memset(fLoGainFirstDeriv,0,range*sizeof(Float_t));
  fLoGainSecondDeriv = new Float_t[range];
  memset(fLoGainSecondDeriv,0,range*sizeof(Float_t));

  return kTRUE;
}


// --------------------------------------------------------------------------
//
// Calculates the arrival time 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;
  Byte_t max       = 0;
  Byte_t maxpos    = 0;
  Int_t  count     = 0;

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

  Float_t PedMean[2];
  PedMean[0] = pedes + ABoffs;
  PedMean[1] = pedes - ABoffs;

  //
  // Check for saturation in all other slices
  //
  while (p<end)
    {

      const Int_t ids      = fHiGainFirst + count ;
      fHiGainSignal[count] = (Float_t)*p - PedMean[(ids+abflag) & 0x1];

      if (*p > max)
        {
          max    = *p;
          maxpos =  p-first;
        }

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

      end = logain + fHiLoLast;

      while (logain<end)
        {

          const Int_t ids      = fHiGainFirst + range ;
          fHiGainSignal[range] = (Float_t)*logain - PedMean[(ids+abflag) & 0x1]; 
          range++;
          
          if (*logain > max)
            {
              max    = *logain;
              maxpos =  logain-first;
            }
          
          if (*logain++ >= fSaturationLimit)
            sat++;

        }
    }
  
  //
  // allow no saturated slice 
  //
  if (sat)
    return;

  //
  // Don't start if the maxpos is too close to the left limit.
  //
  if (maxpos < 1)
    return;

  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])/6.;
  
  //
  // 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   = (Float_t)maxpos-1.;
  Float_t upper   = (Float_t)maxpos;
  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;
  fAbMax          = fHiGainSignal[khi];
  fAbMaxPos       = upper;

  //
  // 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;
        }

      //      *fLog << err << x << "  " << y << " " << fAbMaxPos<< endl;
    }

  //
  // 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   = (Float_t)maxpos+1.;
      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;
            }
          //          *fLog << inf << x << "  " << y << " " << fAbMaxPos << endl;

        }
  }


  //
  // 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-0.055;
  const Float_t lo = fAbMaxPos-step+0.055;
  const Float_t maxpossave = fAbMaxPos;
  
  x     = fAbMaxPos;
  a     = upper - x;
  b     = x - lower;
 
  step  = 0.02; // step size of 42 ps 
 
  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;
        }
      //      *fLog << inf << x << "  " << y << " " << fAbMaxPos << endl;      
    }

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

  //
  // Test the possibility that the absolute maximum has not been found between
  // maxpos and maxpos+0.02, then we have to look between maxpos-0.02 and maxpos
  // which requires new setting of klocont and khicont
  //
  if (x < klo + 0.02)
    {
      klo--;
      khi--;
      upper--;
      lower--;
    }

  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;
        }
      //      *fLog << warn << x << "  " << y << "  " << fAbMaxPos << endl;
    }

  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 > maxpos-3)
    {
      if (fHiGainSignal[klo] < fHalfMax)
        break;
      klo--;
    }

  //
  // 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;

  while (step > 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;

      //      *fLog << inf << x << "  " << y << " " << fHalfMax << endl;

      step /= 2.;
    }


  time  = (Float_t)fHiGainFirst + fAbMaxPos;
  //  *fLog << inf << time << endl;
  //  time  = (Float_t)fHiGainFirst + x;
  dtime = fResolution;
  sum = fAbMax;

#if 0
  //
  // Now integrate the whole thing!
  // 
  Int_t startslice = (Int_t)(x - 0.75);
  Int_t lastslice  = (Int_t)(fAbMaxPos + fRatioMax2Fall*fAbMax);
  Int_t coverage   = lastslice-startslice;

  //
  // make them even
  //
  if (coverage != (coverage & ~1))
    lastslice++;

  if (startslice < 0)
    {
      return;
      //      gLog << warn << GetDescriptor() 
      //           << ": High Gain Bounce against left border, your range is too limited! " << endl;
      startslice = 0;
    }

  if (lastslice > range-1)
    {
      return;
      //      gLog << warn << GetDescriptor() 
      //           << ": High Gain Bounce against right border, your range is too limited! " << endl;
      lastslice = range-1;
    }

  Int_t i = startslice;
  sum = 0.5*fHiGainSignal[i] + 0.75*fHiGainSecondDeriv[i];

  for (i=startslice+1; i<lastslice; i++)
    sum += fHiGainSignal[i] + 1.5*fHiGainSecondDeriv[i];
  
  sum += 0.5*fHiGainSignal[lastslice] + 0.75*fHiGainSecondDeriv[lastslice];
#endif
}


// --------------------------------------------------------------------------
//
// Calculates the arrival time 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) 
{
  
  const Int_t range = fLoGainLast - fLoGainFirst + 1;
  const Byte_t *end = first + range;
  Byte_t *p = first;
  Byte_t max    = 0;
  Byte_t maxpos = 0;
  Int_t count   = 0;

  Float_t pedes        = ped.GetPedestal();
  const Float_t ABoffs = ped.GetPedestalABoffset();
  Float_t PedMean[2];
  PedMean[0] = pedes + ABoffs;
  PedMean[1] = pedes - ABoffs;

  //
  // Check for saturation in all other slices
  //
  while (++p<end)
    {

      const Int_t ids       = fLoGainFirst + count ;
      fLoGainSignal[count] = (Float_t)*p - PedMean[(ids+abflag) & 0x1];

      if (*p > max)
        {
          max    = *p;
          maxpos =  p-first;
        }

      if (*p >= fSaturationLimit)
        {
          sat++;
          break;
        }
      count++;
    }
  
  if (sat)
    return;

  if (maxpos < 2)
    return;
  
  Float_t pp;

  p = first;
  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;
      p++;
    }

  fLoGainSecondDeriv[range-1] = 0.;
  for (Int_t k=range-2;k>=0;k--)
    fLoGainSecondDeriv[k] = (fLoGainSecondDeriv[k]*fLoGainSecondDeriv[k+1] + fLoGainFirstDeriv[k])/6.;
  
  //
  // 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 = (Float_t)maxpos-1.;
  Float_t upper = (Float_t)maxpos;
  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;
  Float_t klocont = fLoGainSignal[klo];
  Float_t khicont = fLoGainSignal[khi];
  fAbMax    = klocont;
  fAbMaxPos = lower;


  //
  // Search for the maximum, starting in interval maxpos-1. If no maximum is found, go to 
  // interval maxpos+1.
  //
  while (x<upper-0.1)
    {

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

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

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

    }

 if (fAbMaxPos < lower+0.1)
    {

      upper = (Float_t)maxpos-1.;
      lower = (Float_t)maxpos-2.;
      x     = lower;
      a     = 1.;
      b     = 0.;
      khi   = maxpos-1;
      klo   = maxpos-2;
      klocont = fLoGainSignal[klo];
      khicont = fLoGainSignal[khi];

      while (x<upper-0.1)
        {

          x += step;
          a -= step;
          b += step;
          
          y = a* klocont
            + b* khicont
            + (a*a*a-a)*fLoGainSecondDeriv[klo] 
            + (b*b*b-b)*fLoGainSecondDeriv[khi];
          
          if (y > fAbMax)
            {
              fAbMax    = y;
              fAbMaxPos = x;
            }
        }
  }
 
 const Float_t up = fAbMaxPos+step-0.035;
 const Float_t lo = fAbMaxPos-step+0.035;
 const Float_t maxpossave = fAbMaxPos;
 
  x     = fAbMaxPos;
  a     = upper - x;
  b     = x - lower;

  step  = 0.025; // step size of 165 ps 

  while (x<up)
    {

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

  x     = maxpossave;
  a     = upper - x;
  b     = x - lower;

  while (x>lo)
    {

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

  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 -1;
  while (klo > maxpos-4)
    {
      if (*(first+klo) < (Byte_t)fHalfMax)
        break;
      klo--;
    }

  //
  // Loop from the beginning of the slice upwards to reach the fHalfMax:
  // With means of bisection:
  // 
  x     = (Float_t)klo;
  a     = 1.;
  b     = 0.;
  klocont = fLoGainSignal[klo]; 
  khicont = fLoGainSignal[klo+1];
  time  = x;

  step = 0.5;
  Bool_t back = kFALSE;

  while (step > fResolution)
    {
      
      if (back)
        {
          x -= step;
          a += step;
          b -= step;
        }
      else
        {
          x += step;
          a -= step;
          b += step;
        }
      
      y = a*klocont
        + b*khicont
        + (a*a*a-a)*fLoGainSecondDeriv[klo] 
        + (b*b*b-b)*fLoGainSecondDeriv[khi];

      if (y >= fHalfMax)
        back = kTRUE;
      else
        back = kFALSE;

      step /= 2.;
    }

  time  = (Float_t)fLoGainFirst + x;
  dtime = fResolution;

  //
  // Now integrate the whole thing!
  // 
  Int_t startslice = (Int_t)(time - 1.5);
  Int_t lastslice  = (Int_t)(fAbMaxPos + 2.*fRatioMax2Fall*fAbMax-1.);
  Int_t coverage   = lastslice-startslice;

  //
  // make them even
  //
  if (coverage != (coverage & ~1))
    lastslice++;

  if (startslice < 0)
    {
      //      gLog << warn << GetDescriptor() 
      //           << ": Low Gain Bounce against left border, your range is too limited! " << endl;
      sum = 0.;
      return;
      startslice = 0;
    }

  if (lastslice > range-1)
    {
      sum = 0.;
      return;
      //      gLog << warn << GetDescriptor()
      //           << ": Low Gain Bounce against right border, your range is too limited! " << endl;
      lastslice = range-1;
    }
  
  Int_t i = startslice;
  sum = 0.5*fLoGainSignal[i] + 0.75*fLoGainSecondDeriv[i];

  for (i=startslice+1; i<lastslice; i++)
    sum += fLoGainSignal[i] + 1.5*fLoGainSecondDeriv[i];

  
  sum += 0.5*fLoGainSignal[lastslice] + 0.75*fLoGainSecondDeriv[lastslice];
  // 
  // subtract the pedestal
  //
  //  sum -= pedes*(lastslice-startslice);
  
  //  sig.SetNumLoGainSlices(fNumLoGainSamples);      

}

