How can I improve this Java Code:
import java.util.ArrayList;
import java.util.Collections;
import java.util.HashSet;
import java.util.PriorityQueue;

interface AStar {

    private static ArrayList<Node> allPath(Node endNode) {
        ArrayList<Node> result = new ArrayList<>();
        result.add(endNode);
        Node currentNode = endNode;

        while (currentNode.getParent() != null) {
            result.add(currentNode.getParent());
            currentNode = currentNode.getParent();
        }

        Collections.reverse(result);
        return result;
    }

    private static Node[] getNeighbours(Node[][] gridNodes, Node currentNode, ArrayList<Obstacle> obstacles) {
        Node[] result = new Node[8];
        Point currentPoint = currentNode.getP();
        int x = currentNode.getP().getX();
        int y = currentNode.getP().getY();
        Point currentNewPoint = new Point(); // avoid creating 8 lines for every neighbour
        Line currentNewLine = new Line(currentPoint, currentNewPoint); // avoid creating 8 lines for every neighbour

        int[] dx = {-1, 0, 1, -1, 1, -1, 0, 1};
        int[] dy = {-1, -1, -1, 0, 0, 1, 1, 1};
        int[] gCost = {14, 10, 14, 10, 10, 14, 10, 14};

        for (int i = 0; i < 8; i++) {
            int nx = x + dx[i];
            int ny = y + dy[i];

            if (nx >= 0 && ny >= 0 && nx < gridNodes.length && ny < gridNodes[0].length) {
                currentNewPoint.setX(nx);
                currentNewPoint.setY(ny);
                currentNewLine.setP2(currentNewPoint);
                if (!currentNewLine.intersectsObstacles(obstacles)) {
                    int tryGCost = currentNode.getgCost() + gCost[i];
                    if (tryGCost < gridNodes[nx][ny].getgCost()) {
                        gridNodes[nx][ny].setgCost(tryGCost);
                        result[i] = gridNodes[nx][ny];
                    }
                }
            }
        }

        return result;
    }

    default ArrayList<Node> bestPath(Point start, Point end, ArrayList<Obstacle> obstacles) {
        PriorityQueue<Node> openNodes = new PriorityQueue<>(Node::comparator);
        HashSet<Node> closedNodes = new HashSet<>();
        Node[][] gridNodes = new Node[1000][1000];

        for (int i = 0; i < gridNodes.length; i++)
            for (int j = 0; j < gridNodes[0].length; j++)
                gridNodes[i][j] = new Node(new Point(i, j), Integer.MAX_VALUE, Integer.MAX_VALUE);

        Node startNode = new Node(start, 0, 0);
        startNode.sethCost(startNode.calculateDistance(end));
        openNodes.add(startNode);

        while (!openNodes.isEmpty()) {
            Node currentNode = openNodes.poll();
            if (currentNode.isEqualToPoint(end))
                return allPath(currentNode);

            closedNodes.add(currentNode);

            Node[] neighbours = getNeighbours(gridNodes, currentNode, obstacles);
            for (Node neighbourNode : neighbours) {
                if (neighbourNode == null || closedNodes.contains(neighbourNode)) continue;

                neighbourNode.setParent(currentNode);
                neighbourNode.sethCost(neighbourNode.calculateDistance(end));

                if (!openNodes.contains(neighbourNode))
                    openNodes.add(neighbourNode);
            }
        }
        return null;
    }
}

import java.util.ArrayList;

/**
 * Class contains 2 points, each one with (x,y) coordinates
 * Can check for intersections on other objects (lines, rectangles)
 *
 * @author Daniel Ramos
 * @version 23/03/2023
 * @inv p1 != p2
 */
public class Line {
    private Point p1;
    private Point p2;

    /**
     * @param p1 Point in 1st quadrant
     * @param p2 Point in 1st quadrant
     */
    public Line(Point p1, Point p2) {
        this.p1 = p1;
        this.p2 = p2;
    }

    /**
     * Checks if Point is in LineSegment
     *
     * @param p3 Point in 1st quadrant
     * @return True if p3 is on same segment of line
     */
    public boolean onSegment(Point p3) {
        return p3.getX() <= Math.max(p1.getX(), p2.getX()) && p3.getX() >= Math.min(p1.getX(), p2.getX()) &&
                p3.getY() <= Math.max(p1.getY(), p2.getY()) && p3.getY() >= Math.min(p1.getY(), p2.getY());
    }

    /**
     * Checks orientation of LineSegment with Point
     *
     * @param p3 Point in 1st quadrant
     * @return 0 if collinear | 1 if clockwise | 2 if counter-clockwise
     */
    public int orientation(Point p3) {
        double result = (p2.getY() - p1.getY()) * (p3.getX() - p2.getX()) -
                (p2.getX() - p1.getX()) * (p3.getY() - p2.getY());

        if (result == 0)
            return 0;
        return result > 0 ? 1 : 2;
    }


    /**
     * Check if 2 lines intersect (have at least 1 point in common)
     *
     * @param line Check intersection
     * @return True if both lines intersect
     */
    public boolean intersectLine(Line line) {
        int o1 = orientation(line.p1);
        int o2 = orientation(line.p2);
        int o3 = line.orientation(p1);
        int o4 = line.orientation(p2);

        if (o1 != o2 && o3 != o4) return true;

        if (o1 == 0 && onSegment(line.p1)) return true;
        if (o2 == 0 && onSegment(line.p2)) return true;
        if (o3 == 0 && line.onSegment(p1)) return true;
        return o4 == 0 && line.onSegment(p2);
    }

    /**
     * Checks if line intersects with obstacle
     *
     * @param obstacle Obstacle to check intersection
     * @return True if line intersects with obstacle
     */
    public boolean intersectsObstacle(Obstacle obstacle) {
        return obstacle.intersectsLine(this);
    }

    /**
     * Returns 1st point
     */
    public Point getP1() {
        return p1;
    }

    /**
     * Returns 2nd point
     */
    public Point getP2() {
        return p2;
    }

    /**
     * Checks if line is collinear with point p
     *
     * @param p Point to check if it's collinear
     * @return True if line is collinear with point p
     */
    public boolean isCollinear(Point p) {
        return (p1.getX() - p.getX()) * (p2.getY() - p.getY()) == (p2.getX() - p.getX()) * (p1.getY() - p.getY());
    }

    /**
     * Distance of the line
     *
     * @return Double with the distance of the line
     */
    public double distance() {
        double dx = p1.getX() - p2.getX();
        double dy = p1.getY() - p2.getY();
        return Math.sqrt(dx * dx + dy * dy);
    }

    @Override
    public String toString() {
        return p1 + " | " + p2;
    }

    public boolean intersectsObstacles(ArrayList<Obstacle> obstacles) {
        if (obstacles == null)
            return false;
        for (Obstacle thisObstacle : obstacles)
            if (intersectsObstacle(thisObstacle))
                return true;
        return false;
    }

    public void setP2(Point currentNewPoint) {
        p2 = currentNewPoint;
    }
}
import java.util.ArrayList;

public class Manager {
    private final Robot[] robots;
    private final RequestQueue requestQueue;

    Manager(ArrayList<Obstacle> obstacles) {
        robots = new Robot[4];
        requestQueue = new RequestQueue();

        robots[0] = new Robot(new Point(0, 0), obstacles);
        robots[1] = new Robot(new Point(999, 0), obstacles);
        robots[2] = new Robot(new Point(999, 999), obstacles);
        robots[3] = new Robot(new Point(0, 999), obstacles);
    }

    public boolean isAnyAvailable() {
        for (Robot currentRobot : robots)
            if (!currentRobot.isMoving())
                return true;
        return false;
    }

    public void addRequest(Request request) {
        requestQueue.add(request);
    }

    public void checkAllRequests() {
        if (!isAnyAvailable())
            return;
        requestQueue.checkAllRequests(robots);
    }

    public void update() {
        checkAllRequests();
        for (Robot currentRobot : robots)
            currentRobot.update();
    }

    public String printRobots(int count) {
        StringBuilder result = new StringBuilder("Step " + count + ":");
        for (Robot currentRobot : robots)
            result.append(currentRobot.toString());
        return result.toString();
    }

    public RequestQueue getRequestQueue() {
        return requestQueue;
    }

    public Robot[] getRobots() {
        return robots;
    }

    public ArrayList<Obstacle> getObstacles() {
        return robots[0].getObstacles();
    }
}
public class Point {
    private int x;
    private int y;

    /**
     * Create Point with coordinates (0,0)
     */
    public Point() {
        x = 0;
        y = 0;
    }

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

    public int getX() {
        return x;
    }

    public int getY() {
        return y;
    }

    /**
     * Returns distance between 2 points
     *
     * @param p Point to calculate distance
     * @return Distance between 2 points
     */
    public double dist(Point p) {
        double dx = x - p.x;
        double dy = y - p.y;
        return Math.sqrt(dx * dx + dy * dy);
    }

    public boolean arePointsCollinear(Point p2, Point p3) {
        int slope1 = (p2.getY() - y) * (p3.getX() - x);
        int slope2 = (p3.getY() - y) * (p2.getX() - x);
        return slope1 == slope2;
    }

    @Override
    public boolean equals(Object o) {
        if (this == o) return true;
        if (!(o instanceof Point point)) return false;
        return x == point.x && y == point.y;
    }

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

    public void setX(int nx) {
        x =nx;
    }

    public void setY(int ny) {
        y = ny;
    }
}/**
 * Class contains 4 points, that counter-clockwise create a rectangle
 *
 * @author Daniel Ramos
 * @version 23/03/2023
 * @inv p1.dist(p3) == p2.dist(p4) && p1.dist(p3) != 0
 */
public class Rectangle extends Polygon {
    /**
     * Creates Rectangle if p1.dist(p3) == p2.dist(p4) && p1.dist(p3) != 0
     *
     * @param points 4 Points of Rectangle
     */
    public Rectangle(Point[] points) {
        super(points);
        if (points[0].dist(points[2]) != points[1].dist(points[3]) || points[0].dist(points[2]) == 0) {
            System.out.println("Retangulo:vi");
            System.exit(0);
        }
    }

    /**
     * Creates Rectangle if p1.dist(p3) == p2.dist(p4) && p1.dist(p3) != 0
     *
     * @param s String should be separated by spaces and needs to have 8 integers
     */
    public Rectangle(String s) {
        this(getValues(s));
    }

    /**
     * Gets all points from String s
     *
     * @param s String should be separated by spaces and needs to have 8 integers
     * @return All points in String
     */
    private static Point[] getValues(String s) {
        String[] aux = s.split(" ");
        return new Point[]{new Point(Integer.parseInt(aux[1]), Integer.parseInt(aux[2])), new Point(Integer.parseInt(aux[3]), Integer.parseInt(aux[4])), new Point(Integer.parseInt(aux[5]), Integer.parseInt(aux[6])), new Point(Integer.parseInt(aux[7]), Integer.parseInt(aux[8]))};
    }


}
import java.util.ArrayList;

public class Request {
    public static final int WAITING = 0;
    public static final int IN_PROGRESS = 1;
    public static final int FINISHED = 2;
    public static final int DENIED = 3;
    private final Point from;
    private final Point to;
    private int status;
    private Trajetoria bestPath; // CACHE

    Request(Point from, Point to, ArrayList<Obstacle> obstacles) {
        this.from = from;
        this.to = to;
        status = WAITING;
        bestPath = new Trajetoria(from, to, obstacles);
    }

    public int getStatus() {
        return status;
    }

    public void setStatus(int status) {
        this.status = status;
    }

    public Point getFrom() {
        return from;
    }

    public Point getTo() {
        return to;
    }

    public Trajetoria getBestPath() {
        return bestPath;
    }
}
import java.util.ArrayDeque;
import java.util.Queue;

public class RequestQueue {
    private final Queue<Request> queueRequest;

    public RequestQueue() {
        this.queueRequest = new ArrayDeque<>();
    }

    public void add(Request request) {
        queueRequest.add(request);
    }

    public void checkAllRequests(Robot[] robots) {
        Request[] thisQueue = queueRequest.toArray(new Request[0]);
        for (Request currentRequest : thisQueue) {
            if (currentRequest.getStatus() == Request.IN_PROGRESS)
                continue;

            if (currentRequest.getStatus() == Request.FINISHED) {
                queueRequest.remove(currentRequest);
                continue;
            }

            checkRequest(currentRequest, robots);
        }
    }

    public void checkRequest(Request currentRequest, Robot[] robots) {
        boolean isEmpty = true;
        Trajetoria[] q = new Trajetoria[4];

        for (int i = 0; i < 4; i++) {
            q[i] = robots[i].checkRequest(currentRequest);
            if (q[i] != null) {
                isEmpty = false;
            } else
                robots[i].refuseRequest(currentRequest);
        }

        if (isEmpty) {
            currentRequest.setStatus(Request.DENIED);
            queueRequest.remove(currentRequest);
            queueRequest.add(currentRequest);
            return;
        }

        for (int i = 0; i < 4; i++) {

        }

        int index = lowestDistance(q);
        robots[index].startWorking(q[index], currentRequest);
    }

    public int lowestDistance(Trajetoria[] q) {
        int lowest = Integer.MAX_VALUE;
        int result = -1;
        for (int i = 0; i < 4; i++)
            if (q[i] != null && q[i].size() < lowest) {
                lowest = q[i].size();
                result = i;
            }
        return result;
    }

    public Request[] getRequestsArray() {
        return queueRequest.toArray(new Request[0]);
    }

    @Override
    public String toString() {
        Request[] x = queueRequest.toArray(new Request[0]);
        StringBuilder result = new StringBuilder();
        for (Request request : x)
            result.append(request.getStatus()).append(" - ");

        return result.toString();
    }
}
import java.util.ArrayList;
import java.util.HashSet;

public class Robot {
    private final Point chargePoint;
    private final Trajetoria currentTrajectory;
    private final ArrayList<Obstacle> obstacles;
    private final HashSet<Request> refusedRequests; // CACHE
    private Point currentPosition;
    private Request currentRequest;
    private double staminaToGoHome;
    private double stamina;
    private boolean isMoving;
    private boolean isLoaded;

    public Robot(Point chargePoint, ArrayList<Obstacle> obstacles) {
        this.chargePoint = chargePoint;
        this.obstacles = obstacles;
        refusedRequests = new HashSet<>();
        currentPosition = new Point(chargePoint.getX(), chargePoint.getY());
        isMoving = false;
        stamina = 100;
        staminaToGoHome = 0;
        currentTrajectory = new Trajetoria();
        currentRequest = null;
        isLoaded = false;
    }

    public ArrayList<Obstacle> getObstacles() {
        return obstacles;
    }

    public Point getCurrentPosition() {
        return currentPosition;
    }

    public Point getChargePoint() {
        return chargePoint;
    }

    public Trajetoria getCurrentTrajectory() {
        return currentTrajectory;
    }

    public double getStamina() {
        return stamina;
    }

    public void setStamina(double x) {
        if (x < 0) {
            System.out.println("ERROR: robot without stamina");
            System.exit(0);
        } else if (x > 100)
            stamina = 100;
        else
            stamina = x;
    }

    public boolean isMoving() {
        return isMoving;
    }

    public void update() {
        if (shouldGoHome())
            isMoving = true;
        updateStamina();
        if (isMoving && !currentTrajectory.isEmpty())
            move();
    }

    public void updateStamina() {
        // IF IT IS MOVING
        if (isMoving)
            setStamina(stamina - 0.1);
            // IF IT IS CHARGING
        else if (chargePoint.equals(currentPosition)) {
            setStamina(stamina + 1);
            if(stamina == 99)
                refusedRequests.clear(); // START CHECKING AT 99
        }
            // IF STOPPED OUTSIDE CHARGE POINT
        else
            setStamina(stamina - 0.01);
    }

    public void move() {
        // CURRENT OBJECTIVE
        currentPosition = currentTrajectory.nextPoint();
        staminaToGoHome = currentTrajectory.size() * 0.1 + 0.2;

        if (currentPosition.equals(chargePoint)) {
            isMoving = false;
            refusedRequests.clear(); // CLEAR REFUSED
        }

        // UPDATE REQUEST
        if (currentRequest != null) {
            if (currentPosition.equals(currentRequest.getFrom()))
                isLoaded = true;
            else if (currentPosition.equals(currentRequest.getTo())) {
                isLoaded = false;
                currentRequest.setStatus(Request.FINISHED);
                currentRequest = null;
                isMoving = false;
                refusedRequests.clear();
            }
        }
    }

    public void startWorking(Trajetoria path, Request request) {
        request.setStatus(Request.IN_PROGRESS);
        currentTrajectory.clear();
        currentTrajectory.addAll(path.getPoints());
        currentRequest = request;
        isMoving = true;
    }

    public Trajetoria checkRequest(Request request) {
        if (isMoving || currentRequest != null || refusedRequests.contains(request))
            return null;

        // FROM CURRENT POSITION TO PACKAGE
        Trajetoria startToFrom = new Trajetoria(currentPosition, request.getFrom(), obstacles);
        double staminaPredict = staminaAfterTraj(startToFrom, stamina);
        if (staminaPredict < 0)
            return null;

        // FROM PACKAGE TO DESTINATION
        Trajetoria fromToTo = request.getBestPath(); // SAVED IN CACHE
        staminaPredict = staminaAfterTraj(fromToTo, staminaPredict);
        if (staminaPredict < 0)
            return null;

        // FROM DESTINATION TO CHARGEPOINT
        Trajetoria toToEnd = new Trajetoria(request.getTo(), chargePoint, obstacles);
        staminaPredict = staminaAfterTraj(toToEnd, staminaPredict);
        if (staminaPredict < 0)
            return null;

        return new Trajetoria(startToFrom, fromToTo, toToEnd);
    }

    public boolean shouldGoHome() {
        return staminaToGoHome > stamina && !chargePoint.equals(currentPosition);
    }

    public double staminaAfterTraj(Trajetoria traj, double stamina) {
        if (traj == null)
            return 0;
        return stamina - traj.size() * 0.1;
    }

    @Override
    public String toString() {
        char x = '*';
        if (!isLoaded) x = '-';
        String stam = String.format("%.2f", stamina);
        return "(" + currentPosition + "," + stam + "," + x + ") ";
    }

    public void refuseRequest(Request currentRequest) {
        refusedRequests.add(currentRequest);
    }
}
import java.util.ArrayDeque;
import java.util.ArrayList;
import java.util.Queue;

public class Trajetoria implements AStar {
    private final Queue<Point> points;

    public Trajetoria() {
        points = new ArrayDeque<>();
    }

    public Trajetoria(Point start, Point end, ArrayList<Obstacle> obstacle) {
        ArrayList<Node> points = bestPath(start, end, obstacle);
        Queue<Point> result = new ArrayDeque<>();
        for (Node n : points)
            result.add(n.getP());
        this.points = result;
    }

    public Trajetoria(Trajetoria t1, Trajetoria t2, Trajetoria t3) {
        t1.remove();
        t2.remove();
        t3.remove();
        points = new ArrayDeque<>();
        points.addAll(t1.getPoints());
        points.addAll(t2.getPoints());
        points.addAll(t3.getPoints());
    }

    public Queue<Point> getPoints() {
        return points;
    }

    private void remove() {
        points.remove();
    }

    public Point nextPoint() {
        return points.poll();
    }

    public boolean isEmpty() {
        return points.isEmpty();
    }

    public void clear() {
        points.clear();
    }

    public void addAll(Queue<Point> path) {
        points.addAll(path);
    }

    public int size() {
        return points.size();
    }

    public Point[] getPointsArray() {
        return points.toArray(new Point[0]);
    }
}