//
// Created by YY on 2019/4/30.
//

#include "../defs.h"
#include "Geometry.h"
#include <stdbool.h>
#include <stdint.h>
#include <cmath>
#include <jni.h>
#include <malloc.h>
#include <initializer_list>
#include <cctype>
#include <iosfwd>
#include <iomanip>
#include <sstream>

#include "../jni_log.h"

using namespace std;

const double EPSILON = 1e-6;
const double EPSILON2 = 1e-3;

inline bool isEqual(double a, double b)
{
    return (fabs(a - b) <= EPSILON);
}

inline bool isEqual2(double a, double b)
{
    return (fabs(a - b) <= EPSILON2);
}

inline double toRadians(double degree)
{
    return (degree * M_PI) / 180.0;
}

inline double toDegree(double radians)
{
    return (radians * 180.0) / M_PI;
}

double round(double number, unsigned int bits) {
    stringstream ss;
    ss << setiosflags(ios::fixed) << setprecision(bits) << number;
    ss >> number;
    return number;
}

void MakeLine(Line *line, const PointF *p1, const PointF *p2)
{
    line->X1 = p1->X;
    line->Y1 = p1->Y;
    line->X2 = p2->X;
    line->Y2 = p2->Y;
}

void MakePolygon(Polygon *polygon, std::initializer_list<PointF> point_set)
{
    int n = 0;

    polygon->point = (PointF *)malloc(point_set.size() * sizeof(PointF));

    for (auto ptr : point_set) {
        polygon->point[n++] = ptr;
    }
    polygon->num = n;
}

void MakeHidePoint(PointF *point, const PointF *bp, const Line *bl)
{
    point->X = (bl->X1 + bl->X2) - bp->X;
    point->Y = (bl->Y1 + bl->Y2) - bp->Y;
}

void CleanPolygon(Polygon *polygon)
{
    if (polygon->point != NULL) {
        free(polygon->point);
        polygon->point = NULL;
    }
    polygon->num = 0;
}

Relation IntersectionOf(const Polygon *polygon1, const Polygon *polygon2)
{
    bool inside = false, outside = false, tangent = false;

    if (polygon1->num == 0 || polygon2->num == 0) {
        return GM_None;
    }

    for (int idx = 0; idx < polygon1->num; ++idx) {
        Relation relation = IntersectionOf(polygon1->point[idx], polygon2);

        if (relation == GM_Containment) {
            inside = true;
        } else if (relation == GM_None) {
            outside = true;
        } else {
            tangent = true;
        }

        if (inside && outside) {
            break;
        }
    }

    if (inside && outside) {
        return GM_Intersection;
    } else if (tangent) {
        return GM_Tangent;
    } else if (inside) {
        return GM_Containment;
    }
    return GM_None;
}

Relation IntersectionOf(Line line, const Polygon *polygon)
{
    if (polygon->num == 0) {
        return GM_None;
    }

    if (polygon->num == 1) {
        return IntersectionOf(polygon->point[0], line);
    }

    bool tangent = false;
    for (int index = 0; index < polygon->num; index++) {
        int index2 = (index + 1) % polygon->num;
        Line line2;

        line2.X1 = polygon->point[index].X;
        line2.Y1 = polygon->point[index].Y;
        line2.X2 = polygon->point[index2].X;
        line2.Y2 = polygon->point[index2].Y;

//        LOGD("line1(%d %d - %d %d) line2(%d %d - %d %d)", line.X1, line.Y1, line.X2, line.Y2,
//             line2.X1, line2.Y1, line2.X2, line2.Y2);

        Relation relation = IntersectionOf(line, line2);

//        LOGD("relation = %d", relation);

        if (relation == GM_Intersection) {
            return relation;
        }
        if (relation == GM_Tangent) {
            tangent = true;
        }
    }

    PointF point2;

    point2.X = line.X1;
    point2.Y = line.Y1;

    return tangent ? GM_Tangent : IntersectionOf(point2, polygon);
}

Relation IntersectionOf(PointF point, const Polygon *polygon)
{
    switch (polygon->num)
    {
        case 0:
            return GM_None;
        case 1:
            if (isEqual(polygon->point[0].X, point.X) &&
                isEqual(polygon->point[0].Y, point.Y)) {
                return GM_Tangent;
            }
            else {
                return GM_None;
            }
        case 2: {
            Line line2;

            line2.X1 = polygon->point[0].X;
            line2.Y1 = polygon->point[0].Y;
            line2.X2 = polygon->point[1].X;
            line2.Y2 = polygon->point[1].Y;
            return IntersectionOf(point, line2);
        }
        default:
            break;
    }

    int counter = 0;
    int i;
    PointF p1;
    int n = polygon->num;

    p1.X = polygon->point[0].X;
    p1.Y = polygon->point[0].Y;

    if (isEqual(point.X, p1.X) && isEqual(point.Y, p1.Y)) {
        return GM_Tangent;
    }

    for (i = 1; i <= n; i++) {
        PointF p2;

        p2.X = polygon->point[i % n].X;
        p2.Y = polygon->point[i % n].Y;

        if (isEqual(point.X, p2.X) && isEqual(point.Y, p2.Y)) {
            return GM_Tangent;
        }

        if (point.Y > fmin(p1.Y, p2.Y)) {
            if (point.Y <= fmax(p1.Y, p2.Y)) {
                if (point.X <= fmax(p1.X, p2.X)) {
                    if (p1.Y != p2.Y) {
                        double xinters = (point.Y - p1.Y) * (p2.X - p1.X) / (p2.Y - p1.Y) + p1.X;

                        if (isEqual(p1.X, p2.X) || point.X <= xinters)
                            counter++;
                    }
                }
            }
        }
        p1 = p2;
    }

    return (counter % 2 == 1) ? GM_Containment : GM_None;
}

Relation IntersectionOf(PointF point, Line line)
{
    double bottomY = fmin(line.Y1, line.Y2);
    double topY = fmax(line.Y1, line.Y2);
    bool heightIsRight = point.Y >= bottomY &&
                         point.Y <= topY;
    //Vertical line, slope is divideByZero error!
    if (isEqual(line.X1, line.X2)) {
        if (isEqual(point.X, line.X1) && heightIsRight) {
            return GM_Tangent;
        } else {
            return GM_None;
        }
    }

    double slope = (line.X2 - line.X1) / (line.Y2 - line.Y1);
    bool onLine = isEqual(line.Y1 - point.Y, slope * (line.X1 - point.X));

    if (onLine && heightIsRight) {
        return GM_Tangent;
    } else {
        return GM_None;
    }
}

Relation IntersectionOf(Line line1, Line line2)
{
    //  Fail if either line segment is zero-length.
    if ((isEqual(line1.X1, line1.X2) && isEqual(line1.Y1, line1.Y2)) || (isEqual(line2.X1, line2.X2) && isEqual(line2.Y1, line2.Y2)))
        return GM_None;

    if ((isEqual(line1.X1, line2.X1) && isEqual(line1.Y1, line2.Y1)) || (isEqual(line1.X2, line2.X1) && isEqual(line1.Y2, line2.Y1)))
        return GM_Intersection;

    if ((isEqual(line1.X1, line2.X2) && isEqual(line1.Y1, line2.Y2)) || (isEqual(line1.X2, line2.X2) && isEqual(line1.Y2, line2.Y2)))
        return GM_Intersection;

    //  (1) Translate the system so that point A is on the origin.
    line1.X2 -= line1.X1; line1.Y2 -= line1.Y1;
    line2.X1 -= line1.X1; line2.Y1 -= line1.Y1;
    line2.X2 -= line1.X1; line2.Y2 -= line1.Y1;

    //  Discover the length of segment A-B.
    double distAB = sqrt(line1.X2 * line1.X2 + line1.Y2 * line1.Y2);

    //  (2) Rotate the system so that point B is on the positive X axis.
    double theCos = line1.X2 / distAB;
    double theSin = line1.Y2 / distAB;
    double newX = line2.X1 * theCos + line2.Y1 * theSin;

    line2.Y1 = line2.Y1 * theCos - line2.X1 * theSin;
    line2.X1 = newX;
    newX = line2.X2 * theCos + line2.Y2 * theSin;
    line2.Y2 = line2.Y2 * theCos - line2.X2 * theSin;
    line2.X2 = newX;

    //  Fail if segment C-D doesn't cross line A-B.
    if ((line2.Y1 < 0 && line2.Y2 < 0) || (line2.Y1 >= 0 && line2.Y2 >= 0)) {
        return GM_None;
    }

    //  (3) Discover the position of the intersection point along line A-B.
    double posAB = line2.X2 + (line2.X1 - line2.X2) * line2.Y2 / (line2.Y2 - line2.Y1);

    //  Fail if segment C-D crosses line A-B outside of segment A-B.
    if (posAB < 0 || posAB > distAB) {
        return GM_None;
    }
    //  (4) Apply the discovered position to line A-B in the original coordinate system.
    return GM_Intersection;
}

double DistanceOf(PointF point1, PointF point2)
{
    return sqrt((point1.X-point2.X)*(point1.X-point2.X) + (point1.Y-point2.Y)*(point1.Y-point2.Y));
}

double DistanceOf(PointF point, Line line)
{
//    float a = sqrt((point.X-line.X1)*(point.X-line.X1) + (point.Y-line.Y1)*(point.Y-line.Y1));
//    float b = sqrt((point.X-line.X2)*(point.X-line.X2) + (point.Y-line.Y2)*(point.Y-line.Y2));
    double c = sqrt((line.X1-line.X2)*(line.X1-line.X2) + (line.Y1-line.Y2)*(line.Y1-line.Y2));

//    float p = (a+b+c)/2;

//    dis = 2 * sqrt(p*(p-a)*(p-b)*(p-c)) / c;

    return fabs(point.X*line.Y1 + point.Y*line.X2 + line.X1*line.Y2 - line.X2*line.Y1 - line.X1*point.Y - point.X*line.Y2) / c;
}

/*********************************************************
 * p2----------->p1 线端和Y轴的夹角
 * @param p1
 * @param p2
 * @return yaw
 */
double YawOf(PointF p1, PointF p2)
{
    double deg = 0.0;

    if (fabs(p1.Y - p2.Y) <= GLB_EPSILON) {
        if (p1.X > p2.X) {
            deg = 90;
        } else {
            deg = 270;
        }
    } else if (fabs(p1.X - p2.X) <= GLB_EPSILON) {
        if (p1.Y > p2.Y) {
            deg = 0;
        } else {
            deg = 180;
        }
    } else {
        deg = atan(fabs(p1.X - p2.X) /
                   fabs(p1.Y - p2.Y));

        deg = toDegree(deg);

        if (p1.X > p2.X &&
            p1.Y > p2.Y) {

        } else if (p1.X < p2.X &&
                   p1.Y > p2.Y) {
            deg = 360 - deg;
        } else if (p1.X < p2.X &&
                   p1.Y < p2.Y) {
            deg = 180 + deg;
        } else if (p1.X > p2.X &&
                   p1.Y < p2.Y) {
            deg = 180 - deg;
        }
    }

    return deg;
}

/**********************************************************
 * base 和 dest的第二点重合时形成的夹角
 * @param base
 * @param dest
 * @return
 */
double CalculateAngle(Line base, Line dest)
{
    double angle = 0;

    double dx = base.X2 - dest.X2;
    double dy = base.Y2 - dest.Y2;

    dest.X1 += dx;
    dest.Y1 += dy;

    double c2 = pow((dest.X1 - base.X1), 2) + pow((dest.Y1 - base.Y1), 2);
    double a2 = pow((base.X1 - base.X2), 2) + pow((base.Y1 - base.Y2), 2);
    double b2 = pow((dest.X1 - base.X2), 2) + pow((dest.Y1 - base.Y2), 2);

    angle = acos((a2 + b2 - c2) / (2 * sqrt(a2) * sqrt(b2)));

    return toDegree(angle);
}

PointF rotatePoint(PointF oldPoint, PointF centre, double degree) {
    PointF newPoint;
    newPoint.X = (oldPoint.X-centre.X)*cos(toRadians(degree)) - (oldPoint.Y-centre.Y)*sin(toRadians(degree)) + centre.X;
    newPoint.Y = (oldPoint.X-centre.X)*sin(toRadians(degree)) + (oldPoint.Y-centre.Y)*cos(toRadians(degree)) + centre.Y;
    return newPoint;
}

// All in
bool InsidePolygon(const Polygon *t1, const Polygon *t2) {
    for (int i = 0; i < t1->num; ++i) {
        if (IntersectionOf(t1->point[i], t2) != GM_Containment) {
            return false;
        }
    }
    return true;
}

// Any part inside
bool PartInsidePolygon(const Polygon *t1, const Polygon *t2) {
    for (int i = 0; i < t1->num; ++i) {
        if (IntersectionOf(t1->point[i], t2) == GM_Containment) {
            return true;
        }
    }
    return false;
}

// All out
bool OutsidePolygon(const Polygon *t1, const Polygon *t2) {
    for (int i = 0; i < t1->num; ++i) {
        if (IntersectionOf(t1->point[i], t2) != GM_None) {
            return false;
        }
    }
    return true;
}

/***************************************************************
 * @brief p3位于由p1--------->p2构成的射线，左侧还是右侧，同向，反向
 * @param p1
 * @param p2
 * @param p3
 * @return 0 - straight, 1 - left, -1 - right, 2 - front, -2 - back
 */
int IntersectionOfLine(PointF p1, PointF p2, PointF p3)
{
    double lr = (p1.X-p3.X)*(p2.Y-p3.Y) - (p1.Y-p3.Y)*(p2.X-p3.X);

    if (fabs(lr) <= EPSILON2) {
        double fb = (p2.X-p1.X)*(p3.X-p1.X) + (p2.Y-p1.Y)*(p3.Y-p1.Y);
        if (fabs(fb) <= EPSILON2)
            return 0;
        else if (fb > 0)
            return 2;
        else
            return -2;
    } else if (lr > 0) {
        return 1;
    } else {
        return -1;
    }
}

// 0 - straight, 1 - left, -1 - right, 2 - front, -2 - back
int IntersectionOfLine(PointF p, Line line)
{
    PointF p1, p2;

    p1.X = line.X1;
    p1.Y = line.Y1;
    p2.X = line.X2;
    p2.Y = line.Y2;

    return IntersectionOfLine(p1, p2, p);
}

/***************************************************************
 * 得到p3于p1,p2组成的直线上的垂点
 * @param p1
 * @param p2
 * @param p3
 * @return
 */
PointF GetVerticalPoint(PointF p1, PointF p2, PointF p3)
{
    PointF p4;

    if (isEqual2(p1.X, p2.X)) {
        p4.Y = p3.Y;
        p4.X = p1.X;
        return p4;
    }
    if (isEqual2(p1.Y, p2.Y)) {
        p4.X = p3.X;
        p4.Y = p1.Y;
        return p4;
    }

    double k = (p2.Y - p1.Y) / (p2.X - p1.X);
    double b = p1.Y - k * p1.X;

    double k2 = (p2.X - p1.X) / (p1.Y - p2.Y);
    double b2 = p3.Y - p3.X * k2;

    p4.X = (b2 - b) / (k - k2);
    p4.Y = k2 * p4.X + b2;

    return p4;
}

/**************************************************************
 * p3于 p1---p2构成直线
 * @param p1
 * @param p2
 * @param p3
 * @return
 */
bool VerticalPointOnLine(PointF point, Line line)
{
    PointF p1, p2;

    p1.X = line.X1;
    p1.Y = line.Y1;

    p2.X = line.X2;
    p2.Y = line.Y2;

    PointF pv = GetVerticalPoint(p1, p2, point);

    if (isEqual2(pv.X, MIN(p1.X, p2.X)) || isEqual2(pv.X, MAX(p1.X, p2.X)) ||
        (pv.X > MIN(p1.X, p2.X) && pv.X < MAX(p1.X, p2.X))) {
        return true;
    }
    return false;
}

bool VerticalPointOnLine(PointF point, Line line, PointF &vp)
{
    PointF p1, p2;

    p1.X = line.X1;
    p1.Y = line.Y1;

    p2.X = line.X2;
    p2.Y = line.Y2;

    PointF pv = GetVerticalPoint(p1, p2, point);
    vp = pv;

    if (isEqual2(pv.X, MIN(p1.X, p2.X)) || isEqual2(pv.X, MAX(p1.X, p2.X)) ||
        (pv.X > MIN(p1.X, p2.X) && pv.X < MAX(p1.X, p2.X))) {
        return true;
    }
    return false;
}

/****************************************************************
 *                      p3
 *                      |  'L'
 *                      |
 * p1------------------>p2
 *                      |
 *                      |   'R'
 *                      p3
 * @param p1
 * @param p2
 * @param L
 * @param dir
 * @return
 */
PointF Calc3Point(PointF p1, PointF p2, double L, char dir)
{
    PointF p3;

    dir = toupper(dir);

    if (dir != 'L' && dir != 'R')
        dir = 'L';

    if (isEqual(p1.X, p2.X)) {
        p3.Y = p2.Y;
        if (p2.Y > p1.Y) {
            p3.X = p2.X + ((dir == 'L')? -L:L);
        } else {
            p3.X = p2.X + ((dir=='L')?L:-L);
        }
        return p3;
    }
    if (isEqual(p1.Y, p2.Y)) {
        p3.X = p2.X;
        if (p2.X > p1.X) {
            p3.Y = p2.Y + ((dir == 'L')? L:-L);
        } else {
            p3.Y = p2.Y + ((dir == 'L')? -L:L);
        }
        return p3;
    }

    double k = (p2.Y - p1.Y) / (p2.X - p1.X);
    double b = p1.Y - k*p1.X;

    double A = 1 + pow(k, 2);
    double B = 2*k*(b - p2.Y) - 2*p2.X;
    double C = pow(b - p2.Y, 2) + pow(p2.X, 2) - pow(L,2);

    double x3, y3;


    if (p1.X < p2.X) {
        x3 = (- B - sqrt(pow(B, 2) - 4*A*C))/(2*A);
    } else {
        x3 = (- B + sqrt(pow(B, 2) - 4*A*C))/(2*A);
    }
    y3 = k * x3 + b;

    p3.X = x3;
    p3.Y = y3;

    p3 = rotatePoint(p3, p2, (dir == 'L') ? 270 : 90);

    return p3;
}

/*******************************************************
 * ori点在yaw方向上延长的距离
 * @param ori
 * @param length
 * @param yaw
 * @return
 */
PointF PointExtend(PointF ori, double length, double yaw)
{
    PointF ext;

    ext.X = ori.X + length * sin(toRadians(yaw));
    ext.Y = ori.Y + length * cos(toRadians(yaw));

    return ext;
}

bool IsSamePoint(PointF p1, PointF p2)
{
    return isEqual(p1.X, p2.X) && isEqual(p1.Y, p2.Y);
}