//cluster finder for the GEM IROC
//author: philipp gadow (bachelor student) E18/E12
//terrible performance, incredibly slow!

#include "TpcClusterFinderSimpleALICE.h"

#include <iostream>
#include <algorithm>
#include <stdexcept>
#include "assert.h"

#include "TpcPadPlane.h"
#include "TpcPad.h"
#include "TpcCluster.h"
#include "TpcDigi.h"
#include "TpcDigiMapper.h"
#include "TpcDigiAmplitude.h"
#include "McId.h"
#include "McIdCollection.h"
#include "TpcPrelimCluster.h"

#include "TStopwatch.h"

#ifdef TESTCHAMBER
#include "Pedestals.h"
#endif




//constructor --------------------------------------------------------------------------------------
TpcClusterFinderSimpleALICE::TpcClusterFinderSimpleALICE(TpcPadPlane* p,
                                               std::vector<TpcCluster*>* ob,
                                               unsigned int timeslice, double G, double C)
                                                   : fpadplane(p), foutput_buffer(ob), fdt(timeslice), noXclust(false),
                                                   splitDigis(0), fG(G), fC(C), maxClusterSlice(4000), sectorize(true), fnSectors(1), zJitter(0.1), verbose(false)
{
    // construct sector map
    std::vector<unsigned int> ids=fpadplane->GetSectorIds();
    fnSectors=ids.size();
    for(unsigned int is=0;is<fnSectors;++is){
        unsigned int Sectorid=ids[is];
        fsectormap[Sectorid]=new std::vector<TpcDigi*>();
    }
    std::cout<<"TpcClusterFinderSimpleALICE: "
            <<fnSectors<<" Sectors instantiated."<<std::endl;

    if (fnSectors==1) return; // no need to build fpadIDsectorIDmap


    // build a map from padIds to SectorIds,
    // the purpose is to put digis at the borders of sectors into the digi buffers of both sectors
    // so that clusters won't be split at sector borders
    std::cerr<<"TpcClusterFinderSimpleALICE::TpcClusterFinderSimpleALICE - building fpadIDsectorIDmap ";

    unsigned int nPads, nNeighbors, sectorID;
    TpcPad* pad;
    std::vector<unsigned int> borderPads; // pads that lie at sector borders

    nPads = fpadplane->GetNPads();

    // loop over pads and find pads at borders
    for (unsigned int iPad=0; iPad<nPads; ++iPad){
        pad = fpadplane->GetPad(iPad);
        sectorID = pad->sectorId();

        fpadIDsectorIDmap[iPad] = new std::vector<unsigned int> (1, sectorID);

        //loop over neighbors
        nNeighbors = pad->nNeighbours();
        for (unsigned int ineigh=0; ineigh<nNeighbors; ++ineigh){
            unsigned int neighID = fpadplane->GetPad(pad->getNeighbour(ineigh))->sectorId();
            if (neighID == sectorID) continue;

            borderPads.push_back(iPad);
            break;
        }
    }

    double overlap(1.1);
    double x, y;
    unsigned int padId, padIdNeigh, nPadsInArea;;


    // loop over digis at borders
    for (unsigned int i=0; i<borderPads.size(); ++i){
        padId = borderPads[i];

        sectorID = fpadplane->GetPad(padId)->sectorId();

        std::vector<TpcPad*> buffer;
        fpadplane->GetPadXY(padId, x, y);
        fpadplane->GetPadList(x, y, overlap, buffer);

        // loop over pads in area
        nPadsInArea = buffer.size();
        for (unsigned int iPad=0; iPad<nPadsInArea; ++iPad){
            padIdNeigh = buffer[iPad]->padId();

            if ( (std::find(fpadIDsectorIDmap[padIdNeigh]->begin(), fpadIDsectorIDmap[padIdNeigh]->end(), sectorID)) == fpadIDsectorIDmap[padIdNeigh]->end() ){
                fpadIDsectorIDmap[padIdNeigh]->push_back(sectorID);
            }
        }
    }

    std::cout<<"... done"<<std::endl;

    // print
    /*std::vector<unsigned int>* sectorIDs;
  for (unsigned int iPad=0; iPad<nPads; ++iPad){
    std::cout<<iPad<<"  ";
    sectorIDs = fpadIDsectorIDmap[iPad];
    for(unsigned int i=0; i<sectorIDs->size(); ++i){
      std::cout<<(*sectorIDs)[i]<<" ";
    }
    std::cout<<"\n";
  }*/


    //this block is to define the z jitter
    McId dummyID(1,1,0);
    McIdCollection dummyColl;
    dummyColl.AddID(dummyID);

    TVector3 zDiff1,zDiff2;
    TpcDigi zDiffDigi1(1,1,1,dummyColl),zDiffDigi2(1,2,1,dummyColl);
    try {
        TpcDigiMapper::getInstance()->map(&zDiffDigi1,zDiff1);
        TpcDigiMapper::getInstance()->map(&zDiffDigi2,zDiff2);
    }
    catch(const std::exception& ex) {
        std::cout<<ex.what()<<std::endl;
    }
    zJitter = zDiff2.z() - zDiff1.z();
    //end of z jitter

}

//destructor --------------------------------------------------------------------------------------
TpcClusterFinderSimpleALICE::~TpcClusterFinderSimpleALICE(){
    std::map<unsigned int,std::vector<TpcDigi*>*>::iterator secIt=fsectormap.begin();
    while(secIt!=fsectormap.end()){
        delete secIt->second;
        ++secIt;
    }
    fsectormap.clear();

    std::map<unsigned int,std::vector<TpcDigi*>*>::iterator padIDIt=fpadIDmap.begin();
    while(padIDIt!=fpadIDmap.end()){
        delete padIDIt->second;
        ++padIDIt;
    }
    fpadIDmap.clear();

}


void 
        TpcClusterFinderSimpleALICE::process(std::vector<TpcDigi*>& alldigis)
{
    unsigned int nalldigi = alldigis.size();
    if(nalldigi<3) return;

    if(sectorize && fnSectors>1){
        std::cerr<<"processing digis sectorwise"<<std::endl;
        // reserve
        std::map<unsigned int,std::vector<TpcDigi*>* >::iterator secIt=fsectormap.begin();
        while(secIt!=fsectormap.end()){ // loop over sectors
            secIt->second->reserve(nalldigi/fsectormap.size()+250);
            ++secIt;
        } // end loop over sectors

        std::vector<unsigned int>* sectorIDs;

        // sectorize on pad plane
        for(int idi=0;idi<nalldigi;++idi){ // loop over digis
            // loop over sectorIDs
            sectorIDs = fpadIDsectorIDmap[alldigis[idi]->padId()];
            for(unsigned int i=0; i<sectorIDs->size(); ++i){
                fsectormap[(*sectorIDs)[i]]->push_back(alldigis[idi]);
            }
        } // end loop over digis

        // now process each sector independently
        secIt=fsectormap.begin();
        while(secIt!=fsectormap.end()){ // loop over sectors
            processSector(*(secIt->second), secIt->first);
            secIt->second->clear(); // clean up
            ++secIt;
        } // end loop over sectors
    }
    else {
        //    std::cerr<<"processing all digis"<<std::endl;
        processSector(alldigis, -1); // do not sectorize!
    }

}

// tools ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

bool
        TpcClusterFinderSimpleALICE::islocalmaximum(TpcDigi* rdigi, std::vector<TpcDigi*> &neighbourpaddigis, unsigned int firstpadidinrow, unsigned int lastpadidinrow, unsigned int timewindow){
    TStopwatch timeformaxsearch;
    timeformaxsearch.Start();
    unsigned int rdigipadID = rdigi->padId();
    neighbourpaddigis.clear();

    //    look at digis in left pad if they are adjacent in time
    if (rdigipadID > firstpadidinrow && rdigipadID < lastpadidinrow){
        unsigned int nleft = fpadIDmap[rdigipadID -1]->size();
        for (unsigned int ileft = 0; ileft < nleft; ileft++){
            if (fpadIDmap[rdigipadID -1]->at(ileft)->t() <= rdigi->t() + timewindow && fpadIDmap[rdigipadID -1]->at(ileft)->t() >= rdigi->t()-timewindow){
                neighbourpaddigis.push_back(fpadIDmap[rdigipadID -1]->at(ileft));
            }
        }

        unsigned int nright = fpadIDmap[rdigipadID +1]->size();
        for (unsigned int iright = 0; iright < nright; iright++){
            if (fpadIDmap[rdigipadID +1]->at(iright)->t() <= rdigi->t() + timewindow && fpadIDmap[rdigipadID +1]->at(iright)->t() >= rdigi->t()- timewindow){
                neighbourpaddigis.push_back(fpadIDmap[rdigipadID +1]->at(iright));
            }
        }

        unsigned int nsame = fpadIDmap[rdigipadID]->size();
        for (unsigned int isame = 0; isame < nsame; isame++){
            if (fpadIDmap[rdigipadID]->at(isame)->t() <= rdigi->t() + timewindow && fpadIDmap[rdigipadID]->at(isame)->t() >= rdigi->t()- timewindow){
                neighbourpaddigis.push_back(fpadIDmap[rdigipadID]->at(isame));
            }
        }
    }
    //    look at digis in right pad if they are adjacent in time

    if (rdigipadID == lastpadidinrow){
        unsigned int nleft = fpadIDmap[rdigipadID -1]->size();
        for (unsigned int ileft = 0; ileft < nleft; ileft++){
            if (fpadIDmap[rdigipadID -1]->at(ileft)->t() <= rdigi->t() + timewindow && fpadIDmap[rdigipadID -1]->at(ileft)->t() >= rdigi->t()-timewindow){
                neighbourpaddigis.push_back(fpadIDmap[rdigipadID -1]->at(ileft));
            }
        }

        unsigned int nsame = fpadIDmap[rdigipadID]->size();
        for (unsigned int isame = 0; isame < nsame; isame++){
            if (fpadIDmap[rdigipadID]->at(isame)->t() <= rdigi->t() + timewindow && fpadIDmap[rdigipadID]->at(isame)->t() >= rdigi->t()- timewindow){
                neighbourpaddigis.push_back(fpadIDmap[rdigipadID]->at(isame));
            }
        }
    }

    //    look at digis on the same pad if they are adjacent in time

    if (rdigipadID == firstpadidinrow){
        unsigned int nright = fpadIDmap[rdigipadID +1]->size();
        for (unsigned int iright = 0; iright < nright; iright++){
            if (fpadIDmap[rdigipadID +1]->at(iright)->t() <= rdigi->t() + timewindow && fpadIDmap[rdigipadID +1]->at(iright)->t() >= rdigi->t()- timewindow){
                neighbourpaddigis.push_back(fpadIDmap[rdigipadID +1]->at(iright));
            }
        }

        unsigned int nsame = fpadIDmap[rdigipadID]->size();
        for (unsigned int isame = 0; isame < nsame; isame++){
            if (fpadIDmap[rdigipadID]->at(isame)->t() <= rdigi->t() + timewindow && fpadIDmap[rdigipadID]->at(isame)->t() >= rdigi->t()- timewindow){
                neighbourpaddigis.push_back(fpadIDmap[rdigipadID]->at(isame));
            }
        }
    }


    double rdigiamp = rdigi->amp();
    //check if there is a local maximum:
    unsigned int nNeighbours = neighbourpaddigis.size();
    for (unsigned int ineigh = 0; ineigh < nNeighbours; ineigh++){

        if (neighbourpaddigis[ineigh]->amp() > rdigiamp){
            timeformaxsearch.Stop();
            Double_t mtime = timeformaxsearch.CpuTime();
            timeformaxsearch.Reset();

            std::cout << " fail ... it takes " << mtime << " to search for maximum"<<std::endl;
            return kFALSE;
        }
    }
    timeformaxsearch.Stop();
    Double_t mtime = timeformaxsearch.CpuTime();
    timeformaxsearch.Reset();

    std::cout << " success !!! it takes " << mtime << " to search for maximum"<<std::endl;
    return kTRUE;
}



unsigned int
        TpcClusterFinderSimpleALICE::padsinrow(unsigned int irow) //better use gemmapper!!!
{
    unsigned int padsInRow = -1;
    if (irow <  4)              padsInRow =  68;
    if (irow >= 4 && irow <  7) padsInRow =  70;
    if (irow >= 7 && irow < 10) padsInRow =  72;
    if (irow >=10 && irow < 13) padsInRow =  74;
    if (irow >=13 && irow < 16) padsInRow =  76;
    if (irow >=16 && irow < 19) padsInRow =  78;
    if (irow >=19 && irow < 22) padsInRow =  80;
    if (irow >=22 && irow < 25) padsInRow =  82;
    if (irow >=25 && irow < 28) padsInRow =  84;
    if (irow >=28 && irow < 31) padsInRow =  86;
    if (irow >=31 && irow < 34) padsInRow =  88;
    if (irow >=34 && irow < 37) padsInRow =  90;
    if (irow >=37 && irow < 40) padsInRow =  92;
    if (irow >=40 && irow < 43) padsInRow =  94;
    if (irow >=43 && irow < 46) padsInRow =  96;
    if (irow >=46 && irow < 49) padsInRow =  98;
    if (irow >=49 && irow < 52) padsInRow = 100;
    if (irow >=52 && irow < 55) padsInRow = 102;
    if (irow >=55 && irow < 58) padsInRow = 104;
    if (irow >=58 && irow < 61) padsInRow = 106;
    if (irow >=61 && irow < 63) padsInRow = 108;
    return padsInRow;
}


//The actual cluster finder (does not utilize the neighbours as in the pad plane for they can be wrong at the edges, does not use prelimclusters)
void
        TpcClusterFinderSimpleALICE::processSector(std::vector<TpcDigi*>& digis, int sectorID)
{

    std::sort(digis.begin(),digis.end(),TpcDigiPadID()); // sort digis in padIDs

    unsigned int ndigi=digis.size();
    if(ndigi<3) return; //make this configurable?

    TStopwatch timer;
    timer.Start();
    //  write all digis sorted in padIDs in the great fpadIDmap

    unsigned int npads = fpadplane->GetNPads();
    //  iterate over each padID and create a vector for the digis for each pad
    for (unsigned int ipadID = 0; ipadID < npads; ipadID++){
        fpadIDmap[ipadID] = new std::vector<TpcDigi*>();
    }
    // the digis are sorted for their padIDs, so iterate over them
    for(unsigned int idigiP=0; idigiP<ndigi; idigiP++){
        TpcDigi* tempDigi = digis[idigiP];
        fpadIDmap[tempDigi->padId()]->push_back(tempDigi);
        if(verbose){
            std::cout <<"padid: "<<tempDigi->padId()<<"size: "<< fpadIDmap[tempDigi->padId()]->size() << std::endl;
        }
    }  //end of digiloop



    timer.Stop();
    Double_t ctime = timer.CpuTime();
    timer.Reset();

    std::cout << " it takes " << ctime << " to make fpadIDmap"<<std::endl;
    //  now the fpadIDmap contains in every padID entry a vector of digis belonging to one pad (this works, tested)

    unsigned int firstpadidInRow = 0;
    unsigned int lastpadidInRow = -1;
    //  cluster in padrows
    for (unsigned int irow = 0; irow < 63; irow++){

        std::vector<TpcDigi*> digisInRow;
        // find last and first pad in row

        if (irow>0){
            firstpadidInRow += padsinrow(irow-1);
        }
        lastpadidInRow += padsinrow(irow);

        if(verbose){
            std::cout <<"first and last pad: " <<firstpadidInRow<< " "<<lastpadidInRow<<std::endl;
        }

        //loop over pads and find digis in padrow
        for (unsigned int iPad = firstpadidInRow; iPad < lastpadidInRow; iPad++){
            digisInRow.insert(digisInRow.end(), (fpadIDmap[iPad])->begin(), (fpadIDmap[iPad])->end());
        }


        std::stable_sort(digisInRow.begin(),digisInRow.end(),TpcDigiAmplitude());

        timer.Start();

        for (unsigned int idigi = 0;idigi < digisInRow.size(); idigi++){
            // check digis in one row for local maxima

            TpcDigi* rdigi = digisInRow[idigi];
            std::vector<TpcDigi*> neighbourpaddigis;
            if (islocalmaximum(rdigi, neighbourpaddigis, firstpadidInRow, lastpadidInRow, 1)){ // if it is a local maximum, the neighbourpads vector will be filled with the neighbouring digis
                // calculate pos, sig and amp from the local maximum and the adjacent digis
                TVector3 pos, sig;
                pos.SetXYZ(0,0,0);
                sig.SetXYZ(0,0,0);

                double amp = 0;
                unsigned int ndigisincluster = neighbourpaddigis.size();

                // loop over digis to calculate cog
                TpcDigi* adigi;
                TVector3 thispos;
                for(unsigned int id=0;id<ndigisincluster;++id){
                    adigi = neighbourpaddigis[id];
                    double a = (double)adigi->amp();
                    try{
                        TpcDigiMapper::getInstance()->map(adigi,thispos);
                    }
                    catch(const std::exception& ex) {
                        std::cout<<ex.what()<<std::endl;
                    }
                    pos += a*thispos;
                    amp += a;
                }
                pos *= 1./amp;




                // calculate errors: ------------------------------------------
                double Dl = TpcDigiMapper::getInstance()->getGas()->Dl();
                double Dt = TpcDigiMapper::getInstance()->getGas()->Dt();
                double driftl = fabs(pos.z() - TpcDigiMapper::getInstance()->zGem());

                double diffSigmaT = Dt * Dt * driftl;
                double diffSigmaL = Dl * Dl * driftl;

                double dx, dy;
                TpcDigiMapper::getInstance()->padsize(neighbourpaddigis[0]->padId(),dx,dy);

                TVector3 df, thissig;
                //old error calc starts here
                for(unsigned int id=0; id<ndigisincluster; ++id){
                    adigi = neighbourpaddigis[id];
                    double a = (double)adigi->amp();
                    try {
                        TpcDigiMapper::getInstance()->map(adigi,thispos);
                    }
                    catch(const std::exception& ex) {
                        std::cout<<ex.what()<<std::endl;
                    }

                    df = thispos-pos;
                    double sigmaX_sq = a*df.X()*df.X();
                    double sigmaY_sq = a*df.Y()*df.Y();
                    double sigmaZ_sq = a*df.Z()*df.Z();

                    thissig.SetXYZ(sigmaX_sq, sigmaY_sq, sigmaZ_sq);
                    sig += thissig;
                } // end second loop over digis

                // normalize sig
                //sig *= 1./amp;
                if(sig.X()<1E-5) sig.SetX(sqrt(dx*dx/12)*fC/amp);
                else sig.SetX(sqrt(sig.X()/amp)*fC/amp);

                if(sig.Y()<1E-5) sig.SetY(sqrt(dy*dy/12)*fC/amp);
                else sig.SetY(sqrt(sig.Y()/amp)*fC/amp);

                if(sig.Z()<1E-5) sig.SetZ(sqrt(zJitter*zJitter/12)*fC/amp);
                else sig.SetZ(sqrt(sig.Z()/amp)*fC/amp);
                //old error calc ends here
                if(verbose){
                    std::cout<<"maximum found! amp: "<< amp <<", pos: " << pos.X()<< " " << pos.Y()<<" " <<pos.Z()<<", sigma: "<< sig.X()<< " " << sig.Y()<<" " <<sig.Z() <<std::endl;
                }

                if (sectorID == -1){ // no sectorization
                    TpcCluster* clu = new TpcCluster(pos,sig,amp,0);
                    //add digis to cluster
                    for(unsigned int i=0;i<neighbourpaddigis.size();++i){
                      if(neighbourpaddigis[i]->isSaturated()){
                        clu->setSaturation(true);
                      }
                      clu->addDigi(neighbourpaddigis[i], 1); //MUST ADD DIGI WEIGHTING!
                    }
                    foutput_buffer->push_back(clu);
                    if(verbose){
                        std::cout<<"cluster saved, now we have #clusters: " << foutput_buffer->size()<<std::endl;
                    }
                }
                else if (fpadplane->GetPad(rdigi->padId())->sectorId() == sectorID){
                    TpcCluster* clu = new TpcCluster(pos,sig,amp,sectorID);
                    //add digis to cluster
                    for(unsigned int i=0;i<neighbourpaddigis.size();++i){
                      if(neighbourpaddigis[i]->isSaturated()){
                        clu->setSaturation(true);
                      }
                      clu->addDigi(neighbourpaddigis[i], 1); //MUST ADD DIGI WEIGHTING!
                    }
                    foutput_buffer->push_back(clu);
                    if(verbose){
                        std::cout<<"cluster saved, with sectorid and now we have #clusters: " << foutput_buffer->size()<<std::endl;
                    }
                }

            }// end of if localmaximum
        }// end of loop over digis in row
        timer.Stop();
        ctime = timer.CpuTime();
        timer.Reset();

        std::cout << " it takes " << ctime << " to cluster a row"<<std::endl;
    }// end of loop over rows
}





void 
        TpcClusterFinderSimpleALICE::reset()
{ 
    splitDigis = 0;

}


void
        TpcClusterFinderSimpleALICE::checkConsistency()
{
    std::cout << "TpcClusterFinderSimpleALICE::checkConsistency() empty implementation" << std::endl;
}