/* ======================================================================== *\
!
! *
! * 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 analysing 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): Hendrik Bartko, 09/2004 <mailto:hbartko@mppmu.mpg.de> 
!   Author(s): Thomas Bretz, 08/2006 <mailto:tbretz@astro.uni-wuerzburg.de>
!
!   Copyright: MAGIC Software Development, 2000-2006
!
!
\* ======================================================================== */

//////////////////////////////////////////////////////////////////////////////
//
//   MExtralgoDigitalFilter
//
//////////////////////////////////////////////////////////////////////////////
#include "MExtralgoDigitalFilter.h"

using namespace std;

Float_t MExtralgoDigitalFilter::ExtractNoise(Int_t iter) const
{
    return Eval(fWeightsAmp, 0, iter-fWeightsPerBin/2);
}

#include <iostream>
void MExtralgoDigitalFilter::Extract()
{
    fSignal    =  0; // default is: no pulse found
    fTime      = -1; // default is: out if range (--> random)
    fSignalDev =  0; // default is: valid
    fTimeDev   =  0; // default is: valid

    // FIXME: How to handle saturation?

    Double_t maxamp = -FLT_MAX;
    Int_t    maxp   = -1;

    //
    // Calculate the sum of the first fWindowSize slices
    //

    // For the case of an even numberof weights/bin there is
    // no central bin.So we create an artificial central bin.
    for (Int_t i=0; i<fNum-fWindowSize+1; i++)
    {
        const Double_t sumamp = Eval(fWeightsAmp, i);
        if (sumamp>maxamp)
        {
            maxamp = sumamp;
            maxp   = i;
        }
    }
    // The number of available slices were smaller than the
    // extraction window size of the extractor
    if (maxp<0)
    {
        fSignalDev = -1;  // means: is invalid
        fTimeDev   = -1;  // means: is invalid
        return;
    }

    // For some reason (by chance or because all slices contain 0)
    // maxamp is 0. This means the signal is zero and no arrival
    // time can be extracted (but both informations are valid)
    if (maxamp==0)
        return;

    Int_t frac = 0;
    const Int_t shift = AlignExtractionWindow(maxp, frac, maxamp);

    // For safety we do another iteration if we have
    // shifted the extraction window
    if (TMath::Abs(shift)>0)
        AlignExtractionWindow(maxp, frac);

    // Now we have found the "final" position: extract time and charge
    const Double_t sumamp = Eval(fWeightsAmp, maxp, frac);

    fSignal = sumamp;
    if (sumamp == 0)
        return;

    const Double_t sumtime = Eval(fWeightsTime, maxp, frac);

    // This is used to align the weights to bins between
    // -0.5/fWeightsPerBin and 0.5/fWeightsPerBin instead of
    // 0 and 1./fWeightsPerBin
    const Double_t binoffset = TMath::Even(fWeightsPerBin) ? 0.5 : 0;

    fTime = maxp /*- 0.5*/ -  Double_t(frac+binoffset)/fWeightsPerBin;

    // To let the lowest value which can be safely extracted be>0:
    // Take also a possible offset from timefineadjust into account
    //  Sould it be: fTime += fWindowSize/2; ???
    fTime += 0.5 + 0.5/fWeightsPerBin;
    // In the ideal case the would never get <0
    // But timefineadjust can be >0.05 (in 60% of the cases)

    // Define in each extractor a lowest and highest extracted value!

    // here, the first high-gain slice is already included
    // in the fTimeShiftHiGain this shifts the time to the
    // start of the rising edge

    // This is from MExtractTimeAndChargeDigitalFilter
    // fTime += 0.5 + 1./fWeightsPerBin;

    // Now there is a difference between the rising edge and
    // the extracted time. This must be applied after
    // the randomization in case of wrog values

    const Float_t timefineadjust = sumtime/sumamp;

    // FIXME: Is 3 a good value?
    //if (TMath::Abs(timefineadjust)*fWeightsPerBin < 3.)
    //    fTime -= timefineadjust;

    //    if (TMath::FloorNint(timefineadjust+0.5)==0)

    //if (TMath::Abs(timefineadjust) < 0.2)
        fTime -= timefineadjust;
}

#include <TH1.h>
#include <TH2.h>
#include <TMatrixD.h>
#include <TArrayF.h>
#include <iostream>
#include <TSpline.h>
#include <TProfile.h>

Bool_t MExtralgoDigitalFilter::CalculateWeights(TH1 &shape, const TH2 &autocorr, TArrayF &weightsamp, TArrayF &weightstime, Int_t wpb)
{
    const Int_t weightsperbin = wpb<=0?shape.GetNbinsX()/autocorr.GetNbinsX():wpb;

    if (wpb<=0 && weightsperbin*autocorr.GetNbinsX()!=shape.GetNbinsX())
    {
        cout << "ERROR - Number of bins mismatch..." << endl;
        cout << "        Shape: " << shape.GetNbinsX() << endl;
        cout << "        ACorr: " << autocorr.GetNbinsX() << endl;
        return kFALSE;
    }

    const TAxis &axe = *shape.GetXaxis();

    const Int_t first = axe.GetFirst()/weightsperbin;
    const Int_t last  = axe.GetLast() /weightsperbin;

    const Int_t width = last-first;

    cout << "Range:  " << first << " <= bin < " << last << endl;
    cout << "Window: " << width << endl;
    cout << "W/Bin:  " << weightsperbin << endl;

    // ---------------------------------------------

    const Float_t sum = shape.Integral(first*weightsperbin, last*weightsperbin-1)/weightsperbin;
    shape.Scale(1./sum);

    cout << "Sum:    " << sum << endl;

//    TGraph gr(&shape);
//    TSpline5 val("Signal", &gr);

    // FIXME: DELETE!!!
    TH1 &derivative = *static_cast<TH1*>(shape.Clone());
    derivative.Reset();

    for (int i=0; i<derivative.GetNbinsX(); i++)
    {
//       const Float_t x = derivative.GetBinCenter(i+1);
//       derivative.SetBinContent(i+1, val.Derivative(x));

        const Float_t binm = shape.GetBinContent(i+1-1);
        const Float_t binp = shape.GetBinContent(i+1+1);

        const Float_t der = (binp-binm)/2./shape.GetBinWidth(1);

        derivative.SetBinContent(i+1, der);

        if (derivative.InheritsFrom(TProfile::Class()))
            static_cast<TProfile&>(derivative).SetBinEntries(i+1,1);
    }

    // ---------------------------------------------

    TMatrixD B(width, width);
    for (Int_t i=0; i<width; i++)
        for (Int_t j=0; j<width; j++)
            B[i][j]=autocorr.GetBinContent(i+1/*first*/, j+1/*first*/);

    const TMatrixD Binv(TMatrixD::kInverted, B);

    // ---------------------------------------------

    weightsamp.Set(width*weightsperbin);
    weightstime.Set(width*weightsperbin);

    for (Int_t i=0; i<weightsperbin; i++)
    {
        TMatrixD g(width, 1);
        TMatrixD d(width, 1);

        for (Int_t bin=0; bin<width; bin++)
        {
            const Int_t idx = weightsperbin*(bin+first) + i;

            g[bin][0]=shape.GetBinContent(idx+1);
            d[bin][0]=derivative.GetBinContent(idx+1);
        }

        const TMatrixD gT(TMatrixD::kTransposed, g);
        const TMatrixD dT(TMatrixD::kTransposed, d);

        const TMatrixD denom  = (gT*(Binv*g))*(dT*(Binv*d)) - (dT*(Binv*g))*(dT*(Binv*g));

        if (denom[0][0]==0)
        {
            cout << "ERROR - Division by zero: denom[0][0]==0 for i=" << i << "." << endl;
            return kFALSE;
        }

        const TMatrixD w_amp  = (dT*(Binv*d))*(gT*Binv) - (gT*(Binv*d))*(dT*Binv);
        const TMatrixD w_time = (gT*(Binv*g))*(dT*Binv) - (gT*(Binv*d))*(gT*Binv);

        for (Int_t bin=0; bin<width; bin++)
        {
            const Int_t idx = weightsperbin*bin + i;

            weightsamp[idx]  = w_amp [0][bin]/denom[0][0];
            weightstime[idx] = w_time[0][bin]/denom[0][0];
        }
    }

    return kTRUE;
}

void MExtralgoDigitalFilter::CalculateWeights2(TH1F &shape, const TH2F &autocorr, TArrayF &weightsamp, TArrayF &weightstime, Int_t wpb)
{
    const Int_t weightsperbin = wpb<=0?shape.GetNbinsX()/autocorr.GetNbinsX():wpb;

    if (wpb<=0 && weightsperbin*autocorr.GetNbinsX()!=shape.GetNbinsX())
    {
        cout << "ERROR - Number of bins mismatch..." << endl;
        cout << "        Shape: " << shape.GetNbinsX() << endl;
        cout << "        ACorr: " << autocorr.GetNbinsX() << endl;
        return;
    }

    const TAxis &axe = *shape.GetXaxis();

    const Int_t first = axe.GetFirst()/weightsperbin;
    const Int_t last  = axe.GetLast() /weightsperbin;

    const Int_t width = last-first;

    cout << "Range:  " << first << " <= bin < " << last << endl;
    cout << "Window: " << width << endl;
    cout << "W/Bin:  " << weightsperbin << endl;

    // ---------------------------------------------

    const Float_t sum = shape.Integral(first*weightsperbin, last*weightsperbin-1)/weightsperbin;
    shape.Scale(1./sum);

    TGraph gr(&shape);
    TSpline5 val("Signal", &gr);

    TH1F derivative(shape);
    derivative.Reset();

    for (int i=0; i<derivative.GetNbinsX(); i++)
    {
        const Float_t x = derivative.GetBinCenter(i+1);
        derivative.SetBinContent(i+1, val.Derivative(x));

     /*
        const Float_t binm = shape.GetBinContent(i+1-1);
        const Float_t binp = shape.GetBinContent(i+1+1);

        const Float_t der = (binp-binm)/2./shape.GetBinWidth(1);

        derivative.SetBinContent(i+1, der);
      */
    }

    // ---------------------------------------------

    TMatrixD B(width, width);
    for (Int_t i=0; i<width; i++)
        for (Int_t j=0; j<width; j++)
            B[i][j]=autocorr.GetBinContent(i+first, j+first);
    B.Invert();

    // ---------------------------------------------

    weightsamp.Set(width*weightsperbin);
    weightstime.Set(width*weightsperbin);

    for (Int_t i=0; i<weightsperbin; i++)
    {
        TMatrixD g(width, 1);
        TMatrixD d(width, 1);

        for (Int_t bin=0; bin<width; bin++)
        {
            const Int_t idx = weightsperbin*(bin+first) + i;

            g[bin][0]=shape.GetBinContent(idx+1);
            d[bin][0]=derivative.GetBinContent(idx+1);
        }

        const TMatrixD gT(TMatrixD::kTransposed, g);
        const TMatrixD dT(TMatrixD::kTransposed, d);

        const TMatrixD denom  = (gT*(B*g))*(dT*(B*d)) - (dT*(B*g))*(dT*(B*g));

        const TMatrixD w_amp  = (dT*(B*d))*(gT*B) - (gT*(B*d))*(dT*B);
        const TMatrixD w_time = (gT*(B*g))*(dT*B) - (gT*(B*d))*(gT*B);

        for (Int_t bin=0; bin<width; bin++)
        {
            const Int_t idx = weightsperbin*bin + i;

            weightsamp[idx]  = w_amp [0][bin]/denom[0][0];
            weightstime[idx] = w_time[0][bin]/denom[0][0];
        }
    }
}
/*
Int_t start = fBinningResolutionHiGain*(fSignalStartBinHiGain + 1);
for (Int_t i = -fBinningResolutionHiGain/2+1; i<=fBinningResolutionHiGain/2; i++)
{
    // i = -4...5
    for (Int_t count=0; count < fWindowSizeHiGain; count++)
    {
        // count=0: pos=10*(start+0) + 10 + i
        // count=1: pos=10*(start+1) + 10 + i
        // count=2: pos=10*(start+2) + 10 + i
        // count=3: pos=10*(start+3) + 10 + i
    }

    for (Int_t count=0; count < fWindowSizeHiGain; count++)
    {
        // count=0: pos = 10*0 + 5 +i-1
        // count=1: pos = 10*1 + 5 +i-1
        // count=2: pos = 10*2 + 5 +i-1
        // count=3: pos = 10*3 + 5 +i-1
    }
}

Int_t start = fBinningResolutionLoGain*fSignalStartBinLoGain + fBinningResolutionLoGain/2;
for (Int_t i = -fBinningResolutionLoGain/2+1; i<=fBinningResolutionLoGain/2; i++)
{
    // i=-4..5
    for (Int_t count=0; count < fWindowSizeLoGain; count++)
    {
        // count=0: pos = 10*(start+0) + 5 + i
        // count=1: pos = 10*(start+1) + 5 + i
        // count=2: pos = 10*(start+2) + 5 + i
        // count=3: pos = 10*(start+3) + 5 + i
    }

    for (Int_t count=0; count < fWindowSizeLoGain; count++)
    {
        // count=0: pos = 10*0 + 5 +i-1
        // count=1: pos = 10*1 + 5 +i-1
        // count=2: pos = 10*2 + 5 +i-1
        // count=3: pos = 10*3 + 5 +i-1
    }
}
*/
