#include "MapTypes.hpp"

const double EARTH_RADIUS = 6378137.0;
const double PI = 3.1415926535897932384626;

namespace jf {
double PointsDis(const Point &p1, const Point &p2, bool consider_alt) { return PointsDis(p1.dLon, p1.dLat, p2.dLon, p2.dLat); }

double PointsDis(double lng1, double lat1, double lng2, double lat2) {
    double radLat1 = Degree2Rad(lat1);
    double radLng1 = Degree2Rad(lng1);
    double radLat2 = Degree2Rad(lat2);
    double radLng2 = Degree2Rad(lng2);
    double a = radLat2 - radLat1;
    double b = radLng2 - radLng1;
    double result = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(radLat1) * cos(radLat2) * pow(sin(b / 2), 2))) * EARTH_RADIUS;
    return result;
}

void PointDis2NewPoint(const Point &ptInPoint, double dLonDis, double dLatDis, Point &ptOutPoint) {
    double dLonAngle = (dLonDis * 180) / (PI * EARTH_RADIUS);
    double dLatAngle = (dLatDis * 180) / (PI * EARTH_RADIUS);
    ptOutPoint = Point(ptInPoint.dLon + dLonAngle, ptInPoint.dLat + dLatAngle, ptInPoint.dAlt);

    return;
}

double Degree2Rad(double angle) { return angle * PI / 180.0; }

double Rad2Degree(double rad) { return rad * 180.0 / PI; }


/**
 * 计算有向线段s->e
 * @param s
 * @param e
 * @return
 */
double PointsAngle(const Point &s, const Point &e) {
    XYZ xyzS = P2XYZ(s);
    XYZ xyzE = P2XYZ(e);

    double dAngle = atan2((xyzE.x - xyzS.x), (xyzE.y - xyzS.y));
    dAngle = dAngle * (180 / PI);

    if (dAngle < 0) {
        dAngle = dAngle + 360;
    }

    return dAngle;
    // return abs(atan(xyzS.y / xyzS.x) - atan(xyzE.y / xyzE.x));

    // double dAngle = 0.0;
    // double dXX = xyzE.x - xyzS.x;
    // double dYY = xyzE.y - xyzS.y;
    // if (dXX == 0.0)
    //     dAngle = PI / 2.0;
    // else
    //     dAngle = atan(fabs(dYY / dXX));
    // if ((dXX < 0.0) && (dYY >= 0.0))
    //     dAngle = PI - dAngle;
    // else if ((dXX < 0.0) && (dYY < 0.0))
    //     dAngle = PI + dAngle;
    // else if ((dXX >= 0.0) && (dYY < 0.0))
    //     dAngle = PI * 2.0 - dAngle;
    // return dAngle;
}

double AngleSub(double dAngle1, double dAngle2) {
    if (std::abs(dAngle1 - dAngle2) > 360.0 - std::abs(dAngle1 - dAngle2)) {
        return 360.0 - std::abs(dAngle1 - dAngle2);
    } else {
        return std::abs(dAngle1 - dAngle2);
    }
}

}  // namespace jf