From e5a1eea9f23d8bef16376821df94d700f18c8255 Mon Sep 17 00:00:00 2001
From: Matthew Morley <matthew.morley.ca@gmail.com>
Date: Wed, 30 Aug 2023 17:29:09 -0400
Subject: [PATCH] Only encode positive IDs

---
 .../targeting/MultiTargetPNPResults.java      | 36 ++++++++++++++++++-
 .../targeting/PhotonPipelineResult.java       | 13 +++++++
 2 files changed, 48 insertions(+), 1 deletion(-)

diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java
index 7f068409dc..e1bfaf8140 100644
--- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java
+++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java
@@ -40,7 +40,8 @@ public static MultiTargetPNPResults createFromPacket(Packet packet) {
         var results = PNPResults.createFromPacket(packet);
         var ids = new ArrayList<Integer>(MAX_IDS);
         for (int i = 0; i < MAX_IDS; i++) {
-            ids.add((int) packet.decodeByte());
+            int targetId = (int) packet.decodeByte();
+            if (targetId > Byte.MIN_VALUE) ids.add(targetId);
         }
         return new MultiTargetPNPResults(results, ids);
     }
@@ -55,4 +56,37 @@ public void populatePacket(Packet packet) {
             }
         }
     }
+
+    @Override
+    public int hashCode() {
+        final int prime = 31;
+        int result = 1;
+        result = prime * result + ((estimatedPose == null) ? 0 : estimatedPose.hashCode());
+        result = prime * result + ((fiducialIDsUsed == null) ? 0 : fiducialIDsUsed.hashCode());
+        return result;
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (this == obj) return true;
+        if (obj == null) return false;
+        if (getClass() != obj.getClass()) return false;
+        MultiTargetPNPResults other = (MultiTargetPNPResults) obj;
+        if (estimatedPose == null) {
+            if (other.estimatedPose != null) return false;
+        } else if (!estimatedPose.equals(other.estimatedPose)) return false;
+        if (fiducialIDsUsed == null) {
+            if (other.fiducialIDsUsed != null) return false;
+        } else if (!fiducialIDsUsed.equals(other.fiducialIDsUsed)) return false;
+        return true;
+    }
+
+    @Override
+    public String toString() {
+        return "MultiTargetPNPResults [estimatedPose="
+                + estimatedPose
+                + ", fiducialIDsUsed="
+                + fiducialIDsUsed
+                + "]";
+    }
 }
diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
index 056fe3877a..6d85487789 100644
--- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
+++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
@@ -225,4 +225,17 @@ public boolean equals(Object obj) {
         } else if (!multiTagResult.equals(other.multiTagResult)) return false;
         return true;
     }
+
+    @Override
+    public String toString() {
+        return "PhotonPipelineResult [targets="
+                + targets
+                + ", latencyMillis="
+                + latencyMillis
+                + ", timestampSeconds="
+                + timestampSeconds
+                + ", multiTagResult="
+                + multiTagResult
+                + "]";
+    }
 }