#include <maps/wikimap/mapspro/services/autocart/libs/post_processing/include/post_processing.h>
#include <maps/wikimap/mapspro/services/autocart/libs/post_processing/include/polygon_regularization.h>

#include <maps/libs/geolib/include/line.h>
#include <maps/libs/geolib/include/polygon.h>
#include <maps/libs/geolib/include/distance.h>
#include <maps/libs/geolib/include/spatial_relation.h>

#include <maps/libs/log8/include/log8.h>

#include <opencv2/opencv.hpp>

#include <vector>
#include <algorithm>
#include <cmath>
#include <optional>

namespace maps {
namespace wiki {
namespace autocart {

namespace {

// Solution of grid regularization method
struct Solution {
    Solution(const GridPoint& initPoint,
             size_t pointsNumber,
             double deviation,
             Direction direction)
        : points(1, initPoint),
          pointsNumber(pointsNumber),
          deviation(deviation),
          direction(direction) {}
    Solution(const std::vector<GridPoint>& points,
             size_t pointsNumber,
             double deviation,
             Direction direction)
        : points(points),
          pointsNumber(pointsNumber),
          deviation(deviation),
          direction(direction) {}
    // Points in rectangular grid, which form polygon boundary
    // More than two points can lie on one edge
    std::vector<GridPoint> points;
    // Corner points number
    size_t pointsNumber;
    // Deviation of solution from original polygon
    double deviation;
    // Next point search direction
    Direction direction;
};

/**
 * @brief Compares two solutions in grid regularization method.
 *     One solution is better than another if it contains fewer points.
 *     If both solutions contain the same number of points, then their
 *     deviations from original polygon are compared.
 * @param lhs - first solution
 * @param rhs - second solution
 * @return true if lhs better than rhs, otherwise false
 */
bool operator<(const Solution& lhs, const Solution& rhs) {
    if (lhs.pointsNumber == rhs.pointsNumber) {
        return lhs.deviation < rhs.deviation;
    } else {
        return lhs.pointsNumber < rhs.pointsNumber;
    }
}

/**
 * @brief Converts point in rectangular grid to point in space.
 * @param gridPoint - point in rectangular grid
 * @param gridSide  - size of rectangular grid
 * @return point in space
 */
geolib3::Point2 getPoint(const GridPoint& gridPoint, double gridSide) {
    return geolib3::Point2(gridPoint.col * gridSide, gridPoint.row * gridSide);
}

/**
 * @brief Calculates distance from point in space to point in rectangular grid.
 * @param spacePoint - point in space
 * @param gridPoint  - point in rectangular grid
 * @param gridSide   - size of rectangular grid
 * @return distance between two points
 */
double distance(const geolib3::Point2& spacePoint,
                const GridPoint& gridPoint,
                double gridSide) {
    geolib3::Point2 point(gridPoint.col * gridSide, gridPoint.row * gridSide);
    return geolib3::distance(spacePoint, point);
}

/**
 * @brief Finds best solution from set of solutions. Best solution must have
 *     minimal points number and deviation. Polygon corresponding to best
 *     solution must not have self-intersection.
 * @param solutions - set of solutions
 * @return best solution, which specifies polygon without self-intersections.
 *     Return std::nullopt, if there is no such solution.
 */
std::optional<Solution>
findBestSolution(const std::vector<Solution>& solutions) {
    std::optional<Solution> bestSolution;
    for (const auto& solution : solutions) {
        if (!bestSolution.has_value() || solution < *bestSolution) {
            if (!isCorrectDirection(solution.points.back(),
                                    solution.points.front(),
                                    solution.direction)) {
                continue;
            }
            if (isSimple(solution.points)) {
                bestSolution = solution;
            }
        }
    }
    return bestSolution;
}

/**
 * @brief Converts OpenCV points to maps::geolib3 points
 * @param points - OpenCV points
 * @return maps::geolib3 points
 */
std::vector<geolib3::Point2>
convertToGeoPoints(const std::vector<cv::Point2f>& points) {
    std::vector<geolib3::Point2> geoPoints;
    geoPoints.reserve(points.size());
    for (const auto& point : points) {
        geoPoints.emplace_back(point.x, point.y);
    }
    return geoPoints;
}

/**
 * @brief Converts maps::geolib points to OpenCV points
 * @param points - maps::geolib points
 * @return OpenCV points
 */
std::vector<cv::Point2f>
convertToCVPoints(const std::vector<geolib3::Point2>& points) {
    std::vector<cv::Point2f> cvPoints;
    cvPoints.reserve(points.size());
    for (const auto& point : points) {
        cvPoints.emplace_back(point.x(), point.y());
    }
    return cvPoints;
}

/**
 * @brief Finds rectangle with minimum area that contains points
 * @param points - set of points
 * @return rotated bounding box
 */
std::vector<cv::Point2f>
getBoundingRectangle(const std::vector<cv::Point2f>& points) {
    geolib3::PointsVector geoPoints = convertToGeoPoints(points);
    geolib3::Polygon2 rect = autocart::getBoundingRectangle(geoPoints);

    std::vector<cv::Point2f> result;
    result.reserve(rect.pointsNumber());
    for (size_t i = 0; i < rect.pointsNumber(); i++) {
        result.emplace_back(rect.pointAt(i).x(), rect.pointAt(i).y());
    }
    return result;
}

/**
 * @brief Sets initial solution for grid regularization method.
 *     Set of initial solutions contains solutions starting at all grid points
 *     around starting point and with all available directions.
 * @param point     - first point of polygon
 * @param tolerance - maximum distance to grid point
 * @param gridSide  - size of rectangular grid
 * @return set of initial solutions
 */
std::vector<Solution>
initSolutions(const geolib3::Point2& point,
              double tolerance,
              double gridSide) {
    GridPointsSet initialSet(point, tolerance, gridSide);

    std::vector<Solution> solutions;
    solutions.reserve(initialSet.pointsNumber() * AVAILABLE_DIRECTIONS.size());
    for (size_t i = 0; i < initialSet.pointsNumber(); i++) {
        const GridPoint& initPoint = initialSet.pointAt(i);
        double deviation = distance(point, initPoint, gridSide);
        for (const auto& direction : AVAILABLE_DIRECTIONS) {
            solutions.emplace_back(initPoint, 1, deviation, direction);
        }
    }
    return solutions;
}

/**
 * @brief Finds next solution for given grid point and direction.
 *     Finds available solution with minimum points number and deviation.
 * @param prevSolutions  - set of all previous solutions
 * @param nextPoint      - next point in rectangular grid
 * @param nextDirection  - next point search direction
 * @param pointDeviation - deviation next grid points from polygon
 * @return next solution if it exists, otherwise std::nullopt
 */
std::optional<Solution>
findNextSolution(const std::vector<Solution>& prevSolutions,
                 const GridPoint& nextPoint,
                 const Direction& nextDirection,
                 double pointDeviation) {
    std::optional<Solution> nextSolution;
    for (const auto& solution : prevSolutions) {
        if (isOppositeDirections(solution.direction, nextDirection)) {
            continue;
        }
        if (!isCorrectDirection(solution.points.back(),
                                nextPoint,
                                solution.direction)) {
            continue;
        }
        double deviation = solution.deviation + pointDeviation;
        size_t pointsNumber = solution.pointsNumber;
        if (solution.direction != nextDirection) {
            pointsNumber++;
        }

        if (nextSolution.has_value()) {
            if (nextSolution->pointsNumber < pointsNumber) {
                continue;
            } else if (nextSolution->pointsNumber == pointsNumber &&
                       nextSolution->deviation < deviation) {
                continue;
            }
        }
        std::vector<GridPoint> nextPoints(solution.points);
        nextPoints.push_back(nextPoint);
        nextSolution = Solution(nextPoints, pointsNumber, deviation, nextDirection);
    }
    return nextSolution;
}

/**
 * @brief Builds orthogonal polygon around original polygon with given tolerance.
 * @param points    - points of boundary polygon
 * @param tolerance - maximum distance to grid point
 * @param gridSide  - size of rectangular grid
 * @return best solution if it exists, otherwise std::nullopt
 */
std::optional<Solution>
buildOrthogonalPolygon(const std::vector<geolib3::Point2>& points,
                       double tolerance,
                       double gridSide) {
    std::vector<Solution> solutions = initSolutions(points.front(),
                                                    tolerance, gridSide);

    std::vector<Solution> nextSolutions;
    for (size_t k = 1; k < points.size(); k++) {
        GridPointsSet ptsSet(points[k], tolerance, gridSide);
        nextSolutions.reserve(ptsSet.pointsNumber() * AVAILABLE_DIRECTIONS.size());
        for (size_t ptsIndx = 0; ptsIndx < ptsSet.pointsNumber(); ptsIndx++) {
            const GridPoint& gridPoint = ptsSet.pointAt(ptsIndx);
            double pointDeviation = distance(points[k], gridPoint, gridSide);
            for (const auto& direction : AVAILABLE_DIRECTIONS) {
                auto nextSolution = findNextSolution(solutions,
                                                     gridPoint,
                                                     direction,
                                                     pointDeviation);
                if (nextSolution.has_value()) {
                    nextSolutions.push_back(*nextSolution);
                }
            }
        }
        solutions = nextSolutions;
        nextSolutions.clear();
    }
    return findBestSolution(solutions);
}

/**
 * @brief Find the longest edge in polygon boundary.
 * @param points - points of polygon boundary
 * @return index of the longest edge in polygon boundary
 */
size_t findLongestEdge(const std::vector<geolib3::Point2>& points) {
    double maxLength = 0.;
    size_t longestSideIndex = 0;
    for (size_t i = 0; i < points.size(); i++) {
        geolib3::Point2 start = points[i];
        geolib3::Point2 end = points[(i + 1) % points.size()];
        double length = geolib3::distance(start, end);
        if (length > maxLength) {
            longestSideIndex = i;
            maxLength = length;
        }
    }
    return longestSideIndex;
}

} // anonymous namespace

GridPointsSet::GridPointsSet(const geolib3::Point2& srcPoint,
                             double tolerance, double gridSide) {
    const double EPS = 1e-3;
    minCol_ = std::ceil((srcPoint.x() - tolerance) / gridSide - EPS);
    maxCol_ = std::floor((srcPoint.x() + tolerance) / gridSide + EPS);
    minRow_ = std::ceil((srcPoint.y() - tolerance) / gridSide - EPS);
    maxRow_ = std::floor((srcPoint.y() + tolerance) / gridSide + EPS);
}

size_t GridPointsSet::pointsNumber() {
    return (maxCol_ - minCol_  + 1) * (maxRow_ - minRow_ + 1);
}

GridPoint GridPointsSet::pointAt(const size_t& index) {
    return GridPoint(minCol_ + index % (maxCol_ - minCol_ + 1),
                     minRow_ + index / (maxCol_ - minCol_ + 1));
}

bool isOppositeDirections(const Direction& direction1,
                          const Direction& direction2) {
    if (direction1 == Direction::RIGHT && direction2 == Direction::LEFT ||
        direction1 == Direction::LEFT && direction2 == Direction::RIGHT ||
        direction1 == Direction::UP && direction2 == Direction::DOWN    ||
        direction1 == Direction::DOWN && direction2 == Direction::UP) {
        return true;
    } else {
        return false;
    }
}

bool isCorrectDirection(const GridPoint& pt1, const GridPoint& pt2,
                        const Direction& direction) {
    if (pt1.col == pt2.col) {
        if (pt1.row <= pt2.row && direction == UP) {
            return true;
        } else if (pt1.row >= pt2.row && direction == DOWN) {
            return true;
        }
    }

    if (pt1.row == pt2.row) {
        if (pt1.col <= pt2.col && direction == RIGHT) {
            return true;
        } else if (pt1.col >= pt2.col && direction == LEFT) {
            return true;
        }
    }
    return false;
}

bool isSimple(const std::vector<GridPoint>& gridPoints) {
    geolib3::PointsVector points;
    for (const auto gridPoint : gridPoints) {
        points.emplace_back(gridPoint.col, gridPoint.row);
    }
    if (points.front() != points.back()) {
        points.push_back(points.front());
    }
    geolib3::Polyline2 boundary(points);
    return geolib3::isSimple(boundary);
}

bool areOnSameEdge(const GridPoint& point1,
                   const GridPoint& point2,
                   const GridPoint& point3) {
    if (point1.col == point2.col && point2.col == point3.col) {
        return true;
    } else if (point1.row == point2.row && point2.row == point3.row) {
        return true;
    } else {
        return false;
    }
}

std::vector<GridPoint>
removeExtraPolygonPoints(const std::vector<GridPoint>& points) {
    std::vector<GridPoint> result = points;
    size_t i = 0;
    while (i < result.size()) {
        const GridPoint& prevPt = (i == 0 ? result.back() : result[i - 1]);
        const GridPoint& curPt = result[i];
        const GridPoint& nextPt = result[(i + 1) % result.size()];
        if (areOnSameEdge(prevPt, curPt, nextPt)) {
            result.erase(result.begin() + i);
        } else {
            i++;
        }
    }
    return result;
}

geolib3::PointsVector
convertToPolygon(const std::vector<GridPoint>& gridPoints, double gridSide) {
    std::vector<GridPoint> clearPoints = removeExtraPolygonPoints(gridPoints);
    geolib3::PointsVector points;
    for (const auto& gridPoint : clearPoints) {
        points.push_back(getPoint(gridPoint, gridSide));
    }
    return points;
}

geolib3::PointsVector
simplifyBoundary(const geolib3::PointsVector& points, double epsilon) {
    geolib3::Polyline2 boundary(points);
    // Douglas–Peucker algorithm does not change position of endpoints
    if (boundary.points().front() != boundary.points().back()) {
        boundary.add(boundary.points().front());
    }
    geolib3::Polyline2 simplifiedBoundary = geolib3::simplify(boundary, epsilon);
    // Delete duplicate points
    geolib3::PointsVector simplifiedPoints = simplifiedBoundary.points();
    simplifiedPoints.pop_back();
    return simplifiedPoints;
}

geolib3::PointsVector
rotatePoints(const geolib3::PointsVector& points, double angleDegree) {
    const double cosa = cos(angleDegree * M_PI / 180.);
    const double sina = sin(angleDegree * M_PI / 180.);

    geolib3::PointsVector rotatedPoints;
    for (const auto& point : points) {
        // multiplication with rotation matrix
        // https://en.wikipedia.org/wiki/Rotation_matrix
        rotatedPoints.emplace_back(point.x() * cosa - point.y() * sina,
                                   point.x() * sina + point.y() * cosa);
    }
    return rotatedPoints;
}

std::vector<cv::Point2f>
gridRegularizePolygon(const std::vector<cv::Point2f>& points,
                      double tolerance, double gridSide) {
    const double DP_EPSILON = 5.;
    const double ANGLE_STEP = 3.;
    const double AREA_THRESHOLD = 30 * 30;
    const double AREA_RATIO_THRESHOLD = 0.85;

    double area = cv::contourArea(points);
    std::vector<cv::Point2f> rectangle = getBoundingRectangle(points);
    double rectArea = cv::contourArea(rectangle);
    if (area < AREA_THRESHOLD || area / rectArea > AREA_RATIO_THRESHOLD) {
        return rectangle;
    }

    geolib3::PointsVector geoPoints = convertToGeoPoints(points);
    geolib3::PointsVector simplifiedPoints = simplifyBoundary(geoPoints, DP_EPSILON);
    std::optional<Solution> bestSolution;
    double bestAngle;
    for (double angle = 0; angle < 90; angle += ANGLE_STEP) {
        geolib3::PointsVector rotPoints = rotatePoints(simplifiedPoints, angle);
        auto solution = buildOrthogonalPolygon(rotPoints, tolerance, gridSide);
        if (solution.has_value()) {
            if (!bestSolution.has_value() || *solution < *bestSolution) {
                bestSolution = solution;
                bestAngle = angle;
            }
        }
    }

    if (bestSolution.has_value()) {
        geolib3::PointsVector bestPoints = convertToPolygon(bestSolution->points, gridSide);
        geolib3::PointsVector rotPoints = rotatePoints(bestPoints, -bestAngle);
        return convertToCVPoints(rotPoints);
    } else {
        return rectangle;
    }
}

geolib3::Point2 project(const geolib3::Segment2& segment,
                        const geolib3::Point2& point,
                        double* alpha) {
    if (segment.start() == segment.end()) {
        *alpha = 0.;
        return segment.start();
    }
    geolib3::Vector2 v1(point.x() - segment.start().x(),
                        point.y() - segment.start().y());
    geolib3::Vector2 v2(segment.end().x() - segment.start().x(),
                        segment.end().y() - segment.start().y());
    *alpha = geolib3::innerProduct(v1, v2) / geolib3::innerProduct(v2, v2);
    geolib3::Point2 projection(segment.start().x() + *alpha * v2.x(),
                               segment.start().y() + *alpha * v2.y());
    return projection;
}

std::vector<geolib3::Point2>
closeBoundary(const std::vector<geolib3::Point2>& points) {
    const double LINE_DIST = 1e-2;
    std::vector<geolib3::Point2> result = points;
    geolib3::Point2 startPoint = result.front();
    geolib3::Segment2 startEdge(result[0], result[1]);
    geolib3::Segment2 endEdge(result[result.size() - 2],
                              result.back());
    double alpha;
    geolib3::Point2 projection = project(endEdge, startPoint, &alpha);
    if (geolib3::distance(geolib3::Line2(startEdge), projection) < LINE_DIST) {
        result.pop_back();
        result.push_back(projection);
        result.erase(result.begin());
    } else {
        result.pop_back();
        result.push_back(projection);
    }
    return result;
}

std::vector<cv::Point2f>
projectionRegularizePolygon(const std::vector<cv::Point2f> &points,
                            double wallLength) {
    const double DP_EPSILON = 5.;
    const double AREA_THRESHOLD = 30 * 30;
    const double AREA_RATIO_THRESHOLD = 0.85;

    double area = cv::contourArea(points);
    std::vector<cv::Point2f> rectangle = getBoundingRectangle(points);
    double rectArea = cv::contourArea(rectangle);
    if (area < AREA_THRESHOLD || area / rectArea > AREA_RATIO_THRESHOLD) {
        return rectangle;
    }

    geolib3::PointsVector geoPoints = convertToGeoPoints(points);
    geolib3::PointsVector simplifiedPoints = simplifyBoundary(geoPoints, DP_EPSILON);
    size_t longestEdgeIndex = findLongestEdge(simplifiedPoints);
    std::rotate(simplifiedPoints.begin(),
                simplifiedPoints.begin() + longestEdgeIndex,
                simplifiedPoints.end());

    std::vector<geolib3::Point2> resultPoints;
    resultPoints.push_back(simplifiedPoints[0]);
    resultPoints.push_back(simplifiedPoints[1]);
    bool hasExtraPoints = true;
    for (size_t i = 2; i < simplifiedPoints.size(); i++) {
        geolib3::Point2 start = resultPoints[resultPoints.size() - 2];
        geolib3::Point2 end = resultPoints.back();
        geolib3::Point2 point = simplifiedPoints[i % simplifiedPoints.size()];
        geolib3::Segment2 segment(start, end);
        double alpha;
        geolib3::Point2 projection = project(segment, point, &alpha);
        if (alpha < 0. && alpha == 1.) {
            continue;
        } else if (0. < alpha && alpha < 1.) {
            // projection lies inside segment
            resultPoints.pop_back();
            hasExtraPoints = false;
            geolib3::Vector2 shift((projection.x() - end.x()) / 2.,
                                  (projection.y() - end.y()) / 2.);
            projection -= shift;
            point -= shift;
        }
        if (hasExtraPoints) {
            resultPoints.pop_back();
        }
        resultPoints.push_back(projection);
        if (geolib3::distance(point, projection) > wallLength) {
            resultPoints.push_back(point);
        }
        hasExtraPoints = true;
    }
    if (resultPoints.size() < 4) {
        return rectangle;
    }
    resultPoints = closeBoundary(resultPoints);

    // check self-intersections
    geolib3::Polyline2 closedBoundary(resultPoints);
    if (resultPoints.front() != resultPoints.back()) {
        closedBoundary.add(resultPoints.front());
    }
    if (geolib3::isSimple(closedBoundary)) {
        return convertToCVPoints(resultPoints);
    } else {
        return rectangle;
    }
}

} //namespace autocart
} //namespace wiki
} //namespace maps

