Задача "Интересное путешествие" от Яндекса, не проходит тест [JAVA]

Обратил внимание на вопрос: Задача "Интересное путешествие" от Яндекса, не проходит тест

У меня ровно такой же вопрос, но для джавы.

Условие задачи:

G. Интересное путешествие

Ограничение времени 1 секунда

Ограничение памяти 64Mb

Ввод стандартный ввод или input.txt

Вывод стандартный вывод или output.txt

Не секрет, что некоторые программисты очень любят путешествовать. Хорошо всем известный программист Петя тоже очень любит путешествовать, посещать музеи и осматривать достопримечательности других городов.

Для перемещений между из города в город он предпочитает использовать машину. При этом он заправляется только на станциях в городах, но не на станциях по пути. Поэтому он очень аккуратно выбирает маршруты, чтобы машина не заглохла в дороге. А ещё Петя очень важный член команды, поэтому он не может себе позволить путешествовать слишком долго. Он решил написать программу, которая поможет ему с выбором очередного путешествия. Но так как сейчас у него слишком много других задач, он попросил вас помочь ему.

Расстояние между двумя городами считается как сумма модулей разности по каждой из координат. Дороги есть между всеми парами городов.

Формат ввода

В первой строке входных данных записано количество городов n (2≤n≤1000). В следующих n строках даны два целых числа: координаты каждого города, не превосходящие по модулю миллиарда. Все города пронумерованы числами от 1 до n в порядке записи во входных данных.

В следующей строке записано целое положительное число k, не превосходящее двух миллиардов, — максимальное расстояние между городами, которое Петя может преодолеть без дозаправки машины.

В последней строке записаны два различных числа — номер города, откуда едет Петя, и номер города, куда он едет.

Формат вывода

Если существуют пути, удовлетворяющие описанным выше условиям, то выведите минимальное количество дорог, которое нужно проехать, чтобы попасть из начальной точки маршрута в конечную. Если пути не существует, выведите -1.

Написал вот такой код:

import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.util.*;
import java.util.stream.Collectors;

public class ShortestPath {
    public static void main(String[] args) throws IOException {
        BufferedReader r = new BufferedReader(new InputStreamReader(System.in));
        int townsCount = Integer.parseInt(r.readLine());
        List<Coordinate> coordinateList = new ArrayList<>();
        for (int i = 0; i < townsCount; i++) {
            String[] coord = r.readLine().split(" ");
            coordinateList.add(new Coordinate(Integer.parseInt(coord[0]), Integer.parseInt(coord[1])));
        }
        int singleTripMaxDistance = Integer.parseInt(r.readLine());
        String[] fromTo = r.readLine().split(" ");
        int from = Integer.parseInt(fromTo[0]) - 1;
        int to = Integer.parseInt(fromTo[1]) - 1;

        if (from == to) {
            System.out.println(0);
            return;
        }
        System.out.println(
                new ShortPathFinder(
                        getConnectedPoints(coordinateList, singleTripMaxDistance)
                ).findShortWay(from, to));
    }

    private static Set<CoordinatePairs> getConnectedPoints(List<Coordinate> coordinateList, int singleTripMaxDistance) {
        Set<CoordinatePairs> connectedPoints = new HashSet<>();
        for (int i = 0; i < coordinateList.size(); i++) {
            for (int j = i + 1; j < coordinateList.size(); j++) {
                int distance = Coordinate.getDistance(coordinateList.get(i), coordinateList.get(j));
                if (distance <= singleTripMaxDistance) {
                    connectedPoints.add(new CoordinatePairs(i, j));
                }
            }
        }
        return connectedPoints;
    }
}

class ShortPathFinder {
    private final Set<CoordinatePairs> connectedPoints;

    public ShortPathFinder(Set<CoordinatePairs> connectedPoints) {
        this.connectedPoints = connectedPoints;
    }

    public int findShortWay(int from, int to) {
        return findShortWay(from, to, 0, new HashSet<>() {{
            add(from);
        }});
    }

    private Integer findShortWay(int from, int to, int stepsDoneForNow, Set<Integer> visitedPoints) {
        Set<Integer> pointsAvailableFrom = getPointsAvailableFrom(from)
                .stream().filter(e -> !visitedPoints.contains(e))
                .collect(Collectors.toSet());

        if (pointsAvailableFrom.stream().anyMatch(pointIndex -> pointIndex == to)) {
            return stepsDoneForNow + 1;
        }
        return pointsAvailableFrom.stream()
                .map(integer -> findShortWay(integer, to, stepsDoneForNow + 1, new HashSet<>(visitedPoints) {{
                            add(integer);
                        }})

                ).min(Integer::compareTo).orElse(-1);
    }


    private Set<Integer> getPointsAvailableFrom(int fromPointId) {
        return connectedPoints.stream()
                .map(coord -> coord.getPointAvailableFrom(fromPointId))
                .filter(Objects::nonNull)
                .collect(Collectors.toSet());

    }
}

class CoordinatePairs {
    private final Integer point1Id;
    private final Integer point2Id;

    public CoordinatePairs(int firstPointId, int secondPointId) {
        if (firstPointId == secondPointId) {
            throw new IllegalArgumentException("firstPointId must not be equal to secondPointId");
        }
        point1Id = Math.min(firstPointId, secondPointId);
        point2Id = Math.max(firstPointId, secondPointId);
    }

    public Integer getPointAvailableFrom(int pointId) {
        if (pointId == point1Id) {
            return point2Id;
        } else if (pointId == point2Id) {
            return point1Id;
        } else {
            return null;
        }
    }

    @Override
    public boolean equals(Object o) {
        if (o == null || getClass() != o.getClass()) return false;
        CoordinatePairs that = (CoordinatePairs) o;
        return Objects.equals(point1Id, that.point1Id) && Objects.equals(point2Id, that.point2Id);
    }

    @Override
    public int hashCode() {
        return Objects.hash(point1Id, point2Id);
    }

    @Override
    public String toString() {
        return "CoordinatePairs{" +
                "point1Id=" + point1Id +
                ", point2Id=" + point2Id +
                '}';
    }
}

class Coordinate {
    private final int x;
    private final int y;

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

    static int getDistance(Coordinate a, Coordinate b) {
        return Math.abs(a.x - b.x) + Math.abs(a.y - b.y);
    }

    @Override
    public boolean equals(Object o) {
        if (o == null || getClass() != o.getClass()) return false;
        Coordinate that = (Coordinate) o;
        return x == that.x && y == that.y;
    }

    @Override
    public int hashCode() {
        return Objects.hash(x, y);
    }
}

Тест кейсы из условия проходят, а вот при сабмите - падает на тестах. На каких конкретно - яндекс не показывает.

Подскажите, где ошибка ?

Update:

Поменял координаты с int на long и теперь падает по времени выполнения:

Update 2

Заменил на поиск в ширину и смог сдвинуться.

Вот так задание прошло:

import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.util.*;

public class ShortestPath {
    public static void main(String[] args) throws IOException {
        BufferedReader r = new BufferedReader(new InputStreamReader(System.in));
        int townsCount = Integer.parseInt(r.readLine());
        List<Coordinate> coordinateList = new ArrayList<>(townsCount);
        for (int i = 0; i < townsCount; i++) {
            String[] coord = r.readLine().split(" ");
            coordinateList.add(new Coordinate(Integer.parseInt(coord[0]), Integer.parseInt(coord[1])));
        }
        int singleTripMaxDistance = Integer.parseInt(r.readLine());
        String[] fromTo = r.readLine().split(" ");
        int fromCoordIndex = Integer.parseInt(fromTo[0]) - 1;
        int toCoordIndex = Integer.parseInt(fromTo[1]) - 1;
        Map<Integer, Set<Integer>> connectedPoints = getConnectedPoints(coordinateList, singleTripMaxDistance);
        if (fromCoordIndex == toCoordIndex || connectedPoints.isEmpty()) {
            System.out.println(-1);
            return;
        }
        System.out.println(
                new ShortPathFinder(connectedPoints, coordinateList.size()).findShortWay(fromCoordIndex, toCoordIndex)
        );
    }

    private static Map<Integer, Set<Integer>> getConnectedPoints(List<Coordinate> coordinateList, int singleTripMaxDistance) {
        System.gc();
        Map<Integer, Set<Integer>> connectedPoints = new HashMap<>();
        for (int i = 0; i < coordinateList.size(); i++) {
            for (int j = i + 1; j < coordinateList.size(); j++) {
                long distance = Coordinate.getDistance(coordinateList.get(i), coordinateList.get(j));
                if (distance <= singleTripMaxDistance) {
                    int finalJ = j;
                    connectedPoints.compute(i, (k, v) -> {
                        if (v == null) {
                            v = new HashSet<>();
                        }
                        v.add(finalJ);
                        return v;
                    });
                    int finalI = i;
                    connectedPoints.compute(j, (k, v) -> {
                        if (v == null) {
                            v = new HashSet<>();
                        }
                        v.add(finalI);
                        return v;
                    });
                }
            }
        }
        return connectedPoints;
    }
}

class ShortPathFinder {
    private final Map<Integer, Set<Integer>> connectedPoints;
    private final Integer pointsSize;

    public ShortPathFinder(Map<Integer, Set<Integer>> connectedPoints, int pointsSize) {
        this.connectedPoints = connectedPoints;
        this.pointsSize = pointsSize;
    }

    public int findShortWay(int from, int to) {
        Queue<Integer> pointsToVisit = new LinkedList<>();
        Set<Integer> visitedPoints = new HashSet<>();
        int[] distances = new int[pointsSize];
        distances[from] = 0;
        pointsToVisit.add(from);
        while (!pointsToVisit.isEmpty()) {
            int currentPoint = pointsToVisit.poll();
            visitedPoints.add(currentPoint);
            for (int availablePointId : getPointsAvailableFrom(currentPoint)) {
                if (visitedPoints.contains(availablePointId)) {
                    continue;
                }
                distances[availablePointId] = distances[currentPoint] + 1;
                visitedPoints.add(availablePointId);
                pointsToVisit.add(availablePointId);
                if(availablePointId == to) {
                    return distances[availablePointId];
                }
            }
        }
        return -1;
    }

    private Set<Integer> getPointsAvailableFrom(int fromPointId) {
        return Optional.ofNullable(connectedPoints.get(fromPointId)).orElse(Collections.emptySet());

    }
}

class Coordinate {
    private final long x;
    private final long y;

    public Coordinate(long x, long y) {
        this.x = x;
        this.y = y;
    }

    static long getDistance(Coordinate a, Coordinate b) {
        return Math.abs(a.x - b.x) + Math.abs(a.y - b.y);
    }
}

Update 3

После применения совета от Станислава получилось следующее решение:

import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.util.*;

public class ShortestPath {
    public static void main(String[] args) throws IOException {
        BufferedReader r = new BufferedReader(new InputStreamReader(System.in));
        int townsCount = Integer.parseInt(r.readLine());
        List<Coordinate> coordinateList = new ArrayList<>(townsCount);
        for (int i = 0; i < townsCount; i++) {
            String[] coord = r.readLine().split(" ");
            coordinateList.add(new Coordinate(Integer.parseInt(coord[0]), Integer.parseInt(coord[1])));
        }
        int singleTripMaxDistance = Integer.parseInt(r.readLine());
        String[] fromTo = r.readLine().split(" ");
        int fromCoordIndex = Integer.parseInt(fromTo[0]) - 1;
        int toCoordIndex = Integer.parseInt(fromTo[1]) - 1;
        Map<Integer, Set<Integer>> connectedPoints = new HashMap<>();
        if (fromCoordIndex == toCoordIndex) {
            System.out.println(-1);
            return;
        }
        System.out.println(
                new ShortPathFinder(connectedPoints, coordinateList, singleTripMaxDistance).findShortWay(fromCoordIndex, toCoordIndex)
        );
    }
}

class ShortPathFinder {
    private final Map<Integer, Set<Integer>> connectedPoints;
    private final List<Coordinate> coordinates;
    private final int singleTripMaxDistance;

    public ShortPathFinder(Map<Integer, Set<Integer>> connectedPoints, List<Coordinate> coordinates, int singleTripMaxDistance) {
        this.connectedPoints = connectedPoints;
        this.coordinates = coordinates;
        this.singleTripMaxDistance = singleTripMaxDistance;
    }

    public int findShortWay(int from, int to) {
        Queue<Integer> pointsToVisit = new LinkedList<>();
        Set<Integer> visitedPoints = new HashSet<>();
        int[] distances = new int[coordinates.size()];
        distances[from] = 0;
        pointsToVisit.add(from);
        while (!pointsToVisit.isEmpty()) {
            int currentPoint = pointsToVisit.poll();
            visitedPoints.add(currentPoint);
            for (int availablePointId : getConnectedPoints(currentPoint)) {
                if (!visitedPoints.contains(availablePointId)) {
                    distances[availablePointId] = distances[currentPoint] + 1;
                    visitedPoints.add(availablePointId);
                    pointsToVisit.add(availablePointId);
                    if (availablePointId == to) {
                        return distances[availablePointId];
                    }
                }
            }
        }
        return -1;
    }

    private Set<Integer> getConnectedPoints(int fromPointId) {
        Set<Integer> connectedPoints = new HashSet<>();
        for (int i = 0; i < coordinates.size(); i++) {
            long distance = Coordinate.getDistance(coordinates.get(fromPointId), coordinates.get(i));
            if (distance <= singleTripMaxDistance) {
                connectedPoints.add(i);
            }
        }
        return connectedPoints;
    }
}

class Coordinate {
    private final long x;
    private final long y;

    public Coordinate(long x, long y) {
        this.x = x;
        this.y = y;
    }

    static long getDistance(Coordinate a, Coordinate b) {
        return Math.abs(a.x - b.x) + Math.abs(a.y - b.y);
    }
}

Потребление памяти снизилось значительно