package pl.wat.ms4ds.terrain;

import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import pl.wat.ms4ds.common.EGeoDirection;

public class GeomUtils {

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

    /**
     * <p> Wyznacznik macierzy (kwadratowej stopnia 3)
     * [[p.x, p.y, 1],
     * [q.x, q.y, 1]
     * [r.x, r.y, 1]]
     * </p>
     * Pozwala na określenie położenie punktu względem wektora.
     *
     * @param p Wsp poczatku wektora
     * @param q Wsp konca wektora
     * @param r Wsp testowanego punktu
     * @return det3 == 0, gdy punkt jest wspolliniowy, det3 wieksze od 0, gdy punkt lezy po lewej stronie wektora,
     * det3 mniejsze od 0, gdy punkt lezy po prawej stronie wektora,
     */
    public static long det3(Coord.Grid p, Coord.Grid q, Coord.Grid r) {
        return p.x * (q.y - r.y) + q.x * (r.y - p.y) + r.x * (p.y - q.y);
    }

    /**
     * <p> Wyznacznik macierzy (kwadratowej stopnia 3)
     * [[p.x, p.y, 1],
     * [q.x, q.y, 1]
     * [r.x, r.y, 1]]
     * </p>
     * Pozwala na określenie położenie punktu względem wektora.
     *
     * @param px Wsp x poczatku wektora
     * @param py Wsp y poczatku wektora
     * @param qx Wsp x końca wektora
     * @param qy Wsp y końca wektora
     * @param rx Wsp x testowanego punktu
     * @param ry Wsp y testowanego punktu
     * @return det3 == 0, gdy punkt jest wspolliniowy, det3 wieksze od 0, gdy punkt lezy po lewej stronie wektora,
     * det3 mniejsze od 0, gdy punkt lezy po prawej stronie wektora,
     */
    public static long det3(int px, int py, int qx, int qy, int rx, int ry) {
        return px * (qy - ry) + qx * (ry - py) + rx * (py - qy);
    }

    /**
     * Sprawdza, czy punkt r należy do odcinka p-q
     *
     * @param p Poczatek odcinka
     * @param q Koniec odcinka
     * @param r Punkt testowany
     * @return true, jesli punkt r lezy na odcinku
     */
    public static boolean include(Coord.Grid p, Coord.Grid q, Coord.Grid r) {
        return det3(p, q, r) == 0 &&
                Math.min(p.x, q.x) <= r.x &&
                r.x <= Math.max(p.x, q.x) &&
                Math.min(p.y, q.y) <= r.y &&
                r.y <= Math.max(p.y, q.y);
    }

    /**
     * Sprawdza, czy punkt r należy do odcinka p-q
     *
     * @param px Wsp x poczatku wektora
     * @param py Wsp y poczatku wektora
     * @param qx Wsp x końca wektora
     * @param qy Wsp y końca wektora
     * @param rx Wsp x testowanego punktu
     * @param ry Wsp y testowanego punktu
     * @return true, jesli punkt r lezy na odcinku
     */
    public static boolean include(int px, int py, int qx, int qy, int rx, int ry) {
        return Math.min(px, qx) <= rx &&
                rx <= Math.max(px, qx) &&
                Math.min(py, qy) <= ry &&
                ry <= Math.max(py, qy) &&
                det3(px, py, qx, qy, rx, ry) == 0;
    }

    /**
     * Sprawdza, czy punkt r należy do poziomego odcinka p-q
     *
     * @param p Poczatek odcinka
     * @param q Koniec odcinka
     * @param r Punkt testowany
     * @return true, jeśli punkt r należy do odcinka p-q
     */
    public static boolean includeHorizontaly(Coord.Grid p, Coord.Grid q, Coord.Grid r) {
        return p.y == r.y &&
                Math.min(p.x, q.x) <= r.x &&
                r.x <= Math.max(p.x, q.x);

//        return det3(p, q, r) == 0 &&
//                Math.min(p.x, q.x) <= r.x &&
//                r.x <= Math.max(p.x, q.x) &&
//                Math.min(p.y, q.y) <= r.y &&
//                r.y <= Math.max(p.y, q.y);
    }

    /**
     * Sprawdza, czy punkt r należy do poziomego odcinka p-q
     *
     * @param px Wsp x poczatku wektora
     * @param py Wsp y poczatku wektora
     * @param qx Wsp x końca wektora
     * @param qy Wsp y końca wektora
     * @param rx Wsp x testowanego punktu
     * @param ry Wsp y testowanego punktu
     * @return true, jeśli punkt r należy do odcinka p-q
     */
    public static boolean includeHorizontaly(int px, int py, int qx, int qy, int rx, int ry) {
        return py == ry &&
                Math.min(px, qx) <= rx &&
                rx <= Math.max(px, qx);
    }

    /**
     * Określenie znaku liczby
     *
     * @param x Testowana liczba
     * @return 0, gdy x == 0, 1 gdy x wieksze od 0, -1 gdy x mniejsze od 0
     */
    public static int sgn(long x) {
        if (x == 0) {
            return 0;
        }
        if (x > 0) {
            return 1;
        }
        return -1;
    }

    /**
     * Sprawdza, czy odcinki p-q i r-s przecinają się wewnetrznie tzn.
     * mają dokładnie jeden punkt wspólny różny od każdego z konców odcinków.
     *
     * @param p punkt początkowy pierwszego odcinka
     * @param q punkt końcowy pierwszego odcinka
     * @param r punkt początkowy drugiego odcinka
     * @param s punkt końcowy drugiego odcinka
     * @return true, jeśli odcinki przecinają się wewnętrznie
     */
    public static boolean cross(Coord.Grid p, Coord.Grid q, Coord.Grid r, Coord.Grid s) {
        long det3_pqr = det3(p, q, r);
        long det3_pqs = det3(p, q, s);
        long det3_rsp = det3(r, s, p);
        long det3_rsq = det3(r, s, q);

        return sgn(det3_pqr) != sgn(det3_pqs) &&
                sgn(det3_rsp) != sgn(det3_rsq) &&
                det3_pqr != 0 &&
                det3_pqs != 0 &&
                det3_rsp != 0 &&
                det3_rsq != 0;
//        return sgn(det3(p, q, r)) != sgn(det3(p, q, s)) &&
//                sgn(det3(r, s, p)) != sgn(det3(r, s, q)) &&
//                !include(p, q, r) &&
//                !include(p, q, s) &&
//                !include(r, s, p) &&
//                !include(r, s, q);
    }

    /**
     * Sprawdza, czy odcinki p-q i r-s przecinają się wewnetrznie tzn.
     * mają dokładnie jeden punkt wspólny różny od każdego z konców odcinków.
     *
     * @param px wsp x punktu początkowego pierwszego odcinka
     * @param py wsp y punktu początkowego pierwszego odcinka
     * @param qx wsp x punktu końcowego pierwszego odcinka
     * @param qy wsp y punktu końcowego pierwszego odcinka
     * @param rx wsp x punktu początkowego drugiego odcinka
     * @param ry wsp y punktu początkowego drugiego  odcinka
     * @param sx wsp x punktu końcowego drugiego  odcinka
     * @param sy wsp y punktu końcowego drugiego  odcinka
     * @return true, jeśli odcinki przecinają się wewnętrznie
     */
    public static boolean cross(int px, int py, int qx, int qy, int rx, int ry, int sx, int sy) {
        long det3_pqr = det3(px, py, qx, qy, rx, ry);
        long det3_pqs = det3(px, py, qx, qy, sx, sy);
        long det3_rsp = det3(rx, ry, sx, sy, px, py);
        long det3_rsq = det3(rx, ry, sx, sy, qx, qy);

        return sgn(det3_pqr) != sgn(det3_pqs) &&
                sgn(det3_rsp) != sgn(det3_rsq) &&
                det3_pqr != 0 &&
                det3_pqs != 0 &&
                det3_rsp != 0 &&
                det3_rsq != 0;
//        return sgn(det3(p, q, r)) != sgn(det3(p, q, s)) &&
//                sgn(det3(r, s, p)) != sgn(det3(r, s, q)) &&
//                !include(p, q, r) &&
//                !include(p, q, s) &&
//                !include(r, s, p) &&
//                !include(r, s, q);
    }

    /**
     * Sprawdza, czy odcinki p-q i r-s mają dowolną część wspólną.
     *
     * @param p punkt początkowy pierwszego odcinka
     * @param q punkt końcowy pierwszego odcinka
     * @param r punkt początkowy drugiego odcinka
     * @param s punkt końcowy drugiego odcinka
     * @return true, jeśli odcinki mają część wspólną
     */
    public static boolean intersection(Coord.Grid p, Coord.Grid q, Coord.Grid r, Coord.Grid s) {
        long det3_pqr = det3(p, q, r);
        long det3_pqs = det3(p, q, s);
        long det3_rsp = det3(r, s, p);
        long det3_rsq = det3(r, s, q);
        return (sgn(det3_pqr) != sgn(det3_pqs) && sgn(det3_rsp) != sgn(det3_rsq)) ||
                (det3_pqr == 0 && det3_pqs == 0 &&
                        ((Math.min(p.x, q.x) <= r.x &&
                                r.x <= Math.max(p.x, q.x) &&
                                Math.min(p.y, q.y) <= r.y &&
                                r.y <= Math.max(p.y, q.y)) ||
                                (Math.min(p.x, q.x) <= s.x &&
                                        s.x <= Math.max(p.x, q.x) &&
                                        Math.min(p.y, q.y) <= s.y &&
                                        s.y <= Math.max(p.y, q.y))));
    }

    /**
     * Wyznacza ograniczenia dolne i górne współrzędnych: xMin, yMin, xMax, yMax.
     *
     * @param vertices
     * @return vertices bounds: 0 - xMin, 1 - yMin, 2 - xMax, 3 - yMax
     */
    public static int[] getBounds(Coord.Grid[] vertices) {
        int xMin = Integer.MAX_VALUE;
        int yMin = Integer.MAX_VALUE;
        int xMax = Integer.MIN_VALUE;
        int yMax = Integer.MIN_VALUE;
        for (Coord.Grid v : vertices) {
            xMin = Math.min(xMin, v.x);
            yMin = Math.min(yMin, v.y);
            xMax = Math.max(xMax, v.x);
            yMax = Math.max(yMax, v.y);
        }
        return new int[]{xMin, yMin, xMax, yMax};
    }

    /**
     * Funkcja bada zawieranie się punktu w ramach wielokąta.
     *
     * @param polygon
     * @param x
     * @param y
     * @return
     */
    public static boolean insidePolygon(Coord.Grid[] polygon, int x, int y) {
        // Sprawdzenie należenia punktu (x, y) do dowolnej krawędzi wielokąta.
        //
        for (int i = 0; i < polygon.length; i++) {
            int x1 = polygon[i].x;
            int y1 = polygon[i].y;
            int ii = (i + 1) % polygon.length;
            int x2 = polygon[ii].x;
            int y2 = polygon[ii].y;
            if (include(x1, y1, x2, y2, x, y)) {
                return true;
            }
        }
        // Sprawdzenie należenia punktu (x, y) do wnętrza wielokąta. Zliczanie liczby przecięć promienia idącego od
        // punktu w prawo równolegle do osi OX.
        // Nieparzysta liczba oznacza należenie.
        //
        int count = 0;
        for (int i = 0; i < polygon.length; i++) {
            int x1 = polygon[i].x;
            int y1 = polygon[i].y;
            int ii = (i + 1) % polygon.length;
            int x2 = polygon[ii].x;
            int y2 = polygon[ii].y;
            if (y < y1 != y < y2) {
                int dy = y2 - y1;
                int a = (x - x1) * dy;
                int b = (y - y1) * (x2 - x1);
                if (dy >= 0) {
                    if (a < b) {
                        count++;
                    }
                } else {
                    if (b < a) {
                        count++;
                    }
                }
            }
        }
        return (count % 2) == 1;
    }

    /**
     * Funkcja bada należenie punktu do prostokąta zorientowanego równolegle do osi współrzędnych.
     *
     * @param xMin  minimalna współrzędna X prostokąta (lewego dolnego rogu)
     * @param yMin  minimalna współrzędna Y prostokąta (lewego dolnego rogu)
     * @param xMax  maksymalna współrzędna X prostokąta (prawego górnego rogu)
     * @param yMax  maksymalna współrzędna Y prostokąta (prawego górnego rogu)
     * @param xTest współrzędna X punktu testowanego
     * @param yTest współrzędna Y punktu testowanego
     * @return true, jeśli testowany punkt należy do prostokąta
     */
    public static boolean insideRectangle(int xMin, int yMin, int xMax, int yMax, int xTest, int yTest) {
        if (xTest < xMin || xTest > xMax) {
            return false;
        }
        return yTest >= yMin && yTest <= yMax;
    }

    // tangens kąta alfa = 22.5 stopnia
    static final double TANG_22_5_DEG = Math.tan(Math.PI / 8);
    // tangens kąta alfa = 22.5 stopnia
    static final double TANG_67_5_DEG = Math.tan(3 * Math.PI / 8);

    /**
     * Wyznacza najbliższy kierunek geograficzny dla zadanego wektora.
     *
     * @param start kwadrat początkowy wektora
     * @param stop  kwadrat końcowy wektora
     * @return kierunek geograficzny klasy EGeoDirection
     */
    public static EGeoDirection estimateDirection(Coord.Grid start, Coord.Grid stop) {
        if (start.x == stop.x && start.y == stop.y) {
            return EGeoDirection.UNDEFINED;
        }
        // polnoc-poludnie
        if (start.x == stop.x) {
            if (start.y < stop.y) {
                return EGeoDirection.NORTH;    //na polnoc OK
            } else {
                // Start.Y >= Stop.Y
                return EGeoDirection.SOUTH;   //na poludnie OK
            }
        }
        // wschod-zachod
        if (start.y == stop.y) {
            if (start.x < stop.x) {
                return EGeoDirection.EAST; //na wschod OK
            } else {
                // Start.X >= Stop.X
                return EGeoDirection.WEST; //na zachod OK
            }
        }
        // pozostale kierunki
        double dx = stop.x - start.x;
        double dy = stop.y - start.y;
        if (dx > 0) {
            double tgAlfa = dy / dx;
            if (tgAlfa >= TANG_67_5_DEG) {
                return EGeoDirection.NORTH;
            } else if (tgAlfa >= TANG_22_5_DEG) {
                return EGeoDirection.NORTHEAST;
            } else if (tgAlfa >= -TANG_22_5_DEG) {
                return EGeoDirection.EAST;
            } else if (tgAlfa >= -TANG_67_5_DEG) {
                return EGeoDirection.SOUTHEAST;
            } else {
                return EGeoDirection.SOUTH;
            }
        } else {
            double tgAlfa = dy / -dx;
            if (tgAlfa >= TANG_67_5_DEG) {
                return EGeoDirection.NORTH;
            } else if (tgAlfa >= TANG_22_5_DEG) {
                return EGeoDirection.NORTHWEST;
            } else if (tgAlfa >= -TANG_22_5_DEG) {
                return EGeoDirection.WEST;
            } else if (tgAlfa >= -TANG_67_5_DEG) {
                return EGeoDirection.SOUTHWEST;
            } else {
                return EGeoDirection.SOUTH;
            }
        }
    }

    private static final EGeoDirection[][] NEIGHBOR_DIRECTIONS = new EGeoDirection[][]{
            {EGeoDirection.SOUTHWEST, EGeoDirection.WEST, EGeoDirection.NORTHWEST},
            {EGeoDirection.SOUTH, EGeoDirection.UNDEFINED, EGeoDirection.NORTH},
            {EGeoDirection.SOUTHEAST, EGeoDirection.EAST, EGeoDirection.NORTHEAST},
    };

    public static EGeoDirection neighborDirection(int xStart, int yStart, int xEnd, int yEnd) {
        // Przesuwam różnice współrzędnych o jeden tzn. {-1, 0, 1} -> {0, 1, 2}, aby użyć do indeksowania.
        //
        int dx = xEnd - xStart + 1;
        int dy = yEnd - yStart + 1;
        return NEIGHBOR_DIRECTIONS[dx][dy];
    }

    /**
     * Funkcja zwraca kierunek geograficzny dla wektora opisanego na sasiednich kwadratach.
     *
     * @param start współrzędne (X, Y) siatki kwadratu początkowego
     * @param stop  współrzędne (X, Y) siatki kwadratu końcowego
     * @return kierunek geograficzny klasy EGeoDirection dla wektora zadanego przez kwadrat początkowy
     * i końcowy lub UNDEFINED dla kwadratów niesąsiednich
     */
    public static EGeoDirection kierunekDlaSasiada(Coord.Grid start, Coord.Grid stop) {
        int d = Math.abs(start.x - stop.x) + Math.abs(start.y - stop.y);
        if (d == 1 || d == 2) {
            if (start.x == stop.x) {
                if (start.y < stop.y) {
                    return EGeoDirection.NORTH;
                }
                return EGeoDirection.SOUTH;
            }
            if (start.y == stop.y) {
                if (start.x < stop.x) {
                    return EGeoDirection.EAST;
                }
                return EGeoDirection.WEST;
            }
            if (start.x < stop.x) {
                if (start.y < stop.y) {
                    return EGeoDirection.NORTHEAST;
                }
                return EGeoDirection.SOUTHEAST;
            }
            if (start.y < stop.y) {
                return EGeoDirection.NORTHWEST;
            }
            return EGeoDirection.SOUTHWEST;
        } else {
            return EGeoDirection.UNDEFINED;
        }
    }

    /**
     * Funkcja zwraca kierunek geograficzny dla wektora opisanego na sasiednich kwadratach.
     *
     * @param xStart współrzędne X siatki kwadratu początkowego
     * @param yStart współrzędne Y siatki kwadratu początkowego
     * @param xStop  współrzędne X siatki kwadratu końcowego
     * @param yStop  współrzędne Y siatki kwadratu końcowego
     * @return kierunek geograficzny dla wektora zadanego przez kwadrat początkowy i końcowy lub UNDEFINED dla kwadratów niesąsiednich
     */
    public static EGeoDirection kierunekDlaSasiada(int xStart, int yStart, int xStop, int yStop) {
        int d = Math.abs(xStart - xStop) + Math.abs(yStart - yStop);
        if (d == 1 || d == 2) {
            if (xStart == xStop) {
                if (yStart < yStop) {
                    return EGeoDirection.NORTH;
                }
                return EGeoDirection.SOUTH;
            }
            if (yStart == yStop) {
                if (xStart < xStop) {
                    return EGeoDirection.EAST;
                }
                return EGeoDirection.WEST;
            }
            if (xStart < xStop) {
                if (yStart < yStop) {
                    return EGeoDirection.NORTHEAST;
                }
                return EGeoDirection.SOUTHEAST;
            }
            if (yStart < yStop) {
                return EGeoDirection.NORTHWEST;
            }
            return EGeoDirection.SOUTHWEST;
        } else {
            return EGeoDirection.UNDEFINED;
        }
    }

    private static final float PRZEKATNA_MK = MapConsts.SS_SIZE * (float) Math.sqrt(2);

    /**
     * Funkcja wyznacza odległość między środkami kwadratów sąsiednich na zadanym kierunku
     *
     * @param kierunek zadany kierunek sąsiedztwa
     * @return odległość między środkami kwadratów sąsiednich [m]
     */
    public static float odlegloscKwadratowSasiednich(EGeoDirection kierunek) {
        switch (kierunek) {
            case NORTH:
            case SOUTH:
            case EAST:
            case WEST:
                return MapConsts.SS_SIZE;
            case NORTHEAST:
            case NORTHWEST:
            case SOUTHEAST:
            case SOUTHWEST:
                return PRZEKATNA_MK;
            default:
                return 0.0f;
        }
    }

    /**
     * Funkcja wyznacza sumaryczną długość drogi podanej jako ciąg sąsiednich kwadratów
     *
     * @param road droga jako ciąg sąsiednich kwadratów
     * @return sumaryczna długość drogi [m]
     */
    public static float dlugoscDrogiPoKwadratch(Coord.Grid[] road) {
        float dl = 0;
        for (int i = 1; i < road.length; i++) {
            dl += Coord.Grid.distance(road[i - 1], road[i]);
        }
        return dl;
    }

    /**
     * Metoda generuje tablicę współrzędnych gridowych kolejnych punktów/kwadratów podanego odcinka.
     * <p>
     * Wykorzystuje algorytm Bresenhama.
     *
     * @param x1 współrzędna x początku
     * @param y1 współrzędna y początku
     * @param x2 współrzędna x końca
     * @param y2 współrzędna y końca
     * @return tablicę kolejnych kwadratów tworzących odcinek o zadanych końcach
     */
    public static Coord.Grid[] generateSquaresOfSegment(int x1, int y1, int x2, int y2) {
        int x = x1;
        int y = y1;
        int dx = x2 - x1;
        int dy = y2 - y1;
        final int sx = Integer.compare(dx, 0);
        final int sy = Integer.compare(dy, 0);
        if (dx < 0) {
            dx = -dx;
        }
        if (dy < 0) {
            dy = -dy;
        }
        final int ddx = 2 * dx;
        final int ddy = 2 * dy;
        int p;
        if (dx >= dy) {
            // poruszamy się po x
            p = ddy - dx;
            Coord.Grid[] res = new Coord.Grid[dx + 1];
            for (int i = 0; i <= dx; i++) {
                res[i] = new Coord.Grid(x, y);
                if (p > 0) {
                    y += sy;
                    p -= ddx;
                }
                x += sx;
                p += ddy;
            }
            return res;
        } else {
            // poruszamy sie po y
            p = ddx - dy;
            Coord.Grid[] res = new Coord.Grid[dy + 1];
            for (int i = 0; i <= dy; i++) {
                res[i] = new Coord.Grid(x, y);
                if (p > 0) {
                    x += sx;
                    p -= ddy;
                }
                y += sy;
                p += ddx;
            }
            return res;
        }
    }

    public static int[] generateSquaresOfSegment2(int x1, int y1, int x2, int y2) {
        int x = x1;
        int y = y1;
        int dx = x2 - x1;
        int dy = y2 - y1;
        final int sx = Integer.compare(dx, 0);
        final int sy = Integer.compare(dy, 0);
        if (dx < 0) {
            dx = -dx;
        }
        if (dy < 0) {
            dy = -dy;
        }
        final int ddx = 2 * dx;
        final int ddy = 2 * dy;
        int p;
        int size;
        int[] res;
        if (dx >= dy) {
            // poruszamy się po x
            p = ddy - dx;
            // rozmiar tablicy 2 razy liczba współrzędnych
            size = (dx + 1) << 1;
            res = new int[size];
            for (int i = 0; i < size; ) {
                res[i++] = x;
                res[i++] = y;
                if (p > 0) {
                    y += sy;
                    p -= ddx;
                }
                x += sx;
                p += ddy;
            }
            return res;
        } else {
            // poruszamy sie po y
            p = ddx - dy;
            // rozmiar tablicy 2 razy liczba współrzędnych
            size = (dy + 1) << 1;
            res = new int[size];
            for (int i = 0; i < size; ) {
                res[i++] = x;
                res[i++] = y;
                if (p > 0) {
                    x += sx;
                    p -= ddy;
                }
                y += sy;
                p += ddx;
            }
            return res;
        }
    }

    static void test(int... args) {
        int z = args[0];
    }

    /**
     * Funkcja wyznacza tablicę kwadratów leżących na łamanej.
     *
     * @param vertices tablica wspolrzednych wierzcholkow tworzacych lamana
     * @return tablica (ciąg) sąsiednich kwadratów leżących na łamanej
     */
    public static Coord.Grid[] generateSquaresOfSegments(Coord.Grid[] vertices) {
        if (vertices.length == 2) {
            return generateSquaresOfSegment(vertices[0].x, vertices[0].y, vertices[1].x, vertices[1].y);
        }
        Coord.Grid[][] tempSegments = new Coord.Grid[vertices.length - 1][];
        int size = 0;
        for (int i = 0; i < tempSegments.length; i++) {
            tempSegments[i] = generateSquaresOfSegment(vertices[i].x, vertices[i].y, vertices[i + 1].x, vertices[i + 1].y);
            if (i == 0) {
                // będą kopiowane wszystkie kwadraty
                size += tempSegments[i].length;
            } else {
                // kopiowane wszystkie poza pierwszym
                size += tempSegments[i].length - 1;
            }
        }
        Coord.Grid[] res = new Coord.Grid[size];
        int destPos = 0;
        for (int i = 0; i < tempSegments.length; i++) {
            int len;
            int srcPos;
            if (i == 0) {
                // Pierwszy segment łamanej (zawiera wszystkie kwadraty).
                srcPos = 0;
                len = tempSegments[i].length;
            } else {
                // Kolejny segment łamanej (bez pierwszego kwadratu).
                srcPos = 1;
                len = tempSegments[i].length - 1;
            }
            System.arraycopy(tempSegments[i], srcPos, res, destPos, len);
            destPos += len;
        }
        return res;
    }

    /**
     * Funkcja wykrywa orientację wierzchołków: zgodność z ruchem wskazówek zegara.
     *
     * @param poly
     * @return
     */
    public static boolean isClockwise(Coord.Grid[] poly) {
        if (poly.length <= 2) {
            return true;
        }
        long sum = 0;
        for (int i = 0; i < poly.length; i++) {
            int next = (i + 1) % poly.length;
            sum += (long) (poly[next].x - poly[i].x) * (poly[next].y + poly[i].y);
        }
        // If sum > 0, it is clockwise. If < 0, it is counter-clockwise.
        return sum >= 0;
    }

    /**
     * Funkcja usuwa z tablicy wejściowej rejon wierzchołków obszaru powtarzające się sąsiednie wierzchołki.
     *
     * @param vertices tablica wierzchołków
     * @return poprawioną tablicę wierzchołków
     */
    public static Coord.Grid[] removeAdjacentDuplicates(Coord.Grid[] vertices) {
        if (vertices == null) {
            return null;
        }
        if (vertices.length < 2) {
            return vertices;
        }
        int i = 0;
        for (int j = i + 1; j < vertices.length; j++) {
            if (!vertices[i].equals(vertices[j])) {
                i++;
                vertices[i] = vertices[j];
            }
        }
        if (i > 0 && vertices[0].equals(vertices[i])) {
            // Pominięcie ostatniego wierzchołka identycznego z pierwszym (zamykający wielokąt).
            i--;
        }
        Coord.Grid[] newArr = new Coord.Grid[i + 1];
        System.arraycopy(vertices, 0, newArr, 0, newArr.length);
        return newArr;
    }

    /**
     * Funkcja usuwa z listy wierzchołki pośrednie tworzące "schodek". Wykorzystywana w przypadku obiektów liniowych
     * (łamanych).
     *
     * @param vertices
     * @return
     */
    public static Coord.Grid[] removeSteps(Coord.Grid[] vertices) {
        if (vertices.length < 3) {
            return vertices;
        }
        boolean[] toRemove = new boolean[vertices.length];
        Coord.Grid node_i;
        Coord.Grid node_ii;
        Coord.Grid node_iii;
        boolean stepDetected = false;
        int count = 0;
        for (int i = 0; i < vertices.length - 2; i++) {
            node_i = vertices[i];
            node_ii = vertices[i + 1];
            node_iii = vertices[i + 2];
            int absX_i_ii = Math.abs(node_ii.x - node_i.x);
            int absY_i_ii = Math.abs(node_ii.y - node_i.y);
            int absX_ii_iii = Math.abs(node_iii.x - node_ii.x);
            int absY_ii_iii = Math.abs(node_iii.y - node_ii.y);
            if (absX_i_ii + absY_i_ii + absX_ii_iii + absY_ii_iii == 2) {
                // the nodes can form a step
                if (absX_i_ii + absX_ii_iii == 1) {
                    // the nodes form a step, so the middle node of the step should be removed
                    toRemove[i + 1] = true;
                    count++;
                    stepDetected = true;
                    // przeskok o usuwany wezel
                    i++;
                }
            }
        }
        if (stepDetected) {
            Coord.Grid[] newArr = new Coord.Grid[vertices.length - count];
            int j = 0;
            for (int i = 0; i < vertices.length; i++) {
                if (!toRemove[i]) {
                    newArr[j++] = vertices[i];
                }
            }
            vertices = newArr;
            return removeSteps(vertices);
        }
        return vertices;
    }

    public static Coord.Grid[] removeCollinearity(Coord.Grid[] vertices, boolean isPolygon) {
        if (vertices.length < 3) {
            return vertices;
        }
        boolean[] toDelete = new boolean[vertices.length];
        Coord.Grid curr_node;
        Coord.Grid next_node;
        Coord.Grid prev_node;
        boolean collinearFound = false;
        int lenMax = vertices.length;
        int iStart = 0;
        if (!isPolygon) {
            // łamana otwarta
            lenMax--;
            iStart = 1;
        }
        final int len = vertices.length;
        int count = 0;
        for (int curr = iStart; curr < lenMax; curr++) {
            int next = (curr + 1) % len;
            int prev = (curr + len - 1) % len;
            curr_node = vertices[curr];
            next_node = vertices[next];
            prev_node = vertices[prev];
            if (GeomUtils.include(prev_node.x, prev_node.y, next_node.x, next_node.y, curr_node.x, curr_node.y)) {
                // i-ty do usuniecia
                toDelete[curr] = true;
                collinearFound = true;
                count++;
            }
        }
        if (collinearFound) {
            Coord.Grid[] newArr = new Coord.Grid[len - count];
            int j = 0;
            for (int i = 0; i < len; i++) {
                if (!toDelete[i]) {
                    newArr[j++] = vertices[i];
                }
            }
            vertices = newArr;
            return removeCollinearity(vertices, isPolygon);
        }
        return vertices;
    }

    /**
     * Funkcja odwraca kolejność wierzchołków w tablicy.
     *
     * @param vertices
     * @return
     */
    public static Coord.Grid[] reverse(Coord.Grid[] vertices) {
        if (vertices == null || vertices.length == 0) {
            return vertices;
        }
        int mid = vertices.length / 2;
        int last = vertices.length - 1;
        for (int i = 0; i < mid; i++) {
            vertices[i] = vertices[last - i];
        }
        return vertices;
    }

    /**
     * Funkcja wyznacza srodek zadanego odcinka
     *
     */
    public static Coord.Grid srodekOdcinka(int x1, int y1, int x2, int y2) {
        int xsr = (x1 + x2) / 2;
        int ysr = (y1 + y2) / 2;
        Coord.Grid srodek = new Coord.Grid(xsr, ysr);
        return srodek;
    }

    /**
     * Funkcja zwraca 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 odleglosc(int x1, int y1, int x2, int y2) {
        // odleglosc miedzy srodkami malych kwadratow
        int xx = x1 - x2;
        xx *= xx;
        int yy = y1 - y2;
        yy *= yy;
        float odl = (float) Math.sqrt(xx + yy);
        odl *= MapConsts.SS_SIZE;
        return odl;
    }

    public static void print(Coord.Grid[] segment) {
        StringBuilder sb = new StringBuilder(300);
        sb.append('[');
        int last = segment.length - 1;
        for (int i = 0; i < segment.length; i++) {
            sb.append('(');
            sb.append(segment[i].x);
            sb.append(',');
            sb.append(segment[i].y);
            sb.append(')');
            if (i < last) {
                sb.append('|');
            }
        }
        sb.append(']');
        System.out.println(sb);
    }

    // =====================================================================

    public static void main(String[] args) {

//        Coord.Grid[] segments = new Coord.Grid[4];
//        segments[0] = new Coord.Grid(1, 1);
//        segments[1] = new Coord.Grid(6, 4);
//        segments[2] = new Coord.Grid(7, 3);
//        segments[3] = new Coord.Grid(8, 9);
//
//        Coord.Grid[] squares = generateSquaresOfSegments(segments);
//        print(squares);

        int x0 = 10;
        int y0 = 10;
        int x1 = 10;
        int y1 = 10;
        EGeoDirection d = neighborDirection(x0, y0, x1, y1);
        x1 = 9;
        y1 = 11;
        d = neighborDirection(x0, y0, x1, y1);
        x1 = 10;
        y1 = 11;
        d = neighborDirection(x0, y0, x1, y1);
        x1 = 11;
        y1 = 11;
        d = neighborDirection(x0, y0, x1, y1);
        x1 = 11;
        y1 = 10;
        d = neighborDirection(x0, y0, x1, y1);
        x1 = 9;
        x1 = 11;
        y1 = 9;
        d = neighborDirection(x0, y0, x1, y1);
        x1 = 10;
        y1 = 9;
        d = neighborDirection(x0, y0, x1, y1);
        x1 = 9;
        y1 = 9;
        d = neighborDirection(x0, y0, x1, y1);
        x1 = 9;
        y1 = 10;
        d = neighborDirection(x0, y0, x1, y1);

        Coord.Grid[] segments = new Coord.Grid[12];
        segments[0] = new Coord.Grid(1, 2);
        segments[1] = new Coord.Grid(2, 3);
        segments[2] = new Coord.Grid(2, 4);
        segments[3] = new Coord.Grid(2, 5);
        segments[4] = new Coord.Grid(3, 5);
        segments[5] = new Coord.Grid(4, 5);
        segments[6] = new Coord.Grid(5, 6);
        segments[7] = new Coord.Grid(6, 6);
        segments[8] = new Coord.Grid(6, 7);
        segments[9] = new Coord.Grid(7, 8);
        segments[10] = new Coord.Grid(8, 8);
        segments[11] = new Coord.Grid(8, 7);

        Coord.Grid[] squares = removeSteps(segments);
        print(segments);
        print(squares);


        double odl = Coord.Grid.distance(1, 1, 2, 2);
        double odl2 = Coord.Grid.distanceApprox(1, 1, 2, 2);
        logger.debug("dist = {}, dist2 = {}, DELTA = {}, delta = {}", odl, odl2, odl - odl2, (odl - odl2) / odl);
        odl = Coord.Grid.distance(1, 1, 21, 21);
        odl2 = Coord.Grid.distanceApprox(1, 1, 21, 21);
        logger.debug("dist = {}, dist2 = {}, DELTA = {}, delta = {}", odl, odl2, odl - odl2, (odl - odl2) / odl);

        odl = Coord.Grid.distance(1, 1, 1, 21);
        odl2 = Coord.Grid.distanceApprox(1, 1, 1, 21);
        logger.debug("dist = {}, dist2 = {}, DELTA = {}, delta = {}", odl, odl2, odl - odl2, (odl - odl2) / odl);

        odl = Coord.Grid.distance(1, 1, 10, 21);
        odl2 = Coord.Grid.distanceApprox(1, 1, 10, 21);
        logger.debug("dist = {}, dist2 = {}, DELTA = {}, delta = {}", odl, odl2, odl - odl2, (odl - odl2) / odl);

        odl = Coord.Grid.distance(1, 1, 100, 210);
        odl2 = Coord.Grid.distanceApprox(1, 1, 100, 210);
        logger.debug("dist = {}, dist2 = {}, DELTA = {}, delta = {}", odl, odl2, odl - odl2, (odl - odl2) / odl);

        odl = Coord.Grid.distance(1, 1, 5, 210);
        odl2 = Coord.Grid.distanceApprox(1, 1, 5, 210);
        logger.debug("dist = {}, dist2 = {}, DELTA = {}, delta = {}", odl, odl2, odl - odl2, (odl - odl2) / odl);


        Coord.Grid p = new Coord.Grid(4, 6);
        Coord.Grid q = new Coord.Grid(8, 2);
        Coord.Grid r = new Coord.Grid(6, 4);
        Coord.Grid s = new Coord.Grid(8, 6);

        Coord.Grid startCoord = new Coord.Grid(10, 10);
        Coord.Grid stopCoord = new Coord.Grid();
        for (int y = 20; y >= 0; y--) {
            for (int x = 0; x <= 20; x++) {
                stopCoord.set(x, y);
                EGeoDirection kier = estimateDirection(startCoord, stopCoord);
                switch (kier) {
                    case NORTH:
                        System.out.print('|');
                        break;
                    case NORTHEAST:
                        System.out.print('/');
                        break;
                    case EAST:
                        System.out.print('-');
                        break;
                    case SOUTHEAST:
                        System.out.print('\\');
                        break;
                    case SOUTH:
                        System.out.print('|');
                        break;
                    case SOUTHWEST:
                        System.out.print('/');
                        break;
                    case WEST:
                        System.out.print('-');
                        break;
                    case NORTHWEST:
                        System.out.print('\\');
                        break;
                    case UNDEFINED:
                        System.out.print('O');
                        break;
                }
            }
            System.out.println();
        }
        System.out.println();

        EGeoDirection kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(10, 15));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(10, 5));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(15, 10));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(5, 10));
        System.out.println(kier);

        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(9, 15));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(11, 15));
        System.out.println(kier);

        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(9, 5));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(11, 5));
        System.out.println(kier);

        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(15, 9));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(15, 11));
        System.out.println(kier);

        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(5, 9));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(5, 11));
        System.out.println(kier);


        // Kierunki pośrednie: PLN-WSC, PLD-WSC, PLN-ZAC, PLD-ZAC
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(15, 15));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(15, 5));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(5, 15));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(5, 5));
        System.out.println(kier);

        // PLN-WSC
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(15, 16));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(15, 14));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(16, 15));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(14, 15));
        System.out.println(kier);

        // PLD-WSC
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(15, 6));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(15, 4));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(16, 5));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(14, 5));
        System.out.println(kier);

        //PLN-ZAC
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(5, 16));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(5, 14));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(6, 15));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(4, 15));
        System.out.println(kier);

        // PLD-ZAC
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(5, 6));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(5, 4));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(6, 5));
        System.out.println(kier);
        kier = estimateDirection(new Coord.Grid(10, 10), new Coord.Grid(4, 5));
        System.out.println(kier);

        boolean przeciecie = intersection(p, q, r, s);
        r.set(8, 2);
        s.set(11, 5);
        przeciecie = intersection(p, q, r, s);
        r.set(6, 4);
        s.set(7, 3);
        przeciecie = intersection(p, q, r, s);
        r.set(6, 4);
        s.set(10, 0);
        przeciecie = intersection(p, q, r, s);
        r.set(4, 4);
        s.set(6, 6);
        przeciecie = intersection(p, q, r, s);
        r.set(4, 8);
        s.set(6, 10);
        przeciecie = intersection(p, q, r, s);
        r.set(2, 8);
        s.set(1, 9);
        przeciecie = intersection(p, q, r, s);


        Coord.Grid[] lamana = {new Coord.Grid(8, 8), new Coord.Grid(3, 4), new Coord.Grid(6, 11)};


        long det = det3(1, 0, 4, 0, 2, 2);
        det = det3(1, 0, 4, 0, 2, -2);

        det = det3(1, 1, 1, 4, 0, 2);
        det = det3(1, 1, 1, 4, 2, 2);

        det = det3(1, 1, 4, 4, 4, 3);
        det = det3(1, 1, 4, 4, 3, 4);


//         Coord.GridCoord[] polygon = new  Coord.GridCoord[21];
//        polygon[0] = new  Coord.GridCoord(12, 18);
//        polygon[1] = new  Coord.GridCoord(16, 18);
//        polygon[2] = new  Coord.GridCoord(18, 13);
//        polygon[3] = new  Coord.GridCoord(21, 13);
//        polygon[4] = new  Coord.GridCoord(22, 9);
//        polygon[5] = new  Coord.GridCoord(20, 7);
//        polygon[6] = new  Coord.GridCoord(17, 7);
//        polygon[7] = new  Coord.GridCoord(15, 9);
//        polygon[8] = new  Coord.GridCoord(13, 7);
//        polygon[9] = new  Coord.GridCoord(15, 5);
//        polygon[10] = new  Coord.GridCoord(17, 5);
//        polygon[11] = new  Coord.GridCoord(14, 2);
//        polygon[12] = new  Coord.GridCoord(8, 8);
//        polygon[13] = new  Coord.GridCoord(9, 3);
//        polygon[14] = new  Coord.GridCoord(6, 4);
//        polygon[15] = new  Coord.GridCoord(3, 4);
//        polygon[16] = new  Coord.GridCoord(2, 10);
//        polygon[17] = new  Coord.GridCoord(6, 11);
//        polygon[18] = new  Coord.GridCoord(6, 13);
//        polygon[19] = new  Coord.GridCoord(7, 15);
//        polygon[20] = new  Coord.GridCoord(10, 13);
//
//
//        // przesunięcie wielokata o wektor
//        for (int k = 0; k < polygon.length; k++) {
//            polygon[k].x += 1000;
//            polygon[k].y += 1000;
//        }
//         Coord.GridCoord p = new  Coord.GridCoord();
//        for (int j = 20; j >= 0; j--) {
//            for (int i = 0; i < 25; i++) {
//                p.x = i + 1000;
//                p.y = j + 1000;
//                char c = ' ';
//                if (insidePolygon(polygon, p)) {
//                    c = 'O';
//                }
//                for (int k = 0; k < polygon.length; k++) {
//                    if (polygon[k].equals(p)) {
//                        c = 'X';
//                        break;
//                    }
//                }
//                System.out.print(c);
//                System.out.print(' ');
//            }
//            System.out.println();
//        }
//		int lp = args.length / 2;
//		for (int i = 0; i < lp; i++) {
//			p.x = Integer.parseInt(args[2 * i]);
//			p.y = Integer.parseInt(args[2 * i + 1]);
//			if (insidePolygon(polygon, p)) {
//				System.out.println("Punkt " + p + " leży wewnatrz");
//			} else {
//				System.out.println("Punkt " + p + " leży na zewnatrz");
//			}
//		}
//		float odl = odleglosc(5000, 4000, 2, 2);
//		odl = odleglosc(1, 1000, 3999, 3);
//		odl = odleglosc(5, 1, 27, 11);
//		odl = odleglosc(10, 1, 2, 20);
//		odl = odleglosc(1, 1, 2, 2);
//
//		 Coord.GridCoord[] rejon = new  Coord.GridCoord[4];
//		rejon[0] = new  Coord.GridCoord(20, 18);
//		rejon[1] = new  Coord.GridCoord(27, 9);
//		rejon[2] = new  Coord.GridCoord(9, 1);
//		rejon[3] = new  Coord.GridCoord(2, 15);
//
//		ArrayList< Coord.GridCoord> kwadraty = kwadratyLamanej(rejon);
//		System.out.println(kwadraty);


//		try {
//			BufferedWriter wyj = new BufferedWriter(new FileWriter("A.txt"));
        // if (args.length == 5 ) {
        // int xp = Integer.parseInt(args[1]);
        // int yp = Integer.parseInt(args[2]);
        // int xk = Integer.parseInt(args[3]);
        // int yk = Integer.parseInt(args[4]);

        /*
         * int xp = 50000; int yp = 8000; int xk = 20; int yk = 1;
         */
//             Coord.GridCoord[] rejon = new  Coord.GridCoord[4];
//			rejon[0] = new  Coord.GridCoord();
//			rejon[0].x = 20;
//			rejon[0].y = 18;
//			rejon[1] = new  Coord.GridCoord();
//			rejon[1].x = 27;
//			rejon[1].y = 9;
//			rejon[2] = new  Coord.GridCoord();
//			rejon[2].x = 9;
//			rejon[2].y = 1;
//			rejon[3] = new  Coord.GridCoord();
//			rejon[3].x = 2;
//			rejon[3].y = 15;
//			 Coord.GridCoord[] kwadraty = kwadratyObszaru(rejon);

        /*
         * WspXY[] kwadraty = kwadratyOdcinka(xp, yp, xk, yk);
         * wyj.write("pocz: ("); wyj.write(Integer.toString(xp));
         * wyj.write(", "); wyj.write(Integer.toString(yp)); wyj.write(")");
         * wyj.newLine(); wyj.write("kon: (");
         * wyj.write(Integer.toString(xk)); wyj.write(", ");
         * wyj.write(Integer.toString(yk)); wyj.write(")"); wyj.newLine();
         */
//			wyj.write(Integer.toString(kwadraty.length));
//			wyj.newLine();
//			for (int i = 0; i < kwadraty.length; i++) {
//				wyj.write("(");
//				wyj.write(Integer.toString(kwadraty[i].x));
//				wyj.write(", ");
//				wyj.write(Integer.toString(kwadraty[i].y));
//				wyj.write(")");
//				wyj.newLine();
//			}
//
//			wyj.close();
//		} catch (IOException e) {
//			e.printStackTrace();
//		}
    }

}