From a87f228c32ea3bf3160cb1fc3a39c2ce0ecc9590 Mon Sep 17 00:00:00 2001
From: Vinzenz Schroeter <vinzenz.f.s@gmail.com>
Date: Sat, 12 Apr 2025 21:56:07 +0200
Subject: [PATCH] heap_remove, sp_brightness_grid_into_packet

---
 include/servicepoint.h |  4 ++++
 src/bitmap.rs          |  6 +++---
 src/brightness_grid.rs | 27 ++++++++++++++++++++++++---
 src/connection.rs      |  8 ++++----
 src/lib.rs             |  6 +++++-
 src/packet.rs          |  4 ++--
 6 files changed, 42 insertions(+), 13 deletions(-)

diff --git a/include/servicepoint.h b/include/servicepoint.h
index 0a0d340..1ece5e6 100644
--- a/include/servicepoint.h
+++ b/include/servicepoint.h
@@ -695,6 +695,10 @@ Brightness sp_brightness_grid_get(BrightnessGrid */*notnull*/ brightness_grid,
  */
 size_t sp_brightness_grid_height(BrightnessGrid */*notnull*/ brightness_grid);
 
+Packet *sp_brightness_grid_into_packet(BrightnessGrid */*notnull*/ grid,
+                                       size_t x,
+                                       size_t y);
+
 /**
  * Loads a [BrightnessGrid] with the specified dimensions from the provided data.
  *
diff --git a/src/bitmap.rs b/src/bitmap.rs
index f203395..71753a5 100644
--- a/src/bitmap.rs
+++ b/src/bitmap.rs
@@ -1,5 +1,5 @@
 use crate::byte_slice::ByteSlice;
-use crate::{heap_drop, heap_move, heap_move_nonnull, SPBitVec};
+use crate::{heap_drop, heap_move, heap_move_nonnull, heap_remove, SPBitVec};
 use servicepoint::{Bitmap, DataRef, Grid};
 use std::ptr::NonNull;
 
@@ -78,7 +78,7 @@ pub unsafe extern "C" fn sp_bitmap_from_bitvec(
     width: usize,
     bitvec: NonNull<SPBitVec>,
 ) -> *mut Bitmap {
-    let bitvec = unsafe { *Box::from_raw(bitvec.as_ptr()) };
+    let bitvec = unsafe { heap_remove(bitvec) };
     if let Ok(bitmap) = Bitmap::from_bitvec(width, bitvec.0) {
         heap_move(bitmap)
     } else {
@@ -196,6 +196,6 @@ pub unsafe extern "C" fn sp_bitmap_unsafe_data_ref(
 pub unsafe extern "C" fn sp_bitmap_into_bitvec(
     bitmap: NonNull<Bitmap>,
 ) -> NonNull<SPBitVec> {
-    let bitmap = unsafe { *Box::from_raw(bitmap.as_ptr()) };
+    let bitmap = unsafe { heap_remove(bitmap) };
     heap_move_nonnull(SPBitVec(bitmap.into()))
 }
diff --git a/src/brightness_grid.rs b/src/brightness_grid.rs
index dcde8cf..f85602a 100644
--- a/src/brightness_grid.rs
+++ b/src/brightness_grid.rs
@@ -1,5 +1,8 @@
-use crate::{heap_drop, heap_move, heap_move_nonnull, ByteSlice};
-use servicepoint::{Brightness, BrightnessGrid, ByteGrid, DataRef, Grid};
+use crate::{heap_drop, heap_move, heap_move_nonnull, heap_remove, ByteSlice};
+use servicepoint::{
+    Brightness, BrightnessGrid, BrightnessGridCommand, ByteGrid, DataRef, Grid,
+    Origin, Packet,
+};
 use std::mem::transmute;
 use std::ptr::NonNull;
 
@@ -167,5 +170,23 @@ pub unsafe extern "C" fn sp_brightness_grid_unsafe_data_ref(
     const _: () = assert!(size_of::<Brightness>() == 1);
 
     let data = unsafe { (*brightness_grid.as_ptr()).data_ref_mut() };
-    unsafe { ByteSlice::from_slice(transmute::<&mut [Brightness], &mut [u8]>(data)) }
+    unsafe {
+        ByteSlice::from_slice(transmute::<&mut [Brightness], &mut [u8]>(data))
+    }
+}
+
+#[no_mangle]
+pub unsafe extern "C" fn sp_brightness_grid_into_packet(
+    grid: NonNull<BrightnessGrid>,
+    x: usize,
+    y: usize,
+) -> *mut Packet {
+    let grid = unsafe { heap_remove(grid) };
+    match Packet::try_from(BrightnessGridCommand {
+        grid,
+        origin: Origin::new(x, y),
+    }) {
+        Ok(packet) => heap_move(packet),
+        Err(_) => std::ptr::null_mut(),
+    }
 }
diff --git a/src/connection.rs b/src/connection.rs
index eec1f43..93e0a21 100644
--- a/src/connection.rs
+++ b/src/connection.rs
@@ -1,4 +1,4 @@
-use crate::{heap_drop, heap_move};
+use crate::{heap_drop, heap_move, heap_remove};
 use servicepoint::{Connection, Header, Packet, TypedCommand, UdpConnection};
 use std::ffi::{c_char, CStr};
 use std::net::{Ipv4Addr, SocketAddrV4};
@@ -67,8 +67,8 @@ pub unsafe extern "C" fn sp_udp_send_packet(
     connection: NonNull<UdpConnection>,
     packet: NonNull<Packet>,
 ) -> bool {
-    let packet = unsafe { Box::from_raw(packet.as_ptr()) };
-    unsafe { connection.as_ref().send(*packet) }.is_ok()
+    let packet = unsafe { heap_remove(packet) };
+    unsafe { connection.as_ref().send(packet) }.is_ok()
 }
 
 /// Sends a [TypedCommand] to the display using the [UdpConnection].
@@ -87,7 +87,7 @@ pub unsafe extern "C" fn sp_udp_send_command(
     connection: NonNull<UdpConnection>,
     command: NonNull<TypedCommand>,
 ) -> bool {
-    let command = *unsafe { Box::from_raw(command.as_ptr()) };
+    let command = unsafe { heap_remove(command) };
     unsafe { connection.as_ref().send(command) }.is_ok()
 }
 
diff --git a/src/lib.rs b/src/lib.rs
index c434ab1..05fb009 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -61,5 +61,9 @@ pub(crate) fn heap_move_nonnull<T>(x: T) -> NonNull<T> {
 }
 
 pub(crate) unsafe fn heap_drop<T>(x: NonNull<T>) {
-    drop(unsafe { Box::from_raw(x.as_ptr()) });
+    drop(unsafe { heap_remove(x) });
+}
+
+pub(crate) unsafe fn heap_remove<T>(x: NonNull<T>) -> T {
+    unsafe { *Box::from_raw(x.as_ptr()) }
 }
diff --git a/src/packet.rs b/src/packet.rs
index 4a30b51..a891ee3 100644
--- a/src/packet.rs
+++ b/src/packet.rs
@@ -1,4 +1,4 @@
-use crate::{heap_drop, heap_move, heap_move_nonnull, ByteSlice};
+use crate::{heap_drop, heap_move, heap_move_nonnull, heap_remove, ByteSlice};
 use servicepoint::{CommandCode, Header, Packet, TypedCommand};
 use std::ptr::NonNull;
 
@@ -10,7 +10,7 @@ use std::ptr::NonNull;
 pub unsafe extern "C" fn sp_packet_from_command(
     command: NonNull<TypedCommand>,
 ) -> *mut Packet {
-    let command = unsafe { *Box::from_raw(command.as_ptr()) };
+    let command = unsafe { heap_remove(command) };
     if let Ok(packet) = command.try_into() {
         heap_move(packet)
     } else {