# PathFinderService.java

    package io.github.js0ny.ilp_coursework.service;

    import com.fasterxml.jackson.core.JsonProcessingException;
    import com.fasterxml.jackson.databind.DeserializationFeature;
    import com.fasterxml.jackson.databind.ObjectMapper;
    import com.fasterxml.jackson.databind.SerializationFeature;
    import com.fasterxml.jackson.datatype.jsr310.JavaTimeModule;

    import io.github.js0ny.ilp_coursework.controller.DroneController;
    import io.github.js0ny.ilp_coursework.data.common.Angle;
    import io.github.js0ny.ilp_coursework.data.common.DroneAvailability;
    import io.github.js0ny.ilp_coursework.data.common.LngLat;
    import io.github.js0ny.ilp_coursework.data.common.Region;
    import io.github.js0ny.ilp_coursework.data.external.Drone;
    import io.github.js0ny.ilp_coursework.data.external.RestrictedArea;
    import io.github.js0ny.ilp_coursework.data.external.ServicePoint;
    import io.github.js0ny.ilp_coursework.data.external.ServicePointDrones;
    import io.github.js0ny.ilp_coursework.data.request.MedDispatchRecRequest;
    import io.github.js0ny.ilp_coursework.data.response.DeliveryPathResponse;
    import io.github.js0ny.ilp_coursework.data.response.DeliveryPathResponse.DronePath;
    import io.github.js0ny.ilp_coursework.data.response.DeliveryPathResponse.DronePath.Delivery;

    import org.springframework.beans.factory.annotation.Autowired;
    import org.springframework.stereotype.Service;

    import java.time.LocalDateTime;
    import java.util.ArrayList;
    import java.util.Comparator;
    import java.util.HashMap;
    import java.util.LinkedHashMap;
    import java.util.LinkedList;
    import java.util.List;
    import java.util.Map;
    import java.util.stream.Collectors;

    /**
     * Class that handles calculations about deliverypath
     *
     * @see DroneInfoService
     * @see DroneController
     * @see DeliveryPathResponse
     */
    @Service
    public class PathFinderService {

        /**
         * Hard stop on how many pathfinding iterations we attempt for a single segment before bailing,
         * useful for preventing infinite loops caused by precision quirks or unexpected map data.
         *
         * @see #computePath(LngLat, LngLat)
         */
        private static final int MAX_SEGMENT_ITERATIONS = 8_000;

        // Services
        private final GpsCalculationService gpsCalculationService;
        private final DroneInfoService droneInfoService;
        private final ObjectMapper objectMapper;
        private final List<Drone> drones;
        private final Map<String, Drone> droneById;
        private final Map<String, Integer> droneServicePointMap;
        private final Map<Integer, LngLat> servicePointLocations;
        private final List<Region> restrictedRegions;

        @Autowired(required = false)
        private TelemetryService telemetryService;

        /**
         * Constructor for PathFinderService. The dependencies are injected by Spring and the
         * constructor pre-computes reference maps used throughout the request lifecycle.
         *
         * @param gpsCalculationService Service handling geometric operations.
         * @param droneInfoService Service that exposes drone metadata and capability information.
         */
        public PathFinderService(
                GpsCalculationService gpsCalculationService, DroneInfoService droneInfoService) {
            this.gpsCalculationService = gpsCalculationService;
            this.droneInfoService = droneInfoService;
            this.objectMapper = new ObjectMapper();
            objectMapper.registerModule(new JavaTimeModule());
            objectMapper.disable(SerializationFeature.WRITE_DATES_AS_TIMESTAMPS);
            objectMapper.disable(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES);

            this.drones = droneInfoService.fetchAllDrones();
            List<ServicePoint> servicePoints = droneInfoService.fetchServicePoints();
            List<ServicePointDrones> servicePointAssignments =
                    droneInfoService.fetchDronesForServicePoints();
            List<RestrictedArea> restrictedAreas = droneInfoService.fetchRestrictedAreas();

            this.droneById = this.drones.stream().collect(Collectors.toMap(Drone::id, drone -> drone));

            this.droneServicePointMap = new HashMap<>();
            for (ServicePointDrones assignment : servicePointAssignments) {
                if (assignment == null || assignment.drones() == null) {
                    continue;
                }
                for (DroneAvailability availability : assignment.drones()) {
                    if (availability == null || availability.id() == null) {
                        continue;
                    }
                    droneServicePointMap.put(availability.id(), assignment.servicePointId());
                }
            }

            this.servicePointLocations =
                    servicePoints.stream()
                            .collect(
                                    Collectors.toMap(
                                            ServicePoint::id, sp -> new LngLat(sp.location())));

            this.restrictedRegions = restrictedAreas.stream().map(RestrictedArea::toRegion).toList();
        }

        /**
         * Produce a delivery plan for the provided dispatch records. Deliveries are grouped per
         * compatible drone and per trip to satisfy each drone move limit.
         *
         * @param records Dispatch records to be fulfilled.
         * @return Aggregated path response with cost and move totals.
         * @see #calculateDeliveryPathAsGeoJson(MedDispatchRecRequest[])
         */
        public DeliveryPathResponse calculateDeliveryPath(MedDispatchRecRequest[] records) {
            if (records == null || records.length == 0) {
                return new DeliveryPathResponse(0f, 0, new DronePath[0]);
            }

            Map<Integer, LocalDateTime> deliveryTimestamps = new HashMap<>();
            for (var r : records) {
                if (isRestricted(r.delivery())) {
                    throw new IllegalStateException(
                            "Delivery "
                                    + r.id()
                                    + " is located within a restricted area and cannot be fulfilled");
                }
                if (r.date() != null && r.time() != null) {
                    deliveryTimestamps.put(r.id(), LocalDateTime.of(r.date(), r.time()));
                }
            }

            Map<String, List<MedDispatchRecRequest>> assigned = assignDeliveries(records);

            List<DronePath> paths = new ArrayList<>();
            float totalCost = 0f;
            int totalMoves = 0;

            for (Map.Entry<String, List<MedDispatchRecRequest>> entry : assigned.entrySet()) {
                String droneId = entry.getKey();
                Drone drone = droneById.get(droneId);
                if (drone == null) {
                    continue;
                }
                Integer spId = droneServicePointMap.get(droneId);
                if (spId == null) {
                    continue;
                }
                LngLat servicePointLocation = servicePointLocations.get(spId);
                if (servicePointLocation == null) {
                    continue;
                }

                List<MedDispatchRecRequest> sortedDeliveries =
                        entry.getValue().stream()
                                .sorted(
                                        Comparator.comparingDouble(
                                                rec ->
                                                        gpsCalculationService.calculateDistance(
                                                                servicePointLocation, rec.delivery())))
                                .toList();

                List<List<MedDispatchRecRequest>> trips =
                        splitTrips(sortedDeliveries, drone, servicePointLocation);

                for (List<MedDispatchRecRequest> trip : trips) {
                    TripResult result = buildTrip(drone, servicePointLocation, trip);
                    if (result != null) {
                        totalCost += result.cost();
                        totalMoves += result.moves();
                        paths.add(result.path());
                    }
                }
            }

            var resp = new DeliveryPathResponse(totalCost, totalMoves, paths.toArray(new DronePath[0]));

            if (telemetryService != null) {
                telemetryService.sendEventAsyncByPathResponse(resp, deliveryTimestamps);
            }

            return resp;
        }

        /*
         * Convenience wrapper around {@link #calculateDeliveryPath} that serializes the
         * result into a
         * GeoJSON FeatureCollection suitable for mapping visualization.
         *
         * @param records Dispatch records to be fulfilled.
         *
         * @return GeoJSON payload representing every delivery flight path.
         *
         * @throws IllegalStateException When the payload cannot be serialized.
         */
        public String calculateDeliveryPathAsGeoJson(MedDispatchRecRequest[] records) {
            DeliveryPathResponse response = calculateDeliveryPath(records);
            Map<String, Object> featureCollection = new LinkedHashMap<>();
            featureCollection.put("type", "FeatureCollection");
            List<Map<String, Object>> features = new ArrayList<>();

            if (response != null && response.dronePaths() != null) {
                for (DronePath dronePath : response.dronePaths()) {
                    if (dronePath == null || dronePath.deliveries() == null) {
                        continue;
                    }
                    for (Delivery delivery : dronePath.deliveries()) {
                        Map<String, Object> feature = new LinkedHashMap<>();
                        feature.put("type", "Feature");

                        Map<String, Object> properties = new LinkedHashMap<>();
                        properties.put("droneId", dronePath.droneId());
                        properties.put("deliveryId", delivery.deliveryId());
                        feature.put("properties", properties);

                        Map<String, Object> geometry = new LinkedHashMap<>();
                        geometry.put("type", "LineString");

                        List<List<Double>> coordinates = new ArrayList<>();
                        if (delivery.flightPath() != null) {
                            for (LngLat point : delivery.flightPath()) {
                                coordinates.add(List.of(point.lng(), point.lat()));
                            }
                        }
                        geometry.put("coordinates", coordinates);
                        feature.put("geometry", geometry);
                        features.add(feature);
                    }
                }
            }

            featureCollection.put("features", features);

            try {
                return objectMapper.writeValueAsString(featureCollection);
            } catch (JsonProcessingException e) {
                throw new IllegalStateException("Failed to generate GeoJSON payload", e);
            }
        }

        /**
         * Group dispatch records by their assigned drone, ensuring every record is routed through
         * {@link #findBestDrone(MedDispatchRecRequest)} exactly once and discarding invalid entries.
         *
         * @param records Dispatch records to be grouped.
         * @return Map keyed by drone ID with the deliveries it should service.
         */
        private Map<String, List<MedDispatchRecRequest>> assignDeliveries(
                MedDispatchRecRequest[] records) {
            Map<String, List<MedDispatchRecRequest>> assignments = new LinkedHashMap<>();
            for (MedDispatchRecRequest record : records) {
                if (record == null || record.delivery() == null) {
                    continue;
                }
                String droneId = findBestDrone(record);
                assignments.computeIfAbsent(droneId, id -> new ArrayList<>()).add(record);
            }
            return assignments;
        }

        /**
         * Choose the best drone for the provided record. Currently that equates to picking the closest
         * compatible drone to the delivery location.
         *
         * @param record Dispatch record that needs fulfillment.
         * @return Identifier of the drone that should fly the mission.
         * @throws IllegalStateException If no available drone can serve the request.
         */
        private String findBestDrone(MedDispatchRecRequest record) {
            double bestScore = Double.MAX_VALUE;
            String bestDrone = null;

            for (Drone drone : drones) {
                if (!droneInfoService.droneMatchesRequirement(drone, record)) {
                    continue;
                }
                String droneId = drone.id();
                Integer servicePointId = droneServicePointMap.get(droneId);
                if (servicePointId == null) {
                    continue;
                }
                LngLat servicePointLocation = servicePointLocations.get(servicePointId);
                if (servicePointLocation == null) {
                    continue;
                }

                double distance =
                        gpsCalculationService.calculateDistance(
                                servicePointLocation, record.delivery());

                if (distance < bestScore) {
                    bestScore = distance;
                    bestDrone = droneId;
                }
            }
            if (bestDrone == null) {
                throw new IllegalStateException("No available drone for delivery " + record.id());
            }
            return bestDrone;
        }

        /**
         * Break a sequence of deliveries into several trips that each respect the drone move limit. The
         * deliveries should already be ordered by proximity for sensible grouping.
         *
         * @param deliveries Deliveries assigned to a drone.
         * @param drone Drone that will service the deliveries.
         * @param servicePoint Starting and ending point of every trip.
         * @return Partitioned trips with at least one delivery each.
         * @throws IllegalStateException If a single delivery exceeds the drone's move limit.
         */
        private List<List<MedDispatchRecRequest>> splitTrips(
                List<MedDispatchRecRequest> deliveries, Drone drone, LngLat servicePoint) {
            List<List<MedDispatchRecRequest>> trips = new ArrayList<>();
            List<MedDispatchRecRequest> currentTrip = new ArrayList<>();
            for (MedDispatchRecRequest delivery : deliveries) {
                currentTrip.add(delivery);
                int requiredMoves = estimateTripMoves(servicePoint, currentTrip);
                if (requiredMoves > drone.capability().maxMoves()) {
                    currentTrip.remove(currentTrip.size() - 1);
                    if (currentTrip.isEmpty()) {
                        throw new IllegalStateException(
                                "Delivery "
                                        + delivery.id()
                                        + " exceeds drone "
                                        + drone.id()
                                        + " move limit");
                    }
                    trips.add(new ArrayList<>(currentTrip));
                    currentTrip.clear();
                    currentTrip.add(delivery);
                }
            }
            if (!currentTrip.isEmpty()) {
                int requiredMoves = estimateTripMoves(servicePoint, currentTrip);
                if (requiredMoves > drone.capability().maxMoves()) {
                    throw new IllegalStateException(
                            "Delivery plan exceeds move limit for drone " + drone.id());
                }
                trips.add(new ArrayList<>(currentTrip));
            }
            return trips;
        }

        /**
         * Build a single trip for the provided drone, including the entire flight path to every
         * delivery and back home. The resulting structure contains the {@link DronePath} representation
         * as well as cost and moves consumed.
         *
         * @param drone Drone executing the trip.
         * @param servicePoint Starting/ending location of the trip.
         * @param deliveries Deliveries to include in the trip in execution order.
         * @return Trip information or {@code null} if no deliveries are provided.
         * @see DeliveryPathResponse.DronePath
         */
        private TripResult buildTrip(
                Drone drone, LngLat servicePoint, List<MedDispatchRecRequest> deliveries) {
            if (deliveries == null || deliveries.isEmpty()) {
                return null;
            }
            List<Delivery> flightPlans = new ArrayList<>();
            LngLat current = servicePoint;
            int moves = 0;

            for (int i = 0; i < deliveries.size(); i++) {
                MedDispatchRecRequest delivery = deliveries.get(i);
                PathSegment toDelivery = computePath(current, delivery.delivery());
                List<LngLat> flightPath = new ArrayList<>(toDelivery.positions());
                if (!flightPath.isEmpty()) {
                    LngLat last = flightPath.get(flightPath.size() - 1);
                    if (!last.isSamePoint(delivery.delivery())) {
                        flightPath.add(delivery.delivery());
                    }
                } else {
                    flightPath.add(current);
                    flightPath.add(delivery.delivery());
                }
                flightPath.add(delivery.delivery());
                moves += toDelivery.moves();

                if (i == deliveries.size() - 1) {
                    PathSegment backHome = computePath(delivery.delivery(), servicePoint);
                    backHome.appendSkippingStart(flightPath);
                    moves += backHome.moves();
                    current = servicePoint;
                } else {
                    current = delivery.delivery();
                }
                flightPlans.add(new Delivery(delivery.id(), flightPath));
            }

            float cost =
                    drone.capability().costInitial()
                            + drone.capability().costFinal()
                            + (float) (drone.capability().costPerMove() * moves);

            DronePath path = new DronePath(drone.parseId(), flightPlans);

            return new TripResult(path, moves, cost);
        }

        /**
         * Estimate the number of moves a prospective trip would need by replaying the path calculation
         * without mutating any persistent state.
         *
         * @param servicePoint Trip origin.
         * @param deliveries Deliveries that would compose the trip.
         * @return Total moves required to fly the proposed itinerary.
         */
        private int estimateTripMoves(LngLat servicePoint, List<MedDispatchRecRequest> deliveries) {
            if (deliveries.isEmpty()) {
                return 0;
            }
            int moves = 0;
            LngLat current = servicePoint;
            for (MedDispatchRecRequest delivery : deliveries) {
                PathSegment segment = computePath(current, delivery.delivery());
                moves += segment.moves();
                current = delivery.delivery();
            }
            moves += computePath(current, servicePoint).moves();
            return moves;
        }

        /**
         * Build a path between {@code start} and {@code target} by repeatedly moving in snapped
         * increments while avoiding restricted zones.
         *
         * @param start Start coordinate.
         * @param target Destination coordinate.
         * @return Sequence of visited coordinates and move count.
         * @see #nextPosition(LngLat, LngLat)
         */
        private PathSegment computePath(LngLat start, LngLat target) {
            List<LngLat> positions = new ArrayList<>();
            if (start == null || target == null) {
                return new PathSegment(positions, 0);
            }
            positions.add(start);
            LngLat current = start;
            int iterations = 0;
            while (!gpsCalculationService.isCloseTo(current, target)
                    && iterations < MAX_SEGMENT_ITERATIONS) {
                LngLat next = nextPosition(current, target);
                if (next.isSamePoint(current)) {
                    break;
                }
                positions.add(next);
                current = next;
                iterations++;
            }
            if (!positions.get(positions.size() - 1).isSamePoint(target)) {
                positions.add(target);
            }
            return new PathSegment(positions, Math.max(0, positions.size() - 1));
        }

        /**
         * Determine the next position on the path from {@code current} toward {@code target},
         * preferring the snapped angle closest to the desired heading that does not infiltrate a
         * restricted region.
         *
         * @param current Current coordinate.
         * @param target Destination coordinate.
         * @return Next admissible coordinate or the original point if none can be found.
         */
        private LngLat nextPosition(LngLat current, LngLat target) {
            double desiredAngle =
                    Math.toDegrees(
                            Math.atan2(target.lat() - current.lat(), target.lng() - current.lng()));
            List<Angle> candidateAngles = buildAngleCandidates(desiredAngle);
            for (Angle angle : candidateAngles) {
                LngLat next = gpsCalculationService.nextPosition(current, angle);
                if (!isRestricted(next)) {
                    return next;
                }
            }
            return current;
        }

        /**
         * Build a sequence of candidate angles centered on the desired heading, expanding symmetrically
         * clockwise and counter-clockwise to explore alternative headings if the primary path is
         * blocked.
         *
         * @param desiredAngle Bearing in degrees between current and target positions.
         * @return Ordered list of candidate snapped angles.
         * @see Angle#snap(double)
         */
        private List<Angle> buildAngleCandidates(double desiredAngle) {
            List<Angle> angles = new LinkedList<>();
            Angle snapped = Angle.snap(desiredAngle);
            angles.add(snapped);
            for (int offset = 1; offset <= 8; offset++) {
                angles.add(snapped.offset(offset));
                angles.add(snapped.offset(-offset));
            }
            return angles;
        }

        /**
         * Check whether the provided coordinate falls inside any restricted region.
         *
         * @param position Coordinate to inspect.
         * @return {@code true} if the position intersects a restricted area.
         * @see #restrictedRegions
         */
        private boolean isRestricted(LngLat position) {
            for (Region region : restrictedRegions) {
                if (gpsCalculationService.checkIsInRegion(position, region)) {
                    return true;
                }
            }
            return false;
        }

        /**
         * Representation of a computed path segment wrapping the visited positions and the number of
         * moves taken to traverse them.
         *
         * @param positions Ordered coordinates that describe the path.
         * @param moves Number of moves consumed by the path.
         */
        private record PathSegment(List<LngLat> positions, int moves) {
            /**
             * Append the positions from this segment to {@code target}, skipping the first coordinate
             * as it is already represented by the last coordinate in the consumer path.
             *
             * @param target Mutable list to append to.
             */
            private void appendSkippingStart(List<LngLat> target) {
                for (int i = 1; i < positions.size(); i++) {
                    target.add(positions.get(i));
                }
            }
        }

        /**
         * Bundle containing the calculated {@link DronePath}, total moves and financial cost for a
         * single trip.
         */
        private record TripResult(DronePath path, int moves, float cost) {}
    }