#include <list>
#include <vector>
#include <cmath>

#include <HEPUnits.hxx>
#include <IHandle.hxx>
#include <IHitSelection.hxx>
#include <IComboHit.hxx>
#include <ICOMETLog.hxx>

#include "IChargeClusters.hxx"

namespace {
    int reverse_charge_compare(const COMET::IHandle<COMET::IHit>& a, 
                               const COMET::IHandle<COMET::IHit>& b) {
        return (a->GetCharge() > b->GetCharge());
    }
}

COMET::IChargeClusters::IChargeClusters(double xGap, double yGap, double zGap) 
    : fXGap(xGap), fYGap(yGap), fZGap(zGap) {}

COMET::IChargeClusters::~IChargeClusters() {}

COMET::IHitSelection* COMET::IChargeClusters::operator()(const COMET::IHitSelection& hits) {
    std::list< COMET::IHandle<COMET::IHit> > freeList;
    COMET::IHitSelection *nhits = new COMET::IHitSelection(hits.GetName());

    for (COMET::IHitSelection::const_iterator hit = hits.begin();
         hit != hits.end(); 
         ++hit) {
        freeList.push_back(*hit);
    }
    freeList.sort(reverse_charge_compare);

    std::vector<COMET::IComboHit*> comboHits;

    while (freeList.size()>0) {
        // Add the first hit of the list to the combo hits, but do it
        // carefully so that the hit really is valid.
        while (freeList.size()>0) {
            COMET::IHandle<COMET::IHit> nextHit = freeList.front();
            freeList.pop_front();
            if (nextHit->GetCharge() <= 0) continue;
            if (std::abs(nextHit->GetPosition().X())>100*unit::meter) continue;
            if (std::abs(nextHit->GetPosition().Y())>100*unit::meter) continue;
            if (std::abs(nextHit->GetPosition().Z())>100*unit::meter) continue;
            comboHits.push_back(new COMET::IComboHit);
            comboHits.back()->OpenHits();
            comboHits.back()->AddHit(nextHit);
            break;
        }

        // Now loop over remaining free list and look for hits to add.
        for (std::vector<COMET::IComboHit*>::iterator comboHit 
                 = comboHits.begin();
             comboHit != comboHits.end();
             ++comboHit) {
            for (COMET::IHitSelection::const_iterator h 
                     = (*comboHit)->GetHits().begin();
                 h != (*comboHit)->GetHits().end();
                 ++h) {
                // Check each of the hits thar are port of the current hit
                // against the freelist
                for (std::list< COMET::IHandle<COMET::IHit> >::iterator i = freeList.begin();
                     i != freeList.end();) {
                    // Don't connect using a low charge hit in the cluster.
                    double xDelta 
                        = (*i)->GetPosition().X() - (*h)->GetPosition().X();
                    double yDelta 
                        = (*i)->GetPosition().Y() - (*h)->GetPosition().Y();
                    double zDelta 
                        = (*i)->GetPosition().Z() - (*h)->GetPosition().Z();
                    
                    if ((*i)->GetCharge() > 1.2*(*h)->GetCharge()) {
                        ++i;
                        continue;
                    }

                    if (fZGap < std::abs(zDelta)) {
                        ++i;
                        continue;
                    }
                    
                    if (fXGap < std::abs(xDelta)) {
                        ++i;
                        continue;
                    }
                    
                    if (fYGap < std::abs(yDelta)) {
                        ++i;
                        continue;
                    }

                    // Should be merged.
                    (*comboHit)->AddHit(*i);

                    // The iterator "h" is not valid, so restart the search
                    // through the hit selection.
                    h = (*comboHit)->GetHits().begin();

                    // remove i from the free list, and increment i in the
                    // process.
                    i = freeList.erase(i);
                }
            }
        }
    }

    for (std::vector<COMET::IComboHit*>::iterator comboHit 
             = comboHits.begin();
         comboHit != comboHits.end();
         ++comboHit) {
        (*comboHit)->CloseHits();
        if ((std::abs((*comboHit)->GetPosition().X()) > 100*unit::meter)
            || (std::abs((*comboHit)->GetPosition().Y()) > 100*unit::meter)
            || (std::abs((*comboHit)->GetPosition().Z()) > 100*unit::meter)) {
            COMETError("Combo hit position out of range.");

            std::cout << "##############################################"
                      << std::endl;
            (*comboHit)->ls("hits");
            std::cout << "##############################################"
                      << std::endl;
            continue;
        }
        COMET::IHandle<COMET::IComboHit> combo(*comboHit);
        nhits->push_back(combo);
    }

    return nhits;
}