////////////////////////////////////////////////////////////////////////////
//                    Header file for this                                    //
////////////////////////////////////////////////////////////////////////////////
#include "HLTriggerOffline/Egamma/interface/EmDQMReco.h"

////////////////////////////////////////////////////////////////////////////////
//                    Collaborating Class Header                              //
////////////////////////////////////////////////////////////////////////////////
#include "DataFormats/EgammaCandidates/interface/Electron.h"
#include "DataFormats/HLTReco/interface/TriggerEventWithRefs.h"
#include "DataFormats/RecoCandidate/interface/RecoEcalCandidate.h"
#include "FWCore/Framework/interface/Frameworkfwd.h"
#include "FWCore/Framework/interface/MakerMacros.h"
#include "FWCore/ParameterSet/interface/ParameterSet.h"
//#include "DataFormats/EgammaCandidates/interface/PhotonFwd.h"
//#include "DataFormats/EgammaCandidates/interface/Photon.h"
#include "DataFormats/L1Trigger/interface/L1EmParticle.h"
#include "DataFormats/L1Trigger/interface/L1EmParticleFwd.h"
//#include "SimDataFormats/HepMCProduct/interface/HepMCProduct.h"
#include "DataFormats/Common/interface/AssociationMap.h"
#include "FWCore/MessageLogger/interface/MessageLogger.h"
#include "SimDataFormats/GeneratorProducts/interface/HepMCProduct.h"

#include "DataFormats/Common/interface/Handle.h"
#include "DataFormats/Common/interface/RefToBase.h"
#include "FWCore/ServiceRegistry/interface/Service.h"
//#include "PhysicsTools/UtilAlgos/interface/TFileService.h"
#include "DataFormats/Common/interface/TriggerResults.h"
#include "DataFormats/HLTReco/interface/TriggerEvent.h"
#include "DataFormats/HLTReco/interface/TriggerObject.h"
#include "DataFormats/HLTReco/interface/TriggerTypeDefs.h"
#include "FWCore/Utilities/interface/Exception.h"
#include <boost/format.hpp>
////////////////////////////////////////////////////////////////////////////////
//                           Root include files                               //
////////////////////////////////////////////////////////////////////////////////
#include "TDirectory.h"
#include "TFile.h"
#include "TH1F.h"
#include <Math/VectorUtil.h>
#include <iostream>
#include <string>
using namespace ROOT::Math::VectorUtil;

//----------------------------------------------------------------------
// class EmDQMReco::FourVectorMonitorElements
//----------------------------------------------------------------------
EmDQMReco::FourVectorMonitorElements::FourVectorMonitorElements(EmDQMReco *_parent,
                                                                DQMStore::IBooker &iBooker,
                                                                const std::string &histogramNameTemplate,
                                                                const std::string &histogramTitleTemplate)
    : parent(_parent) {
  // introducing variables for better code readability later on
  std::string histName;
  std::string histTitle;

  // et
  histName = boost::str(boost::format(histogramNameTemplate) % "et");
  histTitle = boost::str(boost::format(histogramTitleTemplate) % "E_{T}");
  etMonitorElement =
      iBooker.book1D(histName.c_str(), histTitle.c_str(), parent->plotBins, parent->plotPtMin, parent->plotPtMax);

  // eta
  histName = boost::str(boost::format(histogramNameTemplate) % "eta");
  histTitle = boost::str(boost::format(histogramTitleTemplate) % "#eta");
  etaMonitorElement =
      iBooker.book1D(histName.c_str(), histTitle.c_str(), parent->plotBins, -parent->plotEtaMax, parent->plotEtaMax);

  // phi
  histName = boost::str(boost::format(histogramNameTemplate) % "phi");
  histTitle = boost::str(boost::format(histogramTitleTemplate) % "#phi");
  phiMonitorElement =
      iBooker.book1D(histName.c_str(), histTitle.c_str(), parent->plotBins, -parent->plotPhiMax, parent->plotPhiMax);
}

//----------------------------------------------------------------------

void EmDQMReco::FourVectorMonitorElements::fill(const math::XYZTLorentzVector &momentum) {
  etMonitorElement->Fill(momentum.Et());
  etaMonitorElement->Fill(momentum.eta());
  phiMonitorElement->Fill(momentum.phi());
}

//----------------------------------------------------------------------

////////////////////////////////////////////////////////////////////////////////
//                             Constructor                                    //
////////////////////////////////////////////////////////////////////////////////
EmDQMReco::EmDQMReco(const edm::ParameterSet &pset) {
  ////////////////////////////////////////////////////////////
  //          Read from configuration file                  //
  ////////////////////////////////////////////////////////////
  dirname_ = "HLT/HLTEgammaValidation/" + pset.getParameter<std::string>("@module_label");

  // parameters for generator study
  reqNum = pset.getParameter<unsigned int>("reqNum");
  pdgGen = pset.getParameter<int>("pdgGen");
  recoEtaAcc = pset.getParameter<double>("genEtaAcc");
  recoEtAcc = pset.getParameter<double>("genEtAcc");
  // plotting parameters (untracked because they don't affect the physics)
  plotPtMin = pset.getUntrackedParameter<double>("PtMin", 0.);
  plotPtMax = pset.getUntrackedParameter<double>("PtMax", 1000.);
  plotEtaMax = pset.getUntrackedParameter<double>("EtaMax", 2.7);
  plotPhiMax = pset.getUntrackedParameter<double>("PhiMax", 3.15);
  plotBins = pset.getUntrackedParameter<unsigned int>("Nbins", 50);
  useHumanReadableHistTitles = pset.getUntrackedParameter<bool>("useHumanReadableHistTitles", false);

  triggerNameRecoMonPath = pset.getUntrackedParameter<std::string>("triggerNameRecoMonPath", "HLT_MinBias");
  processNameRecoMonPath = pset.getUntrackedParameter<std::string>("processNameRecoMonPath", "HLT");

  recoElectronsInput = consumes<reco::GsfElectronCollection>(
      pset.getUntrackedParameter<edm::InputTag>("recoElectrons", edm::InputTag("gsfElectrons")));
  recoObjectsEBT = consumes<std::vector<reco::SuperCluster>>(edm::InputTag("correctedHybridSuperClusters"));
  recoObjectsEET =
      consumes<std::vector<reco::SuperCluster>>(edm::InputTag("correctedMulti5x5SuperClustersWithPreshower"));
  hltResultsT = consumes<edm::TriggerResults>(edm::InputTag("TriggerResults", "", processNameRecoMonPath));
  triggerObjT = consumes<trigger::TriggerEventWithRefs>(edm::InputTag("hltTriggerSummaryRAW"));

  // preselction cuts
  // recocutCollection_= pset.getParameter<edm::InputTag>("cutcollection");
  recocut_ = pset.getParameter<int>("cutnum");

  // prescale = 10;
  eventnum = 0;

  // just init
  isHltConfigInitialized_ = false;

  ////////////////////////////////////////////////////////////
  //         Read in the Vector of Parameter Sets.          //
  //           Information for each filter-step             //
  ////////////////////////////////////////////////////////////
  std::vector<edm::ParameterSet> filters = pset.getParameter<std::vector<edm::ParameterSet>>("filters");

  int i = 0;
  for (std::vector<edm::ParameterSet>::iterator filterconf = filters.begin(); filterconf != filters.end();
       filterconf++) {
    theHLTCollectionLabels.push_back(filterconf->getParameter<edm::InputTag>("HLTCollectionLabels"));
    theHLTOutputTypes.push_back(filterconf->getParameter<int>("theHLTOutputTypes"));
    // Grab the human-readable name, if it is not specified, use the Collection
    // Label
    theHLTCollectionHumanNames.push_back(
        filterconf->getUntrackedParameter<std::string>("HLTCollectionHumanName", theHLTCollectionLabels[i].label()));

    std::vector<double> bounds = filterconf->getParameter<std::vector<double>>("PlotBounds");
    // If the size of plot "bounds" vector != 2, abort
    assert(bounds.size() == 2);
    plotBounds.push_back(std::pair<double, double>(bounds[0], bounds[1]));
    isoNames.push_back(filterconf->getParameter<std::vector<edm::InputTag>>("IsoCollections"));

    for (unsigned int i = 0; i < isoNames.back().size(); i++) {
      switch (theHLTOutputTypes.back()) {
        case trigger::TriggerL1NoIsoEG:
          histoFillerL1NonIso->isoNameTokens_.push_back(
              consumes<edm::AssociationMap<edm::OneToValue<l1extra::L1EmParticleCollection, float>>>(
                  isoNames.back()[i]));
          break;
        case trigger::TriggerL1IsoEG:  // Isolated Level 1
          histoFillerL1Iso->isoNameTokens_.push_back(
              consumes<edm::AssociationMap<edm::OneToValue<l1extra::L1EmParticleCollection, float>>>(
                  isoNames.back()[i]));
          break;
        case trigger::TriggerPhoton:  // Photon
          histoFillerPho->isoNameTokens_.push_back(
              consumes<edm::AssociationMap<edm::OneToValue<reco::RecoEcalCandidateCollection, float>>>(
                  isoNames.back()[i]));
          break;
        case trigger::TriggerElectron:  // Electron
          histoFillerEle->isoNameTokens_.push_back(
              consumes<edm::AssociationMap<edm::OneToValue<reco::ElectronCollection, float>>>(isoNames.back()[i]));
          break;
        case trigger::TriggerCluster:  // TriggerCluster
          histoFillerClu->isoNameTokens_.push_back(
              consumes<edm::AssociationMap<edm::OneToValue<reco::RecoEcalCandidateCollection, float>>>(
                  isoNames.back()[i]));
          break;
        default:
          throw(cms::Exception("Release Validation Error") << "HLT output type not implemented: theHLTOutputTypes[n]");
      }
    }

    // If the size of the isoNames vector is not greater than zero, abort
    assert(!isoNames.back().empty());
    if (isoNames.back().at(0).label() == "none") {
      plotiso.push_back(false);
    } else {
      plotiso.push_back(true);
    }
    i++;
  }  // END of loop over parameter sets

  // Record number of HLTCollectionLabels
  numOfHLTCollectionLabels = theHLTCollectionLabels.size();
}

///
///
///
void EmDQMReco::dqmBeginRun(const edm::Run &iRun, const edm::EventSetup &iSetup) {
  bool isHltConfigChanged = false;  // change of cfg at run boundaries?
  isHltConfigInitialized_ = hltConfig_.init(iRun, iSetup, "HLT", isHltConfigChanged);
}

////////////////////////////////////////////////////////////////////////////////
//       book DQM histograms                                                  //
////////////////////////////////////////////////////////////////////////////////
void EmDQMReco::bookHistograms(DQMStore::IBooker &iBooker, edm::Run const &iRun, edm::EventSetup const &iSetup) {
  // edm::Service<TFileService> fs;
  iBooker.setCurrentFolder(dirname_);

  ////////////////////////////////////////////////////////////
  //  Set up Histogram of Effiency vs Step.                 //
  //   theHLTCollectionLabels is a vector of InputTags      //
  //    from the configuration file.                        //
  ////////////////////////////////////////////////////////////

  std::string histName = "total_eff";
  std::string histTitle = "total events passing";
  // This plot will have bins equal to 2+(number of
  //        HLTCollectionLabels in the config file)
  totalreco = iBooker.book1D(
      histName.c_str(), histTitle.c_str(), numOfHLTCollectionLabels + 2, 0, numOfHLTCollectionLabels + 2);
  totalreco->setBinLabel(numOfHLTCollectionLabels + 1, "Total");
  totalreco->setBinLabel(numOfHLTCollectionLabels + 2, "Reco");
  for (unsigned int u = 0; u < numOfHLTCollectionLabels; u++) {
    totalreco->setBinLabel(u + 1, theHLTCollectionLabels[u].label());
  }

  histName = "total_eff_RECO_matched";
  histTitle = "total events passing (Reco matched)";
  totalmatchreco = iBooker.book1D(
      histName.c_str(), histTitle.c_str(), numOfHLTCollectionLabels + 2, 0, numOfHLTCollectionLabels + 2);
  totalmatchreco->setBinLabel(numOfHLTCollectionLabels + 1, "Total");
  totalmatchreco->setBinLabel(numOfHLTCollectionLabels + 2, "Reco");
  for (unsigned int u = 0; u < numOfHLTCollectionLabels; u++) {
    totalmatchreco->setBinLabel(u + 1, theHLTCollectionLabels[u].label());
  }

  // MonitorElement* tmphisto;
  MonitorElement *tmpiso;

  ////////////////////////////////////////////////////////////
  // Set up generator-level histograms                      //
  ////////////////////////////////////////////////////////////
  std::string pdgIdString;
  switch (pdgGen) {
    case 11:
      pdgIdString = "Electron";
      break;
    case 22:
      pdgIdString = "Photon";
      break;
    default:
      pdgIdString = "Particle";
  }

  //--------------------

  // reco
  // (note that reset(..) must be used to set the value of the scoped_ptr...)
  histReco.reset(new FourVectorMonitorElements(this,
                                               iBooker,
                                               "reco_%s",  // pattern for histogram name
                                               "%s of " + pdgIdString + "s"));

  //--------------------

  // monpath
  histRecoMonpath.reset(new FourVectorMonitorElements(this,
                                                      iBooker,
                                                      "reco_%s_monpath",  // pattern for histogram name
                                                      "%s of " + pdgIdString + "s monpath"));

  //--------------------

  // TODO: WHAT ARE THESE HISTOGRAMS FOR ? THEY SEEM NEVER REFERENCED ANYWHERE
  // IN THIS FILE... final X monpath
  histMonpath.reset(new FourVectorMonitorElements(this,
                                                  iBooker,
                                                  "final_%s_monpath",  // pattern for histogram name
                                                  "Final %s Monpath"));

  //--------------------

  ////////////////////////////////////////////////////////////
  //  Set up histograms of HLT objects                      //
  ////////////////////////////////////////////////////////////

  // Determine what strings to use for histogram titles
  std::vector<std::string> HltHistTitle;
  if (theHLTCollectionHumanNames.size() == numOfHLTCollectionLabels && useHumanReadableHistTitles) {
    HltHistTitle = theHLTCollectionHumanNames;
  } else {
    for (unsigned int i = 0; i < numOfHLTCollectionLabels; i++) {
      HltHistTitle.push_back(theHLTCollectionLabels[i].label());
    }
  }

  for (unsigned int i = 0; i < numOfHLTCollectionLabels; i++) {
    //--------------------
    // distributions of HLT objects passing filter i
    //--------------------

    //    // Et
    //    histName = theHLTCollectionLabels[i].label()+"et_all";
    //    histTitle = HltHistTitle[i]+" Et (ALL)";
    //    tmphisto =
    //    iBooker.book1D(histName.c_str(),histTitle.c_str(),plotBins,plotPtMin,plotPtMax);
    //    ethist.push_back(tmphisto);
    //
    //    // Eta
    //    histName = theHLTCollectionLabels[i].label()+"eta_all";
    //    histTitle = HltHistTitle[i]+" #eta (ALL)";
    //    tmphisto =
    //    iBooker.book1D(histName.c_str(),histTitle.c_str(),plotBins,-plotEtaMax,plotEtaMax);
    //    etahist.push_back(tmphisto);
    //
    //    // phi
    //    histName = theHLTCollectionLabels[i].label()+"phi_all";
    //    histTitle = HltHistTitle[i]+" #phi (ALL)";
    //    tmphisto =
    //    iBooker.book1D(histName.c_str(),histTitle.c_str(),plotBins,-plotPhiMax,plotPhiMax);
    //    phiHist.push_back(tmphisto);

    standardHist.push_back(std::make_unique<FourVectorMonitorElements>(
        this,
        iBooker,
        theHLTCollectionLabels[i].label() + "%s_all",  // histogram name
        HltHistTitle[i] + " %s (ALL)"                  // histogram title
        ));

    //--------------------
    // distributions of reco object matching HLT object passing filter i
    //--------------------

    // Et
    //    histName = theHLTCollectionLabels[i].label()+"et_RECO_matched";
    //    histTitle = HltHistTitle[i]+" Et (RECO matched)";
    //    tmphisto =
    //    iBooker.book1D(histName.c_str(),histTitle.c_str(),plotBins,plotPtMin,plotPtMax);
    //    ethistmatchreco.push_back(tmphisto);

    //    // Eta
    //    histName = theHLTCollectionLabels[i].label()+"eta_RECO_matched";
    //    histTitle = HltHistTitle[i]+" #eta (RECO matched)";
    //    tmphisto =
    //    iBooker.book1D(histName.c_str(),histTitle.c_str(),plotBins,-plotEtaMax,plotEtaMax);
    //    etahistmatchreco.push_back(tmphisto);
    //
    //    // phi
    //    histName = theHLTCollectionLabels[i].label()+"phi_RECO_matched";
    //    histTitle = HltHistTitle[i]+" #phi (RECO matched)";
    //    tmphisto =
    //    iBooker.book1D(histName.c_str(),histTitle.c_str(),plotBins,-plotPhiMax,plotPhiMax);
    //    phiHistMatchReco.push_back(tmphisto);
    histMatchReco.push_back(std::make_unique<FourVectorMonitorElements>(
        this,
        iBooker,
        theHLTCollectionLabels[i].label() + "%s_RECO_matched",  // histogram name
        HltHistTitle[i] + " %s (RECO matched)"                  // histogram title
        ));

    //--------------------
    // distributions of reco object matching HLT object passing filter i
    //--------------------

    //    // Et
    //    histName =
    //    theHLTCollectionLabels[i].label()+"et_RECO_matched_monpath"; histTitle
    //    = HltHistTitle[i]+" Et (RECO matched, monpath)"; tmphisto =
    //    iBooker.book1D(histName.c_str(),histTitle.c_str(),plotBins,plotPtMin,plotPtMax);
    //    ethistmatchrecomonpath.push_back(tmphisto);
    //
    //    // Eta
    //    histName =
    //    theHLTCollectionLabels[i].label()+"eta_RECO_matched_monpath";
    //    histTitle = HltHistTitle[i]+" #eta (RECO matched, monpath)";
    //    tmphisto =
    //    iBooker.book1D(histName.c_str(),histTitle.c_str(),plotBins,-plotEtaMax,plotEtaMax);
    //    etahistmatchrecomonpath.push_back(tmphisto);
    //
    //    // phi
    //    histName =
    //    theHLTCollectionLabels[i].label()+"phi_RECO_matched_monpath";
    //    histTitle = HltHistTitle[i]+" #phi (RECO matched, monpath)";
    //    tmphisto =
    //    iBooker.book1D(histName.c_str(),histTitle.c_str(),plotBins,-plotPhiMax,plotPhiMax);
    //    phiHistMatchRecoMonPath.push_back(tmphisto);

    histMatchRecoMonPath.push_back(std::make_unique<FourVectorMonitorElements>(
        this,
        iBooker,
        theHLTCollectionLabels[i].label() + "%s_RECO_matched_monpath",  // histogram name
        HltHistTitle[i] + " %s (RECO matched, monpath)"                 // histogram title
        ));
    //--------------------
    // distributions of HLT object that is closest delta-R match to sorted reco
    // particle(s)
    //--------------------

    // Et
    //    histName  = theHLTCollectionLabels[i].label()+"et_reco";
    //    histTitle = HltHistTitle[i]+" Et (reco)";
    //    tmphisto  =
    //    iBooker.book1D(histName.c_str(),histTitle.c_str(),plotBins,plotPtMin,plotPtMax);
    //    histEtOfHltObjMatchToReco.push_back(tmphisto);
    //
    //    // eta
    //    histName  = theHLTCollectionLabels[i].label()+"eta_reco";
    //    histTitle = HltHistTitle[i]+" eta (reco)";
    //    tmphisto  =
    //    iBooker.book1D(histName.c_str(),histTitle.c_str(),plotBins,-plotEtaMax,plotEtaMax);
    //    histEtaOfHltObjMatchToReco.push_back(tmphisto);
    //
    //    // phi
    //    histName  = theHLTCollectionLabels[i].label()+"phi_reco";
    //    histTitle = HltHistTitle[i]+" phi (reco)";
    //    tmphisto  =
    //    iBooker.book1D(histName.c_str(),histTitle.c_str(),plotBins,-plotPhiMax,plotPhiMax);
    //    histPhiOfHltObjMatchToReco.push_back(tmphisto);

    histHltObjMatchToReco.push_back(std::make_unique<FourVectorMonitorElements>(
        this,
        iBooker,
        theHLTCollectionLabels[i].label() + "%s_reco",  // histogram name
        HltHistTitle[i] + " %s (reco)"                  // histogram title
        ));

    //--------------------

    if (!plotiso[i]) {
      tmpiso = nullptr;
      etahistiso.push_back(tmpiso);
      ethistiso.push_back(tmpiso);
      phiHistIso.push_back(tmpiso);

      etahistisomatchreco.push_back(tmpiso);
      ethistisomatchreco.push_back(tmpiso);
      phiHistIsoMatchReco.push_back(tmpiso);

      histEtaIsoOfHltObjMatchToReco.push_back(tmpiso);
      histEtIsoOfHltObjMatchToReco.push_back(tmpiso);
      histPhiIsoOfHltObjMatchToReco.push_back(tmpiso);

    } else {
      //--------------------
      // 2D plot: Isolation values vs X for all objects
      //--------------------

      // X = eta
      histName = theHLTCollectionLabels[i].label() + "eta_isolation_all";
      histTitle = HltHistTitle[i] + " isolation vs #eta (all)";
      tmpiso = iBooker.book2D(histName.c_str(),
                              histTitle.c_str(),
                              plotBins,
                              -plotEtaMax,
                              plotEtaMax,
                              plotBins,
                              plotBounds[i].first,
                              plotBounds[i].second);
      etahistiso.push_back(tmpiso);

      // X = et
      histName = theHLTCollectionLabels[i].label() + "et_isolation_all";
      histTitle = HltHistTitle[i] + " isolation vs Et (all)";
      tmpiso = iBooker.book2D(histName.c_str(),
                              histTitle.c_str(),
                              plotBins,
                              plotPtMin,
                              plotPtMax,
                              plotBins,
                              plotBounds[i].first,
                              plotBounds[i].second);
      ethistiso.push_back(tmpiso);

      // X = phi
      histName = theHLTCollectionLabels[i].label() + "phi_isolation_all";
      histTitle = HltHistTitle[i] + " isolation vs #phi (all)";
      tmpiso = iBooker.book2D(histName.c_str(),
                              histTitle.c_str(),
                              plotBins,
                              -plotPhiMax,
                              plotPhiMax,
                              plotBins,
                              plotBounds[i].first,
                              plotBounds[i].second);
      phiHistIso.push_back(tmpiso);

      //--------------------
      // 2D plot: Isolation values vs X for reco matched objects
      //--------------------

      // X = eta
      histName = theHLTCollectionLabels[i].label() + "eta_isolation_RECO_matched";
      histTitle = HltHistTitle[i] + " isolation vs #eta (reco matched)";
      tmpiso = iBooker.book2D(histName.c_str(),
                              histTitle.c_str(),
                              plotBins,
                              -plotEtaMax,
                              plotEtaMax,
                              plotBins,
                              plotBounds[i].first,
                              plotBounds[i].second);
      etahistisomatchreco.push_back(tmpiso);

      // X = et
      histName = theHLTCollectionLabels[i].label() + "et_isolation_RECO_matched";
      histTitle = HltHistTitle[i] + " isolation vs Et (reco matched)";
      tmpiso = iBooker.book2D(histName.c_str(),
                              histTitle.c_str(),
                              plotBins,
                              plotPtMin,
                              plotPtMax,
                              plotBins,
                              plotBounds[i].first,
                              plotBounds[i].second);
      ethistisomatchreco.push_back(tmpiso);

      // X = eta
      histName = theHLTCollectionLabels[i].label() + "phi_isolation_RECO_matched";
      histTitle = HltHistTitle[i] + " isolation vs #phi (reco matched)";
      tmpiso = iBooker.book2D(histName.c_str(),
                              histTitle.c_str(),
                              plotBins,
                              -plotPhiMax,
                              plotPhiMax,
                              plotBins,
                              plotBounds[i].first,
                              plotBounds[i].second);
      phiHistIsoMatchReco.push_back(tmpiso);

      //--------------------
      // 2D plot: Isolation values vs X for HLT object that
      // is closest delta-R match to sorted reco particle(s)
      //--------------------

      // X = eta
      histName = theHLTCollectionLabels[i].label() + "eta_isolation_reco";
      histTitle = HltHistTitle[i] + " isolation vs #eta (reco)";
      tmpiso = iBooker.book2D(histName.c_str(),
                              histTitle.c_str(),
                              plotBins,
                              -plotEtaMax,
                              plotEtaMax,
                              plotBins,
                              plotBounds[i].first,
                              plotBounds[i].second);
      histEtaIsoOfHltObjMatchToReco.push_back(tmpiso);

      // X = et
      histName = theHLTCollectionLabels[i].label() + "et_isolation_reco";
      histTitle = HltHistTitle[i] + " isolation vs Et (reco)";
      tmpiso = iBooker.book2D(histName.c_str(),
                              histTitle.c_str(),
                              plotBins,
                              plotPtMin,
                              plotPtMax,
                              plotBins,
                              plotBounds[i].first,
                              plotBounds[i].second);
      histEtIsoOfHltObjMatchToReco.push_back(tmpiso);

      // X = phi
      histName = theHLTCollectionLabels[i].label() + "phi_isolation_reco";
      histTitle = HltHistTitle[i] + " isolation vs #phi (reco)";
      tmpiso = iBooker.book2D(histName.c_str(),
                              histTitle.c_str(),
                              plotBins,
                              -plotPhiMax,
                              plotPhiMax,
                              plotBins,
                              plotBounds[i].first,
                              plotBounds[i].second);
      histPhiIsoOfHltObjMatchToReco.push_back(tmpiso);
      //--------------------

    }  // END of HLT histograms
  }
}

////////////////////////////////////////////////////////////////////////////////
//                                Destructor                                  //
////////////////////////////////////////////////////////////////////////////////
EmDQMReco::~EmDQMReco() {}

////////////////////////////////////////////////////////////////////////////////
//                     method called to for each event                        //
////////////////////////////////////////////////////////////////////////////////
void EmDQMReco::analyze(const edm::Event &event, const edm::EventSetup &setup) {
  // protect from hlt config failure
  if (!isHltConfigInitialized_)
    return;

  eventnum++;
  bool plotMonpath = false;
  bool plotReco = true;

  edm::Handle<edm::View<reco::Candidate>> recoObjects;
  edm::Handle<std::vector<reco::SuperCluster>> recoObjectsEB;
  edm::Handle<std::vector<reco::SuperCluster>> recoObjectsEE;

  if (pdgGen == 11) {
    event.getByToken(recoElectronsInput, recoObjects);

    if (recoObjects->size() < (unsigned int)recocut_) {
      // edm::LogWarning("EmDQMReco") << "Less than "<< recocut_ <<" Reco
      // particles with pdgId=" << pdgGen << ".  Only " <<
      // cutRecoCounter->size() << " particles.";
      return;
    }
  } else if (pdgGen == 22) {
    event.getByToken(recoObjectsEBT, recoObjectsEB);
    event.getByToken(recoObjectsEET, recoObjectsEE);

    if (recoObjectsEB->size() + recoObjectsEE->size() < (unsigned int)recocut_) {
      // edm::LogWarning("EmDQMReco") << "Less than "<< recocut_ <<" Reco
      // particles with pdgId=" << pdgGen << ".  Only " << cutRecoCounter.size()
      // << " particles.";
      return;
    }
  }

  edm::Handle<edm::TriggerResults> HLTR;
  event.getByToken(hltResultsT, HLTR);

  ///
  /// NOTE:
  /// hltConfigProvider initialization has been moved to beginRun()
  ///

  /* if (theHLTCollectionHumanNames[0] == "hltL1sRelaxedSingleEgammaEt8"){
    triggerIndex = hltConfig.triggerIndex("HLT_L1SingleEG8");
  } else if (theHLTCollectionHumanNames[0] == "hltL1sRelaxedSingleEgammaEt5") {
    triggerIndex = hltConfig.triggerIndex("HLT_L1SingleEG5");
  } else if (theHLTCollectionHumanNames[0] == "hltL1sRelaxedDoubleEgammaEt5") {
    triggerIndex = hltConfig.triggerIndex("HLT_L1DoubleEG5");
  } else {
    triggerIndex = hltConfig.triggerIndex("");
    } */

  unsigned int triggerIndex;
  triggerIndex = hltConfig_.triggerIndex(triggerNameRecoMonPath);

  // triggerIndex must be less than the size of HLTR or you get a CMSException
  bool isFired = false;
  if (triggerIndex < HLTR->size()) {
    isFired = HLTR->accept(triggerIndex);
  }

  // fill L1 and HLT info
  // get objects possed by each filter
  edm::Handle<trigger::TriggerEventWithRefs> triggerObj;
  event.getByToken(triggerObjT, triggerObj);

  if (!triggerObj.isValid()) {
    edm::LogWarning("EmDQMReco") << "RAW-type HLT results not found, skipping event";
    return;
  }

  ////////////////////////////////////////////////////////////
  //  Fill the bin labeled "Total"                          //
  //   This will be the number of events looked at.         //
  ////////////////////////////////////////////////////////////
  totalreco->Fill(numOfHLTCollectionLabels + 0.5);
  totalmatchreco->Fill(numOfHLTCollectionLabels + .5);

  ////////////////////////////////////////////////////////////
  //  Fill the bin labeled "Total"                          //
  //   This will be the number of events looked at.         //
  ////////////////////////////////////////////////////////////
  // total->Fill(numOfHLTCollectionLabels+0.5);
  // totalmatch->Fill(numOfHLTCollectionLabels+0.5);

  ////////////////////////////////////////////////////////////
  //               Fill reconstruction info                      //
  ////////////////////////////////////////////////////////////
  // the recocut_ highest Et generator objects of the preselected type are our
  // matches

  std::vector<reco::Particle> sortedReco;
  if (plotReco == true) {
    if (pdgGen == 11) {
      for (edm::View<reco::Candidate>::const_iterator recopart = recoObjects->begin(); recopart != recoObjects->end();
           recopart++) {
        reco::Particle tmpcand(
            recopart->charge(), recopart->p4(), recopart->vertex(), recopart->pdgId(), recopart->status());
        sortedReco.push_back(tmpcand);
      }
    } else if (pdgGen == 22) {
      for (std::vector<reco::SuperCluster>::const_iterator recopart2 = recoObjectsEB->begin();
           recopart2 != recoObjectsEB->end();
           recopart2++) {
        float en = recopart2->energy();
        float er = sqrt(pow(recopart2->x(), 2) + pow(recopart2->y(), 2) + pow(recopart2->z(), 2));
        float px = recopart2->energy() * recopart2->x() / er;
        float py = recopart2->energy() * recopart2->y() / er;
        float pz = recopart2->energy() * recopart2->z() / er;
        reco::Candidate::LorentzVector thisLV(px, py, pz, en);
        reco::Particle tmpcand(0, thisLV, math::XYZPoint(0., 0., 0.), 22, 1);
        sortedReco.push_back(tmpcand);
      }
      for (std::vector<reco::SuperCluster>::const_iterator recopart2 = recoObjectsEE->begin();
           recopart2 != recoObjectsEE->end();
           recopart2++) {
        float en = recopart2->energy();
        float er = sqrt(pow(recopart2->x(), 2) + pow(recopart2->y(), 2) + pow(recopart2->z(), 2));
        float px = recopart2->energy() * recopart2->x() / er;
        float py = recopart2->energy() * recopart2->y() / er;
        float pz = recopart2->energy() * recopart2->z() / er;
        reco::Candidate::LorentzVector thisLV(px, py, pz, en);
        reco::Particle tmpcand(0, thisLV, math::XYZPoint(0., 0., 0.), 22, 1);
        sortedReco.push_back(tmpcand);
      }
    }

    std::sort(sortedReco.begin(), sortedReco.end(), pTComparator_);

    // Now the collection of gen particles is sorted by pt.
    // So, remove all particles from the collection so that we
    // only have the top "1 thru recocut_" particles in it

    sortedReco.erase(sortedReco.begin() + recocut_, sortedReco.end());

    for (unsigned int i = 0; i < recocut_; i++) {
      // validity has been implicitily checked by the cut on recocut_ above
      histReco->fill(sortedReco[i].p4());

      //      etreco ->Fill( sortedReco[i].et()  );
      //      etareco->Fill( sortedReco[i].eta() );
      //      phiReco->Fill( sortedReco[i].phi() );

      if (isFired) {
        histRecoMonpath->fill(sortedReco[i].p4());
        plotMonpath = true;
      }

    }  // END of loop over Reconstructed particles

    if (recocut_ >= reqNum)
      totalreco->Fill(numOfHLTCollectionLabels + 1.5);  // this isn't really needed anymore keep for backward comp.
    if (recocut_ >= reqNum)
      totalmatchreco->Fill(numOfHLTCollectionLabels + 1.5);  // this isn't really needed anymore keep for backward comp.
  }

  ////////////////////////////////////////////////////////////
  //            Loop over filter modules                    //
  ////////////////////////////////////////////////////////////
  for (unsigned int n = 0; n < numOfHLTCollectionLabels; n++) {
    // These numbers are from the Parameter Set, such as:
    //   theHLTOutputTypes = cms.uint32(100)
    switch (theHLTOutputTypes[n]) {
      case trigger::TriggerL1NoIsoEG:  // Non-isolated Level 1
        histoFillerL1NonIso->fillHistos(triggerObj, event, n, sortedReco, plotReco, plotMonpath);
        break;
      case trigger::TriggerL1IsoEG:  // Isolated Level 1
        histoFillerL1Iso->fillHistos(triggerObj, event, n, sortedReco, plotReco, plotMonpath);
        break;
      case trigger::TriggerPhoton:  // Photon
        histoFillerPho->fillHistos(triggerObj, event, n, sortedReco, plotReco, plotMonpath);
        break;
      case trigger::TriggerElectron:  // Electron
        histoFillerEle->fillHistos(triggerObj, event, n, sortedReco, plotReco, plotMonpath);
        break;
      case trigger::TriggerCluster:  // TriggerCluster
        histoFillerClu->fillHistos(triggerObj, event, n, sortedReco, plotReco, plotMonpath);
        break;
      default:
        throw(cms::Exception("Release Validation Error") << "HLT output type not implemented: theHLTOutputTypes[n]");
    }
  }  // END of loop over filter modules
}

////////////////////////////////////////////////////////////////////////////////
// fillHistos                                                                 //
//   Called by analyze method.                                                //
////////////////////////////////////////////////////////////////////////////////
template <class T>
void HistoFillerReco<T>::fillHistos(edm::Handle<trigger::TriggerEventWithRefs> &triggerObj,
                                    const edm::Event &iEvent,
                                    unsigned int n,
                                    std::vector<reco::Particle> &sortedReco,
                                    bool plotReco,
                                    bool plotMonpath) {
  std::vector<edm::Ref<T>> recoecalcands;
  if ((triggerObj->filterIndex(dqm->theHLTCollectionLabels[n]) >= triggerObj->size())) {  // only process if available
    return;
  }

  ////////////////////////////////////////////////////////////
  //      Retrieve saved filter objects                     //
  ////////////////////////////////////////////////////////////
  triggerObj->getObjects(
      triggerObj->filterIndex(dqm->theHLTCollectionLabels[n]), dqm->theHLTOutputTypes[n], recoecalcands);
  // Danger: special case, L1 non-isolated
  // needs to be merged with L1 iso
  if (dqm->theHLTOutputTypes[n] == trigger::TriggerL1NoIsoEG) {
    std::vector<edm::Ref<T>> isocands;
    triggerObj->getObjects(triggerObj->filterIndex(dqm->theHLTCollectionLabels[n]), trigger::TriggerL1IsoEG, isocands);
    if (!isocands.empty()) {
      for (unsigned int i = 0; i < isocands.size(); i++)
        recoecalcands.push_back(isocands[i]);
    }
  }  // END of if theHLTOutputTypes == 82

  if (recoecalcands.empty()) {  // stop if no object passed the previous filter
    return;
  }

  if (recoecalcands.size() >= dqm->reqNum)
    dqm->totalreco->Fill(n + 0.5);

  ///////////////////////////////////////////////////
  // check for validity                            //
  // prevents crash in CMSSW_3_1_0_pre6            //
  ///////////////////////////////////////////////////
  for (unsigned int j = 0; j < recoecalcands.size(); j++) {
    if (!(recoecalcands.at(j).isAvailable())) {
      edm::LogError("EmDQMReco") << "Event content inconsistent: TriggerEventWithRefs contains "
                                    "invalid Refs"
                                 << std::endl
                                 << "invalid refs for: " << dqm->theHLTCollectionLabels[n].label();
      return;
    }
  }

  ////////////////////////////////////////////////////////////
  //  Loop over all HLT objects in this filter step, and    //
  //  fill histograms.                                      //
  ////////////////////////////////////////////////////////////
  //  bool foundAllMatches = false;
  //  unsigned int numOfHLTobjectsMatched = 0;
  for (unsigned int i = 0; i < recoecalcands.size(); i++) {
    dqm->standardHist[n]->fill(recoecalcands[i]->p4());

    ////////////////////////////////////////////////////////////
    //  Plot isolation variables (show the not-yet-cut        //
    //  isolation, i.e. associated to next filter)            //
    ////////////////////////////////////////////////////////////
    if (n + 1 < dqm->numOfHLTCollectionLabels) {  // can't plot beyond last
      if (dqm->plotiso[n + 1]) {
        for (unsigned int j = 0; j < isoNameTokens_.size(); j++) {
          edm::Handle<edm::AssociationMap<edm::OneToValue<T, float>>> depMap;
          iEvent.getByToken(isoNameTokens_.at(j), depMap);
          if (depMap.isValid()) {  // Map may not exist if only one candidate
                                   // passes a double filter
            typename edm::AssociationMap<edm::OneToValue<T, float>>::const_iterator mapi =
                depMap->find(recoecalcands[i]);
            if (mapi != depMap->end()) {  // found candidate in isolation map!
              dqm->etahistiso[n + 1]->Fill(recoecalcands[i]->eta(), mapi->val);
              dqm->ethistiso[n + 1]->Fill(recoecalcands[i]->et(), mapi->val);
              dqm->phiHistIso[n + 1]->Fill(recoecalcands[i]->phi(), mapi->val);
            }
          }
        }
      }
    }  // END of if n+1 < then the number of hlt collections
  }

  ////////////////////////////////////////////////////////////
  // Loop over the Reconstructed Particles, and find the        //
  // closest HLT object match.                              //
  ////////////////////////////////////////////////////////////
  if (plotReco == true) {
    for (unsigned int i = 0; i < dqm->recocut_; i++) {
      math::XYZVector currentRecoParticleMomentum = sortedReco[i].momentum();

      // float closestRecoDeltaR = 0.5;
      float closestRecoDeltaR = 1000.;
      int closestRecoEcalCandIndex = -1;
      for (unsigned int j = 0; j < recoecalcands.size(); j++) {
        float deltaR = DeltaR(recoecalcands[j]->momentum(), currentRecoParticleMomentum);

        if (deltaR < closestRecoDeltaR) {
          closestRecoDeltaR = deltaR;
          closestRecoEcalCandIndex = j;
        }
      }

      // If an HLT object was found within some delta-R
      // of this reco particle, store it in a histogram
      if (closestRecoEcalCandIndex >= 0) {
        //        histEtOfHltObjMatchToReco[n] ->Fill(
        //        recoecalcands[closestRecoEcalCandIndex]->et()  );
        //        histEtaOfHltObjMatchToReco[n]->Fill(
        //        recoecalcands[closestRecoEcalCandIndex]->eta() );
        //        histPhiOfHltObjMatchToReco[n]->Fill(
        //        recoecalcands[closestRecoEcalCandIndex]->phi() );

        dqm->histHltObjMatchToReco[n]->fill(recoecalcands[closestRecoEcalCandIndex]->p4());

        // Also store isolation info
        if (n + 1 < dqm->numOfHLTCollectionLabels) {  // can't plot beyond last
          if (dqm->plotiso[n + 1]) {                  // only plot if requested in config
            for (unsigned int j = 0; j < isoNameTokens_.size(); j++) {
              edm::Handle<edm::AssociationMap<edm::OneToValue<T, float>>> depMap;
              iEvent.getByToken(isoNameTokens_.at(j), depMap);
              if (depMap.isValid()) {  // Map may not exist if only one candidate
                                       // passes a double filter
                typename edm::AssociationMap<edm::OneToValue<T, float>>::const_iterator mapi =
                    depMap->find(recoecalcands[closestRecoEcalCandIndex]);
                if (mapi != depMap->end()) {  // found candidate in isolation map!
                  dqm->histEtaIsoOfHltObjMatchToReco[n + 1]->Fill(recoecalcands[closestRecoEcalCandIndex]->eta(),
                                                                  mapi->val);
                  dqm->histEtIsoOfHltObjMatchToReco[n + 1]->Fill(recoecalcands[closestRecoEcalCandIndex]->et(),
                                                                 mapi->val);
                  dqm->histPhiIsoOfHltObjMatchToReco[n + 1]->Fill(recoecalcands[closestRecoEcalCandIndex]->phi(),
                                                                  mapi->val);
                }
              }
            }
          }
        }
      }  // END of if closestEcalCandIndex >= 0
    }

    ////////////////////////////////////////////////////////////
    //        Fill reco matched objects into histograms         //
    ////////////////////////////////////////////////////////////
    unsigned int mtachedRecoParts = 0;
    float minrecodist = 0.3;
    if (n == 0)
      minrecodist = 0.5;  // low L1-resolution => allow wider matching
    for (unsigned int i = 0; i < dqm->recocut_; i++) {
      // match generator candidate
      bool matchThis = false;
      math::XYZVector candDir = sortedReco[i].momentum();
      unsigned int closest = 0;
      double closestDr = 1000.;
      for (unsigned int trigOb = 0; trigOb < recoecalcands.size(); trigOb++) {
        double dr = DeltaR(recoecalcands[trigOb]->momentum(), candDir);
        if (dr < closestDr) {
          closestDr = dr;
          closest = trigOb;
        }
        if (closestDr > minrecodist) {  // it's not really a "match" if it's that far away
          closest = -1;
        } else {
          mtachedRecoParts++;
          matchThis = true;
        }
      }
      if (!matchThis)
        continue;  // only plot matched candidates
      // fill coordinates of mc particle matching trigger object
      //      ethistmatchreco[n] ->Fill( sortedReco[i].et()  );
      //      etahistmatchreco[n]->Fill( sortedReco[i].eta() );
      //      phiHistMatchReco[n]->Fill( sortedReco[i].phi() );
      dqm->histMatchReco[n]->fill(sortedReco[i].p4());

      if (plotMonpath) {
        //        ethistmatchrecomonpath[n]->Fill( sortedReco[i].et() );
        //        etahistmatchrecomonpath[n]->Fill( sortedReco[i].eta() );
        //        phiHistMatchRecoMonPath[n]->Fill( sortedReco[i].phi() );
        dqm->histMatchRecoMonPath[n]->fill(sortedReco[i].p4());
      }
      ////////////////////////////////////////////////////////////
      //  Plot isolation variables (show the not-yet-cut        //
      //  isolation, i.e. associated to next filter)            //
      ////////////////////////////////////////////////////////////
      if (n + 1 < dqm->numOfHLTCollectionLabels) {  // can't plot beyond last
        if (dqm->plotiso[n + 1]) {                  // only plot if requested in config
          for (unsigned int j = 0; j < isoNameTokens_.size(); j++) {
            edm::Handle<edm::AssociationMap<edm::OneToValue<T, float>>> depMapReco;
            iEvent.getByToken(isoNameTokens_.at(j), depMapReco);
            if (depMapReco.isValid()) {  // Map may not exist if only one
                                         // candidate passes a double filter
              typename edm::AssociationMap<edm::OneToValue<T, float>>::const_iterator mapi =
                  depMapReco->find(recoecalcands[closest]);
              if (mapi != depMapReco->end()) {  // found candidate in isolation map!
                dqm->etahistisomatchreco[n + 1]->Fill(sortedReco[i].eta(), mapi->val);
                dqm->ethistisomatchreco[n + 1]->Fill(sortedReco[i].et(), mapi->val);
                dqm->phiHistIsoMatchReco[n + 1]->Fill(sortedReco[i].eta(), mapi->val);
              }
            }
          }
        }
      }  // END of if n+1 < then the number of hlt collections
    }
    // fill total reco matched efficiency
    if (mtachedRecoParts >= dqm->reqNum)
      dqm->totalmatchreco->Fill(n + 0.5);
  }
}

DEFINE_FWK_MODULE(EmDQMReco);


#include <Eigen/Core>
#include "vgl/file/file.hpp"
#include "vgl/file/image_file.hpp"
#include "vgl/gpu_api/gl/shader.hpp"
#include "vgl/gpu_api/gl/vao.hpp"
#include "vgl/gpu_api/gl/texture.hpp"
#include "vgl/gpu_api/gl/framebuffer.hpp"
#include "vgl/gpu_api/gl/buffer.hpp"
#include "vgl/core/window.hpp"
#include "vgl/core/gui.hpp"
#include <glsp/glsp.hpp>
#include <random>

// enable optimus!
extern "C" {
_declspec(dllexport) uint32_t NvOptimusEnablement = 0x00000001;
}
    float gen_random_float(const float lower = 0.0f, const float upper = 1.0f) {
    std::random_device rd;
    std::mt19937 eng(rd());
    const std::uniform_real_distribution<float> uni(lower, upper);
    return uni(eng);
}

int gen_random_int(const int lower = 0, const int upper = 255) {
    std::random_device rd;
    std::mt19937 eng(rd());
    const std::uniform_int_distribution<int> uni(lower, upper);
    return uni(eng);
}

int main() {
    auto w_res = Eigen::Vector2i(1600, 900);
    vgl::Window window(w_res.x(), w_res.y(), "Hello");
    window.enable_gl();
    vgl::ui::Gui gui(window);
    glViewport(0, 0, w_res.x(), w_res.y());
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

    auto vertex_shader_source = glsp::preprocess_file(vgl::file::shaders_path / "minimal/texture.vert").contents;
    auto frag_shader_source = glsp::preprocess_file(vgl::file::shaders_path / "cv/filters/rot13.frag").contents;
    auto vertex_shader = vgl::gl::create_shader(GL_VERTEX_SHADER, vertex_shader_source);
    auto frag_shader = vgl::gl::create_shader(GL_FRAGMENT_SHADER, frag_shader_source);

    const auto program = vgl::gl::create_program({vertex_shader, frag_shader});

    vgl::gl::gltexture tex_id, temp_tex;
    vgl::Image_desc tex_info;

    vgl::gl::glframebuffer temp_fbo;

    auto load_texture = [&]() {
        auto path = vgl::file::open_file_dialog(vgl::file::resources_path / "images");
        if (path) {
            auto image = vgl::file::load_image(path.value(), 4, true);
            tex_info = vgl::img::get_image_desc(image);
            tex_id = vgl::gl::create_texture(GL_TEXTURE_2D);
            glTextureParameteri(tex_id, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
            glTextureParameteri(tex_id, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
            glTextureParameteri(tex_id, GL_TEXTURE_WRAP_S, GL_MIRRORED_REPEAT);
            glTextureParameteri(tex_id, GL_TEXTURE_WRAP_T, GL_MIRRORED_REPEAT);

            auto internal_format = vgl::gl::derive_internal_format(image);
            glTextureStorage2D(tex_id, 1, internal_format, tex_info.width, tex_info.height);
            vgl::gl::set_texture_data_2d(tex_id, image);
            temp_tex = vgl::gl::create_texture(GL_TEXTURE_2D);
            glTextureParameteri(temp_tex, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
            glTextureParameteri(temp_tex, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
            glTextureParameteri(temp_tex, GL_TEXTURE_WRAP_S, GL_MIRRORED_REPEAT);
            glTextureParameteri(temp_tex, GL_TEXTURE_WRAP_T, GL_MIRRORED_REPEAT);
            glTextureStorage2D(temp_tex, 1, GL_RGBA8, tex_info.width, tex_info.height);
            temp_fbo = vgl::gl::create_framebuffer();
            glNamedFramebufferTexture(temp_fbo, GL_COLOR_ATTACHMENT0, temp_tex, 0);
            vgl::gl::attach_draw_buffers(temp_fbo, { GL_COLOR_ATTACHMENT0 });
            vgl::gl::check_framebuffer(temp_fbo);
        }
    };

    auto screen_vao = vgl::gl::create_vertex_array();
    vgl::gl::glbuffer one_time_pad;
    int rot_value = 0;
    int type = 0;
    int max_value = 360;
    int xor_value = gen_random_int();
    Eigen::Matrix3f hill_mat(0.0f);
    hill_mat(0, 0) = gen_random_float();
    hill_mat(0, 1) = gen_random_float();
    hill_mat(0, 2) = gen_random_float();
    hill_mat(1, 0) = gen_random_float();
    hill_mat(1, 1) = gen_random_float();
    hill_mat(1, 2) = gen_random_float();
    hill_mat(2, 0) = gen_random_float();
    hill_mat(2, 1) = gen_random_float();
    hill_mat(2, 2) = gen_random_float();
    while (!window.should_close()) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        gui.start_frame();
        if (ImGui::Begin("Filter settings")) {
            if (ImGui::Button("Load image")) {
                load_texture();
            }
            ImGui::Combo("Rot type", &type, "hsv\0rgb\0hill\0xor\0bitreverse\0onetimepad\0reverse_onetimepad\0");
            if (type == 0) {
                max_value = 360;
            }
            else if (type == 1) {
                max_value = 256;
                rot_value = std::min(rot_value, max_value);
            }
            if (type <= 1) {
                ImGui::SliderInt("Rot x", &rot_value, 0, max_value);
            }
            else if (type == 2) {
                if (ImGui::Button("New random hill matrix")) {
                    hill_mat(0, 0) = gen_random_float();
                    hill_mat(0, 1) = gen_random_float();
                    hill_mat(0, 2) = gen_random_float();
                    hill_mat(1, 0) = gen_random_float();
                    hill_mat(1, 1) = gen_random_float();
                    hill_mat(1, 2) = gen_random_float();
                    hill_mat(2, 0) = gen_random_float();
                    hill_mat(2, 1) = gen_random_float();
                    hill_mat(2, 2) = gen_random_float();
                }
            }
            else if (type == 3) {
                if (ImGui::Button("New random xor value")) {
                    xor_value = gen_random_int();
                }
            }
            else if (type == 5) {
                if (ImGui::Button("New one time pad")) {
                    std::vector<float> one_time_pad_data(tex_info.width * tex_info.height * tex_info.channels);
                    std::random_device rd;
                    std::mt19937 eng(rd());
                    const std::uniform_real_distribution<float> uni(0.0f, 1.0f);
                    std::generate(one_time_pad_data.begin(), one_time_pad_data.end(), [&uni, &eng]() {
                        return uni(eng);
                        });
                    one_time_pad = vgl::gl::create_buffer(one_time_pad_data);
                    glBindBufferBase(GL_SHADER_STORAGE_BUFFER, 0, one_time_pad);
                }
            }
            vgl::gl::update_uniform(program, 0, rot_value);
            vgl::gl::update_uniform(program, 1, type);
            vgl::gl::update_uniform(program, 2, hill_mat);
            vgl::gl::update_uniform(program, 3, xor_value);
            if (tex_id != 0) {
                if (ImGui::Button("Save result")) {
                    auto file_path = vgl::file::save_file_dialog(vgl::file::resources_path / "images/", "png,jpg");
                    if (file_path) {
                        glBindFramebuffer(GL_FRAMEBUFFER, temp_fbo);
                        glViewport(0, 0, tex_info.width, tex_info.height);
                        glUseProgram(program);
                        glBindTextureUnit(0, tex_id);
                        glBindVertexArray(screen_vao);
                        glDrawArrays(GL_TRIANGLES, 0, 3);
                        vgl::Image_desc desc{ tex_info.width, tex_info.height, tex_info.channels };
                        vgl::Image_uc img(desc);

                        glGetTextureImage(temp_tex, 0, GL_RGBA, GL_UNSIGNED_BYTE, img.data.size() * sizeof(unsigned char), img.data.data());
                        if (vgl::file::save_image(img, file_path.value(), true)) {

                        }
                        glBindFramebuffer(GL_FRAMEBUFFER, 0);
                        auto win_size = window.size();
                        glViewport(0, 0, win_size.x(), win_size.y());
                    }
                }
            }
        }
        ImGui::End();
        glUseProgram(program);
        glBindTextureUnit(0, tex_id);
        glBindVertexArray(screen_vao);
        glDrawArrays(GL_TRIANGLES, 0, 3);
        gui.render();
        window.swap_buffers();
        window.poll_events();
    }
}


#include <ros/ros.h>
#include <can_msgs/Frame.h>
#include <diagnostic_updater/diagnostic_updater.h>

#include <hongfu_bms_msg/HongfuStatus.h>

#include <boost/format.hpp>

#define BMS_ID 0xB

enum RegisterIndex {
  kTemperature = 0x08,
  kVoltage = 0x09,
  kCurrent = 0x0A,
  kRSOC = 0x0D,
  kDesignCapacity = 0x18,
  kFullCapacity = 0x10,
  kResidualCapacity = 0x0F,
  kChargeCurrent = 0x14,
  kChargeCycle = 0x17,
  kProductionData = 0x1B,
  kVoltage7 = 0x39,
  kVoltage6 = 0x3A,
  kVoltage5 = 0x3B,
  kVoltage4 = 0x3C,
  kVoltage3 = 0x3D,
  kVoltage2 = 0x3E,
  kVoltage1 = 0x3F,
};

struct BMSStruct {
  float rsoc;
  float current;
  float temperature;
  float voltage;
  float cell_voltage[7];
  float full_capacity;
  float design_capacity;
  float residual_capacity;
  int32_t charge_cycle;
} bms_states;

void BMSDiagnostic(diagnostic_updater::DiagnosticStatusWrapper& status) {
  boost::format key_format;

  status.add("RSOC (%)", bms_states.rsoc);
  status.add("Current (A)", bms_states.current);
  status.add("Temperature %f% (℃)", bms_states.temperature);
  status.add("Voltage (V)", bms_states.voltage);
  status.add("ResidualCapacity (mAh)", bms_states.residual_capacity);
  status.add("FullCapacity (mAh)", bms_states.full_capacity);
  status.add("DesignCapacity (mAh)", bms_states.design_capacity);
  status.add("ChargeCycle", bms_states.charge_cycle);

  for(int i=0; i<7; i++) {
    key_format = boost::format("Cell %1% voltage (V)") % i;
    status.add(key_format.str(), bms_states.cell_voltage[i]);
  }

  status.summary(diagnostic_msgs::DiagnosticStatus::OK, "OK");
}

void CanReceiveCallback(const can_msgs::Frame::ConstPtr& msg) {
  uint16_t msg_id = (msg->id & 0xFFFF) >> 8;

  switch(msg_id) {
    case kTemperature: {
      bms_states.temperature = ((msg->data[1]<<8|msg->data[0])-2731)/10.0;
      // ROS_INFO("Temperature %f", bms_states.temperature);
      break;
    }
    case kVoltage: {
      bms_states.voltage = (msg->data[3]<<24|msg->data[2]<<16|msg->data[1]<<8|msg->data[0])/1000.0;
      // ROS_INFO("Voltage %f", bms_states.voltage);
      break;
    }
    case kCurrent: {
      bms_states.current = (msg->data[3]<<24|msg->data[2]<<16|msg->data[1]<<8|msg->data[0])/1000.0;
      // ROS_INFO("Current %f", bms_states.current);
      break;
    }
    case kRSOC: {
      bms_states.rsoc = (msg->data[1]<<8|msg->data[0]);
      // ROS_INFO("RSOC %f", bms_states.rsoc);
      break;
    }
    case kResidualCapacity: {
      bms_states.residual_capacity = (msg->data[3]<<24|msg->data[2]<<16|msg->data[1]<<8|msg->data[0]);
      // ROS_INFO("ResidualCapacity %f", bms_states.residual_capacity);
      break;
    }
    case kDesignCapacity: {
      bms_states.design_capacity = (msg->data[3]<<24|msg->data[2]<<16|msg->data[1]<<8|msg->data[0]);
      // ROS_INFO("DesignCapacity %f", bms_states.design_capacity);
      break;
    }
    case kFullCapacity: {
      bms_states.full_capacity = (msg->data[3]<<24|msg->data[2]<<16|msg->data[1]<<8|msg->data[0]);
      // ROS_INFO("FullCapacity %f", bms_states.full_capacity);
      break;
    }
    case kChargeCycle: {
      bms_states.charge_cycle = (msg->data[1]<<8|msg->data[0]);
      // ROS_INFO("ChargeCycle %d", bms_states.charge_cycle);
      break;
    }
    case kChargeCurrent: {
      break;
    }
    case kVoltage7: {
      bms_states.cell_voltage[6] = (msg->data[1]<<8|msg->data[0])/1000.0;
      // ROS_INFO("Voltage7 %f", bms_states.cell_voltage[6]);
      break;
    }
    case kVoltage6: {
      bms_states.cell_voltage[5] = (msg->data[1]<<8|msg->data[0])/1000.0;
      // ROS_INFO("Voltage6 %f", bms_states.cell_voltage[5]);
      break;
    }
    case kVoltage5: {
      bms_states.cell_voltage[4] = (msg->data[1]<<8|msg->data[0])/1000.0;
      // ROS_INFO("Voltage5 %f", bms_states.cell_voltage[4]);
      break;
    }
    case kVoltage4: {
      bms_states.cell_voltage[3] = (msg->data[1]<<8|msg->data[0])/1000.0;
      // ROS_INFO("Voltage4 %f", bms_states.cell_voltage[3]);
      break;
    }
    case kVoltage3: {
      bms_states.cell_voltage[2] = (msg->data[1]<<8|msg->data[0])/1000.0;
      // ROS_INFO("Voltage3 %f", bms_states.cell_voltage[2]);
      break;
    }
    case kVoltage2: {
      bms_states.cell_voltage[1] = (msg->data[1]<<8|msg->data[0])/1000.0;
      // ROS_INFO("Voltage2 %f", bms_states.cell_voltage[1]);
      break;
    }
    case kVoltage1: {
      bms_states.cell_voltage[0] = (msg->data[1]<<8|msg->data[0])/1000.0;
      // ROS_INFO("Voltage1 %f", bms_states.cell_voltage[0]);
      break;
    }
  }
  // ROS_INFO("Receive can msg, id 0x%02X, length %d, data 0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X, 0x%02X",
  //           msg_id, msg->dlc, msg->data[0], msg->data[1], msg->data[2], msg->data[3], msg->data[4], msg->data[5], msg->data[6], msg->data[7]);
}

int main(int argc, char *argv[]) {
  ros::init(argc, argv, "hongfu_bms_status_can_node");
  ros::AsyncSpinner spinner(3);
  spinner.start();

  int can_id;
  std::string send_topic, receive_topic;

  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");
  private_nh.param<int>("can_id", can_id, 0x09);
  private_nh.param<std::string>("can_send_topic", send_topic, "sent_messages");
  private_nh.param<std::string>("can_receive_topic", receive_topic, "received_messages");

  ros::Publisher can_pub = nh.advertise<can_msgs::Frame>(send_topic, 1000);
  ros::Subscriber can_sub = nh.subscribe<can_msgs::Frame>(receive_topic, 1000, CanReceiveCallback);

  diagnostic_updater::Updater diagnostic_updater;
  diagnostic_updater.setHardwareID("hongfu_bms");
  diagnostic_updater.add("BMS", BMSDiagnostic);

  can_msgs::Frame frame;
  frame.is_rtr = 0;
  frame.is_extended = 1;
  frame.is_error = 0;
  frame.dlc = 0;

  ROS_INFO("Start...");
  ros::Rate loop_rate(1);
  while(ros::ok()) {
    frame.header.stamp = ros::Time::now();
    frame.id = (BMS_ID<<17) | (0x01<<16) | (kTemperature<<8);
    can_pub.publish(frame);
    ros::WallDuration(0.1).sleep();

    frame.header.stamp = ros::Time::now();
    frame.id = (BMS_ID<<17) | (0x01<<16) | (kVoltage<<8);
    can_pub.publish(frame);
    ros::WallDuration(0.1).sleep();

    frame.header.stamp = ros::Time::now();
    frame.id = (BMS_ID<<17) | (0x01<<16) | (kCurrent<<8);
    can_pub.publish(frame);
    ros::WallDuration(0.1).sleep();

    frame.header.stamp = ros::Time::now();
    frame.id = (BMS_ID<<17) | (0x01<<16) | (kRSOC<<8);
    can_pub.publish(frame);
    ros::WallDuration(0.1).sleep();

    frame.header.stamp = ros::Time::now();
    frame.id = (BMS_ID<<17) | (0x01<<16) | (kFullCapacity<<8);
    can_pub.publish(frame);
    ros::WallDuration(0.1).sleep();

    frame.header.stamp = ros::Time::now();
    frame.id = (BMS_ID<<17) | (0x01<<16) | (kResidualCapacity<<8);
    can_pub.publish(frame);
    ros::WallDuration(0.1).sleep();

    frame.header.stamp = ros::Time::now();
    frame.id = (BMS_ID<<17) | (0x01<<16) | (kDesignCapacity<<8);
    can_pub.publish(frame);
    ros::WallDuration(0.1).sleep();

    frame.header.stamp = ros::Time::now();
    frame.id = (BMS_ID<<17) | (0x01<<16) | (kChargeCycle<<8);
    can_pub.publish(frame);
    ros::WallDuration(0.1).sleep();

    frame.header.stamp = ros::Time::now();
    frame.id = (BMS_ID<<17) | (0x01<<16) | (kVoltage7<<8);
    can_pub.publish(frame);
    ros::WallDuration(0.1).sleep();

    frame.header.stamp = ros::Time::now();
    frame.id = (BMS_ID<<17) | (0x01<<16) | (kVoltage6<<8);
    can_pub.publish(frame);
    ros::WallDuration(0.1).sleep();

    frame.header.stamp = ros::Time::now();
    frame.id = (BMS_ID<<17) | (0x01<<16) | (kVoltage5<<8);
    can_pub.publish(frame);
    ros::WallDuration(0.1).sleep();

    frame.header.stamp = ros::Time::now();
    frame.id = (BMS_ID<<17) | (0x01<<16) | (kVoltage4<<8);
    can_pub.publish(frame);
    ros::WallDuration(0.1).sleep();

    frame.header.stamp = ros::Time::now();
    frame.id = (BMS_ID<<17) | (0x01<<16) | (kVoltage3<<8);
    can_pub.publish(frame);
    ros::WallDuration(0.1).sleep();

    frame.header.stamp = ros::Time::now();
    frame.id = (BMS_ID<<17) | (0x01<<16) | (kVoltage2<<8);
    can_pub.publish(frame);
    ros::WallDuration(0.1).sleep();

    frame.header.stamp = ros::Time::now();
    frame.id = (BMS_ID<<17) | (0x01<<16) | (kVoltage1<<8);
    can_pub.publish(frame);
    ros::WallDuration(0.1).sleep();

    frame.header.stamp = ros::Time::now();
    frame.id = (BMS_ID<<17) | (0x01<<16) | (kChargeCurrent<<8);
    can_pub.publish(frame);
    ros::WallDuration(0.1).sleep();

    diagnostic_updater.update();

    ros::spinOnce();
    loop_rate.sleep();
  }

  return 0;
}


#include <boost/test/data/monomorphic/generators/random.hpp>


// Copyright (c) 2020 OUXT Polaris
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <color_names/color_names.hpp>
#include <memory>
#include <navi_sim/camera_sim_component.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <string>
#include <vector>

namespace navi_sim
{
CameraSimComponent::CameraSimComponent(const rclcpp::NodeOptions & options)
: Node("camera_sim", options), buffer_(get_clock()), listener_(buffer_)
{
  initialize();
}

CameraSimComponent::CameraSimComponent(std::string name, const rclcpp::NodeOptions & options)
: Node(name, options), buffer_(get_clock()), listener_(buffer_)
{
  initialize();
}

void CameraSimComponent::update()
{
  rclcpp::Time now = get_clock()->now();
  vision_msgs::msg::Detection2DArray detection_array;
  sensor_msgs::msg::CameraInfo camera_info;
  camera_info = camera_info_;
  camera_info.header.frame_id = camera_optical_frame_;
  camera_info.header.stamp = now;
  detection_array.header.stamp = now;
  detection_array.header.frame_id = camera_optical_frame_;
  geometry_msgs::msg::TransformStamped transform_stamped;
  try {
    transform_stamped = buffer_.lookupTransform(
      camera_optical_frame_, map_frame_, rclcpp::Time(0), tf2::durationFromSec(1.0));
    geometry_msgs::msg::Pose pose;
    pose.position.x = transform_stamped.transform.translation.x;
    pose.position.y = transform_stamped.transform.translation.y;
    pose.position.z = transform_stamped.transform.translation.z;
    pose.orientation = transform_stamped.transform.rotation;
  } catch (tf2::ExtrapolationException & ex) {
    RCLCPP_ERROR(get_logger(), ex.what());
    return;
  }
  typedef boost::geometry::model::d2::point_xy<double> point;
  typedef boost::geometry::model::polygon<point> polygon_type;
  typedef boost::geometry::model::box<point> box;
  box camera_bbox(point(0, 0), point(camera_info_.width, camera_info_.height));
  const auto names = raycaster_ptr_->getPrimitiveNames();
  for (const auto name : names) {
    const auto vertex = raycaster_ptr_->getVertex(name);
    polygon_type poly;
    typedef boost::geometry::ring_type<polygon_type>::type ring_type;
    ring_type & ring = boost::geometry::exterior_ring(poly);
    bool is_back = false;
    for (const auto & v : vertex) {
      geometry_msgs::msg::PointStamped p;
      p.point.x = v.x;
      p.point.y = v.y;
      p.point.z = v.z;
      tf2::doTransform(p, p, transform_stamped);
      if (p.point.z <= 0) {
        is_back = true;
      }
      cv::Point3d point_3d(p.point.x, p.point.y, p.point.z);
      cv::Point2d point_2d = cam_model_.project3dToPixel(point_3d);
      ring.push_back(point(point_2d.x, point_2d.y));
    }
    if (is_back) {
      continue;
    }
    const box bx = boost::geometry::return_envelope<box>(poly);
    box out;
    if (boost::geometry::intersection(camera_bbox, bx, out)) {
      vision_msgs::msg::Detection2D detection;
      detection.header.frame_id = camera_optical_frame_;
      detection.header.stamp = now;
      // detection.is_tracking = false;
      detection.bbox.center.x = (out.max_corner().x() + out.min_corner().x()) * 0.5;
      detection.bbox.center.y = (out.max_corner().y() + out.min_corner().y()) * 0.5;
      detection.bbox.size_x = out.max_corner().x() - out.min_corner().x();
      detection.bbox.size_y = out.max_corner().y() - out.min_corner().y();
      vision_msgs::msg::ObjectHypothesisWithPose result;
      // result.id = raycaster_ptr_->getObjectType(name);
      // result.score = 1.0;
      detection.results.emplace_back(result);
      detection_array.detections.emplace_back(detection);
    }
  }
  detection_pub_->publish(detection_array);
  camera_info_pub_->publish(camera_info);
  marker_pub_->publish(generateMarker(detection_array.detections));
}

void CameraSimComponent::initialize()
{
  declare_parameter<std::string>("embree_config", "");
  if (has_parameter("embree_config")) {
    std::string embree_config;
    get_parameter("embree_config", embree_config);
    raycaster_ptr_ = std::make_unique<Raycaster>(embree_config);
  } else {
    raycaster_ptr_ = std::make_unique<Raycaster>();
  }
  double vertical_fov;
  declare_parameter<double>("vertical_fov", 2.0978006228796881594);
  get_parameter("vertical_fov", vertical_fov);
  declare_parameter<int>("horizontal_pixels", 720);
  get_parameter("horizontal_pixels", horizontal_pixels_);
  declare_parameter<int>("vertical_pixels", 540);
  get_parameter("vertical_pixels", vertical_pixels_);
  declare_parameter<std::string>("frustum_color", "cyan");
  get_parameter("frustum_color", frustum_color_);
  declare_parameter<std::string>("detection_color", "limegreen");
  get_parameter("detection_color", detection_color_);
  declare_parameter<std::string>("camera_frame", "camera_link");
  get_parameter("camera_frame", camera_frame_);
  declare_parameter<std::string>("camera_optical_frame", "camera_optical_link");
  get_parameter("camera_optical_frame", camera_optical_frame_);
  declare_parameter<std::string>("map_frame", "map");
  get_parameter("map_frame", map_frame_);
  camera_info_ = sensor_msgs::msg::CameraInfo();
  camera_info_.height = vertical_pixels_;
  camera_info_.width = horizontal_pixels_;
  camera_info_.distortion_model = "plumb_bob";
  camera_info_.d = {0, 0, 0, 0, 0};
  double f = static_cast<double>(vertical_pixels_) * 0.5 / std::tan(vertical_fov * 0.5);
  camera_info_.k = {f, 0, static_cast<double>(horizontal_pixels_) * 0.5,
                    0, f, static_cast<double>(vertical_pixels_) * 0.5,
                    0, 0, 1};
  camera_info_.r = {1, 0, 0, 0, 1, 0, 0, 0, 1};
  camera_info_.p = {
    f,
    0,
    static_cast<double>(horizontal_pixels_) * 0.5,
    0,
    0,
    f,
    static_cast<double>(vertical_pixels_) * 0.5,
    0,
    0,
    0,
    1,
    0};
  cam_model_.fromCameraInfo(camera_info_);
  declare_parameter("objects_filename", "objects.json");
  std::string objects_filename;
  get_parameter("objects_filename", objects_filename);
  std::string objects_path =
    ament_index_cpp::get_package_share_directory("navi_sim") + "/config/" + objects_filename;
  namespace fs = boost::filesystem;
  const fs::path path(objects_path);
  boost::system::error_code error;
  const bool result = fs::exists(path, error);
  if (!result || error) {
    throw std::runtime_error(objects_path + " did not find");
  }
  std::ifstream fin;
  fin.open(objects_path, std::ios::in);
  std::string json_string = "";
  std::string line;
  while (std::getline(fin, line)) {
    json_string = json_string + line;
  }
  raycaster_ptr_->addPrimitives(nlohmann::json::parse(json_string));
  detection_pub_ = create_publisher<vision_msgs::msg::Detection2DArray>("detection", 1);
  camera_info_pub_ = create_publisher<sensor_msgs::msg::CameraInfo>("camera_info", 1);
  marker_pub_ = create_publisher<visualization_msgs::msg::MarkerArray>("marker", 1);
  using namespace std::chrono_literals;
  timer_ = create_wall_timer(100ms, std::bind(&CameraSimComponent::update, this));
}

const geometry_msgs::msg::Point CameraSimComponent::internallyDivide(
  const geometry_msgs::msg::Point & p0, const geometry_msgs::msg::Point & p1,
  double x_ratio_in_image, double y_ratio_in_image)
{
  geometry_msgs::msg::Point p;
  p.x = p0.x * (1 - x_ratio_in_image) + p1.x * x_ratio_in_image;
  p.y = p0.y * (1 - y_ratio_in_image) + p1.y * y_ratio_in_image;
  p.z = (p0.z + p1.z) * 0.5;
  return p;
}

const visualization_msgs::msg::MarkerArray CameraSimComponent::generateMarker(
  const std::vector<vision_msgs::msg::Detection2D> & detections)
{
  const auto now = get_clock()->now();
  visualization_msgs::msg::MarkerArray marker;
  visualization_msgs::msg::Marker frustum_marker;
  frustum_marker.header.stamp = now;
  frustum_marker.header.frame_id = camera_optical_frame_;
  frustum_marker.id = 0;
  frustum_marker.ns = "frustum";
  frustum_marker.action = visualization_msgs::msg::Marker::ADD;
  frustum_marker.type = visualization_msgs::msg::Marker::LINE_LIST;
  frustum_marker.scale.x = 0.01;
  frustum_marker.scale.y = 0.01;
  frustum_marker.scale.z = 0.01;
  frustum_marker.color = color_names::makeColorMsg(frustum_color_, 1.0);
  geometry_msgs::msg::Point point_origin;
  point_origin.x = 0;
  point_origin.y = 0;
  point_origin.z = 0;
  cv::Point3d point_lu = cam_model_.projectPixelTo3dRay(cv::Point2d(0, 0));
  geometry_msgs::msg::Point point_lu_msg;
  point_lu_msg.x = point_lu.x;
  point_lu_msg.y = point_lu.y;
  point_lu_msg.z = point_lu.z;
  frustum_marker.points.emplace_back(point_origin);
  frustum_marker.points.emplace_back(point_lu_msg);
  cv::Point3d point_ru = cam_model_.projectPixelTo3dRay(cv::Point2d(horizontal_pixels_, 0));
  geometry_msgs::msg::Point point_ru_msg;
  point_ru_msg.x = point_ru.x;
  point_ru_msg.y = point_ru.y;
  point_ru_msg.z = point_ru.z;
  frustum_marker.points.emplace_back(point_origin);
  frustum_marker.points.emplace_back(point_ru_msg);
  cv::Point3d point_lb = cam_model_.projectPixelTo3dRay(cv::Point2d(0, vertical_pixels_));
  geometry_msgs::msg::Point point_lb_msg;
  point_lb_msg.x = point_lb.x;
  point_lb_msg.y = point_lb.y;
  point_lb_msg.z = point_lb.z;
  frustum_marker.points.emplace_back(point_origin);
  frustum_marker.points.emplace_back(point_lb_msg);
  cv::Point3d point_rb =
    cam_model_.projectPixelTo3dRay(cv::Point2d(horizontal_pixels_, vertical_pixels_));
  geometry_msgs::msg::Point point_rb_msg;
  point_rb_msg.x = point_rb.x;
  point_rb_msg.y = point_rb.y;
  point_rb_msg.z = point_rb.z;
  frustum_marker.points.emplace_back(point_origin);
  frustum_marker.points.emplace_back(point_rb_msg);
  // markers for edge
  frustum_marker.points.emplace_back(point_lu_msg);
  frustum_marker.points.emplace_back(point_ru_msg);
  frustum_marker.points.emplace_back(point_ru_msg);
  frustum_marker.points.emplace_back(point_rb_msg);
  frustum_marker.points.emplace_back(point_rb_msg);
  frustum_marker.points.emplace_back(point_lb_msg);
  frustum_marker.points.emplace_back(point_lb_msg);
  frustum_marker.points.emplace_back(point_lu_msg);
  frustum_marker.frame_locked = true;
  marker.markers.emplace_back(frustum_marker);
  visualization_msgs::msg::Marker detection_marker;
  detection_marker.header.stamp = now;
  detection_marker.header.frame_id = camera_optical_frame_;
  detection_marker.id = 0;
  detection_marker.ns = "detection";
  detection_marker.action = visualization_msgs::msg::Marker::ADD;
  detection_marker.type = visualization_msgs::msg::Marker::LINE_LIST;
  detection_marker.scale.x = 0.01;
  detection_marker.scale.y = 0.01;
  detection_marker.scale.z = 0.01;
  detection_marker.color = color_names::makeColorMsg(detection_color_, 1.0);
  detection_marker.frame_locked = true;
  for (const auto & detection : detections) {
    cv::Point2d point_lu_obj(
      detection.bbox.center.x - detection.bbox.size_x * 0.5,
      detection.bbox.center.y - detection.bbox.size_y * 0.5);
    cv::Point3d lu_ray = cam_model_.projectPixelTo3dRay(point_lu_obj);
    geometry_msgs::msg::Point point_lu_obj_msg;
    point_lu_obj_msg.x = lu_ray.x;
    point_lu_obj_msg.y = lu_ray.y;
    point_lu_obj_msg.z = lu_ray.z;
    cv::Point2d point_ru_point(
      detection.bbox.center.x + detection.bbox.size_x * 0.5,
      detection.bbox.center.y - detection.bbox.size_y * 0.5);
    cv::Point3d ru_ray = cam_model_.projectPixelTo3dRay(point_ru_point);
    geometry_msgs::msg::Point point_ru_obj_msg;
    point_ru_obj_msg.x = ru_ray.x;
    point_ru_obj_msg.y = ru_ray.y;
    point_ru_obj_msg.z = ru_ray.z;
    cv::Point2d point_rb_point(
      detection.bbox.center.x + detection.bbox.size_x * 0.5,
      detection.bbox.center.y + detection.bbox.size_y * 0.5);
    cv::Point3d rb_ray = cam_model_.projectPixelTo3dRay(point_rb_point);
    geometry_msgs::msg::Point point_rb_obj_msg;
    point_rb_obj_msg.x = rb_ray.x;
    point_rb_obj_msg.y = rb_ray.y;
    point_rb_obj_msg.z = rb_ray.z;
    cv::Point2d point_lb_point(
      detection.bbox.center.x - detection.bbox.size_x * 0.5,
      detection.bbox.center.y + detection.bbox.size_y * 0.5);
    cv::Point3d lb_ray = cam_model_.projectPixelTo3dRay(point_lb_point);
    geometry_msgs::msg::Point point_lb_obj_msg;
    point_lb_obj_msg.x = lb_ray.x;
    point_lb_obj_msg.y = lb_ray.y;
    point_lb_obj_msg.z = lb_ray.z;
    detection_marker.points.emplace_back(point_lu_obj_msg);
    detection_marker.points.emplace_back(point_ru_obj_msg);
    detection_marker.points.emplace_back(point_ru_obj_msg);
    detection_marker.points.emplace_back(point_rb_obj_msg);
    detection_marker.points.emplace_back(point_rb_obj_msg);
    detection_marker.points.emplace_back(point_lb_obj_msg);
    detection_marker.points.emplace_back(point_lb_obj_msg);
    detection_marker.points.emplace_back(point_lu_obj_msg);
  }
  marker.markers.emplace_back(detection_marker);
  return marker;
}
}  // namespace navi_sim

RCLCPP_COMPONENTS_REGISTER_NODE(navi_sim::CameraSimComponent)


//
//  LBFGSTimeStepper.hpp
//  DOT
//
//  Created by Minchen Li on 12/14/18.
//

#ifndef LBFGSTimeStepper_hpp
#define LBFGSTimeStepper_hpp

#include "Optimizer.hpp"
#include "METIS.hpp"

#include <Eigen/SparseCholesky>
#include <Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h>

#include <deque>

namespace DOT {

    enum D0Type {
        D0T_PD,
        D0T_RH,
        D0T_H,
        D0T_HI,
        D0T_JH
    };

    template<int dim>
    class LBFGSTimeStepper : public Optimizer<dim>
    {
        typedef Optimizer<dim> Base;

    protected: // data
        D0Type m_D0Type;
        int historySize;
        std::deque<Eigen::VectorXd> dx, dg; // s and t
        std::deque<double> dgTdx; // tTs
        LinSysSolver<Eigen::VectorXi, Eigen::VectorXd> *D0LinSysSolver;
        std::vector<Eigen::Matrix<double, dim, dim + 1>> D_array;
        Eigen::SparseMatrix<double> W, D;

        std::vector<Eigen::VectorXi> nodeLists;
        std::vector<Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>>*> solver; //TODO: use LinSysSolver
        std::vector<idx_t> nodePart;

        Eigen::SparseMatrix<double> sysMtrForIC;
        Eigen::IncompleteCholesky<double> ICSolver;

    public: // constructor and destructor
        LBFGSTimeStepper(const Mesh<dim>& p_data0,
                         const std::vector<Energy<dim>*>& p_energyTerms,
                         const std::vector<double>& p_energyParams,
                         D0Type p_D0Type,
                         bool p_mute = false,
                         const Config& animConfig = Config());
        ~LBFGSTimeStepper(void);

    public: // API
        virtual void precompute(void);
        virtual void updatePrecondMtrAndFactorize(void);

        virtual void getFaceFieldForVis(Eigen::VectorXd& field);

    protected: // overwritten helper
        virtual bool fullyImplicit(void);
        virtual bool solve_oneStep(void);

    protected: // own helper
    };

}

#endif /* LBFGSTimeStepper_hpp */


#include <iostream>
#include <thread>
#include <chrono>
#include <fstream>
#include <vector>
#include <map>
#include <thread>
#include <chrono>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/io/io.h>
#include <pcl/io/vtk_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/surface/vtk_smoothing/vtk_utils.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/progressive_morphological_filter.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/surface/marching_cubes_rbf.h>
#include <pcl/surface/marching_cubes_hoppe.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/surface/poisson.h>
#include <pcl/surface/concave_hull.h>
#include <Eigen/Geometry>

#include "utils.h"
#include "MapManager.h"

// #include <pcl/filters/statistical_outlier_removal.h>
// #include <pcl/filters/radius_outlier_removal.h>
// #include <pcl/filters/extract_indices.h>

typedef pcl::PointXYZRGB PointType;
using namespace std;
using namespace pcl;
using namespace pcl::visualization;
using namespace lass;

void test_load_extrinsics(int argc, char** argv) {
    /*
        argv[1] -- info file
        argv[2] -- list file
        argv[3] -- name of dir
     */
    const string info_fn = argv[1];
    const string list_fn = argv[2];
    const string dir     = argv[3];

    Eigen::Matrix4f left, rear, right;
    Eigen::Matrix3f intrinsics;
    vector<Eigen::Matrix4f> es;
    vector<string>  image_fns;
    vector<int>     camera_types;
    PointCloud<PointXYZRGB>::Ptr pcd(new PointCloud<PointXYZRGB>);

    intrinsics << 400, 0, 500.10765, 0, 400, 511.461426, 0, 0, 1;

    load_info_file(info_fn.c_str(), es);
    load_list_file(list_fn.c_str(), es.size(), image_fns, camera_types);

    LOG(INFO) << "number of cameras: " << es.size() << endl;

    auto mm = make_unique<MapManager>(dir);
    mm->dye_through_semantics();
    mm->filter_outliers();
    mm->update_view();

    Eigen::Vector3f o, d, u;

    pcd->points.resize(es.size() * 5);
    for (int idx = 0; idx < es.size(); idx++) {
        auto &e = es[idx];
        Eigen::Vector3f c_nvm = e.block(0, 3, 3, 1), t_e;
        Eigen::Matrix3f R_nvm = e.block(0, 0, 3, 3), R_e;

        c_nvm(1) *= -1;
        c_nvm(2) *= -1;
        R_nvm(0, 1) *= -1;
        R_nvm(0, 2) *= -1;
        R_nvm(1, 0) *= -1;
        R_nvm(2, 0) *= -1;

        c_nvm = -R_nvm.transpose() * c_nvm;
        d = R_nvm.transpose() * Eigen::Vector3f::UnitZ();

        auto &pt = pcd->points[idx * 5];
        pt.r = pt.g = pt.b = 200;
        pt.x = c_nvm(0);
        pt.y = c_nvm(1);
        pt.z = c_nvm(2);
        for (int s_idx = 1; s_idx < 5; s_idx++) {
            auto &pt = pcd->points[idx * 5 + s_idx];
            pt.r = pt.g = pt.b = 0;
            switch (camera_types[idx]) {
            case 0:
                pt.r = 200;
                break;
            case 1:
                pt.g = 200;
                break;
            case 2:
                pt.b = 200;
                break;
            default:
                break;
            }
            pt.x = c_nvm(0) + d(0) * s_idx * 0.2;
            pt.y = c_nvm(1) + d(1) * s_idx * 0.2;
            pt.z = c_nvm(2) + d(2) * s_idx * 0.2;
        }
    }
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pcd);
    mm->m_viewer->addPointCloud(pcd, rgb, "camera poses");
    mm->m_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "camera poses");
    mm->show_point_cloud();
    // int idx = 0;
    // for (auto &e: es) { // e = [R|c]
    //     Eigen::Vector3f c_nvm = e.block(0, 3, 3, 1), t_e;
    //     Eigen::Matrix3f R_nvm = e.block(0, 0, 3, 3), R_e;
    //     cout << image_fns[idx] << endl;

    //     c_nvm(1) *= -1; c_nvm(2) *= -1;

    //     R_nvm(0, 1) *= -1; R_nvm(0, 2) *= -1;
    //     R_nvm(1, 0) *= -1; R_nvm(2, 0) *= -1;

    //     o = - R_nvm.transpose() * c_nvm;      //< camera to world
    //     e.block(0, 0, 3, 3) = R_nvm.transpose().eval();
    //     e.block(0, 3, 3, 1) = o;

    //     mm->m_viewer->setCameraParameters(intrinsics, e);
    //     // pm->m_viewer->saveScreenshot(image_fns[idx]);
    //     mm->show_point_cloud();
    //     // std::this_thread::sleep_for(std::chrono::microseconds(1000000));

    //     idx++;
    // }
    // mm->show_point_cloud();
}

int main(int argc, char** argv) {
    test_load_extrinsics(argc, argv);
}