From 3c15c1c6137ec2faedddc69b43843e46c9a1c194 Mon Sep 17 00:00:00 2001
From: Stephen Just <stephenjust@gmail.com>
Date: Sat, 30 Mar 2024 12:38:29 -0600
Subject: [PATCH] Make SimpleNote serializable for AScope (#280)

* Make SimpleNote serializable for AScope

* Remove unintended import

---------

Co-authored-by: Alex Schokking <aschokking@gmail.com>
---
 .../subsystems/vision/SimpleNote.java         |  6 ++-
 .../subsystems/vision/SimpleNoteStruct.java   | 42 +++++++++++++++++++
 .../subsystems/vision/VisionSubsystem.java    |  4 +-
 3 files changed, 49 insertions(+), 3 deletions(-)
 create mode 100644 src/main/java/competition/subsystems/vision/SimpleNoteStruct.java

diff --git a/src/main/java/competition/subsystems/vision/SimpleNote.java b/src/main/java/competition/subsystems/vision/SimpleNote.java
index 60a06556..afb1f0dd 100644
--- a/src/main/java/competition/subsystems/vision/SimpleNote.java
+++ b/src/main/java/competition/subsystems/vision/SimpleNote.java
@@ -7,6 +7,8 @@ public class SimpleNote implements StructSerializable {
     final double yaw;
     final double pitch;
 
+    public static final SimpleNoteStruct struct = new SimpleNoteStruct();
+
     public SimpleNote(double area, double yaw, double pitch) {
         this.area = area;
         this.yaw = yaw;
@@ -21,5 +23,7 @@ public double getYaw() {
         return yaw;
     }
 
-    public double getPitch() { return pitch; }
+    public double getPitch() {
+        return pitch;
+    }
 }
diff --git a/src/main/java/competition/subsystems/vision/SimpleNoteStruct.java b/src/main/java/competition/subsystems/vision/SimpleNoteStruct.java
new file mode 100644
index 00000000..bf6921cd
--- /dev/null
+++ b/src/main/java/competition/subsystems/vision/SimpleNoteStruct.java
@@ -0,0 +1,42 @@
+package competition.subsystems.vision;
+
+import edu.wpi.first.util.struct.Struct;
+
+import java.nio.ByteBuffer;
+
+public class SimpleNoteStruct implements Struct<SimpleNote> {
+    @Override
+    public Class<SimpleNote> getTypeClass() {
+        return SimpleNote.class;
+    }
+
+    @Override
+    public String getTypeString() {
+        return "struct:SimpleNote";
+    }
+
+    @Override
+    public int getSize() {
+        return kSizeDouble * 3;
+    }
+
+    @Override
+    public String getSchema() {
+        return "double area; double yaw; double pitch";
+    }
+
+    @Override
+    public SimpleNote unpack(ByteBuffer byteBuffer) {
+        var area = byteBuffer.getDouble();
+        var yaw = byteBuffer.getDouble();
+        var pitch = byteBuffer.getDouble();
+        return new SimpleNote(area, yaw, pitch);
+    }
+
+    @Override
+    public void pack(ByteBuffer byteBuffer, SimpleNote simpleNote) {
+        byteBuffer.putDouble(simpleNote.getArea());
+        byteBuffer.putDouble(simpleNote.getYaw());
+        byteBuffer.putDouble(simpleNote.getPitch());
+    }
+}
diff --git a/src/main/java/competition/subsystems/vision/VisionSubsystem.java b/src/main/java/competition/subsystems/vision/VisionSubsystem.java
index 89ed7dd4..6e502a2f 100644
--- a/src/main/java/competition/subsystems/vision/VisionSubsystem.java
+++ b/src/main/java/competition/subsystems/vision/VisionSubsystem.java
@@ -396,10 +396,10 @@ public void periodic() {
         }, () -> {
             aKitLog.record("CenterCamLargestTargetArea", -1.0);
             aKitLog.record("CenterCamLargestTargetYaw", 0.0);
-            aKitLog.record("CenterCamLargestTargetPitch", 0);
+            aKitLog.record("CenterCamLargestTargetPitch", 0.0);
         });
 
-        // aKitLog.record("CenterlineDetections", centerlineDetections);
+        aKitLog.record("CenterlineDetections", centerlineDetections);
         aKitLog.record("DetectedNotes", detectedNotes);
         aKitLog.record("PassiveDetectedNotes", passiveDetectedNotes);
     }