#include <vector>
#include <bitset>
#include <iostream>

#include <IGeometryDatabase.hxx>
#include <IStrawTrkReconEventFunction.hxx>
#include <IHitSelection.hxx>
#include <IDataHit.hxx>
#include <IMCHit.hxx>
#include <IStrawTrkChannelId.hxx>

#include "HEPUnits.hxx"

// ----------------------------------------------------------------------------
COMET::IStrawTrkReconEventFunction::IStrawTrkReconEventFunction() {
    fStrawTrkTracking = new COMET::IStrawTrkTracking();
    fADCThresholdForHit     = 2000.;
    fFactorForDriftDistance =   10.;
}

COMET::IStrawTrkReconEventFunction::~IStrawTrkReconEventFunction() {
    delete fStrawTrkTracking;
}

void COMET::IStrawTrkReconEventFunction::Initialize()
{
  fStrawTrkTracking->Init();
}

void COMET::IStrawTrkReconEventFunction::BeginOfEvent()
{
  fStrawTrkTracking->BeginOfEvent();
}

void COMET::IStrawTrkReconEventFunction::EndOfEvent()
{
  fStrawTrkTracking->EndOfEvent();
}

void COMET::IStrawTrkReconEventFunction::Finalize(const COMET::ICOMETOutput*) { }


// ----------------------------------------------------------------------------
COMET::IHandle<COMET::IAlgorithmResult> COMET::IStrawTrkReconEventFunction::PrepareHits(COMET::ICOMETEvent& event) {
    COMET::IHandle<COMET::IAlgorithmResult> result(new COMET::IAlgorithmResult("StrawTrk_HitSelection", "First prepared hit selection for ReconStrawTrk"));
    // Iterate for digits
    /*
    COMET::IHandle<COMET::IDigitContainer> digits = event.GetDigits("StrawTrk");
    if(digits){ COMETDebug("Success event.GetDigits"); }
    COMET::IHitSelection* hits = new COMET::IHitSelection("StrawTrkHitSelection", "StrawTrkHitSelection");

    if(!digits){ COMETDebug("Cannot find Straw Track digits"); }
    else{
        hits = MakeHitSelection(digits);
        CalculateDriftDistance(hits);
        if(hits) result->AddHitSelection(hits);
    }
    // Add IAlgorithmResult
    */

    COMET::IHandle<COMET::IHitSelection> hits = event.GetHitSelection("StrawTrk");
    if(!hits){ COMETDebug("Cannot find Straw Track hits"); }
    else{
        COMET::IHitSelection* allHits = new COMET::IHitSelection("StrECAL", "StrECAL combined hits");
        for (COMET::IHitSelection::const_iterator hitIt=hits->begin(); hitIt!=hits->end();
             ++hitIt) {
            allHits->AddHit(*hitIt);
        }
        hits = event.GetHitSelection("ECAL");
        if (hits) {
            for (COMET::IHitSelection::const_iterator hitIt=hits->begin(); hitIt!=hits->end();
                 ++hitIt) {
                allHits->AddHit(*hitIt);
            }
        }
        hits = event.GetHitSelection("TOF");
        if (hits) {
            for (COMET::IHitSelection::const_iterator hitIt=hits->begin(); hitIt!=hits->end();
                 ++hitIt) {
                allHits->AddHit(*hitIt);
            }
        }
        if(allHits) {
          event.Get<COMET::IDataVector>("hits")->push_back(allHits);
          result->AddHitSelection(allHits);
        }
    }

    event.AddFit(result);
    return result;
}


// ----------------------------------------------------------------------------
COMET::IHitSelection* COMET::IStrawTrkReconEventFunction::MakeHitSelection(COMET::IHandle<COMET::IDigitContainer>& digits) {

    COMET::IHitSelection* hits = new COMET::IHitSelection("StrawTrk_Hits", "StrawTrk Hit Selection");

    COMETDebug("Digits StrawTrk has " << digits->size() << " components.");

    for(COMET::IDigitContainer::iterator digitIter = digits->begin(); digitIter != digits->end(); digitIter++){
        COMET::IStrawTrkmcDigit* digit = static_cast<COMET::IStrawTrkmcDigit*>(*digitIter);

        if(!digit){
            COMETDebug("Cannot get a digit from digits StrawTrk");
            continue;
        }

        COMET::IWritableDataHit* hit = MakeDataHit(digit);

        if(!hit){
            COMETDebug("No data hit.");
            delete hit; continue;
        }
        // Set digit proxy
        COMET::IDigitProxy proxy(*digits, digitIter);
        hit->SetDigit(proxy);
        // Add hit
        hits->AddHit(COMET::IHandle<COMET::IDataHit>(hit));
    }

    // Check size
    if(hits->size() == 0){
        COMETDebug("Cannot make hit selection: 1stStrawTrkHits");
        hits = NULL;
    }
    return hits;
}


// ----------------------------------------------------------------------------
COMET::IWritableDataHit* COMET::IStrawTrkReconEventFunction::MakeDataHit(COMET::IStrawTrkmcDigit* digit) {
    COMET::IWritableDataHit* hit = new COMET::IWritableDataHit();

    IStrawTrkChannelId channelId(digit->GetChannelId());
    Double_t           time      = digit->GetFirstTimeTick() * 1.0 * unit::ns;

    /// This hit has no GeometryId
    if(!channelId.IsStrawTrkChannel()){
        delete hit;
        return NULL;
    }

    /// Digitization starts before trigger signal
    if(time <= 0.){
        COMETDebug("Too small charge, ignored.");
        delete hit;
        return NULL;
    }

    const COMET::IWaveformDigit::TWfADC& adcs = digit->GetADCs();

    /// Pulse height selection
    for(COMET::IWaveformDigit::TWfADC::const_iterator
    adc=adcs.begin(); adc!=adcs.end(); adc++){
        if(*adc < fADCThresholdForHit) {
            delete hit;
            return NULL;
        }
    }

    hit->SetChannelId(channelId);
    hit->SetTime(time);

    return hit;
}


// ----------------------------------------------------------------------------
void COMET::IStrawTrkReconEventFunction::CalculateDriftDistance(COMET::IHitSelection* hits) {

}


// ----------------------------------------------------------------------------
COMET::IHandle<COMET::IAlgorithmResult> COMET::IStrawTrkReconEventFunction::ProcessAlgorithms(const COMET::IAlgorithmResult& input, COMET::ICOMETEvent& event) {

    fStrawTrkTracking->SetIterationNo(1);
    COMET::IHandle<COMET::IAlgorithmResult> track1st = fStrawTrkTracking->Process(input);
    COMETNamedDebug("IStrawTrkReconEventFunction", " ProcessAlgorithm track1st");
    event.AddFit(track1st);

    return track1st;
    /*
    COMET::IHandle<COMET::IAlgorithmResult> input2(new COMET::IAlgorithmResult("StrawTrk_Track2nd", "First prepared hit selection for ReconStrawTrk"));

    COMET::IHandle<COMET::IReconObjectContainer> tracks = track1st->GetResultsContainer("StrawTrk_1stTrack");

    COMET::IHitSelection *trackHits = new COMET::IHitSelection("StrawTrk_1stTrack", "StrawTrk_Hits");

    if(tracks) {
        for(COMET::IReconObjectContainer::iterator trackIter = tracks->begin(); trackIter != tracks->end(); trackIter++){
            COMET::IHandle<COMET::IHitSelection> hits = (*trackIter)->GetHits();
            for (COMET::IHitSelection::const_iterator hitIter = hits->begin(); hitIter != hits->end(); ++hitIter) {
                const COMET::IHandle<COMET::IHit> hit = (*hitIter);
                trackHits->AddHit(hit);
            }
        }
    }

    COMETNamedInfo("ITrackFinder", "Size of hits in tracks = " << trackHits->size());
    input2->AddHitSelection(trackHits);
    fStrawTrkTracking->SetIterationNo(2);
    COMET::IHandle<COMET::IAlgorithmResult> track2nd = fStrawTrkTracking->Process(*input2);
    event.AddFit(track2nd);
    */
}