From d357f4738a671f05fb815f48e9bd435e741de7bb Mon Sep 17 00:00:00 2001
From: doinkythederp <doinkythederp@icloud.com>
Date: Mon, 16 Dec 2024 13:46:00 -0800
Subject: [PATCH] feat: implement custom hydrozoa::panic call

---
 Cargo.toml                       |  4 +-
 packages/runtime/src/platform.rs | 70 ++++++++++++++++++++++++++++++++
 packages/runtime/src/sdk.rs      | 26 +++++++++++-
 packages/startup/src/lib.rs      |  2 +-
 4 files changed, 97 insertions(+), 5 deletions(-)

diff --git a/Cargo.toml b/Cargo.toml
index 7c11fd5..13f68a9 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -19,6 +19,6 @@ lto = true
 vex-sdk = "0.23.0"
 
 [workspace.dependencies.vexide]
-version = "0.5.0-rc.2"
+version = "0.5.1"
 default-features = false
-features = ["core", "devices", "panic"]
+features = ["core", "devices", "panic", "display_panics"]
diff --git a/packages/runtime/src/platform.rs b/packages/runtime/src/platform.rs
index 58b210e..46bc2e6 100644
--- a/packages/runtime/src/platform.rs
+++ b/packages/runtime/src/platform.rs
@@ -1,5 +1,14 @@
+use alloc::string::String;
 use core::ptr;
 
+use vexide::{
+    devices::{
+        display::{Font, FontFamily, FontSize, Rect, RenderMode, Text},
+        math::Point2,
+    },
+    prelude::*,
+};
+
 const LINKED_FILE: *const u32 = 0x7800000 as *const u32;
 
 pub fn read_user_program() -> &'static [u8] {
@@ -17,3 +26,64 @@ pub fn flush_serial() {
         }
     }
 }
+
+pub fn draw_error(display: &mut Display, msg: &str) {
+    const ERROR_BOX_MARGIN: i16 = 8;
+    const ERROR_BOX_PADDING: i16 = 8;
+    const LINE_HEIGHT: i16 = 16;
+    const LINE_MAX_WIDTH: usize = 56;
+
+    fn draw_text(screen: &mut Display, buffer: &str, line: i16) {
+        screen.draw_text(
+            &Text::new(
+                buffer,
+                Font::new(FontSize::SMALL, FontFamily::Monospace),
+                Point2 {
+                    x: ERROR_BOX_MARGIN + ERROR_BOX_PADDING,
+                    y: ERROR_BOX_MARGIN + ERROR_BOX_PADDING + (line * LINE_HEIGHT),
+                },
+            ),
+            (255, 255, 255),
+            Some(Rgb::new(0, 0, 0)),
+        );
+    }
+
+    display.set_render_mode(RenderMode::Immediate);
+
+    let error_box_rect = Rect::new(
+        Point2 {
+            x: ERROR_BOX_MARGIN,
+            y: ERROR_BOX_MARGIN,
+        },
+        Point2 {
+            x: Display::HORIZONTAL_RESOLUTION - ERROR_BOX_MARGIN,
+            y: Display::VERTICAL_RESOLUTION - ERROR_BOX_MARGIN,
+        },
+    );
+
+    display.fill(&error_box_rect, (255, 0, 0));
+    display.stroke(&error_box_rect, (255, 255, 255));
+
+    let mut buffer = String::new();
+    let mut line: i16 = 0;
+
+    for (i, character) in msg.char_indices() {
+        if character == '\t' {
+            buffer += "    ";
+        } else if !character.is_ascii_control() {
+            buffer.push(character);
+        }
+
+        if character == '\n' || ((buffer.len() % LINE_MAX_WIDTH == 0) && (i > 0)) {
+            draw_text(display, &buffer, line);
+            line += 1;
+            buffer.clear();
+        }
+    }
+
+    if !buffer.is_empty() {
+        draw_text(display, &buffer, line);
+
+        line += 1;
+    }
+}
diff --git a/packages/runtime/src/sdk.rs b/packages/runtime/src/sdk.rs
index 40841c9..35c42ee 100644
--- a/packages/runtime/src/sdk.rs
+++ b/packages/runtime/src/sdk.rs
@@ -1,14 +1,16 @@
 #![allow(non_snake_case)]
 
+use alloc::{borrow::ToOwned, string::ToString};
 use core::ffi::c_double;
 
 use vex_sdk::{
     V5MotorBrakeMode, V5MotorControlMode, V5MotorEncoderUnits, V5MotorGearset, V5_ControllerId,
     V5_ControllerIndex, V5_DeviceType,
 };
-use wasm3::{store::AsContextMut, Instance, Store};
+use vexide::{core::println, prelude::Display};
+use wasm3::{error::Trap, store::AsContextMut, Instance, Store};
 
-use crate::{teavm::get_cstring, Data};
+use crate::{platform::draw_error, teavm::get_cstring, Data};
 
 macro_rules! link {
     ($instance:ident, $store:ident, mod $module:literal {
@@ -67,6 +69,26 @@ macro_rules! printf_style {
 }
 
 pub fn link(store: &mut Store<Data>, instance: &mut Instance<Data>) -> anyhow::Result<()> {
+    instance.link_closure(
+        &mut *store,
+        "hydrozoa",
+        "panic",
+        |mut ctx, string: i32| -> Result<(), Trap> {
+            let string = get_cstring(&mut ctx, string);
+            let msg = string.to_string_lossy();
+
+            let mut display = unsafe { Display::new() };
+
+            draw_error(&mut display, msg.as_ref());
+
+            loop {
+                unsafe {
+                    vex_sdk::vexTasksRun();
+                }
+            }
+        },
+    )?;
+
     link!(instance, store, mod "vex" {
         // Display
         fn vexDisplayForegroundColor(col: u32);
diff --git a/packages/startup/src/lib.rs b/packages/startup/src/lib.rs
index 7f6ea67..df2374d 100644
--- a/packages/startup/src/lib.rs
+++ b/packages/startup/src/lib.rs
@@ -140,6 +140,6 @@ pub unsafe fn startup() {
         );
 
         // Initialize the heap allocator
-        vexide::core::allocator::vexos::init_heap();
+        vexide::core::allocator::init_heap();
     }
 }