public class AStar extends PathFindingAlgorithm {

    protected boolean postSmoothingOn = false;
    protected boolean repeatedPostSmooth = true;
    protected float heuristicWeight = 1f;

    protected Float[] distance;
    protected boolean[] visited;
    
    protected IndirectHeap<Float> pq; 

    protected int finish;

    
    public AStar(GridGraph graph, int sx, int sy, int ex, int ey) {
        super(graph, graph.sizeX, graph.sizeY, sx, sy, ex, ey);
    }

    public static AStar postSmooth(GridGraph graph, int sx, int sy, int ex, int ey) {
        AStar aStar = new AStar(graph, sx, sy, ex, ey);
        aStar.postSmoothingOn = true;
        return aStar;
    }

    public static AStar dijkstra(GridGraph graph, int sx, int sy, int ex, int ey) {
        AStar aStar = new AStar(graph, sx, sy, ex, ey);
        aStar.heuristicWeight = 0;
        return aStar;
    }
    
    @Override
    public void computePath() {
        int totalSize = (graph.sizeX+1) * (graph.sizeY+1);

        int start = toOneDimIndex(sx, sy);
        finish = toOneDimIndex(ex, ey);
        
        distance = new Float[totalSize];
        parent = new int[totalSize];
        
        initialise(start);
        visited = new boolean[totalSize];
        
        pq = new IndirectHeap<Float>(distance, true);
        pq.heapify();
        
        //float lastDist = -1;
        while (!pq.isEmpty()) {
            //float dist = pq.getMinValue();
            
            int current = pq.popMinIndex();
            
            maybeSaveSearchSnapshot();
            //if (Math.abs(dist - lastDist) > 0.01f) { maybeSaveSearchSnapshot(); lastDist = dist;}
            
            if (current == finish || distance[current] == Float.POSITIVE_INFINITY) {
                //maybeSaveSearchSnapshot();
                break;
            }
            visited[current] = true;

            int x = toTwoDimX(current);
            int y = toTwoDimY(current);

            tryRelaxNeighbour(current, x, y, x-1, y-1);
            tryRelaxNeighbour(current, x, y, x, y-1);
            tryRelaxNeighbour(current, x, y, x+1, y-1);
            
            tryRelaxNeighbour(current, x, y, x-1, y);
            tryRelaxNeighbour(current, x, y, x+1, y);
            
            tryRelaxNeighbour(current, x, y, x-1, y+1);
            tryRelaxNeighbour(current, x, y, x, y+1);
            tryRelaxNeighbour(current, x, y, x+1, y+1);

            //maybeSaveSearchSnapshot();
        }
        
        maybePostSmooth();
    }
    
    protected void tryRelaxNeighbour(int current, int currentX, int currentY, int x, int y) {
        if (!graph.isValidCoordinate(x, y))
            return;
        
        int destination = toOneDimIndex(x,y);
        if (visited[destination])
            return;
        if (!graph.neighbourLineOfSight(currentX, currentY, x, y))
            return;
        
        if (relax(current, destination, weight(currentX, currentY, x, y))) {
            // If relaxation is done.
            pq.decreaseKey(destination, distance[destination] + heuristic(x,y));
        }
    }

    protected float heuristic(int x, int y) {
        //return 0;
        return heuristicWeight*graph.distance(x, y, ex, ey);
    }


    protected float weight(int x1, int y1, int x2, int y2) {
        return graph.distance(x1, y1, x2, y2);
    }
    

    public Float[] getDistance(){return distance;}
    public int[] getParent(){return parent;}
    
    protected boolean relax(int u, int v, float weightUV) {
        // return true iff relaxation is done.
        
        float newWeight = distance[u] + weightUV;
        if (newWeight < distance[v]) {
            distance[v] = newWeight;
            parent[v] = u;
            //maybeSaveSearchSnapshot();
            return true;
        }
        return false;
    }
    
    
    protected final void initialise(int s) {
        for (int i=0; i<distance.length; i++) {
            distance[i] = Float.POSITIVE_INFINITY;
            parent[i] = -1;
        }
        distance[s] = 0f;
    }
    
    private int pathLength() {
        int length = 0;
        int current = finish;
        while (current != -1) {
            current = parent[current];
            length++;
        }
        return length;
    }
    
    protected boolean lineOfSight(int node1, int node2) {
        int x1 = toTwoDimX(node1);
        int y1 = toTwoDimY(node1);
        int x2 = toTwoDimX(node2);
        int y2 = toTwoDimY(node2);
        return graph.lineOfSight(x1, y1, x2, y2);
    }

    protected float physicalDistance(int node1, int node2) {
        int x1 = toTwoDimX(node1);
        int y1 = toTwoDimY(node1);
        int x2 = toTwoDimX(node2);
        int y2 = toTwoDimY(node2);
        return graph.distance(x1, y1, x2, y2);
    }

    protected float physicalDistance(int x1, int y1, int node2) {
        int x2 = toTwoDimX(node2);
        int y2 = toTwoDimY(node2);
        return graph.distance(x1, y1, x2, y2);
    }
    
    protected void maybePostSmooth() {
        if (postSmoothingOn) {
            if (repeatedPostSmooth) {
                while(postSmooth());
            } else {
                postSmooth();
            }
        }
    }
    
    private boolean postSmooth() {
        boolean didSomething = false;

        int current = finish;
        while (current != -1) {
            int next = parent[current]; // we can skip checking this one as it always has LoS to current.
            if (next != -1) {
                next = parent[next];
                while (next != -1) {
                    if (lineOfSight(current,next)) {
                        parent[current] = next;
                        next = parent[next];
                        didSomething = true;
                        maybeSaveSearchSnapshot();
                    } else {
                        next = -1;
                    }
                }
            }
            
            current = parent[current];
        }
        
        return didSomething;
    }
    

    public int[][] getPath() {
        int length = pathLength();
        int[][] path = new int[length][];
        int current = finish;
        
        int index = length-1;
        while (current != -1) {
            int x = toTwoDimX(current);
            int y = toTwoDimY(current);
            
            path[index] = new int[2];
            path[index][0] = x;
            path[index][1] = y;
            
            index--;
            current = parent[current];
        }
        
        return path;
    }
    
    public float getPathLength() {
        int current = finish;
        if (current == -1) return -1;
        
        float pathLength = 0;
        
        int prevX = toTwoDimX(current);
        int prevY = toTwoDimY(current);
        current = parent[current];
        
        while (current != -1) {
            int x = toTwoDimX(current);
            int y = toTwoDimY(current);
            
            pathLength += graph.distance(x, y, prevX, prevY);
            
            current = parent[current];
            prevX = x;
            prevY = y;
        }
        
        return pathLength;
    }
    
    @Override
    protected boolean selected(int index) {
        return visited[index];
    }
} public class BasicThetaStar extends AStarStaticMemory {
    
    public BasicThetaStar(GridGraph graph, int sx, int sy, int ex, int ey) {
        super(graph, sx, sy, ex, ey);
    }
    
    public static BasicThetaStar postSmooth(GridGraph graph, int sx, int sy, int ex, int ey) {
        BasicThetaStar bts = new BasicThetaStar(graph, sx, sy, ex, ey);
        bts.postSmoothingOn = true;
        return bts;
    }
    
    public static BasicThetaStar noHeuristic(GridGraph graph, int sx, int sy, int ex, int ey) {
        BasicThetaStar bts = new BasicThetaStar(graph, sx, sy, ex, ey);
        bts.heuristicWeight = 0;
        return bts;
    }

    @Override
    protected void tryRelaxNeighbour(int current, int currentX, int currentY, int x, int y) {
        if (!graph.isValidCoordinate(x, y))
            return;
        
        int destination = toOneDimIndex(x,y);
        if (visited(destination))
            return;
        if (parent(current) != -1 && parent(current) == parent(destination)) // OPTIMISATION: [TI]
            return; // Idea: don't bother trying to relax if parents are equal. using triangle inequality.
        if (!graph.neighbourLineOfSight(currentX, currentY, x, y))
            return;
        
        if (relax(current, destination, 0)) {
            // If relaxation is done.
            pq.decreaseKey(destination, distance(destination) + heuristic(x,y));
        }
    }

    @Override
    protected boolean relax(int u, int v, float weightUV) {
        // return true iff relaxation is done.
        
        if (lineOfSight(parent(u), v)) {
            u = parent(u);
            
            float newWeight = distance(u) + physicalDistance(u, v);
            if (newWeight < distance(v)) {
                setDistance(v, newWeight);
                setParent(v, u);
                return true;
            }
            return false;
        } else {
            float newWeight = distance(u) + physicalDistance(u, v);
            if (newWeight < distance(v)) {
                setDistance(v, newWeight);
                setParent(v, u);
                return true;
            }
            return false;
        }
    }
    
} public final class JumpPointSearch extends AStarStaticMemory {
    private final int[] neighboursdX;
    private final int[] neighboursdY;
    private int neighbourCount;

    public JumpPointSearch(GridGraph graph, int sx, int sy, int ex, int ey) {
        super(graph, sx, sy, ex, ey);
        neighboursdX = new int[8];
        neighboursdY = new int[8];
    }
    
    public static final JumpPointSearch postSmooth(GridGraph graph, int sx, int sy, int ex, int ey) {
        JumpPointSearch algo = new JumpPointSearch(graph, sx, sy, ex, ey);
        algo.postSmoothingOn = true;
        algo.repeatedPostSmooth = false;
        return algo;
    }
    
    public static final JumpPointSearch repeatedPostSmooth(GridGraph graph, int sx, int sy, int ex, int ey) {
        JumpPointSearch algo = new JumpPointSearch(graph, sx, sy, ex, ey);
        algo.postSmoothingOn = true;
        algo.repeatedPostSmooth = true;
        return algo;
    }

    @Override
    public final void computePath() {
        neighbourCount = 0;
        
        int totalSize = (graph.sizeX+1) * (graph.sizeY+1);

        int start = graph.toOneDimIndex(sx, sy);
        finish = graph.toOneDimIndex(ex, ey);
        
        pq = new ReusableIndirectHeap(totalSize);
        this.initialiseMemory(totalSize, Float.POSITIVE_INFINITY, -1, false);
        
        initialise(start);
        
        while (!pq.isEmpty()) {
            int current = pq.popMinIndex();
            if (current == finish || distance(current) == Float.POSITIVE_INFINITY) {
                maybeSaveSearchSnapshot();
                break;
            }
            setVisited(current, true);

            int x = graph.toTwoDimX(current);
            int y = graph.toTwoDimY(current);
            
            computeNeighbours(current, x, y); // stores neighbours in attribute.

            for (int i=0;i<neighbourCount;++i) {
                int dx = neighboursdX[i];
                int dy = neighboursdY[i];

                int successor = jump(x, y, dx, dy);
                if (successor != -1) {
                    tryRelax(current, x, y, successor);
                }
            }
            
            maybeSaveSearchSnapshot();
        }

        maybePostSmooth();
    }
    
    private final int jump(int x, int y, int dx, int dy) {
        if (dx < 0) {
            if (dy < 0) {
                return jumpDL(x,y);
            } else if (dy > 0) {
                return jumpUL(x,y);
            } else {
                return jumpL(x,y);
            }
        } else if (dx > 0) {
            if (dy < 0) {
                return jumpDR(x,y);
            } else if (dy > 0) {
                return jumpUR(x,y);
            } else {
                return jumpR(x,y);
            }
        } else {
            if (dy < 0) {
                return jumpD(x,y);
            } else {
                return jumpU(x,y);
            }
        }
        
    }
    
    private final int jumpDL(int x, int y) {
        while(true) {
            x -= 1;
            y -= 1;
            if (graph.isBlocked(x, y)) return -1;
            if (x == ex && y == ey) return graph.toOneDimIndex(x,y);
            // diagonal cannot be forced on vertices.
            if (jumpL(x,y) != -1) return graph.toOneDimIndex(x,y);
            if (jumpD(x,y) != -1) return graph.toOneDimIndex(x,y);
        }
    }
    
    private final int jumpDR(int x, int y) {
        while(true) {
            x += 1;
            y -= 1;
            if (graph.isBlocked(x-1, y)) return -1;
            if (x == ex && y == ey) return graph.toOneDimIndex(x,y);
            // diagonal cannot be forced on vertices.
            if (jumpD(x,y) != -1) return graph.toOneDimIndex(x,y);
            if (jumpR(x,y) != -1) return graph.toOneDimIndex(x,y);
        }
    }
    
    private final int jumpUL(int x, int y) {
        while(true) {
            x -= 1;
            y += 1;
            if (graph.isBlocked(x, y-1)) return -1;
            if (x == ex && y == ey) return graph.toOneDimIndex(x,y);
            // diagonal cannot be forced on vertices.
            if (jumpL(x,y) != -1) return graph.toOneDimIndex(x,y);
            if (jumpU(x,y) != -1) return graph.toOneDimIndex(x,y);
        }
    }
    
    private final int jumpUR(int x, int y) {
        while(true) {
            x += 1;
            y += 1;
            if (graph.isBlocked(x-1, y-1)) return -1;
            if (x == ex && y == ey) return graph.toOneDimIndex(x,y);
            // diagonal cannot be forced on vertices.
            if (jumpU(x,y) != -1) return graph.toOneDimIndex(x,y);
            if (jumpR(x,y) != -1) return graph.toOneDimIndex(x,y);
        }
    }
    
    private final int jumpL(int x, int y) {
        while(true) {
            x -= 1;
            if (graph.isBlocked(x, y)) {
                if (graph.isBlocked(x, y-1)) {
                    return -1;
                } else {
                    if (!graph.isBlocked(x-1, y)) return graph.toOneDimIndex(x,y);
                }
            }
            if (graph.isBlocked(x, y-1)) {
                if (!graph.isBlocked(x-1, y-1)) return graph.toOneDimIndex(x,y);
            }
            if (x == ex && y == ey) return graph.toOneDimIndex(x,y);
        }
    }
    
    private final int jumpR(int x, int y) {
        while(true) {
            x += 1;
            if (graph.isBlocked(x-1, y)) {
                if (graph.isBlocked(x-1, y-1)) {
                    return -1;
                } else {
                    if (!graph.isBlocked(x, y)) return graph.toOneDimIndex(x,y);
                }
            }
            if (graph.isBlocked(x-1, y-1)) {
                if (!graph.isBlocked(x, y-1)) return graph.toOneDimIndex(x,y);
            }
            if (x == ex && y == ey) return graph.toOneDimIndex(x,y);
        }
    }
    
    private final int jumpD(int x, int y) {
        while(true) {
            y -= 1;
            if (graph.isBlocked(x, y)) {
                if (graph.isBlocked(x-1, y)) {
                    return -1;
                } else {
                    if (!graph.isBlocked(x, y-1)) return graph.toOneDimIndex(x,y);
                }
            }
            if (graph.isBlocked(x-1, y)) {
                if (!graph.isBlocked(x-1, y-1)) return graph.toOneDimIndex(x,y);
            }
            if (x == ex && y == ey) return graph.toOneDimIndex(x,y);
        }
    }
    
    private final int jumpU(int x, int y) {
        while(true) {
            y += 1;
            if (graph.isBlocked(x, y-1)) {
                if (graph.isBlocked(x-1, y-1)) {
                    return -1;
                } else {
                    if (!graph.isBlocked(x, y)) return graph.toOneDimIndex(x,y);
                }
            }
            if (graph.isBlocked(x-1, y-1)) {
                if (!graph.isBlocked(x-1, y)) return graph.toOneDimIndex(x,y);
            }
            if (x == ex && y == ey) return graph.toOneDimIndex(x,y);
        }
    }

    private final void computeNeighbours(int currentIndex, int cx, int cy) {
        neighbourCount = 0;

        int parentIndex = parent(currentIndex);
        if (parentIndex == -1) {
            // is start node.
            for (int y=-1;y<=1;++y) {
                for (int x=-1;x<=1;++x) {
                    if (x == 0 && y == 0) continue;
                    int px = cx+x;
                    int py = cy+y;
                    if (graph.neighbourLineOfSight(cx,cy,px,py)) {
                        addNeighbour(x, y);
                    }
                }
            }
            return;
        }

        int dirX = cx - graph.toTwoDimX(parentIndex);
        int dirY = cy - graph.toTwoDimY(parentIndex);

        if (dirX < 0) {
            if (dirY < 0) {
                // down-left
                if (!graph.isBlocked(cx-1, cy-1)) {
                    addNeighbour(-1,-1);
                    addNeighbour(-1,0);
                    addNeighbour(0,-1);
                } else {
                    if (!graph.isBlocked(cx-1, cy)) addNeighbour(-1,0);
                    if (!graph.isBlocked(cx, cy-1)) addNeighbour(0,-1);
                }
            } else if (dirY > 0) {
                // up-left
                if (!graph.isBlocked(cx-1, cy)) {
                    addNeighbour(-1,1);
                    addNeighbour(-1,0);
                    addNeighbour(0,1);
                } else {
                    if (!graph.isBlocked(cx-1, cy-1)) addNeighbour(-1,0);
                    if (!graph.isBlocked(cx, cy)) addNeighbour(0,1);
                }
            } else {
                // left
                if (graph.isBlocked(cx,cy)) {
                    //assert !graph.isBlocked(cx-1, cy);
                    addNeighbour(-1,1);
                    addNeighbour(0,1);
                    addNeighbour(-1,0);
                } else {
                    //assert graph.isBlocked(cx,cy-1);
                    //assert !graph.isBlocked(cx-1, cy-1);
                    addNeighbour(-1,-1);
                    addNeighbour(0,-1);
                    addNeighbour(-1,0);
                }
            }
        } else if (dirX > 0) {
            if (dirY < 0) {
                // down-right
                if (!graph.isBlocked(cx, cy-1)) {
                    addNeighbour(1,-1);
                    addNeighbour(1,0);
                    addNeighbour(0,-1);
                } else {
                    if (!graph.isBlocked(cx, cy)) addNeighbour(1,0);
                    if (!graph.isBlocked(cx-1, cy-1)) addNeighbour(0,-1);
                }
            } else if (dirY > 0) {
                // up-right
                if (!graph.isBlocked(cx, cy)) {
                    addNeighbour(1,1);
                    addNeighbour(1,0);
                    addNeighbour(0,1);
                } else {
                    if (!graph.isBlocked(cx, cy-1)) addNeighbour(1,0);
                    if (!graph.isBlocked(cx-1, cy)) addNeighbour(0,1);
                }
            } else {
                // right
                if (graph.isBlocked(cx-1,cy)) {
                    //assert !graph.isBlocked(cx, cy);
                    addNeighbour(1,1);
                    addNeighbour(0,1);
                    addNeighbour(1,0);
                } else {
                    //assert graph.isBlocked(cx-1,cy-1);
                    //assert !graph.isBlocked(cx, cy-1);
                    addNeighbour(1,-1);
                    addNeighbour(0,-1);
                    addNeighbour(1,0);
                }
            }
        } else {
            if (dirY < 0) {
                // down
                if (graph.isBlocked(cx,cy)) {
                    //assert !graph.isBlocked(cx, cy-1);
                    addNeighbour(1,-1);
                    addNeighbour(1,0);
                    addNeighbour(0,-1);
                } else {
                    //assert graph.isBlocked(cx-1,cy);
                    //assert !graph.isBlocked(cx-1, cy-1);
                    addNeighbour(-1,-1);
                    addNeighbour(-1,0);
                    addNeighbour(0,-1);
                }
            } else { //dirY > 0
                // up
                if (graph.isBlocked(cx,cy-1)) {
                    //assert !graph.isBlocked(cx, cy);
                    addNeighbour(1,1);
                    addNeighbour(1,0);
                    addNeighbour(0,1);
                } else {
                    //assert graph.isBlocked(cx-1,cy-1);
                    //assert !graph.isBlocked(cx-1, cy);
                    addNeighbour(-1,1);
                    addNeighbour(-1,0);
                    addNeighbour(0,1);
                }
            }
        }
    }
    
    private final void addNeighbour(int x, int y) {
        neighboursdX[neighbourCount] = x;
        neighboursdY[neighbourCount] = y;
        neighbourCount++;
    }

    private final void tryRelax(int current, int currX, int currY, int destination) {
        if (visited(destination)) return;

        int destX = graph.toTwoDimX(destination);
        int destY = graph.toTwoDimY(destination);
        
        if (relax(current, destination, graph.octileDistance(currX, currY, destX, destY))) {
            // If relaxation is done.
            pq.decreaseKey(destination, distance(destination) + graph.octileDistance(destX, destY, ex, ey));
        }
    }
} public final class Point {
    public final int x;
    public final int y;
    
    public Point(int x, int y) {
        this.x = x;
        this.y = y;
    }

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

    @Override
    public boolean equals(Object obj) {
        Point other = (Point) obj;
        return x == other.x && y == other.y;
    }
    
    @Override
    public String toString() {
        return "(" + x + "," + y + ")";
    }
    
} public class SnapshotItem {
    private static HashMap<SnapshotItem,SnapshotItem> cached;
    public final Integer[] path;
    public final Color color;
    
    private SnapshotItem(Integer[] path, Color color) {
        this.path = path;
        this.color = color;
    }
    
    public static SnapshotItem generate(Integer[] path, Color color) {
        return getCached(new SnapshotItem(path, color));
    }
    
    public static SnapshotItem generate(Integer[] path) {
        return getCached(new SnapshotItem(path, null));
    }
    
    public static void clearCached() {
        if (cached == null) return;
        cached.clear();
        cached = null;
    }
    
    private static SnapshotItem getCached(SnapshotItem item) {
        if (cached == null) {
            cached = new HashMap<>();
        }
        SnapshotItem get = cached.get(item);
        if (get == null) {
            cached.put(item, item);
            return item;
        } else { 
            return get;
        }  
    }

    @Override
    public int hashCode() {
        final int prime = 31;
        int result = 1;
        result = prime * result + ((color == null) ? 0 : color.hashCode());
        result = prime * result + path[0];
        result = prime * result + Arrays.hashCode(path);
        return result;
    }

    @Override
    public boolean equals(Object obj) {
        if (this == obj)
            return true;
        if (obj == null)
            return false;
        if (getClass() != obj.getClass())
            return false;
        SnapshotItem other = (SnapshotItem) obj;
        if (color == null) {
            if (other.color != null)
                return false;
        } else if (!color.equals(other.color))
            return false;
        if (!Arrays.equals(path, other.path))
            return false;
        return true;
    }
} public final class LineOfSightScanner {
    public static ArrayList<List<SnapshotItem>> snapshotList = new ArrayList<>();
    private static ArrayList<SnapshotItem> snapshots = new ArrayList<>();
    private static int snapshot_sx;
    private static int snapshot_sy;
    
    private final GridGraph graph;
    private final int sizeX;
    private final int sizeY;
    
    private static int[][] rightDownExtents;
    private static int[][] leftDownExtents;
    private static LOSInterval[] intervalStack;
    private static int intervalStackSize;
    
    public static int[] successorsX;
    public static int[] successorsY;
    public static int nSuccessors;

    private static void initialiseExtents(GridGraph graph) {
        // Don't reinitialise if graph is the same size as the last time.
        if (rightDownExtents != null && graph.sizeY+2 == rightDownExtents.length && graph.sizeX+1 == rightDownExtents[0].length) return;

        rightDownExtents = new int[graph.sizeY+2][];
        leftDownExtents = new int[graph.sizeY+2][];
        for (int y=0;y<graph.sizeY+2;++y) {
            rightDownExtents[y] = new int[graph.sizeX+1];
            leftDownExtents[y] = new int[graph.sizeX+1];
        }
    }
    
    private static void initialiseStack() {
        if (intervalStack != null) return;
        intervalStack = new LOSInterval[11];
        intervalStackSize = 0;
    }
    
    private static void initialiseSuccessorList() {
        if (successorsX != null) return;
        successorsX = new int[11];
        successorsY = new int[11];
        nSuccessors = 0;
    }
    
    private static final void clearSuccessors() {
        nSuccessors = 0;
    }
    
    private static final void stackPush(LOSInterval interval) {
        if (intervalStackSize >= intervalStack.length) {
            intervalStack = Arrays.copyOf(intervalStack, intervalStack.length*2);
        }
        intervalStack[intervalStackSize] = interval;
        ++intervalStackSize;
        
        //addToSnapshot(interval); // Uncomment for debugging.
    }
    
    private static final void addToSnapshot(LOSInterval interval) {
        snapshots.add(SnapshotItem.generate(new Integer[]{interval.y, interval.xL.n, interval.xL.d, interval.xR.n, interval.xR.d, snapshot_sx, snapshot_sy}, Color.GREEN));
        snapshotList.add(new ArrayList<SnapshotItem>(snapshots));
    }
    
    public static final void clearSnapshots() {
        snapshotList.clear();
        snapshots.clear();
    }
    
    private static final LOSInterval stackPop() {
        LOSInterval temp = intervalStack[intervalStackSize-1];
        --intervalStackSize;
        intervalStack[intervalStackSize] = null;
        return temp;
    }
    
    private static final void clearStack() {
        intervalStackSize = 0;
    }
    
    private static final void addSuccessor(int x, int y) {
        if (nSuccessors >= successorsX.length) {
            successorsX = Arrays.copyOf(successorsX, successorsX.length*2);
            successorsY = Arrays.copyOf(successorsY, successorsY.length*2);
        }
        successorsX[nSuccessors] = x;
        successorsY[nSuccessors] = y;
        ++nSuccessors;
    }
    
    public LineOfSightScanner(GridGraph gridGraph) {
        initialiseExtents(gridGraph);
        initialiseSuccessorList();
        initialiseStack();
        
        graph = gridGraph;
        sizeX = graph.sizeX;
        sizeY = graph.sizeY;
        computeExtents();
    }
    
    private void computeExtents() {
        // graph.isBlocked(x,y) is the same as graph.bottomLeftOfBlockedTile(x,y)
        LineOfSightScanner.initialiseExtents(graph);
        
        for (int y=0;y<sizeY+2;++y) {
            boolean lastIsBlocked = true;
            int lastX = -1;
            for (int x=0;x<=sizeX;++x) {
                leftDownExtents[y][x] = lastX; 
                if (graph.isBlocked(x, y-1) != lastIsBlocked) {
                    lastX = x;
                    lastIsBlocked = !lastIsBlocked;
                }
            }
            lastIsBlocked = true;
            lastX = sizeX+1;
            for (int x=sizeX;x>=0;--x) {
                rightDownExtents[y][x] = lastX; 
                if (graph.isBlocked(x-1, y-1) != lastIsBlocked) {
                    lastX = x;
                    lastIsBlocked = !lastIsBlocked;
                }
            }
        }
    }
    
    /**
     * Stores results in successorsX, successorsY and nSuccessors. 
     */
    public final void computeAllVisibleSuccessors(int sx, int sy) {
        snapshot_sx=sx;snapshot_sy=sy;
        clearSuccessors();
        clearStack();

        generateStartingStates(sx, sy);
        exploreStatesNonTaut(sx, sy);
    }

    /**
     * Stores results in successorsX, successorsY and nSuccessors. 
     */
    public final void computeAllVisibleTautSuccessors(int sx, int sy) {
        snapshot_sx=sx;snapshot_sy=sy;
        clearSuccessors();
        clearStack();

        generateStartingStates(sx, sy);
        exploreStates(sx, sy);
    }

    /**
     * Stores results in successorsX, successorsY and nSuccessors. 
     */
    public final void computeAllVisibleTwoWayTautSuccessors(int sx, int sy) {
        snapshot_sx=sx;snapshot_sy=sy;
        clearSuccessors();
        clearStack();

        generateTwoWayTautStartingStates(sx, sy);
        exploreStates(sx, sy);
    }
    
    /**
     * Stores results in successorsX, successorsY and nSuccessors. 
     * We are moving in direction dx, dy
     */
    public final void computeAllVisibleIncrementalTautSuccessors(int sx, int sy, int dx, int dy) {
        snapshot_sx=sx;snapshot_sy=sy;
        clearSuccessors();
        clearStack();

        generateIncrementalTautStartingStates(sx, sy, dx, dy);
        exploreStates(sx, sy);
    }


    /**
     * Assumption: We are at an outer corner. One of six cases:
     *   BR        BL        TR        TL       TRBL      TLBR
     * XXX|         |XXX      :         :         |XXX   XXX|
     * XXX|...   ...|XXX   ___:...   ...:___   ___|XXX   XXX|___
     *    :         :      XXX|         |XXX   XXX|         |XXX
     *    :         :      XXX|         |XXX   XXX|         |XXX
     *    
     * Assumption: We are also entering from a taut direction.
     * dx > 0, dy > 0 : BR TL
     * dx > 0, dy < 0 : BL TR
     * dx < 0, dy < 0 : BR TL
     * dx < 0, dy > 0 : BL TR
     */
    private final void generateIncrementalTautStartingStates(int sx, int sy, int dx, int dy) {
        boolean rightwardsSearch = false;
        boolean leftwardsSearch = false;
        
        if (dx > 0) {
            // Moving rightwards
            if (dy > 0) {
                //    P
                //   /
                //  B
                boolean brOfBlocked = graph.bottomRightOfBlockedTile(sx, sy);
                boolean tlOfBlocked = graph.topLeftOfBlockedTile(sx, sy);
                
                int rightBound = rightUpExtent(sx,sy);
                Fraction leftExtent;
                Fraction rightExtent;
                
                if (brOfBlocked && tlOfBlocked) {
                    //  |
                    //  |___
                    
                    leftExtent = new Fraction(sx);
                    rightExtent = new Fraction(rightBound);
                    
                    rightwardsSearch = true;
                } else if (brOfBlocked) {
                    //  | /
                    //  |/
                    
                    leftExtent = new Fraction(sx);
                    rightExtent = new Fraction(sx*dy + dx, dy);
                    if (!rightExtent.isLessThanOrEqual(rightBound)) { // rightBound < rightExtent
                        rightExtent = new Fraction(rightBound);
                    }
                    
                } else { // tlOfBlocked
                    //   /
                    //  /__
                    
                    leftExtent = new Fraction(sx*dy + dx, dy);
                    rightExtent = new Fraction(rightBound);
                    
                    rightwardsSearch = true;
                }
                
                if (leftExtent.isLessThanOrEqual(rightExtent)) {
                    this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                }
                
            } else if (dy < 0) {
                //  B
                //   \
                //    P
                boolean trOfBlocked = graph.topRightOfBlockedTile(sx, sy);
                boolean blOfBlocked = graph.bottomLeftOfBlockedTile(sx, sy);
                
                int rightBound = rightDownExtent(sx,sy);
                Fraction leftExtent;
                Fraction rightExtent;
                
                if (trOfBlocked && blOfBlocked) {
                    //  ____
                    //  |
                    //  |
                    
                    leftExtent = new Fraction(sx);
                    rightExtent = new Fraction(rightBound);
                    
                    rightwardsSearch = true;
                } else if (trOfBlocked) {
                    //  .
                    //  |\
                    //  | \
                    
                    leftExtent = new Fraction(sx);
                    rightExtent = new Fraction(sx*-dy + dx, -dy);
                    if (!rightExtent.isLessThanOrEqual(rightBound)) { // rightBound < rightExtent
                        rightExtent = new Fraction(rightBound);
                    }
                    
                } else { // blOfBlocked
                    //  ___
                    //  \
                    //   \
                    leftExtent = new Fraction(sx*-dy + dx, -dy);
                    rightExtent = new Fraction(rightBound);
                    
                    rightwardsSearch = true;
                }
                
                if (leftExtent.isLessThanOrEqual(rightExtent)) {
                    this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                }
                
                
            } else { // dy == 0
                
                // B--P

                if (graph.bottomRightOfBlockedTile(sx, sy)) {
                    // |
                    // |___

                    Fraction leftExtent = new Fraction(sx);
                    Fraction rightExtent = new Fraction(rightUpExtent(sx,sy));
                    this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                    
                } else if (graph.topRightOfBlockedTile(sx, sy)) { // topRightOfBlockedTile
                    // ____
                    // |
                    // |

                    Fraction leftExtent = new Fraction(sx);
                    Fraction rightExtent = new Fraction(rightDownExtent(sx,sy));
                    this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                }
                
                rightwardsSearch = true;
            }
            
        } else if (dx < 0) {
            // Moving leftwards
            
            if (dy > 0) {
                //  B
                //   \
                //    P
                boolean blOfBlocked = graph.bottomLeftOfBlockedTile(sx, sy);
                boolean trOfBlocked = graph.topRightOfBlockedTile(sx, sy);
                
                int leftBound = leftUpExtent(sx,sy);
                Fraction leftExtent;
                Fraction rightExtent;
                
                if (blOfBlocked && trOfBlocked) {
                    //     |
                    //  ___|
                    
                    leftExtent = new Fraction(leftBound);
                    rightExtent = new Fraction(sx);
                    
                    leftwardsSearch = true;
                } else if (blOfBlocked) {
                    //  \ |
                    //   \|
                    
                    leftExtent = new Fraction(sx*dy + dx, dy);
                    rightExtent = new Fraction(sx);
                    if (leftExtent.isLessThan(leftBound)) { // leftExtent < leftBound
                        leftExtent = new Fraction(leftBound);
                    }
                    
                } else { // trOfBlocked
                    //   \
                    //  __\
                    
                    leftExtent = new Fraction(leftBound);
                    rightExtent = new Fraction(sx*dy + dx, dy);
                    
                    leftwardsSearch = true;
                }
                
                if (leftExtent.isLessThanOrEqual(rightExtent)) {
                    this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                }
                
            } else if (dy < 0) {
                //    B
                //   /
                //  P
                boolean tlOfBlocked = graph.topLeftOfBlockedTile(sx, sy);
                boolean brOfBlocked = graph.bottomRightOfBlockedTile(sx, sy);
                
                int leftBound = leftDownExtent(sx,sy);
                Fraction leftExtent;
                Fraction rightExtent;
                
                if (tlOfBlocked && brOfBlocked) {
                    //  ____
                    //     |
                    //     |
                    
                    leftExtent = new Fraction(leftBound);
                    rightExtent = new Fraction(sx);
                    
                    leftwardsSearch = true;
                } else if (tlOfBlocked) {
                    //   /|
                    //  / |
                    
                    leftExtent = new Fraction(sx*-dy + dx, -dy);
                    rightExtent = new Fraction(sx);
                    if (leftExtent.isLessThan(leftBound)) { // leftExtent < leftBound
                        leftExtent = new Fraction(leftBound);
                    }
                    
                } else { // brOfBlocked
                    //  ___
                    //    /
                    //   /
                    
                    leftExtent = new Fraction(leftBound);
                    rightExtent = new Fraction(sx*-dy + dx, -dy);
                    
                    leftwardsSearch = true;
                }
                
                if (leftExtent.isLessThanOrEqual(rightExtent)) {
                    this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                }
                
                
            } else { // dy == 0
                
                // P--B

                if (graph.bottomLeftOfBlockedTile(sx, sy)) {
                    //    |
                    // ___|

                    Fraction leftExtent = new Fraction(leftUpExtent(sx,sy));
                    Fraction rightExtent = new Fraction(sx);
                    this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                    
                } else if (graph.topLeftOfBlockedTile(sx, sy)) {
                    // ____
                    //    |
                    //    |

                    Fraction leftExtent = new Fraction(leftDownExtent(sx,sy));
                    Fraction rightExtent = new Fraction(sx);
                    this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                }
                
                leftwardsSearch = true;
                
            }
        } else { // dx == 0
            // Direct upwards or direct downwards.
            if (dy > 0) {
                // Direct upwards
                
                //  P
                //  |
                //  B
                
                if (graph.topLeftOfBlockedTile(sx, sy)) {
                    // |
                    // |___

                    Fraction leftExtent = new Fraction(sx);
                    Fraction rightExtent = new Fraction(rightUpExtent(sx,sy));
                    this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);

                    rightwardsSearch = true;
                    
                } else if (graph.topRightOfBlockedTile(sx, sy)) {
                    //    |
                    // ___|

                    Fraction leftExtent = new Fraction(leftUpExtent(sx,sy));
                    Fraction rightExtent = new Fraction(sx);
                    this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);

                    leftwardsSearch = true;
                    
                } else {
                    Fraction x = new Fraction(sx);
                    stackPush(new LOSInterval(sy+1, x, x, LOSInterval.BOTH_INCLUSIVE));
                }
                
            } else { // dy < 0
                // Direct downwards
                
                //  B
                //  |
                //  P

                if (graph.bottomLeftOfBlockedTile(sx, sy)) {
                    // ____
                    // |
                    // |

                    Fraction leftExtent = new Fraction(sx);
                    Fraction rightExtent = new Fraction(rightDownExtent(sx,sy));
                    this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                    
                    rightwardsSearch = true;
                    
                } else if (graph.bottomRightOfBlockedTile(sx, sy)) {
                    // ____
                    //    |
                    //    |

                    Fraction leftExtent = new Fraction(leftDownExtent(sx,sy));
                    Fraction rightExtent = new Fraction(sx);
                    this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);

                    leftwardsSearch = true;
                    
                } else {
                    Fraction x = new Fraction(sx);
                    stackPush(new LOSInterval(sy-1, x, x, LOSInterval.BOTH_INCLUSIVE));
                }
            }
        }
        
        // Direct Search Left
        if (leftwardsSearch) {
            // Direct Search Left
            // Assumption: Not blocked towards left.
            addSuccessor(leftAnyExtent(sx, sy),sy); 
        }
        
        if (rightwardsSearch) {
            // Direct Search Right
            // Assumption: Not blocked towards right.
            addSuccessor(rightAnyExtent(sx, sy),sy);
        }
        
        
        
    }

    /**
     * Assumption: We are at an outer corner. One of six cases:
     *   BR        BL        TR        TL       TRBL      TLBR
     * XXX|         |XXX      :         :         |XXX   XXX|
     * XXX|...   ...|XXX   ___:...   ...:___   ___|XXX   XXX|___
     *    :         :      XXX|         |XXX   XXX|         |XXX
     *    :         :      XXX|         |XXX   XXX|         |XXX
     */
    private final void generateTwoWayTautStartingStates(int sx, int sy) {
        boolean bottomLeftOfBlocked = graph.bottomLeftOfBlockedTile(sx, sy);
        boolean bottomRightOfBlocked = graph.bottomRightOfBlockedTile(sx, sy);
        boolean topLeftOfBlocked = graph.topLeftOfBlockedTile(sx, sy);
        boolean topRightOfBlocked = graph.topRightOfBlockedTile(sx, sy);

        // Generate up-left direction
        if (topRightOfBlocked || bottomLeftOfBlocked) {
            Fraction leftExtent = new Fraction(leftUpExtent(sx,sy));
            Fraction rightExtent = new Fraction(sx);

            this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);
        }
        
        // Generate up-right direction
        if (bottomRightOfBlocked || topLeftOfBlocked) {
            Fraction leftExtent = new Fraction(sx);
            Fraction rightExtent = new Fraction(rightUpExtent(sx,sy));

            this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);
        }

        // Generate down-left direction
        if (bottomRightOfBlocked || topLeftOfBlocked) {
            Fraction leftExtent = new Fraction(leftDownExtent(sx,sy));
            Fraction rightExtent = new Fraction(sx);

            this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);
        }
        
        // Generate down-right direction
        if (topRightOfBlocked || bottomLeftOfBlocked) {
            Fraction leftExtent = new Fraction(sx);
            Fraction rightExtent = new Fraction(rightDownExtent(sx,sy));

            this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);
        }
        
        // Search leftwards
        if (!topRightOfBlocked || !bottomRightOfBlocked) {
            int x = leftAnyExtent(sx, sy);
            int y = sy;
            if (!(graph.topRightOfBlockedTile(x, y) && graph.bottomRightOfBlockedTile(x, y))) {
                addSuccessor(x,y);
            }
        }

        // Search rightwards
        if (!topLeftOfBlocked || !bottomLeftOfBlocked) {
            int x = rightAnyExtent(sx, sy);
            int y = sy;
            if (!(graph.topLeftOfBlockedTile(x, y) && graph.bottomLeftOfBlockedTile(x, y))) {
                addSuccessor(x,y);
            }
        }
    }

    private final void generateStartingStates(int sx, int sy) {
        boolean bottomLeftOfBlocked = graph.bottomLeftOfBlockedTile(sx, sy);
        boolean bottomRightOfBlocked = graph.bottomRightOfBlockedTile(sx, sy);
        boolean topLeftOfBlocked = graph.topLeftOfBlockedTile(sx, sy);
        boolean topRightOfBlocked = graph.topRightOfBlockedTile(sx, sy);
        
        // Generate up
        if (!bottomLeftOfBlocked || !bottomRightOfBlocked) {
            Fraction leftExtent, rightExtent;
            
            if (bottomLeftOfBlocked) {
                // Explore up-left
                leftExtent = new Fraction(leftUpExtent(sx, sy));
                rightExtent = new Fraction(sx);
            } else if (bottomRightOfBlocked) {
                // Explore up-right
                leftExtent = new Fraction(sx);
                rightExtent = new Fraction(rightUpExtent(sx, sy));
            } else {
                // Explore up-left-right
                leftExtent = new Fraction(leftUpExtent(sx, sy));
                rightExtent = new Fraction(rightUpExtent(sx, sy));
            }

            this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);
        }

        // Generate down
        if (!topLeftOfBlocked || !topRightOfBlocked) {
            Fraction leftExtent, rightExtent;
            
            if (topLeftOfBlocked) {
                // Explore down-left
                leftExtent = new Fraction(leftDownExtent(sx, sy));
                rightExtent = new Fraction(sx);
            } else if (topRightOfBlocked) {
                // Explore down-right
                leftExtent = new Fraction(sx);
                rightExtent = new Fraction(rightDownExtent(sx, sy));
            } else {
                // Explore down-left-right
                leftExtent = new Fraction(leftDownExtent(sx, sy));
                rightExtent = new Fraction(rightDownExtent(sx, sy));
            }

            this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);
        }

        // Search leftwards
        if (!topRightOfBlocked || !bottomRightOfBlocked) {
            int x = leftAnyExtent(sx, sy);
            int y = sy;
            if (!(graph.topRightOfBlockedTile(x, y) && graph.bottomRightOfBlockedTile(x, y))) {
                addSuccessor(x,y);
            }
        }

        // Search rightwards
        if (!topLeftOfBlocked || !bottomLeftOfBlocked) {
            int x = rightAnyExtent(sx, sy);
            int y = sy;
            if (!(graph.topLeftOfBlockedTile(x, y) && graph.bottomLeftOfBlockedTile(x, y))) {
                addSuccessor(x,y);
            }
        }
    }
    
    private final void exploreStates(int sx, int sy) {
        while (intervalStackSize > 0) {
            LOSInterval currState = stackPop();
            boolean leftInclusive = (currState.inclusive & LOSInterval.LEFT_INCLUSIVE) != 0;
            boolean rightInclusive = (currState.inclusive & LOSInterval.RIGHT_INCLUSIVE) != 0;
            //System.out.println("POP " + currState);

            boolean zeroLengthInterval = currState.xR.isEqualTo(currState.xL);
            
            if (currState.y > sy) {
                // Upwards
                
                // Insert endpoints if integer.
                if (leftInclusive && currState.xL.isWholeNumber()) {
                    /* The two cases   _
                     *  _             |X|
                     * |X|'.           ,'
                     *      '.       ,'
                     *        B     B
                     */
                    
                    int x = currState.xL.n;
                    int y = currState.y;
                    boolean topRightOfBlockedTile = graph.topRightOfBlockedTile(x, y);
                    boolean bottomRightOfBlockedTile = graph.bottomRightOfBlockedTile(x, y);
                    
                    if (x <= sx && topRightOfBlockedTile && !bottomRightOfBlockedTile) {
                        addSuccessor(x, y);
                        leftInclusive = false;
                    }
                    else if (sx <= x && bottomRightOfBlockedTile && !topRightOfBlockedTile) {
                        addSuccessor(x, y);
                        leftInclusive = false;
                    }
                }
                if (rightInclusive && currState.xR.isWholeNumber()) {
                    /*   _   The two cases
                     *  |X|             _
                     *  '.           ,'|X|
                     *    '.       ,'
                     *      B     B
                     */
                    
                    int x = currState.xR.n;
                    int y = currState.y;
                    boolean bottomLeftOfBlockedTile = graph.bottomLeftOfBlockedTile(x, y);
                    boolean topLeftOfBlockedTile = graph.topLeftOfBlockedTile(x, y);
                    
                    if (x <= sx && bottomLeftOfBlockedTile && !topLeftOfBlockedTile) {
                        if (leftInclusive || !zeroLengthInterval) {
                            addSuccessor(x, y);
                            rightInclusive = false;
                        }
                    }
                    else if (sx <= x && topLeftOfBlockedTile  && !bottomLeftOfBlockedTile) {
                        if (leftInclusive || !zeroLengthInterval) {
                            addSuccessor(x, y);
                            rightInclusive = false;
                        }
                    }
                }
                
                
                
                // Generate Upwards
                /*
                 * =======      =====    =====
                 *  \   /       / .'      '. \
                 *   \ /   OR  /.'    OR    '.\
                 *    B       B                B
                 */

                // (Px-Bx)*(Py-By+1)/(Py-By) + Bx
                int dy = currState.y - sy;
                Fraction leftProjection = currState.xL.minus(sx).multiplyDivide(dy+1, dy).plus(sx);

                int leftBound = leftUpExtent(currState.xL.ceil(), currState.y);
                if (currState.xL.isWholeNumber() && graph.bottomRightOfBlockedTile(currState.xL.n, currState.y)) leftBound = currState.xL.n;
                
                if (leftProjection.isLessThan(leftBound)) { // leftProjection < leftBound
                    leftProjection = new Fraction(leftBound);
                    leftInclusive = true;
                }

                // (Px-Bx)*(Py-By+1)/(Py-By) + Bx
                Fraction rightProjection = currState.xR.minus(sx).multiplyDivide(dy+1, dy).plus(sx);
                
                int rightBound = rightUpExtent(currState.xR.floor(), currState.y);
                if (currState.xR.isWholeNumber() && graph.bottomLeftOfBlockedTile(currState.xR.n, currState.y)) rightBound = currState.xR.n;

                if (!rightProjection.isLessThanOrEqual(rightBound)) { // rightBound < rightProjection
                    rightProjection = new Fraction(rightBound);
                    rightInclusive = true;
                }

                // Call Generate
                if (leftInclusive && rightInclusive) {
                    if (leftProjection.isLessThanOrEqual(rightProjection)) {
                        generateUpwards(leftProjection, rightProjection, sx, sy, currState.y, true, true);
                    }
                }
                else if (leftProjection.isLessThan(rightProjection)) {
                    generateUpwards(leftProjection, rightProjection, sx, sy, currState.y, leftInclusive, rightInclusive);
                }
            }
            else {
                // Upwards
                
                // Insert endpoints if integer.
                if (leftInclusive && currState.xL.isWholeNumber()) {
                    /* The two cases
                     *        B     B
                     *  _   ,'       '.
                     * |X|.'           '.
                     *                |X|
                     */
                    
                    int x = currState.xL.n;
                    int y = currState.y;
                    boolean bottomRightOfBlockedTile = graph.bottomRightOfBlockedTile(x, y);
                    boolean topRightOfBlockedTile = graph.topRightOfBlockedTile(x, y);
                    
                    if (x <= sx && bottomRightOfBlockedTile && !topRightOfBlockedTile) {
                        addSuccessor(x, y);
                        leftInclusive = false;
                    }
                    else if (sx <= x && topRightOfBlockedTile && !bottomRightOfBlockedTile) {
                        addSuccessor(x, y);
                        leftInclusive = false;
                    }
                }
                if (rightInclusive && currState.xR.isWholeNumber()) {
                    /*       The two cases
                     *      B     B
                     *    .'       '.   _
                     *  .'           '.|X|
                     *  |X|
                     */
                    
                    int x = currState.xR.n;
                    int y = currState.y;
                    boolean topLeftOfBlockedTile = graph.topLeftOfBlockedTile(x, y);
                    boolean bottomLeftOfBlockedTile = graph.bottomLeftOfBlockedTile(x, y);
                    
                    if (x <= sx && topLeftOfBlockedTile && !bottomLeftOfBlockedTile) {
                        if (leftInclusive || !zeroLengthInterval) {
                            addSuccessor(x, y);
                            rightInclusive = false;
                        }
                    }
                    else if (sx <= x && bottomLeftOfBlockedTile  && !topLeftOfBlockedTile) {
                        if (leftInclusive || !zeroLengthInterval) {
                            addSuccessor(x, y);
                            rightInclusive = false;
                        }
                    }
                }


                
                // Generate downwards
                /*
                 *    B       B                B
                 *   / \   OR  \'.    OR    .'/
                 *  /   \       \ '.      .' /
                 * =======      =====    =====
                 */

                // (Px-Bx)*(Py-By+1)/(Py-By) + Bx
                int dy = sy - currState.y; 
                Fraction leftProjection = currState.xL.minus(sx).multiplyDivide(dy+1, dy).plus(sx);
                
                int leftBound = leftDownExtent(currState.xL.ceil(), currState.y);
                if (currState.xL.isWholeNumber() && graph.topRightOfBlockedTile(currState.xL.n, currState.y)) leftBound = currState.xL.n;
                
                if (leftProjection.isLessThan(leftBound)) { // leftProjection < leftBound
                    leftProjection = new Fraction(leftBound);
                    leftInclusive = true;
                }

                // (Px-Bx)*(Py-By+1)/(Py-By) + Bx
                Fraction rightProjection = currState.xR.minus(sx).multiplyDivide(dy+1, dy).plus(sx);

                int rightBound = rightDownExtent(currState.xR.floor(), currState.y);
                if (currState.xR.isWholeNumber() && graph.topLeftOfBlockedTile(currState.xR.n, currState.y)) rightBound = currState.xR.n;
                
                if (!rightProjection.isLessThanOrEqual(rightBound)) { // rightBound < rightProjection
                    rightProjection = new Fraction(rightBound);
                    rightInclusive = true;
                }

                // Call Generate
                if (leftInclusive && rightInclusive) {
                    if (leftProjection.isLessThanOrEqual(rightProjection)) {
                        generateDownwards(leftProjection, rightProjection, sx, sy, currState.y, true, true);
                    }
                }
                else if (leftProjection.isLessThan(rightProjection)) {
                    generateDownwards(leftProjection, rightProjection, sx, sy, currState.y, leftInclusive, rightInclusive);
                }
            }
        }
    }
    
    

    private final void exploreStatesNonTaut(int sx, int sy) {
        while (intervalStackSize > 0) {
            LOSInterval currState = stackPop();
            boolean leftInclusive = (currState.inclusive & LOSInterval.LEFT_INCLUSIVE) != 0;
            boolean rightInclusive = (currState.inclusive & LOSInterval.RIGHT_INCLUSIVE) != 0;
            //System.out.println("POP " + currState);

            boolean zeroLengthInterval = currState.xR.isEqualTo(currState.xL);
            
            if (currState.y > sy) {
                // Upwards
                
                // Insert endpoints if integer.
                if (leftInclusive && currState.xL.isWholeNumber()) {
                    /* The two cases   _
                     *  _             |X|
                     * |X|'.           ,'
                     *      '.       ,'
                     *        B     B
                     */
                    
                    int x = currState.xL.n;
                    int y = currState.y;
                    
                    if (graph.isOuterCorner(x, y)) {
                        addSuccessor(x, y);
                        leftInclusive = false;
                    }
                }
                if (rightInclusive && currState.xR.isWholeNumber()) {
                    /*   _   The two cases
                     *  |X|             _
                     *  '.           ,'|X|
                     *    '.       ,'
                     *      B     B
                     */
                    
                    int x = currState.xR.n;
                    int y = currState.y;
                    
                    if (graph.isOuterCorner(x, y)) {
                        addSuccessor(x, y);
                        rightInclusive = false;
                    }
                }
                
                
                
                // Generate Upwards
                /*
                 * =======      =====    =====
                 *  \   /       / .'      '. \
                 *   \ /   OR  /.'    OR    '.\
                 *    B       B                B
                 */

                // (Px-Bx)*(Py-By+1)/(Py-By) + Bx
                int dy = currState.y - sy;
                Fraction leftProjection = currState.xL.minus(sx).multiplyDivide(dy+1, dy).plus(sx);

                int leftBound = leftUpExtent(currState.xL.ceil(), currState.y);
                if (currState.xL.isWholeNumber() && graph.bottomRightOfBlockedTile(currState.xL.n, currState.y)) leftBound = currState.xL.n;
                
                if (leftProjection.isLessThan(leftBound)) { // leftProjection < leftBound
                    leftProjection = new Fraction(leftBound);
                    leftInclusive = true;
                }

                // (Px-Bx)*(Py-By+1)/(Py-By) + Bx
                Fraction rightProjection = currState.xR.minus(sx).multiplyDivide(dy+1, dy).plus(sx);
                
                int rightBound = rightUpExtent(currState.xR.floor(), currState.y);
                if (currState.xR.isWholeNumber() && graph.bottomLeftOfBlockedTile(currState.xR.n, currState.y)) rightBound = currState.xR.n;

                if (!rightProjection.isLessThanOrEqual(rightBound)) { // rightBound < rightProjection
                    rightProjection = new Fraction(rightBound);
                    rightInclusive = true;
                }

                // Call Generate
                if (leftInclusive && rightInclusive) {
                    if (leftProjection.isLessThanOrEqual(rightProjection)) {
                        generateUpwards(leftProjection, rightProjection, sx, sy, currState.y, true, true);
                    }
                }
                else if (leftProjection.isLessThan(rightProjection)) {
                    generateUpwards(leftProjection, rightProjection, sx, sy, currState.y, leftInclusive, rightInclusive);
                }
            }
            else {
                // Upwards
                
                // Insert endpoints if integer.
                if (leftInclusive && currState.xL.isWholeNumber()) {
                    /* The two cases
                     *        B     B
                     *  _   ,'       '.
                     * |X|.'           '.
                     *                |X|
                     */
                    
                    int x = currState.xL.n;
                    int y = currState.y;
                    
                    if (graph.isOuterCorner(x, y)) {
                        addSuccessor(x, y);
                        leftInclusive = false;
                    }
                }
                if (rightInclusive && currState.xR.isWholeNumber()) {
                    /*       The two cases
                     *      B     B
                     *    .'       '.   _
                     *  .'           '.|X|
                     *  |X|
                     */
                    
                    int x = currState.xR.n;
                    int y = currState.y;
                    if (graph.isOuterCorner(x, y)) {
                        addSuccessor(x, y);
                        rightInclusive = false;
                    }
                }


                
                // Generate downwards
                /*
                 *    B       B                B
                 *   / \   OR  \'.    OR    .'/
                 *  /   \       \ '.      .' /
                 * =======      =====    =====
                 */

                // (Px-Bx)*(Py-By+1)/(Py-By) + Bx
                int dy = sy - currState.y; 
                Fraction leftProjection = currState.xL.minus(sx).multiplyDivide(dy+1, dy).plus(sx);
                
                int leftBound = leftDownExtent(currState.xL.ceil(), currState.y);
                if (currState.xL.isWholeNumber() && graph.topRightOfBlockedTile(currState.xL.n, currState.y)) leftBound = currState.xL.n;
                
                if (leftProjection.isLessThan(leftBound)) { // leftProjection < leftBound
                    leftProjection = new Fraction(leftBound);
                    leftInclusive = true;
                }

                // (Px-Bx)*(Py-By+1)/(Py-By) + Bx
                Fraction rightProjection = currState.xR.minus(sx).multiplyDivide(dy+1, dy).plus(sx);

                int rightBound = rightDownExtent(currState.xR.floor(), currState.y);
                if (currState.xR.isWholeNumber() && graph.topLeftOfBlockedTile(currState.xR.n, currState.y)) rightBound = currState.xR.n;
                
                if (!rightProjection.isLessThanOrEqual(rightBound)) { // rightBound < rightProjection
                    rightProjection = new Fraction(rightBound);
                    rightInclusive = true;
                }

                // Call Generate
                if (leftInclusive && rightInclusive) {
                    if (leftProjection.isLessThanOrEqual(rightProjection)) {
                        generateDownwards(leftProjection, rightProjection, sx, sy, currState.y, true, true);
                    }
                }
                else if (leftProjection.isLessThan(rightProjection)) {
                    generateDownwards(leftProjection, rightProjection, sx, sy, currState.y, leftInclusive, rightInclusive);
                }
            }
        }
    }
    
    
    
    

    private final int leftUpExtent(int xL, int y) {
        return xL > sizeX ? sizeX : leftDownExtents[y+1][xL];
    }

    private final int leftDownExtent(int xL, int y) {
        return xL > sizeX ? sizeX : leftDownExtents[y][xL];
    }
    
    private final int leftAnyExtent(int xL, int y) {
        return Math.max(leftDownExtents[y][xL], leftDownExtents[y+1][xL]);
    }

    private final int rightUpExtent(int xR, int y) {
        return xR < 0 ? 0 : rightDownExtents[y+1][xR];
    }

    private final int rightDownExtent(int xR, int y) {
        return xR < 0 ? 0 : rightDownExtents[y][xR];
    }

    private final int rightAnyExtent(int xR, int y) {
        return Math.min(rightDownExtents[y][xR], rightDownExtents[y+1][xR]);
    }

    private final void generateUpwards(Fraction leftBound, Fraction rightBound, int sx, int sy, int currY, boolean leftInclusive, boolean rightInclusive) {
        generateAndSplitIntervals(
                currY + 2, currY + 1,
                sx, sy,
                leftBound, rightBound,
                leftInclusive, rightInclusive);
    }

    private final void generateDownwards(Fraction leftBound, Fraction rightBound, int sx, int sy, int currY, boolean leftInclusive, boolean rightInclusive) {
        generateAndSplitIntervals(
                currY - 1, currY - 1,
                sx, sy,
                leftBound, rightBound,
                leftInclusive, rightInclusive);
    }
    
    /**
     * Called by generateUpwards / Downwards.
     * Note: Unlike Anya, 0-length intervals are possible.
     */
    private final void generateAndSplitIntervals(int checkY, int newY, int sx, int sy, Fraction leftBound, Fraction rightBound, boolean leftInclusive, boolean rightInclusive) {
        Fraction left = leftBound;
        int leftFloor = left.floor();

        // Up: !bottomRightOfBlockedTile && bottomLeftOfBlockedTile
        if (leftInclusive && left.isWholeNumber() && !graph.isBlocked(leftFloor-1, checkY-1) && graph.isBlocked(leftFloor, checkY-1)) {
            stackPush(new LOSInterval(newY, left, left, LOSInterval.BOTH_INCLUSIVE));
        }

        // Divide up the intervals.
        while(true) {
            int right = rightDownExtents[checkY][leftFloor]; // it's actually rightDownExtents for exploreDownwards. (thus we use checkY = currY - 2)
            if (rightBound.isLessThanOrEqual(right)) break; // right < rightBound            
            
            // Only push unblocked ( bottomRightOfBlockedTile )
            if (!graph.isBlocked(right-1, checkY-1)) {
                stackPush(new LOSInterval(newY, left, new Fraction(right), leftInclusive ? LOSInterval.BOTH_INCLUSIVE : LOSInterval.RIGHT_INCLUSIVE));
            }
            
            leftFloor = right;
            left = new Fraction(leftFloor);
            leftInclusive = true;
        }

        // The last interval will always be here.
        // if !bottomLeftOfBlockedTile(leftFloor, checkY)
        if (!graph.isBlocked(leftFloor, checkY-1)) {
            int inclusive = (leftInclusive ? LOSInterval.LEFT_INCLUSIVE : 0) | (rightInclusive ? LOSInterval.RIGHT_INCLUSIVE : 0); 
            stackPush(new LOSInterval(newY, left, rightBound, inclusive));
        } else {
            // The possibility of there being one degenerate interval at the end. ( !bottomLeftOfBlockedTile(xR, checkY) )
            if (rightInclusive && rightBound.isWholeNumber() && !graph.isBlocked(rightBound.n, checkY-1)) {
                stackPush(new LOSInterval(newY, rightBound, rightBound, LOSInterval.BOTH_INCLUSIVE));
            }
        }
    }

    public static void clearMemory() {
        snapshotList.clear();
        snapshots.clear();
        rightDownExtents = null;
        leftDownExtents = null;
        intervalStack = null;
        successorsX = null;
        successorsY = null;
        System.gc();
    }
    
} public final class LineOfSightScannerDouble {
    private static double EPSILON = 0.0000001;

    public static ArrayList<List<SnapshotItem>> snapshotList = new ArrayList<>();
    private static ArrayList<SnapshotItem> snapshots = new ArrayList<>();
    private static int snapshot_sx;
    private static int snapshot_sy;
    
    private final GridGraph graph;
    private final int sizeX;
    private final int sizeY;
    
    private static int[][] rightDownExtents;
    private static int[][] leftDownExtents;
    private static LOSIntervalDouble[] intervalStack;
    private static int intervalStackSize;
    
    public static int[] successorsX;
    public static int[] successorsY;
    public static int nSuccessors;

    // Double API    
    private final double toDouble(int n) {return (double)n;}
    private final double toDouble(int n, int d) {return (double)n / d;}
    private final boolean isLessThanOrEqual(double a, double b) {
        return a < b + EPSILON;
    }
    private final boolean isLessThan(double a, double b) {
        return a < b - EPSILON;
    }
    private final boolean isWholeNumber(double n) {
        return Math.abs(n - (int)(n+0.5)) < EPSILON;
    }
    private final boolean isEqualTo(double a, double b) {
        return Math.abs(a - b) < EPSILON;
    }
    private final int floor(double n) {
        return (int)(n + EPSILON);
    }
    private final int ceil(double n) {
        return (int)(n + 1 - EPSILON);
    }
    private final int round(double n) {
        return (int)(n + 0.5);
    }

    private static void initialiseExtents(GridGraph graph) {
        // Don't reinitialise if graph is the same size as the last time.
        if (rightDownExtents != null && graph.sizeY+2 == rightDownExtents.length && graph.sizeX+1 == rightDownExtents[0].length) return;

        rightDownExtents = new int[graph.sizeY+2][];
        leftDownExtents = new int[graph.sizeY+2][];
        for (int y=0;y<graph.sizeY+2;++y) {
            rightDownExtents[y] = new int[graph.sizeX+1];
            leftDownExtents[y] = new int[graph.sizeX+1];
        }
    }
    
    private static void initialiseStack() {
        if (intervalStack != null) return;
        intervalStack = new LOSIntervalDouble[11];
        intervalStackSize = 0;
    }
    
    private static void initialiseSuccessorList() {
        if (successorsX != null) return;
        successorsX = new int[11];
        successorsY = new int[11];
        nSuccessors = 0;
    }
    
    private static final void clearSuccessors() {
        nSuccessors = 0;
    }
    
    private static final void stackPush(LOSIntervalDouble interval) {
        if (intervalStackSize >= intervalStack.length) {
            intervalStack = Arrays.copyOf(intervalStack, intervalStack.length*2);
        }
        intervalStack[intervalStackSize] = interval;
        ++intervalStackSize;
        
        //addToSnapshot(interval); // Uncomment for debugging.
    }
    
    private static final void addToSnapshot(LOSIntervalDouble interval) {
        final int RES = 100000;
        int xLn = (int)(interval.xL*RES);
        int xRn = (int)(interval.xR*RES);

        snapshots.add(SnapshotItem.generate(new Integer[]{interval.y, xLn, RES, xRn, RES, snapshot_sx, snapshot_sy}, Color.GREEN));
        snapshotList.add(new ArrayList<SnapshotItem>(snapshots));
    }
    
    public static final void clearSnapshots() {
        snapshotList.clear();
        snapshots.clear();
    }
    
    private static final LOSIntervalDouble stackPop() {
        LOSIntervalDouble temp = intervalStack[intervalStackSize-1];
        --intervalStackSize;
        intervalStack[intervalStackSize] = null;
        return temp;
    }
    
    private static final void clearStack() {
        intervalStackSize = 0;
    }
    
    private static final void addSuccessor(int x, int y) {
        if (nSuccessors >= successorsX.length) {
            successorsX = Arrays.copyOf(successorsX, successorsX.length*2);
            successorsY = Arrays.copyOf(successorsY, successorsY.length*2);
        }
        successorsX[nSuccessors] = x;
        successorsY[nSuccessors] = y;
        ++nSuccessors;
    }
    
    public LineOfSightScannerDouble(GridGraph gridGraph) {
        initialiseExtents(gridGraph);
        initialiseSuccessorList();
        initialiseStack();
        
        graph = gridGraph;
        sizeX = graph.sizeX;
        sizeY = graph.sizeY;
        computeExtents();
    }
    
    private void computeExtents() {
        // graph.isBlocked(x,y) is the same as graph.bottomLeftOfBlockedTile(x,y)
        LineOfSightScannerDouble.initialiseExtents(graph);
        
        for (int y=0;y<sizeY+2;++y) {
            boolean lastIsBlocked = true;
            int lastX = -1;
            for (int x=0;x<=sizeX;++x) {
                leftDownExtents[y][x] = lastX; 
                if (graph.isBlocked(x, y-1) != lastIsBlocked) {
                    lastX = x;
                    lastIsBlocked = !lastIsBlocked;
                }
            }
            lastIsBlocked = true;
            lastX = sizeX+1;
            for (int x=sizeX;x>=0;--x) {
                rightDownExtents[y][x] = lastX; 
                if (graph.isBlocked(x-1, y-1) != lastIsBlocked) {
                    lastX = x;
                    lastIsBlocked = !lastIsBlocked;
                }
            }
        }
    }
    
    /**
     * Stores results in successorsX, successorsY and nSuccessors. 
     */
    public final void computeAllVisibleSuccessors(int sx, int sy) {
        snapshot_sx=sx;snapshot_sy=sy;
        clearSuccessors();
        clearStack();

        generateStartingStates(sx, sy);
        exploreStatesNonTaut(sx, sy);
    }

    /**
     * Stores results in successorsX, successorsY and nSuccessors. 
     */
    public final void computeAllVisibleTautSuccessors(int sx, int sy) {
        snapshot_sx=sx;snapshot_sy=sy;
        clearSuccessors();
        clearStack();

        generateStartingStates(sx, sy);
        exploreStates(sx, sy);
    }

    /**
     * Stores results in successorsX, successorsY and nSuccessors. 
     */
    public final void computeAllVisibleTwoWayTautSuccessors(int sx, int sy) {
        snapshot_sx=sx;snapshot_sy=sy;
        clearSuccessors();
        clearStack();

        generateTwoWayTautStartingStates(sx, sy);
        exploreStates(sx, sy);
    }
    
    /**
     * Stores results in successorsX, successorsY and nSuccessors. 
     * We are moving in direction dx, dy
     */
    public final void computeAllVisibleIncrementalTautSuccessors(int sx, int sy, int dx, int dy) {
        snapshot_sx=sx;snapshot_sy=sy;
        clearSuccessors();
        clearStack();

        generateIncrementalTautStartingStates(sx, sy, dx, dy);
        exploreStates(sx, sy);
    }


    /**
     * Assumption: We are at an outer corner. One of six cases:
     *   BR        BL        TR        TL       TRBL      TLBR
     * XXX|         |XXX      :         :         |XXX   XXX|
     * XXX|...   ...|XXX   ___:...   ...:___   ___|XXX   XXX|___
     *    :         :      XXX|         |XXX   XXX|         |XXX
     *    :         :      XXX|         |XXX   XXX|         |XXX
     *    
     * Assumption: We are also entering from a taut direction.
     * dx > 0, dy > 0 : BR TL
     * dx > 0, dy < 0 : BL TR
     * dx < 0, dy < 0 : BR TL
     * dx < 0, dy > 0 : BL TR
     */
    private final void generateIncrementalTautStartingStates(int sx, int sy, int dx, int dy) {
        boolean rightwardsSearch = false;
        boolean leftwardsSearch = false;
        
        if (dx > 0) {
            // Moving rightwards
            if (dy > 0) {
                //    P
                //   /
                //  B
                boolean brOfBlocked = graph.bottomRightOfBlockedTile(sx, sy);
                boolean tlOfBlocked = graph.topLeftOfBlockedTile(sx, sy);
                
                int rightBound = rightUpExtent(sx,sy);
                double leftExtent;
                double rightExtent;
                
                if (brOfBlocked && tlOfBlocked) {
                    //  |
                    //  |___
                    
                    leftExtent = toDouble(sx);
                    rightExtent = toDouble(rightBound);
                    
                    rightwardsSearch = true;
                } else if (brOfBlocked) {
                    //  | /
                    //  |/
                    
                    leftExtent = toDouble(sx);
                    rightExtent = toDouble(sx*dy + dx, dy);
                    if (!isLessThanOrEqual(rightExtent, rightBound)) { // rightBound < rightExtent
                        rightExtent = toDouble(rightBound);
                    }
                    
                } else { // tlOfBlocked
                    //   /
                    //  /__
                    
                    leftExtent = toDouble(sx*dy + dx, dy);
                    rightExtent = toDouble(rightBound);
                    
                    rightwardsSearch = true;
                }
                
                if (isLessThanOrEqual(leftExtent, rightExtent)) {
                    this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                }
                
            } else if (dy < 0) {
                //  B
                //   \
                //    P
                boolean trOfBlocked = graph.topRightOfBlockedTile(sx, sy);
                boolean blOfBlocked = graph.bottomLeftOfBlockedTile(sx, sy);
                
                int rightBound = rightDownExtent(sx,sy);
                double leftExtent;
                double rightExtent;
                
                if (trOfBlocked && blOfBlocked) {
                    //  ____
                    //  |
                    //  |
                    
                    leftExtent = toDouble(sx);
                    rightExtent = toDouble(rightBound);
                    
                    rightwardsSearch = true;
                } else if (trOfBlocked) {
                    //  .
                    //  |\
                    //  | \
                    
                    leftExtent = toDouble(sx);
                    rightExtent = toDouble(sx*-dy + dx, -dy);
                    if (!isLessThanOrEqual(rightExtent, rightBound)) { // rightBound < rightExtent
                        rightExtent = toDouble(rightBound);
                    }
                    
                } else { // blOfBlocked
                    //  ___
                    //  \
                    //   \
                    leftExtent = toDouble(sx*-dy + dx, -dy);
                    rightExtent = toDouble(rightBound);
                    
                    rightwardsSearch = true;
                }
                
                if (isLessThanOrEqual(leftExtent, rightExtent)) {
                    this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                }
                
                
            } else { // dy == 0
                
                // B--P

                if (graph.bottomRightOfBlockedTile(sx, sy)) {
                    // |
                    // |___

                    double leftExtent = toDouble(sx);
                    double rightExtent = toDouble(rightUpExtent(sx,sy));
                    this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                    
                } else if (graph.topRightOfBlockedTile(sx, sy)) { // topRightOfBlockedTile
                    // ____
                    // |
                    // |

                    double leftExtent = toDouble(sx);
                    double rightExtent = toDouble(rightDownExtent(sx,sy));
                    this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                }
                
                rightwardsSearch = true;
            }
            
        } else if (dx < 0) {
            // Moving leftwards
            
            if (dy > 0) {
                //  B
                //   \
                //    P
                boolean blOfBlocked = graph.bottomLeftOfBlockedTile(sx, sy);
                boolean trOfBlocked = graph.topRightOfBlockedTile(sx, sy);
                
                int leftBound = leftUpExtent(sx,sy);
                double leftExtent;
                double rightExtent;
                
                if (blOfBlocked && trOfBlocked) {
                    //     |
                    //  ___|
                    
                    leftExtent = toDouble(leftBound);
                    rightExtent = toDouble(sx);
                    
                    leftwardsSearch = true;
                } else if (blOfBlocked) {
                    //  \ |
                    //   \|
                    
                    leftExtent = toDouble(sx*dy + dx, dy);
                    rightExtent = toDouble(sx);
                    if (isLessThan(leftExtent, leftBound)) { // leftExtent < leftBound
                        leftExtent = toDouble(leftBound);
                    }
                    
                } else { // trOfBlocked
                    //   \
                    //  __\
                    
                    leftExtent = toDouble(leftBound);
                    rightExtent = toDouble(sx*dy + dx, dy);
                    
                    leftwardsSearch = true;
                }
                
                if (isLessThanOrEqual(leftExtent, rightExtent)) {
                    this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                }
                
            } else if (dy < 0) {
                //    B
                //   /
                //  P
                boolean tlOfBlocked = graph.topLeftOfBlockedTile(sx, sy);
                boolean brOfBlocked = graph.bottomRightOfBlockedTile(sx, sy);
                
                int leftBound = leftDownExtent(sx,sy);
                double leftExtent;
                double rightExtent;
                
                if (tlOfBlocked && brOfBlocked) {
                    //  ____
                    //     |
                    //     |
                    
                    leftExtent = toDouble(leftBound);
                    rightExtent = toDouble(sx);
                    
                    leftwardsSearch = true;
                } else if (tlOfBlocked) {
                    //   /|
                    //  / |
                    
                    leftExtent = toDouble(sx*-dy + dx, -dy);
                    rightExtent = toDouble(sx);
                    if (isLessThan(leftExtent, leftBound)) { // leftExtent < leftBound
                        leftExtent = toDouble(leftBound);
                    }
                    
                } else { // brOfBlocked
                    //  ___
                    //    /
                    //   /
                    
                    leftExtent = toDouble(leftBound);
                    rightExtent = toDouble(sx*-dy + dx, -dy);
                    
                    leftwardsSearch = true;
                }
                
                if (isLessThanOrEqual(leftExtent, rightExtent)) {
                    this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                }
                
                
            } else { // dy == 0
                
                // P--B

                if (graph.bottomLeftOfBlockedTile(sx, sy)) {
                    //    |
                    // ___|

                    double leftExtent = toDouble(leftUpExtent(sx,sy));
                    double rightExtent = toDouble(sx);
                    this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                    
                } else if (graph.topLeftOfBlockedTile(sx, sy)) {
                    // ____
                    //    |
                    //    |

                    double leftExtent = toDouble(leftDownExtent(sx,sy));
                    double rightExtent = toDouble(sx);
                    this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                }
                
                leftwardsSearch = true;
                
            }
        } else { // dx == 0
            // Direct upwards or direct downwards.
            if (dy > 0) {
                // Direct upwards
                
                //  P
                //  |
                //  B
                
                if (graph.topLeftOfBlockedTile(sx, sy)) {
                    // |
                    // |___

                    double leftExtent = toDouble(sx);
                    double rightExtent = toDouble(rightUpExtent(sx,sy));
                    this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);

                    rightwardsSearch = true;
                    
                } else if (graph.topRightOfBlockedTile(sx, sy)) {
                    //    |
                    // ___|

                    double leftExtent = toDouble(leftUpExtent(sx,sy));
                    double rightExtent = toDouble(sx);
                    this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);

                    leftwardsSearch = true;
                    
                } else {
                    double x = toDouble(sx);
                    stackPush(new LOSIntervalDouble(sy+1, x, x, LOSIntervalDouble.BOTH_INCLUSIVE));
                }
                
            } else { // dy < 0
                // Direct downwards
                
                //  B
                //  |
                //  P

                if (graph.bottomLeftOfBlockedTile(sx, sy)) {
                    // ____
                    // |
                    // |

                    double leftExtent = toDouble(sx);
                    double rightExtent = toDouble(rightDownExtent(sx,sy));
                    this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);
                    
                    rightwardsSearch = true;
                    
                } else if (graph.bottomRightOfBlockedTile(sx, sy)) {
                    // ____
                    //    |
                    //    |

                    double leftExtent = toDouble(leftDownExtent(sx,sy));
                    double rightExtent = toDouble(sx);
                    this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);

                    leftwardsSearch = true;
                    
                } else {
                    double x = toDouble(sx);
                    stackPush(new LOSIntervalDouble(sy-1, x, x, LOSIntervalDouble.BOTH_INCLUSIVE));
                }
            }
        }
        
        // Direct Search Left
        if (leftwardsSearch) {
            // Direct Search Left
            // Assumption: Not blocked towards left.
            addSuccessor(leftAnyExtent(sx, sy),sy); 
        }
        
        if (rightwardsSearch) {
            // Direct Search Right
            // Assumption: Not blocked towards right.
            addSuccessor(rightAnyExtent(sx, sy),sy);
        }
        
        
        
    }

    /**
     * Assumption: We are at an outer corner. One of six cases:
     *   BR        BL        TR        TL       TRBL      TLBR
     * XXX|         |XXX      :         :         |XXX   XXX|
     * XXX|...   ...|XXX   ___:...   ...:___   ___|XXX   XXX|___
     *    :         :      XXX|         |XXX   XXX|         |XXX
     *    :         :      XXX|         |XXX   XXX|         |XXX
     */
    private final void generateTwoWayTautStartingStates(int sx, int sy) {
        boolean bottomLeftOfBlocked = graph.bottomLeftOfBlockedTile(sx, sy);
        boolean bottomRightOfBlocked = graph.bottomRightOfBlockedTile(sx, sy);
        boolean topLeftOfBlocked = graph.topLeftOfBlockedTile(sx, sy);
        boolean topRightOfBlocked = graph.topRightOfBlockedTile(sx, sy);

        // Generate up-left direction
        if (topRightOfBlocked || bottomLeftOfBlocked) {
            double leftExtent = toDouble(leftUpExtent(sx,sy));
            double rightExtent = toDouble(sx);

            this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);
        }
        
        // Generate up-right direction
        if (bottomRightOfBlocked || topLeftOfBlocked) {
            double leftExtent = toDouble(sx);
            double rightExtent = toDouble(rightUpExtent(sx,sy));

            this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);
        }

        // Generate down-left direction
        if (bottomRightOfBlocked || topLeftOfBlocked) {
            double leftExtent = toDouble(leftDownExtent(sx,sy));
            double rightExtent = toDouble(sx);

            this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);
        }
        
        // Generate down-right direction
        if (topRightOfBlocked || bottomLeftOfBlocked) {
            double leftExtent = toDouble(sx);
            double rightExtent = toDouble(rightDownExtent(sx,sy));

            this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);
        }
        
        // Search leftwards
        if (!topRightOfBlocked || !bottomRightOfBlocked) {
            int x = leftAnyExtent(sx, sy);
            int y = sy;
            if (!(graph.topRightOfBlockedTile(x, y) && graph.bottomRightOfBlockedTile(x, y))) {
                addSuccessor(x,y);
            }
        }

        // Search rightwards
        if (!topLeftOfBlocked || !bottomLeftOfBlocked) {
            int x = rightAnyExtent(sx, sy);
            int y = sy;
            if (!(graph.topLeftOfBlockedTile(x, y) && graph.bottomLeftOfBlockedTile(x, y))) {
                addSuccessor(x,y);
            }
        }
    }

    private final void generateStartingStates(int sx, int sy) {
        boolean bottomLeftOfBlocked = graph.bottomLeftOfBlockedTile(sx, sy);
        boolean bottomRightOfBlocked = graph.bottomRightOfBlockedTile(sx, sy);
        boolean topLeftOfBlocked = graph.topLeftOfBlockedTile(sx, sy);
        boolean topRightOfBlocked = graph.topRightOfBlockedTile(sx, sy);
        
        // Generate up
        if (!bottomLeftOfBlocked || !bottomRightOfBlocked) {
            double leftExtent, rightExtent;
            
            if (bottomLeftOfBlocked) {
                // Explore up-left
                leftExtent = toDouble(leftUpExtent(sx, sy));
                rightExtent = toDouble(sx);
            } else if (bottomRightOfBlocked) {
                // Explore up-right
                leftExtent = toDouble(sx);
                rightExtent = toDouble(rightUpExtent(sx, sy));
            } else {
                // Explore up-left-right
                leftExtent = toDouble(leftUpExtent(sx, sy));
                rightExtent = toDouble(rightUpExtent(sx, sy));
            }

            this.generateUpwards(leftExtent, rightExtent, sx, sy, sy, true, true);
        }

        // Generate down
        if (!topLeftOfBlocked || !topRightOfBlocked) {
            double leftExtent, rightExtent;
            
            if (topLeftOfBlocked) {
                // Explore down-left
                leftExtent = toDouble(leftDownExtent(sx, sy));
                rightExtent = toDouble(sx);
            } else if (topRightOfBlocked) {
                // Explore down-right
                leftExtent = toDouble(sx);
                rightExtent = toDouble(rightDownExtent(sx, sy));
            } else {
                // Explore down-left-right
                leftExtent = toDouble(leftDownExtent(sx, sy));
                rightExtent = toDouble(rightDownExtent(sx, sy));
            }

            this.generateDownwards(leftExtent, rightExtent, sx, sy, sy, true, true);
        }

        // Search leftwards
        if (!topRightOfBlocked || !bottomRightOfBlocked) {
            int x = leftAnyExtent(sx, sy);
            int y = sy;
            if (!(graph.topRightOfBlockedTile(x, y) && graph.bottomRightOfBlockedTile(x, y))) {
                addSuccessor(x,y);
            }
        }

        // Search rightwards
        if (!topLeftOfBlocked || !bottomLeftOfBlocked) {
            int x = rightAnyExtent(sx, sy);
            int y = sy;
            if (!(graph.topLeftOfBlockedTile(x, y) && graph.bottomLeftOfBlockedTile(x, y))) {
                addSuccessor(x,y);
            }
        }
    }
    
    private final void exploreStates(int sx, int sy) {
        while (intervalStackSize > 0) {
            LOSIntervalDouble currState = stackPop();
            boolean leftInclusive = (currState.inclusive & LOSIntervalDouble.LEFT_INCLUSIVE) != 0;
            boolean rightInclusive = (currState.inclusive & LOSIntervalDouble.RIGHT_INCLUSIVE) != 0;
            //System.out.println("POP " + currState);

            boolean zeroLengthInterval = isEqualTo(currState.xR, currState.xL);
            
            if (currState.y > sy) {
                // Upwards
                
                // Insert endpoints if integer.
                if (leftInclusive && isWholeNumber(currState.xL)) {
                    /* The two cases   _
                     *  _             |X|
                     * |X|'.           ,'
                     *      '.       ,'
                     *        B     B
                     */
                    
                    int x = round(currState.xL);
                    int y = currState.y;
                    boolean topRightOfBlockedTile = graph.topRightOfBlockedTile(x, y);
                    boolean bottomRightOfBlockedTile = graph.bottomRightOfBlockedTile(x, y);
                    
                    if (x <= sx && topRightOfBlockedTile && !bottomRightOfBlockedTile) {
                        addSuccessor(x, y);
                        leftInclusive = false;
                    }
                    else if (sx <= x && bottomRightOfBlockedTile && !topRightOfBlockedTile) {
                        addSuccessor(x, y);
                        leftInclusive = false;
                    }
                }
                if (rightInclusive && isWholeNumber(currState.xR)) {
                    /*   _   The two cases
                     *  |X|             _
                     *  '.           ,'|X|
                     *    '.       ,'
                     *      B     B
                     */
                    
                    int x = round(currState.xR);
                    int y = currState.y;
                    boolean bottomLeftOfBlockedTile = graph.bottomLeftOfBlockedTile(x, y);
                    boolean topLeftOfBlockedTile = graph.topLeftOfBlockedTile(x, y);
                    
                    if (x <= sx && bottomLeftOfBlockedTile && !topLeftOfBlockedTile) {
                        if (leftInclusive || !zeroLengthInterval) {
                            addSuccessor(x, y);
                            rightInclusive = false;
                        }
                    }
                    else if (sx <= x && topLeftOfBlockedTile  && !bottomLeftOfBlockedTile) {
                        if (leftInclusive || !zeroLengthInterval) {
                            addSuccessor(x, y);
                            rightInclusive = false;
                        }
                    }
                }
                
                
                
                // Generate Upwards
                /*
                 * =======      =====    =====
                 *  \   /       / .'      '. \
                 *   \ /   OR  /.'    OR    '.\
                 *    B       B                B
                 */

                // (Px-Bx)*(Py-By+1)/(Py-By) + Bx
                int dy = currState.y - sy;
                double leftProjection = (double)(currState.xL - sx)*(dy+1)/dy + sx;

                int leftBound = leftUpExtent(ceil(currState.xL), currState.y);
                if (isWholeNumber(currState.xL) && graph.bottomRightOfBlockedTile(round(currState.xL), currState.y)) leftBound = round(currState.xL);
                
                if (isLessThan(leftProjection, leftBound)) { // leftProjection < leftBound
                    leftProjection = toDouble(leftBound);
                    leftInclusive = true;
                }

                // (Px-Bx)*(Py-By+1)/(Py-By) + Bx
                double rightProjection = (double)(currState.xR - sx)*(dy+1)/dy + sx;
                
                int rightBound = rightUpExtent(floor(currState.xR), currState.y);
                if (isWholeNumber(currState.xR) && graph.bottomLeftOfBlockedTile(round(currState.xR), currState.y)) rightBound = round(currState.xR);

                if (!isLessThanOrEqual(rightProjection, rightBound)) { // rightBound < rightProjection
                    rightProjection = toDouble(rightBound);
                    rightInclusive = true;
                }

                // Call Generate
                if (leftInclusive && rightInclusive) {
                    if (isLessThanOrEqual(leftProjection, rightProjection)) {
                        generateUpwards(leftProjection, rightProjection, sx, sy, currState.y, true, true);
                    }
                }
                else if (isLessThan(leftProjection, rightProjection)) {
                    generateUpwards(leftProjection, rightProjection, sx, sy, currState.y, leftInclusive, rightInclusive);
                }
            }
            else {
                // Upwards
                
                // Insert endpoints if integer.
                if (leftInclusive && isWholeNumber(currState.xL)) {
                    /* The two cases
                     *        B     B
                     *  _   ,'       '.
                     * |X|.'           '.
                     *                |X|
                     */
                    
                    int x = round(currState.xL);
                    int y = currState.y;
                    boolean bottomRightOfBlockedTile = graph.bottomRightOfBlockedTile(x, y);
                    boolean topRightOfBlockedTile = graph.topRightOfBlockedTile(x, y);
                    
                    if (x <= sx && bottomRightOfBlockedTile && !topRightOfBlockedTile) {
                        addSuccessor(x, y);
                        leftInclusive = false;
                    }
                    else if (sx <= x && topRightOfBlockedTile && !bottomRightOfBlockedTile) {
                        addSuccessor(x, y);
                        leftInclusive = false;
                    }
                }
                if (rightInclusive && isWholeNumber(currState.xR)) {
                    /*       The two cases
                     *      B     B
                     *    .'       '.   _
                     *  .'           '.|X|
                     *  |X|
                     */
                    
                    int x = round(currState.xR);
                    int y = currState.y;
                    boolean topLeftOfBlockedTile = graph.topLeftOfBlockedTile(x, y);
                    boolean bottomLeftOfBlockedTile = graph.bottomLeftOfBlockedTile(x, y);
                    
                    if (x <= sx && topLeftOfBlockedTile && !bottomLeftOfBlockedTile) {
                        if (leftInclusive || !zeroLengthInterval) {
                            addSuccessor(x, y);
                            rightInclusive = false;
                        }
                    }
                    else if (sx <= x && bottomLeftOfBlockedTile  && !topLeftOfBlockedTile) {
                        if (leftInclusive || !zeroLengthInterval) {
                            addSuccessor(x, y);
                            rightInclusive = false;
                        }
                    }
                }


                
                // Generate downwards
                /*
                 *    B       B                B
                 *   / \   OR  \'.    OR    .'/
                 *  /   \       \ '.      .' /
                 * =======      =====    =====
                 */

                // (Px-Bx)*(Py-By+1)/(Py-By) + Bx
                int dy = sy - currState.y; 
                double leftProjection = (double)(currState.xL - sx)*(dy+1)/dy + sx;
                
                int leftBound = leftDownExtent(ceil(currState.xL), currState.y);
                if (isWholeNumber(currState.xL) && graph.topRightOfBlockedTile(round(currState.xL), currState.y)) leftBound = round(currState.xL);
                
                if (isLessThan(leftProjection, leftBound)) { // leftProjection < leftBound
                    leftProjection = toDouble(leftBound);
                    leftInclusive = true;
                }

                // (Px-Bx)*(Py-By+1)/(Py-By) + Bx
                double rightProjection = (double)(currState.xR - sx)*(dy+1)/dy + sx;

                int rightBound = rightDownExtent(floor(currState.xR), currState.y);
                if (isWholeNumber(currState.xR) && graph.topLeftOfBlockedTile(round(currState.xR), currState.y)) rightBound = round(currState.xR);
                
                if (!isLessThanOrEqual(rightProjection, rightBound)) { // rightBound < rightProjection
                    rightProjection = toDouble(rightBound);
                    rightInclusive = true;
                }

                // Call Generate
                if (leftInclusive && rightInclusive) {
                    if (isLessThanOrEqual(leftProjection, rightProjection)) {
                        generateDownwards(leftProjection, rightProjection, sx, sy, currState.y, true, true);
                    }
                }
                else if (isLessThan(leftProjection, rightProjection)) {
                    generateDownwards(leftProjection, rightProjection, sx, sy, currState.y, leftInclusive, rightInclusive);
                }
            }
        }
    }
    
    

    private final void exploreStatesNonTaut(int sx, int sy) {
        while (intervalStackSize > 0) {
            LOSIntervalDouble currState = stackPop();
            boolean leftInclusive = (currState.inclusive & LOSIntervalDouble.LEFT_INCLUSIVE) != 0;
            boolean rightInclusive = (currState.inclusive & LOSIntervalDouble.RIGHT_INCLUSIVE) != 0;
            //System.out.println("POP " + currState);

            boolean zeroLengthInterval = isEqualTo(currState.xR, currState.xL);
            
            if (currState.y > sy) {
                // Upwards
                
                // Insert endpoints if integer.
                if (leftInclusive && isWholeNumber(currState.xL)) {
                    /* The two cases   _
                     *  _             |X|
                     * |X|'.           ,'
                     *      '.       ,'
                     *        B     B
                     */
                    
                    int x = round(currState.xL);
                    int y = currState.y;
                    
                    if (graph.isOuterCorner(x, y)) {
                        addSuccessor(x, y);
                        //leftInclusive = false;
                    }
                }
                if (rightInclusive && isWholeNumber(currState.xR)) {
                    /*   _   The two cases
                     *  |X|             _
                     *  '.           ,'|X|
                     *    '.       ,'
                     *      B     B
                     */
                    
                    int x = round(currState.xR);
                    int y = currState.y;
                    
                    if (graph.isOuterCorner(x, y)) {
                        addSuccessor(x, y);
                        //rightInclusive = false;
                    }
                }
                
                
                
                // Generate Upwards
                /*
                 * =======      =====    =====
                 *  \   /       / .'      '. \
                 *   \ /   OR  /.'    OR    '.\
                 *    B       B                B
                 */

                // (Px-Bx)*(Py-By+1)/(Py-By) + Bx
                int dy = currState.y - sy;
                double leftProjection = (double)(currState.xL - sx)*(dy+1)/dy + sx;

                int leftBound = leftUpExtent(ceil(currState.xL), currState.y);
                if (isWholeNumber(currState.xL) && graph.bottomRightOfBlockedTile(round(currState.xL), currState.y)) leftBound = round(currState.xL);
                
                if (isLessThan(leftProjection, leftBound)) { // leftProjection < leftBound
                    leftProjection = toDouble(leftBound);
                    leftInclusive = true;
                }

                // (Px-Bx)*(Py-By+1)/(Py-By) + Bx
                double rightProjection = (double)(currState.xR - sx)*(dy+1)/dy + sx;
                
                int rightBound = rightUpExtent(floor(currState.xR), currState.y);
                if (isWholeNumber(currState.xR) && graph.bottomLeftOfBlockedTile(round(currState.xR), currState.y)) rightBound = round(currState.xR);

                if (!isLessThanOrEqual(rightProjection, rightBound)) { // rightBound < rightProjection
                    rightProjection = toDouble(rightBound);
                    rightInclusive = true;
                }

                // Call Generate
                if (leftInclusive && rightInclusive) {
                    if (isLessThanOrEqual(leftProjection, rightProjection)) {
                        generateUpwards(leftProjection, rightProjection, sx, sy, currState.y, true, true);
                    }
                }
                else if (isLessThan(leftProjection, rightProjection)) {
                    generateUpwards(leftProjection, rightProjection, sx, sy, currState.y, leftInclusive, rightInclusive);
                }
            }
            else {
                // Upwards
                
                // Insert endpoints if integer.
                if (leftInclusive && isWholeNumber(currState.xL)) {
                    /* The two cases
                     *        B     B
                     *  _   ,'       '.
                     * |X|.'           '.
                     *                |X|
                     */
                    
                    int x = round(currState.xL);
                    int y = currState.y;
                    
                    if (graph.isOuterCorner(x, y)) {
                        addSuccessor(x, y);
                        //leftInclusive = false;
                    }
                }
                if (rightInclusive && isWholeNumber(currState.xR)) {
                    /*       The two cases
                     *      B     B
                     *    .'       '.   _
                     *  .'           '.|X|
                     *  |X|
                     */
                    
                    int x = round(currState.xR);
                    int y = currState.y;
                    if (graph.isOuterCorner(x, y)) {
                        addSuccessor(x, y);
                        //rightInclusive = false;
                    }
                }


                
                // Generate downwards
                /*
                 *    B       B                B
                 *   / \   OR  \'.    OR    .'/
                 *  /   \       \ '.      .' /
                 * =======      =====    =====
                 */

                // (Px-Bx)*(Py-By+1)/(Py-By) + Bx
                int dy = sy - currState.y; 
                double leftProjection = (double)(currState.xL - sx)*(dy+1)/dy + sx;
                
                int leftBound = leftDownExtent(ceil(currState.xL), currState.y);
                if (isWholeNumber(currState.xL) && graph.topRightOfBlockedTile(round(currState.xL), currState.y)) leftBound = round(currState.xL);
                
                if (isLessThan(leftProjection, leftBound)) { // leftProjection < leftBound
                    leftProjection = toDouble(leftBound);
                    leftInclusive = true;
                }

                // (Px-Bx)*(Py-By+1)/(Py-By) + Bx
                double rightProjection = (double)(currState.xR - sx)*(dy+1)/dy + sx;

                int rightBound = rightDownExtent(floor(currState.xR), currState.y);
                if (isWholeNumber(currState.xR) && graph.topLeftOfBlockedTile(round(currState.xR), currState.y)) rightBound = round(currState.xR);
                
                if (!isLessThanOrEqual(rightProjection, rightBound)) { // rightBound < rightProjection
                    rightProjection = toDouble(rightBound);
                    rightInclusive = true;
                }

                // Call Generate
                if (leftInclusive && rightInclusive) {
                    if (isLessThanOrEqual(leftProjection, rightProjection)) {
                        generateDownwards(leftProjection, rightProjection, sx, sy, currState.y, true, true);
                    }
                }
                else if (isLessThan(leftProjection, rightProjection)) {
                    generateDownwards(leftProjection, rightProjection, sx, sy, currState.y, leftInclusive, rightInclusive);
                }
            }
        }
    }
    
    
    
    

    private final int leftUpExtent(int xL, int y) {
        return xL > sizeX ? sizeX : leftDownExtents[y+1][xL];
    }

    private final int leftDownExtent(int xL, int y) {
        return xL > sizeX ? sizeX : leftDownExtents[y][xL];
    }
    
    private final int leftAnyExtent(int xL, int y) {
        return Math.max(leftDownExtents[y][xL], leftDownExtents[y+1][xL]);
    }

    private final int rightUpExtent(int xR, int y) {
        return xR < 0 ? 0 : rightDownExtents[y+1][xR];
    }

    private final int rightDownExtent(int xR, int y) {
        return xR < 0 ? 0 : rightDownExtents[y][xR];
    }

    private final int rightAnyExtent(int xR, int y) {
        return Math.min(rightDownExtents[y][xR], rightDownExtents[y+1][xR]);
    }

    private final void generateUpwards(double leftBound, double rightBound, int sx, int sy, int currY, boolean leftInclusive, boolean rightInclusive) {
        generateAndSplitIntervals(
                currY + 2, currY + 1,
                sx, sy,
                leftBound, rightBound,
                leftInclusive, rightInclusive);
    }

    private final void generateDownwards(double leftBound, double rightBound, int sx, int sy, int currY, boolean leftInclusive, boolean rightInclusive) {
        generateAndSplitIntervals(
                currY - 1, currY - 1,
                sx, sy,
                leftBound, rightBound,
                leftInclusive, rightInclusive);
    }
    
    /**
     * Called by generateUpwards / Downwards.
     * Note: Unlike Anya, 0-length intervals are possible.
     */
    private final void generateAndSplitIntervals(int checkY, int newY, int sx, int sy, double leftBound, double rightBound, boolean leftInclusive, boolean rightInclusive) {
        double left = leftBound;
        int leftFloor = floor(left);

        // Up: !bottomRightOfBlockedTile && bottomLeftOfBlockedTile
        if (leftInclusive && isWholeNumber(left) && !graph.isBlocked(leftFloor-1, checkY-1) && graph.isBlocked(leftFloor, checkY-1)) {
            stackPush(new LOSIntervalDouble(newY, left, left, LOSIntervalDouble.BOTH_INCLUSIVE));
        }

        // Divide up the intervals.
        while(true) {
            int right = rightDownExtents[checkY][leftFloor]; // it's actually rightDownExtents for exploreDownwards. (thus we use checkY = currY - 2)
            if (isLessThanOrEqual(rightBound, right)) break; // right < rightBound            
            
            // Only push unblocked ( bottomRightOfBlockedTile )
            if (!graph.isBlocked(right-1, checkY-1)) {
                stackPush(new LOSIntervalDouble(newY, left, toDouble(right), leftInclusive ? LOSIntervalDouble.BOTH_INCLUSIVE : LOSIntervalDouble.RIGHT_INCLUSIVE));
            }
            
            leftFloor = right;
            left = toDouble(leftFloor);
            leftInclusive = true;
        }

        // The last interval will always be here.
        // if !bottomLeftOfBlockedTile(leftFloor, checkY)
        if (!graph.isBlocked(leftFloor, checkY-1)) {
            int inclusive = (leftInclusive ? LOSIntervalDouble.LEFT_INCLUSIVE : 0) | (rightInclusive ? LOSIntervalDouble.RIGHT_INCLUSIVE : 0); 
            stackPush(new LOSIntervalDouble(newY, left, rightBound, inclusive));
        } else {
            // The possibility of there being one degenerate interval at the end. ( !bottomLeftOfBlockedTile(xR, checkY) )
            if (rightInclusive && isWholeNumber(rightBound) && !graph.isBlocked(round(rightBound), checkY-1)) {
                stackPush(new LOSIntervalDouble(newY, rightBound, rightBound, LOSIntervalDouble.BOTH_INCLUSIVE));
            }
        }
    }

    public static void clearMemory() {
        snapshotList.clear();
        snapshots.clear();
        rightDownExtents = null;
        leftDownExtents = null;
        intervalStack = null;
        successorsX = null;
        successorsY = null;
        System.gc();
    }
    
} public class SparseVisibilityGraph {

    private static SparseVisibilityGraph storedVisibilityGraph;
    private static GridGraph storedGridGraph;
    
    private final GridGraph graph;
    private LineOfSightScannerDouble losScanner;
    private final int sizeXPlusOne;
    private final int sizeYPlusOne;

    private int[] nodeIndex; // Flattened 2D Array
    private int startIndex = -1;
    private int endIndex = -1;
    
    private Runnable saveSnapshot;

    private int originalSize;
    private int maxSize;
    private SVGNode[] nodes;
    private int nNodes;

    private int[] startStoredNeighbours;
    private float[] startStoredWeights;
    private int startStoredNumNeighbours;
    private int[] endStoredNeighbours;
    private float[] endStoredWeights;
    private int endStoredNumNeighbours;

    public SparseVisibilityGraph(GridGraph graph) {
        this.graph = graph;
        this.sizeXPlusOne = graph.sizeX+1;
        this.sizeYPlusOne = graph.sizeY+1;
    }

    public final void setSaveSnapshotFunction(Runnable saveSnapshot) {
        this.saveSnapshot = saveSnapshot;
    }

    public final void initialise(int sx, int sy, int ex, int ey) {
        // Check if graph already initialised
        if (nodes == null) {
            long _st = System.nanoTime();
            losScanner = new LineOfSightScannerDouble(graph);
            
            nodes = new SVGNode[11];
            nNodes = 0;
            
            addNodes();
            
            originalSize = nNodes;
            maxSize = nNodes + 2;
            nodes = Arrays.copyOf(nodes, maxSize);
            
            addAllEdges();
            
            long _ed = System.nanoTime();
            System.out.println("Construction Time: " + (_ed-_st)/1000000.);
        }

        restoreOriginalGraph();
        addStartAndEnd(sx,sy,ex,ey);
    }

    private final void addNodes() {
        nodeIndex = new int[sizeYPlusOne*sizeXPlusOne];
        for (int y=0; y<sizeYPlusOne; y++) {
            for (int x=0; x<sizeXPlusOne; x++) {
                if (graph.isOuterCorner(x, y)) {
                    nodeIndex[y*sizeXPlusOne + x] = assignNode(x, y);
                } else {
                    nodeIndex[y*sizeXPlusOne + x] = -1;
                }
            }
        }
    }

    private final int assignNode(int x, int y) {
        int index = nNodes;
        if (index >= nodes.length) nodes = Arrays.copyOf(nodes, nodes.length*2);
        nodes[index] = new SVGNode(x,y);
        nNodes++;
        return index;
    }
    
    private final void restoreOriginalGraph() {
        // Safeguard against multiple restores.
        if (startIndex == -1) return;
        
        // Reset hasEdgeToGoal array.
        markHasEdgeToGoal(false);
        // PostCondition: hasEdgeToGoal has all values == false.
        if (endIndex >= originalSize) {
            SVGNode e = nodes[endIndex];
            nodeIndex[e.y*sizeXPlusOne + e.x] = -1;
        } else {
            SVGNode endNode = nodes[endIndex];
            endNode.outgoingEdges = endStoredNeighbours;
            endNode.edgeWeights = endStoredWeights;
            endNode.nEdges = endStoredNumNeighbours;
            endStoredNeighbours = null;
            endStoredWeights = null;
        }
        
        if (startIndex >= originalSize) {
            SVGNode s = nodes[startIndex];
            nodeIndex[s.y*sizeXPlusOne + s.x] = -1;
        } else {
            SVGNode startNode = nodes[startIndex];
            startNode.outgoingEdges = startStoredNeighbours;
            startNode.edgeWeights = startStoredWeights;
            startNode.nEdges = startStoredNumNeighbours;
            startStoredNeighbours = null;
            startStoredWeights = null;
        }

        // Reset size.
        nNodes = originalSize;
        
        startIndex = -1;
        endIndex = -1;
    }

    public final void addStartAndEnd(int sx, int sy, int ex, int ey) {
        
        // START:
        if (nodeIndex[sy*sizeXPlusOne + sx] == -1) {
            startIndex = nNodes;
            ++nNodes;
            
            nodeIndex[sy*sizeXPlusOne + sx] = startIndex;
            nodes[startIndex] = new SVGNode(sx, sy);
        } else {
            startIndex = nodeIndex[sy*sizeXPlusOne + sx];
            SVGNode startNode = nodes[startIndex];
            startStoredNeighbours = startNode.outgoingEdges;
            startStoredWeights = startNode.edgeWeights;
            startStoredNumNeighbours = startNode.nEdges;
            startNode.outgoingEdges = new int[11];
            startNode.edgeWeights = new float[11];
            startNode.nEdges = 0;
        }
        addEdgesToVisibleNeighbours(startIndex, sx, sy);
        
        // END:
        if (nodeIndex[ey*sizeXPlusOne + ex] == -1) {
            endIndex = nNodes;
            ++nNodes;

            nodeIndex[ey*sizeXPlusOne + ex] = endIndex;
            nodes[endIndex] = new SVGNode(ex, ey);
            
        } else {
            endIndex = nodeIndex[ey*sizeXPlusOne + ex];
            SVGNode endNode = nodes[endIndex];
            endStoredNeighbours = endNode.outgoingEdges;
            endStoredWeights = endNode.edgeWeights;
            endStoredNumNeighbours = endNode.nEdges;
            endNode.outgoingEdges = new int[11];
            endNode.edgeWeights = new float[11];
            endNode.nEdges = 0;
        }
        addEdgesToVisibleNeighbours(endIndex, ex, ey);
        
        // Mark all hasEdgeToGoal vertices.
        markHasEdgeToGoal(true);
    }

    private final int addEdgesToVisibleNeighbours(int index, int x, int y) {
        losScanner.computeAllVisibleTautSuccessors(x, y);
        int nSuccessors = losScanner.nSuccessors;
        for (int i=0;i<nSuccessors;++i) {
            int toX = losScanner.successorsX[i];
            int toY = losScanner.successorsY[i];
            int targetIndex = nodeIndex[toY*sizeXPlusOne + toX];

            float weight = graph.distance(x, y, toX, toY);
            addEdge(index, targetIndex, weight);
        }
        
        return index;
    }

    private final void markHasEdgeToGoal(boolean value) {
        int[] endNeighbours = nodes[endIndex].outgoingEdges;
        int n = nodes[endIndex].nEdges;
        for (int i=0;i<n;++i) {
            nodes[endNeighbours[i]].hasEdgeToGoal = value;
        }
    }
    
    private final void addAllEdges() {
        int fromX, fromY, toX, toY;
        for (int i=0;i<nNodes;++i) {
            fromX = xCoordinateOf(i);
            fromY = yCoordinateOf(i);
            
            losScanner.computeAllVisibleTwoWayTautSuccessors(fromX, fromY);
            int nSuccessors = losScanner.nSuccessors;
            for (int j=0;j<nSuccessors;++j) {
                toX = losScanner.successorsX[j];
                toY = losScanner.successorsY[j];
                int toIndex = nodeIndex[toY*sizeXPlusOne + toX];

                float weight = graph.distance(fromX, fromY, toX, toY);
                addEdge(i, toIndex, weight);
            }
        }
    }
    
    private final void addEdge(int fromI, int toI, float weight) {
        nodes[fromI].addEdge(toI, weight);
    }
    
    public final int xCoordinateOf(int index) {
        return nodes[index].x;
    }

    public final int yCoordinateOf(int index) {
        return nodes[index].y;
    }

    public final int size() {
        return nNodes;
    }
    
    public final int maxSize() {
        return maxSize;
    }
    
    public final int computeSumDegrees() {
        int sum = 0;
        for (int i=0;i<nNodes;++i) {
            sum += nodes[i].nEdges;
        }
        return sum;
    }

    public final SVGNode getOutgoingEdges(int source) {
        return nodes[source];
    }

    public final int startNode() {
        return startIndex;
    }

    public final int endNode() {
        return endIndex;
    }
    
    private final void maybeSaveSnapshot() {
        if (saveSnapshot != null) saveSnapshot.run();
    }
    
    
    private static final SparseVisibilityGraph repurpose(SparseVisibilityGraph oldGraph) {
        SparseVisibilityGraph newGraph = new SparseVisibilityGraph(oldGraph.graph);
        newGraph.nodeIndex = oldGraph.nodeIndex;
        newGraph.startIndex = oldGraph.startIndex;
        newGraph.endIndex = oldGraph.endIndex;
        newGraph.nodes = oldGraph.nodes;
        newGraph.nNodes = oldGraph.nNodes;
        newGraph.originalSize = oldGraph.originalSize;
        newGraph.maxSize = oldGraph.maxSize;
        newGraph.losScanner = oldGraph.losScanner;

        newGraph.startStoredNeighbours = oldGraph.startStoredNeighbours;
        newGraph.startStoredWeights = oldGraph.startStoredWeights;
        newGraph.startStoredNumNeighbours = oldGraph.startStoredNumNeighbours;
        newGraph.endStoredNeighbours = oldGraph.endStoredNeighbours;
        newGraph.endStoredWeights = oldGraph.endStoredWeights;
        newGraph.endStoredNumNeighbours = oldGraph.endStoredNumNeighbours;
        
        return newGraph;
    }
    
    public static final SparseVisibilityGraph getStoredGraph(GridGraph graph) {
        SparseVisibilityGraph visibilityGraph = null;
        if (storedGridGraph != graph || storedVisibilityGraph == null) {
            //("Get new graph");
            visibilityGraph = new SparseVisibilityGraph(graph);
            storedVisibilityGraph = visibilityGraph;
            storedGridGraph = graph;
        } else {
            //("Reuse graph");
            visibilityGraph = repurpose(storedVisibilityGraph);
            storedVisibilityGraph = visibilityGraph;
        }
        return visibilityGraph;
    }

    private final void printAllEdges() {
        for (int i=0;i<nNodes;++i) {
            SVGNode node = nodes[i];
            System.out.println("Node " + i + " : " + node.nEdges + " edges");
            for (int j=0;j<node.nEdges;++j) {
                System.out.println(i + " -> " + node.outgoingEdges[j]);
            }
        }
    }

    public static void clearMemory() {
        storedVisibilityGraph = null;
        storedGridGraph = null;
        System.gc();
    }
    
} public class VisibilityGraphAlgorithmOptimised extends AStarStaticMemory {
    protected VisibilityGraphOptimised visibilityGraph;
    protected boolean reuseGraph = false;
    
    private VisibilityGraphAlgorithmOptimised(GridGraph graph, int sx, int sy, int ex, int ey) {
        super(graph, sx, sy, ex, ey);
    }
    
    public static VisibilityGraphAlgorithmOptimised graphReuse(GridGraph graph, int sx, int sy, int ex, int ey) {
        VisibilityGraphAlgorithmOptimised algo = new VisibilityGraphAlgorithmOptimised(graph, sx, sy, ex, ey);
        algo.reuseGraph = true;
        return algo;
    }

    public VisibilityGraphOptimised getVisibilityGraph() {
        return visibilityGraph;
    }
    
    @Override
    public void computePath() {
        setupVisibilityGraph();

        int size = visibilityGraph.size();
        int memorySize = visibilityGraph.maxSize();
        pq = new ReusableIndirectHeap(size, memorySize);
        this.initialiseMemory(memorySize, Float.POSITIVE_INFINITY, -1, false);
        
        initialise(visibilityGraph.startNode());
        int finish = visibilityGraph.endNode();

        if (graph.lineOfSight(sx, sy, ex, ey)) {
            // There is a direct path from (sx, sy) to (ex, ey).
            if (sx != ex || sy != ey) {
                setParent(finish, visibilityGraph.startNode());
            }
            return;
        }

        while (!pq.isEmpty()) {
            int current = pq.popMinIndex();
            setVisited(current, true);
TimeCounter.counterA++;
            if (current == finish) {
                break;
            }
            
            VGNode node = visibilityGraph.getOutgoingEdges(current);
            int[] outgoingEdges = node.outgoingEdges;
            float[] outgoingWeights = node.edgeWeights;
            int nEdges = node.nEdges;
            
            for (int i=0;i<nEdges;++i) {
                int dest = outgoingEdges[i];
                float weight = outgoingWeights[i];
                
                if (!visited(dest) && relax(current, dest, weight)) {
                    // If relaxation is done.
                    int destX = visibilityGraph.xCoordinateOf(dest);
                    int destY = visibilityGraph.yCoordinateOf(dest);
                    
                    pq.decreaseKey(dest, distance(dest) + graph.distance(destX, destY, ex, ey));
                }
            }
            if (node.hasEdgeToGoal) {
                float weight = graph.distance(node.x, node.y, ex, ey);
                if (relax(current, finish, weight)) {
                    pq.decreaseKey(finish, distance(finish));
                }
            }
            
            maybeSaveSearchSnapshot();
        }
    }

    protected void setupVisibilityGraph() {
        if (reuseGraph) {
            visibilityGraph = VisibilityGraphOptimised.getStoredGraph(graph);
        } else {
            visibilityGraph = new VisibilityGraphOptimised(graph);
        }
        
        if (isRecording()) {
            visibilityGraph.setSaveSnapshotFunction(()->saveVisibilityGraphSnapshot());
            visibilityGraph.initialise(sx, sy, ex, ey);
            saveVisibilityGraphSnapshot();
        } else {
            visibilityGraph.initialise(sx, sy, ex, ey);
        }
    }

    protected final boolean relax(int u, int v, float weightUV) {
        // return true iff relaxation is done.
        float newWeight = distance(u) + weightUV;
        if (newWeight < distance(v)) {
            int p = parent(u);
            if (p != -1) {
                int x1 = visibilityGraph.xCoordinateOf(p);
                int y1 = visibilityGraph.yCoordinateOf(p);
                int x2 = visibilityGraph.xCoordinateOf(u);
                int y2 = visibilityGraph.yCoordinateOf(u);
                int x3 = visibilityGraph.xCoordinateOf(v);
                int y3 = visibilityGraph.yCoordinateOf(v);
                
                if (!graph.isTaut(x1,y1,x2,y2,x3,y3)) return false;
            }
            setDistance(v, newWeight);
            setParent(v, u);
            return true;
        }
        return false;
    }
    

    private int pathLength() {
        int length = 0;
        int current = visibilityGraph.endNode();
        while (current != -1) {
            current = parent(current);
            length++;
        }
        return length;
    }

    @Override
    public int[][] getPath() {
        int length = pathLength();
        int[][] path = new int[length][];
        int current = visibilityGraph.endNode();
        
        int index = length-1;
        while (current != -1) {
            int x = visibilityGraph.xCoordinateOf(current);
            int y = visibilityGraph.yCoordinateOf(current);
            
            path[index] = new int[2];
            path[index][0] = x;
            path[index][1] = y;
            
            index--;
            current = parent(current);
        }
        
        return path;
    }

    @Override
    protected int goalParentIndex() {
        return visibilityGraph.endNode();
    }

    @Override
    protected Integer[] snapshotEdge(int endIndex) {
        Integer[] edge = new Integer[4];
        int startIndex = parent(endIndex);
        edge[0] = visibilityGraph.xCoordinateOf(startIndex);
        edge[1] = visibilityGraph.yCoordinateOf(startIndex);
        edge[2] = visibilityGraph.xCoordinateOf(endIndex);
        edge[3] = visibilityGraph.yCoordinateOf(endIndex);
        return edge;
    }

    @Override
    protected Integer[] snapshotVertex(int index) {
        if (selected(index)) {
            Integer[] edge = new Integer[2];
            edge[0] = visibilityGraph.xCoordinateOf(index);
            edge[1] = visibilityGraph.yCoordinateOf(index);
            return edge;
        }
        return null;
    }
    
    protected void saveVisibilityGraphSnapshot() {
        /*if (!isRecording()) {
            return;
        }*/
        int size = visibilityGraph.size();
        
        List<SnapshotItem> snapshotItemList = new ArrayList<>(size);

        for (int i=0;i<size;i++) {
            VGNode node = visibilityGraph.getOutgoingEdges(i);
            int[] outgoingEdges = node.outgoingEdges;
            int nEdges = node.nEdges;
            
            for (int j=0;j<nEdges;++j) {
                int source = i;
                int dest = outgoingEdges[j];
                
                if (source < dest) {
                    Integer[] path = new Integer[4];
                    path[0] = visibilityGraph.xCoordinateOf(source);
                    path[1] = visibilityGraph.yCoordinateOf(source);
                    path[2] = visibilityGraph.xCoordinateOf(dest);
                    path[3] = visibilityGraph.yCoordinateOf(dest);
                    
                    SnapshotItem snapshotItem = SnapshotItem.generate(path, Color.GREEN);
                    snapshotItemList.add(snapshotItem);
                }
            }
        }
        addSnapshot(snapshotItemList);
    }
    
    @Override
    public void printStatistics() {
        System.out.println("Nodes: " + visibilityGraph.size());
        System.out.println("Edges: " + (visibilityGraph.computeSumDegrees()/2));
    }
} public class VisibilityGraphOptimised {

    private static VisibilityGraphOptimised storedVisibilityGraph;
    private static GridGraph storedGridGraph;
    
    private final GridGraph graph;
    private LineOfSightScanner losScanner;
    private int[][] nodeIndex;
    private int startIndex = -1;
    private int endIndex = -1;
    
    private Runnable saveSnapshot;

    private int originalSize;
    private int maxSize;
    private VGNode[] nodes;
    private int nNodes;

    private int[] startStoredNeighbours;
    private float[] startStoredWeights;
    private int startStoredNumNeighbours;
    private int[] endStoredNeighbours;
    private float[] endStoredWeights;
    private int endStoredNumNeighbours;

    public VisibilityGraphOptimised(GridGraph graph) {
        this.graph = graph;
    }

    public final void setSaveSnapshotFunction(Runnable saveSnapshot) {
        this.saveSnapshot = saveSnapshot;
    }

    public final void initialise(int sx, int sy, int ex, int ey) {
        // Check if graph already initialised
        if (nodes == null) {
            long _st = System.nanoTime();
            losScanner = new LineOfSightScanner(graph);
            
            nodes = new VGNode[11];
            nNodes = 0;
            
            addNodes();
            
            originalSize = nNodes;
            maxSize = nNodes + 2;
            nodes = Arrays.copyOf(nodes, maxSize);
            
            addAllEdges();
            
            long _ed = System.nanoTime();
            System.out.println("Construction Time: " + (_ed-_st)/1000000.);
        }

        restoreOriginalGraph();
        addStartAndEnd(sx,sy,ex,ey);
    }

    private final void addNodes() {
        nodeIndex = new int[graph.sizeY+1][];
        for (int y=0;y<nodeIndex.length;y++) {
            nodeIndex[y] = new int[graph.sizeX+1];
            for (int x=0; x<nodeIndex[y].length; x++) {
                if (graph.isOuterCorner(x, y)) {
                    nodeIndex[y][x] = assignNode(x, y);
                } else {
                    nodeIndex[y][x] = -1;
                }
            }
        }
    }

    private final int assignNode(int x, int y) {
        int index = nNodes;
        if (index >= nodes.length) nodes = Arrays.copyOf(nodes, nodes.length*2);
        nodes[index] = new VGNode(x,y);
        nNodes++;
        return index;
    }
    
    private final void restoreOriginalGraph() {
        // Safeguard against multiple restores.
        if (startIndex == -1) return;
        
        // Reset hasEdgeToGoal array.
        markHasEdgeToGoal(false);
        // PostCondition: hasEdgeToGoal has all values == false.
        if (endIndex >= originalSize) {
            VGNode e = nodes[endIndex];
            nodeIndex[e.y][e.x] = -1;
        } else {
            VGNode endNode = nodes[endIndex];
            endNode.outgoingEdges = endStoredNeighbours;
            endNode.edgeWeights = endStoredWeights;
            endNode.nEdges = endStoredNumNeighbours;
            endStoredNeighbours = null;
            endStoredWeights = null;
        }
        
        if (startIndex >= originalSize) {
            VGNode s = nodes[startIndex];
            nodeIndex[s.y][s.x] = -1;
        } else {
            VGNode startNode = nodes[startIndex];
            startNode.outgoingEdges = startStoredNeighbours;
            startNode.edgeWeights = startStoredWeights;
            startNode.nEdges = startStoredNumNeighbours;
            startStoredNeighbours = null;
            startStoredWeights = null;
        }

        // Reset size.
        nNodes = originalSize;
        
        startIndex = -1;
        endIndex = -1;
    }

    public final void addStartAndEnd(int sx, int sy, int ex, int ey) {
        
        // START:
        if (nodeIndex[sy][sx] == -1) {
            startIndex = nNodes;
            ++nNodes;
            
            nodeIndex[sy][sx] = startIndex;
            nodes[startIndex] = new VGNode(sx, sy);
        } else {
            startIndex = nodeIndex[sy][sx];
            VGNode startNode = nodes[startIndex];
            startStoredNeighbours = startNode.outgoingEdges;
            startStoredWeights = startNode.edgeWeights;
            startStoredNumNeighbours = startNode.nEdges;
            startNode.outgoingEdges = new int[11];
            startNode.edgeWeights = new float[11];
            startNode.nEdges = 0;
        }
        addEdgesToVisibleNeighbours(startIndex, sx, sy);
        
        // END:
        if (nodeIndex[ey][ex] == -1) {
            endIndex = nNodes;
            ++nNodes;

            nodeIndex[ey][ex] = endIndex;
            nodes[endIndex] = new VGNode(ex, ey);
            
        } else {
            endIndex = nodeIndex[ey][ex];
            VGNode endNode = nodes[endIndex];
            endStoredNeighbours = endNode.outgoingEdges;
            endStoredWeights = endNode.edgeWeights;
            endStoredNumNeighbours = endNode.nEdges;
            endNode.outgoingEdges = new int[11];
            endNode.edgeWeights = new float[11];
            endNode.nEdges = 0;
        }
        addEdgesToVisibleNeighbours(endIndex, ex, ey);
        
        // Mark all hasEdgeToGoal vertices.
        markHasEdgeToGoal(true);
    }

    private final int addEdgesToVisibleNeighbours(int index, int x, int y) {
        losScanner.computeAllVisibleSuccessors(x, y);
        int nSuccessors = losScanner.nSuccessors;
        for (int i=0;i<nSuccessors;++i) {
            int toX = losScanner.successorsX[i];
            int toY = losScanner.successorsY[i];
            int targetIndex = nodeIndex[toY][toX];

            float weight = graph.distance(x, y, toX, toY);
            addEdge(index, targetIndex, weight);
        }
        
        return index;
    }

    private final void markHasEdgeToGoal(boolean value) {
        int[] endNeighbours = nodes[endIndex].outgoingEdges;
        int n = nodes[endIndex].nEdges;
        for (int i=0;i<n;++i) {
            nodes[endNeighbours[i]].hasEdgeToGoal = value;
        }
    }
    
    private final void addAllEdges() {
        int fromX, fromY, toX, toY;
        for (int i=0;i<nNodes;++i) {
            fromX = xCoordinateOf(i);
            fromY = yCoordinateOf(i);
            
            losScanner.computeAllVisibleSuccessors(fromX, fromY);
            int nSuccessors = losScanner.nSuccessors;
            for (int j=0;j<nSuccessors;++j) {
                toX = losScanner.successorsX[j];
                toY = losScanner.successorsY[j];
                int toIndex = nodeIndex[toY][toX];

                float weight = graph.distance(fromX, fromY, toX, toY);
                addEdge(i, toIndex, weight);
            }
        }
    }
    
    private final void addEdge(int fromI, int toI, float weight) {
        nodes[fromI].addEdge(toI, weight);
    }
    
    public final int xCoordinateOf(int index) {
        return nodes[index].x;
    }

    public final int yCoordinateOf(int index) {
        return nodes[index].y;
    }

    public final int size() {
        return nNodes;
    }
    
    public final int maxSize() {
        return maxSize;
    }
    
    public final int computeSumDegrees() {
        int sum = 0;
        for (int i=0;i<nNodes;++i) {
            sum += nodes[i].nEdges;
        }
        return sum;
    }

    public final VGNode getOutgoingEdges(int source) {
        return nodes[source];
    }

    public final int startNode() {
        return startIndex;
    }

    public final int endNode() {
        return endIndex;
    }
    
    private final void maybeSaveSnapshot() {
        if (saveSnapshot != null) saveSnapshot.run();
    }
    
    
    private static final VisibilityGraphOptimised repurpose(VisibilityGraphOptimised oldGraph) {
        VisibilityGraphOptimised newGraph = new VisibilityGraphOptimised(oldGraph.graph);
        newGraph.nodeIndex = oldGraph.nodeIndex;
        newGraph.startIndex = oldGraph.startIndex;
        newGraph.endIndex = oldGraph.endIndex;
        newGraph.nodes = oldGraph.nodes;
        newGraph.nNodes = oldGraph.nNodes;
        newGraph.originalSize = oldGraph.originalSize;
        newGraph.maxSize = oldGraph.maxSize;
        newGraph.losScanner = oldGraph.losScanner;

        newGraph.startStoredNeighbours = oldGraph.startStoredNeighbours;
        newGraph.startStoredWeights = oldGraph.startStoredWeights;
        newGraph.startStoredNumNeighbours = oldGraph.startStoredNumNeighbours;
        newGraph.endStoredNeighbours = oldGraph.endStoredNeighbours;
        newGraph.endStoredWeights = oldGraph.endStoredWeights;
        newGraph.endStoredNumNeighbours = oldGraph.endStoredNumNeighbours;
        
        return newGraph;
    }
    
    public static final VisibilityGraphOptimised getStoredGraph(GridGraph graph) {
        VisibilityGraphOptimised visibilityGraph = null;
        if (storedGridGraph != graph || storedVisibilityGraph == null) {
            //("Get new graph");
            visibilityGraph = new VisibilityGraphOptimised(graph);
            storedVisibilityGraph = visibilityGraph;
            storedGridGraph = graph;
        } else {
            //("Reuse graph");
            visibilityGraph = repurpose(storedVisibilityGraph);
            storedVisibilityGraph = visibilityGraph;
        }
        return visibilityGraph;
    }

    private final void printAllEdges() {
        for (int i=0;i<nNodes;++i) {
            VGNode node = nodes[i];
            System.out.println("Node " + i + " : " + node.nEdges + " edges");
            for (int j=0;j<node.nEdges;++j) {
                System.out.println(i + " -> " + node.outgoingEdges[j]);
            }
        }
    }

    public static void clearMemory() {
        storedVisibilityGraph = null;
        storedGridGraph = null;
        System.gc();
    }
    
} public class RecursiveStrictThetaStar extends BasicThetaStar {
    private int DEPTH_LIMIT = -1;
    private float BUFFER_VALUE = 0.42f;

    public RecursiveStrictThetaStar(GridGraph graph, int sx, int sy, int ex, int ey) {
        super(graph, sx, sy, ex, ey);
    }

    public static RecursiveStrictThetaStar setBuffer(GridGraph graph, int sx, int sy, int ex, int ey, float bufferValue) {
        RecursiveStrictThetaStar algo = new RecursiveStrictThetaStar(graph, sx, sy, ex, ey);
        algo.BUFFER_VALUE = bufferValue;
        return algo;
    }
    
    public static RecursiveStrictThetaStar noHeuristic(GridGraph graph, int sx, int sy, int ex, int ey) {
        RecursiveStrictThetaStar algo = new RecursiveStrictThetaStar(graph, sx, sy, ex, ey);
        algo.heuristicWeight = 0f;
        return algo;
    }
    
    public static RecursiveStrictThetaStar depthLimit(GridGraph graph, int sx, int sy, int ex, int ey, int depthLimit) {
        RecursiveStrictThetaStar algo = new RecursiveStrictThetaStar(graph, sx, sy, ex, ey);
        algo.DEPTH_LIMIT = depthLimit;
        return algo;
    }
    
    public static RecursiveStrictThetaStar postSmooth(GridGraph graph, int sx, int sy, int ex, int ey) {
        RecursiveStrictThetaStar algo = new RecursiveStrictThetaStar(graph, sx, sy, ex, ey);
        algo.postSmoothingOn = true;
        return algo;
    }

    @Override
    public void computePath() {
        int totalSize = (graph.sizeX+1) * (graph.sizeY+1);

        int start = toOneDimIndex(sx, sy);
        finish = toOneDimIndex(ex, ey);

        pq = new ReusableIndirectHeap(totalSize);
        this.initialiseMemory(totalSize, Float.POSITIVE_INFINITY, -1, false);
        
        initialise(start);
        
        while (!pq.isEmpty()) {
            int current = pq.popMinIndex();
            tryFixBufferValue(current);
            
            if (current == finish || distance(current) == Float.POSITIVE_INFINITY) {
                maybeSaveSearchSnapshot();
                break;
            }
            setVisited(current, true);

            int x = toTwoDimX(current);
            int y = toTwoDimY(current);
            

            tryRelaxNeighbour(current, x, y, x-1, y-1);
            tryRelaxNeighbour(current, x, y, x, y-1);
            tryRelaxNeighbour(current, x, y, x+1, y-1);
            
            tryRelaxNeighbour(current, x, y, x-1, y);
            tryRelaxNeighbour(current, x, y, x+1, y);
            
            tryRelaxNeighbour(current, x, y, x-1, y+1);
            tryRelaxNeighbour(current, x, y, x, y+1);
            tryRelaxNeighbour(current, x, y, x+1, y+1);

            maybeSaveSearchSnapshot();
        }
        
        maybePostSmooth();
    }
    
    protected float heuristic(int x, int y) {
        return heuristicWeight*graph.distance(x, y, ex, ey);
        
        // MOD 2 :: Increased Goal Heuristic - Not needed when a Penalty value of 0.42 is used.
        /*if (x == ex && y == ey) {
            return 0.18f; // 0.18f
        } else { 
            return heuristicWeight*graph.distance(x, y, ex, ey);
        }*/
    }

    private void tryFixBufferValue(int current) {
        if (parent(current) < 0 && parent(current) != -1) {
            setParent(current, parent(current) - Integer.MIN_VALUE);
            setDistance(current, distance(parent(current)) + physicalDistance(current, parent(current)));
        }
    }
    

    protected void tryRelaxNeighbour(int current, int currentX, int currentY, int x, int y) {
        if (!graph.isValidCoordinate(x, y))
            return;
        
        int destination = toOneDimIndex(x,y);
        if (visited(destination))
            return;
        if (parent(current) != -1 && parent(current) == parent(destination)) // OPTIMISATION: [TI]
            return; // Idea: don't bother trying to relax if parents are equal. using triangle inequality.
        if (!graph.neighbourLineOfSight(currentX, currentY, x, y))
            return;
        
        if (relax(current, destination, weight(currentX, currentY, x, y))) {
            // If relaxation is done.
            pq.decreaseKey(destination, distance(destination) + heuristic(x,y));
        }
    }
    
    @Override
    protected boolean relax(int u, int v, float weightUV) {
        // return true iff relaxation is done.
        return tautRelax(u,v,DEPTH_LIMIT);
    }
    
    // This gives very good paths... but the recursion level is too deep.
    private boolean tautRelax(int u, int v, int depth) {
        if (isTaut(v, u)) {
            return tryRelaxVertex(u, v, false);
        } else {
            int par = parent(u);
            if (lineOfSight(par, v)) {
                if (depth == 0) {
                    return tryRelaxVertex(par,v,!isTaut(v,par));
                }
                return tautRelax(par, v, depth-1);
            } else {
                return tryRelaxVertex(u, v, true);
            }
        }
    }
    
    private boolean tryRelaxVertex(int u, int v, boolean addBuffer) {
        int newParent = u;
        float newWeight = distance(u) + physicalDistance(u,v);
        if (addBuffer) {
            newWeight += BUFFER_VALUE;
            newParent += Integer.MIN_VALUE;
        }
        if (newWeight < distance(v)) {
            if (isMergeableWithParent(u, v)) {
                newParent = parent(u);
            }
            setDistance(v, newWeight);
            setParent(v, newParent);
            return true;
        }
        return false;
    }
    
    // If parent(u),u,v collinear, remove u from path, except when u is at an outer corner.
    private boolean isMergeableWithParent(int u, int v) {
        if (u == -1) return false;
        int p = parent(u);
        if (p == -1) return false; // u is start point.
        int ux = toTwoDimX(u);
        int uy = toTwoDimY(u);
        if (isOuterCorner(ux, uy)) return false; // u is outer corner
        
        int vx = toTwoDimX(v);
        int vy = toTwoDimY(v);
        int px = toTwoDimX(p);
        int py = toTwoDimY(p);
        
        return isCollinear(px,py,ux,uy,vx,vy);
    }
    
    protected final boolean isOuterCorner(int x, int y) {
        boolean a = graph.isBlocked(x-1, y-1);
        boolean b = graph.isBlocked(x, y-1);
        boolean c = graph.isBlocked(x, y);
        boolean d = graph.isBlocked(x-1, y);
        
        return ((!a && !c) || (!d && !b)) && (a || b || c || d);
        
        /* NOTE:
         *   ___ ___
         *  |   |||||
         *  |...X'''| <-- this is considered a corner in the above definition
         *  |||||___|
         *  
         *  The definition below excludes the above case.
         */
    }
    
    protected final boolean isCollinear(int x1, int y1, int x2, int y2, int x3, int y3) {
        // (y2-y1)/(x2-x1) == (y3-y2)/(x3-x2)
        // <=>
        return (y2-y1)*(x3-x2) == (y3-y2)*(x2-x1);
    }

    
    /**
     * Checks whether the path v, u, p=parent(u) is taut.
     */
    private boolean isTaut(int v, int u) {
        int p = parent(u); // assert u != -1
        if (p == -1) return true;
        int x1 = toTwoDimX(v);
        int y1 = toTwoDimY(v);
        int x2 = toTwoDimX(u);
        int y2 = toTwoDimY(u);
        int x3 = toTwoDimX(p);
        int y3 = toTwoDimY(p);
        return graph.isTaut(x1, y1, x2, y2, x3, y3);
    }
} public class StrictThetaStar extends BasicThetaStar {
    private float BUFFER_VALUE = 0.42f;

    public StrictThetaStar(GridGraph graph, int sx, int sy, int ex, int ey) {
        super(graph, sx, sy, ex, ey);
    }
    
    public static StrictThetaStar setBuffer(GridGraph graph, int sx, int sy, int ex, int ey, float bufferValue) {
        StrictThetaStar algo = new StrictThetaStar(graph, sx, sy, ex, ey);
        algo.BUFFER_VALUE = bufferValue;
        return algo;
    }
    
    public static StrictThetaStar noHeuristic(GridGraph graph, int sx, int sy, int ex, int ey) {
        StrictThetaStar algo = new StrictThetaStar(graph, sx, sy, ex, ey);
        algo.heuristicWeight = 0f;
        return algo;
    }
    
    public static StrictThetaStar postSmooth(GridGraph graph, int sx, int sy, int ex, int ey) {
        StrictThetaStar algo = new StrictThetaStar(graph, sx, sy, ex, ey);
        algo.postSmoothingOn = true;
        return algo;
    }

    @Override
    public void computePath() {
        int totalSize = (graph.sizeX+1) * (graph.sizeY+1);

        int start = toOneDimIndex(sx, sy);
        finish = toOneDimIndex(ex, ey);

        pq = new ReusableIndirectHeap(totalSize);
        this.initialiseMemory(totalSize, Float.POSITIVE_INFINITY, -1, false);
        
        initialise(start);
        
        while (!pq.isEmpty()) {
            int current = pq.popMinIndex();
            tryFixBufferValue(current);
            
            if (current == finish || distance(current) == Float.POSITIVE_INFINITY) {
                maybeSaveSearchSnapshot();
                break;
            }
            setVisited(current, true);

            int x = toTwoDimX(current);
            int y = toTwoDimY(current);
            

            tryRelaxNeighbour(current, x, y, x-1, y-1);
            tryRelaxNeighbour(current, x, y, x, y-1);
            tryRelaxNeighbour(current, x, y, x+1, y-1);
            
            tryRelaxNeighbour(current, x, y, x-1, y);
            tryRelaxNeighbour(current, x, y, x+1, y);
            
            tryRelaxNeighbour(current, x, y, x-1, y+1);
            tryRelaxNeighbour(current, x, y, x, y+1);
            tryRelaxNeighbour(current, x, y, x+1, y+1);

            maybeSaveSearchSnapshot();
        }
        
        maybePostSmooth();
    }
    
    protected float heuristic(int x, int y) {
        return heuristicWeight*graph.distance(x, y, ex, ey);

        // MOD 2 :: Increased Goal Heuristic - Not needed when a Penalty value of 0.42 is used.
        /*if (x == ex && y == ey) {
            return 1.1f;
        } else { 
            return heuristicWeight*graph.distance(x, y, ex, ey);
        }*/
    }

    private void tryFixBufferValue(int current) {
        if (parent(current) < 0 && parent(current) != -1) {
            setParent(current, parent(current) - Integer.MIN_VALUE);
            setDistance(current, distance(parent(current)) + physicalDistance(current, parent(current)));
        }
    }
    

    protected void tryRelaxNeighbour(int current, int currentX, int currentY, int x, int y) {
        if (!graph.isValidCoordinate(x, y))
            return;
        
        int destination = toOneDimIndex(x,y);
        if (visited(destination))
            return;
        if (parent(current) != -1 && parent(current) == parent(destination)) // OPTIMISATION: [TI]
            return; // Idea: don't bother trying to relax if parents are equal. using triangle inequality.
        if (!graph.neighbourLineOfSight(currentX, currentY, x, y))
            return;
        
        if (relax(current, destination, weight(currentX, currentY, x, y))) {
            // If relaxation is done.
            pq.decreaseKey(destination, distance(destination) + heuristic(x,y));
        }
    }
    
    @Override
    protected boolean relax(int u, int v, float weightUV) {
        // return true iff relaxation is done.
        int par = parent(u);
        if (lineOfSight(parent(u), v)) {
            float newWeight = distance(par) + physicalDistance(par, v);
            return relaxTarget(v, par, newWeight);
        } else {
            float newWeight = distance(u) + physicalDistance(u, v);
            return relaxTarget(v, u, newWeight);
        }
    }

    private boolean relaxTarget(int v, int par, float newWeight) {
        if (newWeight < distance(v)) {
            if (!isTaut(v, par)) {
                newWeight += BUFFER_VALUE;
                par += Integer.MIN_VALUE;
            }
            setDistance(v, newWeight);
            setParent(v, par);
            return true;
        }
        return false;
    }

    
    /**
     * Checks whether the path v, u, p=parent(u) is taut.
     */
    private boolean isTaut(int v, int u) {
        int p = parent(u); // assert u != -1
        if (p == -1) return true;
        int x1 = toTwoDimX(v);
        int y1 = toTwoDimY(v);
        int x2 = toTwoDimX(u);
        int y2 = toTwoDimY(u);
        int x3 = toTwoDimX(p);
        int y3 = toTwoDimY(p);
        return graph.isTaut(x1, y1, x2, y2, x3, y3);
    }
} public class SG16Algorithm extends PathFindingAlgorithm {

    private ConvexHullHeuristic convexHullHeuristic;
    private SG16VisibilityGraph convexHullGraph;
    private ReusableIndirectHeap pq; 
    private int start;
    private int finish;

    public SG16Algorithm(GridGraph graph, int sx, int sy, int ex, int ey) {
        super(graph, graph.sizeX, graph.sizeY, sx, sy, ex, ey);
    }

    @Override
    public void computePath() {

        // 1. Generate convex hulls
        convexHullGraph = new SG16VisibilityGraph(graph);
        if (isRecording()) convexHullGraph.setSnapshotAction(() -> generateConvexHullSnapshot());
        convexHullGraph.initialise(sx, sy, ex, ey);
        convexHullHeuristic = convexHullGraph.getConvexHullHeuristic();

        int size = convexHullGraph.size();
        int memorySize = size+2;
        start = size;
        finish = size+1;

        pq = new ReusableIndirectHeap(size, memorySize);
        this.initialiseMemory(memorySize, Float.POSITIVE_INFINITY, -1, false);
        
        // 2. Search
        if (graph.lineOfSight(sx, sy, ex, ey)) {
            // There is a direct path from (sx, sy) to (ex, ey).
            if (sx != ex || sy != ey) {
                setParent(finish, start);
            }
            return;
        } 

        // Initialise Start
        setVisited(start, true);
        setDistance(start, 0);
        expand(start, sx ,sy);

        // pq
        while (!pq.isEmpty())
        {
            int current = pq.popMinIndex();
            setVisited(current, true);

            if (current == finish) {
                return;
            }

            int currX = convexHullGraph.getX(current);
            int currY = convexHullGraph.getY(current);
            
            expand(current, currX, currY);
        }
    }

    private final void expand(int currIndex, int currX, int currY) {
        // find neighbours
        ConvexHullRPSScanner scanner = convexHullGraph.computeAllVisibleSuccessors(currX, currY, ex, ey);
        //maybeSaveSearchSnapshot(scanner, currX, currY);

        int nNeighbours = scanner.nSuccessors;
        for (int i=0; i<nNeighbours; ++i)
        {
            int succX = scanner.successorsX[i];
            int succY = scanner.successorsY[i];
            if (!graph.lineOfSight(currX, currY, succX, succY)) continue;

            int succ = convexHullGraph.indexOf(succX, succY); // index of successor

            relax(currIndex, currX, currY, succ, succX, succY);
        }

        // Check if the goal node is a successor
        if (graph.lineOfSight(currX, currY, ex, ey)) {
            relax(currIndex, currX, currY, finish, ex, ey);
        }

        maybeSaveSearchSnapshotWithSuccessors(scanner, currX, currY);
    }

    private final void relax(int u, int ux, int uy, int v, int vx, int vy) {
        // Theta*-style relaxations
        int par = parent(u);
        if (par != -1) {
            int parX = getX(par);
            int parY = getY(par);
            if (graph.lineOfSight(parX, parY, vx, vy)) {
                u = par;
                ux = parX;
                uy = parY;
            }
        }
            
        float newWeight = distance(u) + graph.distance(ux, uy, vx, vy);
        if (newWeight < distance(v)) {
            setDistance(v, newWeight);
            setParent(v, u);
            pq.decreaseKey(v, newWeight + heuristic(vx, vy));
        }
    }


    private final float heuristic(int nx, int ny) {
        // SLD heuristic (naive)
        return graph.distance(nx, ny, ex, ey);
        // Convex hull heuristic
        //if (isRecording() && (nx!=ex||ny!=ey)) {
            //List<SnapshotItem> snapshot = computeSearchSnapshot();
            //snapshot.add(SnapshotItem.generate(new Integer[]{nx, ny}, Color.ORANGE));
            //snapshot.addAll(convexHullHeuristic.snapshotLines(nx, ny));
            //addSnapshot(snapshot);
        //}
        //return (float)convexHullHeuristic.heuristic(nx, ny);
    }

    private int pathLength() {
        int length = 0;
        int current = finish;
        while (current != -1) {
            current = parent(current);
            length++;
        }
        return length;
    }

    @Override
    public int[][] getPath() {
        int length = pathLength();
        int[][] path = new int[length][];
        int current = finish;
        
        int index = length-1;
        while (current != -1) {
            int x, y;
            if (current == start) {
                x = sx;
                y = sy;
            } else if (current == finish) {
                x = ex;
                y = ey;
            } else {
                x = convexHullGraph.getX(current);
                y = convexHullGraph.getY(current);
            }
            
            path[index] = new int[2];
            path[index][0] = x;
            path[index][1] = y;
            
            index--;
            current = parent(current);
        }
        
        return path;
    }

    @Override
    public float getPathLength() {
        return 0;
    }

    private final int parent(int index) {
        return Memory.parent(index);
    }
    
    private final void setParent(int index, int value) {
        Memory.setParent(index, value);
    }
    
    private final float distance(int index) {
        return Memory.distance(index);
    }
    
    private final void setDistance(int index, float value) {
        Memory.setDistance(index, value);
    }
    
    private final boolean visited(int index) {
        return Memory.visited(index);
    }
    
    private final void setVisited(int index, boolean value) {
        Memory.setVisited(index, value);
    }

    private final void generateConvexHullSnapshot() {
        addSnapshot(convexHullGraph.generateConvexHullSnapshot());
    }

    @Override
    protected int goalParentIndex() {
        return finish;
    }

    @Override
    protected Integer[] snapshotEdge(int endIndex) {
        Integer[] edge = new Integer[4];
        int startIndex = parent(endIndex);
        edge[0] = getX(startIndex);
        edge[1] = getY(startIndex);
        edge[2] = getX(endIndex);
        edge[3] = getY(endIndex);
        return edge;
    }

    @Override
    protected Integer[] snapshotVertex(int index) {
        if (selected(index)) {
            Integer[] edge = new Integer[2];
            edge[0] = getX(index);
            edge[1] = getY(index);
            return edge;
        }
        return null;
    }

    private final int getX(int index) {
        if (index == start) return sx;
        else if (index == finish) return ex;
        return convexHullGraph.getX(index);
    }

    private final int getY(int index) {
        if (index == start) return sy;
        else if (index == finish) return ey;
        return convexHullGraph.getY(index);
    }

    @Override
    protected List<SnapshotItem> computeSearchSnapshot() {
        List<SnapshotItem> snapshot = convexHullGraph.generateConvexHullSnapshot();
        snapshot.addAll(super.computeSearchSnapshot());
        return snapshot;
    }

    private final void maybeSaveSearchSnapshot(ConvexHullRPSScanner scanner, int currX, int currY) {
        if (!isRecording()) return;
        List<SnapshotItem> snapshot = computeSearchSnapshot();
        snapshot.addAll(scanner.snapshotLines());
        snapshot.add(SnapshotItem.generate(new Integer[]{currX, currY}, Color.BLUE));
        addSnapshot(snapshot);
    }

    private final void maybeSaveSearchSnapshotWithSuccessors(ConvexHullRPSScanner scanner, int currX, int currY) {
        if (!isRecording()) return;
        List<SnapshotItem> snapshot = computeSearchSnapshot();
        snapshot.addAll(scanner.snapshotLinesAndSuccessors(currX, currY));
        snapshot.add(SnapshotItem.generate(new Integer[]{currX, currY}, Color.BLUE));
        addSnapshot(snapshot);
    }
} public class DrawCanvas extends JPanel {

    // DRAWING CONFIGURATION
    private static final int MAX_RES = 700;
    
    private static final Color UNBLOCKEDTILE_COLOR = Color.WHITE;
    private static final Color BLOCKEDTILE_COLOR = new Color(127,127,127);
    
    private static final Color STARTPOINT_COLOR = new Color(0,160,0);
    private static final Color ENDPOINT_COLOR = Color.YELLOW;
    // DRAWING CONFIGURATION - END
    
    public final int resY;
    public final int resX;
    
    private BufferedImage cachedGridGraphImage;
    private final Drawer gridGraphDrawer;
    private Drawer gridLineDrawer;
    private Drawer gridPointDrawer;
    private Drawer startEndPointDrawer;
    protected final GridGraph gridGraph;
    
    public DrawCanvas(GridGraph gridGraph, GridLineSet gridLineSet) {
        super();
        int sizeX = gridGraph.sizeX;
        int sizeY = gridGraph.sizeY;
        if (sizeX < sizeY) {
            resY = MAX_RES;
            resX = resY*sizeX/sizeY;
        } else {
            resX = MAX_RES;
            resY = resX*sizeY/sizeX;
        }
        
        setPreferredSize(new Dimension(resX,resY));
        
        this.gridGraph = gridGraph;
        gridGraphDrawer = new GridGraphDrawer(gridGraph, resX, resY);
        if (gridLineSet != null) {
            gridLineDrawer = new GridLineDrawer(gridGraph, gridLineSet, resX, resY);
        }
    }
    
    public DrawCanvas(GridGraph gridGraph) {
        this(gridGraph, null);
    }

    public void setStartAndEnd(int sx, int sy, int ex, int ey) {
        GridPointSet gridPointSet = new GridPointSet();
        gridPointSet.addPoint(sx,  sy, STARTPOINT_COLOR);
        gridPointSet.addPoint(ex,  ey, ENDPOINT_COLOR);
        startEndPointDrawer = new VariableSizePointDrawer(gridGraph,
                gridPointSet, resX, resY, 1.3f);
    }
    
    @Override
    public void paintComponent(Graphics g) {
        super.paintComponent(g);
        
        clearToColor(g);
        
        if (cachedGridGraphImage == null) {
            cachedGridGraphImage = new BufferedImage(getWidth(), getHeight(), BufferedImage.TYPE_INT_ARGB);
            Graphics g2 = cachedGridGraphImage.getGraphics();
            g2.setColor(BLOCKEDTILE_COLOR);
            gridGraphDrawer.draw(g2);
        }
        g.drawImage(cachedGridGraphImage, 0, 0, this);
        
        
        if (gridLineDrawer != null) {
            gridLineDrawer.draw(g);
        }
        if (gridPointDrawer != null) {
            gridPointDrawer.draw(g);
        }
        if (startEndPointDrawer != null) {
            startEndPointDrawer.draw(g);
        }
    }
    
    private void clearToColor(Graphics g) {
        g.setColor(UNBLOCKEDTILE_COLOR);
        g.fillRect(0,0,resX,resY);
    }
    
    
    public void changeSet(GridLineSet gridLineSet, GridPointSet gridPointSet) {
        gridLineDrawer = new GridLineDrawer(gridGraph, gridLineSet, resX, resY);
        gridPointDrawer = new GridPointDrawer(gridGraph, gridPointSet, resX, resY);
        repaint();
    }
    
    public void changeSet(GridPointSet gridPointSet) {
        gridPointDrawer = new GridPointDrawer(gridGraph, gridPointSet, resX, resY);
        repaint();
    }

    public void changeSet(GridLineSet gridLineSet) {
        gridLineDrawer = new GridLineDrawer(gridGraph, gridLineSet, resX, resY);
        repaint();
    }
    
} public class GridLineSet {

    private ArrayList<Line> lineList;
    private ArrayList<FractionLine> fractionLineList;

    public class Line {
        public final Color color;
        public final int x1;
        public final int y1;
        public final int x2;
        public final int y2;

        public Line(int x1, int y1, int x2, int y2, Color color) {
            this.color = color;
            this.x1 = x1;
            this.y1 = y1;
            this.x2 = x2;
            this.y2 = y2;
        }
    }
    
    public class FractionLine {
        public final Color color;
        public final Fraction x1;
        public final Fraction y1;
        public final Fraction x2;
        public final Fraction y2;

        public FractionLine(Fraction x1, Fraction y1, Fraction x2,
                Fraction y2, Color color) {
            this.color = color;
            this.x1 = x1;
            this.y1 = y1;
            this.x2 = x2;
            this.y2 = y2;
        }
    }
    
    public GridLineSet() {
        lineList = new ArrayList<>();
        fractionLineList = new ArrayList<>();
    }

    public void addLine(int x1, int y1, int x2, int y2, Color color) {
        lineList.add(new Line(x1,y1,x2,y2,color));
    }

    public void addLine(Fraction x1, Fraction y1, Fraction x2, Fraction y2, Color color) {
        fractionLineList.add(new FractionLine(x1,y1,x2,y2,color));
    }
    
    public Collection<Line> getLineList() {
        return lineList;
    }
    
    public Collection<FractionLine> getFractionLineList() {
        return fractionLineList;
    }
} public class GridObjects {
    private static final Color POINT_COLOR = new Color(0,64,255);
    private static final Color LINE_COLOR = Color.RED;
    
    
    public final GridLineSet gridLineSet;
    public final GridPointSet gridPointSet;

    public GridObjects(GridLineSet gridLineSet, GridPointSet gridPointSet) {
        this.gridLineSet = gridLineSet;
        this.gridPointSet = gridPointSet;
    }
    
    private GridObjects() {
        this.gridLineSet = null;
        this.gridPointSet = null;
    }

    public static GridObjects nullObject() {
        return new GridObjects();
    }
    
    public boolean isNull() {
        return gridLineSet == null && gridPointSet == null;
    }
    
    /**
     * Convert a snapshot of an algorithm into a GridObjects instance.
     */
    public static GridObjects create(List<SnapshotItem> snapshot) {
        GridLineSet gridLineSet = new GridLineSet();
        GridPointSet gridPointSet = new GridPointSet();
        
        for (SnapshotItem item : snapshot) {
            Integer[] path = item.path;
            Color color = item.color;
            if (path.length == 4) {
                gridLineSet.addLine(path[0], path[1], path[2], path[3], or(LINE_COLOR,color));
            } else if (path.length == 2) {
                gridPointSet.addPoint(path[0], path[1], or(POINT_COLOR,color));
            } else if (path.length == 7) {
                // y, xLn, xLd, xRn, xRd, px, py
                Fraction y = new Fraction (path[0]);
                Fraction xL = new Fraction(path[1], path[2]);
                Fraction xR = new Fraction(path[3], path[4]);
                Fraction xMid = xR.minus(xL).multiplyDivide(1, 2).plus(xL);
                Fraction px = new Fraction (path[5]);
                Fraction py = new Fraction (path[6]);
                gridLineSet.addLine(px, py, xL, y, or(Color.CYAN,color));
                //gridLineSet.addLine(px, py, xMid, y, or(Color.CYAN,color));
                gridLineSet.addLine(px, py, xR, y, or(Color.CYAN,color));
                gridLineSet.addLine(xL, y, xR, y, or(LINE_COLOR,color));
                gridPointSet.addPoint(path[5], path[6], or(Color.BLUE,color));
            } else if (path.length == 5) {
                Fraction y = new Fraction (path[0]);
                Fraction xL = new Fraction(path[1], path[2]);
                Fraction xR = new Fraction(path[3], path[4]);
                gridLineSet.addLine(xL, y, xR, y, or(Color.GREEN,color));
            }
        }
        return new GridObjects(gridLineSet,gridPointSet);
    }

    private static Color or(Color color, Color original) {
        return (original==null?color:original);
    }
    
    
} public class GridPointSet {
    private ArrayList<ColourPoint> pointList;
    private int minCircleSize;

    public class ColourPoint {
        public final Color color;
        public final int x;
        public final int y;
        
        public ColourPoint(int x, int y, Color color) {
            this.color = color;
            this.x = x;
            this.y = y;
        }
    }
    
    public GridPointSet() {
        pointList = new ArrayList<>();
    }
    
    public void setMinCircleSize() {
        minCircleSize = 8;
    }
    
    public void clear() {
        pointList.clear();
    }
    
    public void addPoint(int x, int y, Color color) {
        pointList.add(new ColourPoint(x,y,color));
    }
    
    public Collection<ColourPoint> getPointList() {
        return pointList;
    }

    public int minCircleSize() {
        if (minCircleSize > 0) {
            return minCircleSize;
        }
        return Integer.MIN_VALUE;
    }
} public class GridAndGoals {
	public final GridGraph gridGraph;
	public final StartGoalPoints startGoalPoints;
	
	public GridAndGoals(GridGraph gridGraph, StartGoalPoints startGoalPoints) {
		this.gridGraph = gridGraph;
		this.startGoalPoints = startGoalPoints;
	}
	
	public GridAndGoals(GridGraph gridGraph, int sx, int sy, int ex, int ey) {
		this.gridGraph = gridGraph;
		this.startGoalPoints = new StartGoalPoints(sx, sy, ex, ey);
	}

    public final void validateStartAndEndPoints() {
        int sx = startGoalPoints.sx;
        int sy = startGoalPoints.sy;
        int ex = startGoalPoints.ex;
        int ey = startGoalPoints.ey;
        int sizeX = gridGraph.sizeX;
        int sizeY = gridGraph.sizeY;

        ArrayList<String> issues = new ArrayList<>();

        if (sx < 0) issues.add("sx < 0");
        if (sy < 0) issues.add("sy < 0");
        if (sx > sizeX) issues.add("sx > sizeX");
        if (sy > sizeY) issues.add("sy > sizeY");

        if (ex < 0) issues.add("ex < 0");
        if (ey < 0) issues.add("ey < 0");
        if (ex > sizeX) issues.add("ex > sizeX");
        if (ey > sizeY) issues.add("ey > sizeY");
        
        if (issues.size() > 0) {
            throw new UnsupportedOperationException("INVALID START/END POINTS: " + issues);
        }
    }
}
 public class GridGraph {

    private boolean[] tiles; // Flattened 2D Array
    public final int sizeX;
    public final int sizeY;
    public final int sizeXplusOne;

    private static final float SQRT_TWO = (float)Math.sqrt(2);
    private static final double SQRT_TWO_DOUBLE = Math.sqrt(2);
    private static final float SQRT_TWO_MINUS_ONE = (float)(Math.sqrt(2) - 1);
    
    public GridGraph(int sizeX, int sizeY) {
        this.sizeX = sizeX;
        this.sizeY = sizeY;
        this.sizeXplusOne = sizeX+1;
        
        tiles = new boolean[sizeY*sizeX];
    }
    
    public final void setBlocked(int x, int y, boolean value) {
        tiles[sizeX*y + x] = value;
    }
    
    public final void trySetBlocked(int x, int y, boolean value) {
        if (isValidBlock(x,y))
            tiles[sizeX*y + x] = value;
    }
    
    public final boolean isBlocked(int x, int y) {
        if (x >= sizeX || y >= sizeY) return true;
        if (x < 0 || y < 0) return true;
        return tiles[sizeX*y + x];
    }
    
    public final boolean isBlockedRaw(int x, int y) {
        return tiles[sizeX*y + x];
    }
    
    public final boolean isValidCoordinate(int x, int y) {
        return (x <= sizeX && y <= sizeY &&
                x >= 0 && y >= 0);
    }
    
    public final boolean isValidBlock(int x, int y) {
        return (x < sizeX && y < sizeY &&
                x >= 0 && y >= 0);
    }

    public final int toOneDimIndex(int x, int y) {
        return y*sizeXplusOne + x;
    }
    
    public final int toTwoDimX(int index) {
        return index%sizeXplusOne;
    }
    
    public final int toTwoDimY(int index) {
        return index/sizeXplusOne;
    }

    public final boolean isUnblockedCoordinate(int x, int y) {
        return !topRightOfBlockedTile(x,y) ||
                !topLeftOfBlockedTile(x,y) ||
                !bottomRightOfBlockedTile(x,y) ||
                !bottomLeftOfBlockedTile(x,y);
    }
    
    public final boolean topRightOfBlockedTile(int x, int y) {
        return isBlocked(x-1, y-1);
    }

    public final boolean topLeftOfBlockedTile(int x, int y) {
        return isBlocked(x, y-1);
    }

    public final boolean bottomRightOfBlockedTile(int x, int y) {
        return isBlocked(x-1, y);
    }

    public final boolean bottomLeftOfBlockedTile(int x, int y) {
        return isBlocked(x, y);
    }
    
    /**
     * x1,y1,x2,y2 refer to the top left corner of the tile.
     * @param x1 Condition: x1 between 0 and sizeX inclusive.
     * @param y1 Condition: y1 between 0 and sizeY inclusive.
     * @param x2 Condition: x2 between 0 and sizeX inclusive.
     * @param y2 Condition: y2 between 0 and sizeY inclusive.
     * @return distance.
     */
    public final float distance(int x1, int y1, int x2, int y2) {
        int xDiff = x2 - x1;
        int yDiff = y2 - y1;
        
        if (yDiff == 0) {
            return (float)Math.abs(xDiff);
        }
        if (xDiff == 0) {
            return (float)Math.abs(yDiff);
        }
        if (xDiff == yDiff || xDiff == -yDiff) {
            return SQRT_TWO*Math.abs(xDiff);
        }
        
        int squareDistance = xDiff*xDiff + yDiff*yDiff;
        
        return (float)Math.sqrt(squareDistance);
    }
    
    public final double distance_double(int x1, int y1, int x2, int y2) {
        int xDiff = x2 - x1;
        int yDiff = y2 - y1;
        
        if (xDiff == 0) {
            return Math.abs(yDiff);
        }
        if (yDiff == 0) {
            return Math.abs(xDiff);
        }
        if (xDiff == yDiff || xDiff == -yDiff) {
            return SQRT_TWO_DOUBLE*Math.abs(xDiff);
        }
        
        int squareDistance = xDiff*xDiff + yDiff*yDiff;
        
        return Math.sqrt(squareDistance);
    }
    
    /**
     * Octile distance:
     *   min(dx,dy)*sqrt(2) + (max(dx,dy)-min(dx,dy))
     * = min(dx,dy)*(sqrt(2)-1) + max(dx,dy)
     */
    public final float octileDistance(int x1, int y1, int x2, int y2) {
        int dx = x1-x2;
        int dy = y1-y2;
        if (dx<0) dx = -dx;
        if (dy<0) dy = -dy;
        
        int min = dx;
        int max = dy;
        if (dy < dx) {
            min = dy;
            max = dx;
        }
        
        return min*SQRT_TWO_MINUS_ONE + max;
    }

    /**
     * Same as lineOfSight, but only works with a vertex and its 8 immediate neighbours.
     * Also (x1,y1) != (x2,y2)
     */
    public final boolean neighbourLineOfSight(int x1, int y1, int x2, int y2) {
        if (x1 == x2) {
            if (y1 > y2) {
                return !isBlocked(x1,y2) || !isBlocked(x1-1,y2);
            } else { // y1 < y2
                return !isBlocked(x1,y1) || !isBlocked(x1-1,y1);
            }
        } else if (x1 < x2) {
            if (y1 == y2) {
                return !isBlocked(x1,y1) || !isBlocked(x1,y1-1);
            } else if (y1 < y2) {
                return !isBlocked(x1,y1);
            } else { // y2 < y1
                return !isBlocked(x1,y2);
            }
        } else { // x2 < x1
            if (y1 == y2) {
                return !isBlocked(x2,y1) || !isBlocked(x2,y1-1);
            } else if (y1 < y2) {
                return !isBlocked(x2,y1);
            } else { // y2 < y1
                return !isBlocked(x2,y2);
            }
        }
    }


    /**
     * @return true iff there is line-of-sight from (x1,y1) to (x2,y2).
     */
    public final boolean lineOfSight(int x1, int y1, int x2, int y2) {
        int dy = y2 - y1;
        int dx = x2 - x1;

        int f = 0;

        int signY = 1;
        int signX = 1;
        int offsetX = 0;
        int offsetY = 0;
        
        if (dy < 0) {
            dy *= -1;
            signY = -1;
            offsetY = -1;
        }
        if (dx < 0) {
            dx *= -1;
            signX = -1;
            offsetX = -1;
        }
        
        if (dx >= dy) {
            while (x1 != x2) {
                f += dy;
                if (f >= dx) {
                    if (isBlocked(x1 + offsetX, y1 + offsetY))
                        return false;
                    y1 += signY;
                    f -= dx;
                }
                if (f != 0 && isBlocked(x1 + offsetX, y1 + offsetY))
                    return false;
                if (dy == 0 && isBlocked(x1 + offsetX, y1) && isBlocked(x1 + offsetX, y1 - 1))
                    return false;
                
                x1 += signX;
            }
        }
        else {
            while (y1 != y2) {
                f += dx;
                if (f >= dy) {
                    if (isBlocked(x1 + offsetX, y1 + offsetY))
                        return false;
                    x1 += signX;
                    f -= dy;
                }
                if (f != 0 && isBlocked(x1 + offsetX, y1 + offsetY))
                    return false;
                if (dx == 0 && isBlocked(x1, y1 + offsetY) && isBlocked(x1 - 1, y1 + offsetY))
                    return false;
                
                y1 += signY;
            }
        }
        return true;
    }

    public final Point findFirstBlockedTile(int x1, int y1, int dx, int dy) {
        
        int f = 0;

        int signY = 1;
        int signX = 1;
        int offsetX = 0;
        int offsetY = 0;
        
        if (dy < 0) {
            dy *= -1;
            signY = -1;
            offsetY = -1;
        }
        if (dx < 0) {
            dx *= -1;
            signX = -1;
            offsetX = -1;
        }
        
        if (dx >= dy) {
            while (true) {
                f += dy;
                if (f >= dx) {
                    if (isBlocked(x1 + offsetX, y1 + offsetY))
                        return new Point(x1+offsetX, y1+offsetY);
                    y1 += signY;
                    f -= dx;
                }
                if (f != 0 && isBlocked(x1 + offsetX, y1 + offsetY))
                    return new Point(x1+offsetX, y1+offsetY);
                if (dy == 0 && isBlocked(x1 + offsetX, y1) && isBlocked(x1 + offsetX, y1 - 1))
                    return new Point(x1+offsetX, -1);
                
                x1 += signX;
            }
        }
        else {
            while (true) {
                f += dx;
                if (f >= dy) {
                    if (isBlocked(x1 + offsetX, y1 + offsetY))
                        return new Point(x1+offsetX, y1+offsetY);
                    x1 += signX;
                    f -= dy;
                }
                if (f != 0 && isBlocked(x1 + offsetX, y1 + offsetY))
                    return new Point(x1+offsetX, y1+offsetY);
                if (dx == 0 && isBlocked(x1, y1 + offsetY) && isBlocked(x1 - 1, y1 + offsetY))
                    return new Point(-1, y1+offsetY);
                
                y1 += signY;
            }
        }
        //return null;
    }
    

    /**
     * Used by Accelerated A* and MazeAnalysis.
     * leftRange is the number of blocks you can move left before hitting a blocked tile.
     * downRange is the number of blocks you can move down before hitting a blocked tile.
     * For blocked tiles, leftRange, downRange are both -1.
     * 
     * How to use the maxRange property:
     * 
     *  x,y is the starting point.
     *  k is the number of tiles diagonally up-right of the starting point.
     *  int i = x-y+sizeY;
     *  int j = Math.min(x, y);
     *  return maxRange[i][j + k] - k;
     */
    public final int[][] computeMaxDownLeftRanges() {
        int[][] downRange = new int[sizeY+1][sizeX+1];
        int[][] leftRange = new int[sizeY+1][sizeX+1];

        for (int y=0; y<sizeY; ++y) {
            if (isBlocked(0, y))
                leftRange[y][0] = -1;
            else 
                leftRange[y][0] = 0;
            
            for (int x=1; x<sizeX; ++x) {
                if (isBlocked(x, y)) {
                    leftRange[y][x] = -1;
                } else {
                    leftRange[y][x] = leftRange[y][x-1] + 1;
                }
            }
        }

        for (int x=0; x<sizeX; ++x) {
            if (isBlocked(x, 0))
                downRange[0][x] = -1;
            else 
                downRange[0][x] = 0;
            
            for (int y=1; y<sizeY; ++y) {
                if (isBlocked(x, y)) {
                    downRange[y][x] = -1;
                } else {
                    downRange[y][x] = downRange[y-1][x] + 1;
                }
            }
        }
        
        for (int x=0; x<sizeX+1; ++x) {
            downRange[sizeY][x] = -1;
            leftRange[sizeY][x] = -1;
        }
        
        for (int y=0; y<sizeY; ++y) {
            downRange[y][sizeX] = -1;
            leftRange[y][sizeX] = -1;
        }
        
        int[][] maxRanges = new int[sizeX+sizeY+1][];
        int maxSize = Math.min(sizeX, sizeY) + 1;
        for (int i=0; i<maxRanges.length; ++i) {
            int currSize = Math.min(i+1, maxRanges.length-i);
            if (maxSize < currSize)
                currSize = maxSize;
            maxRanges[i] = new int[currSize];
            
            int currX = i - sizeY;
            if (currX < 0) currX = 0;
            int currY = currX - i + sizeY;
            for (int j=0; j<maxRanges[i].length; ++j) {
                maxRanges[i][j] = Math.min(downRange[currY][currX], leftRange[currY][currX]);
                currY++;
                currX++;
            }
            
        }
        return maxRanges;
    }
    
    /**
     * @return the percentage of blocked tiles as compared to the total grid size.
     */
    public final float getPercentageBlocked() {
        return (float)getNumBlocked() / (sizeX*sizeY);
    }
    
    /**
     * @return the number of blocked tiles in the grid.
     */
    public final int getNumBlocked() {
        int nBlocked = 0;
        for (int y=0; y<sizeY; y++) {
            for (int x=0; x<sizeX; x++) {
                if (isBlocked(x, y)) {
                    nBlocked++;
                }
            }
        }
        return nBlocked;
    }

    public final boolean isOuterCorner(int x, int y) {
        boolean a = isBlocked(x-1, y-1);
        boolean b = isBlocked(x, y-1);
        boolean c = isBlocked(x, y);
        boolean d = isBlocked(x-1, y);
        
        return ((!a && !c) || (!d && !b)) && (a || b || c || d);
        
        /* NOTE
         *   ___ ___
         *  |   |||||
         *  |...X'''| <-- this is considered a corner in the above definition
         *  |||||___|
         *  
         *  The definition below excludes the above case.
         */
        
        /*int results = 0;
        if(a)results++;
        if(b)results++;
        if(c)results++;
        if(d)results++;
        return (results == 1);*/
    }

    /**
     * Checks whether the path (x1,y1),(x2,y2),(x3,y3) is taut.
     */
    public final boolean isTaut(int x1, int y1, int x2, int y2, int x3, int y3) {
        if (x1 < x2) {
            if (y1 < y2) {
                return isTautFromBottomLeft(x1, y1, x2, y2, x3, y3);
            } else if (y2 < y1) {
                return isTautFromTopLeft(x1, y1, x2, y2, x3, y3);
            } else { // y1 == y2
                return isTautFromLeft(x1, y1, x2, y2, x3, y3);
            }
        } else if (x2 < x1) {
            if (y1 < y2) {
                return isTautFromBottomRight(x1, y1, x2, y2, x3, y3);
            } else if (y2 < y1) {
                return isTautFromTopRight(x1, y1, x2, y2, x3, y3);
            } else { // y1 == y2
                return isTautFromRight(x1, y1, x2, y2, x3, y3);
            }
        } else { // x2 == x1
            if (y1 < y2) {
                return isTautFromBottom(x1, y1, x2, y2, x3, y3);
            } else if (y2 < y1) {
                return isTautFromTop(x1, y1, x2, y2, x3, y3);
            } else { // y1 == y2
                throw new UnsupportedOperationException("v == u?");
            }
        }
    }

    
    private final boolean isTautFromBottomLeft(int x1, int y1, int x2, int y2, int x3,
            int y3) {
        if (x3 < x2 || y3 < y2) return false;
        
        int compareGradients = (y2-y1)*(x3-x2) - (y3-y2)*(x2-x1); // m1 - m2
        if (compareGradients < 0) { // m1 < m2
            return bottomRightOfBlockedTile(x2, y2);
        } else if (compareGradients > 0) { // m1 > m2
            return topLeftOfBlockedTile(x2, y2);
        } else { // m1 == m2
            return true;
        }
    }

    
    private final boolean isTautFromTopLeft(int x1, int y1, int x2, int y2, int x3,
            int y3) {
        if (x3 < x2 || y3 > y2) return false;
        
        int compareGradients = (y2-y1)*(x3-x2) - (y3-y2)*(x2-x1); // m1 - m2
        if (compareGradients < 0) { // m1 < m2
            return bottomLeftOfBlockedTile(x2, y2);
        } else if (compareGradients > 0) { // m1 > m2
            return topRightOfBlockedTile(x2, y2);
        } else { // m1 == m2
            return true;
        }
    }
    
    private final boolean isTautFromBottomRight(int x1, int y1, int x2, int y2, int x3,
            int y3) {
        if (x3 > x2 || y3 < y2) return false;
        int compareGradients = (y2-y1)*(x3-x2) - (y3-y2)*(x2-x1); // m1 - m2
        if (compareGradients < 0) { // m1 < m2
            return topRightOfBlockedTile(x2, y2);
        } else if (compareGradients > 0) { // m1 > m2
            return bottomLeftOfBlockedTile(x2, y2);
        } else { // m1 == m2
            return true;
        }
    }

    
    private final boolean isTautFromTopRight(int x1, int y1, int x2, int y2, int x3,
            int y3) {
        if (x3 > x2 || y3 > y2) return false;
        
        int compareGradients = (y2-y1)*(x3-x2) - (y3-y2)*(x2-x1); // m1 - m2
        if (compareGradients < 0) { // m1 < m2
            return topLeftOfBlockedTile(x2, y2);
        } else if (compareGradients > 0) { // m1 > m2
            return bottomRightOfBlockedTile(x2, y2);
        } else { // m1 == m2
            return true;
        }
    }

    
    private final boolean isTautFromLeft(int x1, int y1, int x2, int y2, int x3,
            int y3) {
        if (x3 < x2) return false;
        
        int dy = y3 - y2;
        if (dy < 0) { // y3 < y2
            return topRightOfBlockedTile(x2, y2);
        } else if (dy > 0) { // y3 > y2
            return bottomRightOfBlockedTile(x2, y2);
        } else { // y3 == y2
            return true;
        }
    }

    private final boolean isTautFromRight(int x1, int y1, int x2, int y2, int x3,
            int y3) {
        if (x3 > x2) return false;
        
        int dy = y3 - y2;
        if (dy < 0) { // y3 < y2
            return topLeftOfBlockedTile(x2, y2);
        } else if (dy > 0) { // y3 > y2
            return bottomLeftOfBlockedTile(x2, y2);
        } else { // y3 == y2
            return true;
        }
    }

    private final boolean isTautFromBottom(int x1, int y1, int x2, int y2, int x3,
            int y3) {
        if (y3 < y2) return false;
        
        int dx = x3 - x2;
        if (dx < 0) { // x3 < x2
            return topRightOfBlockedTile(x2, y2);
        } else if (dx > 0) { // x3 > x2
            return topLeftOfBlockedTile(x2, y2);
        } else { // x3 == x2
            return true;
        }
    }

    private final boolean isTautFromTop(int x1, int y1, int x2, int y2, int x3,
            int y3) {
        if (y3 > y2) return false;
        
        int dx = x3 - x2;
        if (dx < 0) { // x3 < x2
            return bottomRightOfBlockedTile(x2, y2);
        } else if (dx > 0) { // x3 > x2
            return bottomLeftOfBlockedTile(x2, y2);
        } else { // x3 == x2
            return true;
        }
    }
} public class ReachableNodes extends BreadthFirstSearch {

    private ReachableNodes(GridGraph graph, int sx, int sy, int ex,
            int ey) {
        super(graph, sx, sy, ex, ey);
    }
    
    /**
     * Computes the set of all nodes reachable from (sx,sy) by an unblocked path.
     * @param graph the grid to use.
     * @param sx x-coordinate of root node
     * @param sy y-coordinate of root node
     * @return An ArrayList of Point objects (nodes reachable from (sx,sy) via an unblocked path).
     */
    public static ArrayList<Point> computeReachable(GridGraph graph, int sx, int sy) {
        ReachableNodes nodes = new ReachableNodes(graph, sx, sy, -10, -10);
        ArrayList<Point> list = new ArrayList<>();

        nodes.computePath();
        for (int i=0; i<nodes.visited.length; i++) {
            if (nodes.visited[i]) {
                int x = nodes.toTwoDimX(i);
                int y = nodes.toTwoDimY(i);
                list.add(new Point(x, y));
            }
        }
        
        return list;
    }
    
    /**
     * Similar to computeReachable, but this one also helps to mark the markVisited[] boolean array.
     */
    public static ArrayList<Point> computeReachable(GridGraph graph, int sx, int sy, boolean[] markVisited) {
        ReachableNodes nodes = new ReachableNodes(graph, sx, sy, -10, -10);
        ArrayList<Point> list = new ArrayList<>();

        nodes.computePath();
        for (int i=0; i<nodes.visited.length; i++) {
            if (nodes.visited[i]) {
                int x = nodes.toTwoDimX(i);
                int y = nodes.toTwoDimY(i);
                list.add(new Point(x, y));
                markVisited[i] = true;
            }
        }
        
        return list;
    }
} public class StartGoalPoints {
	public final int sx, sy, ex, ey;

	public StartGoalPoints(int sx, int sy, int ex, int ey) {
		this.sx = sx;
		this.sy = sy;
		this.ex = ex;
		this.ey = ey;
	};
}
 public class AutomataGenerator {

    public static GridAndGoals generateUnseeded(int sizeX, int sizeY, int unblockedRatio, int iterations, float resolutionMultiplier, int cutoffOffset, boolean bordersAreBlocked, int sx, int sy, int ex, int ey) {
        GridGraph gridGraph = generate(false, 0, sizeX, sizeY, unblockedRatio, iterations, resolutionMultiplier, cutoffOffset, bordersAreBlocked);
        return new GridAndGoals(gridGraph, sx, sy, ex, ey);
    }

    public static GridAndGoals generateSeeded(long seed, int sizeX, int sizeY, int unblockedRatio, int iterations, float resolutionMultiplier, int cutoffOffset, boolean bordersAreBlocked, int sx, int sy, int ex, int ey) {
        GridGraph gridGraph = generate(true, seed, sizeX, sizeY, unblockedRatio, iterations, resolutionMultiplier, cutoffOffset, bordersAreBlocked);
        return new GridAndGoals(gridGraph, sx, sy, ex, ey);
    }
    
    public static GridGraph generateSeededGraphOnly(long seed, int sizeX, int sizeY, int unblockedRatio, int iterations, float resolutionMultiplier, int cutoffOffset, boolean bordersAreBlocked) {
        GridGraph gridGraph = generate(true, seed, sizeX, sizeY, unblockedRatio, iterations, resolutionMultiplier, cutoffOffset, bordersAreBlocked);
        return gridGraph;
    }

    public static GridAndGoals generateUnseededDynamicCutoff(int sizeX, int sizeY, float initialPercentBlocked, int iterations, float resolutionMultiplier, boolean bordersAreBlocked, int sx, int sy, int ex, int ey) {
        GridGraph gridGraph = generateDynamicCutoff(false, 0, sizeX, sizeY, initialPercentBlocked, iterations, resolutionMultiplier, bordersAreBlocked);
        return new GridAndGoals(gridGraph, sx, sy, ex, ey);
    }

    public static GridAndGoals generateSeededDynamicCutoff(long seed, int sizeX, int sizeY, float initialPercentBlocked, int iterations, float resolutionMultiplier, boolean bordersAreBlocked, int sx, int sy, int ex, int ey) {
        GridGraph gridGraph = generateDynamicCutoff(true, seed, sizeX, sizeY, initialPercentBlocked, iterations, resolutionMultiplier, bordersAreBlocked);
        return new GridAndGoals(gridGraph, sx, sy, ex, ey);
    }
    
    public static GridGraph generateSeededGraphOnlyDynamicCutoff(long seed, int sizeX, int sizeY, float initialPercentBlocked, int iterations, float resolutionMultiplier, boolean bordersAreBlocked) {
        GridGraph gridGraph = generateDynamicCutoff(true, seed, sizeX, sizeY, initialPercentBlocked, iterations, resolutionMultiplier, bordersAreBlocked);
        return gridGraph;
    }

    private static GridGraph generate(boolean seededRandom, long seed, int sizeX, int sizeY, int unblockedRatio, int iterations, float resolutionMultiplier, int cutoffOffset, boolean bordersAreBlocked) {
        GridGraph gridGraph = new GridGraph(sizeX, sizeY);

        Random rand = new Random();
        if (!seededRandom) {
            seed = rand.nextInt();
            System.out.println("Starting random with random seed = " + seed);
        } else {
            System.out.println("Starting random with predefined seed = " + seed);
        }
        rand = new Random(seed);
        
        generateRandomMap(rand, gridGraph, unblockedRatio, iterations, resolutionMultiplier, cutoffOffset, bordersAreBlocked);
        
        return gridGraph;
    }

    private static GridGraph generateDynamicCutoff(boolean seededRandom, long seed, int sizeX, int sizeY, float initialPercentBlocked, int iterations, float resolutionMultiplier, boolean bordersAreBlocked) {
        GridGraph gridGraph = new GridGraph(sizeX, sizeY);

        Random rand = new Random();
        if (!seededRandom) {
            seed = rand.nextInt();
            System.out.println("Starting random with random seed = " + seed);
        } else {
            System.out.println("Starting random with predefined seed = " + seed);
        }
        rand = new Random(seed);
        
        generateRandomMapDynamicCutoff(rand, gridGraph, initialPercentBlocked, iterations, resolutionMultiplier, bordersAreBlocked);
        
        return gridGraph;
    }
    
    
    private static void generateRandomMap(Random rand, GridGraph gridGraph, int frequency, int iterations, float resolutionMultiplier, int cutoffOffset, boolean bordersAreBlocked) {
        int sizeX = gridGraph.sizeX;
        int sizeY = gridGraph.sizeY;
        int resolution = Math.max((int)((sizeX+sizeY)*resolutionMultiplier/150), 1);
        int cutoff = (int)(0.8f*resolution*resolution + 1.75f*resolution + 0.8f) + cutoffOffset;

        System.out.println("Resolution " + resolution + ", Cutoff " + cutoff);
        
        boolean[][] grid = new boolean[sizeY][];
        // Count: used for DP computation of number of blocked neighbours.
        //  Note: count includes the current tile as well. We subtract it when we compare with cutoff.
        int[][] count = new int[sizeY][];
        for (int y=0;y<sizeY;++y) {
            grid[y] = new boolean[sizeX];
            count[y] = new int[sizeX];
            for (int x=0;x<sizeX;++x) {
                grid[y][x] = rand.nextInt()%frequency == 0;
            }
        }
        
        for (int itr=0;itr<iterations;++itr) {
            if (bordersAreBlocked) runAutomataIterationBlockedBorders(sizeX, sizeY, resolution, grid, count);
            else runAutomataIterationUnblockedBorders(sizeX, sizeY, resolution, grid, count);
            
            for (int y=0;y<sizeY;++y) {
                for (int x=0;x<sizeX;++x) {
                    grid[y][x] = (count[y][x] - (grid[y][x] ? 1 : 0) >= cutoff);
                }
            }
        }
        
        for (int y=0;y<sizeY;++y) {
            for (int x=0;x<sizeX;++x) {
                gridGraph.setBlocked(x, y, grid[y][x]);
            }
        }
    }


    private static void generateRandomMapDynamicCutoff(Random rand, GridGraph gridGraph, float percentBlocked, int iterations, float resolutionMultiplier, boolean bordersAreBlocked) {
        int sizeX = gridGraph.sizeX;
        int sizeY = gridGraph.sizeY;
        int resolution = Math.max((int)((sizeX+sizeY)*resolutionMultiplier/150), 1);

        boolean[][] grid = new boolean[sizeY][];
        // Count: used for DP computation of number of blocked neighbours.
        //  Note: count includes the current tile as well. We subtract it when we compare with cutoff.
        int[][] count = new int[sizeY][];
        for (int y=0;y<sizeY;++y) {
            grid[y] = new boolean[sizeX];
            count[y] = new int[sizeX];
            for (int x=0;x<sizeX;++x) {
                grid[y][x] = rand.nextFloat() < percentBlocked;
            }
        }
        
        int maxCount = (resolution*2+1);
        maxCount = maxCount*maxCount - 1;
        
        for (int itr=0;itr<iterations;++itr) {
            if (bordersAreBlocked) runAutomataIterationBlockedBorders(sizeX, sizeY, resolution, grid, count);
            else runAutomataIterationUnblockedBorders(sizeX, sizeY, resolution, grid, count);
            
            // Adjust counts to exclude the center.
            for (int y=0;y<sizeY;++y) {
                for (int x=0;x<sizeX;++x) {
                    count[y][x] = (count[y][x] - (grid[y][x] ? 1 : 0));
                }
            }
            
            // Compute mean.
            long totalCount = 0;
            for (int y=0;y<sizeY;++y) {
                for (int x=0;x<sizeX;++x) {
                    totalCount += count[y][x];
                }
            }
            float mean = (float)totalCount / (sizeX*sizeY);
            
            // Compute approximately the value at the pth percentile. (where p = percentBlocked)
            int nBins = (int)Math.sqrt(sizeX*sizeY);
            
            // bin 0: <= low
            // bin n+1: >= low+range
            // bin i: low + d*(i-1) < x < low + d*(i)
            int[] bins = new int[nBins+2];
            float[] binAverage = new float[nBins+2];
            
            float low = mean*0.5f;
            float range = (mean + maxCount)/2 - low;
            float d = range/nBins;
            for (int y=0;y<sizeY;++y) {
                for (int x=0;x<sizeX;++x) {
                    int bin = (int)((count[y][x] - low)*nBins / range) + 1;
                    bin = Math.max(Math.min(bin, nBins+1), 0); // Clamp bin to [0,n+1]
                    binAverage[bin] = (binAverage[bin]*bins[bin] + count[y][x]) / (bins[bin]+1);
                    bins[bin]++;
                }
            }
            
            // cutoff = value at pth percentile (similar to median)
            // using the value at the pth percentile allows us to maintain the same percentageBlocked each iteration.
            int remainingCumSum = (int)(sizeX*sizeY * (1-percentBlocked));
            float cutoff = -1;
            for (int i=0;i<bins.length;++i) {
                remainingCumSum -= bins[i];
                if (remainingCumSum < 0) {
                    cutoff = binAverage[i];
                    break;
                }
            }
            
            for (int y=0;y<sizeY;++y) {
                for (int x=0;x<sizeX;++x) {
                    grid[y][x] = count[y][x] >= cutoff;
                }
            }
        }
        
        for (int y=0;y<sizeY;++y) {
            for (int x=0;x<sizeX;++x) {
                gridGraph.setBlocked(x, y, grid[y][x]);
            }
        }
    }
    
    protected static void runAutomataIterationUnblockedBorders(int sizeX, int sizeY, int resolution, boolean[][] grid, int[][] count) {
        /*
         * Note: for brevity, the following code:
         * nBlocked += (px < 0 || py < 0 || px >= sizeX || py >= sizeY || grid[py][px]) ? 1 : 0;
         * 
         * Is a shortened version of:
         * if (px < 0 || py < 0 || px >= sizeX || py >= sizeY) {
         *     nBlocked++;
         * } else {
         *     nBlocked += grid[py][px] ? 1 : 0;
         * }
         */
        
        { // Base case: y = 0
            int y = 0;
            { // Base case: x = 0
                int x = 0;
                int nBlocked = 0;
                for (int i=-resolution;i<=resolution;++i) {
                    for (int j=-resolution;j<=resolution;++j) {
                        int px = x + i;
                        int py = y + j;
                        nBlocked += (px < 0 || py < 0 || px >= sizeX || py >= sizeY || !grid[py][px]) ? 0 : 1;
                    }
                }
                
                count[y][x] = nBlocked;
            }

            // y = 0, x > 0
            for (int x=1;x<sizeX;++x) {
                int nBlocked = count[y][x-1];

                { // subtract for (x-1-r,?)
                    int px = x - resolution - 1;
                    for (int j=-resolution;j<=resolution;++j) {
                        int py = y + j;
                        nBlocked -= (px < 0 || py < 0 || px >= sizeX || py >= sizeY || !grid[py][px]) ? 0 : 1;
                    }
                }
                
                { // add for (x+r,?)
                    int px = x + resolution;
                    for (int j=-resolution;j<=resolution;++j) {
                        int py = y + j;
                        nBlocked += (px < 0 || py < 0 || px >= sizeX || py >= sizeY || !grid[py][px]) ? 0 : 1;
                    }
                }
                
                count[y][x] = nBlocked;
            }
        }

        // y > 0
        for (int y=1;y<sizeY;++y) {
            // y > 0, x = 0
            {
                int x = 0;
                int nBlocked = count[y-1][x];

                { // subtract for (?,y-1-r)
                    int py = y - resolution - 1;
                    for (int i=-resolution;i<=resolution;++i) {
                        int px = x + i;
                        nBlocked -= (px < 0 || py < 0 || px >= sizeX || py >= sizeY || !grid[py][px]) ? 0 : 1;
                    }
                }
                
                { // add for (?,y+r)
                    int py = y + resolution;
                    for (int i=-resolution;i<=resolution;++i) {
                        int px = x + i;
                        nBlocked += (px < 0 || py < 0 || px >= sizeX || py >= sizeY || !grid[py][px]) ? 0 : 1;
                    }
                }

                count[y][x] = nBlocked;
            }
            
            // y > 0, x > 0
            for (int x=1;x<sizeX;++x) {
                int nBlocked = count[y-1][x] + count[y][x-1] - count[y-1][x-1];

                { // add (x-1-r,y-1-r)
                    int px = x - resolution - 1;
                    int py = y - resolution - 1;
                    nBlocked += (px < 0 || py < 0 || px >= sizeX || py >= sizeY || !grid[py][px]) ? 0 : 1;
                }
                { // add (x+r,y+r)
                    int px = x + resolution;
                    int py = y + resolution;
                    nBlocked += (px < 0 || py < 0 || px >= sizeX || py >= sizeY || !grid[py][px]) ? 0 : 1;
                }
                { // subtract (x-1-r,y+r)
                    int px = x - resolution - 1;
                    int py = y + resolution;
                    nBlocked -= (px < 0 || py < 0 || px >= sizeX || py >= sizeY || !grid[py][px]) ? 0 : 1;
                }
                { // subtract (x+r,y-1-r)
                    int px = x + resolution;
                    int py = y - resolution - 1;
                    nBlocked -= (px < 0 || py < 0 || px >= sizeX || py >= sizeY || !grid[py][px]) ? 0 : 1;
                }
                
                count[y][x] = nBlocked;
            }
        }
    }

    private static void runAutomataIterationBlockedBorders(int sizeX, int sizeY, int resolution, boolean[][] grid, int[][] count) {
        /*
         * Note: for brevity, the following code:
         * nBlocked += (px < 0 || py < 0 || px >= sizeX || py >= sizeY || grid[py][px]) ? 1 : 0;
         * 
         * Is a shortened version of:
         * if (px < 0 || py < 0 || px >= sizeX || py >= sizeY) {
         *     nBlocked++;
         * } else {
         *     nBlocked += grid[py][px] ? 1 : 0;
         * }
         */
        
        { // Base case: y = 0
            int y = 0;
            { // Base case: x = 0
                int x = 0;
                int nBlocked = 0;
                for (int i=-resolution;i<=resolution;++i) {
                    for (int j=-resolution;j<=resolution;++j) {
                        int px = x + i;
                        int py = y + j;
                        nBlocked += (px < 0 || py < 0 || px >= sizeX || py >= sizeY || grid[py][px]) ? 1 : 0;
                    }
                }
                
                count[y][x] = nBlocked;
            }

            // y = 0, x > 0
            for (int x=1;x<sizeX;++x) {
                int nBlocked = count[y][x-1];

                { // subtract for (x-1-r,?)
                    int px = x - resolution - 1;
                    for (int j=-resolution;j<=resolution;++j) {
                        int py = y + j;
                        nBlocked -= (px < 0 || py < 0 || px >= sizeX || py >= sizeY || grid[py][px]) ? 1 : 0;
                    }
                }
                
                { // add for (x+r,?)
                    int px = x + resolution;
                    for (int j=-resolution;j<=resolution;++j) {
                        int py = y + j;
                        nBlocked += (px < 0 || py < 0 || px >= sizeX || py >= sizeY || grid[py][px]) ? 1 : 0;
                    }
                }
                
                count[y][x] = nBlocked;
            }
        }

        // y > 0
        for (int y=1;y<sizeY;++y) {
            // y > 0, x = 0
            {
                int x = 0;
                int nBlocked = count[y-1][x];

                { // subtract for (?,y-1-r)
                    int py = y - resolution - 1;
                    for (int i=-resolution;i<=resolution;++i) {
                        int px = x + i;
                        nBlocked -= (px < 0 || py < 0 || px >= sizeX || py >= sizeY || grid[py][px]) ? 1 : 0;
                    }
                }
                
                { // add for (?,y+r)
                    int py = y + resolution;
                    for (int i=-resolution;i<=resolution;++i) {
                        int px = x + i;
                        nBlocked += (px < 0 || py < 0 || px >= sizeX || py >= sizeY || grid[py][px]) ? 1 : 0;
                    }
                }

                count[y][x] = nBlocked;
            }
            
            // y > 0, x > 0
            for (int x=1;x<sizeX;++x) {
                int nBlocked = count[y-1][x] + count[y][x-1] - count[y-1][x-1];

                { // add (x-1-r,y-1-r)
                    int px = x - resolution - 1;
                    int py = y - resolution - 1;
                    nBlocked += (px < 0 || py < 0 || px >= sizeX || py >= sizeY || grid[py][px]) ? 1 : 0;
                }
                { // add (x+r,y+r)
                    int px = x + resolution;
                    int py = y + resolution;
                    nBlocked += (px < 0 || py < 0 || px >= sizeX || py >= sizeY || grid[py][px]) ? 1 : 0;
                }
                { // subtract (x-1-r,y+r)
                    int px = x - resolution - 1;
                    int py = y + resolution;
                    nBlocked -= (px < 0 || py < 0 || px >= sizeX || py >= sizeY || grid[py][px]) ? 1 : 0;
                }
                { // subtract (x+r,y-1-r)
                    int px = x + resolution;
                    int py = y - resolution - 1;
                    nBlocked -= (px < 0 || py < 0 || px >= sizeX || py >= sizeY || grid[py][px]) ? 1 : 0;
                }
                
                count[y][x] = nBlocked;
            }
        }
    }

    /**
     * Generates a truly random map for the gridGraph.
     * No longer used as this does not generate very good or realistic grids.
     */
    private static void generateRandomMap_slow(Random rand, GridGraph gridGraph, int frequency, int iterations, int cutoffOffset) {
        int sizeX = gridGraph.sizeX;
        int sizeY = gridGraph.sizeY;
        int resolution = Math.max((sizeX+sizeY)/150, 1);
        int cutoff = (int)(0.8f*resolution*resolution + 1.75f*resolution + 0.8f) + cutoffOffset;

        System.out.println("Resolution " + resolution + ", Cutoff " + cutoff);
        
        boolean[][] grid = new boolean[sizeY][];
        boolean[][] grid2 = new boolean[sizeY][];
        for (int y=0;y<sizeY;++y) {
            grid[y] = new boolean[sizeX];
            grid2[y] = new boolean[sizeX];
            for (int x=0;x<sizeX;++x) {
                grid[y][x] = rand.nextInt()%frequency == 0;
            }
        }
        
        for (int itr=0;itr<iterations;++itr) {
            
            for (int y=0;y<sizeY;++y) {
                for (int x=0;x<sizeX;++x) {
                    int nBlocked = 0;
                    for (int i=-resolution;i<=resolution;++i) {
                        for (int j=-resolution;j<=resolution;++j) {
                            if (i == 0 && j == 0) continue;
                            int px = x + i;
                            int py = y + j;
                            if (px < 0 || py < 0 || px >= sizeX || py >= sizeY) {
                                nBlocked++;
                            } else {
                                nBlocked += grid[py][px] ? 1 : 0;
                            }
                        }
                    }
                    grid2[y][x] = nBlocked >= cutoff;
                }
            }
            
            boolean[][] temp = grid;
            grid = grid2;
            grid2 = temp;
        }
        
        for (int y=0;y<sizeY;++y) {
            for (int x=0;x<sizeX;++x) {
                gridGraph.setBlocked(x, y, grid[y][x]);
            }
        }
    }
    
    
} public class DefaultGenerator {
    
    /**
     * Generates a graph using the parameters specified in the arguments.
     * @param _seed The random seed to be used
     * @param _sizeX x-axis size
     * @param _sizeY y-axis size
     * @param _ratio chance of spawning a cluster of blocked tiles is 1 in unblockedRatio.
     * @param _sx x-coordinate of start point
     * @param _sy y-coordinate of start point
     * @param _ex x-coordinate of goal point
     * @param _ey y-coordinate of goal point
     * @return the generated gridGraph.
     */
    public static GridAndGoals generateSeededOld(int seed, int sizeX, int sizeY, int unblockedRatio, int sx, int sy, int ex, int ey) {
        GridGraph gridGraph = generate(true, seed, sizeX, sizeY, unblockedRatio, sx, sy, ex, ey);
        return new GridAndGoals(gridGraph, sx, sy, ex, ey);
    }
    
    public static GridAndGoals generateUnseededOld(int sizeX, int sizeY, int unblockedRatio, int sx, int sy, int ex, int ey) {
        GridGraph gridGraph = generate(false, 0, sizeX, sizeY, unblockedRatio, sx, sy, ex, ey);
        return new GridAndGoals(gridGraph, sx, sy, ex, ey);
    }
    
    /**
     * Does not remove the block at 0,0
     */
    public static GridAndGoals generateSeeded(int seed, int sizeX, int sizeY, int unblockedRatio, int sx, int sy, int ex, int ey) {
        GridGraph gridGraph = generate(true, seed, sizeX, sizeY, unblockedRatio, -1,-1,-1,-1);
        return new GridAndGoals(gridGraph, sx, sy, ex, ey);
    }

    /**
     * Does not remove the block at 0,0
     */
    public static GridAndGoals generateUnseeded(int sizeX, int sizeY, int unblockedRatio, int sx, int sy, int ex, int ey) {
        GridGraph gridGraph = generate(false, 0, sizeX, sizeY, unblockedRatio, -1,-1,-1,-1);
        return new GridAndGoals(gridGraph, sx, sy, ex, ey);
    }

    /**
     * For backwards compatibility. Removes the block at 0,0.
     */
    public static GridGraph generateSeededGraphOnlyOld(int seed, int sizeX, int sizeY, int unblockedRatio) {
        return generate(true, seed, sizeX, sizeY, unblockedRatio, 0,0,0,0);
    }

    /**
     * Does not remove the block at 0,0
     */
    public static GridGraph generateSeededGraphOnly(int seed, int sizeX, int sizeY, int unblockedRatio) {
        return generate(true, seed, sizeX, sizeY, unblockedRatio, -1,-1,-1,-1);
    }

    /**
     * Removes the blocks at sx,sy and ex,ey.
     */
    public static GridGraph generateSeededGraphOnly(int seed, int sizeX, int sizeY, int unblockedRatio, int sx, int sy, int ex, int ey) {
        return generate(true, seed, sizeX, sizeY, unblockedRatio, sx, sy, ex, ey);
    }

    public static GridAndGoals generateSeededTrueRandomGraph(long seed, int sizeX, int sizeY, int frequency, int sx, int sy, int ex, int ey) {
        return new GridAndGoals(generateSeededTrueRandomGraphOnly(seed, sizeX, sizeY, frequency), sx, sy, ex, ey);
    }
    
    public static GridGraph generateSeededTrueRandomGraphOnly(long seed, int sizeX, int sizeY, int frequency) {
        GridGraph gridGraph = new GridGraph(sizeX, sizeY);
        //System.out.println("True Random Graph with predefined seed = " + seed);
        Random rand = new Random(seed);
        generateRandomMap(rand, gridGraph, frequency);
        return gridGraph;
    }
    
    /**
     * Generates a graph using the static parameters defined at the beginning of the class.
     */
    private static GridGraph generate(boolean seededRandom, int seed, int sizeX, int sizeY, int unblockedRatio, int sx, int sy, int ex, int ey) {
        GridGraph gridGraph = new GridGraph(sizeX, sizeY);

        Random rand = new Random();
        if (!seededRandom) {
            seed = rand.nextInt();
            System.out.println("Starting random with random seed = " + seed);
        } else {
            System.out.println("Starting random with predefined seed = " + seed);
        }
        rand = new Random(seed);
        
        generateRandomBlockMap(rand, gridGraph, unblockedRatio);
        fillCorners(rand, gridGraph);

        gridGraph.trySetBlocked(sx, sy, false);
        gridGraph.trySetBlocked(ex, ey, false);
        return gridGraph;
    }
    
    
    /**
     * Generates a truly random map for the gridGraph.
     * No longer used as this does not generate very good or realistic grids.
     */
    private static void generateRandomMap(Random rand, GridGraph gridGraph, int frequency) {
        for (int x = 0; x < gridGraph.sizeX; x++) {
            for (int y = 0; y < gridGraph.sizeY; y++) {
                gridGraph.setBlocked(x, y, rand.nextInt()%frequency == 0);
            }
        }
    }

    /**
     * Post-processing step to remove situations like this:
     * <pre>
     *   ___ ___
     *  |   |||||
     *  |...X'''|
     *  |||||___|</pre>
     * An unblocked path can pass through x from the top left to the bottom right.<br>
     * We fix this by filling up the gaps with additional blocks.
     */
    private static void fillCorners(Random rand, GridGraph gridGraph) {
        boolean didSomething = true;;
        while (didSomething) {
            didSomething = false;
            for (int x = 0; x < gridGraph.sizeX; x++) {
                for (int y = 0; y < gridGraph.sizeY; y++) {
                    if (gridGraph.isBlocked(x, y)) {
                        if (gridGraph.isValidBlock(x+1, y+1) && gridGraph.isBlocked(x+1, y+1)) {
                            if (!gridGraph.isBlocked(x+1, y) && !gridGraph.isBlocked(x, y+1)) {
                                if (rand.nextBoolean())
                                    gridGraph.setBlocked(x+1, y, true);
                                else
                                    gridGraph.setBlocked(x, y+1, true);
                                didSomething = true;
                            }
                        }

                        if (gridGraph.isValidBlock(x-1, y+1) && gridGraph.isBlocked(x-1, y+1)) {
                            if (!gridGraph.isBlocked(x-1, y) && !gridGraph.isBlocked(x, y+1)) {
                                if (rand.nextBoolean())
                                    gridGraph.setBlocked(x-1, y, true);
                                else
                                    gridGraph.setBlocked(x, y+1, true);
                                didSomething = true;
                            }
                        }
                    }
                }
            }
        }
    }

    /**
     * Generates a random "block" map for the gridGraph.<br>
     * This generates more realistic grids by spawning clusters of blocks
     * instead of simply spawning random blocks.
     */
    private static void generateRandomBlockMap(Random rand, GridGraph gridGraph, int frequency) {
        for (int x = 0; x < gridGraph.sizeX; x++) {
            for (int y = 0; y < gridGraph.sizeY; y++) {
                if (rand.nextInt(frequency) == 0) {
                    switch(rand.nextInt(3)) {
                        case 0:
                            gridGraph.trySetBlocked(x, y, true);
                            gridGraph.trySetBlocked(x, y+1, true);
                            gridGraph.trySetBlocked(x+1, y, true);
                            gridGraph.trySetBlocked(x+1, y+1, true);
                            break;
                        case 1:
                            gridGraph.trySetBlocked(x, y-1, true);
                            gridGraph.trySetBlocked(x, y, true);
                            gridGraph.trySetBlocked(x, y+1, true);
                            break;
                        case 2:
                            gridGraph.trySetBlocked(x-1, y, true);
                            gridGraph.trySetBlocked(x, y, true);
                            gridGraph.trySetBlocked(x+1, y, true);
                            break;
                    }
                }
            }
        }
    }

}
 public class Utility {

    /**
     * Compute the length of a given path. (Using euclidean distance)
     */
    public static double computePathLength(GridGraph gridGraph, int[][] path) {
        path = removeDuplicatesInPath(path);
        double pathLength = 0;
        for (int i=0; i<path.length-1; i++) {
            pathLength += gridGraph.distance_double(path[i][0], path[i][1],
                            path[i+1][0], path[i+1][1]);
        }
        return pathLength;
    }
    
    public static double computeOptimalPathLengthOnline(GridGraph gridGraph, Point start, Point end) {
        return computeOptimalPathLengthOnline(gridGraph, start.x, start.y, end.x, end.y);
    }
    
    public static double computeOptimalPathLengthOnline(GridGraph gridGraph, int sx, int sy, int ex, int ey) {
        int[][] path = computeOptimalPathOnline(gridGraph, sx, sy, ex, ey);
        return computePathLength(gridGraph, path);
    }
    
    public static int[][] computeOptimalPathOnline(GridGraph gridGraph, int sx, int sy, int ex, int ey) {
        // Optimal Online algorithm.
        PathFindingAlgorithm algo = new Anya16(gridGraph, sx, sy, ex, ey);
        algo.computePath();
        int[][] path = algo.getPath();
        path = removeDuplicatesInPath(path);
        return path;
    }
    
    public static double computeOptimalPathLengthOffline(GridGraph gridGraph, Point start, Point end) {
        return computeOptimalPathLengthOffline(gridGraph, start.x, start.y, end.x, end.y);
    }
    
    public static double computeOptimalPathLengthOffline(GridGraph gridGraph, int sx, int sy, int ex, int ey) {
        int[][] path = computeOptimalPathOffline(gridGraph, sx, sy, ex, ey);
        return computePathLength(gridGraph, path);
    }
    
    public static int[][] computeOptimalPathOffline(GridGraph gridGraph, int sx, int sy, int ex, int ey) {
        // Optimal Offline algorithm. (preprocessing for faster pathfinding)
        PathFindingAlgorithm algo = EdgeNLevelSparseVisibilityGraphAlgorithm.graphReuse(gridGraph, sx, sy, ex, ey);
        algo.computePath();
        int[][] path = algo.getPath();
        path = removeDuplicatesInPath(path);
        return path;
    }
    
    public static ArrayList<StartEndPointData> fixProblemPathLength(GridGraph gridGraph, ArrayList<StartEndPointData> problems) {
        ArrayList<StartEndPointData> fixedProblems = new ArrayList<>();
        for (StartEndPointData problem : problems) {
            //System.out.println(problem.start + " | " + problem.end);
            double shortestPathLength = computeOptimalPathLengthOnline(gridGraph, problem.start, problem.end);
            fixedProblems.add(new StartEndPointData(problem.start, problem.end, shortestPathLength));
            if (shortestPathLength > problem.shortestPath + 0.0001) System.out.println("REPAIRING: " + problem.shortestPath + " -> " + shortestPathLength);
        }
        return fixedProblems;
    }

    public static int[][] removeDuplicatesInPath(int[][] path) {
        if (path.length <= 2) return path;
        
        int[][] newPath = new int[path.length][];
        int index = 0;
        newPath[0] = path[0];
        for (int i=1; i<path.length-1; ++i) {
            if (isCollinear(path[i][0], path[i][1], path[i+1][0], path[i+1][1], newPath[index][0], newPath[index][1])) {
                // skip
            } else {
                index++;
                newPath[index] = path[i];
            }
        }
        index++;
        newPath[index] = path[path.length-1];
        return Arrays.copyOf(newPath, index+1);
    }
    
    private static boolean isCollinear(int x1, int y1, int x2, int y2, int x3, int y3) {
        return (y3-y1)*(x2-x1) == (x3-x1)*(y2-y1);
    }
    
    /**
     * Generates a path between two points on a grid.
     * @return an array of int[2] indicating the coordinates of the path.
     */
    public static int[][] generatePath(AlgoFunction algoFunction, GridGraph gridGraph,
            int sx, int sy, int ex, int ey) {
        PathFindingAlgorithm algo = algoFunction.getAlgo(gridGraph, sx, sy, ex, ey);
        algo.computePath();
        
        int[][] path = algo.getPath();
        return path;
    }
    
    public static boolean isPathTaut(GridGraph gridGraph, int[][] path) {
        int v1 = 0;
        int v2 = 1;
        for (int v3=2; v3<path.length; ++v3) {
            if (!gridGraph.isTaut(path[v1][0], path[v1][1], path[v2][0], path[v2][1], path[v3][0], path[v3][1])) return false;
            ++v1;
            ++v2;
        }
        return true;
    }

    public static boolean isOptimal(double length, double optimalLength) {
        //System.out.println(length + " | " + optimalLength + " | " + ((length - optimalLength) < 0.0001)); 
        return (length - optimalLength) < 0.0001;
    }
    
    public static boolean validateMazeConnectedSetSize(GridGraph gridGraph, float cutoffLargestRatioToRemaining) {
        MazeAnalysis.Options options = new MazeAnalysis.Options();
        options.largestRatioToSecond = true;
        options.largestRatioToRemaining = true;
        MazeAnalysis mazeAnalysis = new MazeAnalysis(gridGraph, options);
        System.out.println(mazeAnalysis.largestRatioToRemaining + " " + mazeAnalysis.largestRatioToSecond);
        return mazeAnalysis.largestRatioToRemaining >= cutoffLargestRatioToRemaining;
    }
    
    public static void cleanUpPreallocatedMemory() {
        Anya.clearMemory();
        Anya16.clearMemory();
        VisibilityGraph.clearMemory();
        SparseVisibilityGraph.clearMemory();
        SparseVisibilityGraphAlgorithmFibHeap.clearMemory();
        Memory.clearMemory();
        ReusableIndirectHeap.clearMemory();
        LineOfSightScanner.clearMemory();
        EdgeNLevelSparseVisibilityGraph.clearMemory();
        EdgeNLevelSparseVisibilityGraphAlgorithmFibHeap.clearMemory();
        VisibilityGraphOptimised.clearMemory();
        SnapshotItem.clearCached();
        //IVGAlgorithm.clearCached();
        
        System.gc(); System.gc();
    }
} public class BenchmarkGraphSets {
    public static String[] getSetNames() {
        return new String[] {
            "bg512",
            "dao",
            "sc1",
            "wc3maps512",
            "room8",
            "room16",
            "room32",
            "room64",
            "mazes1",
            "mazes2",
            "mazes4",
            "mazes8",
            "mazes16",
            "mazes32",
            "random10",
            "random15",
            "random20",
            "random25",
            "random30",
            "random35",
            "random40",
        };
    }
    
    public static String[] getBenchmarkSet(String setName) {
        switch (setName) {
            case "mazes16": return new String[] {
                "maze512-16-7",
                "maze512-16-5",
                "maze512-16-9",
                "maze512-16-6",
                "maze512-16-2",
                "maze512-16-1",
                "maze512-16-0",
                "maze512-16-4",
                "maze512-16-8",
                "maze512-16-3",
            };
            case "random15": return new String[] {
                "random512-15-4",
                "random512-15-8",
                "random512-15-1",
                "random512-15-5",
                "random512-15-2",
                "random512-15-3",
                "random512-15-6",
                "random512-15-0",
                "random512-15-7",
                "random512-15-9",
            };
            case "sc1": return new String[] {
                "BrokenSteppes",
                "Sandstorm",
                "RedCanyons",
                "Entanglement",
                "WaypointJunction",
                "IceFloes",
                "Sanctuary",
                "Backwoods",
                "SpaceAtoll",
                "CrashSites",
                "GhostTown",
                "ArcticStation",
                "CatwalkAlley",
                "TheFrozenSea",
                "LakeShore",
                "GladiatorPits",
                "ValleyofRe",
                "Caldera",
                "Labyrinth",
                "Predators",
                "Isolation",
                "WinterConquest",
                "Hellfire",
                "TaleofTwoCities",
                "Enigma",
                "OrbitalGully",
                "HotZone",
                "NovaStation",
                "Rosewood",
                "Sirocco",
                "BlackLotus",
                "ShroudPlatform",
                "JungleSiege",
                "CrescentMoon",
                "Tribes",
                "Archipelago",
                "FireWalker",
                "RiverLethe",
                "IceMountain",
                "BigGameHunters",
                "Turbo",
                "TheatreofWar",
                "Legacy",
                "ThinIce",
                "FloodedPlains",
                "Aurora",
                "Brushfire",
                "Aftershock",
                "Eruption",
                "Triskelion",
                "Nightshade",
                "Ramparts",
                "Medusa",
                "WatersEdge",
                "GreenerPastures",
                "DarkContinent",
                "PrimevalIsles",
                "TheHighway",
                "Octopus",
                "SpaceDebris",
                "Cauldron",
                "AcrosstheCape",
                "Elderlands",
                "Inferno",
                "BlastFurnace",
                "SteppingStones",
                "EbonLakes",
                "Crossroads",
                "SpringThaw",
                "Expedition",
                "SapphireIsles",
                "Desolation",
                "WheelofWar",
                "WarpGates",
                "Typhoon",
            };
            case "bg512": return new String[] {
                "AR0404SR",
                "AR0041SR",
                "AR0709SR",
                "AR0505SR",
                "AR0012SR",
                "AR0602SR",
                "AR0413SR",
                "AR0044SR",
                "AR0405SR",
                "AR0503SR",
                "AR0406SR",
                "AR0201SR",
                "AR0410SR",
                "AR0412SR",
                "AR0604SR",
                "AR0202SR",
                "AR0301SR",
                "AR0601SR",
                "AR0203SR",
                "AR0304SR",
                "AR0513SR",
                "AR0011SR",
                "AR0316SR",
                "AR0704SR",
                "AR0603SR",
                "AR0509SR",
                "AR0045SR",
                "AR0018SR",
                "AR0317SR",
                "AR0014SR",
                "AR0711SR",
                "AR0418SR",
                "AR0072SR",
                "AR0700SR",
                "AR0046SR",
                "AR0500SR",
                "AR0017SR",
                "AR0506SR",
                "AR0306SR",
                "AR0502SR",
                "AR0314SR",
                "AR0020SR",
                "AR0510SR",
                "AR0414SR",
                "AR0504SR",
                "AR0204SR",
                "AR0411SR",
                "AR0517SR",
                "AR0701SR",
                "AR0042SR",
                "AR0400SR",
                "AR0206SR",
                "AR0331SR",
                "AR0516SR",
                "AR0600SR",
                "AR0605SR",
                "AR0305SR",
                "AR0511SR",
                "AR0205SR",
                "AR0013SR",
                "AR0702SR",
                "AR0309SR",
                "AR0526SR",
                "AR0043SR",
                "AR0016SR",
                "AR0300SR",
                "AR0705SR",
                "AR0015SR",
                "AR0307SR",
                "AR0071SR",
                "AR0310SR",
                "AR0070SR",
                "AR0303SR",
                "AR0302SR",
                "AR0308SR",
            };
            case "mazes4": return new String[] {
                "maze512-4-9",
                "maze512-4-0",
                "maze512-4-1",
                "maze512-4-2",
                "maze512-4-8",
                "maze512-4-7",
                "maze512-4-3",
                "maze512-4-5",
                "maze512-4-6",
                "maze512-4-4",
            };
            case "mazes8": return new String[] {
                "maze512-8-3",
                "maze512-8-1",
                "maze512-8-9",
                "maze512-8-6",
                "maze512-8-7",
                "maze512-8-8",
                "maze512-8-5",
                "maze512-8-4",
                "maze512-8-2",
                "maze512-8-0",
            };
            case "room8": return new String[] {
                "8room_005",
                "8room_007",
                "8room_003",
                "8room_001",
                "8room_009",
                "8room_008",
                "8room_000",
                "8room_004",
                "8room_006",
                "8room_002",
            };
            case "room32": return new String[] {
                "32room_004",
                "32room_007",
                "32room_008",
                "32room_009",
                "32room_005",
                "32room_003",
                "32room_001",
                "32room_000",
                "32room_006",
                "32room_002",
            };
            case "mazes2": return new String[] {
                "maze512-2-2",
                "maze512-2-4",
                "maze512-2-3",
                "maze512-2-1",
                "maze512-2-0",
                "maze512-2-7",
                "maze512-2-9",
                "maze512-2-6",
                "maze512-2-5",
                "maze512-2-8",
            };
            case "mazes1": return new String[] {
                "maze512-1-9",
                "maze512-1-6",
                "maze512-1-0",
                "maze512-1-4",
                "maze512-1-3",
                "maze512-1-7",
                "maze512-1-5",
                "maze512-1-1",
                "maze512-1-2",
                "maze512-1-8",
            };
            case "room64": return new String[] {
                "64room_003",
                "64room_001",
                "64room_004",
                "64room_006",
                "64room_007",
                "64room_000",
                "64room_002",
                "64room_005",
                "64room_008",
                "64room_009",
            };
            case "random40": return new String[] {
                "random512-40-5",
                "random512-40-7",
                "random512-40-2",
                "random512-40-0",
                "random512-40-3",
                "random512-40-8",
                "random512-40-1",
                "random512-40-9",
                "random512-40-6",
                "random512-40-4",
            };
            case "dao": return new String[] {
                "lak100c",
                "orz100d",
                "orz901d",
                "lak511d",
                "den001d",
                "orz701d",
                "den204d",
                "lak503d",
                "den401d",
                "den020d",
                "orz000d",
                "lak100d",
                "oth999d",
                "hrt002d",
                "brc100d",
                "den520d",
                "den601d",
                "lak403d",
                "den200d",
                "lak514d",
                "orz106d",
                "lak101d",
                "brc504d",
                "lak202d",
                "den500d",
                "lak109d",
                "orz101d",
                "lak203d",
                "brc999d",
                "lak103d",
                "hrt001d",
                "orz302d",
                "lak100n",
                "brc202d",
                "hrt201n",
                "lak107d",
                "lak401d",
                "brc501d",
                "den505d",
                "lak108d",
                "orz107d",
                "lgt602d",
                "brc203d",
                "lak505d",
                "lak507d",
                "orz301d",
                "ost000t",
                "lak405d",
                "lak512d",
                "lak308d",
                "ost004d",
                "lak304d",
                "brc200d",
                "den201d",
                "orz103d",
                "ost003d",
                "lak300d",
                "hrt201d",
                "lak506d",
                "orz105d",
                "den206d",
                "den101d",
                "brc502d",
                "ost000a",
                "combat2",
                "orz703d",
                "lak200d",
                "den400d",
                "ost001d",
                "ost100d",
                "arena2",
                "brc201d",
                "den403d",
                "lak106d",
                "orz700d",
                "lak105d",
                "lgt601d",
                "den011d",
                "lgt605d",
                "den203d",
                "lak307d",
                "den602d",
                "den900d",
                "lak504d",
                "ost002d",
                "brc000d",
                "orz300d",
                "lak104d",
                "brc101d",
                "isound1",
                "orz203d",
                "den408d",
                "lak303d",
                "orz304d",
                "den998d",
                "lak519d",
                "combat",
                "orz500d",
                "orz303d",
                "ost102d",
                "den901d",
                "oth001d",
                "brc300d",
                "den510d",
                "den600d",
                "lak404d",
                "den207d",
                "lak250d",
                "lak102d",
                "lak513d",
                "brc505d",
                "den404d",
                "rmtst01",
                "den312d",
                "den000d",
                "lak515d",
                "den502d",
                "lgt604d",
                "den202d",
                "oth000d",
                "lak302d",
                "den005d",
                "lak510d",
                "orz702d",
                "orz201d",
                "den501d",
                "brc503d",
                "den200n",
                "den308d",
                "lak400d",
                "rmtst",
                "den407d",
                "lgt101d",
                "lgt300d",
                "ost101d",
                "arena",
                "orz704d",
                "brc204d",
                "lak110d",
                "lak526d",
                "orz102d",
                "orz900d",
                "lgt600d",
                "lak201d",
                "den405d",
                "den009d",
                "orz800d",
                "den012d",
                "brc997d",
                "hrt000d",
                "orz200d",
                "orz999d",
                "lgt603d",
                "rmtst03",
                "orz601d",
                "den504d",
            };
            case "wc3maps512": return new String[] {
                "plunderisle",
                "theglaive",
                "hillsofglory",
                "plaguelands",
                //"harrow",
                "thecrucible",
                "frostsabre",
                "battleground",
                //"ogremound",
                "gardenofwar",
                "heart2heart",
                "losttemple",
                "bootybay",
                //"forestwalk",
                "stromguarde",
                "icecrown",
                "golemsinthemist",
                "deadwaterdrop",
                "darkforest",
                "gnollwood",
                "blastedlands",
                "scorchedbasin",
                "swampofsorrows",
                "duskwood",
                "riverrun",
                "harvestmoon",
                "tranquilpaths",
                "petrifiedforest",
                "nighthaven",
                "isleofdread",
                "divideandconquer",
                //"dragonmountain",
                "dragonfire",
                "bloodvenomfalls",
                "drywatergulch",
                "timbermawhold",
                "moonglade",
                //"legends",
                "dustwallowkeys",
                "plainsofsnow",
                "mysticisles",
            };
            case "room16": return new String[] {
                "16room_000",
                "16room_004",
                "16room_007",
                "16room_005",
                "16room_009",
                "16room_002",
                "16room_001",
                "16room_008",
                "16room_003",
                "16room_006",
            };
            case "mazes32": return new String[] {
                "maze512-32-7",
                "maze512-32-2",
                "maze512-32-6",
                "maze512-32-9",
                "maze512-32-3",
                "maze512-32-5",
                "maze512-32-8",
                "maze512-32-1",
                "maze512-32-0",
                "maze512-32-4",
            };
            case "random25": return new String[] {
                "random512-25-6",
                "random512-25-8",
                "random512-25-9",
                "random512-25-5",
                "random512-25-3",
                "random512-25-7",
                "random512-25-4",
                "random512-25-1",
                "random512-25-0",
                "random512-25-2",
            };
            case "random20": return new String[] {
                "random512-20-0",
                "random512-20-7",
                "random512-20-6",
                "random512-20-5",
                "random512-20-8",
                "random512-20-1",
                "random512-20-3",
                "random512-20-9",
                "random512-20-4",
                "random512-20-2",
            };
            case "random30": return new String[] {
                "random512-30-2",
                "random512-30-9",
                "random512-30-5",
                "random512-30-6",
                "random512-30-8",
                "random512-30-7",
                "random512-30-3",
                "random512-30-1",
                "random512-30-0",
                "random512-30-4",
            };
            case "random35": return new String[] {
                "random512-35-7",
                "random512-35-2",
                "random512-35-0",
                "random512-35-9",
                "random512-35-8",
                "random512-35-5",
                "random512-35-4",
                "random512-35-3",
                "random512-35-1",
                "random512-35-6",
            };
            case "random10": return new String[] {
                "random512-10-6",
                "random512-10-5",
                "random512-10-7",
                "random512-10-9",
                "random512-10-1",
                "random512-10-8",
                "random512-10-2",
                "random512-10-3",
                "random512-10-4",
                "random512-10-0",
            };
        };
        return null;
    }

    public static void testMapLoading() {
        // Test that all maps can load properly.
        String[] setNames = getSetNames();
        for (String setName : setNames) {
            System.out.println("Loading set: " + setName);
            String[] mapNames = getBenchmarkSet(setName);
            int nProblems = 0;
            for (String mapName : mapNames) {
                MazeAndTestCases mazeAndTestCases = BenchmarkGraphImporter.loadBenchmark(mapName);
                nProblems += mazeAndTestCases.problems.size();
            }
            System.out.println(nProblems + " problems");
        }
        System.out.println("Test complete: All maps loaded successfully.");
    }
} public class FileIO {
    private PrintWriter printWriter;
    private String SEPARATOR_COLUMN = "\t";
    private String SEPARATOR_ROW = System.lineSeparator();
    
    public static boolean makeDirs(String path) {
        return (new File(path)).mkdirs();
    }

    public FileIO(String filename) {
        try {
            printWriter = new PrintWriter(filename);
        } catch (FileNotFoundException e) {
            e.printStackTrace();
        }
    }
    
    public static FileIO csv(String filename) {
        FileIO fileIO = new FileIO(filename);
        fileIO.SEPARATOR_COLUMN = ",";
        return fileIO;
    }
    
    public void writeLine(String string) {
        printWriter.write(string);
        printWriter.write(SEPARATOR_ROW);
    }
    
    public void writeRow(String...strings) {
        String separatorColumn = "";
        for (String string : strings) {
            printWriter.write(separatorColumn);
            printWriter.write(string);
            separatorColumn = SEPARATOR_COLUMN;
        }
        printWriter.write(SEPARATOR_ROW);
    }
    
    public void close() {
        printWriter.close();
    }
    
    public void flush() {
        printWriter.flush();
    }
} public class GraphImporter {
    private GridGraph gridGraph;
    
    private GraphImporter(String filepath) {
        boolean[][] result = null;

        File file = new File(filepath);
        try {
            FileReader fileReader = new FileReader(file);
            Scanner sc = new Scanner(fileReader);
            int x = sc.nextInt();
            int y = sc.nextInt();
            result = new boolean[y][];
            for (int i=0; i<y; i++) {
                result[i] = new boolean[x];
                for (int j=0; j<x; j++) {
                    result[i][j] = (sc.nextInt() != 0);
                }
            }
            sc.close();
        } catch (FileNotFoundException e) {
            System.out.println("File " + filepath + " not found");
        }
        /* Commented out because of Unreachable Catch Clause warning.
        catch (IOException e) {
            e.printStackTrace();
        }*/
        
        create(result);          
    }

    private void create(boolean[][] result) {
        gridGraph = new GridGraph(result[0].length, result.length);
        for (int y=0;y<result.length;y++) {
            for (int x=0;x<result[0].length;x++) {
                gridGraph.setBlocked(x, y, result[y][x]);
            }
        }
    }

    private void createDoubleSize(boolean[][] result) {
        int size = 2;
        
        gridGraph = new GridGraph(result[0].length*size, result.length*size);
        for (int y=0;y<result.length*size;y++) {
            for (int x=0;x<result[0].length*size;x++) {
                gridGraph.setBlocked(x, y, result[y/size][x/size]);
            }
        }
    }
    
    private GridGraph retrieve() {
        return gridGraph;
    }
    
    public static ArrayList<String> getAllMazeNames() {
        ArrayList<String> mazeNames = new ArrayList<>();
        
        String path = AnyAnglePathfinding.PATH_MAZEDATA + "/";
        File dir = new File(path);
        File[] files = dir.listFiles();
        for (File file : files) {
            mazeNames.add(file.getName());
        }
        
        return mazeNames;
    }

    /**
     * Import a graph from a file in the predefinedgrids directory.
     * Look into the GraphImporter documentation for details on how to create a grid file.
     */
    public static GridGraph importGraphFromFile(String filename) {
        String filepath = AnyAnglePathfinding.PATH_GRAPHIMPORT + filename;
        return importGraphFromFilePath(filepath);
    }

    /**
     * Import a graph from a file in the predefinedgrids directory,
     * and also set the start and goal points.
     */
    public static GridAndGoals importGraphFromFile(String filename, int sx, int sy, int ex, int ey) {
        String filepath = AnyAnglePathfinding.PATH_GRAPHIMPORT + filename;
        return importGraphFromFilePath(filepath, sx, sy, ex, ey);
    }

    /**
     * Import a graph from a file in the AnyAnglePathfinding directory.
     */
    public static GridGraph importGraphFromFilePath(String filepath) {
        GridGraph gridGraph;
        GraphImporter graphImporter = new GraphImporter(filepath);
        gridGraph = graphImporter.retrieve();
        return gridGraph;
    }

    /**
     * Import a graph from a file in the AnyAnglePathfinding directory,
     * and also set the start and goal points.
     */
    public static GridAndGoals importGraphFromFilePath(String filepath, int sx, int sy, int ex, int ey) {
        GridGraph gridGraph = GraphImporter.importGraphFromFilePath(filepath);
        return new GridAndGoals(gridGraph, sx, sy, ex, ey);
    }

    public static GridAndGoals loadStoredMaze(String mazeName, String problemName) {
        String filepath = AnyAnglePathfinding.PATH_MAZEDATA + mazeName + "/maze.txt";
        TwoPoint tp = readProblem(problemName);
        return importGraphFromFilePath(filepath, tp.p1.x, tp.p1.y, tp.p2.x, tp.p2.y);
    }
    
    public static GridGraph loadStoredMaze(String mazeName) {
        String filepath = AnyAnglePathfinding.PATH_MAZEDATA + mazeName + "/maze.txt";
        return importGraphFromFilePath(filepath);
    }

    public static ArrayList<TwoPoint> loadStoredMazeProblems(String mazeName) {
        String path = AnyAnglePathfinding.PATH_MAZEDATA + mazeName + "/";
        File dir = new File(path);
        File[] files = dir.listFiles((file, name) -> name.endsWith(".problem"));
        
        ArrayList<TwoPoint> list = new ArrayList<>(files.length);
        for (File file : files) {
            TwoPoint tp = readProblem(file);
            list.add(tp);
        }
        return list;
    }

    public static ArrayList<StartEndPointData> loadStoredMazeProblemData(String mazeName) {
        String path = AnyAnglePathfinding.PATH_MAZEDATA + mazeName + "/";
        File dir = new File(path);
        File[] files = dir.listFiles((file, name) -> name.endsWith(".problem"));
        
        ArrayList<StartEndPointData> list = new ArrayList<>(files.length);
        for (File file : files) {
            TwoPoint tp = readProblem(file);
            String sp = readFile(file).get("shortestPathLength");
            double shortestPath = Double.parseDouble(sp);
            list.add(new StartEndPointData(tp.p1, tp.p2, shortestPath));
        }
        return list;
    }
    
    private static TwoPoint readProblem(File file) {
        String s = file.getName();
        s = s.substring(0, s.lastIndexOf('.')); // remove extension
        return readProblem(s);
    }

    private static TwoPoint readProblem(String problemName) {
        String[] args = problemName.split("[-_]");
        
        if (args.length != 4)
            throw new UnsupportedOperationException("Invalid problem name: " + problemName);
        return new TwoPoint(
                Integer.parseInt(args[0]),
                Integer.parseInt(args[1]),
                Integer.parseInt(args[2]),
                Integer.parseInt(args[3])
                );
    }
    
    private static HashMap<String, String> readFile(File file) {
        HashMap<String,String> dict = new HashMap<>();
        try {
            FileReader fr = new FileReader(file);
            BufferedReader br = new BufferedReader(fr);
            String s = br.readLine();
            while (s != null) {
                put(dict, s);
                s = br.readLine();
            }
    
        } catch (IOException e) {
            e.printStackTrace();
        }
        
        return dict;
    }
        
    private static void put(HashMap<String,String> dict, String input) {
        String[] args = input.split(":", 2);
        dict.put(args[0].trim(), args[1].trim());
    }
} public class GridPolygonGenerator {

    private final GridGraph graph;
    private final int sizeX;
    private final int sizeY;
    
    public GridPolygonGenerator(GridGraph graph) {
        this.graph = graph;
        this.sizeX = graph.sizeX;
        this.sizeY = graph.sizeY;
    }

    public static RPSScanner createRpsScannerFromGrid(GridGraph graph) {
        GridPolygonGenerator generator = new GridPolygonGenerator(graph);
        GridRPSPolygonTracer tracer = new GridRPSPolygonTracer(graph);

        int sizeX = graph.sizeX;
        int sizeY = graph.sizeY;

        for (int y=0; y<sizeY; ++y) {
            for (int x=1; x<=sizeX; ++x) {
                boolean bottomRightOfBlocked = graph.isBlockedRaw(x-1,y);
                boolean bottomLeftOfBlocked = (x != sizeX && graph.isBlockedRaw(x,y));
                boolean topRightOfBlocked = (y != 0 && graph.isBlockedRaw(x-1,y-1));
                boolean topLeftOfBlocked = (y != 0 && x != sizeX && graph.isBlockedRaw(x,y-1));

                if (bottomRightOfBlocked && !bottomLeftOfBlocked && !(topRightOfBlocked && !topLeftOfBlocked)) {
                    tracer.traceFromVertex(x, y);
                }
            }
        }
        tracer.postProcess();

        return new RPSScanner(tracer.vertices, tracer.edges, graph);
    }
} public class RPSScanner {

    public static ArrayList<List<SnapshotItem>> snapshotList = new ArrayList<>();

    private final void saveSnapshot(int sx, int sy, Vertex v) {
        ArrayList<SnapshotItem> snapshot = new ArrayList<>();

        snapshot.add(SnapshotItem.generate(new Integer[]{sx, sy, v.x, v.y}, Color.RED));

        // Snapshot current state of heap
        int heapSize = edgeHeap.size();
        Edge[] edges = edgeHeap.getEdgeList();
        for (int k=0; k<heapSize; ++k) {
            Color colour = (k == 0) ? Color.CYAN : Color.GREEN;
            Edge e = edges[k];
            snapshot.add(SnapshotItem.generate(new Integer[]{e.u.x, e.u.y, e.v.x, e.v.y}, colour));
        }

        snapshotList.add(new ArrayList<SnapshotItem>(snapshot));
    }
    
    public static final void clearSnapshots() {
        snapshotList.clear();
    }


    public int nSuccessors;
    public int[] successorsX;
    public int[] successorsY;

    private final Vertex[] vertices;
    private final RPSEdgeHeap edgeHeap;
    private final GridGraph graph;

    public static class Vertex {
        public int x;
        public int y;
        public double angle;
        public Edge edge1; // edge1 goes forward
        public Edge edge2; // edge2 goes backward

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

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

    public static class Edge {
        private static final double EPSILON = 0.00000001;

        public Vertex u; // u goes backward
        public Vertex v; // v goes forward
        public int heapIndex;
        //public double distance;

        public Edge(RPSScanner.Vertex u, RPSScanner.Vertex v) {
            this.u = u;
            this.v = v;
        }

        // Condition: e2 comes after e1 in rotational plane sweep order.
        private static final boolean isLessThan(Edge e1, Edge e2, int sx, int sy) {
            // compare u and e.v
            if (e1.u.x == e2.v.x && e1.u.y == e2.v.y) {
                // have to do flipping
                double m1x = e1.midX();
                double m1y = e1.midY();
                double e1_distance_sx_sy_m1x_m1y = e1.distance(sx, sy, m1x, m1y);
                double diff1 = e1_distance_sx_sy_m1x_m1y - e2.distance(sx, sy, m1x, m1y);

                double m2x = e2.midX();
                double m2y = e2.midY();
                double diff2 = e1.distance(sx, sy, m2x, m2y) - e2.distance(sx, sy, m2x, m2y);

                if (diff1*diff2 < 0) {
                    // opposite signs
                    // need to flip m1 to the opposite side.
                    // u1 == v2
                    int du1x = e1.u.x - sx;
                    int du1y = e1.u.y - sy;
                    double dm1x = m1x - sx;
                    double dm1y = m1y - sy;

                    // m1r is the reflected midpoint about the line s,u1
                    // m1r = m1 - 2*(dm1 - <dm1,du1>/<du1,du1> du1)
                    double dotprodRatio = (dm1x*du1x + dm1y*du1y) / (du1x*du1x + du1y*du1y);
                    double m1rx = m1x - 2*(dm1x - dotprodRatio*du1x);
                    double m1ry = m1y - 2*(dm1y - dotprodRatio*du1y);
                    return e1_distance_sx_sy_m1x_m1y < e2.distance(sx, sy, m1rx, m1ry);
                }
                return diff1 < 0;
            } else {
                // Find a ray (sx,sy)->(tx,ty) that crosses both line segments
                int du1x = e1.u.x - sx;
                int du1y = e1.u.y - sy;
                int du2x = e2.u.x - sx;
                int du2y = e2.u.y - sy;
                int crossProd = du1x*du2y - du1y*du2x;

                double tx, ty;
                if (crossProd >= 0) {
                    tx = (double)(e1.u.x + e2.v.x)/2;
                    ty = (double)(e1.u.y + e2.v.y)/2;
                } else {
                    tx = (double)(e2.u.x + e2.v.x)/2;
                    ty = (double)(e2.u.y + e2.v.y)/2;
                }
                //System.out.println("Ray -> " + tx + ", " + ty);

                return e1.distance(sx, sy, tx, ty) < e2.distance(sx, sy, tx, ty);
            }
        }

        public final boolean isLessThan(Edge e, int sx, int sy) {
            if ((u.x == sx && u.y == sy) || (v.x == sx && v.y == sy)) return true;
            if ((e.u.x == sx && e.u.y == sy) || (e.v.x == sx && e.v.y == sy)) return false;

            int dv1x = v.x - sx;
            int dv1y = v.y - sy;
            int dv2x = e.v.x - sx;
            int dv2y = e.v.y - sy;

            int crossProd = dv1x*dv2y - dv1y*dv2x;
            if (crossProd >= 0) {
                // this -> e
                return isLessThan(this, e, sx, sy);
            } else {
                // e -> this
                return !isLessThan(e, this, sx, sy);
            }
        }

        private final double midX() {
            return (double)(u.x + v.x)/2;
        }

        private final double midY() {
            return (double)(u.y + v.y)/2;
        }

        private final double distance(int sx, int sy, double tx, double ty) {
            double dx = tx - sx;
            double dy = ty - sy;
            int dex = v.x - u.x;
            int dey = v.y - u.y;
            int dux = u.x - sx;
            int duy = u.y - sy;

            double denom = dey*dx - dex*dy;
            if (Math.abs(denom) < EPSILON) {
                // collinear (degenerate case)
                int dvx = v.x - sx;
                int dvy = v.y - sy;
                // (sx,sy) lies between u and v
                if (dux*dvx + duy*dvy <= 0) return 0;
                return Math.min(dux*dux + duy*duy, dvx*dvx + dvy*dvy);
            } else {
                // not collinear
                int numer = dux*dey - duy*dex;
                return (dx*dx+dy*dy)*numer*numer/(denom*denom);
            }
        }

        @Override
        public String toString() {
            return u.x + ", " + u.y + ", " + v.x + ", " + v.y;
        }
    }

    public RPSScanner(Vertex[] vertices, Edge[] edges, GridGraph graph) {
        successorsX = new int[11];
        successorsY = new int[11];
        nSuccessors = 0;
        this.vertices = vertices;
        this.edgeHeap = new RPSEdgeHeap(edges);
        this.graph = graph;
    }

    private final void clearNeighbours() {
        nSuccessors = 0;
    }

    private final void addNeighbour(int x, int y) {
        if (nSuccessors >= successorsX.length) {
            successorsX = Arrays.copyOf(successorsX, successorsX.length*2);
            successorsY = Arrays.copyOf(successorsY, successorsY.length*2);
        }
        successorsX[nSuccessors] = x;
        successorsY[nSuccessors] = y;
        ++nSuccessors;
    }

    private final void initialiseScan(int sx, int sy) {
        
        // Compute angles
        for (int i=0; i<vertices.length; ++i) {
            Vertex v = vertices[i];
            if (v.x != sx || v.y != sy) {
                v.angle = Math.atan2(v.y-sy, v.x-sx);
                if (v.angle < 0) v.angle += 2*Math.PI;
            } else {
                v.angle = -1;
                Vertex n1 = v.edge1.v;
                Vertex n2 = v.edge2.u;
                if (graph.isOuterCorner(n1.x, n1.y)) addNeighbour(n1.x, n1.y);
                if (graph.isOuterCorner(n2.x, n2.y)) addNeighbour(n2.x, n2.y);
            }
        }
        sortVertices(sx, sy);

        edgeHeap.clear();
        Edge[] edges = edgeHeap.getEdgeList();
        // Note: iterating through the edges like this is a very dangerous operation.
        // That's because the edges array changes as you insert the edges into the heap.
        // Reason why it works: When we swap two edges when inserting, both edges have already been checked.
        //                      That's because we only swap with edges in the heap, which have a lower index than i.
        for (int i=0; i<edges.length; ++i) {
            Edge edge = edges[i];
            if (intersectsPositiveXAxis(sx, sy, edge)) {
                edgeHeap.insert(edge, sx, sy);
            }
        }
    }

    public final void computeAllVisibleSuccessors(int sx, int sy) {
        clearNeighbours();
        if (vertices.length == 0) return;
        if (!graph.isUnblockedCoordinate(sx, sy)) return;

        initialiseScan(sx, sy);

        // This queue is used to enforce the order:
        // INSERT TO EDGEHEAP -> ADD AS NEIGHBOUR -> DELETE FROM EDGEHEAP
        //     for all vertices with the same angle from (sx,sy).
        Vertex[] vertexQueue = new Vertex[11];
        int vertexQueueSize = 0;

        int i = 0;
        // Skip vertex if it is (sx,sy).
        while (vertices[i].x == sx && vertices[i].y == sy) ++i;

        for (; i<vertices.length; ++i) {
            if (vertexQueueSize >= vertexQueue.length) {
                vertexQueue = Arrays.copyOf(vertexQueue, vertexQueue.length*2);
            }
            vertexQueue[vertexQueueSize++] = vertices[i];

            if (i+1 == vertices.length || !isSameAngle(sx, sy, vertices[i], vertices[i+1])) {
                // Clear queue

                // Insert all first
                for (int j=0; j<vertexQueueSize; ++j) {
                    Vertex v = vertexQueue[j];
                    maybeAddEdge(sx, sy, v, v.edge1);
                    maybeAddEdge(sx, sy, v, v.edge2);
                }

                // Add all
                for (int j=0; j<vertexQueueSize; ++j) {
                    Vertex v = vertexQueue[j];
                    //saveSnapshot(sx, sy, v); // UNCOMMENT FOR TRACING

                    Edge edge = edgeHeap.getMin();
                    if (!linesIntersect(sx, sy, v.x, v.y, edge.u.x, edge.u.y, edge.v.x, edge.v.y)) {
                        if (graph.isOuterCorner(v.x, v.y)) {
                            addNeighbour(v.x, v.y);
                        }
                    }
                }

                // Delete all
                for (int j=0; j<vertexQueueSize; ++j) {
                    Vertex v = vertexQueue[j];
                    maybeDeleteEdge(sx, sy, v, v.edge1);
                    maybeDeleteEdge(sx, sy, v, v.edge2);
                }

                // Clear queue
                vertexQueueSize = 0;
            }
        }
    }

    public final void computeAllVisibleTautSuccessors(int sx, int sy) {
        clearNeighbours();
        if (vertices.length == 0) return;
        if (!graph.isUnblockedCoordinate(sx, sy)) return;

        initialiseScan(sx, sy);


        // This queue is used to enforce the order:
        // INSERT TO EDGEHEAP -> ADD AS NEIGHBOUR -> DELETE FROM EDGEHEAP
        //     for all vertices with the same angle from (sx,sy).
        Vertex[] vertexQueue = new Vertex[11];
        int vertexQueueSize = 0;

        int i = 0;
        // Skip vertex if it is (sx,sy).
        while (vertices[i].x == sx && vertices[i].y == sy) ++i;

        for (; i<vertices.length; ++i) {
            if (vertexQueueSize >= vertexQueue.length) {
                vertexQueue = Arrays.copyOf(vertexQueue, vertexQueue.length*2);
            }
            vertexQueue[vertexQueueSize++] = vertices[i];

            if (i+1 == vertices.length || !isSameAngle(sx, sy, vertices[i], vertices[i+1])) {
                // Clear queue

                // Insert all first
                for (int j=0; j<vertexQueueSize; ++j) {
                    Vertex v = vertexQueue[j];
                    maybeAddEdge(sx, sy, v, v.edge1);
                    maybeAddEdge(sx, sy, v, v.edge2);
                }

                // Add all
                for (int j=0; j<vertexQueueSize; ++j) {
                    Vertex v = vertexQueue[j];
                    if (!isTautSuccessor(sx, sy, v.x, v.y)) continue;
                    //saveSnapshot(sx, sy, v); // UNCOMMENT FOR TRACING

                    Edge edge = edgeHeap.getMin();
                    if (!linesIntersect(sx, sy, v.x, v.y, edge.u.x, edge.u.y, edge.v.x, edge.v.y)) {
                        addNeighbour(v.x, v.y);
                    }
                }

                // Delete all
                for (int j=0; j<vertexQueueSize; ++j) {
                    Vertex v = vertexQueue[j];
                    maybeDeleteEdge(sx, sy, v, v.edge1);
                    maybeDeleteEdge(sx, sy, v, v.edge2);
                }

                // Clear queue
                vertexQueueSize = 0;
            }
        }
    }


    public final void computeAllVisibleTwoWayTautSuccessors(int sx, int sy) {
        clearNeighbours();
        if (vertices.length == 0) return;
        if (!graph.isUnblockedCoordinate(sx, sy)) return;

        initialiseScan(sx, sy);

        // We exclude the non-taut region (excludeStart, excludeEnd)
        double EPSILON = 0.00000001;
        double excludeStart = 99999;
        double excludeEnd = 99998;
        // Setting the interval (excludeStart, excludeEnd)
        if (graph.bottomLeftOfBlockedTile(sx, sy)) {
            if (!graph.topRightOfBlockedTile(sx, sy)) {
                excludeStart = Math.PI + EPSILON;
                excludeEnd = 3*Math.PI/2 - EPSILON;
            }
        } else if (graph.bottomRightOfBlockedTile(sx, sy)) {
            if (!graph.topLeftOfBlockedTile(sx, sy)) {
                excludeStart = 3*Math.PI/2 + EPSILON;
                excludeEnd = 2*Math.PI - EPSILON;
            }
        } else if (graph.topRightOfBlockedTile(sx, sy)) {
            excludeStart = 0 + EPSILON;
            excludeEnd = Math.PI/2 - EPSILON;
        } else if (graph.topLeftOfBlockedTile(sx, sy)) {
            excludeStart = Math.PI/2 + EPSILON;
            excludeEnd = Math.PI - EPSILON;
        }


        // This queue is used to enforce the order:
        // INSERT TO EDGEHEAP -> ADD AS NEIGHBOUR -> DELETE FROM EDGEHEAP
        //     for all vertices with the same angle from (sx,sy).
        Vertex[] vertexQueue = new Vertex[11];
        int vertexQueueSize = 0;

        int i = 0;
        // Skip vertex if it is (sx,sy).
        while (vertices[i].x == sx && vertices[i].y == sy) ++i;

        for (; i<vertices.length; ++i) {
            if (vertexQueueSize >= vertexQueue.length) {
                vertexQueue = Arrays.copyOf(vertexQueue, vertexQueue.length*2);
            }
            vertexQueue[vertexQueueSize++] = vertices[i];
            double currentAngle = vertices[i].angle;

            if (i+1 == vertices.length || !isSameAngle(sx, sy, vertices[i], vertices[i+1])) {
                // Clear queue

                // Insert all first
                for (int j=0; j<vertexQueueSize; ++j) {
                    Vertex v = vertexQueue[j];
                    maybeAddEdge(sx, sy, v, v.edge1);
                    maybeAddEdge(sx, sy, v, v.edge2);
                }

                // Add all (if it doesn't fall within the interval (excludeStart, excludeEnd) )
                if (currentAngle <= excludeStart || excludeEnd <= currentAngle) {
                    for (int j=0; j<vertexQueueSize; ++j) {
                        Vertex v = vertexQueue[j];
                        if (!isTautSuccessor(sx, sy, v.x, v.y)) continue;
                        //saveSnapshot(sx, sy, v); // UNCOMMENT FOR TRACING

                        Edge edge = edgeHeap.getMin();
                        if (!linesIntersect(sx, sy, v.x, v.y, edge.u.x, edge.u.y, edge.v.x, edge.v.y)) {
                            addNeighbour(v.x, v.y);
                        }
                    }
                }

                // Delete all
                for (int j=0; j<vertexQueueSize; ++j) {
                    Vertex v = vertexQueue[j];
                    maybeDeleteEdge(sx, sy, v, v.edge1);
                    maybeDeleteEdge(sx, sy, v, v.edge2);
                }

                // Clear queue
                vertexQueueSize = 0;
            }
        }
    }

    // Assumptions:
    // 1. (sx, sy) != (nx, ny)
    // 2. (sx, sy) has line of sight to (nx, ny)
    // 3. (nx, ny) is an outer corner tile.
    private final boolean isTautSuccessor(int sx, int sy, int nx, int ny) {
        int dx = nx - sx;
        int dy = ny - sy;
        if (dx == 0 || dy == 0) return graph.isOuterCorner(nx, ny);

        if (dx > 0) {
            if (dy > 0) {
                return !graph.bottomLeftOfBlockedTile(nx, ny);
            } else { // (dy < 0)
                return !graph.topLeftOfBlockedTile(nx, ny);
            }
        } else { // (dx < 0)
            if (dy > 0) {
                return !graph.bottomRightOfBlockedTile(nx, ny);
            } else { // (dy < 0)
                return !graph.topRightOfBlockedTile(nx, ny);
            }
        }
    }

    private final void sortVertices(int sx, int sy) {
        Arrays.sort(vertices, (a,b) -> Double.compare(a.angle, b.angle));
    }

    private final boolean isSameAngle(int sx, int sy, Vertex u, Vertex v) {
        int dx1 = u.x - sx;
        int dy1 = u.y - sy;
        int dx2 = v.x - sx;
        int dy2 = v.y - sy;

        return dx1*dx2 + dy1*dy2 > 0 && dx1*dy2 == dx2*dy1;
    }

    private final void maybeAddEdge(int sx, int sy, Vertex curr, Edge edge) {
        if (curr != edge.v) return;

        int dux = edge.u.x - sx;
        int duy = edge.u.y - sy;
        int dvx = edge.v.x - sx;
        int dvy = edge.v.y - sy;

        int crossProd = dux*dvy - dvx*duy;
        if (crossProd < 0) {
            // Add/delete
            edgeHeap.insert(edge, sx, sy);
        } else if (crossProd == 0) {
            int dotProd = dux*dvx + duy*dvy;
            if (dotProd > 0) {
                // Don't add
            } else if (dotProd < 0) {
                // Add/delete
                edgeHeap.insert(edge, sx, sy);
            } else { // dotProd == 0
                // Add edge and neighbour
                edgeHeap.insert(edge, sx, sy);
                edgeHeap.insert(edge.u.edge2, sx, sy);
            }
        }
    }

    private final void maybeDeleteEdge(int sx, int sy, Vertex curr, Edge edge) {
        if (curr != edge.u) return;

        int dux = edge.u.x - sx;
        int duy = edge.u.y - sy;
        int dvx = edge.v.x - sx;
        int dvy = edge.v.y - sy;

        int crossProd = dux*dvy - dvx*duy;
        if (crossProd < 0) {
            // Add/delete
            edgeHeap.delete(edge, sx, sy);
        } else if (crossProd == 0) {
            int dotProd = dux*dvx + duy*dvy;
            if (dotProd > 0) {
                // Don't add
            } else if (dotProd < 0) {
                // Add/delete
                edgeHeap.delete(edge, sx, sy);
            } else { // dotProd == 0
                // Delete edge and neighbour
                edgeHeap.delete(edge, sx, sy);
                edgeHeap.delete(edge.v.edge1, sx, sy);
            }
        }
    }

    private final boolean intersectsPositiveXAxis(int sx, int sy, Edge edge) {
        if (sx == edge.u.x && sy == edge.u.y) {
            return anglesIntersectPositiveXAxis(sx, sy, edge.u.edge2.u, edge.v);
        } else if (sx == edge.v.x && sy == edge.v.y) {
            return anglesIntersectPositiveXAxis(sx, sy, edge.u, edge.v.edge1.v);
        } else  {
            return intersectsPositiveXAxis(sx, sy, edge.u, edge.v);
        }
    }

    private final boolean intersectsPositiveXAxis(int sx, int sy, Vertex edgeU, Vertex edgeV) {
        if (anglesIntersectPositiveXAxis(sx, sy, edgeU, edgeV)) {
            int dux = edgeU.x - sx;
            int duy = edgeU.y - sy;
            int dvx = edgeV.x - sx;
            int dvy = edgeV.y - sy;

            int crossProd = dux*dvy - dvx*duy;
            if (crossProd < 0) {
                return true;
            } else if (crossProd == 0) {
                int dotProd = dux*dvx + duy*dvy;
                if (dotProd > 0) {
                    // Don't add
                } else if (dotProd < 0) {
                    return true;
                } else { // (dotProd == 0)
                    // Never happens.
                    throw new UnsupportedOperationException("This should not happen");
                }
            }
        }
        return false;
    }

    private final boolean anglesIntersectPositiveXAxis(int sx, int sy, Vertex edgeU, Vertex edgeV) {
        return edgeU.angle <= edgeV.angle && !isSameAngle(sx, sy, edgeU, edgeV);
    }

    private final boolean linesIntersect(int sx, int sy, int tx, int ty, int ux, int uy, int vx, int vy) {
        int line1dx = tx - sx;
        int line1dy = ty - sy;
        int cross1 = (ux-sx)*line1dy - (uy-sy)*line1dx;
        int cross2 = (vx-sx)*line1dy - (vy-sy)*line1dx;

        int line2dx = vx - ux;
        int line2dy = vy - uy;
        int cross3 = (sx-ux)*line2dy - (sy-uy)*line2dx;
        int cross4 = (tx-ux)*line2dy - (ty-uy)*line2dx;

        if (cross1 != 0 && cross2 != 0 && cross3 != 0 && cross4 != 0) {
            return ((cross1 > 0) != (cross2 > 0)) && ((cross3 > 0) != (cross4 > 0));
        }

        // There exists a cross product that is 0. One of the degenerate cases.
        // Not possible: (sx == ux && sy == uy) or (sx == vx && sy == vy)
        if (tx == ux && ty == uy) {
            if (sx == vx && sy == vy) return true;
            int dx1 = sx-tx;
            int dy1 = sy-ty;
            int dx2 = vx-tx;
            int dy2 = vy-ty;
            int dx3 = sx-vx;
            int dy3 = sy-vy;
            return (dx1*dx2 + dy1*dy2 > 0) && (dx1*dx3 + dy1*dy3 > 0) && (dx1*dy2 == dx2*dy1);
        } else if (tx == vx && ty == vy) {
            if (sx == ux && sy == uy) return true;
            int dx1 = sx-tx;
            int dy1 = sy-ty;
            int dx2 = ux-tx;
            int dy2 = uy-ty;
            int dx3 = sx-ux;
            int dy3 = sy-uy;
            return (dx1*dx2 + dy1*dy2 > 0) && (dx1*dx3 + dy1*dy3 > 0) && (dx1*dy2 == dx2*dy1);
        } else {
            // No equalities whatsoever.
            // We consider this case an intersection if they intersect.

            int prod1 = cross1*cross2;
            int prod2 = cross3*cross4;

            if (prod1 == 0 && prod2 == 0) {
                // All four points collinear.
                int minX1; int minY1; int maxX1; int maxY1;
                int minX2; int minY2; int maxX2; int maxY2;
                
                if (sx < tx) {minX1 = sx; maxX1 = tx;}
                else {minX1 = tx; maxX1 = sx;}

                if (sy < ty) {minY1 = sy; maxY1 = ty;}
                else {minY1 = ty; maxY1 = sy;}

                if (ux < vx) {minX2 = ux; maxX2 = vx;}
                else {minX2 = vx; maxX2 = ux;}

                if (uy < vy) {minY2 = uy; maxY2 = vy;}
                else {minY2 = vy; maxY2 = uy;}

                return !(maxX1 < minX2 || maxY1 < minY2 || maxX2 < minX1 || maxY2 < minY1);
            }

            return (prod1 <= 0 && prod2 <= 0);
        }
    }


    public void drawLines(GridLineSet gridLineSet, GridPointSet gridPointSet) {
        for (int i=0; i<vertices.length; ++i) {
            Vertex v = vertices[i];
            gridPointSet.addPoint(v.x, v.y, Color.YELLOW);
        }

        Edge[] edges = edgeHeap.getEdgeList();
        for (int i=0; i<edges.length; ++i) {
            Edge e = edges[i];
            gridLineSet.addLine(e.u.x, e.u.y, e.v.x, e.v.y, Color.RED);
        }
    }

    public void snapshotHeap(GridLineSet gridLineSet) {
        Edge[] edges = edgeHeap.getEdgeList();
        for (int i=0; i<edgeHeap.size(); ++i) {
            Color colour = (i == 0) ? Color.ORANGE : Color.RED;
            Edge e = edges[i];
            gridLineSet.addLine(e.u.x, e.u.y, e.v.x, e.v.y, colour);
        }
    }
}
import java.awt.Color;
import java.util.ArrayList;
import java.util.LinkedList;
import java.util.List;
import java.util.Random;
import algorithms.AStar;
import algorithms.BasicThetaStar;
import algorithms.JumpPointSearch;
import algorithms.datatypes.Point;
import algorithms.datatypes.SnapshotItem;
import algorithms.sparsevgs.LineOfSightScanner;
import algorithms.sparsevgs.LineOfSightScannerDouble;
import algorithms.sparsevgs.SparseVisibilityGraph;
import algorithms.sparsevgs.VisibilityGraphAlgorithmOptimised;
import algorithms.sparsevgs.VisibilityGraphOptimised;
import algorithms.strictthetastar.RecursiveStrictThetaStar;
import algorithms.strictthetastar.StrictThetaStar;
import algorithms.sg16.SG16Algorithm;
import draw.DrawCanvas;
import draw.GridLineSet;
import draw.GridObjects;
import draw.GridPointSet;
import grid.GridAndGoals;
import grid.GridGraph;
import grid.ReachableNodes;
import grid.StartGoalPoints;
import main.graphgeneration.AutomataGenerator;
import main.graphgeneration.DefaultGenerator;
import main.utility.Utility;
import uiandio.BenchmarkGraphSets;
import uiandio.FileIO;
import uiandio.GraphImporter;
import algorithms.rotationalplanesweep.GridPolygonGenerator;
import algorithms.rotationalplanesweep.RPSScanner;
package main;



public class Experiment {
    
    public static void run() {
        //findMapsWithLargeMainConnectedSets();
//        testVisibilityGraphSize();
//        testAbilityToFindGoal();
//        findStrictThetaStarIssues();
//        findUpperBound();
        testAlgorithmOptimality();
//        BenchmarkGraphSets.testMapLoading();
        //testAgainstReferenceAlgorithm();
        //countTautPaths();
//        other();
//        testLOSScan();
        //testRPSScan();
    }
    
    /**
     * Custom code for experiments.
     */
    public static void other() {
        // This is how to generate test data for the grid. (Use the VisibilityGraph algorithm to generate optimal path lengths)
//      ArrayList<Point> points = ReachableNodes.computeReachable(gridGraph, 5, 5);
//      System.out.println(points.size());
//
//      generateRandomTestDataAndPrint(gridGraph);

      //This is how to conduct a running time / path length test for tha algorithm:
//      TestResult test1 = testAlgorithm(gridGraph, sx, sy, ex, ey, 1, 1);
//      System.out.println(test1);
//      TestResult test2 = testAlgorithm(gridGraph, sx, sy, ex, ey, 30, 25);
//      System.out.println(test2);
    }

    /**
     * Generates and prints out random test data for the gridGraph in question. <br>
     * Note: the algorithm used is the one specified in the algoFunction.
     * Use setDefaultAlgoFunction to choose the algorithm.
     * @param gridGraph the grid to test.
     */
    private static void generateRandomTestDataAndPrint(GridGraph gridGraph) {
        AlgoFunction algo = AnyAnglePathfinding.setDefaultAlgoFunction();
        ArrayList<Point> points = ReachableNodes.computeReachable(gridGraph, 5, 5);
    
        LinkedList<Integer> startX = new LinkedList<>();
        LinkedList<Integer> startY = new LinkedList<>();
        LinkedList<Integer> endX = new LinkedList<>();
        LinkedList<Integer> endY = new LinkedList<>();
        LinkedList<Double> length = new LinkedList<>();
        
        int size = points.size();
        System.out.println("Points: " + size);
        
        for (int i=0; i<100; i++) {
            Random random = new Random();
            int first = random.nextInt(size);
            int last = random.nextInt(size-1);
            if (last == first) last = size-1; // prevent first and last from being the same
    
            Point s = points.get(first);
            Point f = points.get(last);
            int[][] path = Utility.generatePath(algo, gridGraph, s.x, s.y, f.x, f.y);
            if (path.length >= 2) {
                double len = Utility.computePathLength(gridGraph, path);
                startX.offer(s.x);
                startY.offer(s.y);
                endX.offer(f.x);
                endY.offer(f.y);
                length.offer(len);
            }
            if (i%10 == 0) System.out.println("Computed: " + i);
        }
        System.out.println(startX);
        System.out.println(startY);
        System.out.println(endX);
        System.out.println(endY);
        System.out.println(length);
    }

    /**
     * Returns true iff there is a path from the start to the end. Uses the current algorithm to check.<br>
     * Use setDefaultAlgoFunction to choose the algorithm.
     */
    private static boolean hasSolution(AlgoFunction algo, GridGraph gridGraph, StartGoalPoints p) {
        int[][] path = Utility.generatePath(algo, gridGraph, p.sx, p.sy, p.ex, p.ey);
        return path.length > 1;
    }
    
    private static void testLOSScan() {
        GridAndGoals gridAndGoals = AnyAnglePathfinding.loadMaze();
        GridGraph gridGraph = gridAndGoals.gridGraph;
        ArrayList<GridObjects> gridObjectsList = new ArrayList<>();
        GridLineSet gridLineSet = new GridLineSet();;
        GridPointSet gridPointSet = new GridPointSet();