#include <maps/wikimap/mapspro/services/autocart/libs/detection/include/utils.h>

namespace maps {
namespace wiki {
namespace autocart {

Orientation orientation(const cv::Point &a,
                        const cv::Point &b,
                        const cv::Point &c)
{
    const int val = (b.y - a.y) * (c.x - b.x) - (b.x - a.x) * (c.y - b.y);
    if (val == 0)
        return Orientation::Colinear;
    return (val > 0) ? Orientation::Clockwise : Orientation::CounterClockwise;
}
bool isSegmentIntersect(const cv::Point &segm1s,
                        const cv::Point &segm1e,
                        const cv::Point &segm2s,
                        const cv::Point &segm2e)
{
    Orientation o1 = orientation(segm1s, segm1e, segm2s);
    Orientation o2 = orientation(segm1s, segm1e, segm2e);
    Orientation o3 = orientation(segm2s, segm2e, segm1s);
    Orientation o4 = orientation(segm2s, segm2e, segm1e);

    return (o1 != o2 && o3 != o4);
}
double calcAngle(const cv::Point &v1,
                 const cv::Point &c,
                 const cv::Point &v2)
{
    const cv::Point v1c = v1 - c;
    const cv::Point v2c = v2 - c;
    return acos(v1c.ddot(v2c) / cv::norm(v1c) / cv::norm(v2c));
}

bool projectPointOnSegment(const cv::Point &pt,
                           const cv::Point &segms,
                           const cv::Point &segme,
                           cv::Point &result)
{
    const cv::Point v1 = pt - segms;
    const cv::Point v2 = segme - segms;
    const double t = v1.ddot(v2) / v2.ddot(v2);
    if (t < 0. || t > 1.0)
        return false;
    result = (cv::Point)((cv::Point2d)segms + t * (cv::Point2d)v2);
    return true;
}
bool projectPointOnSegment(const cv::Point2d &pt,
                           const cv::Point2d &segms,
                           const cv::Point2d &segme,
                           cv::Point2d &result)
{
    const cv::Point2d v1 = pt - segms;
    const cv::Point2d v2 = segme - segms;
    const double t = v1.ddot(v2) / v2.ddot(v2);
    if (t < 0. || t > 1.0)
        return false;
    result = (cv::Point2d)segms + t * (cv::Point2d)v2;
    return true;
}
double segmentsDistance(const cv::Point2d &segm1s,
                        const cv::Point2d &segm1e,
                        const cv::Point2d &segm2s,
                        const cv::Point2d &segm2e)
{
    if (isSegmentIntersect(segm1s, segm1e, segm2s, segm2e))
        return 0.;
    double distMin = DBL_MAX;
    {
        double dist = cv::norm(segm1s - segm2s);
        distMin = MIN(dist, distMin);
        dist = cv::norm(segm1s - segm2e);
        distMin = MIN(dist, distMin);
        dist = cv::norm(segm1e - segm2s);
        distMin = MIN(dist, distMin);
        dist = cv::norm(segm1e - segm2e);
        distMin = MIN(dist, distMin);
    }
    cv::Point2d temp;
    if (projectPointOnSegment(segm1s, segm2s, segm2e, temp))
    {
        const double dist = cv::norm(segm1s - temp);
        distMin = MIN(dist, distMin);
    }
    if (projectPointOnSegment(segm1e, segm2s, segm2e, temp))
    {
        const double dist = cv::norm(segm1e - temp);
        distMin = MIN(dist, distMin);
    }
    if (projectPointOnSegment(segm2s, segm1s, segm1e, temp))
    {
        const double dist = cv::norm(segm2s - temp);
        distMin = MIN(dist, distMin);
    }
    if (projectPointOnSegment(segm2e, segm1s, segm1e, temp))
    {
        const double dist = cv::norm(segm2e - temp);
        distMin = MIN(dist, distMin);
    }
    return distMin;
}
bool isClockwise(const std::vector<cv::Point> &polygon)
{
    const size_t pgnVertsCnt = polygon.size();
    int sum = 0;
    for (size_t i = 0; i < pgnVertsCnt - 1; i++)
    {
        sum += (polygon[i + 1].x - polygon[i].x) * (polygon[i + 1].y + polygon[i].y);
    }
    if (polygon.front() != polygon.back())
        sum += (polygon.front().x - polygon.back().x) * (polygon.front().y + polygon.back().y);

    return (0 > sum);
}
void makeRectangle(const cv::Point &base,
                   double angle,
                   double edgeLen0,
                   double edgeLen1,
                   int orient,
                   std::vector<cv::Point> &polygon)
{
    polygon.resize(4);

    const cv::Point2d shift0(          edgeLen0 * cos(angle),          edgeLen0 * sin(angle));
    const cv::Point2d shift1(-orient * edgeLen1 * sin(angle), orient * edgeLen1 * cos(angle));

    polygon[0] = base;
    polygon[1] = (cv::Point2d)base + shift0;
    polygon[2] = (cv::Point2d)base + shift0 + shift1;
    polygon[3] = (cv::Point2d)base + shift1;
}

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