#include "Gaussian.h"

#include "CoordSys.h"
#include "Wgs84.h"

namespace jf {

const double ZoneOffset = 1e6;
// const int ZoneWidth = 2.7;
const double EastOffset = 500000.0;

#define use_gaussian_convert_deprecated
#define pi 3.1415926535897

double GetValidZoneWidth_impl(const Point& left, const Point& right) {
    for (double d = 2; d < 4; d += 0.1) {
        if (int((left.dLon + 1.5) / d) == int((right.dLon + 1.5) / d)) {
            return d;
        }
    }
    assert(false);
    return 0.0;
}

XYZ gaussian_convert_deprecated(const Point& inp, double ZoneWidth) {
    int Zone = int((inp.dLon + 1.5) / ZoneWidth);
    double L0 = Zone * ZoneWidth;

    double dL = inp.dLon - L0 * 1.0;

    double dLRadian = Degree2Rad(dL);
    double latRadian = Degree2Rad(inp.dLat);

    double X = GetX(latRadian);
    double N = GetN(latRadian);

    double t = tan(latRadian);
    double t2 = t * t;
    double sinB = sin(latRadian);
    double cosB = cos(latRadian);
    double ita2 = ep2 * cosB * cosB;

    double temp = dLRadian * cosB;
    double temp2 = temp * temp;
    double yPart1 = 0.5 * N * temp * temp * t;
    double yPart2 = N * t * (5.0 - t2 + 9.0 * ita2 + 4.0 * ita2 * ita2) * temp2 * temp2 / 24.0;
    double yPart3 = temp2 * temp2 * temp2 * N * t * (61.0 - 58.0 * t2 + t2 * t2) / 720.0;
    double y = X + yPart1 + yPart2 + yPart3;

    double xPart1 = N * temp;
    double xPart2 = (1.0 - t2 + ita2) * N * temp * temp2 / 6.0;
    double xPart3 = (5.0 - 18.0 * t2 + t2 * t2 + 14.0 * ita2 - 58.0 * ita2 * t2) * N * temp * temp2 * temp2 / 120.0;

    double x = Zone * 1.0 * ZoneOffset * 1.0 + EastOffset + xPart1 + xPart2 + xPart3;
    // double h = inLonLat[2];
    return XYZ{x, y, inp.dAlt};
}

XYZ Convert(const Point& inp, double ZoneWidth) {
#ifdef use_gaussian_convert_deprecated
    return gaussian_convert_deprecated(inp, ZoneWidth);
#else
    return cartesian_convert(inp);
#endif
}

Point gaussian_inverse_deprecated(const XYZ& inxyz, double ZoneWidth) {
    int Zone = int(inxyz.x / ZoneOffset);

    double L0 = Zone * ZoneWidth;
    double L0_radian = Degree2Rad(L0);

    double X0 = Zone * ZoneOffset + EastOffset;
    double Y0 = 0;

    double x = inxyz.x - X0;
    double y = inxyz.y - Y0;

    double Br = GetLatByX(y);

    double cosB = cos(Br);
    double secB = 1.0 / cosB;
    double ita2 = ep2 * cosB * cosB;
    double t = tan(Br);
    double t2 = t * t;
    double V = GetV(Br);
    double V2 = V * V;
    double N = c / V;
    double M = N / V / V;
    double D = x / N;

    double tmp3 = (1.0 + 2.0 * t2 + ita2) * D * D * D / 6.0;
    double tmp5 = (5.0 + 28.0 * t2 + 24.0 * t2 * t2 + 6.0 * ita2 + 8.0 * ita2 * t2) * D * D * D * D * D / 120.0;
    double l = secB * (D - tmp3 + tmp5);
    double L_radian = L0_radian + l;
    double tmp2 = D * D / 2.0;
    double tmp4 = (5.0 + 3.0 * t2 + ita2 - 9.0 * ita2 * t2) * D * D * D * D / 24.0;
    double tmp6 = (61.0 + 90.0 * t2 + 45.0 * t2 * t2) * D * D * D * D * D * D / 720.0;
    double B_radian = Br - t * V2 * (tmp2 - tmp4 + tmp6);

    double B = Rad2Degree(B_radian);
    double L = Rad2Degree(L_radian);
    return Point{L, B, inxyz.z};
}

Point Inverse(const XYZ& inxyz, double ZoneWidth) {
#ifdef use_gaussian_convert_deprecated
    return gaussian_inverse_deprecated(inxyz, ZoneWidth);
#else
    return cartesian_inverse(inxyz);
#endif
}

}  // namespace jf
