package com.shcd.utils.geo;

import org.geotools.geometry.jts.JTS;
import org.geotools.referencing.CRS;
import org.locationtech.jts.geom.Coordinate;
import org.opengis.referencing.FactoryException;
import org.opengis.referencing.crs.CoordinateReferenceSystem;
import org.opengis.referencing.operation.MathTransform;
import org.opengis.referencing.operation.TransformException;


public class CoordinateUtils {
    public static double pi = 3.1415926535897932384626;
    public static double x_pi = 3.14159265358979324 * 3000.0 / 180.0;
    public static double a = 6378245.0;
    public static double ee = 0.00669342162296594323;

    //上海城建 参数
    private static final double[] vm = {0.0174532925199433, 0.0002908882086657216, 0.0000048481368110953599};
    private static final double HALFPI = 1.5707963267948966;
    private static final double EPS = 1.0e-12;
    private static  final double SPI = 3.14159265359;
    private static  final double TWOPI = 6.2831853071795864769;
    private static  final double ONEPI = 3.14159265358979323846;
    private static  final double FC1 = 1.0;
    private static  final double FC2 = 0.5;
    private static  final double FC3 = 0.16666666666666666666;
    private static  final double FC4 = 0.08333333333333333333;
    private static  final double FC5 = 0.05;
    private static final  double FC6 = 0.03333333333333333333;
    private static  final double FC7 = 0.02380952380952380952;
    private static final  double FC8 = 0.01785714285714285714;
    private static  final double C00 = 1.0;
    private static  final double C02 = 0.25;
    private static  final double C04 = 0.046875;
    private static  final double C06 = 0.01953125;
    private static  final double C08 = 0.01068115234375;
    private static  final double C22 = 0.75;
    private static  final double C44 = 0.46875;
    private static final  double C46 = 0.01302083333333333333;
    private static  final double C48 = 0.00712076822916666666;
    private static  final double C66 = 0.36458333333333333333;
    private static final  double C68 = 0.00569661458333333333;
    private static  final double C88 = 0.3076171875;
    private static final double HUGE_VAL = 1.0 / 0.0;
    private static final double RAD_TO_DEG = 57.29577951308232;
    private static final int MAX_ITER = 10;

    public static boolean isSH(double lon, double lat) {
        if (lon < 120 || lon > 123)
            return false;
        if (lat < 30 || lat > 32)
            return false;
        return true;
    }


    private static double transformLat(double x, double y) {
        double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y
                + 0.2 * Math.sqrt(Math.abs(x));
        ret += (20.0 * Math.sin(6.0 * x * pi) + 20.0 * Math.sin(2.0 * x * pi)) * 2.0 / 3.0;
        ret += (20.0 * Math.sin(y * pi) + 40.0 * Math.sin(y / 3.0 * pi)) * 2.0 / 3.0;
        ret += (160.0 * Math.sin(y / 12.0 * pi) + 320 * Math.sin(y * pi / 30.0)) * 2.0 / 3.0;
        return ret;
    }

    private static double transformLon(double x, double y) {
        double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1
                * Math.sqrt(Math.abs(x));
        ret += (20.0 * Math.sin(6.0 * x * pi) + 20.0 * Math.sin(2.0 * x * pi)) * 2.0 / 3.0;
        ret += (20.0 * Math.sin(x * pi) + 40.0 * Math.sin(x / 3.0 * pi)) * 2.0 / 3.0;
        ret += (150.0 * Math.sin(x / 12.0 * pi) + 300.0 * Math.sin(x / 30.0
                * pi)) * 2.0 / 3.0;
        return ret;
    }

    private static double[] transform(double lat, double lon) {
        if (outOfChina(lat, lon)) {
            return new double[]{lat, lon};
        }
        double dLat = transformLat(lon - 105.0, lat - 35.0);
        double dLon = transformLon(lon - 105.0, lat - 35.0);
        double radLat = lat / 180.0 * pi;
        double magic = Math.sin(radLat);
        magic = 1 - ee * magic * magic;
        double sqrtMagic = Math.sqrt(magic);
        dLat = (dLat * 180.0) / ((a * (1 - ee)) / (magic * sqrtMagic) * pi);
        dLon = (dLon * 180.0) / (a / sqrtMagic * Math.cos(radLat) * pi);
        double mgLat = lat + dLat;
        double mgLon = lon + dLon;
        return new double[]{mgLat, mgLon};
    }

    public static boolean outOfChina(double lat, double lon) {
        if (lon < 72.004 || lon > 137.8347)
            return true;
        if (lat < 0.8293 || lat > 55.8271)
            return true;
        return false;
    }

    /**
     * 84 to 火星坐标系 (GCJ-02) World Geodetic System ==> Mars Geodetic System
     *
     * @param lat
     * @param lon
     * @return
     */
    public static double[] gps84_To_Gcj02(double lat, double lon) {
        if (outOfChina(lat, lon)) {
            return new double[]{lat, lon};
        }
        double dLat = transformLat(lon - 105.0, lat - 35.0);
        double dLon = transformLon(lon - 105.0, lat - 35.0);
        double radLat = lat / 180.0 * pi;
        double magic = Math.sin(radLat);
        magic = 1 - ee * magic * magic;
        double sqrtMagic = Math.sqrt(magic);
        dLat = (dLat * 180.0) / ((a * (1 - ee)) / (magic * sqrtMagic) * pi);
        dLon = (dLon * 180.0) / (a / sqrtMagic * Math.cos(radLat) * pi);
        double mgLat = lat + dLat;
        double mgLon = lon + dLon;
        return new double[]{mgLat, mgLon};
    }

    /**
     * * 火星坐标系 (GCJ-02) to 84 * * @param lon * @param lat * @return
     */
    public static double[] gcj02_To_Gps84(double lat, double lon) {
        double[] gps = transform(lat, lon);
        double lontitude = lon * 2 - gps[1];
        double latitude = lat * 2 - gps[0];
        return new double[]{latitude, lontitude};
    }

    /**
     * 火星坐标系 (GCJ-02) 与百度坐标系 (BD-09) 的转换算法 将 GCJ-02 坐标转换成 BD-09 坐标
     *
     * @param lat
     * @param lon
     */
    public static double[] gcj02_To_Bd09(double lat, double lon) {
        double x = lon, y = lat;
        double z = Math.sqrt(x * x + y * y) + 0.00002 * Math.sin(y * x_pi);
        double theta = Math.atan2(y, x) + 0.000003 * Math.cos(x * x_pi);
        double tempLon = z * Math.cos(theta) + 0.0065;
        double tempLat = z * Math.sin(theta) + 0.006;
        double[] gps = {tempLat, tempLon};
        return gps;
    }

    /**
     * * 火星坐标系 (GCJ-02) 与百度坐标系 (BD-09) 的转换算法 * * 将 BD-09 坐标转换成GCJ-02 坐标 * * @param
     * bd_lat * @param bd_lon * @return
     */
    public static double[] bd09_To_Gcj02(double lat, double lon) {
        double x = lon - 0.0065, y = lat - 0.006;
        double z = Math.sqrt(x * x + y * y) - 0.00002 * Math.sin(y * x_pi);
        double theta = Math.atan2(y, x) - 0.000003 * Math.cos(x * x_pi);
        double tempLon = z * Math.cos(theta);
        double tempLat = z * Math.sin(theta);
        double[] gps = {tempLat, tempLon};
        return gps;
    }

    /**
     * 将gps84转为bd09
     *
     * @param lat
     * @param lon
     * @return
     */
    public static double[] gps84_To_bd09(double lat, double lon) {
        double[] gcj02 = gps84_To_Gcj02(lat, lon);
        double[] bd09 = gcj02_To_Bd09(gcj02[0], gcj02[1]);
        return bd09;
    }

    public static double[] bd09_To_gps84(double lat, double lon) {
        double[] gcj02 = bd09_To_Gcj02(lat, lon);
        double[] gps84 = gcj02_To_Gps84(gcj02[0], gcj02[1]);
        //保留小数点后七位
        gps84[0] = retain7(gps84[0]);
        gps84[1] = retain7(gps84[1]);
        return gps84;
    }

    /**
     * 保留小数点后六位
     *
     * @param num
     * @return
     */
    private static double retain7(double num) {
        String result = String.format("%.7f", num);
        return Double.valueOf(result);
    }


    /**
     * gps84 转 墨卡托
     *
     * @param lat
     * @param lon
     * @return
     */
    public static double[] gps84_To_Mercator(double lon, double lat) {
        double[] xy = new double[2];
        double x = lon * 20037508.342789 / 180;
        double y = Math.log(Math.tan((90 + lat) * Math.PI / 360)) / (Math.PI / 180);
        y = y * 20037508.34789 / 180;
        xy[0] = x;
        xy[1] = y;
        return xy;
    }

    /**
     * 墨卡托转gps84
     *
     * @param mercatorX
     * @param mercatorY
     * @return
     */
    public static double[] mercator_To_gps84(double mercatorX, double mercatorY) {
        double[] xy = new double[2];
        double x = mercatorX / 20037508.34 * 180;
        double y = mercatorY / 20037508.34 * 180;
        y = 180 / Math.PI * (2 * Math.atan(Math.exp(y * Math.PI / 180)) - Math.PI / 2);
        xy[0] = x;
        xy[1] = y;
        return xy;

    }

    public static double[] gps84_To_CGCS2000(double lon, double lat) {
        Coordinate tar = null;
        double[] res = new double[2];
        try {
            //封装点，这个是通用的，也可以用POINT（y,x）
            // private static WKTReader reader = new WKTReader( geometryFactory );
            Coordinate sour = new Coordinate(lat, lon);
            //这里要选择转换的坐标系是可以随意更换的
            CoordinateReferenceSystem source = CRS.decode("EPSG:4326");
            CoordinateReferenceSystem target = CRS.decode("EPSG:4528");
            //建立转换，下面两个我屏掉的转换方式会报出需要3/7参数的异常
            MathTransform transform = CRS.findMathTransform(source, target, true);
            tar = new Coordinate();
            //转换
            JTS.transform(sour, tar, transform);

            res[0] = tar.y;
            res[1] = tar.x;
            return res;
        } catch (FactoryException | TransformException e) {
            e.printStackTrace();
            return null;
        }
    }

    public static double[] CGCS2000_To_gps84(double x, double y) {
        Coordinate tar = null;
        double[] res = new double[2];
        try {
            //封装点，这个是通用的，也可以用POINT（y,x）
            // private static WKTReader reader = new WKTReader( geometryFactory );
            Coordinate sour = new Coordinate(y, x);
            //这里要选择转换的坐标系是可以随意更换的
            CoordinateReferenceSystem source = CRS.decode("EPSG:4528");
            CoordinateReferenceSystem target = CRS.decode("EPSG:4326");
            //建立转换，下面两个我屏掉的转换方式会报出需要3/7参数的异常
            MathTransform transform = CRS.findMathTransform(source, target, true);
            tar = new Coordinate();
            //转换
            JTS.transform(sour, tar, transform);

            res[0] = tar.y;
            res[1] = tar.x;
            return res;
        } catch (FactoryException | TransformException e) {
            e.printStackTrace();
            return null;
        }
    }

    private static PJconsts PJ_init_84() {
        PJconsts PJ = new PJconsts();
        PJ.proj = "tmerc";
        PJ.lat_0 = 31.235434;
        PJ.lon_0 = 121.467163;
        PJ.ellps = "WGS84";
        double a = 6378137.0;
        PJ.a = a;
        double rf = 298.257223563;
        double es = 1.0 / rf * (2.0 - 1.0 / rf);
        PJ.es = es;
        PJ.e = Math.sqrt(es);
        PJ.ra = 1.0 / a;
        PJ.one_es = 1.0 - es;
        PJ.rone_es = 1.0 / (1 - es);
        PJ.geoc = 0;
        PJ.over = 0;
        PJ.lam0 = PJ.lon_0 * vm[0];
        double phi = PJ.lat_0 * vm[0];
        PJ.phi0 = phi;
        PJ.x0 = 0.0;
        PJ.y0 = 0.0;
        PJ.k0 = 1.0;
        PJ.to_meter = 1.0;
        PJ.fr_meter = 1.0;
        PJ.from_greenwich = 0.0;
        PJ.esp = es / (1.0 - es);

        double[] en = new double[5];
        double t;
        en[0] = C00 - es * (C02 + es * (C04 + es * (C06 + es * C08)));
        en[1] = es * (C22 - es * (C04 + es * (C06 + es * C08)));
        en[2] = (t = es * es) * (C44 - es * (C46 + es * C48));
        en[3] = (t *= es) * (C66 - es * C68);
        en[4] = t * es * C88;
        PJ.en = en;
        PJ.ml0 = pj_mlfn(phi, Math.sin(phi), Math.cos(phi), en);
        return PJ;
    }

    private static double adjlon(double lon) {
        if (Math.abs(lon) <= SPI) return (lon);
        lon += ONEPI;  /* adjust to 0..2pi rad */
        lon -= TWOPI * Math.floor(lon / TWOPI); /* remove integral # of 'revolutions'*/
        lon -= ONEPI;  /* adjust back to -pi..pi rad */
        return (lon);
    }

    private static double pj_mlfn(double phi, double sphi, double cphi, double en[]) {
        cphi *= sphi;
        sphi *= sphi;
        return (en[0] * phi - cphi * (en[1] + sphi * (en[2]
                + sphi * (en[3] + sphi * en[4]))));
    }


    private static double pj_inv_mlfn(double arg, double es, double[] en) {
        double s, t, phi, k = 1. / (1. - es);
        int i;
        phi = arg;
        for (i = MAX_ITER; i != 0; --i) { /* rarely goes over 2 iterations */
            s = Math.sin(phi);
            t = 1. - es * s * s;
            phi -= t = (pj_mlfn(phi, s, Math.cos(phi), en) - arg) * (t * Math.sqrt(t)) * k;
            if (Math.abs(t) < EPS) {
                return phi;
            }
        }
        return phi;

    }


    private static double[] pj_inv(double x, double y, PJconsts PJ) {
        x = (x * PJ.to_meter - PJ.x0) * PJ.ra; /* descale and de-offset */
        y = (y * PJ.to_meter - PJ.y0) * PJ.ra;

        /* inverse project */
        double lpphi = pj_inv_mlfn(PJ.ml0 + y / PJ.k0, PJ.es, PJ.en);
        double lplam = 0;
        double n, con, cosphi, d, ds, sinphi, t;

        if (Math.abs(lpphi) >= (Math.PI / 2)) {
            lpphi = y < 0.0 ? -HALFPI : HALFPI;
            lplam = 0;
        } else {
            sinphi = Math.sin(lpphi);
            cosphi = Math.cos(lpphi);
            t = Math.abs(cosphi) > 1e-10 ? sinphi / cosphi : 0.;
            n = PJ.esp * cosphi * cosphi;
            d = x * Math.sqrt(con = 1.0 - PJ.es * sinphi * sinphi) / PJ.k0;
            con *= t;
            t *= t;
            ds = d * d;
            lpphi -= (con * ds / (1. - PJ.es)) * FC2 * (1. -
                    ds * FC4 * (5. + t * (3. - 9. * n) + n * (1. - 4 * n) -
                            ds * FC6 * (61. + t * (90. - 252. * n +
                                    45. * t) + 46. * n
                                    - ds * FC8 * (1385. + t * (3633. + t * (4095. + 1574. * t)))
                            )));
            lplam = d * (FC1 -
                    ds * FC3 * (1. + 2. * t + n -
                            ds * FC5 * (5. + t * (28. + 24. * t + 8. * n) + 6. * n
                                    - ds * FC7 * (61. + t * (662. + t * (1320. + 720. * t)))
                            ))) / cosphi;

        }

        lplam += PJ.lam0; /* reduce from del lp.lam */
        if (PJ.over != 0) {
            lplam = adjlon(lplam); /* adjust longitude to CM */
        }
        if (PJ.geoc == 1 && Math.abs(Math.abs(lpphi) - HALFPI) > EPS) {
            lpphi = Math.atan(PJ.one_es * Math.tan(lpphi));
        }
        return new double[]{lplam, lpphi};
    }

    /**
     * 上海城建 -> GPS84
     * @param x
     * @param y
     * @return
     */
    public static double[] SHXY_To_gps84(double x, double y) {
        double[] res = new double[2];

        PJconsts PJ = PJ_init_84();
        double u = retain10(x);
        double v = retain10(y);

        double[] inv_res = pj_inv(u, v, PJ);
        u = inv_res[0];
        v = inv_res[1];

        //pj_inv
        v *= RAD_TO_DEG;
        u *= RAD_TO_DEG;

        double out1 = u;
        double out2 = v;

        double real_coordx, real_coordy;
        double err_x, err_y;

        err_x = 0.00250135507339599 - out1 * (1.16351950848744e-05) - out2 * (3.42854049203957e-05);
        err_y = -0.00292909033177498 + out1 * (2.53739773884895e-05) - out2 * (5.49535857571321e-06);

        real_coordx = out1 - (err_x);
        real_coordy = out2 - (err_y);

        res[0] = real_coordx;
        res[1] = real_coordy;

        return res;

    }

    private static double retain10(double num) {
        String result = String.format("%.10f", num);
        return Double.valueOf(result);
    }

    /**
     * GPS84 -> 上海城建
     * @param lon
     * @param lat
     * @return
     */
    public static double[] gps84_To_SHXY(double lon, double lat) {
        double[] result = new double[2];
        PJconsts PJ = PJ_init_84();

        double u = lon * vm[0];
        double v = lat * vm[0];

        double x;
        double y;
        double t;
        /* check for forward and latitude or longitude overange */
        if ((t = Math.abs(v) - HALFPI) > EPS || Math.abs(u) > 10.0) {
            result[0] = HUGE_VAL;
            result[1] = HUGE_VAL;
            return result;
        } else { /* proceed with projection */

            if (Math.abs(t) <= EPS)
                v = v < 0.0 ? -HALFPI : HALFPI;
            else if (Math.abs(PJ.geoc) >= EPS)
                v = Math.atan(PJ.rone_es * Math.tan(v));

            u -= PJ.lam0;    /* compute del lp.lam */
            if (Math.abs(PJ.over) <= EPS)
                u = adjlon(u); /* adjust del longitude */

            /* e_forward  project */
            double al, als, n, cosphi, sinphi;

            sinphi = Math.sin(v);
            cosphi = Math.cos(v);
            t = Math.abs(cosphi) > 1e-10 ? sinphi / cosphi : 0.;
            t *= t;
            al = cosphi * u;
            als = al * al;
            al /= Math.sqrt(1.0 - PJ.es * sinphi * sinphi);
            n = PJ.esp * cosphi * cosphi;
            x = PJ.k0 * al * (FC1 +
                    FC3 * als * (1. - t + n +
                            FC5 * als * (5. + t * (t - 18.) + n * (14. - 58. * t)
                                    + FC7 * als * (61. + t * (t * (179. - t) - 479.))
                            )));

            y = (pj_mlfn(v, sinphi, cosphi, PJ.en) - PJ.ml0 +
                    sinphi * al * u * FC2 * (1. +
                            FC4 * als * (5. - t + n * (9. + 4. * n) +
                                    FC6 * als * (61. + t * (t - 58.) + n * (270. - 330 * t)
                                            + FC8 * als * (1385. + t * (t * (543. - t) - 3111.))
                                    ))));
            /* adjust for major axis and easting/northings */

            x = PJ.fr_meter * (PJ.a * x + PJ.x0);
            y = PJ.fr_meter * (PJ.a * y + PJ.y0);

            double out1 = x;
            double out2 = y;
            double err_x = -1.51153279022372 + out2 / 10000.0 * 0.0820229524611545 + out1 / 10000.0 * 0.255890565368556;
            double err_y = 2.0044994152667 - out2 / 10000.0 * 0.254853140425539 + out1 / 10000.0 * 0.0820493970293696;
            double real_coordx = out2 + (err_x);
            double real_coordy = out1 + (err_y);
            out2 = real_coordx;
            out1 = real_coordy;

            //二次线性纠正
            err_x = 0.491057954263098 + out2 / 10000.0 * 0.163251063170758 - out1 / 10000.0 * 0.000408333266113368;
            err_y = 0.497750115591061 + out2 / 10000.0 * 0.000829632345512648 + out1 / 10000.0 * 0.163798831228589;
            real_coordx = out2 - (err_x);
            real_coordy = out1 - (err_y);
            out2 = real_coordx;
            out1 = real_coordy;
            result[0] = out1;
            result[1] = out2;

            return result;

        }
    }


    public static void main(String[] args) throws FactoryException, TransformException {
        double lon = 121.173807372;
        double lat = 31.1229825198;

        double[] res = gps84_To_Gcj02(lon,lat);

        System.out.println(res[0]+","+res[1]);

        double[] res0 = CGCS2000_To_gps84(res[0],res[1]);
        System.out.println(res0[0]+","+res0[1]);

        double x = -27978.4292436;
        double y = -12433.5866811;
        double[] sh = SHXY_To_gps84(x,y);
        System.out.println(sh[0]+","+sh[1]);

    }


}
