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

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

#include "IChargeClusters2D.hxx"

namespace {
    class IHit_compare {
    public:
        IHit_compare(double prec, const TVector3& zPlane) 
            : fPrec(prec), fZPlane(zPlane) {
        }

        int operator () (const COMET::IHandle<COMET::IHit>& a, 
                         const COMET::IHandle<COMET::IHit>& b) {
            TVector3 A = a->GetPosition();
            TVector3 B = b->GetPosition();
            
            if( std::abs(A.Dot(fZPlane)-B.Dot(fZPlane)) < fPrec ) 
                return (a->GetCharge() > b->GetCharge());
            
            return  (A.Dot(fZPlane) < B.Dot(fZPlane));
        }

    private:
        double fPrec;
        TVector3 fZPlane;
    };
};

COMET::IChargeClusters2D::IChargeClusters2D(double xGap, double yGap, double prec,
                                         TVector3 plane) 
    : fXGap(xGap), fYGap(yGap), fPrec(prec) {
    TVector3 X(1.,0.,0.); 
    TVector3 Y(0.,1.,0.); 
    TVector3 Z(0.,0.,1.); 
    
    // fZPlane is the plane that contains the hits 
    
    fChargeRatio = 10000.0;
    fMaxHitsToCluster = 1000000;

    fZPlane = plane.Unit(); 
    
    if( fZPlane.Angle(Y) > 1e-3 ) {
        fXPlane = Y.Cross(fZPlane);
        fYPlane = fXPlane.Cross(fZPlane);
    }
    else {    
        fXPlane = Y;
        fYPlane = Z;
        fZPlane = X;
    }
    
}


COMET::IChargeClusters2D::~IChargeClusters2D() {}

COMET::IHitSelection* 
COMET::IChargeClusters2D::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);
    }

    // Sort them per plane and charge 
    freeList.sort(IHit_compare(fPrec,fZPlane));

    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 that are part of the current hit
                // against the freelist
                for (std::list< COMET::IHandle<COMET::IHit> >::iterator i 
                         = freeList.begin();
                     i != freeList.end();) {
                    double zDelta = (*i)->GetPosition().Dot(fZPlane) 
                        - (*h)->GetPosition().Dot(fZPlane);

                    // Check to see if we've gotten to the current plane.  If
                    // we haven't, skip the new hit and look at the next one.
		    if( zDelta < -fPrec ){
                        ++i;
                        continue;
		    }

                    // Check to see if we've moved past current plane.  If
                    // we've already moved past the current plane we can stop
                    // the loop.
                    if ( zDelta > fPrec ) break; 

                    // Don't add a high charge hit to a cluster based on it's
                    // proximity to a low charge hit.  The high charge hit
                    // might still be added if it's close to another high
                    // charge hit.
                    if ((*i)->GetCharge() > fChargeRatio*(*h)->GetCharge()) {
                        ++i;
                        continue;
                    }		    
                    
                    double xDelta = (*i)->GetPosition().Dot(fXPlane) 
                        - (*h)->GetPosition().Dot(fXPlane);
                    double yDelta = (*i)->GetPosition().Dot(fYPlane) 
                        - (*h)->GetPosition().Dot(fYPlane);

                    // If the new hit is farther than fXGap from the old hit,
                    // don't add it to the cluster.
                    if (fXGap < std::abs(xDelta)) {
                        ++i;
                        continue;
                    }
                    
                    // If the new hit is farther than fZGap from the old hit,
                    // don't add it to the cluster.
                    if (fYGap < std::abs(yDelta)) {
                        ++i;
                        continue;
                    }

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

                    // Merging the hit into the combo hit invalidates the
                    // iterator "h" so restart the search at the begining of
                    // the combohit hit selection.
                    h = (*comboHit)->GetHits().begin();

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


    // Now close the combo hits and add them to the new hit selection.
    // Make one additional check: that the number of hits in a cluster
    // is less than maximum value.  This check allows for the possibility
    // to exclude clusters with too many hits (though the option is turned 
    // off by default).  If the maximum number is exceeded, then 
    // the constituent hits are saved individually rather than being clustered.
    for (std::vector<COMET::IComboHit*>::iterator comboHit 
             = comboHits.begin();
         comboHit != comboHits.end();
         ++comboHit) {
	if((*comboHit)->GetHits().size() <= (unsigned) fMaxHitsToCluster){
	    (*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
			  << "##############################################"
			  << std::endl;
		continue;
	    }
	    COMET::IHandle<COMET::IComboHit> combo(*comboHit);
	    nhits->push_back(combo);
	}else{	    
	    for (COMET::IHitSelection::const_iterator hit = (*comboHit)->GetHits().begin();
		 hit != (*comboHit)->GetHits().end(); ++hit) {
		nhits->push_back(*hit);
	    }	
            delete (*comboHit);	    
	}       
    }

    return nhits;
}