#include "CaloDetIdAssociator.h"
#include "DataFormats/GeometrySurface/interface/Plane.h"
bool CaloDetIdAssociator::crossedElement(const GlobalPoint& point1,
                                         const GlobalPoint& point2,
                                         const DetId& id,
                                         const double toleranceInSigmas,
                                         const SteppingHelixStateInfo* initialState) const {
  std::vector<GlobalPoint> pointBuffer;
  const std::pair<const_iterator, const_iterator>& points = getDetIdPoints(id, pointBuffer);
  // fast check
  bool xLess(false), xIn(false), xMore(false);
  bool yLess(false), yIn(false), yMore(false);
  bool zLess(false), zIn(false), zMore(false);
  double xMin(point1.x()), xMax(point2.x());
  double yMin(point1.y()), yMax(point2.y());
  double zMin(point1.z()), zMax(point2.z());
  if (xMin > xMax)
    std::swap(xMin, xMax);
  if (yMin > yMax)
    std::swap(yMin, yMax);
  if (zMin > zMax)
    std::swap(zMin, zMax);
  for (std::vector<GlobalPoint>::const_iterator it = points.first; it != points.second; ++it) {
    if (it->x() < xMin) {
      xLess = true;
    } else {
      if (it->x() > xMax)
        xMore = true;
      else
        xIn = true;
    }
    if (it->y() < yMin) {
      yLess = true;
    } else {
      if (it->y() > yMax)
        yMore = true;
      else
        yIn = true;
    }
    if (it->z() < zMin) {
      zLess = true;
    } else {
      if (it->z() > zMax)
        zMore = true;
      else
        zIn = true;
    }
  }
  if (((xLess && !xIn && !xMore) || (!xLess && !xIn && xMore)) ||
      ((yLess && !yIn && !yMore) || (!yLess && !yIn && yMore)) ||
      ((zLess && !zIn && !zMore) || (!zLess && !zIn && zMore)))
    return false;

  // Define plane normal to the trajectory direction at the first point
  GlobalVector vector = (point2 - point1).unit();
  float r21 = 0;
  float r22 = vector.z() / sqrt(1 - pow(vector.x(), 2));
  float r23 = -vector.y() / sqrt(1 - pow(vector.x(), 2));
  float r31 = vector.x();
  float r32 = vector.y();
  float r33 = vector.z();
  float r11 = r22 * r33 - r23 * r32;
  float r12 = r23 * r31;
  float r13 = -r22 * r31;

  Surface::RotationType rotation(r11, r12, r13, r21, r22, r23, r31, r32, r33);
  Plane::PlanePointer plane = Plane::build(point1, rotation);
  double absoluteTolerance = -1;
  if (toleranceInSigmas > 0 && initialState) {
    TrajectoryStateOnSurface tsos = initialState->getStateOnSurface(*plane);
    if (tsos.isValid() and tsos.hasError()) {
      LocalError localErr = tsos.localError().positionError();
      localErr.scale(toleranceInSigmas);
      float xx = localErr.xx();
      float xy = localErr.xy();
      float yy = localErr.yy();

      float denom = yy - xx;
      float phi = 0.;
      if (xy == 0 && denom == 0)
        phi = M_PI_4;
      else
        phi = 0.5 * atan2(2. * xy, denom);  // angle of MAJOR axis
      // Unrotate the error ellipse to get the semimajor and minor axes. Then place points on
      // the endpoints of semiminor an seminajor axes on original(rotated) error ellipse.
      LocalError rotErr = localErr.rotate(-phi);  // xy covariance of rotErr should be zero
      float semi1 = sqrt(rotErr.xx());
      float semi2 = sqrt(rotErr.yy());
      absoluteTolerance = std::max(semi1, semi2);
    }
  }

  // distance between the points.
  double trajectorySegmentLength = (point2 - point1).mag();

  // we need to find the angle that covers all points.
  // if it's bigger than 180 degree, we are inside
  // otherwise we are outside, i.e. the volume is not crossed
  bool allBehind = true;
  bool allTooFar = true;
  std::vector<GlobalPoint>::const_iterator p = points.first;
  if (p == points.second) {
    edm::LogWarning("TrackAssociator") << "calo geometry for element " << id.rawId() << "is empty. Ignored";
    return false;
  }
  LocalPoint localPoint = plane->toLocal(*p);
  double minPhi = localPoint.phi();
  double maxPhi = localPoint.phi();
  if (localPoint.z() < 0)
    allTooFar = false;
  else {
    allBehind = false;
    if (localPoint.z() < trajectorySegmentLength)
      allTooFar = false;
  }
  ++p;
  for (; p != points.second; ++p) {
    localPoint = plane->toLocal(*p);
    double localPhi = localPoint.phi();
    if (localPoint.z() < 0)
      allTooFar = false;
    else {
      allBehind = false;
      if (localPoint.z() < trajectorySegmentLength)
        allTooFar = false;
    }
    if (localPhi >= minPhi && localPhi <= maxPhi)
      continue;
    if (localPhi + 2 * M_PI >= minPhi && localPhi + 2 * M_PI <= maxPhi)
      continue;
    if (localPhi - 2 * M_PI >= minPhi && localPhi - 2 * M_PI <= maxPhi)
      continue;
    // find the closest limit
    if (localPhi > maxPhi) {
      double delta1 = fabs(localPhi - maxPhi);
      double delta2 = fabs(localPhi - 2 * M_PI - minPhi);
      if (delta1 < delta2)
        maxPhi = localPhi;
      else
        minPhi = localPhi - 2 * M_PI;
      continue;
    }
    if (localPhi < minPhi) {
      double delta1 = fabs(localPhi - minPhi);
      double delta2 = fabs(localPhi + 2 * M_PI - maxPhi);
      if (delta1 < delta2)
        minPhi = localPhi;
      else
        maxPhi = localPhi + 2 * M_PI;
      continue;
    }
    cms::Exception("FatalError")
        << "Algorithm logic error - this should never happen. Problems with trajectory-volume matching.";
  }
  if (allBehind)
    return false;
  if (allTooFar)
    return false;
  if (fabs(maxPhi - minPhi) > M_PI)
    return true;

  // now if the tolerance is positive, check how far we are
  // from the closest line segment
  if (absoluteTolerance < 0)
    return false;
  double distanceToClosestLineSegment = 1e9;
  std::vector<GlobalPoint>::const_iterator i, j;
  for (i = points.first; i != points.second; ++i)
    for (j = i + 1; j != points.second; ++j) {
      LocalPoint p1(plane->toLocal(*i));
      LocalPoint p2(plane->toLocal(*j));
      // now we deal with high school level math to get
      // the triangle paramaters
      double side1squared = p1.perp2();
      double side2squared = p2.perp2();
      double side3squared = (p2.x() - p1.x()) * (p2.x() - p1.x()) + (p2.y() - p1.y()) * (p2.y() - p1.y());
      double area = fabs(p1.x() * p2.y() - p2.x() * p1.y()) / 2;
      // all triangle angles must be smaller than 90 degree
      // otherwise the projection is out of the line segment
      if (side1squared + side2squared > side3squared && side2squared + side3squared > side1squared &&
          side1squared + side3squared > side1squared) {
        double h(2 * area / sqrt(side3squared));
        if (h < distanceToClosestLineSegment)
          distanceToClosestLineSegment = h;
      } else {
        if (sqrt(side1squared) < distanceToClosestLineSegment)
          distanceToClosestLineSegment = sqrt(side1squared);
        if (sqrt(side2squared) < distanceToClosestLineSegment)
          distanceToClosestLineSegment = sqrt(side2squared);
      }
    }
  if (distanceToClosestLineSegment < absoluteTolerance)
    return true;
  return false;
}

void CaloDetIdAssociator::check_setup() const {
  DetIdAssociator::check_setup();
  if (geometry_ == nullptr)
    throw cms::Exception("CaloGeometry is not set");
}

GlobalPoint CaloDetIdAssociator::getPosition(const DetId& id) const {
  return geometry_->getSubdetectorGeometry(id)->getGeometry(id)->getPosition();
}

void CaloDetIdAssociator::getValidDetIds(unsigned int subDectorIndex, std::vector<DetId>& detIds) const {
  if (subDectorIndex != 0)
    cms::Exception("FatalError")
        << "Calo sub-dectors are all handle as one sub-system, but subDetectorIndex is not zero.\n";
  detIds = geometry_->getValidDetIds(DetId::Calo, 1);
}

std::pair<DetIdAssociator::const_iterator, DetIdAssociator::const_iterator> CaloDetIdAssociator::getDetIdPoints(
    const DetId& id, std::vector<GlobalPoint>& points) const {
  const CaloSubdetectorGeometry* subDetGeom = geometry_->getSubdetectorGeometry(id);
  if (!subDetGeom) {
    LogDebug("TrackAssociator") << "Cannot find sub-detector geometry for " << id.rawId() << "\n";
    return std::pair<const_iterator, const_iterator>(dummy_.end(), dummy_.end());
  }
  auto cellGeom = subDetGeom->getGeometry(id);
  if (!cellGeom) {
    LogDebug("TrackAssociator") << "Cannot find CaloCell geometry for " << id.rawId() << "\n";
    return std::pair<const_iterator, const_iterator>(dummy_.end(), dummy_.end());
  }
  const CaloCellGeometry::CornersVec& cor(cellGeom->getCorners());
  return std::pair<const_iterator, const_iterator>(cor.begin(), cor.end());
}
