package com.imdroid.sideslope.bd;

import static java.lang.Math.*;

public class GeoCoordConverterUtil {
    // WGS84 模型常量
    public static final double WGS84_A = 6378137.0;
    public static final double WGS84_B = 6356752.3142;
    public static final double WGS84_F = (1.0 / 298.257223563);
    public static final double WGS84_E2 = 0.00669437999014;
    public static final double EPSILON = 1e-12;

    public static class LLA_Coordinate {
        double Latitude;
        double Longitude;
        double Altitude;

        public double getLatitude() {
            return Latitude;
        }

        public void setLatitude(double latitude) {
            Latitude = latitude;
        }

        public double getLongitude() {
            return Longitude;
        }

        public void setLongitude(double longitude) {
            Longitude = longitude;
        }

        public double getAltitude() {
            return Altitude;
        }

        public void setAltitude(double altitude) {
            Altitude = altitude;
        }

        public LLA_Coordinate(double latitude, double longitude, double altitude) {
            if (Double.isNaN(latitude) || Double.isNaN(longitude) || Double.isNaN(altitude)) {
                throw new IllegalArgumentException("Invalid LLA coordinates: NaN values are not allowed.");
            }
            if (Double.isInfinite(latitude) || Double.isInfinite(longitude) || Double.isInfinite(altitude)) {
                throw new IllegalArgumentException("Invalid LLA coordinates: Infinite values are not allowed.");
            }
            if (latitude < -90.0 || latitude > 90.0) {
                throw new IllegalArgumentException("Invalid latitude value: must be between -90 and 90 degrees.");
            }
            if (longitude < -180.0 || longitude > 180.0) {
                throw new IllegalArgumentException("Invalid longitude value: must be between -180 and 180 degrees.");
            }

            this.Latitude = latitude;
            this.Longitude = longitude;
            this.Altitude = altitude;
        }
        public String toString() {
            return "LLA_Coordinate{" +
                    "Latitude=" + Latitude +
                    ", Longitude=" + Longitude +
                    ", Altitude=" + Altitude +
                    '}';
        }

    }

    public static class ECEF_Coordinate{
        double ECEF_X;
        double ECEF_Y;
        double ECEF_Z;

        public double getECEF_X() {
            return ECEF_X;
        }

        public double getECEF_Y() {
            return ECEF_Y;
        }

        public double getECEF_Z() {
            return ECEF_Z;
        }

        public void setECEF_X(double ECEF_X) {
            this.ECEF_X = ECEF_X;
        }

        public void setECEF_Y(double ECEF_Y) {
            this.ECEF_Y = ECEF_Y;
        }

        public void setECEF_Z(double ECEF_Z) {
            this.ECEF_Z = ECEF_Z;
        }

        public ECEF_Coordinate(double ecefX, double ecefY, double ecefZ) {
            if (Double.isNaN(ecefX) || Double.isNaN(ecefY) || Double.isNaN(ecefZ)) {
                throw new IllegalArgumentException("Invalid ECEF coordinates: NaN values are not allowed.");
            }
            if (Double.isInfinite(ecefX) || Double.isInfinite(ecefY) || Double.isInfinite(ecefZ)) {
                throw new IllegalArgumentException("Invalid ECEF coordinates: Infinite values are not allowed.");
            }

            this.ECEF_X = ecefX;
            this.ECEF_Y = ecefY;
            this.ECEF_Z = ecefZ;
        }

        public String toString() {
            return "ECEF_Coordinate{" +
                    "ECEF_X=" + ECEF_X +
                    ", ECEF_Y=" + ECEF_Y +
                    ", ECEF_Z=" + ECEF_Z +
                    '}';
        }

    }

    public static class ENU_Coordinate{
        double ENU_E;
        double ENU_N;
        double ENU_U;
        public ENU_Coordinate(double E, double N, double U) {
            if (Double.isNaN(E) || Double.isNaN(N) || Double.isNaN(U)) {
                throw new IllegalArgumentException("Invalid ENU coordinates: NaN values are not allowed.");
            }
            if (Double.isInfinite(E) || Double.isInfinite(N) || Double.isInfinite(U)) {
                throw new IllegalArgumentException("Invalid ENU coordinates: Infinite values are not allowed.");
            }

            this.ENU_E = E;
            this.ENU_N = N;
            this.ENU_U = U;
        }

        public String toString() {
            return "ENU_Coordinate{" +
                    "ENU_E=" + ENU_E +
                    ", ENU_N=" + ENU_N +
                    ", ENU_U=" + ENU_U +
                    '}';
        }

        public double getENU_E() {
            return ENU_E;
        }

        public void setENU_E(double ENU_E) {
            this.ENU_E = ENU_E;
        }

        public double getENU_N() {
            return ENU_N;
        }

        public void setENU_N(double ENU_N) {
            this.ENU_N = ENU_N;
        }

        public double getENU_U() {
            return ENU_U;
        }

        public void setENU_U(double ENU_U) {
            this.ENU_U = ENU_U;
        }
    }


    /**
     * 将地理坐标（纬度、经度、海拔）转换为地心地固坐标（ECEF）。
     *
     * <p>该方法将给定的纬度、经度和高程（LLA）转换为 WGS84 参考系下的 ECEF 坐标。
     * 转换过程中使用了 WGS84 椭球体模型常数。</p>
     *
     * <p>使用的公式参考自：
     * <a href="https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates">
     * 地理坐标转换 - 维基百科</a></p>
     *
     * @param LLA 要转换的地理坐标。纬度和经度以度为单位，海拔以米为单位。
     * @return 一个 {@link ECEF_Coordinate} 对象，表示转换后的 ECEF 坐标。X、Y 和 Z 的值以米为单位。
     */
    public static ECEF_Coordinate LLA2ECEF(LLA_Coordinate LLA){
        // 度转弧度
        double lat_rad = LLA.Latitude * PI / 180.0;
        double lon_rad = LLA.Longitude * PI / 180.0;

        // 中间变量
        double N = WGS84_A / sqrt(1.0 - WGS84_E2  * sin(lat_rad) * sin(lat_rad));

        //结果
        double x = (N + LLA.Altitude) * cos(lat_rad) * cos(lon_rad);
        double y = (N + LLA.Altitude) * cos(lat_rad) * sin(lon_rad);
        double z = ((1.0 - WGS84_E2) * N + LLA.Altitude) * sin(lat_rad);

        return new ECEF_Coordinate(x, y, z);
    }

    /**
     * 将地心地固坐标（ECEF）转换为地理坐标（纬度、经度、海拔）。
     *
     * <p>该方法将给定的 ECEF 坐标转换为 WGS84 参考系下的地理坐标（LLA）。
     * 转换过程中使用了 WGS84 椭球体模型常数。</p>
     *
     * <p>使用的公式参考自：
     * <a href="https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates">
     * 地理坐标转换 - 维基百科</a></p>
     *
     * @param ECEF 要转换的 ECEF 坐标。X、Y 和 Z 的值以米为单位。
     * @return 一个 {@link LLA_Coordinate} 对象，表示转换后的地理坐标。纬度和经度以度为单位，海拔以米为单位。
     */
    public static LLA_Coordinate ECEF2LLA(ECEF_Coordinate ECEF){

        double X = ECEF.ECEF_X;
        double Y = ECEF.ECEF_Y;
        double Z = ECEF.ECEF_Z;

        if (X == 0 && Y == 0) {
            double lat = Z > 0 ? 90.0 : -90.0;
            double lon = 0.0;
            double h = abs(Z) - WGS84_A * sqrt(1 - WGS84_E2);

            return new LLA_Coordinate(lat, lon, h);
        }

        // 公式涉及的中间变量
        double lambda = atan2(Y, X);
        double phi;
        double h;
        double N;
        double p = sqrt(X*X + Y*Y);
        // 第一个猜测 h≈0 开始，然后更新 N
        double phi_new = atan2(Z, p*(1-WGS84_E2));
        // 迭代公式
        do {
            phi = phi_new;
            N = WGS84_A / sqrt(1 - WGS84_E2 * sin(phi) * sin(phi));
            h = p / cos(phi) - N;
            phi_new = atan2(Z, (1-(WGS84_E2*N)/(N+h))*p);
        } while (abs(phi_new - phi) > EPSILON);

        double lat = phi_new * 180.0 / PI;
        double lon = lambda * 180.0 / PI;

        return new LLA_Coordinate(lat, lon, h);
    }

    /**
     * 将地心地固（ECEF）坐标转换为东-北-天（ENU）坐标。
     *
     * <p>该方法将给定的两个 ECEF 坐标（基准站和流动站）转换为以基准站为原点的 ENU 坐标。
     * 转换过程中使用了 WGS84 参考系。</p>
     *
     * <p>使用的公式参考自：
     * <a href="https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates">
     * 地理坐标转换 - 维基百科</a></p>
     *
     * @param referenceStationECEF 基准站的 ECEF 坐标。X、Y 和 Z 的值以米为单位。
     * @param roverStationECEF 流动站的 ECEF 坐标。X、Y 和 Z 的值以米为单位。
     * @return 一个 {@link ENU_Coordinate} 对象，表示转换后的 ENU 坐标。E、N 和 U 的值以米为单位。
     */
    public static ENU_Coordinate ECEF2ENU(ECEF_Coordinate referenceStationECEF, ECEF_Coordinate roverStationECEF) {
        ECEF_Coordinate differenceECEF = new ECEF_Coordinate(0,0,0);
        differenceECEF.ECEF_X = roverStationECEF.ECEF_X - referenceStationECEF.ECEF_X;
        differenceECEF.ECEF_Y = roverStationECEF.ECEF_Y - referenceStationECEF.ECEF_Y;
        differenceECEF.ECEF_Z = roverStationECEF.ECEF_Z - referenceStationECEF.ECEF_Z;
        // 得到计算中所需要的 基站的 LLA
        LLA_Coordinate referenceStationLLA = ECEF2LLA(referenceStationECEF);

        // 将经纬度转换为弧度，用于三角函数计算
        double lat = referenceStationLLA.Latitude * Math.PI / 180;
        double lon = referenceStationLLA.Longitude * Math.PI / 180;

        double sin_lat = Math.sin(lat);
        double cos_lat = Math.cos(lat);
        double sin_lon = Math.sin(lon);
        double cos_lon = Math.cos(lon);

        // 构建计算中所用到的矩阵S
        double[][] S = {
                {-sin_lon, cos_lon, 0},
                {-sin_lat * cos_lon, -sin_lat * sin_lon, cos_lat},
                {cos_lat * cos_lon, cos_lat * sin_lon, sin_lat}
        };

        ENU_Coordinate enu = new ENU_Coordinate(0,0,0);
        enu.ENU_E = S[0][0] * differenceECEF.ECEF_X + S[0][1] * differenceECEF.ECEF_Y + S[0][2] * differenceECEF.ECEF_Z;
        enu.ENU_N = S[1][0] * differenceECEF.ECEF_X + S[1][1] * differenceECEF.ECEF_Y + S[1][2] * differenceECEF.ECEF_Z;
        enu.ENU_U = S[2][0] * differenceECEF.ECEF_X + S[2][1] * differenceECEF.ECEF_Y + S[2][2] * differenceECEF.ECEF_Z;

        return enu;
    }






}
