#include "GenQPDF.h"
#include "TMath.h"
#include "TFile.h"
#include "TCanvas.h"
#include "TLegend.h"
#include "TLine.h"
#include "TTree.h"
#include "TH1.h"
#include "TH2.h"
#include "TF1.h"
#include "TGraph.h"
#include "TLegend.h"
#include "TLegendEntry.h"
#include "TGraphSmooth.h"
#include "TString.h"
#include "TStopwatch.h"
#include "Minuit2/FCNBase.h"
#include "SniperKernel/AlgFactory.h"
#include "Event/RecHeader.h"
#include "Event/CalibHeader.h"
#include "Geometry/RecGeomSvc.h"
#include "Identifier/Identifier.h"
#include "Identifier/CdID.h"
#include <boost/filesystem.hpp>
#include "RootWriter/RootWriter.h"
#include "PMTCalibSvc/PMTCalibSvc.h"

#include "DataRegistritionSvc/DataRegistritionSvc.h"
#include "BufferMemMgr/IDataMemMgr.h"
#include "EvtNavigator/NavBuffer.h"
#include <map>
#include <fstream>
#include <vector>
#include <string>
#include <sstream>
#include "TAxis.h"
#include "TROOT.h"
#include <Math/Functor.h>
#include <Math/OneDimFunctionAdapter.h>
#include <functional>
#include "Math/Integrator.h"

namespace fs = boost::filesystem;

DECLARE_ALGORITHM(GenQPDF);

GenQPDF::GenQPDF(const std::string& name)
:   AlgBase(name)
{
    m_iEvt = -1.;
    PMTID = 0;
    TotalPMT = 17612;
    qThres = 0.125;
    aThres = 0.1;
    MoveL = 5;
    high_mu = 0.5;

    spe_bin_width = 0.05;
    spebina = -2.0;
    spebinb = 18.;
    spebin_num = (spebinb-spebina)/spe_bin_width;
    startBin = 1;
    endBin = 1;

    SPEGain = 1.;
    SPESgima = 0.3;

    //////////////
    // run options
    peRange = 1;
    pdfNSample = 1E7;
    enableHighMuFit = false;
    enableUserSPEs = false;
    userSPEFile = "";
    jobA = 0;
    jobB = 1;
    runmode = 1;
    outdir = "Test";

    declProp("jobA", jobA = 0);
    declProp("jobB", jobB = 1);
    declProp("peRange", peRange = 1);
    declProp("runmode", runmode = 1);
    declProp("enableHighMuFit", enableHighMuFit = false);
    declProp("enableUserSPEs", enableUserSPEs = false);
    declProp("userSPEFile", userSPEFile);
    declProp("outdir", outdir = "Test");

}

GenQPDF::~GenQPDF()
{
    std::vector< TH1D* >::iterator hIterEnd = HSPEs.end();
    std::vector< TGraph* >::iterator GraphIterEnd = GrSPEs.end();
    hIter = HSPEs.begin();
    GraphIter = GrSPEs.begin();
    while(hIter!=hIterEnd) {delete *hIter;*hIter = NULL; ++hIter;}
    while(GraphIter!=GraphIterEnd) {delete *GraphIter;*GraphIter=NULL;++GraphIter;}

    hIterEnd = HNPEs.end();
    hIter = HNPEs.begin();
    while(hIter!=hIterEnd) {delete *hIter;*hIter = NULL; ++hIter;}

}

class  PrdSPEs {
    public:
    PrdSPEs(TGraph* SPEs_):grSPEs(SPEs_){}
    PrdSPEs(){}
    void SetSpec(TGraph* SPEs_) {
        grSPEs = SPEs_;
    }

    Double_t operator()(Double_t* x, Double_t* par) {
        double result = grSPEs->Eval(x[0]);
        if(result>1e-16) return grSPEs->Eval(x[0]);
        else return 1e-16;
    }
    private:
    TGraph* grSPEs;
};

bool GenQPDF::initialize()
{
    SniperPtr<PMTCalibSvc> calSvc(getParent(), "PMTCalibSvc");
    if (calSvc.invalid()) {
        LogError << "Can't Locate  PMTCalibSvc."
                 << std::endl;
        return false;
    }

    hChi2_ndf = new TH1F("hChi2_ndf", "hChi2_ndf", 400, 0, 5);
    hMu = new TH1F("hMu", "hMu", 200, 0, 2);
    hGain_Smooth = new TH1F("hGain_Smooth", "hGain_Smooth", 200, 0.8, 1.2);
    hGain_Hist = new TH1F("hGain_Hist", "hGain_Hist", 200, 0.8, 1.2);
    hMu_Compare = new TH1F("hMu_Compare", "hMu_Compare", 1000, 0, 10);
    ///////////////////////
    // Loading SPE spectrum
    ///////////////////////

    if((calSvc->getRelativeDE()).at(0)>1e-8) PDE = calSvc->getRelativeDE();
    LogDebug<<"PDE[0]:"<<PDE[0]<<std::endl;

    if((calSvc->getDarkRate()).at(0)>1e-8) DC = calSvc->getDarkRate();
    LogDebug<<"DC[0]:"<<DC[0]<<std::endl;

    if(enableUserSPEs) {
        TFile* SPEsFile = TFile::Open(userSPEFile.c_str(), "READ");
        for(int i=0;i<TotalPMT;i++) {
            TH1D* spe_temp = (TH1D*)SPEsFile->Get(Form("ch%d_charge_spec", i));
            HSPEs.push_back(spe_temp);
        }
    }
    else HSPEs = calSvc->getChargeSpec();

    HSPEs[0]->Rebin(1);
    double bin_width_temp = HSPEs[0]->GetBinWidth(1);
    int nbin_temp = HSPEs[0]->GetNbinsX();

    SetSPEsHistPara(nbin_temp, HSPEs[0]->GetBinCenter(1) - bin_width_temp/2., HSPEs[0]->GetBinCenter(nbin_temp) + bin_width_temp/2., bin_width_temp);
    PrintHistInfo();

    LogInfo<<"Total number of PMTs:"<<TotalPMT<<std::endl;

    startBin = HSPEs[0]->FindBin(qThres);
    endBin = HSPEs[0]->FindBin(spebinb);

    for(int i=0;i<TotalPMT;i++) {
        if(i>0) HSPEs[i]->Rebin(1);
        double EntrySPEs = HSPEs[i]->Integral(startBin, endBin);
        HSPEs[i]->Scale(1./EntrySPEs);
    }
    ///////////////////////////
    // Loading high mu spectrum
    ///////////////////////////
    if(enableHighMuFit) {
        for(int i=0;i<TotalPMT;i++) {
            DC[i] *= 1250.;
            DC[i] /= 1.0e9;
        }
        TFile* NPEsFile = TFile::Open("/junofs/users/zhangxt/20inch/rec/deconvolution/runs/20191009samplephy/Laser/center/photon_110000/user_calibCorr.root", "READ");
        for(int i=0;i<TotalPMT;i++) {
            TH1D* npes_temp = (TH1D*)NPEsFile->Get(Form("ch%d_charge_spec", i));
            npes_temp->SetName(Form("ch%d_highMu_charge_spec", i));
            HNPEs.push_back(npes_temp);
            double N = npes_temp->Integral(startBin, endBin);
            double mu_cal = -log((19800.-N)/19800.) - DC[i];
            hMu->Fill(mu_cal);
            hMu_Compare->Fill(mu_cal/PDE[i]);
        }
    }

    fcn = new MyFCN(this);
    qminuit = ROOT::Math::Factory::CreateMinimizer("Minuit2", "Migrad");

    m_PDFOutput = new TFile(Form("%s/NPEPDF/SPEsQPDF_%d.root", outdir.c_str(), jobA), "RECREATE");
    hMu->Write();
    hMu_Compare->Write();
    //if(runmode == 2) m_PDFInput = TFile::Open("NPEQpdf.root", "READ");
    //m_FitOutput = new TFile("FitResultsJ19.root", "RECREATE");

    cHSpes = new TCanvas("cHSpes","Graph Example",200,10,700,500);
    cHSpes->SetLogy();
    plotName = Form("%s/PHOLog/QSpecFitPlots_run%d", outdir.c_str(), jobA);
    if(enableHighMuFit) cHSpes->Print(plotName+".pdf[", "pdf");

    PMTID = jobA;

    LogInfo  << objName()
        << "   initialized successfully"
        << std::endl;

    return true;
}

bool GenQPDF::execute()
{
    m_iEvt++;

    // std::ofstream fitResultOut;
//if(enableHighMuFit) fitResultOut.open(Form("%s/FITLog/fitResult_%d.txt", outdir.c_str(), jobA), std::ios::out);
    double gain_smooth = 0., gain_hist = 0., gain_fit = 0.;

    for(PMTID=jobA;PMTID<jobB&&PMTID<TotalPMT;PMTID++) {

        LogDebug << " ---------------------------------PMT channel:" << PMTID <<std::endl;
        TH1F* hSpec = new TH1F("hSpec", "hSpec", spebin_num, spebina, spebinb);

        int bintemp1 = HSPEs[0]->FindBin(qThres);
        int bintemp2 = HSPEs[0]->FindBin(spebinb);
        for(int i=bintemp1;i<=bintemp2;i++) {
            hSpec->SetBinContent(i, HSPEs[PMTID]->GetBinContent(i));
        }
        std::cout<<hSpec->GetSumOfWeights()<<std::endl;

        PrintHistInfo();

        nPEQPDF = new std::vector< TH1F* >;

        /*
        int endPos = spebin_num;
        PrimaryFit(hSpec, endPos);

        ROOT::Math::IntegratorOneDimOptions::SetDefaultRelTolerance(1.E-6);
        PrdSPEs* PrdSPEs_PMT = new PrdSPEs(GrSPEs[PMTID-jobA]);
        TF1* funcPrdSPEs_PMT = new TF1("funcPrdSPEs_PMT", PrdSPEs_PMT, qThres, hSpec->GetBinCenter(endPos), 0, "PrdSPEs");
        LogInfo<<"Upper charge limit of smooth SPEs:"<<hSpec->GetBinCenter(endPos)<<std::endl;
        */
        double* RecordQ_temp = (double*)malloc(sizeof(double)*pdfNSample);
        for(int nPE = 0; nPE < peRange; nPE ++) {
            LogInfo<<"---------------------------------:"<<nPE<<std::endl;

            double* RecordQ = (double*)malloc(sizeof(double)*pdfNSample);

            TH1F* htemp = new TH1F(Form("channel%d_nPE%d_qpdf", PMTID, nPE+1), Form("channel%d_nPE%d_qpdf", PMTID, nPE+1), 200./spe_bin_width, 0, 200);
            nPEQPDF->push_back(htemp);
            for(int i=0;i<pdfNSample;i++) {
                //double temp = funcPrdSPEs_PMT->GetRandom();
                double temp = hSpec->GetRandom();

                if(nPE==0) *(RecordQ + i) = temp;
                else *(RecordQ + i) = temp + *(RecordQ_temp + i);

                *(RecordQ_temp + i) = *(RecordQ + i);

                nPEQPDF->at(nPE)->Fill(*(RecordQ + i));
            }
            free(RecordQ);
            double entry_temp = nPEQPDF->at(nPE)->GetSumOfWeights();
            nPEQPDF->at(nPE)->Scale(1./entry_temp);
            if(runmode == 2) {
                m_PDFOutput->cd();
                nPEQPDF->at(nPE)->Write();
            }

            if(nPE==0) {
                gain_smooth = nPEQPDF->at(nPE)->GetMean();

                /*
                int bintemp1 = HSPEs[0]->FindBin(qThres);
                TH1F* hSpec = (TH1F*)HSPEs[PMTID]->Clone("hSpec");
                for(int i=1;i<bintemp1;i++) {
                    hSpec->SetBinContent(i, 0.);
                }*/
                gain_hist = hSpec->GetMean();
                hGain_Hist->Fill(gain_hist);

                hSpec->Fit("gaus", "MREQ0", "", 0.5, 1.5);
                TF1* GausTemp = (TF1*)hSpec->GetFunction("gaus");
                gain_fit = GausTemp->GetParameter(1);
                SPEGain = gain_fit;
                SPESgima = GausTemp->GetParameter(2);

                delete GausTemp;
            }

        }
        free(RecordQ_temp);

        //delete PrdSPEs_PMT;
        //delete funcPrdSPEs_PMT;

        if(enableHighMuFit) {
            ROOT::Math::Functor recfunc(*fcn, 2);
            qminuit->SetFunction(recfunc);
            qminuit->SetMaxFunctionCalls(1E6);
            qminuit->SetMaxIterations(1E6);
            qminuit->SetTolerance(1e-6);

            double Entry = HNPEs[PMTID]->Integral(startBin, endBin);
            qminuit->SetVariable(0,"mu", high_mu, 0.001);
            qminuit->SetVariable(1,"evts", Entry, 1.);

            qminuit->SetUpperLimitedVariable(0, "mu", high_mu, 0.001, 200);
            qminuit->SetLowerLimitedVariable(0, "mu", high_mu, 0.001, 1.e-12);

            qminuit->SetFixedVariable(1, "evts", Entry);

            int ief = qminuit->Minimize();
            LogDebug << "The ief is: " << ief << std::endl;
            const double *xs = qminuit->X();
            const double *xe = qminuit->Errors();
            double mu_fit = xs[0];
            double N_fit = xs[1];
            double mu_fit_err = xe[0];
            double chi2_ndf = m_Calculate_Chi2(mu_fit, N_fit);

            mu_fit -= DC[PMTID];
            double mu_cal = -log((19800.-N_fit)/19800.) - DC[PMTID];

            // fitResultOut<<PMTID<<'\t'<<chi2_ndf<<'\t'<<mu_fit<<'\t'<<mu_fit_err<<'\t'<<mu_cal<<'\t'<<mu_fit/PDE[PMTID]<<'\t'<<gain_smooth<<'\t'<<gain_hist<<'\t'<<gain_fit<<std::endl;
            if(chi2_ndf>2.) LogDebug<<"Found largeChi2 !"<<std::endl;

            LogDebug<<"==========================================================="<<std::endl;
            LogDebug<<"The fitted mu is "<<mu_fit<<'\t'<<mu_cal<<'\t'<<mu_fit_err<<std::endl;
            LogDebug<<"The gain is "<<gain_smooth<<'\t'<<gain_hist<<'\t'<<gain_fit<<std::endl;
            LogDebug<<"The fitted evts is "<<N_fit<<std::endl;
            LogDebug<<"The fitted chi2/ndf is "<<chi2_ndf<<std::endl;
            LogDebug<<"==========================================================="<<std::endl;
            TH1F* FitResults = new TH1F(Form("FitQSpec_%d",PMTID), Form("FitQSpec_%d",PMTID), spebin_num, spebina, spebinb);
            double binwf = HNPEs[PMTID]->GetBinWidth(1)/nPEQPDF->at(0)->GetBinWidth(1);
            binwf = HNPEs[PMTID]->GetBinWidth(1);
            std::cout<<binwf<<std::endl;
            double Q1 = SPEGain;
            double S1 = SPESgima;
            int kRange = 8;
            for(int i=1;i<HNPEs[PMTID]->GetNbinsX();i++) {
                double X = HNPEs[PMTID]->GetBinCenter(i);
                double Y = 0.;
                for(int npe=1;npe<kRange;npe++) {
                    double kk = double(npe);
                    //double prob_temp = nPEQPDF->at(npe-1)->Interpolate(X);
                    double prob_temp =  TMath::Gaus(X, kk*Q1, sqrt(kk)*S1)/sqrt(2.*TMath::Pi()*kk*S1*S1);
                    if(prob_temp<1e-12) continue;
                    Y += TMath::Poisson(double(npe), mu_fit)*prob_temp*binwf;
                }
                Y *= 1./(1.-exp(-mu_fit));
                FitResults->SetBinContent(i, N_fit*Y);
            }
            /*
            m_FitOutput->cd();
            if(PMTID==2) {
                FitResults->Write();
                HNPEs[2]->Write();
            }*/
            if(runmode==1) {
                if(PMTID<100) {
                    cHSpes->cd();
                    cHSpes->Clear();
                    HNPEs[PMTID]->Draw();
                    HNPEs[PMTID]->SetTitle(Form("FitMu = %0.2f, Chi2/ndf = %0.2f", mu_fit, chi2_ndf));
                    HNPEs[PMTID]->GetXaxis()->SetRangeUser(-1, 10);
                    FitResults->SetLineColor(2);
                    FitResults->SetLineWidth(1);
                    FitResults->Draw("SAME");

                    cHSpes->Print(plotName+".pdf","pdf");
                }
            }
            delete FitResults;

            qminuit->Clear();

        }
        delete hSpec;
        std::vector< TH1F* >::iterator hIterTemp = nPEQPDF->begin();
        while(hIterTemp!=nPEQPDF->end()) {delete *hIterTemp;*hIterTemp = NULL; ++hIterTemp;}
        delete nPEQPDF;

    }
    return true;

}

double GenQPDF::m_Calculate_Chi2(double mu, double evtnum)
{
    double Chi2 = 0.;
    double binwf = HNPEs[PMTID]->GetBinWidth(1)/nPEQPDF->at(0)->GetBinWidth(1);
    binwf = HNPEs[PMTID]->GetBinWidth(1);
    double ndf = 0.;
    double Q1 = SPEGain;
    double S1 = SPESgima;
    int kRange = 8;
    for(int i=startBin;i<HNPEs[PMTID]->GetNbinsX();i++) {
        double X = HNPEs[PMTID]->GetBinCenter(i);
        double y = HNPEs[PMTID]->GetBinContent(i);
        double Y = 0.;
        if(y<10.) continue;
        for(int npe=1;npe<kRange;npe++) {
            double kk = double(npe);
            //double prob_temp = nPEQPDF->at(npe-1)->Interpolate(X);
            double prob_temp =  TMath::Gaus(X, kk*Q1, sqrt(kk)*S1)/sqrt(2.*TMath::Pi()*kk*S1*S1);
            if(prob_temp<1e-12) continue;
            Y += TMath::Poisson(double(npe), mu)*prob_temp*binwf;
        }
        Y *= 1./(1.-exp(-mu ));
        Chi2 += TMath::Power(y - evtnum*Y, 2.)/y;
        ndf ++;
    }
    //std::cout<<ndf<<'\t'<<Chi2/ndf<<std::endl;
    return Chi2/(ndf-1.);
}

bool GenQPDF::PrimaryFit(TH1F *hSpec_, int& endPos_)
{

    TH1F* hSpec = (TH1F*)hSpec_->Clone("hSpec");
    int mark = 0;
    for(int i=hSpec->FindBin(1.);i<=spebin_num;i++) {
        if(hSpec->GetBinContent(i)==0) mark++;
        else mark = 0;
        if(mark>=5) {endPos_ = i; break;}
    }
    /////////////////
    // Smooth process
    int Bb = hSpec->FindBin(qThres);
    int Be = hSpec->FindBin(spebinb) - MoveL;
    const int MoveG = Be - Bb;

    double subX[MoveG][MoveL];
    double subY[MoveG][MoveL];
    for(int i=0;i<MoveG;i++) {
        for(int j=0;j<MoveL;j++) {
            subX[i][j] = hSpec->GetBinCenter(Bb+i+j);
            subY[i][j] = hSpec->GetBinContent(Bb+i+j);
        }
    }

    TGraph* subGr[MoveG];
    TGraph* subGrSmooth[MoveG];
    TGraphSmooth* gs[MoveG];
    double xLimitA[MoveG];
    double xLimitB[MoveG];
    for(int i=0;i<MoveG;i++) {
        subGr[i] = new TGraph(MoveL, subX[i], subY[i]);
        subGr[i]->SetName(Form("subGr%d",i));
        gs[i] = new TGraphSmooth("normal");
        //subGrSmooth[i] = gs[i]->SmoothSuper(subGr[i],"",3);
        subGrSmooth[i] = gs[i]->SmoothLowess(subGr[i],"",0.2);

        subGrSmooth[i]->SetName(Form("subGrSmooth%d",i));
        xLimitA[i] = subGrSmooth[i]->GetX()[0];
        xLimitB[i] = subGrSmooth[i]->GetX()[MoveL-1];
    }

    double X[spebin_num];
    double Y[spebin_num];
    double AvgN[spebin_num];
    for(int i = 0;i<spebin_num;i++) {
        AvgN[i] = 0.;
        Y[i] = 0.;
        X[i] = hSpec->GetBinCenter(i+1);
        for(int s=0;s<MoveG;s++) {
            if(X[i]>xLimitA[s]&&X[i]<xLimitB[s]) {Y[i] += subGrSmooth[s]->Eval(X[i]); AvgN[i]++;}
        }
        if(AvgN[i]>1e-8) Y[i] /= AvgN[i];
    }

    TGraph* SPEsSmooth = new TGraph(endPos_, X, Y);
    SPEsSmooth->SetName(Form("channel%d_SPEsSmooth", PMTID));
    GrSPEs.push_back(SPEsSmooth);

    m_PDFOutput->cd();
    SPEsSmooth->Write();

    delete hSpec;
    return true;
}

bool GenQPDF::finalize()
{
    cHSpes->Print(plotName+".pdf]", "pdf");

    if(runmode == 2) {
        m_PDFOutput->cd();
        for(int pmtid=jobA;pmtid<jobB&&PMTID<TotalPMT;pmtid++) {
            HSPEs[pmtid]->Write();
            if(enableHighMuFit) HNPEs[pmtid]->Write();
        }
        m_PDFOutput->Close();
    }
    LogInfo  << objName()
        << "   finalized successfully"
        << std::endl;
    return true;
}


#include "Kernels/TimeBase.h"
#include "Kernels/Time.h"

#ifndef NDEBUG
extern long long libxsmm_num_total_flops;
#endif

#include <Kernels/common.hpp>
#include <Kernels/denseMatrixOps.hpp>

#include <cstring>
#include <cassert>
#include <stdint.h>
#include <omp.h>
#include <Eigen/Dense>

#include "Equations/poroelastic/Model/PoroelasticSetup.h"

#include <yateto.h>

GENERATE_HAS_MEMBER(ET)
GENERATE_HAS_MEMBER(sourceMatrix)

seissol::kernels::TimeBase::TimeBase(){
  m_derivativesOffsets[0] = 0;
  for (int order = 0; order < CONVERGENCE_ORDER; ++order) {
    if (order > 0) {
      m_derivativesOffsets[order] = tensor::dQ::size(order-1) + m_derivativesOffsets[order-1];
    }
  }
}

void seissol::kernels::Time::setHostGlobalData(GlobalData const* global) {
  for (int n = 0; n < CONVERGENCE_ORDER; ++n) {
    if (n > 0) {
      for (int d = 0; d < 3; ++d) {
        m_krnlPrototype.kDivMTSub(d,n) = init::kDivMTSub::Values[tensor::kDivMTSub::index(d,n)];
      }
    }
    m_krnlPrototype.selectModes(n) = init::selectModes::Values[tensor::selectModes::index(n)];
  }
  for (int k = 0; k < NUMBER_OF_QUANTITIES; k++) {
    m_krnlPrototype.selectQuantity(k) = init::selectQuantity::Values[tensor::selectQuantity::index(k)];
    m_krnlPrototype.selectQuantityG(k) = init::selectQuantityG::Values[tensor::selectQuantityG::index(k)];
  }
  m_krnlPrototype.timeInt = init::timeInt::Values;
  m_krnlPrototype.wHat = init::wHat::Values;
}

void seissol::kernels::Time::setGlobalData(const CompoundGlobalData& global) {
  setHostGlobalData(global.onHost);

#ifdef ACL_DEVICE
  logError() << "Poroelasticity does not work on GPUs.";
#endif
}

void seissol::kernels::Time::executeSTP( double                      i_timeStepWidth,
                                         LocalData&                  data,
                                         real                        o_timeIntegrated[tensor::I::size()],
                                         real*                       stp )

{
  alignas(PAGESIZE_STACK) real stpRhs[tensor::spaceTimePredictorRhs::size()];
  assert( ((uintptr_t)stp) % PAGESIZE_STACK  == 0);
  std::fill(std::begin(stpRhs), std::end(stpRhs), 0);
  std::fill(stp, stp + tensor::spaceTimePredictor::size(), 0);
  kernel::spaceTimePredictor krnl = m_krnlPrototype;

  //libxsmm can not generate GEMMs with alpha!=1. As a workaround we multiply the
  //star matrices with dt before we execute the kernel.
  real A_values[init::star::size(0)];
  real B_values[init::star::size(1)];
  real C_values[init::star::size(2)];
  for (size_t i = 0; i < init::star::size(0); i++) {
    A_values[i] = i_timeStepWidth * data.localIntegration.starMatrices[0][i];
    B_values[i] = i_timeStepWidth * data.localIntegration.starMatrices[1][i];
    C_values[i] = i_timeStepWidth * data.localIntegration.starMatrices[2][i];
  }
  krnl.star(0) = A_values;
  krnl.star(1) = B_values;
  krnl.star(2) = C_values;

  //The matrix Zinv depends on the timestep
  //If the timestep is not as expected e.g. when approaching a sync point
  //we have to recalculate it
  if (i_timeStepWidth != data.localIntegration.specific.typicalTimeStepWidth) {
    auto sourceMatrix = init::ET::view::create(data.localIntegration.specific.sourceMatrix);
    real ZinvData[NUMBER_OF_QUANTITIES][CONVERGENCE_ORDER*CONVERGENCE_ORDER];
    model::zInvInitializerForLoop<0, NUMBER_OF_QUANTITIES, decltype(sourceMatrix)>(ZinvData, sourceMatrix, i_timeStepWidth);
    for (size_t i = 0; i < NUMBER_OF_QUANTITIES; i++) {
      krnl.Zinv(i) = ZinvData[i];
    }
  } else {
    for (size_t i = 0; i < NUMBER_OF_QUANTITIES; i++) {
      krnl.Zinv(i) = data.localIntegration.specific.Zinv[i];
    }
  }
  krnl.Gk = data.localIntegration.specific.G[10] * i_timeStepWidth;
  krnl.Gl = data.localIntegration.specific.G[11] * i_timeStepWidth;
  krnl.Gm = data.localIntegration.specific.G[12] * i_timeStepWidth;

  krnl.Q = const_cast<real*>(data.dofs);
  krnl.I = o_timeIntegrated;
  krnl.timestep = i_timeStepWidth;
  krnl.spaceTimePredictor = stp;
  krnl.spaceTimePredictorRhs = stpRhs;
  krnl.execute();
}

void seissol::kernels::Time::computeAder( double i_timeStepWidth,
                                          LocalData& data,
                                          LocalTmp& tmp,
                                          real o_timeIntegrated[tensor::I::size()],
                                          real* o_timeDerivatives,
                                          double startTime,
                                          bool updateDisplacement)
{
  /*
   * assert alignments.
   */
  assert( ((uintptr_t)data.dofs)              % ALIGNMENT == 0 );
  assert( ((uintptr_t)o_timeIntegrated )      % ALIGNMENT == 0 );
  assert( ((uintptr_t)o_timeDerivatives)      % ALIGNMENT == 0 || o_timeDerivatives == NULL );

  alignas(PAGESIZE_STACK) real temporaryBuffer[tensor::spaceTimePredictor::size()];
  real* stpBuffer = (o_timeDerivatives != nullptr) ? o_timeDerivatives : temporaryBuffer;
  executeSTP( i_timeStepWidth, data, o_timeIntegrated, stpBuffer );
}

void seissol::kernels::Time::flopsAder( unsigned int        &o_nonZeroFlops,
                                        unsigned int        &o_hardwareFlops ) {
  // reset flops
  o_nonZeroFlops = 0; o_hardwareFlops =0;

  o_nonZeroFlops = kernel::spaceTimePredictor::NonZeroFlops;
  o_hardwareFlops = kernel::spaceTimePredictor::HardwareFlops;
  //we multiply the star matrices with dt before we execute the kernel
  o_nonZeroFlops += 3*init::star::size(0);
  o_hardwareFlops += 3*init::star::size(0);
}

unsigned seissol::kernels::Time::bytesAder()
{
  unsigned reals = 0;

  // DOFs load, tDOFs load, tDOFs write
  reals += tensor::Q::size() + 2 * tensor::I::size();
  // star matrices, source matrix
  reals += yateto::computeFamilySize<tensor::star>();
  // Zinv
  reals += yateto::computeFamilySize<tensor::Zinv>();
  // G
  reals += 3;

  /// \todo incorporate derivatives

  return reals * sizeof(real);
}

void seissol::kernels::Time::computeIntegral( double                            i_expansionPoint,
                                              double                            i_integrationStart,
                                              double                            i_integrationEnd,
                                              const real*                       i_timeDerivatives,
                                              real                              o_timeIntegrated[tensor::I::size()])
{
  /*
   * assert alignments.
   */
  assert( ((uintptr_t)i_timeDerivatives)  % ALIGNMENT == 0 );
  assert( ((uintptr_t)o_timeIntegrated)   % ALIGNMENT == 0 );

  // assert that this is a forwared integration in time
  assert( i_integrationStart + (real) 1.E-10 > i_expansionPoint   );
  assert( i_integrationEnd                   > i_integrationStart );

  /*
   * compute time integral.
   */
  // compute lengths of integration intervals
  real l_deltaTLower = i_integrationStart - i_expansionPoint;
  real l_deltaTUpper = i_integrationEnd   - i_expansionPoint;

  // initialization of scalars in the taylor series expansion (0th term)
  real l_firstTerm  = (real) 1;
  real l_secondTerm = (real) 1;
  real l_factorial  = (real) 1;

  kernel::derivativeTaylorExpansion intKrnl;
  intKrnl.I = o_timeIntegrated;
  for (unsigned i = 0; i < yateto::numFamilyMembers<tensor::dQ>(); ++i) {
    intKrnl.dQ(i) = i_timeDerivatives + m_derivativesOffsets[i];
  }

  // iterate over time derivatives
  for(int der = 0; der < CONVERGENCE_ORDER; ++der ) {
    l_firstTerm  *= l_deltaTUpper;
    l_secondTerm *= l_deltaTLower;
    l_factorial  *= (real)(der+1);

    intKrnl.power  = l_firstTerm - l_secondTerm;
    intKrnl.power /= l_factorial;

    intKrnl.execute(der);
  }
}

void seissol::kernels::Time::computeTaylorExpansion( real         time,
                                                     real         expansionPoint,
                                                     real const*  timeDerivatives,
                                                     real         timeEvaluated[tensor::Q::size()] ) {
  /*
   * assert alignments.
   */
  assert( ((uintptr_t)timeDerivatives)  % ALIGNMENT == 0 );
  assert( ((uintptr_t)timeEvaluated)    % ALIGNMENT == 0 );

  // assert that this is a forward evaluation in time
  assert( time >= expansionPoint );

  real deltaT = time - expansionPoint;

  static_assert(tensor::I::size() == tensor::Q::size(), "Sizes of tensors I and Q must match");

  kernel::derivativeTaylorExpansion intKrnl;
  intKrnl.I = timeEvaluated;
  for (unsigned i = 0; i < yateto::numFamilyMembers<tensor::dQ>(); ++i) {
    intKrnl.dQ(i) = timeDerivatives + m_derivativesOffsets[i];
  }
  intKrnl.power = 1.0;

  // iterate over time derivatives
  for(int derivative = 0; derivative < CONVERGENCE_ORDER; ++derivative) {
    intKrnl.execute(derivative);
    intKrnl.power *= deltaT / real(derivative+1);
  }
}

void seissol::kernels::Time::flopsTaylorExpansion(long long& nonZeroFlops, long long& hardwareFlops) {
  // reset flops
  nonZeroFlops = 0; hardwareFlops = 0;

  // interate over derivatives
  for (unsigned der = 0; der < CONVERGENCE_ORDER; ++der) {
    nonZeroFlops  += kernel::derivativeTaylorExpansion::nonZeroFlops(der);
    hardwareFlops += kernel::derivativeTaylorExpansion::hardwareFlops(der);
  }
}


//  (C) Copyright Gennadiy Rozental 2011-2014.
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.
//
//!@file
//!@brief simple dataset size abstraction (can be infinite)
// ***************************************************************************

#ifndef BOOST_TEST_DATA_SIZE_HPP_102211GER
#define BOOST_TEST_DATA_SIZE_HPP_102211GER

// Boost.Test
#include <boost/test/data/config.hpp>

// STL
#include <iosfwd>

#include <boost/test/detail/suppress_warnings.hpp>

//____________________________________________________________________________//

namespace boost {
namespace unit_test {
namespace data {

// ************************************************************************** //
// **************                    size_t                    ************** //
// ************************************************************************** //

//! Utility for handling the size of a datasets
class size_t {
    struct dummy { void nonnull() {} };
    typedef void (dummy::*safe_bool)();
public:
    // Constructors
    size_t( std::size_t s = 0 )         : m_value( s ), m_infinity( false ) {}
    explicit        size_t( bool )      : m_value( 0 ), m_infinity( true ) {}
    template<typename T>
    size_t( T v )                       : m_value( static_cast<std::size_t>(v) ), m_infinity( false ) {}

    // Access methods
    std::size_t     value() const       { return m_value; }
    bool            is_inf() const      { return m_infinity; }
    operator        safe_bool() const   { return is_inf() || m_value != 0 ? &dummy::nonnull : 0; }

    // Unary operators
    data::size_t    operator--()        { if( !is_inf() ) m_value--; return *this; }
    data::size_t    operator--(int)     { data::size_t res(*this); if( !is_inf() ) m_value--; return res; }
    data::size_t    operator++()        { if( !is_inf() ) m_value++; return *this; }
    data::size_t    operator++(int)     { data::size_t res(*this); if( !is_inf() ) m_value++; return res; }

    // Binary operators
    data::size_t&   operator+=( std::size_t rhs ) { if( !is_inf() ) m_value += rhs; return *this; }
    data::size_t&   operator+=( data::size_t rhs )
    {
        if( !is_inf() ) {
            if( rhs.is_inf() )
                *this = rhs;
            else
                m_value += rhs.value();
        }
        return *this;
    }
    data::size_t&   operator-=( std::size_t rhs ) { if( !is_inf() ) m_value -= rhs; return *this; }
    data::size_t&   operator-=( data::size_t rhs )
    {
        if( !is_inf() ) {
            if( value() < rhs.value() )
                m_value = 0;
            else
                m_value -= rhs.value();
        }
        return *this;
    }

private:
    // Data members
    std::size_t     m_value;
    bool            m_infinity;
};

namespace { const data::size_t BOOST_TEST_DS_INFINITE_SIZE( true ); }

//____________________________________________________________________________//

// Binary operators
inline bool operator>(data::size_t lhs, std::size_t rhs)    { return lhs.is_inf()  || (lhs.value() > rhs); }
inline bool operator>(std::size_t lhs, data::size_t rhs)    { return !rhs.is_inf() && (lhs > rhs.value()); }
inline bool operator>(data::size_t lhs, data::size_t rhs)   { return lhs.is_inf() ^ rhs.is_inf() ? lhs.is_inf() : lhs.value() > rhs.value(); }

inline bool operator>=(data::size_t lhs, std::size_t rhs )  { return lhs.is_inf()  || (lhs.value() >= rhs); }
inline bool operator>=(std::size_t lhs, data::size_t rhs)   { return !rhs.is_inf() && (lhs >= rhs.value()); }
inline bool operator>=(data::size_t lhs, data::size_t rhs)  { return lhs.is_inf() ^ rhs.is_inf() ? lhs.is_inf() : lhs.value() >= rhs.value(); }

inline bool operator<(data::size_t lhs, std::size_t rhs)    { return !lhs.is_inf() && (lhs.value() < rhs); }
inline bool operator<(std::size_t lhs, data::size_t rhs)    { return rhs.is_inf()  || (lhs < rhs.value()); }
inline bool operator<(data::size_t lhs, data::size_t rhs)   { return lhs.is_inf() ^ rhs.is_inf() ? rhs.is_inf() : lhs.value() < rhs.value(); }

inline bool operator<=(data::size_t lhs, std::size_t rhs)   { return !lhs.is_inf() && (lhs.value() <= rhs); }
inline bool operator<=(std::size_t lhs, data::size_t rhs)   { return rhs.is_inf()  || (lhs <= rhs.value()); }
inline bool operator<=(data::size_t lhs, data::size_t rhs)  { return lhs.is_inf() ^ rhs.is_inf() ? rhs.is_inf() : lhs.value() <= rhs.value(); }

inline bool operator==(data::size_t lhs, std::size_t rhs)   { return !lhs.is_inf() && (lhs.value() == rhs); }
inline bool operator==(std::size_t lhs, data::size_t rhs)   { return !rhs.is_inf() && (lhs == rhs.value()); }
inline bool operator==(data::size_t lhs, data::size_t rhs)  { return !(lhs.is_inf() ^ rhs.is_inf()) && lhs.value() == rhs.value(); }

inline bool operator!=(data::size_t lhs, std::size_t rhs)   { return lhs.is_inf() || (lhs.value() != rhs); }
inline bool operator!=(std::size_t lhs, data::size_t rhs)   { return rhs.is_inf() || (lhs != rhs.value()); }
inline bool operator!=(data::size_t lhs, data::size_t rhs)  { return lhs.is_inf() ^ rhs.is_inf() || lhs.value() != rhs.value(); }

inline data::size_t operator+(data::size_t lhs, std::size_t rhs)  { return lhs.is_inf() ? lhs : data::size_t( lhs.value()+rhs ); }
inline data::size_t operator+(std::size_t lhs, data::size_t rhs)  { return rhs.is_inf() ? rhs : data::size_t( lhs+rhs.value() ); }
inline data::size_t operator+(data::size_t lhs, data::size_t rhs) { return lhs.is_inf() || rhs.is_inf() ? data::size_t(true) : data::size_t( lhs.value()+rhs.value() ); }

inline data::size_t operator*(data::size_t lhs, std::size_t rhs)  { return lhs.is_inf() ? lhs : data::size_t( lhs.value()*rhs ); }
inline data::size_t operator*(std::size_t lhs, data::size_t rhs)  { return rhs.is_inf() ? rhs : data::size_t( lhs*rhs.value() ); }
inline data::size_t operator*(data::size_t lhs, data::size_t rhs) { return lhs.is_inf() || rhs.is_inf() ? data::size_t(true) : data::size_t( lhs.value()*rhs.value() ); }

//____________________________________________________________________________//

template<typename CharT1, typename Tr>
inline std::basic_ostream<CharT1,Tr>&
operator<<( std::basic_ostream<CharT1,Tr>& os, data::size_t const& s )
{
    if( s.is_inf() )
        os << "infinity";
    else
        os << s.value();

    return os;
}

//____________________________________________________________________________//

} // namespace data
} // namespace unit_test
} // namespace boost

#include <boost/test/detail/enable_warnings.hpp>

#endif // BOOST_TEST_DATA_SIZE_HPP_102211GER


/*
 [auto_generated]
 boost/numeric/odeint/stepper/generation/generation_runge_kutta_cash_karp54_classic.hpp

 [begin_description]
 Enable the factory functions for the controller and the dense output of the
 Runge-Kutta-Cash-Karp 54 method with the classical implementation.
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_RUNGE_KUTTA_CASH_KARP54_CLASSIC_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_RUNGE_KUTTA_CASH_KARP54_CLASSIC_HPP_INCLUDED

#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54_classic.hpp>
#include <boost/numeric/odeint/stepper/generation/make_controlled.hpp>

namespace boost {
namespace numeric {
namespace odeint {

// Specializations for runge_kutta_cash_karp54
template< class State , class Value , class Deriv , class Time , class Algebra , class Operations , class Resize >
struct get_controller< runge_kutta_cash_karp54_classic< State , Value , Deriv , Time , Algebra , Operations , Resize > >
{
    typedef runge_kutta_cash_karp54_classic< State , Value , Deriv , Time , Algebra , Operations , Resize > stepper_type;
    typedef controlled_runge_kutta< stepper_type > type;
};

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_RUNGE_KUTTA_CASH_KARP54_CLASSIC_HPP_INCLUDED


#include <ros/ros.h>

#include <geometry_msgs/Point.h>
#include <boost/ref.hpp>
#include <visualization_msgs/Marker.h>
#include "eigen3/Eigen/Dense"

const std::string marker_ns = "ns";
const int mesh_id = 3;

visualization_msgs::Marker makeMarker(const Eigen::Vector3d& pt, const std::string& meshname)
{
  visualization_msgs::Marker marker;
  marker.action = visualization_msgs::Marker::ADD;
  marker.header.frame_id = "world";
  marker.header.stamp = ros::Time::now();
  marker.ns = marker_ns;
  marker.id = mesh_id;
  marker.type = visualization_msgs::Marker::MESH_RESOURCE;
  marker.pose.position.x = pt(0);
  marker.pose.position.y = pt(1);
  marker.pose.position.z = pt(2);
  marker.pose.orientation.x = 0.0;
  marker.pose.orientation.y = 0.0;
  marker.pose.orientation.z = 0.0;
  marker.pose.orientation.w = 1.0;
  marker.scale.x = 1.0;
  marker.scale.y = 1.0;
  marker.scale.z = 1.0;
  marker.color.r = 0.2f;
  marker.color.g = 0.2f;
  marker.color.b = 0.2f;
  marker.color.a = 1.0;
  marker.lifetime = ros::Duration();
  marker.mesh_resource = "file://" + meshname;
  return marker;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "mesh_tracking_test");
  ros::NodeHandle pnh ("~");

  std::string mesh_file;
  pnh.param<std::string>("mesh_file", mesh_file, "");

  ros::NodeHandle nh;
  ros::Publisher pub = nh.advertise<visualization_msgs::Marker>("markers", 1000);

  ros::Rate rate (0.5);

  while (ros::ok())
  {
    ros::spinOnce();
    pub.publish(makeMarker(Eigen::Vector3d(0,0,0), mesh_file));
    rate.sleep();
  }

  return 0;
}