public static int pointInArray(Point p, ArrayList<PathNode> array) {
        for (int c = 0; c < array.size(); c++) {
            if (p.equals(array.get(c).getLocation())) {
                return c;
            }
        }
        return -1;

    }