#include <algorithm>
#include <cmath>

#include "IDensityCluster.hxx"




struct GreaterCluster :
    public std::binary_function<COMET::IHitSelection, COMET::IHitSelection, bool>{
    bool operator()(const COMET::IHitSelection& c1, const COMET::IHitSelection& c2){
        return c1.size() > c2.size();
    }
};  

struct SmallCluster :
    public std::unary_function<COMET::IHitSelection, bool>{
    SmallCluster(unsigned int size) : fSize(size) { }
    bool operator()(const COMET::IHitSelection& rhs){
        return rhs.size() < fSize;
    }

    unsigned int fSize;
};







COMET::IDensityCluster::IDensityCluster(unsigned int MinPts, double epsilon) :
    fMinClusterSize(1),
    fMinPoints(MinPts),
    fMinDistance(epsilon) { }
  
    
void COMET::IDensityCluster::Cluster(const COMET::IHitSelection& hits)
{
  
    std::copy(hits.begin(), hits.end(), std::back_inserter(fRemainingHits));
    while (!fRemainingHits.empty())
    {

        COMET::IHitSelection cluster;
        COMET::IHitSelection seeds = this->FindSeeds(fRemainingHits);
        if (seeds.size() < fMinPoints) break;
        
        for (COMET::IHitSelection::iterator h = seeds.begin();
             h != seeds.end(); ++h)
        {
            COMET::IHitSelection::iterator where
                = std::find(fRemainingHits.begin(), fRemainingHits.end(), *h);
            if (where != fRemainingHits.end())
                    fRemainingHits.erase(where);
        }
        
        std::copy(seeds.begin(), seeds.end(), std::back_inserter(cluster));

        while (!seeds.empty())
        {
            COMET::IHandle<COMET::IHit> currentP = seeds.front();
            COMET::IHitSelection result = this->GetNeighbors(currentP, fRemainingHits);
            if (result.size() >= fMinPoints){
                for (COMET::IHitSelection::iterator h = result.begin();
                     h != result.end(); ++h){
                    seeds.push_back(*h);
                    cluster.push_back(*h);
                    COMET::IHitSelection::iterator where =
                        std::find(fRemainingHits.begin(),
                                  fRemainingHits.end(), *h);
                    if (where != fRemainingHits.end())
                            fRemainingHits.erase(where);
                }
                
            }
            seeds.erase(seeds.begin());
            
        }

        fClusters.push_back(cluster);
    }

    std::sort(fClusters.begin(), fClusters.end(), GreaterCluster());
    fClusters.erase(std::remove_if(fClusters.begin(), fClusters.end(),
                                   SmallCluster(fMinClusterSize)),
                    fClusters.end());

   
    
}

COMET::IHitSelection
COMET::IDensityCluster::FindSeeds(const COMET::IHitSelection& remaining)
{
  
    COMET::IHitSelection hits;
    std::copy(remaining.begin(), remaining.end(), std::back_inserter(hits));
    COMET::IHitSelection maxseeds = this->GetNeighbors(hits.front(), remaining);
    maxseeds.push_back(hits.front());
    for (COMET::IHitSelection::iterator h = hits.begin();
         h != hits.end(); ++h)
    {
        COMET::IHitSelection seeds = this->GetNeighbors(*h, remaining); 
        if (maxseeds.size() < seeds.size()) { 
            maxseeds = seeds;
            maxseeds.push_back(*h);
        }
    }
    
    return maxseeds;
}

COMET::IHitSelection
COMET::IDensityCluster::GetNeighbors(COMET::IHandle<COMET::IHit> hit, const COMET::IHitSelection& hits)
{
    COMET::IHitSelection remaining;
    std::copy(hits.begin(), hits.end(), std::back_inserter(remaining));
    COMET::IHitSelection::iterator where
        = std::find(remaining.begin(), remaining.end(), hit);
    if (where != remaining.end())
            remaining.erase(where);
        
    COMET::IHitSelection neighbors;
    for (COMET::IHitSelection::iterator h = remaining.begin();
         h != remaining.end(); ++h)
    {
        double distance = this->CalculateDistance(hit, *h);
        if (distance < fMinDistance)
                neighbors.push_back(*h);
    }
    
    
    return neighbors;
}

double COMET::IDensityCluster::CalculateDistance(COMET::IHandle<COMET::IHit> lhs, COMET::IHandle<COMET::IHit> rhs)
{
    double x1 = lhs->GetPosition().Z();
    double y1 = 0.0;
    if (lhs->IsXHit()) y1 = lhs->GetPosition().X();
    if (lhs->IsYHit()) y1 = lhs->GetPosition().Y();

    double x2 = rhs->GetPosition().Z();
    double y2 = 0.0;
    if (rhs->IsXHit()) y2 = rhs->GetPosition().X();
    if (rhs->IsYHit()) y2 = rhs->GetPosition().Y();

    return std::sqrt(std::pow(x1-x2, 2) + std::pow(y1-y2, 2));

}

unsigned int COMET::IDensityCluster::ls()
{

    unsigned int count = 0;
    for (unsigned int i = 0 ; i < fClusters.size(); ++i)
            count += this->GetCluster(i).size();

    count += fRemainingHits.size();
    return count;
}