// -*- C++ -*-
//
// Package:    ConversionSeedFilterCharge
// Class:      ConversionSeedFilterCharge
//
/**\class ConversionSeedFilterCharge ConversionSeedFilterCharge.cc RecoTracker/TkSeedGenerator/src/ConversionSeedFilterCharge.cc

 Description: <one line class summary>

 Implementation:
     <Notes on implementation>
*/
//
// Original Author:  Giuseppe Cerati
//         Created:  Thu Mar 11 10:48:48 CET 2010
//
//

#include <memory>
#include "DataFormats/Math/interface/deltaPhi.h"
#include "DataFormats/TrajectorySeed/interface/TrajectorySeedCollection.h"
#include "FWCore/Framework/interface/Event.h"
#include "FWCore/Framework/interface/Frameworkfwd.h"
#include "FWCore/Framework/interface/MakerMacros.h"
#include "FWCore/Framework/interface/global/EDProducer.h"
#include "FWCore/ParameterSet/interface/ParameterSet.h"
#include "Geometry/CommonDetUnit/interface/GeomDet.h"
#include "Geometry/Records/interface/TrackerDigiGeometryRecord.h"
#include "Geometry/TrackerGeometryBuilder/interface/TrackerGeometry.h"
#include "MagneticField/Engine/interface/MagneticField.h"
#include "MagneticField/Records/interface/IdealMagneticFieldRecord.h"
#include "TrackingTools/Records/interface/TrackingComponentsRecord.h"
#include "TrackingTools/TrajectoryState/interface/TrajectoryStateOnSurface.h"
#include "TrackingTools/TrajectoryState/interface/TrajectoryStateTransform.h"

class ConversionSeedFilterCharge : public edm::global::EDProducer<> {
public:
  explicit ConversionSeedFilterCharge(const edm::ParameterSet&);
  ~ConversionSeedFilterCharge() override = default;

private:
  void produce(edm::StreamID, edm::Event&, const edm::EventSetup&) const override;
  const edm::ESGetToken<TrackerGeometry, TrackerDigiGeometryRecord> geomToken;
  const edm::ESGetToken<MagneticField, IdealMagneticFieldRecord> mfToken;

  edm::EDGetTokenT<TrajectorySeedCollection> inputCollPos;
  edm::EDGetTokenT<TrajectorySeedCollection> inputCollNeg;
  const double deltaPhiCut, deltaCotThetaCut, deltaRCut, deltaZCut;

  const uint32_t maxInputSeeds;
};

ConversionSeedFilterCharge::ConversionSeedFilterCharge(const edm::ParameterSet& cfg)
    : geomToken(esConsumes()),
      mfToken(esConsumes()),
      inputCollPos(consumes<TrajectorySeedCollection>(cfg.getParameter<edm::InputTag>("seedCollectionPos"))),
      inputCollNeg(consumes<TrajectorySeedCollection>(cfg.getParameter<edm::InputTag>("seedCollectionNeg"))),
      deltaPhiCut(cfg.getParameter<double>("deltaPhiCut")),
      deltaCotThetaCut(cfg.getParameter<double>("deltaCotThetaCut")),
      deltaRCut(cfg.getParameter<double>("deltaRCut")),
      deltaZCut(cfg.getParameter<double>("deltaZCut")),
      maxInputSeeds(cfg.getParameter<uint32_t>("maxInputSeeds")) {
  produces<TrajectorySeedCollection>();
}

void ConversionSeedFilterCharge::produce(edm::StreamID, edm::Event& iEvent, const edm::EventSetup& iSetup) const {
  using namespace edm;
  using namespace std;
  Handle<TrajectorySeedCollection> pInPos;
  iEvent.getByToken(inputCollPos, pInPos);
  Handle<TrajectorySeedCollection> pInNeg;
  iEvent.getByToken(inputCollNeg, pInNeg);

  const TrackerGeometry* theG = &iSetup.getData(geomToken);
  const MagneticField* theMF = &iSetup.getData(mfToken);

  auto result = std::make_unique<TrajectorySeedCollection>();
  result->reserve(pInPos->size());

  if (pInPos->size() < maxInputSeeds && pInNeg->size() < maxInputSeeds) {
    edm::LogInfo("ConversionSeedFilterCharge")
        << "New Event \t Pos " << pInPos->size() << " \t Neg " << pInNeg->size() << std::endl;

    std::vector<int> inResult;
    for (TrajectorySeedCollection::const_iterator iS1 = pInPos->begin(); iS1 != pInPos->end(); ++iS1) {
      PTrajectoryStateOnDet state1 = iS1->startingState();
      DetId detId1(state1.detId());
      TrajectoryStateOnSurface tsos1 =
          trajectoryStateTransform::transientState(state1, &(theG->idToDet(detId1)->surface()), theMF);
      double phi1 = tsos1.globalMomentum().phi();
      double cotTheta1 = 1 / tan(tsos1.globalMomentum().theta());
      double r1 = tsos1.globalPosition().perp();
      double z1 = tsos1.globalPosition().z();
      //cout << "detId1=" << detId1 << " phi1=" << phi1 << " cotTheta1=" << cotTheta1 << " r1=" << r1 << " z1=" << z1 << endl;

      bool pushed = false;
      for (TrajectorySeedCollection::const_iterator iS2 = pInNeg->begin(); iS2 != pInNeg->end(); ++iS2) {
        PTrajectoryStateOnDet state2 = iS2->startingState();
        DetId detId2(state2.detId());
        TrajectoryStateOnSurface tsos2 =
            trajectoryStateTransform::transientState(state2, &(theG->idToDet(detId2)->surface()), theMF);

        double deltaPhi = fabs(reco::deltaPhi(phi1, tsos2.globalMomentum().phi()));
        double deltaCotTheta = fabs(cotTheta1 - 1 / tan(tsos2.globalMomentum().theta()));
        double deltaR = fabs(r1 - tsos2.globalPosition().perp());
        double deltaZ = fabs(z1 - tsos2.globalPosition().z());

        //	 double phi2 = tsos2.globalMomentum().phi();
        //	 double cotTheta2 = 2/tan(tsos2.globalMomentum().theta());
        //	 double r2 = tsos2.globalPosition().perp();
        //	 double z2 = tsos2.globalPosition().z();
        //cout << "j=" << j << " detId2=" << detId2 << " phi2=" << phi2 << " cotTheta2=" << cotTheta2 << " r2=" << r2 << " z2=" << z2 << endl;

        if (deltaPhi < deltaPhiCut && deltaCotTheta < deltaCotThetaCut && deltaR < deltaRCut && deltaZ < deltaZCut) {
          edm::LogInfo("ConversionSeedFilterCharge") << "[SearchAmongSeeds] match in pos " << iS1 - pInPos->begin()
                                                     << " " << iS2 - pInNeg->begin() << std::endl;
          //cout << "match" << endl;
          if (!pushed) {
            result->push_back(*iS1);
            pushed = true;
          }
          if (std::find(inResult.begin(), inResult.end(), iS2 - pInNeg->begin()) == inResult.end()) {
            result->push_back(*iS2);
            inResult.push_back(iS2 - pInNeg->begin());
          }
        }
      }
    }
  }

  edm::LogInfo("ConversionSeedFilterCharge") << "\nNew Event : result size " << result->size() << std::endl;

  //cout << "sizes: pInPos=" << pInPos->size() << " pInNeg=" << pInNeg->size() << " result=" << result->size() << endl;
  iEvent.put(std::move(result));
}

DEFINE_FWK_MODULE(ConversionSeedFilterCharge);
