#include "ReconObjectUtils.hxx"
#include "TrackingUtils.hxx"
#include "IPIDManager.hxx"

#include "IIntegerDatum.hxx"


namespace ReconObjectUtils {
  //*****************************************************************************
  void  GetConstituentsInDetector(COMET::IHandle<COMET::IReconBase> t1, COMET::IReconBase::Status det, COMET::IReconObjectContainer& outputContainer){
  //*****************************************************************************

    if (t1->GetDetectors() == det){
      outputContainer.push_back(t1);
      return;
    }

    COMET::IHandle<COMET::IReconObjectContainer> t1const = t1->GetConstituents();
    if (!t1const) return;

    COMET::IReconObjectContainer::const_iterator it1;

    for (it1=t1const->begin();it1!=t1const->end(); it1++)
      GetConstituentsInDetector(*it1, det,outputContainer);
  }

  //*****************************************************************************
  COMET::IHandle<COMET::IReconBase>  GetConstituentNotUsingDetector(COMET::IHandle<COMET::IReconBase> t1, COMET::IReconBase::Status det){
  //*****************************************************************************
    if (!t1->UsesDetector(det))
      return t1;

    COMET::IHandle<COMET::IReconObjectContainer> t1const = t1->GetConstituents();
    if (!t1const) return  COMET::IHandle<COMET::IReconBase>();

    COMET::IReconObjectContainer::const_iterator it1;
    for (it1=t1const->begin();it1!=t1const->end(); it1++){
      if (!(*it1)->UsesDetector(det)) return *it1;
      COMET::IHandle<COMET::IReconBase> tmp = GetConstituentNotUsingDetector(*it1, det);
      if(tmp) return tmp;
    }

    return  COMET::IHandle<COMET::IReconBase>();  
  }

  //*****************************************************************************
  /// Return all lowest-level constituents of an object. Setting the includeComposite
  /// parameter returns all constituents, irrespective of what level they are at.
  COMET::IReconObjectContainer GetConstituentsRecursive(COMET::IHandle<COMET::IReconBase> t1, bool includeComposite) {
  //*****************************************************************************
    // Add the constituent tracks to a reconobject container.
    COMET::IReconObjectContainer allt1const;

    COMET::IHandle<COMET::IReconObjectContainer> t1const = t1->GetConstituents();
    if (!t1const) {
      allt1const.push_back(t1);
    } else {
      if (includeComposite) {
        allt1const.push_back(t1);
      }

      COMET::IReconObjectContainer::const_iterator it;
      COMET::IReconObjectContainer::const_iterator it2;

      for (it = t1const->begin(); it != t1const->end(); it++) {
        COMET::IReconObjectContainer t2const = GetConstituentsRecursive(*it, includeComposite);

        for (it2 = t2const.begin(); it2 != t2const.end(); it2++) {
          allt1const.push_back(*it2);
        }
      }
    }

    return allt1const;
  }

  //*****************************************************************************
  /// Add the specified detector Status to the object and all its constituents.
  void AddDetectorRecursive(COMET::IHandle<COMET::IReconBase> object, COMET::IReconBase::Status det) {
  //*****************************************************************************
    // Add detector to the object itself
    object->AddDetector(det);

    // Add detector to the object in the nodes
    const COMET::IReconNodeContainer& tnodes = object->GetNodes();
    COMET::IReconNodeContainer::const_iterator it;

    for (it = tnodes.begin(); it != tnodes.end(); it++) {
      AddDetectorRecursive((*it)->GetObject(), det);
    }

    // Add detector to each of the constituents
    COMET::IHandle<COMET::IReconObjectContainer> cons = object->GetConstituents();

    if (cons) {
      COMET::IReconObjectContainer::const_iterator it1;

      for (it1 = cons->begin(); it1 != cons->end(); it1++) {
        AddDetectorRecursive(*it1, det);
      }
    }
  }

  //*****************************************************************************
  /// Get all objects in the input container that USE the detector specified.
  void GetObjectsInDetector(const COMET::IReconObjectContainer& inputContainer, COMET::IReconBase::Status det, COMET::IReconObjectContainer& outputContainer) {
  //*****************************************************************************
    COMET::IReconObjectContainer::const_iterator objectIter1;

    for (objectIter1 = inputContainer.begin(); objectIter1 != inputContainer.end(); objectIter1++) {
      if ((*objectIter1)->UsesDetector(det))
        outputContainer.push_back(*objectIter1);
    }
  }

  //*****************************************************************************
  /// Get the highest-level constituent of an object that EXACTLY MATCHES the specified detector Status.
  COMET::IHandle<COMET::IReconBase> GetConstituentInDetector(COMET::IHandle<COMET::IReconBase> t1, COMET::IReconBase::Status det) {
  //*****************************************************************************
    if (t1->GetDetectors() == det) {
      return t1;
    }

    COMET::IHandle<COMET::IReconObjectContainer> t1const = t1->GetConstituents();
    if (!t1const) {
      return COMET::IHandle<COMET::IReconBase>();
    }

    COMET::IReconObjectContainer::const_iterator it1;

    for (it1 = t1const->begin(); it1 != t1const->end(); it1++) {
      COMET::IHandle<COMET::IReconBase> tmp = GetConstituentInDetector(*it1, det);

      if (tmp) {
        return tmp;
      }
    }

    return COMET::IHandle<COMET::IReconBase>();
  }

  //*****************************************************************************
  /// Get all the constituents of an object that EXACTLY MATCH the specified detector Status.
  /// The sub-constituents of objects that match are not included.
  COMET::IReconObjectContainer GetAllConstituentsInDetector(COMET::IHandle<COMET::IReconBase> t1, COMET::IReconBase::Status det) {
  //*****************************************************************************
    COMET::IReconObjectContainer outputCont;

    if (t1->GetDetectors() == det) {
      outputCont.push_back(t1);
    } else {
      COMET::IHandle<COMET::IReconObjectContainer> t1const = t1->GetConstituents();
      if (t1const) {
        COMET::IReconObjectContainer::const_iterator it1;
        COMET::IReconObjectContainer::const_iterator it2;

        for (it1 = t1const->begin(); it1 != t1const->end(); it1++) {
          COMET::IReconObjectContainer t2const = GetAllConstituentsInDetector(*it1, det);

          for (it2 = t2const.begin(); it2 != t2const.end(); it2++) {
            outputCont.push_back(*it2);
          }
        }
      }
    }

    return outputCont;
  }

  //*****************************************************************************
  void GetAllConstituentsInDetector(const COMET::IReconObjectContainer& inputContainer, COMET::IReconBase::Status det, COMET::IReconObjectContainer& outputContainer) {
  //*****************************************************************************
  COMET::IReconObjectContainer::const_iterator objectIter1;
  COMET::IReconObjectContainer::const_iterator objectIter2;


  for (objectIter1 = inputContainer.begin(); objectIter1 != inputContainer.end(); objectIter1++) {
    COMET::IHandle<COMET::IReconBase> obj1 = *objectIter1;
    if(!obj1)
    continue;

    COMET::IReconObjectContainer outputCont1 = GetAllConstituentsInDetector(obj1, det);

    for (objectIter2 = outputCont1.begin(); objectIter2 != outputCont1.end(); objectIter2++)
    outputContainer.push_back(*objectIter2);
  }

  }


  //******************************************************
  void GetAllConstituentsNotUsingDetector(const COMET::IReconObjectContainer& inputContainer, COMET::IReconBase::Status det, COMET::IReconObjectContainer& outputContainer){
  //******************************************************
  COMET::IReconObjectContainer::const_iterator objectIter1;


  for (objectIter1 = inputContainer.begin(); objectIter1 != inputContainer.end(); objectIter1++) {

    COMET::IHandle<COMET::IReconBase> obj1 = *objectIter1;
    if(!obj1)
    continue;

    COMET::IHandle<COMET::IReconBase> const1 =  GetConstituentNotUsingDetector(obj1, det);
    
    if(const1)
    outputContainer.push_back(const1);
  }

  }

  //*****************************************************************************
  /// Get the highest-level constituent of an object that USES the specified detector.
  COMET::IHandle<COMET::IReconBase> GetConstituentUsingDetector(COMET::IHandle<COMET::IReconBase> t1, COMET::IReconBase::Status det) {
  //*****************************************************************************
    COMET::IHandle<COMET::IReconObjectContainer> t1const = t1->GetConstituents();

    if (!t1const) {
      return COMET::IHandle<COMET::IReconBase>();
    }

    COMET::IReconObjectContainer::const_iterator it1;
    for (it1 = t1const->begin(); it1 != t1const->end(); it1++) {
      if ((*it1)->UsesDetector(det)) {
        return *it1;
      }

      COMET::IHandle<COMET::IReconBase> tmp = GetConstituentUsingDetector(*it1, det);

      if (tmp) {
        return tmp;
      }
    }

    return COMET::IHandle<COMET::IReconBase>();
  }

  //*****************************************************************************
  /// Whether object2 is a lowest-level constituent of object1.
  bool IsConstituent(COMET::IHandle<COMET::IReconBase> object1, COMET::IHandle<COMET::IReconBase> object2) {
  //*****************************************************************************
    COMET::IReconObjectContainer const1 = GetConstituentsRecursive(object1);
    COMET::IReconObjectContainer::const_iterator it;

    for (it = const1.begin(); it != const1.end(); it++) {
      if (GetPointer(*it) == GetPointer(object2)) {
        return true;
      }
    }

    return false;
  }

  //*****************************************************************************
  /// Whether object1's constituents are a subset of object2's constituents (if object2 has more
  /// constituents than object1), or object2's constituents are a subset of object1's constituents
  /// (if object1 has more constituents than object2),
  bool IsContained(COMET::IHandle<COMET::IReconBase> object1, COMET::IHandle<COMET::IReconBase> object2) {
  //*****************************************************************************
    COMET::IReconObjectContainer const2 = GetConstituentsRecursive(object2);
    COMET::IReconObjectContainer const1 = GetConstituentsRecursive(object1);

    COMET::IReconObjectContainer::const_iterator it;
    if (const1.size() > const2.size()) {
      for (it = const2.begin(); it != const2.end(); it++) {
        if (!IsConstituent(object1, *it)) {
          return false;
        }
      }
    } else {
      for (it = const1.begin(); it != const1.end(); it++) {
        if (!IsConstituent(object2, *it)) {
          return false;
        }
      }
    }

    return true;
  }

  //*****************************************************************************
  COMET::IHandle<COMET::IReconBase> CopyObject(COMET::IHandle<COMET::IReconBase> object){
  //*****************************************************************************

    if (!object) return COMET::IHandle<COMET::IReconBase>();

    COMET::IHandle<COMET::IReconBase> object2;

    COMET::IHandle<COMET::IReconPID> pid = object;
    if (pid) object2 =  COMET::IHandle<COMET::IReconPID>(new COMET::IReconPID(*pid));
    else{
      COMET::IHandle<COMET::IReconTrack> track = object;
      if (track) object2 =  COMET::IHandle<COMET::IReconTrack>(new COMET::IReconTrack(*track));
      else{
        COMET::IHandle<COMET::IReconShower> shower = object;
        if (shower) object2 =  COMET::IHandle<COMET::IReconShower>(new COMET::IReconShower(*shower));
        else{
          COMET::IHandle<COMET::IReconCluster> cluster = object;
          if (cluster) object2 =  COMET::IHandle<COMET::IReconCluster>(new COMET::IReconCluster(*cluster));
          else{
            COMET::IHandle<COMET::IReconVertex> vertex = object;
            if (vertex) object2 =  COMET::IHandle<COMET::IReconVertex>(new COMET::IReconVertex(*vertex));
          }
        }
      }
    }
    // these two fields are not copied by default in the constructor
    if (TrackingUtils::GetSenseOK(*object)) 
      TrackingUtils::SetSenseOK(*object2,true); 
    if (TrackingUtils::GetMatchingChi2(*object)!=0) 
      TrackingUtils::SetMatchingChi2(*object2,TrackingUtils::GetMatchingChi2(*object)); 
    if (TrackingUtils::GetMergedNoRefitStatus(*object)) 
      TrackingUtils::SetMergedNoRefitStatus(*object2,true); 

    // copy the algorithm name
    object2->SetAlgorithmName(object->GetAlgorithmName().c_str());

    // copy the PID weights stored in the IDatum
    COMET::pman().CopyPIDWeightsFromIDatums(object, object2);

    // copy the broken tracks
    COMET::IHandle<COMET::IDataVector> broken =  object->Get< COMET::IDataVector >("brokenID");
    if (broken){
      for (unsigned int i=0;i<broken->size();i++){
        UInt_t ID =  broken->At<COMET::IIntegerDatum>(i)->GetValue(); 
        // copy the broken tracks of the original object into the copy object
    COMET::IHandle<COMET::IDataVector> broken2 =  object2->Get< COMET::IDataVector >("brokenID");
    if (!broken2) {
      // Make sure that there aren't any data vector entries with a
      // conflicting name.
      COMET::IDatum* h = object2->FindDatum("brokenID");
      while (h) {
      object2->erase(h);
      delete h;
      h = object2->FindDatum("brokenID");
      }
      // Create the needed object.
      COMET::IDataVector* c  = new COMET::IDataVector("brokenID"); 
      object2->AddDatum(c);
      // And fill the handle so the creation happens transparently.
      broken2 = object2->Get< COMET::IDataVector >("brokenID");
    }

    COMET::IIntegerDatum* dID = new COMET::IIntegerDatum("ID","ID");
    dID->SetValue(ID);
    broken2->push_back(dID);
      }
    }

    // Save the original object in a container
    SetOriginalObject(object,object2);
    
    
    return object2;
    
  }

  //*****************************************************************************
  void CopyObjects(COMET::IHandle<COMET::IReconObjectContainer> objects1, 
      COMET::IHandle<COMET::IReconObjectContainer> objects2){
  //*****************************************************************************

    if (!objects1 || !objects2) return;

    COMET::IReconObjectContainer::const_iterator pp1;
    for(pp1 = objects1->begin(); pp1 != objects1->end(); pp1++){

      // create the copy
      COMET::IHandle<COMET::IReconBase> object2 = CopyObject(*pp1);

      // add the copy 
      objects2->push_back(object2);

    }
  }

  //*****************************************************************************
  void CopyState(const COMET::IReconState& state1, COMET::IReconState& state2){

  //*****************************************************************************

    for (int i=0; i<state2.GetDimensions(); ++i) {
      state2.SetValue(i,state1.GetValue(i));
      for (int j=0; j<state2.GetDimensions(); ++j) {
        state2.SetCovarianceValue(i,j,state1.GetCovarianceValue(i,j));
      }
    }
  }


  //*****************************************************************************
  COMET::IHandle<COMET::IReconBase> CreateNewObject(const std::string& type){
  //*****************************************************************************

  // create the correct object type
  COMET::IHandle<COMET::IReconBase> object;
  if (type == "Track")
    object = COMET::IHandle<COMET::IReconBase>(new COMET::IReconTrack);
  else if (type=="PID")
    object = COMET::IHandle<COMET::IReconBase>(new COMET::IReconPID);
  else if (type=="Shower")
    object = COMET::IHandle<COMET::IReconBase>(new COMET::IReconShower);
  else if (type=="Cluster")
    object = COMET::IHandle<COMET::IReconBase>(new COMET::IReconCluster);
  //  else if (type=="Particle")
  //    object = COMET::IHandle<COMET::IReconBase>(new COMET::TReconParticle);
  else{
    std::cout << "unknown object type '" << type << "'" << std::endl;
  }
  
  return object;

  }

  //*****************************************************************************
  void SetOriginalObject(COMET::IHandle<COMET::IReconBase> object1, COMET::IHandle<COMET::IReconBase> object2){
  //*****************************************************************************

    // Save the original object in a container
    COMET::IHandle<COMET::IReconObjectContainer> original =
      object2->Get<COMET::IReconObjectContainer>("original");
    if (!original) {
      // Make sure that there aren't any data vector entries with a
      // conflicting name.
      COMET::IDatum* h = object2->FindDatum("original");
      while (h) {
        object2->erase(h);
        delete h;
        h = object2->FindDatum("original");
      }
      // Create the needed object.
      COMET::IReconObjectContainer* c
        = new COMET::IReconObjectContainer("original", 
            "Origin of this object");
      object2->AddDatum(c);
      // And fill the handle so the creation happens transparently.
      original = object2->Get<COMET::IReconObjectContainer>("original");
    }
    //if object1 already has original objects container then 
    //save it as well, this will help to have the original objects thread through all copy calls
    COMET::IHandle<COMET::IReconObjectContainer> originalOld = object1->Get<COMET::IReconObjectContainer>("original");
    if (originalOld){
      COMET::IReconObjectContainer::const_iterator it;
      for (it=originalOld->begin();it!=originalOld->end(); it++){
        COMET::IHandle<COMET::IReconBase> obj  = *it;
        if(obj)
          original->push_back(obj);
      }
    }
    //..and add the object itself
    original->push_back(object1);
  }


  //*****************************************************************************
  COMET::IHandle<COMET::IReconState> CreateNewState(const std::string& type){
  //*****************************************************************************

  // create the correct state type
  COMET::IHandle<COMET::IReconState> tstate;

  if (type == "Track")
    tstate = COMET::IHandle<COMET::IReconState>(new COMET::ITrackState());
  else if (type=="PID")
    tstate = COMET::IHandle<COMET::IReconState>(new COMET::IPIDState());
  else if (type=="Shower")
    tstate = COMET::IHandle<COMET::IReconState>(new COMET::IShowerState());
  else if (type=="Cluster")
    tstate = COMET::IHandle<COMET::IReconState>(new COMET::IClusterState());
  //  else if (type=="Particle")
  //    tstate = COMET::IHandle<COMET::IReconState>(new COMET::IParticleState());
  else{
    std::cout << "unknown state type '" << type << "'" << std::endl;
  }
  
  return tstate;

  }


  //*****************************************************************************
  /// Copy any objects in tc1 that don't have the kRan flag set into tc2.
  void CopyUnusedObjects(COMET::IHandle<COMET::IReconObjectContainer> tc1, COMET::IHandle<COMET::IReconObjectContainer> tc2) {
  //*****************************************************************************
    if (tc1 && tc2) {
      CopyUnusedObjects(*tc1, *tc2);
    }
  }

  //*****************************************************************************
  /// Copy any objects in tc1 that don't have the kRan flag set into tc2.
  void CopyUnusedObjects(const COMET::IReconObjectContainer& tc1, COMET::IReconObjectContainer& tc2) {
  //*****************************************************************************
    COMETVerbose("Copy unused objects from container " << tc1.GetName() << " to " << tc2.GetName());

    COMET::IReconObjectContainer::const_iterator pp1;
    for (pp1 = tc1.begin(); pp1 != tc1.end(); pp1++) {
      COMET::IHandle<COMET::IReconBase> rbase = *pp1;

      // Object not used by another
      if (!rbase->CheckStatus(COMET::IReconBase::kRan)) {
        tc2.push_back(rbase);
      }
    }
  }

  //*****************************************************************************
  /// Combine the objects in objects1 and objects2 into objects3.
  void MergeContainers(COMET::IHandle<COMET::IReconObjectContainer> objects1, COMET::IHandle<COMET::IReconObjectContainer> objects2, COMET::IHandle<COMET::IReconObjectContainer> objects3) {
  //*****************************************************************************
    if (objects1 && objects2) {
      COMETVerbose("Merging containers " << objects1->GetName() << "  and " << objects2->GetName() << ": ");
    }

    COMET::IReconObjectContainer::const_iterator pp;

    if (objects1) {
      COMETVerbose("   #" << objects1->GetName() << " objects:  " << objects1->size());

      //-------- Loop over DET1 objects -------------------
      for (pp = objects1->begin(); pp != objects1->end(); pp++) {
        COMET::IHandle<COMET::IReconBase> object = *pp;
        objects3->push_back(object);
      }
    }

    if (objects2) {
      COMETVerbose("   #" << objects2->GetName() << " objects:  " << objects2->size());

      //-------- Loop over DET1 objects -------------------
      for (pp = objects2->begin(); pp != objects2->end(); pp++) {
        COMET::IHandle<COMET::IReconBase> object = *pp;
        objects3->push_back(object);
      }
    }

    COMETVerbose("   ---> #" << objects3->GetName() << " objects:  " << objects3->size());
  }

  //*****************************************************************************
  /// Combine the objects in objects1 and objects2 into objects3.
  void MergeContainers(const COMET::IReconObjectContainer& objects1, const COMET::IReconObjectContainer& objects2, COMET::IReconObjectContainer& objects3) {
  //*****************************************************************************
    COMETVerbose("Merging containers " << objects1.GetName() << "  and " << objects2.GetName() << ": ");

    COMET::IReconObjectContainer::const_iterator pp;

    COMETVerbose("   #" << objects1.GetName() << " objects:  " << objects1.size());

    //-------- Loop over DET1 objects -------------------
    for (pp = objects1.begin(); pp != objects1.end(); pp++) {
      COMET::IHandle<COMET::IReconBase> object = *pp;
      objects3.push_back(object);
    }

    COMETVerbose("   #" << objects2.GetName() << " objects:  " << objects2.size());

    //-------- Loop over DET1 objects -------------------
    for (pp = objects2.begin(); pp != objects2.end(); pp++) {
      COMET::IHandle<COMET::IReconBase> object = *pp;
      objects3.push_back(object);
    }

    COMETVerbose("   ---> #" << objects3.GetName() << " objects:  " << objects3.size());
  }

  //*****************************************************************************
  /// Retrieve from the event the specified container from the specified fit.
  /// For example, TrackingUtils::GetContainer(event, "ReconECal", "final")
  COMET::IHandle<COMET::IReconObjectContainer> GetContainer(COMET::ICOMETEvent& event, const std::string& fit, const std::string& container_name) {
  //*****************************************************************************
    COMET::IHandle<COMET::IReconObjectContainer> container = COMET::IHandle<COMET::IReconObjectContainer>();

    if (event.GetFit(fit.c_str())) {
      if (event.GetFit(fit.c_str())->GetResultsContainer(container_name.c_str())) {
        container = (event.GetFit(fit.c_str())->GetResultsContainer(container_name.c_str()));
      }
    }

    return container;
  }
  //*******************************************
  void  GetUnusedObjects(const COMET::IReconObjectContainer& inputContainer, 
      COMET::IReconObjectContainer& outputContainer){
  //*******************************************

    COMET::IReconObjectContainer::const_iterator objectIter1;
    for(objectIter1 = inputContainer.begin(); objectIter1 != inputContainer.end(); objectIter1++){
      COMET::IHandle<COMET::IReconBase> rbase = *objectIter1;  
      // Object not used by another
      if (!(rbase->CheckStatus(COMET::IReconBase::kRan))){
        outputContainer.push_back(rbase);
      }            
    }

  }


  //*****************************************************************************
  COMET::IHandle<COMET::IReconBase> GetOriginalObject(COMET::IHandle<COMET::IReconBase> object){
  //*****************************************************************************

    COMET::IHandle<COMET::IReconObjectContainer> original =
      object->Get<COMET::IReconObjectContainer>("original");

    if (!original) return COMET::IHandle<COMET::IReconBase>();

    return *(original->begin());

  }

}