// fastjet stuff
#include "fastjet/ClusterSequence.hh"
#include "fastjet/MulguisinPlugin.hh"
#include "fastjet/DummyJet.hh"
// other stuff
#include <vector>
#include <list>
#include <sstream>
#include "SortByEt.h"
#include <algorithm>

FASTJET_BEGIN_NAMESPACE      // defined in fastjet/internal/base.hh

using namespace std;
using namespace cms;

//------------------------------------------------------
// some tools
//------------------------------------------------------
template <class T> 
T deltaPhi (T phi1, T phi2) { 
  T result = phi1 - phi2;
  while (result > M_PI) result -= 2*M_PI;
  while (result <= -M_PI) result += 2*M_PI;
  return result;
}

template <class T>
T deltaR2 (T eta1, T phi1, T eta2, T phi2) {
  T deta = eta1 - eta2;
  T dphi = deltaPhi (phi1, phi2);
  return deta*deta + dphi*dphi;
}

typedef std::vector< std::pair<double,int> > PVector;

//------------------------------------------------------
bool CompareEt( PseudoJet a, PseudoJet b)
{
   return a.Et() > b.Et();
}



string MulguisinPlugin::description () const {
  ostringstream desc;
  desc << "Mulguisin plugin with R = " << theConeRadius << " and seed threshold = " << theSeedThreshold;
  return desc.str();
}

void MulguisinPlugin::run_clustering(ClusterSequence & clust_seq) const {

  vector<PseudoJet> input;
  for (unsigned int i=0 ; i<clust_seq.jets().size() ; i++) {
    input.push_back(clust_seq.jets()[i]);
  }
  sort(input.begin(), input.end(), CompareEt);
  vector< vector<PseudoJet> > protojets;

  double radius2 = theConeRadius*theConeRadius;
  int count_part = 0; 
  for ( vector<PseudoJet>::iterator part = input.begin() ; part != input.end(); part++) {
    bool standalone = true;
    double p_eta = part->eta();
    double p_phi = part->phi();
    double p_et  = part->Et();
    double shortest_distance = 9999;
    vector< vector<PseudoJet> >::iterator nearest_tower;
    // Calculate distance between already existed jet's constituents
    for ( vector< vector<PseudoJet> >::iterator protojet =  protojets.begin() ; protojet != protojets.end() ; protojet++) {
      vector<PseudoJet>::iterator seed = protojet->begin();
      if ( deltaR2( p_eta, p_phi, seed->eta(), seed->phi()) > radius2*4) continue;
      for ( vector<PseudoJet>::iterator constituent = protojet->begin(); constituent != protojet->end(); constituent++   )  {   
         double distance = deltaR2( p_eta, p_phi, constituent->eta(), constituent->phi() );
         if (  distance < radius2 && distance < shortest_distance )    {
            shortest_distance = distance;
            nearest_tower = protojet;
            standalone = false;
         }
        }
    }
    // End Jet loop
    if ( standalone ) {
      if ( p_et < theSeedThreshold ) continue;
   vector< PseudoJet> new_jet;
   new_jet.push_back( *part);
   protojets.push_back(new_jet);

    }
    else {
   nearest_tower->push_back( *part );    
    }
  }

   PVector p_vector;
 for( vector< vector<PseudoJet> >::iterator jet = protojets.begin() ; jet != protojets.end() ; jet++){

   vector<PseudoJet>::iterator constituent;
   constituent = jet->begin();
   int jet_k = constituent->cluster_hist_index(); 
   constituent++;
   while( constituent != jet->end()) {
      int jet_i = jet_k;
      int jet_j = constituent->cluster_hist_index();
      double dij=0.0;
      PseudoJet newjet = clust_seq.jets()[jet_i] + clust_seq.jets()[jet_j];
      clust_seq.plugin_record_ij_recombination(jet_i, jet_j, dij, newjet, jet_k);
      constituent++;
   }

   
   double d_iB = clust_seq.jets()[jet_k].perp2();
   //clust_seq.plugin_record_iB_recombination( jet_k, d_iB);
   p_vector.push_back( make_pair(d_iB, jet_k) );
  }
// Et ordering
   PVector::iterator it = p_vector.begin();
   PVector::iterator et = p_vector.end();
   sort(it,et );

   for( ; it != et ; ++it )
   {
    clust_seq.plugin_record_iB_recombination( it->second, it->first);
   } 


}

FASTJET_END_NAMESPACE      // defined in fastjet/internal/base.hh