JPS pathfinding.

/*
 * This program is free software: you can redistribute it and/or modify it under
 * the terms of the GNU General Public License as published by the Free Software
 * Foundation, either version 3 of the License, or (at your option) any later
 * version.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
 * details.
 *
 * You should have received a copy of the GNU General Public License along with
 * this program. If not, see <http://www.gnu.org/licenses/>.
 */
package com.java.evacs.geoengine;

import javolution.util.FastList;

import java.util.concurrent.locks.ReentrantLock;

/**
 * ==============================================<br>
 * NodeBuffer - JumpPointSearch pathfinding engine<br>
 * ==============================================<br>
 * @author Deedlit(piotrekloud@gmail.com)
 */
public class NodeBuffer
{
    private final ReentrantLock _lock = new ReentrantLock();
    private final int _mapSize;
    private final Node[][] _buffer;

    private long _timeStamp = 0;
    private long _lastElapsedTime = 0;

    private int startX, startY, endX, endY;

    private int[] tmpXY;
    private int[][] neighbors;

    private float ng;
    private Node tmpNode, cur;
    private Node[] successors, possibleSuccess;
    private FastList<Node> trail;

    private Heap _heap;
    private final Grid _grid;

    public NodeBuffer(int size, Grid grid)
    {
        _mapSize = size;
        _buffer = new Node[_mapSize][_mapSize];
        _heap = new Heap();
        _grid = grid;
    }

    public final boolean lock()
    {
        return _lock.tryLock();
    }

    public final FastList<Node> findPath(int x, int y, int tx, int ty)
    {
        _heap = new Heap();

        startX = x;
        startY = y;
        endX = tx;
        endY = ty;

        getNode(startX, startY).updateGHFP(0, 0, null);
        heapAdd(getNode(startX, startY));
        while (true)
        {
            cur = heapPopNode();
            if (cur.x == endX && cur.y == endY)
            {
                trail = pathCreate(cur);
                return trail;
            }
            possibleSuccess = identifySuccessors(cur);
            for (int i = 0; i < possibleSuccess.length; i++)
            {
                if (possibleSuccess[i] != null)
                {
                    heapAdd(possibleSuccess[i]);
                }
            }
            if (heapSize() == 0)
            {
                break;
            }
        }
        return null;
    }


    /**
     * zwraca węzły do których były skoki z podanego węzła
     *
     * @param node
     *
     * @return
     */
    public Node[] identifySuccessors(Node node)
    {
        successors = new Node[8];
        neighbors = getNeighborsPrune(node);
        for (int i = 0; i < neighbors.length; i++)
        {
            tmpXY = jump(neighbors[i][0], neighbors[i][1], node.x, node.y);
            if (tmpXY[0] != -1)
            {
                int x = tmpXY[0];
                int y = tmpXY[1];
                ng = (toPointApprox(x, y, node.x, node.y) + node.g);
                if (getNode(x, y).f <= 0 || getNode(x, y).g > ng)
                {
                    getNode(x, y).updateGHFP(toPointApprox(x, y, node.x, node.y) + node.g, toPointApprox(x, y, endX, endY), node);
                    successors[i] = getNode(x, y);
                }
            }
        }
        return successors;
    }

    /**
     * metoda rekursywnie szuka w kierunku nadrzędnego położenia (px,py), z obecnego (x,y)
     * zatrzyma się i zwróci aktualną wartość w 3 sytuacjach:
     *
     * 1) aktualna pozycja, to pozycja docelowa (endX, endY)
     * 2) aktualna pozycja to wymuszona sąsiadująca do której można przejść.
     * 3) aktualna jest satysfakcjonująca jako krok do pozycji satysfakcjonującej 1) lub 2)
     *
     * @param x (int)
     * @param y (int)
     * @param px (int)
     * @param py (int)
     *
     * @return (int[]={x, y})
     */
    public int[] jump(int x, int y, int px, int py)
    {
        int[] jx = {-1, -1};
        int[] jy = {-1, -1};
        int dx = (x - px) / Math.max(Math.abs(x - px), 1); //ponieważ nadrzędne nie zawsze są w sąsiedztwie, to jest wykorzystywane w celu znalezienia nadrzędnych -> podrzędnych (dla x)
        int dy = (y - py) / Math.max(Math.abs(y - py), 1); //ponieważ nadrzędne nie zawsze są w sąsiedztwie, to jest wykorzystywane w celu znalezienia nadrzędnych -> podrzędnych (dla y)

        if (!walkable(x, y))
        { //jeśli ta pozycja jest przeszkodą, zwróć null {-1,-1}
            return tmpInt(-1, -1);
        }
        if (x == this.endX && y == this.endY)
        {   //znaleziono punkt docelowy!
            return tmpInt(x, y);
        }
        if (dx != 0 && dy != 0)
        {  //gdy x i y zmieniły się, to poruszamy się po skosie
            if ((walkable(x - dx, y + dy) && !walkable(x - dx, y)) ||
                    (walkable(x + dx, y - dy) && !walkable(x, y - dy)))
            {
                return tmpInt(x, y);
            }
        }
        else
        { //sprawdź w poziomie/pionie
            if (dx != 0)
            { //poruszanie po x
                if ((walkable(x + dx, y + 1) && !walkable(x, y + 1)) ||
                        (walkable(x + dx, y - 1) && !walkable(x, y - 1)))
                {
                    return tmpInt(x, y);
                }
            }
            else
            {
                if ((walkable(x + 1, y + dy) && !walkable(x + 1, y)) ||  //poruszanie po y
                        (walkable(x - 1, y + dy) && !walkable(x - 1, y)))
                {
                    return tmpInt(x, y);
                }
            }
        }

        if (dx != 0 && dy != 0)
        { //gdy poruszamy się po skosie, sprawdź w poszukiwaniu poziomych/pionowych punktów skoku
            jx = jump(x + dx, y, x, y);
            jy = jump(x, y + dy, x, y);
            if (jx[0] != -1 || jy[0] != -1)
            {
                return tmpInt(x, y);
            }
        }
        if (walkable(x + dx, y) || walkable(x, y + dy))
        { //poruszamy się po skosie, należy upewnić się że jeden poziomy/pionowy sąsiad umożliwia ruch
            return jump(x + dx, y + dy, x, y);
        }
        else
        { //jeśli staramy się poruszać po skosie, ale jest blokada na dwóch rogach dotykających sąsiednich węzłów, wracamy wartość null
            return tmpInt(-1, -1);
        }
    }

    /**
     * Zwraca węzły, które należy połączyć szeregowo w zależności od lokalizacji macierzystego, w stosunku do danego węzła.
     *
     * @param node (Node)
     *
     * @return (ArrayList<Node>)
     */
    public int[][] getNeighborsPrune(Node node)
    {
        Node parent = node.parent;    //nadrzędny węzeł
        int x = node.x;
        int y = node.y;
        int px, py, dx, dy;
        int[][] neighbors = new int[5][2];
        //odrzucanie węzłów
        if (parent != null)
        {
            px = parent.x;
            py = parent.y;
            //oblicz znormalizowany kierunek ruchu
            dx = (x - px) / Math.max(Math.abs(x - px), 1);
            dy = (y - py) / Math.max(Math.abs(y - py), 1);
            //szukaj po przekątnych
            if (dx != 0 && dy != 0)
            {
                if (walkable(x, y + dy))
                {
                    neighbors[0] = (tmpInt(x, y + dy));
                }
                if (walkable(x + dx, y))
                {
                    neighbors[1] = (tmpInt(x + dx, y));
                }
                if (walkable(x, y + dy) || walkable(x + dx, y))
                {
                    neighbors[2] = (tmpInt(x + dx, y + dy));
                }
                if (!walkable(x - dx, y) && walkable(x, y + dy))
                {
                    neighbors[3] = (tmpInt(x - dx, y + dy));
                }
                if (!walkable(x, y - dy) && walkable(x + dx, y))
                {
                    neighbors[4] = (tmpInt(x + dx, y - dy));
                }
            }
            else
            {
                if (dx == 0)
                {
                    if (walkable(x, y + dy))
                    {
                        if (walkable(x, y + dy))
                        {
                            neighbors[0] = (tmpInt(x, y + dy));
                        }
                        if (!walkable(x + 1, y))
                        {
                            neighbors[1] = (tmpInt(x + 1, y + dy));
                        }
                        if (!walkable(x - 1, y))
                        {
                            neighbors[2] = (tmpInt(x - 1, y + dy));
                        }
                    }
                }
                else
                {
                    if (walkable(x + dx, y))
                    {
                        if (walkable(x + dx, y))
                        {
                            neighbors[0] = (tmpInt(x + dx, y));
                        }
                        if (!walkable(x, y + 1))
                        {
                            neighbors[1] = (tmpInt(x + dx, y + 1));
                        }
                        if (!walkable(x, y - 1))
                        {
                            neighbors[2] = (tmpInt(x + dx, y - 1));
                        }
                    }
                }
            }
        }
        else
        {//zwróć sąsiadujące
            return getNeighbors(node);
        }

        return neighbors; //zwróć sąsiadujące węzły
    }

    public final void free()
    {
        trail = null;
        tmpXY = null;
        neighbors = null;

        ng = 0;
        tmpNode = null;
        cur = null;
        successors = null;
        possibleSuccess = null;

        _heap = null;

        Node node;
        for (int i = 0; i < _mapSize; i++)
            for (int j = 0; j < _mapSize; j++)
            {
                node = _buffer[i][j];
                if (node != null)
                    node.free();

                _buffer[i][j] = null;
            }

        _lock.unlock();
        _lastElapsedTime = System.currentTimeMillis() - _timeStamp;
    }

    public final long getElapsedTime()
    {
        return _lastElapsedTime;
    }

    public int[][] getNeighbors(Node node)
    {
        int[][] neighbors = new int[8][2];
        int x = node.x;
        int y = node.y;
        boolean d0 = false;
        boolean d1 = false;
        boolean d2 = false;
        boolean d3 = false;

        if (walkable(x, y - 1))
        {
            neighbors[0] = (tmpInt(x, y - 1));
            d0 = d1 = true;
        }
        if (walkable(x + 1, y))
        {
            neighbors[1] = (tmpInt(x + 1, y));
            d1 = d2 = true;
        }
        if (walkable(x, y + 1))
        {
            neighbors[2] = (tmpInt(x, y + 1));
            d2 = d3 = true;
        }
        if (walkable(x - 1, y))
        {
            neighbors[3] = (tmpInt(x - 1, y));
            d3 = d0 = true;
        }
        if (d0 && walkable(x - 1, y - 1))
        {
            neighbors[4] = (tmpInt(x - 1, y - 1));
        }
        if (d1 && walkable(x + 1, y - 1))
        {
            neighbors[5] = (tmpInt(x + 1, y - 1));
        }
        if (d2 && walkable(x + 1, y + 1))
        {
            neighbors[6] = (tmpInt(x + 1, y + 1));
        }
        if (d3 && walkable(x - 1, y + 1))
        {
            neighbors[7] = (tmpInt(x - 1, y + 1));
        }
        return neighbors;
    }

    /**
     * Sprawdzenie czy węzeł x,y można przejść.
     *
     * @param x (int)
     * @param y (int)
     *
     * @return (boolean) true jeśli węzeł nie jest przeszkodą i nie jest poza mapą.
     */
    public boolean walkable(int x, int y)
    {
        try
        {
            return getNode(x, y).pass;
        }
        catch (Exception e)
        {
            return false;
        }
    }

    public FastList<Node> pathCreate(Node node)
    {
        FastList<Node> trail = new FastList<Node>();
        while (node.parent != null)
        {
            try
            {
                trail.add(0, new Node(node));
            }
            catch (Exception e) {}
            node = node.parent;
        }
        return trail;
    }

    public void heapAdd(Node node)
    {
        float[] tmp = {node.x, node.y, node.f};
        _heap.add(tmp);
    }

//--------------------------STOS-----------------------------------//

    /** @return (int) rozmiar stosu */
    public int heapSize()
    {
        return _heap.getSize();
    }

    /** @return (Node) zwraca poprawny węzeł */
    public Node heapPopNode()
    {
        float[] tmp = _heap.pop();
        return getNode((int) tmp[0], (int) tmp[1]);
    }

//---------------------------------------------------------//

    public int[] tmpInt(int x, int y)
    {
        int[] tmpIntsTmpInt = {x, y};  //create the tmpInt's tmpInt[]
        return tmpIntsTmpInt;         //return it
    }

    /**
     * getFunc - węzeł w x, y
     *
     * @param x (int)
     * @param y (int)
     *
     * @return (Node)
     */
    public Node getNode(int x, int y)
    {
        if (_buffer[x][y] != null)
            return _buffer[x][y];
        try
        {
            _buffer[x][y] = new Node(_grid.getNode(x, y));
            return _buffer[x][y];
        }
        catch (Exception e)
        {
            return null;
        }
    }

    public float toPointApprox(float x, float y, int tx, int ty)
    {
        return (float) Math.sqrt(Math.pow(Math.abs(x - tx), 2) + Math.pow(Math.abs(y - ty), 2));
    }
}