#include "RecoLocalTracker/SiStripRecHitConverter/interface/StripCPE.h"
#include "Geometry/CommonTopologies/interface/StripTopology.h"
#include "Geometry/CommonTopologies/interface/TkRadialStripTopology.h"
#include "Geometry/TrackerGeometryBuilder/interface/StripGeomDetType.h"
#include <algorithm>
#include <cmath>
#include <cassert>

StripCPE::StripCPE(edm::ParameterSet& conf,
                   const MagneticField& mag,
                   const TrackerGeometry& geom,
                   const SiStripLorentzAngle& LorentzAngle,
                   const SiStripBackPlaneCorrection& BackPlaneCorrection,
                   const SiStripConfObject& confObj,
                   const SiStripLatency& latency)
    : peakMode_(latency.singleReadOutMode() == 1),
      geom_(geom),
      magfield_(mag),
      LorentzAngleMap_(LorentzAngle),
      BackPlaneCorrectionMap_(BackPlaneCorrection) {
  typedef std::map<std::string, SiStripModuleGeometry> map_t;
  map_t modules;
  modules["IB1"] = SiStripModuleGeometry::IB1;
  modules["IB2"] = SiStripModuleGeometry::IB2;
  modules["OB1"] = SiStripModuleGeometry::OB1;
  modules["OB2"] = SiStripModuleGeometry::OB2;
  modules["W1A"] = SiStripModuleGeometry::W1A;
  modules["W2A"] = SiStripModuleGeometry::W2A;
  modules["W3A"] = SiStripModuleGeometry::W3A;
  modules["W1B"] = SiStripModuleGeometry::W1B;
  modules["W2B"] = SiStripModuleGeometry::W2B;
  modules["W3B"] = SiStripModuleGeometry::W3B;
  modules["W4"] = SiStripModuleGeometry::W4;
  modules["W5"] = SiStripModuleGeometry::W5;
  modules["W6"] = SiStripModuleGeometry::W6;
  modules["W7"] = SiStripModuleGeometry::W7;

  const unsigned size =
      static_cast<unsigned int>(
          max_element(modules.begin(), modules.end(), [&](auto& arg1, auto& arg2) { return arg1.second < arg2.second; })
              ->second) +
      1;
  xtalk1.resize(size);
  xtalk2.resize(size);

  for (map_t::const_iterator it = modules.begin(); it != modules.end(); it++) {
    const std::string modeS(peakMode_ ? "Peak" : "Deco"), xtalk1S("xtalk1_" + it->first + modeS),
        xtalk2S("xtalk2_" + it->first + modeS);

    if (!confObj.isParameter(xtalk1S))
      throw cms::Exception("SiStripConfObject does not contain: ") << xtalk1S;
    if (!confObj.isParameter(xtalk2S))
      throw cms::Exception("SiStripConfObject does not contain: ") << xtalk2S;

    xtalk1[static_cast<int>(it->second)] = confObj.get<double>(xtalk1S);
    xtalk2[static_cast<int>(it->second)] = confObj.get<double>(xtalk2S);
  }

  fillParams();
}

StripClusterParameterEstimator::LocalValues StripCPE::localParameters(const SiStripCluster& cluster,
                                                                      const GeomDetUnit& det) const {
  StripCPE::Param const& p = param(det);
  const float barycenter = cluster.barycenter();
  const float fullProjection =
      p.coveredStrips(p.drift + LocalVector(0, 0, -p.thickness), p.topology->localPosition(barycenter));
  const float strip = barycenter - 0.5f * (1.f - p.backplanecorrection) * fullProjection;

  return std::make_pair(p.topology->localPosition(strip), p.topology->localError(strip, 1.f / 12.f));
}

float StripCPE::Param::coveredStrips(const LocalVector& lvec, const LocalPoint& lpos) const {
  return topology->coveredStrips(lpos + 0.5f * lvec, lpos - 0.5f * lvec);
}

LocalVector StripCPE::driftDirection(const StripGeomDetUnit* det) const {
  LocalVector lbfield = (det->surface()).toLocal(magfield_.inTesla(det->surface().position()));

  float tanLorentzAnglePerTesla = LorentzAngleMap_.getLorentzAngle(det->geographicalId().rawId());

  float dir_x = -tanLorentzAnglePerTesla * lbfield.y();
  float dir_y = tanLorentzAnglePerTesla * lbfield.x();
  float dir_z = 1.f;  // E field always in z direction

  return LocalVector(dir_x, dir_y, dir_z);
}

void StripCPE::fillParams() {
  auto const& dus = geom_.detUnits();
  m_off = dus.size();
  for (unsigned int i = 1; i < 7; ++i) {
    LogDebug("LookingForFirstStrip")
        << "Subdetector " << i << " GeomDetEnumerator " << GeomDetEnumerators::tkDetEnum[i] << " offset "
        << geom_.offsetDU(GeomDetEnumerators::tkDetEnum[i]) << " is it strip? "
        << (geom_.offsetDU(GeomDetEnumerators::tkDetEnum[i]) != dus.size()
                ? dus[geom_.offsetDU(GeomDetEnumerators::tkDetEnum[i])]->type().isTrackerStrip()
                : false);
    if (geom_.offsetDU(GeomDetEnumerators::tkDetEnum[i]) != dus.size() &&
        dus[geom_.offsetDU(GeomDetEnumerators::tkDetEnum[i])]->type().isTrackerStrip()) {
      if (geom_.offsetDU(GeomDetEnumerators::tkDetEnum[i]) < m_off)
        m_off = geom_.offsetDU(GeomDetEnumerators::tkDetEnum[i]);
    }
  }
  LogDebug("LookingForFirstStrip") << " Chosen offset: " << m_off;
  m_Params.resize(dus.size() - m_off);
  for (auto i = m_off; i != dus.size(); ++i) {
    auto& p = m_Params[i - m_off];
    const StripGeomDetUnit* stripdet = (const StripGeomDetUnit*)(dus[i]);
    assert(stripdet->index() == int(i));
    //    assert(stripdet->geographicalId().subdetId()>1); // not pixel..
    assert(stripdet->type().isTrackerStrip());  // not pixel

    const Bounds& bounds = stripdet->specificSurface().bounds();
    p.maxLength = std::sqrt(std::pow(bounds.length(), 2.f) + std::pow(bounds.width(), 2.f));
    p.thickness = bounds.thickness();
    p.invThickness = 1.f / p.thickness;
    p.drift = driftDirection(stripdet) * p.thickness;
    p.topology = (StripTopology*)(&stripdet->topology());
    p.nstrips = p.topology->nstrips();
    p.moduleGeom = SiStripDetId(stripdet->geographicalId()).moduleGeometry();
    p.backplanecorrection = BackPlaneCorrectionMap_.getBackPlaneCorrection(stripdet->geographicalId().rawId());

    const TkRadialStripTopology* rtop =
        dynamic_cast<const TkRadialStripTopology*>(&stripdet->specificType().specificTopology());
    p.pitch_rel_err2 =
        (rtop)
            ? pow(0.5f * rtop->angularWidth() * rtop->stripLength() / rtop->localPitch(LocalPoint(0, 0, 0)), 2.f) / 12.f
            : 0.f;
  }
}
