package pl.wat.ms4ds.terrain;

import pl.wat.ms4ds.common.EGeoDirection;
import pl.wat.ms4ds.common.ERodzajDzialania;
import pl.wat.ms4ds.common.ERodzajPodwozia;

import java.util.*;

public final class AStar {

    private static class Node {

        int x;
        int y;
        boolean koncowy;
        boolean zamkniety;
        double kosztOdStartu;
        double kosztZHeurystyka;
        Node poprzednik;
        EGeoDirection zKierunku;

        private static Hashtable<Node, Node> allNodes = new Hashtable<>();
        private static Node roboczy = new Node(0, 0);

        static Node dajAStarNode(int x, int y) {
            if (x < 0 || y < 0) {
                return null;
            }
            roboczy.x = x;
            roboczy.y = y;
            Node node = allNodes.get(roboczy);
            if (node == null) {
                node = new Node(x, y);
                allNodes.put(node, node);
            }
            return node;
        }

        ArrayList<Node> dajNiezamknietychSasiadow() {
            ArrayList<Node> wynik = new ArrayList<>();
            Node sasiad;
            sasiad = Node.dajAStarNode(this.x - 1, this.y - 1);
            if (null != sasiad && !sasiad.zamkniety)
                wynik.add(sasiad);
            sasiad = Node.dajAStarNode(this.x - 1, this.y);
            if (null != sasiad && !sasiad.zamkniety)
                wynik.add(sasiad);
            sasiad = Node.dajAStarNode(this.x - 1, this.y + 1);
            if (null != sasiad && !sasiad.zamkniety)
                wynik.add(sasiad);
            sasiad = Node.dajAStarNode(this.x, this.y - 1);
            if (null != sasiad && !sasiad.zamkniety)
                wynik.add(sasiad);
            sasiad = Node.dajAStarNode(this.x, this.y + 1);
            if (null != sasiad && !sasiad.zamkniety)
                wynik.add(sasiad);
            sasiad = Node.dajAStarNode(this.x + 1, this.y - 1);
            if (null != sasiad && !sasiad.zamkniety)
                wynik.add(sasiad);
            sasiad = Node.dajAStarNode(this.x + 1, this.y);
            if (null != sasiad && !sasiad.zamkniety)
                wynik.add(sasiad);
            sasiad = Node.dajAStarNode(this.x + 1, this.y + 1);
            if (null != sasiad && !sasiad.zamkniety)
                wynik.add(sasiad);
            return wynik;
        }

        static void reset() {
            allNodes.clear();
        }

        Node(int x, int y) {
            this.x = x;
            this.y = y;
            koncowy = false;
            zamkniety = false;
            kosztOdStartu = 0.0;
            kosztZHeurystyka = Double.MAX_VALUE;
            poprzednik = null;
            zKierunku = EGeoDirection.UNDEFINED;
        }

        Node(Coord.Grid id) {
            this(id.x, id.y);
        }

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

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

        @Override
        public boolean equals(Object obj) {
            if (this == obj) {
                return true;
            }
            if (obj == null) {
                return false;
            }
            if (obj instanceof Node other) {
                if (x != other.x) {
                    return false;
                }
                return y == other.y;
            }
            return false;
        }
    }

    /**
     * Funkcja wyznacza drogę po sąsiednich kwadratach przechodzącą przez punkty profilujące punktyProfilujace
     *
     * @param punktyProfilujace punkty profilujące drogę (droga musi je zawierać)
     * @param staryKierunek     kierunek z którego osiągnięto punkt startowy (1. punkt profilujący)
     * @param podwozie          rodzaj podwozia
     * @param rodzajDzialania   rodzaj działania, w ramach którego określana jest droga
     * @return uporządkowana kolekcja współrzędnych kolejnych kwadratów drogi zawierająca kwadrat startowy i docelowy lub kolekcja pusta, gdy nie istnieje droga
     */
    public static ArrayList<Coord.Grid> wyznaczDroge(Coord.Grid[] punktyProfilujace, EGeoDirection staryKierunek,
                                                     ERodzajPodwozia podwozie, ERodzajDzialania rodzajDzialania) {
        if (null == punktyProfilujace || 0 == punktyProfilujace.length) {
            return null;
        }
        Node.reset();
        ArrayList<Coord.Grid> wynik = new ArrayList<>();
        Coord.Grid start;
        Coord.Grid stop = punktyProfilujace[0];
        ArrayList<Coord.Grid> odcinek;
        for (int i = 1; i < punktyProfilujace.length; i++) {
            start = stop;
            stop = punktyProfilujace[i];
            EGeoDirection staryKier;
            if (wynik.size() > 1) {
                staryKier = GeomUtils.kierunekDlaSasiada(wynik.get(wynik.size() - 2), wynik.get(wynik.size() - 1));
            } else {
                staryKier = staryKierunek;
            }
            if (i == 1) {
                odcinek = wyznaczDroge(start, stop, staryKier, true, podwozie, rodzajDzialania);
            } else {
                odcinek = wyznaczDroge(start, stop, staryKier, false, podwozie, rodzajDzialania);
            }
            if (odcinek.size() == 0) {
                // gdy nie istnieje droga między danymi punktami profilującymi, to zwracam kolekcję pustą
                return odcinek;
            }
            wynik.addAll(odcinek);
            Node.reset();
        }
        return wynik;
    }

    /**
     * Funkcja wyznacza drogę po sąsiednich kwadratach od kwadratu startowego start do docelowego stop
     *
     * @param start           współrzędne kwadratu startowego
     * @param stop            współrzędne kwadratu docelowego
     * @param staryKierunek   kierunek z którego osiągnięto punkt startowy (1. punkt profilujący)
     * @param podwozie        rodzaj podwozia
     * @param rodzajDzialania rodzaj działania, w ramach którego określana jest droga
     * @param zawieraStartowy parametr wskazujący, czy wyznaczona droga ma zawierać kwadrat startowy
     * @return uporządkowana kolekcja współrzędnych kolejnych kwadratów drogi zawierająca kwadrat startowy (jeśli zawieraStartowy==true) i docelowy lub kolekcja pusta, gdy nie istnieje droga
     */
    public static ArrayList<Coord.Grid> wyznaczDroge(Coord.Grid start, Coord.Grid stop, EGeoDirection staryKierunek,
                                                     boolean zawieraStartowy, ERodzajPodwozia podwozie, ERodzajDzialania rodzajDzialania) {

        PriorityQueue<Node> listaOtwarta = new PriorityQueue<>(100, (o1, o2) -> Double.compare(o1.kosztZHeurystyka, o2.kosztZHeurystyka));
        boolean naPrzelaj = false;
        switch (rodzajDzialania) {
            case DZIALANIE_ATAK:
            case DZIALANIE_MAGIC_MOVEMENT:
            case DZIALANIE_OBRONA:
            case DZIALANIE_OPL:
            case DZIALANIE_WRIA:
            case DZIALANIE_ROZMIESZCZENIE:
            case DZIALANIE_ZESRODKOWANIE:
            case DZIALANIE_MINOWANIE:
                naPrzelaj = true;
                break;
            default:
                break;

        }
        Node aktualny = Node.dajAStarNode(stop.x, stop.y);
        aktualny.koncowy = true;
        ArrayList<Node> sasiedzi;
        aktualny = Node.dajAStarNode(start.x, start.y);
        aktualny.zKierunku = staryKierunek;
        listaOtwarta.add(aktualny);
        ArrayList<Coord.Grid> wynik = new ArrayList<Coord.Grid>();
        int licznik_zabezpieczajacy = 200000;
        while (listaOtwarta.size() > 0 && licznik_zabezpieczajacy-- > 0) {
            aktualny = listaOtwarta.remove();
            if (aktualny.koncowy) {
                while (null != aktualny) {
                    wynik.add(new Coord.Grid(aktualny.x, aktualny.y));
                    aktualny = aktualny.poprzednik;
                }
                if (!zawieraStartowy) {
                    //Usuwam poczatek drogi
                    wynik.remove(wynik.size() - 1);
                }
                Collections.reverse(wynik);
                return wynik;
            }
            sasiedzi = aktualny.dajNiezamknietychSasiadow();
            for (Node sasiad : sasiedzi) {
//                double stopienPrzejezdnosci = Teren.getStopienPrzejezdnosci(aktualny.x, aktualny.y, sasiad.x, sasiad.y,
//                        aktualny.zKierunku, podwozie);
                double stopienPrzejezdnosci = 1;
                if (stopienPrzejezdnosci < 0.005f) {
                    continue;
                }
                if (naPrzelaj) {
                    stopienPrzejezdnosci = Math.max(Teren.minStopienPrzejezdNaPrzelaj, stopienPrzejezdnosci);
                }
                double nowyKosztOdStartu = aktualny.kosztOdStartu + Coord.Grid.distance(aktualny.x, aktualny.y, sasiad.x, sasiad.y) / stopienPrzejezdnosci;
                double nowyKosztZHeurystyka = nowyKosztOdStartu + Coord.Grid.distance(sasiad.x, sasiad.y, stop.x, stop.y);
                if (sasiad.kosztZHeurystyka > nowyKosztZHeurystyka) {
                    //UPDATE kosztow i zmiany w kolejce
                    sasiad.kosztOdStartu = nowyKosztOdStartu;
                    sasiad.kosztZHeurystyka = nowyKosztZHeurystyka;
                    sasiad.poprzednik = aktualny;
                    sasiad.zKierunku = GeomUtils.kierunekDlaSasiada(aktualny.x, aktualny.y, sasiad.x, sasiad.y);
                    listaOtwarta.remove(sasiad);
                    listaOtwarta.add(sasiad);
                }
            }
            aktualny.zamkniety = true;
        }
        return wynik;
    }

    /**
     * Funkcja wyznacza drogę po sąsiednich kwadratach od kwadratu startowego start do docelowego stop
     *
     * @param start           współrzędne kwadratu startowego
     * @param stop            współrzędne kwadratu docelowego
     * @param staryKierunek   kierunek z którego osiągnięto punkt startowy (1. punkt profilujący)
     * @param podwozie        rodzaj podwozia
     * @param rodzajDzialania rodzaj działania, w ramach którego określana jest droga
     * @param zawieraStartowy parametr wskazujący, czy wyznaczona droga ma zawierać kwadrat startowy
     * @return uporządkowana kolekcja współrzędnych kolejnych kwadratów drogi zawierająca kwadrat startowy (jeśli zawieraStartowy==true) i docelowy lub kolekcja pusta, gdy nie istnieje droga
     */
    public static ArrayList<Coord.Grid> wyznaczDrogeNew(Coord.Grid start, Coord.Grid stop, EGeoDirection staryKierunek,
                                                        boolean zawieraStartowy, ERodzajPodwozia podwozie,
                                                        ERodzajDzialania rodzajDzialania,
                                                        double szerokoscPokonywRowow,
                                                        double glebokoscBrodzenia,
                                                        double predkoscPlywania) {
        PriorityQueue<Node> listaOtwarta = new PriorityQueue<>(100, new Comparator<Node>() {
            public int compare(Node o1, Node o2) {
                return Double.compare(o1.kosztZHeurystyka, o2.kosztZHeurystyka);
            }
        });
//        PriorityQueue<AStarNode> listaOtwarta = new PriorityQueue<>(100, (AStarNode o1, AStarNode o2) -> Double.compare(o1.kosztZHeurystyka, o2.kosztZHeurystyka));
        boolean naPrzelaj = false;
        switch (rodzajDzialania) {
            case DZIALANIE_ATAK:
            case DZIALANIE_MAGIC_MOVEMENT:
            case DZIALANIE_OBRONA:
            case DZIALANIE_OPL:
            case DZIALANIE_WRIA:
            case DZIALANIE_ROZMIESZCZENIE:
            case DZIALANIE_ZESRODKOWANIE:
            case DZIALANIE_MINOWANIE:
                naPrzelaj = true;
                break;
            default:
                break;

        }
        Node aktualny = Node.dajAStarNode(stop.x, stop.y);
        aktualny.koncowy = true;
        ArrayList<Node> sasiedzi;
        aktualny = Node.dajAStarNode(start.x, start.y);
        aktualny.zKierunku = staryKierunek;
        listaOtwarta.add(aktualny);
        ArrayList<Coord.Grid> wynik = new ArrayList<Coord.Grid>();
        int licznik_zabezpieczajacy = 200000;
        while (listaOtwarta.size() > 0 && licznik_zabezpieczajacy-- > 0) {
            aktualny = listaOtwarta.remove();
            if (aktualny.koncowy) {
                while (null != aktualny) {
                    wynik.add(new Coord.Grid(aktualny.x, aktualny.y));
                    aktualny = aktualny.poprzednik;
                }
                if (!zawieraStartowy) {
                    //Usuwam poczatek drogi
                    wynik.remove(wynik.size() - 1);
                }
                Collections.reverse(wynik);
                return wynik;
            }
            sasiedzi = aktualny.dajNiezamknietychSasiadow();
            for (Node sasiad : sasiedzi) {
//                double stopienPrzejezdnosci = Teren.getStopienPrzejezdnosciNew(aktualny.x, aktualny.y, sasiad.x, sasiad.y,
//                        aktualny.zKierunku, podwozie, szerokoscPokonywRowow, glebokoscBrodzenia, predkoscPlywania);
                double stopienPrzejezdnosci = 1;
                if (stopienPrzejezdnosci < 0.005f) {
                    continue;
                }
                if (naPrzelaj) {
                    stopienPrzejezdnosci = Math.max(Teren.minStopienPrzejezdNaPrzelaj, stopienPrzejezdnosci);
                }
                double nowyKosztOdStartu = aktualny.kosztOdStartu + Coord.Grid.distance(aktualny.x, aktualny.y, sasiad.x, sasiad.y) / stopienPrzejezdnosci;
                double nowyKosztZHeurystyka = nowyKosztOdStartu + Coord.Grid.distance(sasiad.x, sasiad.y, stop.x, stop.y);
                if (sasiad.kosztZHeurystyka > nowyKosztZHeurystyka) {
                    //UPDATE kosztow i zmiany w kolejce
                    sasiad.kosztOdStartu = nowyKosztOdStartu;
                    sasiad.kosztZHeurystyka = nowyKosztZHeurystyka;
                    sasiad.poprzednik = aktualny;
                    sasiad.zKierunku = GeomUtils.kierunekDlaSasiada(aktualny.x, aktualny.y, sasiad.x, sasiad.y);
                    listaOtwarta.remove(sasiad);
                    listaOtwarta.add(sasiad);
                }
            }
            aktualny.zamkniety = true;
        }
        return wynik;
    }

}