package pl.wat.ms4ds.terrain;

import org.slf4j.Logger;
import org.slf4j.LoggerFactory;

public class Coord {

    private static final Logger logger = LoggerFactory.getLogger(Coord.class);

    public static class Geo {
        public double lat;
        public double lon;

        public Geo() {
        }

        public Geo(double lat, double lon) {
            this.lat = lat;
            this.lon = lon;
        }

        public Geo(GeoCoord other) {
            lat = other.lat;
            lon = other.lon;
        }

        @Override
        public String toString() {
            return "Geo{" +
                    "lat=" + lat +
                    ", lon=" + lon +
                    '}';
        }
    }

    /**
     * Współrzędne punktu odwzorowania kartograficznego PUWG 1992.
     * <p>
     * Wartości współrzędnych [metry].
     *
     */
    public static class Puwg {
        /**
         * Współrzędna X (oś odcietych) odwzorowania kartograficznego [metry].
         */
        public double easting;
        /**
         * Współrzędna Y (oś rzędnych) odwzorowania kartograficznego [metry].
         */
        public double northing;

        public Puwg() {
            this.easting = 0;
            this.northing = 0;
        }

        public Puwg(double easting, double northing) {
            this.easting = easting;
            this.northing = northing;
        }

        @Override
        public String toString() {
            return "Puwg{" +
                    "easting=" + easting +
                    ", northing=" + northing +
                    '}';
        }
    }

    public static class Grid {

        /**
         * Współrzędna pozioma (oś OX) w siatce kwadratów. Indeks kolumny.
         */
        public int x;
        /**
         * Współrzędna pionowa (oś OY) w siatce kwadratów. Indeks wiersza.
         */
        public int y;

        /**
         * Konstruktor klasy na bazie współrzędnych geograficznych.
         *
         * @param lon długość geograficzna
         * @param lat szerokość geograficzna
         */
        public Grid(double lon, double lat) {
            double xms_f = (lon + 180) * MapConsts.DEG_MS;
            long xms = (long) xms_f;
            x = zamienWspXmsNaIdKwadratuX(xms);

            double yms_f = (lat + 90) * MapConsts.DEG_MS;
            long yms = (long) yms_f;
            y = zamienWspYmsNaIdKwadratuY(yms);
        }

        @Override
        public String toString() {
            return "Grid{" +
                    "x=" + x +
                    ", y=" + y +
                    '}';
        }

        private static final double ODWROT_SS_DX_MS = 1.0 / MapConsts.SS_DX_MS;
        private static final double ODWROT_SS_DY_MS = 1.0 / MapConsts.SS_DY_MS;

        public static double distance(Grid a, Grid b) {
            int dx = a.x - b.x;
            int dy = a.y - b.y;
            return Math.sqrt(dx * dx + dy * dy) * MapConsts.DL_MK;
        }

        public static double distance(int x1, int y1, int x2, int y2) {
            int dx = x2 - x1;
            int dy = y2 - y1;
            return Math.sqrt(dx * dx + dy * dy) * MapConsts.DL_MK;
        }

        private static final float DL_MK2 = MapConsts.DL_MK * 0.0009765625f;

        /**
         * Funkcja zwraca aproksymowaną odległość między środkami małych kwadratów [m].
         *
         * @param x1 Wsp X id malego kwadratu pocz.
         * @param y1 Wsp Y id malego kwadratu pocz.
         * @param x2 Wsp X id malego kwadratu konc.
         * @param y2 Wsp Y id malego kwadratu konc.
         * @return Odległość miedzy srodkami malych kwadratow [m].
         */
        public static float distanceApprox(int x1, int y1, int x2, int y2) {
            int dx = x2 - x1;
            int dy = y2 - y1;
            int min, max, approx;
            if (dx < 0) dx = -dx;
            if (dy < 0) dy = -dy;
            if (dx < dy) {
                min = dx;
                max = dy;
            } else {
                min = dy;
                max = dx;
            }
            approx = (max * 1007) + (min * 441);
            if (max < (min << 4)) {
                approx -= (max * 40);
            }
            float odl = approx * DL_MK2;
            return odl;
        }

        public Grid() {
            x = -1;
            y = -1;
        }

        public Grid(int x, int y) {
            this.x = x;
            this.y = y;
        }

        public Grid(Grid orig) {
            this.x = orig.x;
            this.y = orig.y;
        }

        public int getX() {
            return x;
        }

        public void setX(int x) {
            this.x = x;
        }

        public int getY() {
            return y;
        }

        public void setY(int y) {
            this.y = y;
        }

        public void set(Grid orig) {
            x = orig.x;
            y = orig.y;
        }

        public void set(int x, int y) {
            this.x = x;
            this.y = y;
        }

        @Override
        public final boolean equals(Object o) {
            if (!(o instanceof Grid grid)) return false;

            return x == grid.x && y == grid.y;
        }

        @Override
        public final int hashCode() {
            int result = x;
            result = 31 * result + y;
            return result;
        }

    }

    /**
     * Zamienia współrzęną GridCoord.x na długość geograficzną środka małego kwadratu
     *
     * @param idX współrzęna x GridCoord
     * @return długość geograficzna
     */
    public static float zamienIdKwadratuXNaDlugoscGeo(int idX) {
        long xms = zamienIdKwadratuXNaWspXms(idX);
        double lon = (double) xms / (double) MapConsts.DEG_MS - 180;
        return (float) lon;
    }

    /**
     * Zamienia współrzęną GridCoord.y na szerokość geograficzną środka małego kwadratu
     *
     * @param idY współrzęna y GridCoord
     * @return szerokość geograficzna
     */
    public static float zamienIdKwadratuYNaSzerokoscGeo(int idY) {
        long yms = zamienIdKwadratuYNaWspYms(idY);
        double lat = (double) yms / (double) MapConsts.DEG_MS - 90;
        return (float) lat;
    }

    /**
     * Funkcja zamienia wsp. X GridCoord na wsp. geo xms w milisekundach.
     * Zwracana wspolrzedna lezy w srodku kwadratu.
     */
    public static long zamienIdKwadratuXNaWspXms(int idKwX) {
        // indeksowanie kwadratow pola walki zaczyna sie od (0, 0)
        // przesuniecie wspolrzednych do srodka kwadratu
        long xms = MapConsts.X_REF_MS + (long) ((idKwX + 0.5) * MapConsts.SS_DX_MS);
        xms %= MapConsts.ANGLE_360_MS;
        return xms;
    }

    /**
     * Funkcja zamienia wsp. Y GridCoord na wsp. geo yms w milisekundach.
     * Zwracana wspolrzedna lezy w srodku kwadratu.
     */
    public static long zamienIdKwadratuYNaWspYms(int idKwY) {
        // indeksowanie kwadratow pola walki zaczyna sie od (0, 0)
        // przesuniecie wspolrzednych do srodka kwadratu
        long yms = MapConsts.Y_REF_MS + (long) ((idKwY + 0.5) * MapConsts.SS_DY_MS);
        return yms;
    }

    /**
     * Zamienia długość geograficzną na współrzędna x GridCoord.
     *
     * @param lon długość geograficzna
     * @return współrzędna x klasy GridCoord
     */
    public static int zamienDlugoscGeoNaIdKwadratuX(double lon) {
        double xms_f = (lon + 180) * MapConsts.DEG_MS;
        return zamienWspXmsNaIdKwadratuX((long) xms_f);
    }

    /**
     * Zamienia szerokość geograficzną na współrzędna y GridCoord.
     *
     * @param lat szerokość geograficzna
     * @return współrzędna y klasy GridCoord
     */
    public static int zamienSzerokoscGeoNaIdKwadratuY(double lat) {
        double yms_f = (lat + 90) * MapConsts.DEG_MS;
        return zamienWspYmsNaIdKwadratuY((long) yms_f);
    }

    /**
     * Funkcja zamienia dlugosc geog. xms w milisekundach na IdKwadrat.x.
     *
     * @param xms długość geograficzna (lon) w milisekundach
     * @return współrzędna GridCoord.x
     */
    public static int zamienWspXmsNaIdKwadratuX(long xms) {
        // wspolrzedne geograficzne w milisekundach zawieraja sie w zakresie:
        // 0 <= x < 360 dlugosc geograficzna
        // 0 <= y <= 180 szerokosc geograficzna
        if ((xms < 0) || (xms >= 360 * MapConsts.DEG_MS)) {
            // poza zakresem
            return -1;
        }
        long x = xms;
        if (x < MapConsts.X_REF_MS) {
//			// poza zakresem
            long dx = x + MapConsts.ANGLE_360_MS - MapConsts.X_REF_MS;
            if (dx > MapConsts.DX_REF_MS) {
                return -1;
            }
            x += MapConsts.ANGLE_360_MS;
        }
//		if (x > MapConsts.X_REF_MS + MapConsts.DX_REF_MS) {
//			// poza zakresem
//			return -1;
//		}
        // indeksowanie kwadratow pola walki zaczyna sie od (0, 0)
        double xx = (x - MapConsts.X_REF_MS) * ODWROT_SS_DX_MS;
//		x = (x - MapConsts.X_REF_MS) / MapConsts.SS_DX_MS;
        return (int) xx;
    }

    /**
     * Funkcja zamienia szerokosc geog. yms w milisekundach na IdKwadrat.y.
     *
     * @param yms szerokosc geograficzna (lat) w milisekundach
     * @return współrzędna GridCoord.y
     */
    public static int zamienWspYmsNaIdKwadratuY(long yms) {
        // wspolrzedne geograficzne w milisekundach zawieraja sie w zakresie:
        // 0 <= x < 360 dlugosc geograficzna
        // 0 <= y <= 180 szerokosc geograficzna
        if (yms < MapConsts.Y_REF_MS) {
            // poza zakresem
            return -1;
        }
        // indeksowanie kwadratow pola walki zaczyna sie od (0, 0)
        double yy = (yms - MapConsts.Y_REF_MS) * ODWROT_SS_DY_MS;
//		long y = (yms - MapConsts.Y_REF_MS) / MapConsts.SS_DY_MS;
        return (int) yy;
    }

    private static final double ODWROT_SS_DX_MS = 1.0 / MapConsts.SS_DX_MS;
    private static final double ODWROT_SS_DY_MS = 1.0 / MapConsts.SS_DY_MS;

    /**
     * Funkcja służy do konwersji współrzednych elipsoidalnych WGS84 (lat/lon) na płaskie X-northing, Y-easting
     * odwzorowania kartograficznego 1992.
     *
     * @param puwgCoord współrzędne PUWG1992 (easting, northing) [metry]
     * @param lat       szerokość geograficzna WSG-84 [stopnie dziesiętnie]
     * @param lon       długość geograficzna WSG-84 [stopnie dziesiętnie]
     */
    public static void convertWGS84ToPUWG1992(double lat, double lon, Coord.Puwg puwgCoord) {
        if (lon < 13.5 || lon > 25.5) {
            //Błędna wartość długości geograficznej (zwracana wartość 999999999999999)
            puwgCoord.easting = 999999999999999.0;
            puwgCoord.northing = 999999999999999.0;
            return;
        }
        double latRad = lat * DEG_2_RAD;
        double dlam = (lon - 19.0) * DEG_2_RAD;
        double dlam_pow_2 = dlam * dlam;
        double dlam_pow_3 = dlam_pow_2 * dlam;
        double dlam_pow_4 = dlam_pow_3 * dlam;
        double s = Math.sin(latRad);
        double c = Math.cos(latRad);
        double c_pow_2 = c * c;
        double c_pow_3 = c_pow_2 * c;
        double c_pow_4 = c_pow_3 * c;
        double t = s / c;
        double t_pow_2 = t * t;
        double t_pow_3 = t_pow_2 * t;
        double t_pow_4 = t_pow_3 * t;
        double t_pow_5 = t_pow_4 * t;
        double eta = E2_SQUARED * c_pow_2;
        double eta_pow_2 = eta * eta;
        double eta_pow_3 = eta_pow_2 * eta;
        double eta_pow_4 = eta_pow_3 * eta;
        double sn = sphsn(latRad);
        double tmd = sphtmd(latRad);
        double t1, t2, t3, t4, t5;
        t1 = tmd * OK;
        double sns = sn * s;
        t2 = sns * c * OK / 2.0;
        t3 = sns * c_pow_3 * OK * (5.0 - t_pow_2 + 9.0 * eta + 4.0 * eta_pow_2) / 24.0;
        t4 = sns * c_pow_4 * c * OK * (61.0 - 58.0 * t_pow_2 + t_pow_4
                + 270.0 * eta - 330.0 * t_pow_2 * eta + 445.0 * eta_pow_2
                + 324.0 * eta_pow_3 - 680.0 * t_pow_2 * eta_pow_2
                + 88.0 * eta_pow_4 - 600.0 * t_pow_2 * eta_pow_3 - 192.0 * t_pow_2 * eta_pow_4) / 720.0;
        t5 = sns * c_pow_4 * c_pow_3 * OK * (1385.0 - 3111.0 * t_pow_2
                + 543.0 * t_pow_4 - t_pow_5 * t) / 40320.0;
        puwgCoord.northing = -5300000.0 + t1 + dlam_pow_2 * t2 + dlam_pow_4 * t3
                + dlam_pow_4 * dlam_pow_2 * t4 + dlam_pow_4 * dlam_pow_4 * t5;
        t1 = sn * c * OK;
        t2 = sn * c_pow_3 * OK * (1.0 - t_pow_2 + eta) / 6.0;
        t3 = sn * c_pow_4 * c * OK * (5.0 - 18.0 * t_pow_2 + t_pow_4 + 14.0 * eta
                - 58.0 * t_pow_2 * eta + 13.0 * eta_pow_2 + 4.0 * eta_pow_3
                - 64.0 * t_pow_2 * eta_pow_2 - 24.0 * t_pow_2 * eta_pow_3) / 120.0;
        t4 = sn * c_pow_4 * c_pow_3 * OK * (61.0 - 479.0 * t_pow_2 + 179.0 * t_pow_4 - t_pow_5 * t) / 5040.0;
        puwgCoord.easting = 500000.0 + dlam * t1 + dlam_pow_3 * t2
                + dlam_pow_4 * dlam * t3 + dlam_pow_4 * dlam_pow_3 * t4;// + 0.5;
    }

    /**
     * Funkcja do konwersji współrzędnych płaskich X/Y odwzorowania kartograficznego 1992 na elipsoidalne lat/lon elipsoide WGS84.
     * <p>
     * PUWGCoord.proj: odwzorowanie kartograficzne (proj = 1 odpowiada odwzorowaniu 1992, natomiast każda inna odwzorowaniu 2000)
     *
     * @param northing współrzędne na osi OY odwzorowania kartograficznego PUWG-1992 do konwersji [metry]
     * @param easting  współrzędne na osi OX odwzorowania kartograficznego PUWG-1992 do konwersji [metry]
     * @param geoCoord współrzędne geograficzne odwzorowania WGS-84 po konwersji [stopnie]
     */
    public static void convertPUWG1992ToWGS84(double northing, double easting, Coord.Geo geoCoord) {
        double tmd = (northing + 5300000.0) / OK;
        double sr = sphsr(0.0);
        double ftphi = tmd / sr;
        for (int i = 0; i < 5; i++) {
            ftphi += (tmd - sphtmd(ftphi)) / sphsr(ftphi);
        }
        sr = sphsr(ftphi);
        double sn = sphsn(ftphi);
        double sn_pow_2 = sn * sn;
        double sn_pow_3 = sn_pow_2 * sn;
        double sn_pow_4 = sn_pow_3 * sn;
        double sn_pow_5 = sn_pow_4 * sn;
        double sn_pow_7 = sn_pow_5 * sn_pow_2;
        double s = Math.sin(ftphi);
        double c = Math.cos(ftphi);
        double t = s / c;
        double t_pow_2 = t * t;
        double t_pow_4 = t_pow_2 * t_pow_2;
        double t_pow_6 = t_pow_4 * t_pow_2;
        double eta = E2_SQUARED * (c * c);
        double eta_pow_2 = eta * eta;
        double eta_pow_3 = eta_pow_2 * eta;
        double eta_pow_4 = eta_pow_2 * eta_pow_2;
        double de = easting - 500000.0;
        double de_pow_2 = de * de;
        double de_pow_3 = de_pow_2 * de;
        double de_pow_4 = de_pow_3 * de;
        double t0, t1, t2, t3;
        t0 = t / (2.0 * sr * sn * OK_POW_2);
        t1 = t * (5.0 + 3.0 * t_pow_2 + eta - 4.0 * eta_pow_2 - 9.0 * t_pow_2 * eta) / (24.0 * sr * sn_pow_3 * OK_POW_4);
        t2 = t * (61.0 + 90.0 * t_pow_2 + 46.0 * eta + 45.0 * t_pow_4 - 252.0 * t_pow_2 * eta - 3.0 * eta_pow_2
                + 100.0 * eta_pow_3 - 66.0 * t_pow_2 * eta_pow_2 - 90.0 * t_pow_4 * eta + 88.0 * eta_pow_4
                + 225.0 * t_pow_4 * eta_pow_2 + 84.0 * t_pow_2 * eta_pow_3 - 192.0 * t_pow_2 * eta_pow_4) / (720.0 * sr * sn_pow_5 * OK_POW_6);
        t3 = t * (1385.0 + 3633 * t_pow_2 + 4095.0 * t_pow_4 + 1575.0 * t_pow_6) / (40320 * sr * sn_pow_7 * (OK_POW_8));
        geoCoord.lat = ftphi - de_pow_2 * t0 + de_pow_4 * t1 - de_pow_3 * de_pow_3 * t2 + de_pow_4 * de_pow_3 * t3;
        t0 = 1.0 / (sn * c * OK);
        t1 = (1.0 + 2.0 * t_pow_2 + eta) / (6.0 * sn_pow_3 * c * (OK_POW_3));
        t2 = (5.0 + 6.0 * eta + 28.0 * t_pow_2 - 3.0 * eta_pow_2 + 8.0 * t_pow_2 * eta
                + 24.0 * t_pow_4 - 4.0 * eta_pow_3 + 4.0 * t_pow_2 * eta_pow_2
                + 24.0 * t_pow_2 * eta_pow_3) / (120.0 * sn_pow_5 * c * (OK_POW_5));
        t3 = (61.0 + 662.0 * t_pow_2 + 1320.0 * t_pow_4 + 720.0 * t_pow_6) / (5040.0 * sn_pow_7 * c * OK_POW_7);
        double dlam = de * t0 - de_pow_3 * t1 + de_pow_3 * de_pow_2 * t2 - de_pow_3 * de_pow_4 * t3;
        // 19.0 * DEG_2_RAD == 0.33161255787892263;
//        geoCoord.lon = 0.33161255787892263 + dlam;
//        geoCoord.lon *= RAD_2_DEG;
        geoCoord.lon = dlam * RAD_2_DEG;
        geoCoord.lon += 19.0;
        geoCoord.lat *= RAD_2_DEG;
    }

    ////////////////////////////////////////////////////////////////////////////////
    // Funkcje pomocnicze i stałe

    /// /////////////////////////////////////////////////////////////////////////////

    static double calculateESquared(double a, double b) {
        a *= a;
        b *= b;
        return (a - b) / a;
    }

    static double calculateE2Squared(double a, double b) {
        a *= a;
        b *= b;
        return (a - b) / b;
    }

    static double denom(double sphi) {
        double sinSphi = Math.sin(sphi);
        return Math.sqrt(1.0 - E_SQUARED * (sinSphi * sinSphi));
    }

    static double sphsr(double sphi) {
        double dn = denom(sphi);
        return A * (1.0 - E_SQUARED) / (dn * dn * dn);
    }

    static double sphsn(double sphi) {
        double sinSphi = Math.sin(sphi);
        return A / Math.sqrt(1.0 - E_SQUARED * (sinSphi * sinSphi));
    }

    static double sphtmd(double sphi) {
        return (AP * sphi) - (BP * Math.sin(2.0 * sphi)) + (CP * Math.sin(4.0 * sphi))
                - (DP * Math.sin(6.0 * sphi)) + (EP * Math.sin(8.0 * sphi));
    }

    private static final double DEG_2_RAD = Math.PI / 180.0;
    private static final double RAD_2_DEG = 180.0 / Math.PI;

    /**
     * Dlługość dużej półsi, w metrach dla elipsoidy WGS-84.
     */
    private static final double A = 6378137.0;

    /**
     * double f: spłaszczenie elipsoidalne dla elipsoidy WGS-84, 1 / 298.257223563
     */
//    private static final double F = 1.0 / 298.257223563;
    private static final double B = A * (1.0 - 1.0 / 298.257223563);
    private static final double E_SQUARED = calculateESquared(A, B);
    private static final double E2_SQUARED = calculateE2Squared(A, B);
    private static final double TN = (A - B) / (A + B);
    private static final double AP = A * (1.0 - TN + 5.0 * (TN * TN - TN * TN * TN) / 4.0 + 81.0 * TN * TN * TN * TN - TN * TN * TN * TN * TN / 64.0);
    private static final double BP = 3.0 * A * (TN - TN * TN + 7.0 * (TN * TN * TN - TN * TN * TN * TN) / 8.0 + 55.0 * TN * TN * TN * TN * TN / 64.0) / 2.0;
    private static final double CP = 15.0 * A * (TN * TN - TN * TN * TN + 3.0 * (TN * TN * TN * TN - TN * TN * TN * TN * TN) / 4.0) / 16.0;
    private static final double DP = 35.0 * A * (TN * TN * TN - TN * TN * TN * TN + 11.0 * TN * TN * TN * TN * TN / 16.0) / 48.0;
    private static final double EP = 315.0 * A * (TN * TN * TN * TN - TN * TN * TN * TN * TN) / 512.0;

    /**
     * Współczynnik zniekształcenia skali mapy w południku osiowym dla odwzorowania kartograficznego PUWG-1992.
     */
    private static final double OK = 0.9993;
    private static final double OK_POW_2 = OK * OK;
    private static final double OK_POW_3 = OK_POW_2 * OK;
    private static final double OK_POW_4 = OK_POW_3 * OK;
    private static final double OK_POW_5 = OK_POW_4 * OK;
    private static final double OK_POW_6 = OK_POW_5 * OK;
    private static final double OK_POW_7 = OK_POW_6 * OK;
    private static final double OK_POW_8 = OK_POW_7 * OK;


}