From 221707afb0c4f9c6d219fa15aa81c07e08cec80c Mon Sep 17 00:00:00 2001
From: js0ny <contact@js0ny.net>
Date: Thu, 27 Nov 2025 11:59:25 +0000
Subject: [PATCH] feat(dtos): Add util function for PathFinder

---
 .../ilp_coursework/data/common/Angle.java     | 43 +++++++++++++------
 .../ilp_coursework/data/common/LngLat.java    | 12 ++++++
 .../ilp_coursework/data/external/Drone.java   |  8 ++++
 3 files changed, 50 insertions(+), 13 deletions(-)

diff --git a/src/main/java/io/github/js0ny/ilp_coursework/data/common/Angle.java b/src/main/java/io/github/js0ny/ilp_coursework/data/common/Angle.java
index ea07457..bcdd0c2 100644
--- a/src/main/java/io/github/js0ny/ilp_coursework/data/common/Angle.java
+++ b/src/main/java/io/github/js0ny/ilp_coursework/data/common/Angle.java
@@ -12,35 +12,51 @@ public record Angle(double degrees) {
     public Angle {
         if (degrees < 0 || degrees >= 360) {
             throw new IllegalArgumentException(
-                "Angle must be in range [0, 360). Got: " + degrees
-            );
+                    "Angle must be in range [0, 360). Got: " + degrees);
         }
 
         // Should be a multiple of 22.5 (one of the 16 major directions)
         double remainder = degrees % STEP;
 
-        // Floating point modulo may have tiny errors, e.g. 45.0 % 22.5 could be 0.0 or 1.0e-15
-        // So we need to check if the remainder is small enough, or close enough to STEP (handling negative errors)
-        if (
-            Math.abs(remainder) > EPSILON &&
-            Math.abs(remainder - STEP) > EPSILON
-        ) {
+        // Floating point modulo may have tiny errors, e.g. 45.0 % 22.5 could be 0.0 or
+        // 1.0e-15
+        // So we need to check if the remainder is small enough, or close enough to STEP
+        // (handling negative errors)
+        if (Math.abs(remainder) > EPSILON &&
+                Math.abs(remainder - STEP) > EPSILON) {
             throw new IllegalArgumentException(
-                "Angle must be a multiple of 22.5 (one of the 16 major directions). Got: " +
-                    degrees
-            );
+                    "Angle must be a multiple of 22.5 (one of the 16 major directions). Got: " +
+                            degrees);
         }
     }
 
     public static Angle fromIndex(int index) {
         if (index < 0 || index > 15) {
             throw new IllegalArgumentException(
-                "Direction index must be between 0 and 15"
-            );
+                    "Direction index must be between 0 and 15");
         }
         return new Angle(index * STEP);
     }
 
+    public static Angle snap(double rawAngle) {
+        double normalized = normalize(rawAngle);
+        double snapped = Math.round(normalized / STEP) * STEP;
+        return new Angle(normalize(snapped));
+    }
+
+    public Angle offset(int increments) {
+        double rotated = degrees + increments * STEP;
+        return new Angle(normalize(rotated));
+    }
+
+    private static double normalize(double angle) {
+        double normalized = angle % 360;
+        if (normalized < 0) {
+            normalized += 360;
+        }
+        return normalized;
+    }
+
     public static double toRadians(double degrees) {
         return Math.toRadians(degrees);
     }
@@ -48,4 +64,5 @@ public record Angle(double degrees) {
     public double toRadians() {
         return Math.toRadians(degrees);
     }
+
 }
diff --git a/src/main/java/io/github/js0ny/ilp_coursework/data/common/LngLat.java b/src/main/java/io/github/js0ny/ilp_coursework/data/common/LngLat.java
index 6208b22..99ac2ea 100644
--- a/src/main/java/io/github/js0ny/ilp_coursework/data/common/LngLat.java
+++ b/src/main/java/io/github/js0ny/ilp_coursework/data/common/LngLat.java
@@ -8,6 +8,8 @@ package io.github.js0ny.ilp_coursework.data.common;
  * @param lat latitude of the coordinate/point
  */
 public record LngLat(double lng, double lat) {
+    private static final double EPSILON = 1e-9;
+
     public LngLat {
         if (lat < -90 || lat > 90) {
             throw new IllegalArgumentException(
@@ -25,4 +27,14 @@ public record LngLat(double lng, double lat) {
     public LngLat(LngLatAlt coord) {
         this(coord.lng(), coord.lat());
     }
+
+    public boolean isSamePoint(LngLat other) {
+        if (other == null) {
+            return false;
+        }
+        return (
+            Math.abs(lng - other.lng()) < EPSILON &&
+            Math.abs(lat - other.lat()) < EPSILON
+        );
+    }
 }
diff --git a/src/main/java/io/github/js0ny/ilp_coursework/data/external/Drone.java b/src/main/java/io/github/js0ny/ilp_coursework/data/external/Drone.java
index 62baae2..29a6c2d 100644
--- a/src/main/java/io/github/js0ny/ilp_coursework/data/external/Drone.java
+++ b/src/main/java/io/github/js0ny/ilp_coursework/data/external/Drone.java
@@ -9,4 +9,12 @@ public record Drone(
         String name,
         String id,
         DroneCapability capability) {
+
+    public int parseId() {
+        try {
+            return Integer.parseInt(id);
+        } catch (NumberFormatException e) {
+            return id.hashCode();
+        }
+    }
 }