#pragma once

#ifndef RASTER_EDGE_EVALUATOR_HPP

#define RASTER_EDGE_EVALUATOR_HPP

#include "raster_edge_evaluator.h"

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

//    Raster Edge Evaluator implementation

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

template <typename ranit=""></typename>

RasterEdgeEvaluator<ranit>::RasterEdgeEvaluator(const iterator_type &begin,</ranit>

                                                const iterator_type &end,

                                                double tolerance,

                                                double maxLength)

    : tcg::polyline_ops::StandardDeviationEvaluator<ranit>(begin, end)</ranit>

    , m_tolerance(tolerance)

    , m_maxLength(maxLength) {}

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

template <typename ranit=""></typename>

typename RasterEdgeEvaluator<ranit>::penalty_type</ranit>

RasterEdgeEvaluator<ranit>::penalty(const iterator_type &a,</ranit>

                                    const iterator_type &b) {

  return tcg::point_ops::norm(*b - *a) *

         tcg::polyline_ops::StandardDeviationEvaluator<ranit>::penalty(a, b);</ranit>

}

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

template <typename ranit=""></typename>

typename RasterEdgeEvaluator<ranit>::iterator_type</ranit>

RasterEdgeEvaluator<ranit>::furthestFrom(const iterator_type &start) {</ranit>

  // Build the furthest possible forward difference for every vertex between

  // begin and end.

  point_type displace, oldDisplace;

  point_type leftConstraint, rightConstraint;

  point_type newLeftConstraint, newRightConstraint;

  point_type leftDirConstr, rightDirConstr, dir, oldDir;

  iterator_type it = start, jt;

  const double sqMaxLength = sq(m_maxLength);

  // Initialize search

  leftConstraint = rightConstraint = point_type();

  leftDirConstr = rightDirConstr = point_type();

  oldDir = oldDisplace = point_type();

  if (it != this->m_begin) --it;  // Chop left

  jt = it;

  for (++jt; jt != this->m_end; ++jt) {

    // Retrieve displacement from *it

    displace = point_type(jt->x - it->x, jt->y - it->y);

    dir = point_type(displace.x - oldDisplace.x, displace.y - oldDisplace.y);

    // Max length

    if (oldDir.x != 0 || oldDir.y != 0) {

      if (sq(displace.x) + sq(displace.y) > sqMaxLength) break;

    } else

      leftDirConstr = rightDirConstr = dir;

    // Test displacement against the oldDisplacement. If it's reversing the

    // direction, make it invalid.

    if (cross(oldDir, dir) > 0) leftDirConstr = dir;

    if (cross(oldDir, dir) < 0) rightDirConstr = dir;

    // Test constraints

    /*if(cross(rightDirConstr, leftDirConstr) <= 0 &&

leftDirConstr * rightDirConstr < 0)

break;*/

    if (cross(rightDirConstr, leftDirConstr) < 0) break;

    if (cross(displace, leftConstraint) < 0) break;

    if (cross(displace, rightConstraint) > 0) break;

    if (std::max({displace.x, -displace.x, displace.y, -displace.y}) >

        m_tolerance) {

      // Update m_tolerance constraints

      newLeftConstraint.x =

          displace.x + (displace.y < 0 || (displace.y == 0 && displace.x < 0)

                            ? m_tolerance

                            : -m_tolerance);

      newLeftConstraint.y =

          displace.y + (displace.x > 0 || (displace.x == 0 && displace.y < 0)

                            ? m_tolerance

                            : -m_tolerance);

      if (cross(newLeftConstraint, leftConstraint) >= 0)

        leftConstraint = newLeftConstraint;

      newRightConstraint.x =

          displace.x + (displace.y > 0 || (displace.y == 0 && displace.x < 0)

                            ? m_tolerance

                            : -m_tolerance);

      newRightConstraint.y =

          displace.y + (displace.x < 0 || (displace.x == 0 && displace.y < 0)

                            ? m_tolerance

                            : -m_tolerance);

      if (cross(newRightConstraint, rightConstraint) <= 0)

        rightConstraint = newRightConstraint;

    }

    oldDisplace = displace;

    oldDir      = dir;

  }

  if (jt != this->m_end) --jt;  // Chop Right

  return start +

         std::max(

             (int)std::min(jt - start - 1, this->m_end - this->m_begin - 2), 1);

}

#endif  // RASTER_EDGE_EVALUATOR_HPP