#include "QTruncatedFit.hh"
#include "Math/Factory.h"
#include <iostream>
#include "QBaseType.hh"
#include "TMath.h"

QObjectImp(QTruncatedFit);

using namespace Diana;
using std::cout;
using std::endl;

const QVector* QTruncatedFit::fTemplate = 0;
const QVector* QTruncatedFit::fPulse = 0;
const QDouble* QTruncatedFit::fThreshold=0;
const ROOT::Math::Functor* QTruncatedFit::FitFunc=0;

QTruncatedFit::QTruncatedFit()
{
    Clear();
}


QTruncatedFit::QTruncatedFit(const QVector& Template ,
                             QDouble Threshold){    
    Clear();
    Initialize(Template,Threshold);
}


void QTruncatedFit::Initialize(const QVector& Template ,
                               QDouble Threshold){
    // create minimizer giving a name and a name (optionally) for the specific
    // algorithm
    // possible choices are:
    //     minName                  algoName
    // Minuit /Minuit2             Migrad, Simplex,Combined,Scan  (default is Migrad)
    //  Minuit2                     Fumili2
    //  Fumili
    //  GSLMultiMin                ConjugateFR, ConjugatePR, BFGS,
    //                              BFGS2, SteepestDescent
    //  GSLMultiFit
    //   GSLSimAn
    //   Genetic
    
    fitter = ROOT::Math::Factory::CreateMinimizer("Minuit2", "");
    
    // set tolerance , etc...
    fitter->SetMaxFunctionCalls(1000000); // for Minuit/Minuit2
    fitter->SetMaxIterations(10000);  // for GSL
    fitter->SetTolerance(0.001);
    fitter->SetPrintLevel(-1);
    
    //set FitFunction
    FitFunc= new const ROOT::Math::Functor(QTruncatedFit::RMSCalc,3);
    fitter->SetFunction(*FitFunc);
    
    //Prepare Template
    Template_manip = NormalizeTemplate(Template);
    fTemplate = &Template_manip;
    //Set Threshold
    fThreshold = &Threshold;
}


QError QTruncatedFit::Fit(const QPulse& Pulse, 
                          const int& pulse_trigger_position){
    
    BufferClear();
    
    QError err = QERR_SUCCESS;
    
    //Get Pulse
    Pulse_manip =Pulse.GetSamples();
    
    //Calculate Baseline
    PBaseline=0.;
    int maxidx = 0.75*pulse_trigger_position;
    for(int i =0;i<maxidx;i++){
        PBaseline+=Pulse_manip[i];
    }
    PBaseline/=maxidx;
    
    //Calculate Number of degrees of freedom
    size_t oldidx=0;
    size_t first_decay=0;
    n_freedom=0;
    for (size_t i=0;i<Pulse_manip.Size();i++) {
        if(Pulse_manip[i]-PBaseline<=*fThreshold){
                n_freedom++;
                if((i-oldidx)>1 and first_decay==0){
                    first_decay=i;
                }
                oldidx=i;
        }
    }
    
    //Calculate Amplitude with best SNR available point
    amp_exp=(double)Pulse_manip[first_decay]/(double)Template_manip[first_decay];
    
    //Calculate Expected Jitter
    if(TMaxIndex==-1){
        TMaxIndex=Template_manip.GetMaxIndex();
    }
    t_exp = (double)Pulse_manip.GetMaxIndex()-(double)TMaxIndex;

    //Copy pulse for manipulation
    fPulse = &Pulse_manip;
    if(!fPulse) return QERR_UNKNOWN_ERR;

    //Calculate Expected Amplitude
    amp_exp=(double)TMath::Max(amp_exp,TMath::Abs(fPulse->GetMax()-PBaseline));
    
    //Set Fit Parameters
    double step[2] = {amp_exp*0.001,t_exp*0.001};
    double variable[3] = { amp_exp,t_exp,PBaseline};
    
    // Set the free variables to be minimized !
    fitter->SetVariable(0,"A",variable[0], step[0]);
    fitter->SetVariable(1,"t",variable[1], step[1]);
    fitter->SetFixedVariable(2,"O",variable[2]);
    
    //perform fit    
    fitter->Minimize();
    
    //Get Fit Results
    fAmpl = fitter->X()[0];
    fJitter = fitter->X()[1];
    fOffset = fitter->X()[2];
    RMS = TMath::Sqrt(fitter->MinValue()/n_freedom);
    
    fFittedPulse =fAmpl * Template_manip + fOffset;
    fFittedPulse = fFittedPulse.ShiftReal(fJitter);
    
    fTrunc= *fThreshold+PBaseline;
    
    return err;

}

double QTruncatedFit::RMSCalc(const double *par) {
    //minimisation function computing the sum of squares of residuals
    
    double rms = 0;
    
    QDouble a;
    QVector tmpl=par[0] * (*(fTemplate))+par[2];
    tmpl=tmpl.ShiftReal(par[1]);
    
    QVector ptemp=*(fPulse);

    for (size_t i=0;i<tmpl.Size();i++) {
        if(ptemp[i]-par[2]<=*fThreshold){
                a=(ptemp[i]-tmpl[i]);
                rms+=a*a;
        }
    }
    return rms;
}

QVector QTruncatedFit::NormalizeTemplate(const QVector& vec)
{
    QDouble TBaseline=0;
    QVector ret = vec;
    TMaxIndex=ret.GetMaxIndex();
    int cc = 0;
    for(size_t i =0;i<0.6*TMaxIndex;i++){
        TBaseline+=ret[i];
	    cc++;
    }
    TBaseline/=cc;
    ret-=TBaseline;
    ret /= ret.GetMax();
    return ret;
}