package pl.wat.ms4ds.terrain;

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

import java.util.ArrayList;

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 wpolliniowy, 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 det3(px, py, qx, qy, rx, ry) == 0 &&
                Math.min(px, qx) <= rx &&
                rx <= Math.max(px, qx) &&
                Math.min(py, qy) <= ry &&
                ry <= Math.max(py, qy);
    }

    /**
     * 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))));
    }

    /**
     * Sprawdza, czy punkt należy do wielokata w ogólnym przypadku (boki wielokata bez samoprzecięć).
     *
     * @param polygon Kolejne wierzchołki wielokata.
     * @param p       Testowany punkt.
     * @return true, jeśli testowany punkt leży wewnątrz wielokąta
     */
    public static boolean insidePolygon(Coord.Grid[] polygon, Coord.Grid p) {
        // sprawdzenie czy punkt p należy do jednego z boków wielokąta
        for (int i = 0; i < polygon.length; i++) {
            int i_plus_1 = (i + 1) % polygon.length;
            if (include(polygon[i], polygon[i_plus_1], p)) {
                return true;
            }
        }
        // punkty p i p1 wyznaczają półprostą równoległą do osi OX
        // współrzędna X punktu p1 musi być większa od największej
        // współrzędnej X wśród wszystkich wierzchołków wielokąta
        Coord.Grid p1 = new Coord.Grid(p.x + 1, p.y);
        for (int i = 0; i < polygon.length; i++) {
            p1.x = Math.max(polygon[i].x, p1.x);
        }
        p1.x++;
        // licznik przecięć
        int cross_count = 0;
        // przechodzenie po wszystkich bokach
        int i = 0;
        while (i < polygon.length) {
            int i_plus_1 = (i + 1) % polygon.length;
            int i_plus_2 = (i + 2) % polygon.length;
            int i_minus_1 = (i + polygon.length - 1) % polygon.length;
            int sgn_det3_i_minus_1 = sgn(det3(p, p1, polygon[i_minus_1]));
            int sgn_det3_i_plus_1 = sgn(det3(p, p1, polygon[i_plus_1]));
            int sgn_det3_i_plus_2 = sgn(det3(p, p1, polygon[i_plus_2]));
            if (includeHorizontaly(p, p1, polygon[i])) {
                // polprosta p-p1 zawiera wierzchołek w(i)
                if (includeHorizontaly(p, p1, polygon[i_plus_1])) {
                    // polprosta p-p1 zawiera bok w(i)w(i+1)
                    if (sgn_det3_i_minus_1 != sgn_det3_i_plus_2) {
                        // punkt wczesniejszy w(i-1) i dalszy w(i+2) testowanego boku wielokata
                        // leza po przeciwnych stronach polprostej p-p1, zatem ilosc przeciec +1
                        cross_count++;
                    }
                    // pomijamy nastepny wierzcholek
                    i++;
                } else {
                    // polprosta p-p1 zawiera TYLKO wierzchołek w(i)
                    if (sgn_det3_i_minus_1 != 0 && sgn_det3_i_minus_1 != sgn_det3_i_plus_1) {
                        // wierzcholki sasiadujace z wierzchołkiem w(i) tj. w(i-1) i w(i+1)
                        // leza po przeciwnych stronach polprostej p-p1, zatem ilosc przeciec +1
                        cross_count++;
                    }
                }
            } else {
                // sprawdzenie czy polprosta p-p1
                // przecina bok w(i)w(i+1) wielokąta wewnatrz odcinka
                if (cross(p, p1, polygon[i], polygon[i_plus_1])) {
                    cross_count++;
                }
            }
            i++;
        }
        // obliczenie wyniku (nieparzysty oznacza należenie punktu do wielokata)
        return (cross_count % 2) == 1;
    }

    /**
     * Sprawdza, czy punkt należy do wielokata w ogólnym przypadku (boki wielokata bez samoprzecięć).
     *
     * @param polygon Kolejne wierzchołki wielokata.
     * @param px      Wsp. x testowanego punktu.
     * @param py      Wsp. y testowanego punktu.
     * @return true, jeśli testowany punkt leży wewnątrz wielokąta
     */
    public static boolean insidePolygon(Coord.Grid[] polygon, int px, int py) {
        // sprawdzenie czy punkt p nie należy do jednego z boków wielokąta
        for (int i = 0; i < polygon.length; i++) {
            int i_plus_1 = (i + 1) % polygon.length;
            if (include(polygon[i].x, polygon[i].y, polygon[i_plus_1].x, polygon[i_plus_1].y, px, py)) {
                return true;
            }
        }
        // punkty p i p1 wyznaczają półprostą równoległą do osi OX
        // współrzędna X punktu p1 musi być większa od największej
        // współrzędnej X wśród wszystkich wierzchołków wielokąta
        Coord.Grid p1 = new Coord.Grid(px + 1, py);
        for (int i = 0; i < polygon.length; i++) {
            p1.x = Math.max(polygon[i].x, p1.x);
        }
        p1.x++;
        // licznik przecięć
        int cross_count = 0;
        // przechodzenie po wszystkich bokach
        int i = 0;
        while (i < polygon.length) {
            int i_plus_1 = (i + 1) % polygon.length;
            int i_plus_2 = (i + 2) % polygon.length;
            int i_minus_1 = (i + polygon.length - 1) % polygon.length;
            int sgn_det3_i_minus_1 = sgn(det3(px, py, p1.x, p1.y, polygon[i_minus_1].x, polygon[i_minus_1].y));
            if (includeHorizontaly(px, py, p1.x, p1.y, polygon[i].x, polygon[i].y)) {
                // polprosta p-p1 zawiera wierzchołek w(i)
                if (includeHorizontaly(px, py, p1.x, p1.y, polygon[i_plus_1].x, polygon[i_plus_1].y)) {
                    int sgn_det3_i_plus_2 = sgn(det3(px, py, p1.x, p1.y, polygon[i_plus_2].x, polygon[i_plus_2].y));
                    // polprosta p-p1 zawiera bok w(i)w(i+1)
                    if (sgn_det3_i_minus_1 != sgn_det3_i_plus_2) {
                        // punkt wczesniejszy w(i-1) i dalszy w(i+2) testowanego boku wielokata
                        // leza po przeciwnych stronach polprostej p-p1, zatem ilosc przeciec +1
                        cross_count++;
                    }
                    // pomijamy nastepny wierzcholek
                    i++;
                } else {
                    int sgn_det3_i_plus_1 = sgn(det3(px, py, p1.x, p1.y, polygon[i_plus_1].x, polygon[i_plus_1].y));
                    // polprosta p-p1 zawiera TYLKO wierzchołek w(i)
                    if (sgn_det3_i_minus_1 != 0 && sgn_det3_i_minus_1 != sgn_det3_i_plus_1) {
                        // wierzcholki sasiadujace z wierzchołkiem w(i) tj. w(i-1) i w(i+1)
                        // leza po przeciwnych stronach polprostej p-p1, zatem ilosc przeciec +1
                        cross_count++;
                    }
                }
            } else {
                // sprawdzenie czy polprosta p-p1
                // przecina bok w(i)w(i+1) wielokąta wewnatrz odcinka
                if (cross(px, py, p1.x, p1.y, polygon[i].x, polygon[i].y, polygon[i_plus_1].x, polygon[i_plus_1].y)) {
                    cross_count++;
                }
            }
            i++;
        }
        // obliczenie wyniku (nieparzysty oznacza należenie punktu do wielokata)
        return (cross_count % 2) == 1;
    }

    /**
     * Wyznacza kierunek przeciwny dla zadanego, przy założeniu, że kierunki geograficzne numerowane są od 0 do 7
     *
     * @param kierunek zadany kierunek
     * @return kierunek przeciwny dla zadanego
     */
    public static int kierunekPrzeciwny(int kierunek) {
        int kierPrzeciwny = (kierunek + 4) % 8;
        return kierPrzeciwny;
    }

    // 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 kierunek(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;
            }
        }
    }

    /**
     * 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;
    }

    /**
     * Funkcja wyznacza tablicę kwadratów leżących na łamanej.
     *
     * @param punktyLamanej tablica wspolrzednych tworzących łamaną
     * @return kwadratyWyj   kolekcja kwadratów leżących na łamanej
     */
    public static ArrayList<Coord.Grid> kwadratyLamanej(Coord.Grid[] punktyLamanej) {
        ArrayList<Coord.Grid> kwadratyWyj = new ArrayList<Coord.Grid>();
        if (punktyLamanej.length == 2) {
            Coord.Grid[] kwadraty = kwadratyOdcinka(punktyLamanej[0], punktyLamanej[1]);
            for (int i = 0; i < kwadraty.length; i++) {
                kwadratyWyj.add(kwadraty[i]);
            }
            return kwadratyWyj;
        }
        for (int i = 0; i < punktyLamanej.length - 1; i++) {
            Coord.Grid[] kwadraty = kwadratyOdcinka(punktyLamanej[i], punktyLamanej[i + 1]);
            int lko;
            if (i < punktyLamanej.length - 2) {
                // odcinek lamanej nie jest ostatni, zatem bez ostatniego kwadratu odcinka,
                // gdyz powtorzy sie jako pierwszy w nastepnym odcinku lamanej
                lko = kwadraty.length - 1;
            } else {
                lko = kwadraty.length;
            }
            for (int j = 0; j < lko; j++) {
                kwadratyWyj.add(kwadraty[j]);
            }
        }
        return kwadratyWyj;
    }

    /**
     * Funkcja wyznacza tablicę kwadratów leżących na łamanej.
     *
     * @param punktyLamanej tablica wspolrzednych odcinków tworzących łamaną
     * @return tablica (ciąg) sąsiednich kwadratów leżących na łamanej
     */
    public static Coord.Grid[] kwadratyLamanej2(Coord.Grid[] punktyLamanej) {
        Coord.Grid[] kwadratyWyj;
        if (punktyLamanej.length == 2) {
            kwadratyWyj = kwadratyOdcinka(punktyLamanej[0], punktyLamanej[1]);
            return kwadratyWyj;
        }
        int maxLen = 0;
        for (int i = 0; i < punktyLamanej.length - 1; i++) {
            maxLen += Math.abs(punktyLamanej[i].x - punktyLamanej[i + 1].x) + Math.abs(punktyLamanej[i].y - punktyLamanej[i + 1].y) + 1;
        }
        Coord.Grid[] helpTab = new Coord.Grid[maxLen];
        int lastLen = 0;
        for (int i = 0; i < punktyLamanej.length - 1; i++) {
            Coord.Grid[] kwadraty = kwadratyOdcinka(punktyLamanej[i], punktyLamanej[i + 1]);
            // liczba kwadratów bieżącego odcinka do zapamietania
            int lko;
            if (i < punktyLamanej.length - 2) {
                // odcinek lamanej nie jest ostatni, zatem bez ostatniego kwadratu odcinka,
                // gdyz powtorzy sie jako pierwszy w nastepnym odcinku lamanej
                lko = kwadraty.length - 1;
            } else {
                // ostatni odcinek łamanej, zatem wszystkie kwadraty odcinka
                lko = kwadraty.length;
            }
            System.arraycopy(kwadraty, 0, helpTab, lastLen, lko);
//            for (int j = 0; j < lko; j++) {
//                helpTab[lastLen] = kwadraty[j];
//                lastLen++;
//            }
        }
        kwadratyWyj = new Coord.Grid[lastLen];
        System.arraycopy(helpTab, 0, kwadratyWyj, 0, lastLen);
//        for (int i = 0; i < lastLen; i++) {
//            kwadratyWyj[i] = helpTab[i];
//        }
        return kwadratyWyj;
    }

    /**
     * Funkcja wyznacza tablice (ciąg) sąsiednich kwadratow na odcinku zadanym przez wspolrzedne jego koncow.
     *
     * @param pocz wspolrzedna poczatku odcinka
     * @param kon  wspolrzedna konca odcinka
     * @return tablica sąsiednich kwadratow lezacych na odcinku
     */
    public static Coord.Grid[] kwadratyOdcinka(Coord.Grid pocz, Coord.Grid kon) {
        return kwadratyOdcinka(pocz.x, pocz.y, kon.x, kon.y);
    }

    /**
     * Funkcja wyznacza tablice (ciąg) sąsiednich kwadratow na odcinku zadanym przez wspolrzedne jego koncow.
     *
     * @param xp wspolrzedna x poczatku
     * @param yp wspolrzedna y poczatku
     * @param xk wspolrzedna x konca
     * @param yk wspolrzedna y konca
     * @return tablica sąsiednich kwadratow lezacych na odcinku
     */
    public static Coord.Grid[] kwadratyOdcinka(int xp, int yp, int xk, int yk) {
        Coord.Grid[] kwadraty;
        if ((xp == xk) && (yp == yk)) {
            // odcinek skladajacy sie z jednego kwadratu
            kwadraty = new Coord.Grid[1];
            kwadraty[0] = new Coord.Grid(xp, yp);
            return kwadraty;
        }
        if (xp == xk) {
            // odcinek pionowy
            kwadraty = kwadratyOdcinkaPionowego(xp, yp, xk, yk);
            return kwadraty;
        }
        if (yp == yk) {
            // odcinek poziomy
            kwadraty = kwadratyOdcinkaPoziomego(xp, yp, xk, yk);
            return kwadraty;
        }
        int dx = Math.abs(xk - xp);
        int dy = Math.abs(yk - yp);
        if (dx == dy) {
            // odcinek na przekatnej duzego kwadratu
            kwadraty = kwadratyOdcinkaNaPrzekatnej(xp, yp, xk, yk);
        } else {
            // odcinek na przekatnej duzego prostokata
            kwadraty = kwadratyOdcinkaDowolnego(xp, yp, xk, yk);
        }
        return kwadraty;
    }

    /**
     * Funkcja pomocnicza.
     * Funkcja zwraca kwadraty odcinka/wektora zorientowanego względem osi OX pod katem 90 stop. lub 270 stop.
     *
     * @param xp wspolrzedna x poczatku
     * @param yp wspolrzedna y poczatku
     * @param xk wspolrzedna x konca
     * @param yk wspolrzedna y konca
     * @return tablica kwadratow lezacych na odcinku
     */
    private static Coord.Grid[] kwadratyOdcinkaPionowego(int xp, int yp, int xk, int yk) {
        Coord.Grid[] kwadraty = new Coord.Grid[Math.abs(yp - yk) + 1];
        if (yp < yk) {
            // zorientowanie wektora do gory
            for (int i = 0; i < kwadraty.length; i++) {
                kwadraty[i] = new Coord.Grid(xp, yp + i);
            }
        } else {
            // zorientowanie wektora do dolu
            for (int i = 0; i < kwadraty.length; i++) {
                kwadraty[i] = new Coord.Grid(xp, yp - i);
            }
        }
        return kwadraty;
    }

    /**
     * Funkcja pomocnicza.
     * Funkcja zwraca kwadraty odcinka/wektora zorientowanego względem osi OX pod katem 0 stop. lub 180 stop..
     *
     * @param xp wspolrzedna x poczatku
     * @param yp wspolrzedna y poczatku
     * @param xk wspolrzedna x konca
     * @param yk wspolrzedna y konca
     * @return tablica kwadratow lezacych na odcinku
     */
    private static Coord.Grid[] kwadratyOdcinkaPoziomego(int xp, int yp, int xk, int yk) {
        Coord.Grid[] kwadraty = new Coord.Grid[Math.abs(xp - xk) + 1];
        if (xp < xk) {
            // zorientowanie wektora w prawo
            for (int i = 0; i < kwadraty.length; i++) {
                kwadraty[i] = new Coord.Grid(xp + i, yp);
            }
        } else {
            // zorientowanie wektora w lewo
            for (int i = 0; i < kwadraty.length; i++) {
                kwadraty[i] = new Coord.Grid(xp - i, yp);
            }
        }
        return kwadraty;
    }

    /**
     * Funkcja pomocnicza.
     * Zwraca kwadraty leżące na odcinku/wektorze zorientowanym względem osi OX pod kątem innym niż: 0, 90, 180, 270.
     *
     * @param xp wspolrzedna x poczatku
     * @param yp wspolrzedna y poczatku
     * @param xk wspolrzedna x konca
     * @param yk wspolrzedna y konca
     * @return tablica kwadratow lezacych na odcinku
     */
    private static Coord.Grid[] kwadratyOdcinkaDowolnego(int xp, int yp, int xk, int yk) {
        int x = xp;
        int y = yp;
        int dx = xk - xp;
        int dy = yk - yp;
        Coord.Grid[] tab;
        int abs_dx = Math.abs(dx);
        int abs_dy = Math.abs(dy);
        if (abs_dx > abs_dy) {
            // przypadek, gdy poruszamy sie po osi OX
            // wyznaczenie wspolczynnikow prostej zawierającej odcinek na podstawie środków kwadratów
            double a = (double) dy / (double) dx;
            double b = yp + 0.5 - a * (xp + 0.5);
            tab = new Coord.Grid[abs_dx + 1];
            int i = 0;
            tab[i] = new Coord.Grid(x, y);
            if (dx > 0) {
                // idziemy w prawo w kierunku rosnacych wartosci x
                if (dy > 0) {
                    // prosta zaierająca odcinek jest funkcją rosnącą
                    while (x < xk) {
                        x++;
                        int yy_ref = y + 1;
                        double yy_l_test = a * x + b;
                        double yy_h_test = a * (x + 1) + b;
                        double dy_l = Math.abs(yy_ref - yy_l_test);
                        double dy_h = Math.abs(yy_ref - yy_h_test);
                        if (dy_l <= dy_h) {
                            y++;
                        }
                        i++;
                        tab[i] = new Coord.Grid(x, y);
                    }
                } else {
                    // prosta zawierająca odcinek jest funkcją malejącą
                    while (x < xk) {
                        x++;
                        int yy_ref = y;
                        double yy_l_test = a * x + b;
                        double yy_h_test = a * (x + 1) + b;
                        double dy_l = Math.abs(yy_ref - yy_l_test);
                        double dy_h = Math.abs(yy_ref - yy_h_test);
                        if (dy_l <= dy_h) {
                            y--;
                        }
                        i++;
                        tab[i] = new Coord.Grid(x, y);
                    }
                }
            } else {
                // idziemy w lewo w kierunku malejacych wartosci x
                if (dy > 0) {
                    // prosta zaierająca odcinek jest funkcją malejącą
                    while (x > xk) {
                        x--;
                        int yy_ref = y + 1;
                        double yy_l_test = a * x + b;
                        double yy_h_test = a * (x + 1) + b;
                        double dy_l = Math.abs(yy_ref - yy_l_test);
                        double dy_h = Math.abs(yy_ref - yy_h_test);
                        if (dy_l >= dy_h) {
                            y++;
                        }
                        i++;
                        tab[i] = new Coord.Grid(x, y);
                    }
                } else {
                    // prosta zaierająca odcinek jest funkcją rosnącą
                    while (x > xk) {
                        x--;
                        int yy_ref = y;
                        double yy_l_test = a * x + b;
                        double yy_h_test = a * (x + 1) + b;
                        double dy_l = Math.abs(yy_ref - yy_l_test);
                        double dy_h = Math.abs(yy_ref - yy_h_test);
                        if (dy_l >= dy_h) {
                            y--;
                        }
                        i++;
                        tab[i] = new Coord.Grid(x, y);
                    }
                }
            }
        } else {
            // przypadek, gdy poruszamy sie po osi OY
            // zamiana wspolrzednych koncow odcinka
            tab = kwadratyOdcinkaDowolnego(yp, xp, yk, xk);
            // zamiana wspolrzednych x <-> y
            for (int i = 0; i < tab.length; i++) {
                int temp = tab[i].x;
                tab[i].x = tab[i].y;
                tab[i].y = temp;
            }
        }
        return tab;
    }

    /**
     * Funkcja pomocnicza.
     * Zwraca kwadraty lezace na odcinku zorientowanaym pod dowolnym katem wzgledem osi OX.
     *
     * @param xp wspolrzedna x poczatku
     * @param yp wspolrzedna y poczatku
     * @param xk wspolrzedna x konca
     * @param yk wspolrzedna y konca
     * @return tablica kwadratow lezacych na odcinku
     */
    private static Coord.Grid[] kwadratyOdcinkaDowolnego2(int xp, int yp, int xk, int yk) {
        int dx = xk - xp;
        int dy = yk - yp;
        if (Math.abs(dx) > Math.abs(dy)) {
            // przypadek, gdy poruszamy sie po osi OX
            // wyznaczenie wspolczynnikow prostej (wartosci w metrach)
            float a;
            float b;
            float dxf = (float) dx;
            float dyf = (float) dy;
            a = dyf / dxf;
            // wspolrzedne srodka kwadratu (xp, yp)
            int xxp = xp * MapConsts.SS_SIZE + MapConsts.SS_SIZE / 2;
            int yyp = yp * MapConsts.SS_SIZE + MapConsts.SS_SIZE / 2;
            b = yyp - a * xxp;

            if (dx > 0) {
                // idziemy w prawo w kierunku rosnacych wartosci x
                Coord.Grid[] temp = new Coord.Grid[Math.abs(dx) + Math.abs(dy) + 1];
                int dl = 0;
                temp[dl] = new Coord.Grid();
                temp[dl].x = xp;
                temp[dl].y = yp;
                dl++;
                for (int x = xp + 1; x <= xk - 1; x++) {
                    // wspolrzedne wierzcholkow kwadratu w metrach
                    int xd, xg;
                    int yd, yg;
                    int y1, y2;
                    xd = (x - 1) * MapConsts.SS_SIZE;
                    xg = x * MapConsts.SS_SIZE;
                    float ydf = a * xd + b;
                    float ygf = a * xg + b;
                    yd = (int) ydf;
                    yg = (int) ygf;
                    y1 = yd / MapConsts.SS_SIZE + 1;
                    y2 = yg / MapConsts.SS_SIZE + 1;
                    temp[dl] = new Coord.Grid();
                    temp[dl].x = x;
                    temp[dl].y = y1;
                    dl++;
                    if (y1 != y2) {
                        temp[dl] = new Coord.Grid();
                        temp[dl].x = x;
                        temp[dl].y = y2;
                        dl++;
                    }
                }
                temp[dl] = new Coord.Grid();
                temp[dl].x = xk;
                temp[dl].y = yk;
                dl++;
                Coord.Grid[] kwadraty = new Coord.Grid[dl];
                System.arraycopy(temp, 0, kwadraty, 0, dl);
//                for (int i = 0; i < kwadraty.length; i++) {
//                    // przepisanie referencji na wspolrzedne kwadratow
//                    kwadraty[i] = temp[i];
//                }
                return kwadraty;
            } else {
                // idziemy w lewo w kierunku malejacych wartosci x
                // zamiana koncow odcinka
                Coord.Grid[] temp = kwadratyOdcinkaDowolnego2(xk, yk, xp, yp);
                Coord.Grid[] kwadraty = new Coord.Grid[temp.length];
                for (int i = 0; i < kwadraty.length; i++) {
                    kwadraty[i] = temp[kwadraty.length - 1 - i];
                }
                return kwadraty;
            }
        } else {
            // przypadek, gdy poruszamy sie po osi OY
            // zamiana wspolrzednych koncow odcinka
            Coord.Grid[] kwadraty = kwadratyOdcinkaDowolnego2(yp, xp, yk, xk);
            // przepisanie wspolrzednych do nowej tablicy
            // z zamiana wspolrzednych
            for (int i = 0; i < kwadraty.length; i++) {
                int temp_x = kwadraty[i].x;
                kwadraty[i].x = kwadraty[i].y;
                kwadraty[i].y = temp_x;
            }
            return kwadraty;
        }
    }

    /**
     * Funkcja pomocnicza.
     * Zwraca kwadraty lezace na odcinku zorientowanaym pod katem 45 lub 135 stop. wzgledem osi OX.
     *
     * @param xp wspolrzedna x poczatku
     * @param yp wspolrzedna y poczatku
     * @param xk wspolrzedna x konca
     * @param yk wspolrzedna y konca
     * @return tablica kwadratow lezacych na odcinku
     */
    private static Coord.Grid[] kwadratyOdcinkaNaPrzekatnej(int xp, int yp, int xk, int yk) {
        int dx = xk - xp;
        int dy = yk - yp;
        Coord.Grid[] kwadraty = new Coord.Grid[Math.abs(dx) + 1];
        if ((dx > 0) && (dy > 0)) {
            // wektor typu "/" do gory
            for (int i = 0; i < kwadraty.length; i++) {
                kwadraty[i] = new Coord.Grid(xp + i, yp + i);
            }
        } else if ((dx > 0) && (dy < 0)) {
            // wektor typu "\" do dolu
            for (int i = 0; i < kwadraty.length; i++) {
                kwadraty[i] = new Coord.Grid(xp + i, yp - i);
            }
        } else if ((dx < 0) && (dy < 0)) {
            // wektor typu "/" do dolu
            for (int i = 0; i < kwadraty.length; i++) {
                kwadraty[i] = new Coord.Grid(xp - i, yp - i);
            }
        } else {
            // wektor typu "\" do gory
            for (int i = 0; i < kwadraty.length; i++) {
                kwadraty[i] = new Coord.Grid(xp - i, yp + i);
            }
        }
        return kwadraty;
    }

    /**
     * Funkcja wyznacza ograniczenie dolne wspolrzednych rejonu
     *
     * @param rejon
     * @return ograniczenie dolne wspolrzednych rejonu
     */
    public static Coord.Grid wspXYOgranDolne(Coord.Grid[] rejon) {
        if (rejon == null) {
            return null;
        }
        Coord.Grid kw_min = new Coord.Grid();
        kw_min.x = rejon[0].x;
        kw_min.y = rejon[0].y;
        for (int i = 1; i < rejon.length; i++) {
            if (kw_min.x > rejon[i].x) {
                kw_min.x = rejon[i].x;
            }
            if (kw_min.y > rejon[i].y) {
                kw_min.y = rejon[i].y;
            }
        }
        return kw_min;
    }

    /**
     * Funkcja wyznacza ograniczenie górne wspolrzednych rejonu
     *
     * @param rejon
     * @return ograniczenie górne wspolrzednych rejonu
     */
    public static Coord.Grid wspXYOgranGorne(Coord.Grid[] rejon) {
        if (rejon == null) {
            return null;
        }
        Coord.Grid kw_max = new Coord.Grid();
        kw_max.x = rejon[0].x;
        kw_max.y = rejon[0].y;
        for (int i = 1; i < rejon.length; i++) {
            if (kw_max.x < rejon[i].x) {
                kw_max.x = rejon[i].x;
            }
            if (kw_max.y < rejon[i].y) {
                kw_max.y = rejon[i].y;
            }
        }
        return kw_max;
    }

    /**
     * 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 nalezyDoProstokata(int xMin, int yMin, int xMax, int yMax, int xTest, int yTest) {
        if (xTest < xMin || xTest > xMax) {
            return false;
        }
        if (yTest < yMin || yTest > yMax) {
            return false;
        }
        return true;
    }

    /**
     * Funkcja usuwa z tablicy wejściowej rejon wierzchołków obszaru powtarzające się sąsiednie wierzchołki
     *
     * @param rejon tablica wierzchołków obszaru
     * @return poprawioną tablicę wierzchołków obszaru
     */
    public static Coord.Grid[] poprawRejon(Coord.Grid[] rejon) {
        if ((rejon == null) || (rejon.length <= 0)) {
            return null;
        }
        int j = 0;
        Coord.Grid[] rejon_temp = new Coord.Grid[rejon.length];
        rejon_temp[j] = rejon[j];
        for (int i = j + 1; i < rejon.length; i++) {
            if (!rejon[i].equals(rejon_temp[j])) {
                if (i == rejon.length - 1) {
                    if (rejon[i].equals(rejon_temp[0])) {
                        break;
                    }
                }
                j++;
                rejon_temp[j] = rejon[i];
            }
        }
        if ((j > 0) && (rejon_temp[0].equals(rejon_temp[j]))) {
            j--;
        }
        Coord.Grid[] rejon_popr = new Coord.Grid[j + 1];
        System.arraycopy(rejon_temp, 0, rejon_popr, 0, rejon_popr.length);
//        for (int k = 0; k < rejonPopr.length; k++) {
//            rejonPopr[k] = rejon_temp[k];
//        }
        return rejon_popr;
    }

    /**
     * Funkcja wyznacza tablice (kolekcję) kwadratow należących do zadanego obszaru
     *
     * @param rej kolejne wierzchołki obszaru zadawane prawoskretnie
     * @return tablica kwadratów należących do zadanego obszaru
     */
    public static Coord.Grid[] kwadratyObszaru(Coord.Grid[] rej) {
        if (rej == null) {
            return null;
        }
        Coord.Grid[] kwadraty = null;
        Coord.Grid[] rejon = poprawRejon(rej);
        if (rejon.length == 1) { //TODO Tu raczej jest blad
            kwadraty = new Coord.Grid[1];
            return kwadraty;
        }
        if (rejon.length == 2) {
            return kwadratyOdcinka(rejon[0].x, rejon[0].y, rejon[1].x, rejon[1].y);
        }
        Coord.Grid kw_min = wspXYOgranDolne(rejon);
        Coord.Grid kw_max = wspXYOgranGorne(rejon);
        int dl = (kw_max.x - kw_min.x + 1) * (kw_max.y - kw_min.y + 1);
        Coord.Grid[] kwadraty_temp = new Coord.Grid[dl];
        // sprawdzenie kazdego kwadratu z duzego obszaru
        // (prostokat opisany na rejonie)
        dl = 0;
        for (int x = kw_min.x; x <= kw_max.x; x++) {
            for (int y = kw_min.y; y <= kw_max.y; y++) {
                if (insidePolygon(rejon, x, y)) {
//                    if (nalezyDoObszaru(rejon, i, j)) {
                    // kwadrat o indeksach (i,j) nalezy do obszaru
                    kwadraty_temp[dl] = new Coord.Grid(x, y);
                    dl++;
                }
            }
        }
        kwadraty = new Coord.Grid[dl];
        System.arraycopy(kwadraty_temp, 0, kwadraty, 0, dl);
//        for (int i = 0; i < kwadraty.length; i++) {
//            kwadraty[i] = new  Coord.GridCoord();
//            kwadraty[i].x = kwadraty_temp[i].x;
//            kwadraty[i].y = kwadraty_temp[i].y;
//        }
        return kwadraty;
    }

    /**
     * Funkcja wyznacza srodek zadanego odcinka
     *
     * @param odc odcinek wejściowy
     * @return współrzędne środka zadanego odcinka
     */
    public static Coord.Grid srodekOdcinka(Coord.Grid[] odc) {
        Coord.Grid srodek = null;
        int xsr = 0, ysr = 0;
        if ((odc != null) && (odc.length > 0)) {
            srodek = new Coord.Grid();
            for (int j = 0; j < odc.length; j++) {
                xsr = xsr + odc[j].x;
                ysr = ysr + odc[j].y;
            }
            srodek.x = (int) xsr / odc.length;
            srodek.y = (int) ysr / odc.length;
        }
        return srodek;
    }

    /**
     * Funkcja wyznacza srodek zadanego odcinka
     *
     * @param pocz początek wejściowego odcinka
     * @param kon  koniec wejsciowego odcinka
     * @return współrzędne środka zadanego odcinka
     */
    public static Coord.Grid srodekOdcinka(Coord.Grid pocz, Coord.Grid kon) {
        Coord.Grid srodek = null;
        int xsr = 0, ysr = 0;
        if ((pocz == null) || (kon == null)) {
            return srodek;
        }
        xsr = (pocz.x + kon.x) / 2;
        ysr = (pocz.y + kon.y) / 2;
        srodek = new Coord.Grid(xsr, ysr);
        return srodek;
    }

    /**
     * Funkcja wyznacza ważony środek odcinka
     *
     * @param pocz początek wejściowego odcinka
     * @param kon  koniec wejsciowego odcinka
     * @param waga waga współrzędnych początku odcinka
     * @return ważone współrzędne środka zadanego odcinka
     */
    public static Coord.Grid srodekOdcinkaWazony(Coord.Grid pocz,
                                                 Coord.Grid kon, float waga) {
        Coord.Grid srodek = null;
        if ((waga < 0.0f) || (waga > 1.0f)) {
            return srodek;
        }
        int xsr = 0, ysr = 0;
        if ((pocz == null) || (kon == null)) {
            return srodek;
        }
        float f_xsr = pocz.x * waga + kon.x * (1.0f - waga);
        float f_ysr = pocz.y * waga + kon.y * (1.0f - waga);
        xsr = (int) f_xsr;
        ysr = (int) f_ysr;
        srodek = new Coord.Grid(xsr, ysr);
        return srodek;
    }

    /**
     * Funkcja wyznacza pozostałą długość drogi odcinka od zadanego punktu tej drogi do jej końca
     *
     * @param droga zadana droga
     * @param pocz  punkt od którego liczona pozostała długość drogi
     * @return pozostała długość odcinka od punktu pocz [m]
     */
    public static float wyznaczPozostalaDlugoscDrogi(ArrayList<Coord.Grid> droga, int pocz) {
        if ((droga == null) || (droga.size() == 0) || (pocz < 0) || (pocz >= droga.size())) {
            return 0.0f;
        }
        double dl = 0.0f;
        Coord.Grid idkw1 = droga.get(pocz);
        Coord.Grid idkw2;
        for (int j = pocz + 1; j < droga.size() - 1; j++) {
            idkw2 = droga.get(j);
            dl += Coord.Grid.distance(idkw1.x, idkw1.y, idkw2.x, idkw2.y);
            idkw1 = idkw2;
        }
        return (float) dl;
    }

    /**
     * Funkcja zwraca odleglosc miedzy srodkami malych kwadratow [m].
     *
     * @param p1 współrzędna malego kwadratu pocz.
     * @param p2 współrzędna malego kwadratu konc.
     * @return Odległość między środkami małych kwadratów [m].
     */
    public static float odleglosc(Coord.Grid p1, Coord.Grid p2) {
        // odleglosc miedzy srodkami malych kwadratow
        if (null == p1 || null == p2) {
            return -1.0f;
        }
        int xx = p1.x - p2.x;
        xx *= xx;
        int yy = p1.y - p2.y;
        yy *= yy;
        float odl = (float) Math.sqrt(xx + yy);
        odl *= MapConsts.SS_SIZE;
        return odl;
    }

    /**
     * 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;
    }

    /**
     * Funkcja zwraca odległość między środkami małych kwadratów [m].
     *
     * @param dx odległość w kwadratach na osi OX.
     * @param dy odległość w kwadratach na osi OY.
     * @return Odległość miedzy srodkami malych kwadratow [m].
     */
    public static float odleglosc(int dx, int dy) {
        float odl = (float) Math.sqrt(dx * dx + dy * dy);
        odl *= MapConsts.SS_SIZE;
        return odl;
    }

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

    public static void main(String[] args) {
        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 = kierunek(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 = kierunek(new Coord.Grid(10, 10), new Coord.Grid(10, 15));
        System.out.println(kier);
        kier = kierunek(new Coord.Grid(10, 10), new Coord.Grid(10, 5));
        System.out.println(kier);
        kier = kierunek(new Coord.Grid(10, 10), new Coord.Grid(15, 10));
        System.out.println(kier);
        kier = kierunek(new Coord.Grid(10, 10), new Coord.Grid(5, 10));
        System.out.println(kier);

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

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

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

        kier = kierunek(new Coord.Grid(10, 10), new Coord.Grid(5, 9));
        System.out.println(kier);
        kier = kierunek(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 = kierunek(new Coord.Grid(10, 10), new Coord.Grid(15, 15));
        System.out.println(kier);
        kier = kierunek(new Coord.Grid(10, 10), new Coord.Grid(15, 5));
        System.out.println(kier);
        kier = kierunek(new Coord.Grid(10, 10), new Coord.Grid(5, 15));
        System.out.println(kier);
        kier = kierunek(new Coord.Grid(10, 10), new Coord.Grid(5, 5));
        System.out.println(kier);

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

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

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

        // PLD-ZAC
        kier = kierunek(new Coord.Grid(10, 10), new Coord.Grid(5, 6));
        System.out.println(kier);
        kier = kierunek(new Coord.Grid(10, 10), new Coord.Grid(5, 4));
        System.out.println(kier);
        kier = kierunek(new Coord.Grid(10, 10), new Coord.Grid(6, 5));
        System.out.println(kier);
        kier = kierunek(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)};
        Coord.Grid[] tab = kwadratyLamanej2(lamana);


        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();
//		}
    }

}