/*
 * HP3478A internal USB-GPIB adapter & extension
 *
 * DESCRIPTION:
 * This firmware acts as a bridge between USB-CDC and GPIB while also working
 * in standalone mode inside HP3478A and MITM-ing GPIB and adding features.
 *
 * TODO:
 * - Implement a VTable for features, to replace the massive switch
 * to make consistnet entry and exits
 * - Break main() up into app_process_usb() and app_process_gpib()?
 * - Data logging?
 */
#include <ctype.h>
#include <float.h>
#include <math.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "aht20.h"
#include "ch32fun.h"
#include "config.h"
#include "fsusb.h"
#include "gpib_defs.h"
#include "i2c_bitbang.h"
#include "systick.h"

#define FW_VERSION "1.2.0"

#define PIN_VBUS PB10
#define PIN_BUZZ PC13

// USB
#define USB_HW_IS_ACTIVE() (!((USBFSCTX.USBFS_DevSleepStatus) & 0x02))
#define USB_RX_BUF_SIZE 1024
#define USB_RX_MASK (USB_RX_BUF_SIZE - 1)
#define USB_TX_BUF_SIZE 2048
#define USB_TX_MASK (USB_TX_BUF_SIZE - 1)

// HP3478A
#define HP_DISP_LEN 12  // 12 chars
// max str length required to fill 12 segments.
// Worst case: 12 chars + 12 dots/commas + 1 Null terminator = 25 bytes
#define HP_DISP_BUF_SIZE ((HP_DISP_LEN * 2) + 1)
#define DMM_OL_THRESHOLD 9.0e9  // HP sends +9.9999E+9 on overload
#define DMM_OL_NEG_THRESHOLD -9.0e9

#define CFG_ENTRY(member, dtype) {#member, offsetof(fw_config_t, member), dtype}

// Command system
typedef enum {
  CMD_UNKNOWN = 0,

  // Controller config
  CMD_VER,   // Get Firmware Version
  CMD_HELP,  // List commands
  CMD_CFG_SET,
  CMD_CFG_GET,
  CMD_CFG_LIST,
  CMD_SAVECFG,   // Save configuration
  CMD_RST,       // Reset the controller
  CMD_ADDR,      // Set GPIB primary address
  CMD_MODE,      // Set Controller vs Device mode (we're always the controller)
  CMD_TIMEOUT,   // Set read timeouts
  CMD_DMM_LOOP,  // Toggle extra HP3478A features

  // Data transport/formatting
  CMD_READ,        // Read data from bus
  CMD_WRITE,       // Write data to bus
  CMD_AUTO,        // Auto-read mode (Prologix header)
  CMD_EOI,         // Enables or disables the assertion of the EOI signal
  CMD_EOS,         // GPIB termination character
  CMD_EOR,         // End of receive
  CMD_EOT_ENABLE,  // the character to be appended to the USB output from the
                   // interface to the host
  CMD_EOT_CHAR,    // specific EOT char (e.g. \n or \r)

  // GPIB Bus Management
  CMD_TRG,    // Group Execute Trigger (GET)
  CMD_CLR,    // Selected Device Clear (SDC)
  CMD_DCL,    // Device Clear (Universal - clears all)
  CMD_SPOLL,  // Serial Poll
  CMD_SRQ,    // Service Request check
  CMD_STAT,   // Status check

  // Lower Level Bus Control
  CMD_LOC,  // Go To Local (Release remote control)
  CMD_GTL,  // Go To Local (GPIB command)
  CMD_LLO,  // Local Lockout (Disable front panel)
  CMD_REN,  // Remote Enable line
  CMD_IFC,  // Interface Clear (Bus Reset)

  // HP3478A features
  CMD_DISP,      // Write on display
  CMD_MATH,      // MIN/MAX/AVG
  CMD_ENV,       // Temp/Hum sensor
  CMD_CONT,      // Continuity mode
  CMD_TEMP,      // Temperature mode
  CMD_REL,       // Relative mode
  CMD_XOHM,      // Extended Ohm range
  CMD_DBM,       // dBm
  CMD_DIODE,     // Diode test
  CMD_AUTOHOLD,  // Autohold (latch) reading
  CMD_NORM       // Normal functionality

} cmd_id_t;

typedef struct {
  const char* name;
  cmd_id_t id;
} cmd_entry_t;

// Modes/config
typedef enum {
  AUTO_OFF = 0,
  AUTO_ON = 1,     // read after every write
  AUTO_QUERY = 2,  // read only if command ends in ?
  // AUTO_CONT = 3    // continuous read (triggered by ++read)
} auto_mode_t;

typedef enum {
  MODE_PASSTHROUGH = 0,  // Standard USB-GPIB bridge
  MODE_MENU,             // User is cycling options on DMM display
  MODE_FEAT_REL,         // Relative Mode active
  MODE_FEAT_AUTOHOLD,    // Autohold
  MODE_FEAT_CONT,        // Continuity Mode active
  MODE_FEAT_DIODE,       // Diode test mode
  MODE_FEAT_TEMP,        // PT1000 Temp Mode active
  MODE_FEAT_DBM,         // Power ratio using a 50R impedance as ref
  MODE_FEAT_XOHM,        // Extended Ohms active
  MODE_FEAT_STATS,       // AVG/MIN/MAX
} work_mode_t;

// Menu/UI
typedef enum {
  MENU_REL = 0,
  MENU_AUTOHOLD,
  MENU_CONT,
  MENU_DIODE,
  MENU_TEMP,
  MENU_DBM,
  MENU_XOHM,
  MENU_STATS,
  MENU_EXIT,
  MENU_MAX_ITEMS
} menu_item_t;

// Sub-menu states
typedef enum {
  SUBMENU_NONE = 0,
  SUBMENU_TEMP_SENS,  // step 1: sensor Type
  SUBMENU_TEMP_WIRE,  // step 2a: wire mode (skipped for Type K)
  SUBMENU_TEMP_NTC    // step 2b: NTC Value (Thermistor only)
} submenu_state_t;

// Logic

// Sensor Types
typedef enum {
  SENS_PT1000 = 0,
  SENS_THERMISTOR,
  SENS_TYPE_K,
  SENS_MAX_ITEMS
} temp_sensor_t;

typedef enum { WIRE_2W = 0, WIRE_4W, WIRE_MAX_ITEMS } wire_mode_t;

typedef enum {
  // Generic
  NTC_10K_3950 = 0,  // Generic china 3950
  NTC_10K_3435,      // alt 10k
  NTC_50K_3950,      // alt 50k
  NTC_100K_3950,     // alt 100k

  // YSI 44000
  NTC_YSI_2252,   // YSI 44004 (Mix B)
  NTC_YSI_3K,     // YSI 44005 (Mix B)
  NTC_YSI_5K,     // YSI 44007 (Mix B)
  NTC_YSI_10K_A,  // YSI 44006 (Mix H)
  NTC_YSI_10K_B,  // YSI 44016 (Mix B)
  NTC_YSI_30K,    // YSI 44008 (Mix H)
  NTC_YSI_100K,   // YSI 44011 (Mix H)
  NTC_YSI_1MEG,   // YSI 44015 (Mix H)

  NTC_MAX_ITEMS
} ntc_preset_t;

typedef struct {
  double r0;         // R @ 25C
  double beta;       // beta coefficient (typ. 3000-4500)
  const char* name;  // display name
} ntc_def_t;

// Logic states

// GPIB session
typedef enum { SESSION_WRITE, SESSION_READ } session_mode_t;

typedef enum {
  DIODE_STATE_OPEN = 0,  // probes open
  DIODE_STATE_CHECKING,  // voltage @ diode range, checking stability
  DIODE_STATE_SHORT,     // voltage too low, silent
  DIODE_STATE_DONE       // chirped, latched silent
} diode_state_t;

typedef struct {
  const char* cmd_func;
  const char* cmd_range;
  const char* cmd_digits;
  const char* cmd_az;
  char unit_str[5];  // "VDC", "OHM", etc.
} dmm_decoded_state_t;

// App state

typedef union {
  struct {
    double latched_val;    // val currently frozen on the LCD
    double candidate_val;  // val we are currently testing for stability
    uint8_t stable_count;  // how many counts has the candidate been stable for
    bool is_populated;     // has a val ever been latched?
    char unit[5];
  } autohold;

  struct {
    double offset;
    uint8_t stable_count;
    char unit[5];
  } rel;

  struct {
    double min;
    double max;
    double sum;
    uint32_t count;
    uint32_t disp_timer;
    uint8_t view_mode;
    char unit[5];
  } stats;

  struct {
    double r1;
    bool calibrated;
  } xohm;

  struct {
    diode_state_t connected;  // touchy?
    uint32_t chirp_start;     // when we began the touchy
  } diode;

  struct {
    // we're using very fast ADC mode for cont, so throttle disp update
    // although.. ideally this should be done globally somehow
    uint32_t last_disp_update;
  } cont;

  // menu state (only used when in MODE_MENU)
  struct {
    uint32_t timer;
    uint8_t layer;    // submenu_state_t
    uint8_t sub_pos;  // position in submenu
  } menu;

} mode_data_t;

typedef struct {
  // Hardware State Flags
  uint8_t usb_online : 1;
  uint8_t usb_raw_prev : 1;
  uint8_t env_sensor_present : 1;
  uint8_t dmm_online : 1;
  uint8_t has_saved_state : 1;
  uint8_t tone_timer_pending : 1;
  uint8_t dmm_loop : 1;
  uint8_t reserved : 1;

  uint32_t usb_timeout_cycles;  // calculated from sys_cfg.usb_timeout_target_ms

  // Timers
  uint32_t usb_ts;
  uint32_t env_last_read;
  uint32_t last_poll_time;
  uint32_t ignore_input_start_ts;
  uint32_t tone_start_ts;  // buzzer
  uint32_t tone_duration;

  // Logic
  work_mode_t current_mode;
  int menu_pos;  // high-level menu position (REL, TEMP etc.)

  // Config Selection
  temp_sensor_t temp_sensor;
  wire_mode_t temp_wire_mode;
  ntc_preset_t temp_ntc_preset;

  // Environmental Data
  aht20_data current_env;

  // DMM Restore
  uint8_t saved_state_bytes[5];

  // Display shadow buffer (cache)
  char last_disp_sent[HP_DISP_BUF_SIZE];

  // Unionized Mode Data
  mode_data_t data;
} app_state_t;

typedef union {
  // command processing
  struct {
    char line_buf[128];
    char fmt_buf[128];
  } cmd;

  // general io ctx
  struct {
    // for gpib
    char raw_data[256];
  } io;

  // display ctx
  struct {
    char full_cmd[64];
  } disp;

  uint8_t raw[256];
} app_scratchpad_t;

// Consts/LUTs

static const cmd_entry_t COMMAND_TABLE[] = {
    // Controller config
    {"ver", CMD_VER},
    {"help", CMD_HELP},
    {"?", CMD_HELP},
    {"savecfg", CMD_SAVECFG},
    {"config_set", CMD_CFG_SET},  // ++config_set name value
    {"set", CMD_CFG_SET},         // Alias: ++set name value
    {"config_get", CMD_CFG_GET},  // ++config_get name
    {"get", CMD_CFG_GET},         // Alias: ++get name
    {"config", CMD_CFG_LIST},     // ++config
    {"rst", CMD_RST},
    {"addr", CMD_ADDR},    // Set target GPIB address
    {"mode", CMD_MODE},    // 0=Device, 1=Controller
    {"tmo", CMD_TIMEOUT},  // Set read timeout
    {"read_tmo_ms", CMD_TIMEOUT},
    {"dmm_loop", CMD_DMM_LOOP},  // Toggle extra feats

    // Protocol/formatting
    {"auto", CMD_AUTO},  // Auto-read data after write
    {"eoi", CMD_EOI},    // Enable/Disable EOI assert
    {"eos", CMD_EOS},
    {"eor", CMD_EOR},
    {"eot_enable", CMD_EOT_ENABLE},
    {"eot_char", CMD_EOT_CHAR},

    // Common
    {"read", CMD_READ},
    {"write", CMD_WRITE},
    {"trg", CMD_TRG},    // Trigger instrument (GET)
    {"clr", CMD_CLR},    // Clear device buffer (SDC)
    {"stat", CMD_STAT},  // Controller Status

    // GPIB Bus Lines/States
    {"dcl", CMD_DCL},      // "Device Clear" (Resets state of ALL devices)
    {"spoll", CMD_SPOLL},  // "Serial Poll" (Read status byte)
    {"srq", CMD_SRQ},      // Check "Service Request" line status
    {"loc", CMD_LOC},      // Return to local (front panel) control
    {"gtl", CMD_GTL},      // "Go To Local" message
    {"llo", CMD_LLO},      // "Local Lockout" (Disables front panel buttons)
    {"ren", CMD_REN},      // "Remote Enable"
    {"ifc", CMD_IFC},      // "Interface Clear" (Hard bus reset)

    // HP3478A
    {"cont", CMD_CONT},      // Continuity Test
    {"diode", CMD_DIODE},    // Diode Test
    {"xohm", CMD_XOHM},      // Extended ohm
    {"dbm", CMD_DBM},        // dBm
    {"temp", CMD_TEMP},      // Temperature
    {"rel", CMD_REL},        // Relative (Delta) measurement
    {"math", CMD_MATH},      // MIN/MAX/AVG
    {"disp", CMD_DISP},      // Set Display Text
    {"env", CMD_ENV},        // Temp/Hum sensor
    {"hold", CMD_AUTOHOLD},  // Autohold mode
    {"norm", CMD_NORM},      // Reset to Normal/DC Volts

    {NULL, CMD_UNKNOWN}};

// UI Strings
static const char* MENU_NAMES[] = {"REL", "AUTOHOLD", "CONT",  "DIODE", "TEMP",
                                   "DBM", "XOHM",     "STATS", "EXIT"};

static const char* SENSOR_NAMES[] = {"PT1000", "THERM", "TYPE K"};

static const char* WIRE_NAMES[] = {"2-WIRE", "4-WIRE"};

// some common NTC defs
static const ntc_def_t NTC_DEFS[] = {
    // Generic/China
    {10000.0, 3950.0, "10K 3950"},    // black bead)
    {10000.0, 3435.0, "10K 3435"},    // euro?
    {50000.0, 3950.0, "50K 3950"},    // generic
    {100000.0, 3950.0, "100K 3950"},  // generic
    // From YSI datasheet
    {2252.0, 3891.0, "2.252K YSI"},  // 44004
    {3000.0, 3891.0, "3K YSI"},      // 44005
    {5000.0, 3891.0, "5K YSI"},      // 44007
    {10000.0, 3574.0, "10K YSI A"},  // Mix H (YSI 10k)
    {10000.0, 3891.0, "10K YSI B"},  // Mix B (matches 2.2K curve)
    {30000.0, 3810.0, "30K YSI"},    // 44008
    {100000.0, 3988.0, "100K YSI"},  // 44011
    {1000000.0, 4582.0, "1MEG YSI"}  // 44015
};

// buzz
static const uint32_t ONLINE_NOTES[] = {3100, 80, 4200, 120};
// static const uint32_t OFFLINE_NOTES[] = {4200, 80, 3100, 120};

static const cfg_field_t CONFIG_MAP[] = {
    // addressing
    CFG_ENTRY(my_addr, CFG_TYPE_UINT8),
    CFG_ENTRY(dmm_addr, CFG_TYPE_UINT8),
    CFG_ENTRY(target_addr, CFG_TYPE_UINT8),

    // protocol
    CFG_ENTRY(eot_char, CFG_TYPE_UINT8),
    CFG_ENTRY(eot_enable, CFG_TYPE_UINT8),
    CFG_ENTRY(eoi_assert, CFG_TYPE_UINT8),
    CFG_ENTRY(eos_mode, CFG_TYPE_UINT8),
    CFG_ENTRY(eor_mode, CFG_TYPE_UINT8),
    CFG_ENTRY(auto_read, CFG_TYPE_UINT8),

    // timings (hw)
    CFG_ENTRY(gpib_timeout_ms, CFG_TYPE_UINT32),
    CFG_ENTRY(poll_interval_ms, CFG_TYPE_UINT32),
    CFG_ENTRY(env_sensor_read_interval_ms, CFG_TYPE_UINT32),
    CFG_ENTRY(dmm_recovery_delay_ms, CFG_TYPE_UINT32),
    CFG_ENTRY(usb_debounce_connect_ms, CFG_TYPE_UINT32),
    CFG_ENTRY(usb_debounce_disconnect_ms, CFG_TYPE_UINT32),
    CFG_ENTRY(usb_timeout_target_ms, CFG_TYPE_UINT32),

    // timings (ui)
    CFG_ENTRY(menu_dot_interval_ms, CFG_TYPE_UINT32),
    CFG_ENTRY(menu_commit_delay_ms, CFG_TYPE_UINT32),
    CFG_ENTRY(menu_sublayer_delay_ms, CFG_TYPE_UINT32),
    CFG_ENTRY(menu_debounce_ms, CFG_TYPE_UINT32),
    CFG_ENTRY(menu_lockout_ms, CFG_TYPE_UINT32),
    CFG_ENTRY(stats_cycle_time_ms, CFG_TYPE_UINT32),
    CFG_ENTRY(cont_disp_update_ms, CFG_TYPE_UINT32),
    CFG_ENTRY(diode_stable_ms, CFG_TYPE_UINT32),

    // buzzer
    CFG_ENTRY(buzzer_chirp_hz, CFG_TYPE_UINT32),
    CFG_ENTRY(buzzer_chirp_ms, CFG_TYPE_UINT32),
    CFG_ENTRY(buzzer_cont_hz, CFG_TYPE_UINT32),

    // math/cal
    CFG_ENTRY(rtd_a, CFG_TYPE_DOUBLE),
    CFG_ENTRY(rtd_b, CFG_TYPE_DOUBLE),
    CFG_ENTRY(rtd_r0, CFG_TYPE_DOUBLE),
    CFG_ENTRY(cjc_fallback_temp, CFG_TYPE_DOUBLE),
    CFG_ENTRY(cjc_self_heating_offset, CFG_TYPE_DOUBLE),
    CFG_ENTRY(type_k_scale, CFG_TYPE_DOUBLE),
    CFG_ENTRY(dbm_ref_z, CFG_TYPE_DOUBLE),
    CFG_ENTRY(diode_th_short, CFG_TYPE_DOUBLE),
    CFG_ENTRY(diode_th_open, CFG_TYPE_DOUBLE),

    // thresholds
    CFG_ENTRY(autohold_threshold, CFG_TYPE_DOUBLE),
    CFG_ENTRY(autohold_change_req, CFG_TYPE_DOUBLE),
    CFG_ENTRY(autohold_min_val, CFG_TYPE_DOUBLE),
    CFG_ENTRY(cont_threshold_ohms, CFG_TYPE_DOUBLE),

    // logic counts
    CFG_ENTRY(autohold_stable_count, CFG_TYPE_UINT32),
    CFG_ENTRY(rel_stable_count, CFG_TYPE_UINT32),
};

static const size_t CONFIG_MAP_SIZE = sizeof(CONFIG_MAP) / sizeof(cfg_field_t);

// Globals

// Flash config
__attribute__((section(".external"))) volatile fw_config_t flash_config = {
    .magic = CONFIG_MAGIC,
    .version = CONFIG_VERSION,

    .my_addr = MY_ADDR,
    .dmm_addr = DEFAULT_DMM_ADDR,
    .target_addr = DEFAULT_DMM_ADDR,

    // prologix std defaults
    .auto_read = 0,       // ++auto 0 (off)
    .eoi_assert = true,   // ++eoi 1 (assert EOI on write end)
    .eos_mode = 0,        // ++eos 0 (CR+LF appended to writes)
    .eor_mode = 0,        // ++eor 0 (terminates read on CR+LF)
    .eot_enable = false,  // ++eot_enable 0 (off)
    .eot_char = 0,        // ++eot_char 0

    .gpib_timeout_ms = DEFAULT_GPIB_TIMEOUT_MS,
    .poll_interval_ms = POLL_INTERVAL_MS,
    .env_sensor_read_interval_ms = ENV_SENSOR_READ_INTERVAL_MS,
    .dmm_recovery_delay_ms = DMM_RECOVERY_DELAY_MS,
    .usb_debounce_connect_ms = USB_DEBOUNCE_CONNECT_MS,
    .usb_debounce_disconnect_ms = USB_DEBOUNCE_DISCONNECT_MS,
    .usb_timeout_target_ms = USB_TIMEOUT_TARGET_MS,

    .menu_dot_interval_ms = MENU_DOT_INTERVAL_MS,
    .menu_commit_delay_ms = MENU_COMMIT_DELAY_MS,
    .menu_sublayer_delay_ms = MENU_SUBLAYER_DELAY_MS,
    .menu_debounce_ms = MENU_DEBOUNCE_MS,
    .menu_lockout_ms = MENU_LOCKOUT_MS,
    .stats_cycle_time_ms = STATS_CYCLE_TIME_MS,
    .cont_disp_update_ms = CONT_DISP_UPDATE_MS,

    // Physics
    .rtd_a = RTD_A,
    .rtd_b = RTD_B,
    .rtd_r0 = RTD_R0,
    .cjc_fallback_temp = CJC_FALLBACK_TEMP,
    .cjc_self_heating_offset = CJC_SELF_HEATING_OFFSET,
    .type_k_scale = TYPE_K_SCALE,
    .dbm_ref_z = DBM_REF_Z,

    // Thresholds
    .diode_th_short = DIODE_TH_SHORT,
    .diode_th_open = DIODE_TH_OPEN,
    .diode_stable_ms = DIODE_STABLE_MS,

    // Buzzer
    .buzzer_chirp_hz = BUZZER_CHIRP_HZ,
    .buzzer_chirp_ms = BUZZER_CHIRP_MS,
    .buzzer_cont_hz = BUZZER_CONT_HZ,

    .autohold_threshold = AUTOHOLD_THRESHOLD,
    .autohold_change_req = AUTOHOLD_CHANGE_REQ,
    .autohold_min_val = AUTOHOLD_MIN_VAL,
    .cont_threshold_ohms = CONT_THRESHOLD_OHMS,

    .autohold_stable_count = AUTOHOLD_STABLE_COUNT,
    .rel_stable_count = REL_STABLE_COUNT};

static fw_config_t sys_cfg;

// App state
static app_state_t app = {.current_mode = MODE_PASSTHROUGH,

                          .usb_online = false,
                          .dmm_online = false,
                          .has_saved_state = false,
                          .tone_timer_pending = false};

static app_scratchpad_t scratch;

// USB
volatile uint8_t usb_rx_buffer[USB_RX_BUF_SIZE];
volatile uint32_t usb_rx_head = 0;
volatile uint32_t usb_rx_tail = 0;

volatile uint8_t usb_tx_buffer[USB_TX_BUF_SIZE];
volatile uint32_t usb_tx_head = 0;
volatile uint32_t usb_tx_tail = 0;

static uint8_t cdc_line_coding[7] = {0x00, 0xC2, 0x01, 0x00, 0x00, 0x00, 0x08};
extern volatile uint8_t usb_debug;  // DEBUG

// Audio state
volatile bool is_buzzer_pulsing = false;
static uint32_t current_buzz_freq = 0;

// LUT for GPIB writes
static uint32_t gpib_write_lut[256];

// systick
volatile uint32_t systick_millis;

static void systick_init(void) {
  SysTick->CTLR = 0x0000;
  SysTick->CMP = SysTick->CNT + SYSTICK_ONE_MILLISECOND;
  systick_millis = 0;
  SysTick->CTLR = SYSTICK_CTLR_STE |   // Enable Counter
                  SYSTICK_CTLR_STIE |  // Enable Interrupts
                  SYSTICK_CTLR_STCLK;  // Set Clock Source to HCLK/1

  NVIC_EnableIRQ(SysTick_IRQn);
}

void SysTick_Handler(void) __attribute__((interrupt));
void SysTick_Handler(void) {
  SysTick->CMP = SysTick->CNT + SYSTICK_ONE_MILLISECOND;
  SysTick->SR = 0;
  systick_millis++;
}

// Config

static const cfg_field_t* find_config_field(const char* name) {
  for (size_t i = 0; i < CONFIG_MAP_SIZE; i++) {
    if (strcasecmp(CONFIG_MAP[i].name, name) == 0) {
      return &CONFIG_MAP[i];
    }
  }
  return NULL;
}

inline static void config_apply_to_app(void) {
  app.usb_timeout_cycles =
      ((FUNCONF_SYSTEM_CORE_CLOCK / 1000 * sys_cfg.usb_timeout_target_ms) /
       CYCLES_PER_LOOP);
  app.dmm_loop = 1;  // again, preferably read from config if someone wants this
                     // to be persistent
}

static void config_reset_defaults(void) {
  sys_cfg.magic = CONFIG_MAGIC;
  sys_cfg.version = CONFIG_VERSION;

  sys_cfg.my_addr = MY_ADDR;
  sys_cfg.dmm_addr = DEFAULT_DMM_ADDR;
  sys_cfg.target_addr = DEFAULT_DMM_ADDR;

  sys_cfg.auto_read = 0;       // ++auto 0 (off)
  sys_cfg.eoi_assert = true;   // ++eoi 1 (assert EOI on write end)
  sys_cfg.eos_mode = 0;        // ++eos 0 (CR+LF appended to writes)
  sys_cfg.eor_mode = 0;        // ++eor 0 (terminates read on CR+LF)
  sys_cfg.eot_enable = false;  // ++eot_enable 0 (off)
  sys_cfg.eot_char = 0;        // ++eot_char 0

  sys_cfg.gpib_timeout_ms = DEFAULT_GPIB_TIMEOUT_MS;
  sys_cfg.poll_interval_ms = POLL_INTERVAL_MS;
  sys_cfg.env_sensor_read_interval_ms = ENV_SENSOR_READ_INTERVAL_MS;
  sys_cfg.dmm_recovery_delay_ms = DMM_RECOVERY_DELAY_MS;
  sys_cfg.usb_debounce_connect_ms = USB_DEBOUNCE_CONNECT_MS;
  sys_cfg.usb_debounce_disconnect_ms = USB_DEBOUNCE_DISCONNECT_MS;
  sys_cfg.usb_timeout_target_ms = USB_TIMEOUT_TARGET_MS;

  sys_cfg.menu_dot_interval_ms = MENU_DOT_INTERVAL_MS;
  sys_cfg.menu_commit_delay_ms = MENU_COMMIT_DELAY_MS;
  sys_cfg.menu_sublayer_delay_ms = MENU_SUBLAYER_DELAY_MS;
  sys_cfg.menu_debounce_ms = MENU_DEBOUNCE_MS;
  sys_cfg.menu_lockout_ms = MENU_LOCKOUT_MS;
  sys_cfg.stats_cycle_time_ms = STATS_CYCLE_TIME_MS;
  sys_cfg.cont_disp_update_ms = CONT_DISP_UPDATE_MS;
  // Physics
  sys_cfg.rtd_a = RTD_A;
  sys_cfg.rtd_b = RTD_B;
  sys_cfg.rtd_r0 = RTD_R0;
  sys_cfg.cjc_fallback_temp = CJC_FALLBACK_TEMP;
  sys_cfg.cjc_self_heating_offset = CJC_SELF_HEATING_OFFSET;
  sys_cfg.type_k_scale = TYPE_K_SCALE;
  sys_cfg.dbm_ref_z = DBM_REF_Z;
  // Thresholds
  sys_cfg.diode_th_short = DIODE_TH_SHORT;
  sys_cfg.diode_th_open = DIODE_TH_OPEN;
  sys_cfg.diode_stable_ms = DIODE_STABLE_MS;

  sys_cfg.autohold_threshold = AUTOHOLD_THRESHOLD;
  sys_cfg.autohold_change_req = AUTOHOLD_CHANGE_REQ;
  sys_cfg.autohold_min_val = AUTOHOLD_MIN_VAL;
  sys_cfg.cont_threshold_ohms = CONT_THRESHOLD_OHMS;

  // Buzzer
  sys_cfg.buzzer_chirp_hz = BUZZER_CHIRP_HZ;
  sys_cfg.buzzer_chirp_ms = BUZZER_CHIRP_MS;
  sys_cfg.buzzer_cont_hz = BUZZER_CONT_HZ;

  sys_cfg.autohold_stable_count = AUTOHOLD_STABLE_COUNT;
  sys_cfg.rel_stable_count = REL_STABLE_COUNT;
}

static void config_save(void) {
  uint16_t* source_ptr = (uint16_t*)&sys_cfg;
  uint32_t start_addr = (uint32_t)&flash_config;

  int total_bytes = sizeof(fw_config_t);
  int total_halfwords = (total_bytes + 1) / 2;
  int pages_to_erase = (total_bytes + ERASE_PAGE_SIZE - 1) / ERASE_PAGE_SIZE;

  printf("config: saving %d bytes to %08lx\n", total_bytes, start_addr);

  // unlock flash
  if (FLASH->CTLR & CR_LOCK_Set) {
    FLASH->KEYR = FLASH_KEY1;
    FLASH->KEYR = FLASH_KEY2;
  }

  // erase loop
  for (int p = 0; p < pages_to_erase; p++) {
    uint32_t page_addr = start_addr + (p * ERASE_PAGE_SIZE);

    FLASH->CTLR &= ~CR_PAGE_ER;  // clear
    FLASH->CTLR |= CR_PAGE_ER;   // set page erase
    FLASH->ADDR = page_addr;
    FLASH->CTLR |= CR_STRT_Set;  // start erase

    while (FLASH->STATR & SR_BSY);  // wait

    if (FLASH->STATR & SR_WRPRTERR) {
      printf("config: erase error (WPR)\n");
      FLASH->CTLR |= CR_LOCK_Set;
      return;
    }
    FLASH->CTLR &= ~CR_PAGE_ER;  // clear erase flag
  }

  // slower than fast program but we rarely do this, so meh
  FLASH->CTLR |= CR_PG_Set;

  for (int i = 0; i < total_halfwords; i++) {
    uint32_t write_addr = start_addr + (i * 2);

    *(__IO uint16_t*)write_addr = source_ptr[i];

    while (FLASH->STATR & SR_BSY);  // wait

    if (FLASH->STATR & SR_WRPRTERR || FLASH->STATR & FLASH_STATR_PGERR) {
      printf("config: write error @ %08lx\n", write_addr);
      break;
    }
  }

  FLASH->CTLR &= ~CR_PG_Set;   // disable PG
  FLASH->CTLR |= CR_LOCK_Set;  // lock

  printf("config: saved\n");
}

static void config_init(void) {
  const fw_config_t* src = (const fw_config_t*)&flash_config;

  if (src->magic == CONFIG_MAGIC) {
    memcpy(&sys_cfg, src, sizeof(fw_config_t));
    printf("config: loaded from flash\n");
  } else {
    printf("config: flash invalid/blank, loading defaults\n");
    config_reset_defaults();
    // autosave defaults?
    // config_save();
  }
}

// helpers
static bool starts_with_nocase(const char* str, const char* prefix) {
  while (*prefix) {
    if (tolower((unsigned char)*str) != tolower((unsigned char)*prefix)) {
      return false;
    }
    str++;
    prefix++;
  }
  return true;
}

static char* skip_spaces(char* str) {
  while (*str && isspace((unsigned char)*str)) {
    str++;
  }
  return str;
}

static cmd_id_t parse_command_id(const char* cmd) {
  for (int i = 0; COMMAND_TABLE[i].name != NULL; i++) {
    if (starts_with_nocase(cmd, COMMAND_TABLE[i].name)) {
      int len = strlen(COMMAND_TABLE[i].name);
      char next = cmd[len];
      if (next == 0 || isspace((unsigned char)next)) {
        return COMMAND_TABLE[i].id;
      }
    }
  }
  return CMD_UNKNOWN;
}

static void double_to_str(char* buf, size_t buf_size, double val, int prec) {
  if (buf_size == 0) return;
  buf[0] = '\0';
  size_t offset = 0;

  if (isnan(val)) {
    if (buf_size > 3) strcpy(buf, "NAN");
    return;
  }
  if (isinf(val)) {
    if (buf_size - offset > 3) strcpy(buf + offset, "INF");
    return;
  }
  if (signbit(val)) {
    if (offset < buf_size - 1) buf[offset++] = '-';
    val = -val;
  }

  if (val > (double)UINT32_MAX) {
    if (buf_size > 3) strcpy(buf, "OVFL");  // overflow
    return;
  }

  if (prec < 0) prec = 0;
  if (prec > 9) prec = 9;  // limit precision

  // calc multiplier
  uint32_t multiplier = 1;
  for (int i = 0; i < prec; i++) multiplier *= 10;

  uint32_t int_part = (uint32_t)val;
  // fractional component
  double remainder = val - (double)int_part;

  // scale and round the remainder
  remainder = (remainder * (double)multiplier) + 0.5;
  uint32_t frac_part = (uint32_t)remainder;

  // handle rounding rollover
  if (frac_part >= multiplier) {
    frac_part = 0;
    int_part++;
    // if int_part overflows here it wraps to 0
  }

  // print integer part
  int res = snprintf(buf + offset, buf_size - offset, "%lu", int_part);

  if (res < 0 || (size_t)res >= buf_size - offset) {
    buf[buf_size - 1] = '\0';
    return;
  }
  offset += (size_t)res;

  // print decimal point and fraction
  if (prec > 0 && offset < buf_size - 1) {
    buf[offset++] = '.';

    uint64_t divider = multiplier / 10;

    while (divider > 0 && offset < buf_size - 1) {
      uint32_t digit = frac_part / divider;
      buf[offset++] = (char)('0' + digit);

      frac_part %= divider;
      divider /= 10;
    }
  }

  buf[offset] = '\0';
}

// "Note that period, comma, and semicolon go between characters"
static int count_non_visual_chars(const char* s) {
  int c = 0;
  while (*s) {
    if (*s == '.' || *s == ',' || *s == ';') c++;
    s++;
  }
  return c;
}

static void format_metric_value(char* buffer, size_t buf_len, double val,
                                const char* unit, int auto_scale) {
  // must hold 12 visible chars + 1 null.
  if (buf_len <= HP_DISP_LEN) return;

  memset(buffer, ' ', HP_DISP_LEN);
  buffer[HP_DISP_LEN] = '\0';

  // scale
  double scaled = val;
  double abs_val = fabs(val);
  char suffix = 0;

  if (auto_scale) {
    if (abs_val >= 1.0e9) {
      scaled *= 1.0e-9;
      suffix = 'G';
    } else if (abs_val >= 1.0e6) {
      scaled *= 1.0e-6;
      suffix = 'M';
    } else if (abs_val >= 1.0e3) {
      scaled *= 1.0e-3;
      suffix = 'K';
    }
  }
  double abs_s = fabs(scaled);

  // unit + suffix
  size_t unit_len = strlen(unit);
  size_t suffix_len = (suffix != 0) ? 1 : 0;
  size_t meta_vis_len = unit_len + suffix_len;

  if (meta_vis_len > (HP_DISP_LEN - 3)) {
    // truncate unit?
  }

  int int_digits = 1;
  if (abs_s >= 1.0) {
    double t = abs_s;
    while (t >= 10.0) {
      t /= 10.0;
      int_digits++;
    }
  }

  bool is_neg = (scaled < 0.0);
  // visual slots needed for non-decimal part (sign + ints + space separator)
  int separator = (meta_vis_len > 0) ? 1 : 0;
  int reserved_vis = is_neg + int_digits + separator + meta_vis_len;

  // calculate allowed decimals (visual)
  int allowed_prec = HP_DISP_LEN - reserved_vis;
  if (allowed_prec < 0) allowed_prec = 0;

  int desired_prec = 5;
  if (abs_s >= 10000.0)
    desired_prec = 1;
  else if (abs_s >= 1000.0)
    desired_prec = 2;
  else if (abs_s >= 100.0)
    desired_prec = 3;
  else if (abs_s >= 10.0)
    desired_prec = 4;

  int final_prec = (desired_prec < allowed_prec) ? desired_prec : allowed_prec;

  // render num
  char num_buf[32];
  double_to_str(num_buf, sizeof(num_buf), scaled, final_prec);

  // calc padding
  int num_bytes = strlen(num_buf);
  int num_dots = count_non_visual_chars(num_buf);
  int num_vis_len = num_bytes - num_dots;

  // total visual slots used so far
  int total_vis_used = num_vis_len + meta_vis_len;

  // to push unit to the right edge
  int spaces_needed = HP_DISP_LEN - total_vis_used;
  if (spaces_needed < 1 && meta_vis_len > 0)
    spaces_needed = 1;  // enforce 1 space if possible?
  if (total_vis_used + spaces_needed > HP_DISP_LEN) {
    // crunch: if overflow (very large number), reduce spaces
    spaces_needed = HP_DISP_LEN - total_vis_used;
    if (spaces_needed < 0) spaces_needed = 0;
  }

  // [number] [spaces] [suffix] [unit]
  // num
  strcpy(buffer, num_buf);
  // spaces
  int current_len = strlen(buffer);
  for (int i = 0; i < spaces_needed; i++) {
    buffer[current_len++] = ' ';
  }
  buffer[current_len] = '\0';
  // suffix
  if (suffix) {
    buffer[current_len++] = suffix;
    buffer[current_len] = '\0';
  }
  // unit
  if (unit_len > 0) {
    strcat(buffer, unit);
  }
}

static double parse_double(const char* s) {
  double mantissa = 0.0;
  int exponent = 0;
  int sign = 1;
  bool decimal_seen = false;
  int decimal_counts = 0;

  // skip whitespace
  while (*s == ' ') s++;

  // handle sign
  if (*s == '+') {
    s++;
  } else if (*s == '-') {
    sign = -1;
    s++;
  }

  // parse Mantissa as pure int
  while (*s) {
    if (*s >= '0' && *s <= '9') {
      mantissa = (mantissa * 10.0) + (*s - '0');
      if (decimal_seen) {
        decimal_counts++;
      }
    } else if (*s == '.') {
      decimal_seen = true;
    } else if (*s == 'E' || *s == 'e') {
      s++;
      exponent = atoi(s);
      break;
    } else {
      break;
    }
    s++;
  }

  exponent -= decimal_counts;

  double power = 1.0;
  int e_abs = abs(exponent);
  while (e_abs-- > 0) power *= 10.0;

  if (exponent > 0) {
    mantissa *= power;
  } else {
    mantissa /= power;
  }

  return mantissa * sign;
}

static void decode_dmm_state_bytes(const uint8_t* state_bytes,
                                   dmm_decoded_state_t* out) {
  uint8_t b1 = state_bytes[0];
  uint8_t b2 = state_bytes[1];

  // decode function & unit
  switch ((b1 >> 5) & 0x07) {
    case 2:
      out->cmd_func = HP3478A_FUNC_AC_VOLTS;
      strcpy(out->unit_str, "VAC");
      break;
    case 3:
      out->cmd_func = HP3478A_FUNC_OHMS_2WIRE;
      strcpy(out->unit_str, "OHM");
      break;
    case 4:
      out->cmd_func = HP3478A_FUNC_OHMS_4WIRE;
      strcpy(out->unit_str, "OHM");
      break;
    case 5:
      out->cmd_func = HP3478A_FUNC_DC_CURRENT;
      strcpy(out->unit_str, "ADC");
      break;
    case 6:
      out->cmd_func = HP3478A_FUNC_AC_CURRENT;
      strcpy(out->unit_str, "AAC");
      break;
    case 7:
      out->cmd_func = HP3478A_FUNC_OHMS_EXT;
      strcpy(out->unit_str, "OHM");
      break;
    default:  // case 1 or anything else
      out->cmd_func = HP3478A_FUNC_DC_VOLTS;
      strcpy(out->unit_str, "VDC");
      break;
  }

  // decode range (bits 4-2) + autorange (byte 2 bit 1)
  if (b2 & 0x02) {
    out->cmd_range = HP3478A_RANGE_AUTO;
  } else {
    switch ((b1 >> 2) & 0x07) {
      case 1:
        out->cmd_range = HP3478A_RANGE_NEG_2;
        break;
      case 2:
        out->cmd_range = HP3478A_RANGE_NEG_1;
        break;
      case 3:
        out->cmd_range = HP3478A_RANGE_0;
        break;
      case 4:
        out->cmd_range = HP3478A_RANGE_1;
        break;
      case 5:
        out->cmd_range = HP3478A_RANGE_2;
        break;
      case 6:
        out->cmd_range = HP3478A_RANGE_3;
        break;
      case 7:
        out->cmd_range = HP3478A_RANGE_4;
        break;
      default:
        out->cmd_range = HP3478A_RANGE_0;
        break;
    }
  }

  // decode digits (bits 1-0)
  switch (b1 & 0x03) {
    case 2:
      out->cmd_digits = HP3478A_DIGITS_4_5;
      break;
    case 3:
      out->cmd_digits = HP3478A_DIGITS_3_5;
      break;
    default:
      out->cmd_digits = HP3478A_DIGITS_5_5;
      break;
  }

  // decode autozero (byte 2 bit 2)
  out->cmd_az = (b2 & 0x04) ? HP3478A_AUTOZERO_ON : HP3478A_AUTOZERO_OFF;
}

// constructs the base configuration string (F R N Z) from decoded state
static void build_restoration_string(char* buffer,
                                     const dmm_decoded_state_t* state) {
  strcpy(buffer, HP3478A_DISP_NORMAL);
  strcat(buffer, state->cmd_func);
  strcat(buffer, state->cmd_range);
  strcat(buffer, state->cmd_digits);
  strcat(buffer, state->cmd_az);
}

static bool is_query(const char* cmd) {
  if (!cmd || !*cmd) return false;
  size_t len = strlen(cmd);
  while (len > 0 && isspace((unsigned char)cmd[len - 1])) {
    len--;
  }
  if (len > 0 && cmd[len - 1] == '?') return true;
  return false;
}

#ifdef GPIB_DEBUG
static void gpib_dump_state(const char* context) {
  uint8_t d = 0;
  if (!GPIB_READ(PIN_DIO1)) d |= 0x01;
  if (!GPIB_READ(PIN_DIO2)) d |= 0x02;
  if (!GPIB_READ(PIN_DIO3)) d |= 0x04;
  if (!GPIB_READ(PIN_DIO4)) d |= 0x08;
  if (!GPIB_READ(PIN_DIO5)) d |= 0x10;
  if (!GPIB_READ(PIN_DIO6)) d |= 0x20;
  if (!GPIB_READ(PIN_DIO7)) d |= 0x40;
  if (!GPIB_READ(PIN_DIO8)) d |= 0x80;

  printf("\n[GPIB DUMP] %s\n", context);
  printf("  M: ATN=%d IFC=%d REN=%d EOI=%d | S: SRQ=%d\n", GPIB_READ(PIN_ATN),
         GPIB_READ(PIN_IFC), GPIB_READ(PIN_REN), GPIB_READ(PIN_EOI),
         GPIB_READ(PIN_SRQ));
  printf("  H: DAV=%d NRFD=%d NDAC=%d\n", GPIB_READ(PIN_DAV),
         GPIB_READ(PIN_NRFD), GPIB_READ(PIN_NDAC));
  printf("  D: 0x%02X\n", d);
}
#else
#define gpib_dump_state(x) ((void)0)
#endif

inline static void gpib_write_data(uint8_t b) {
  GPIOB->BSHR = gpib_write_lut[b];
}

inline static uint8_t gpib_read_data(void) {
  // read all 16 pins, invert (gpib is active low)
  uint32_t r = ~(GPIOB->INDR);
  uint8_t b = 0;

  // parallel extraction
  b |= (r >> SHIFT_GRP_9) & MASK_GRP_9;  // handles D1 & D6
  b |= (r >> SHIFT_GRP_7) & MASK_GRP_7;  // handles D2 & D7
  b |= (r >> SHIFT_D3) & (1 << 2);
  b |= (r >> SHIFT_D4) & (1 << 3);
  b |= (r >> SHIFT_D5) & (1 << 4);
  b |= (r >> SHIFT_D8) & (1 << 7);

  return b;
}

inline static int gpib_wait_pin(int pin, int expected_state) {
  uint32_t start = millis();

  while (GPIB_READ(pin) != expected_state) {
    if ((millis() - start) > sys_cfg.gpib_timeout_ms) {
#ifdef GPIB_DEBUG
      // Print which specific pin failed
      char* pin_name = "UNKNOWN";
      if (pin == PIN_NRFD)
        pin_name = "NRFD";
      else if (pin == PIN_NDAC)
        pin_name = "NDAC";
      else if (pin == PIN_DAV)
        pin_name = "DAV";

      printf("[GPIB ERR] Timeout waiting for %s to be %d\n", pin_name,
             expected_state);
      gpib_dump_state("TIMEOUT STATE");
#endif
      return -1;
    }
  }
  return 0;
}

static int gpib_write_byte(uint8_t data, int assert_eoi) {
#ifdef GPIB_DEBUG
  // printf("[TX] 0x%02X (EOI=%d)... ", data, assert_eoi);
#endif

  // wait for listeners to be ready
  if (gpib_wait_pin(PIN_NRFD, 1) < 0) {
    return -1;
  }

  gpib_write_data(data);

  // assert EOI if this is the last byte
  if (assert_eoi) {
    GPIB_ASSERT(PIN_EOI);
  }

  Delay_Us(1);  // T1

  GPIB_ASSERT(PIN_DAV);

  // wait for listeners ack
  if (gpib_wait_pin(PIN_NDAC, 1) < 0) {
    GPIB_RELEASE(PIN_DAV);
    GPIB_RELEASE(PIN_EOI);
#ifdef GPIB_DEBUG
    printf("NDAC stuck LOW, (device didn't accept)\n");
#endif
    return -2;
  }

  GPIB_RELEASE(PIN_DAV);
  GPIB_RELEASE(PIN_EOI);

  // float bus
  gpib_write_data(0x00);

  return 0;
}

static int gpib_read_byte(uint8_t* data, bool* eoi_asserted) {
  // sssert busy state
  GPIB_ASSERT(PIN_NDAC);  // not accepted yet
  GPIB_ASSERT(PIN_NRFD);  // not ready yet

  // float data lines
  gpib_write_data(0x00);

  // Delay_Us(10);

  // signal ready for data
  GPIB_RELEASE(PIN_NRFD);

  // wait for talker to assert DAV
  if (gpib_wait_pin(PIN_DAV, 0) < 0) {
    GPIB_RELEASE(PIN_NDAC);
    GPIB_RELEASE(PIN_NRFD);
#ifdef GPIB_DEBUG
    printf("[GPIB] Read timeout waiting for DAV Low (Talker not ready)\n");
#endif
    return -1;  // timeout
  }

  Delay_Us(1);  // T2

  // read data and EOI status
  *data = gpib_read_data();
  *eoi_asserted = (GPIB_READ(PIN_EOI) == 0);  // active LOW

  // signal not ready (processing data)
  GPIB_ASSERT(PIN_NRFD);
  // signal data accepted
  GPIB_RELEASE(PIN_NDAC);

  // wait for talker to release DAV
  if (gpib_wait_pin(PIN_DAV, 1) < 0) {
    GPIB_RELEASE(PIN_NRFD);
#ifdef GPIB_DEBUG
    printf("[GPIB] Read timeout waiting for DAV High (Talker stuck)\n");
#endif
    return -2;  // timeout
  }

  // prepare for next byte
  GPIB_ASSERT(PIN_NDAC);

  return 0;
}

// Sets up Talker/Listener for data transfer
static int gpib_start_session(uint8_t target_addr, session_mode_t mode) {
  GPIB_ASSERT(PIN_ATN);
  Delay_Us(1);

  // Unlisten everyone first to clear bus state
  if (gpib_write_byte(GPIB_CMD_UNL, 0) < 0) {
    GPIB_RELEASE(PIN_ATN);
    return -1;
  }

  uint8_t talker = (mode == SESSION_WRITE) ? MY_ADDR : target_addr;
  uint8_t listener = (mode == SESSION_WRITE) ? target_addr : MY_ADDR;

  // Untalk, Set Talker, Set Listener
  if (gpib_write_byte(GPIB_CMD_UNT, 0) < 0) goto err;
  if (gpib_write_byte(GPIB_CMD_TAD | talker, 0) < 0) goto err;
  if (gpib_write_byte(GPIB_CMD_LAD | listener, 0) < 0) goto err;

  if (mode == SESSION_READ) {
    gpib_write_data(0x00);  // Float Data
    GPIB_ASSERT(PIN_NDAC);  // Drive LOW (Not Accepted)
    GPIB_ASSERT(PIN_NRFD);  // Drive LOW (Not Ready)
  }

  GPIB_RELEASE(PIN_ATN);  // Switch to Data Mode
  return 0;

err:
  GPIB_RELEASE(PIN_ATN);
  return -1;
}

// Bus management

// Assert Interface Clear (IFC)
static void gpib_interface_clear(void) {
  GPIB_ASSERT(PIN_IFC);
  Delay_Us(150);  // IEEE-488 requires >100us
  GPIB_RELEASE(PIN_IFC);
}

// Control Remote Enable (REN)
static void gpib_remote_enable(int enable) {
  if (enable) {
    GPIB_ASSERT(PIN_REN);
  } else {
    GPIB_RELEASE(PIN_REN);
  }
}

// Check SRQ Line (Active Low)
inline static int gpib_check_srq(void) { return !GPIB_READ(PIN_SRQ); }

// Universal Commands (Affects All Devices)

// Universal Device Clear (DCL)
// Resets logic of ALL devices on the bus
static int gpib_universal_clear(void) {
  GPIB_ASSERT(PIN_ATN);
  Delay_Us(1);

  if (gpib_write_byte(GPIB_CMD_DCL, 0) < 0) {
    GPIB_RELEASE(PIN_ATN);
    return -1;
  }

  GPIB_RELEASE(PIN_ATN);
  return 0;
}

// Local Lockout (LLO)
// Disables front panel "Local" buttons on all devices
static int gpib_local_lockout(void) {
  GPIB_ASSERT(PIN_ATN);
  Delay_Us(1);

  // LLO is universal, no addressing needed
  if (gpib_write_byte(GPIB_CMD_LLO, 0) < 0) {
    GPIB_RELEASE(PIN_ATN);
    return -1;
  }

  GPIB_RELEASE(PIN_ATN);
  return 0;
}

// Addressed cmds

// Selected Device Clear (SDC)
// Resets logic of ONLY the targeted device
static int gpib_device_clear(uint8_t addr) {
  GPIB_ASSERT(PIN_ATN);
  Delay_Us(1);

  if (gpib_write_byte(GPIB_CMD_UNL, 0) < 0) goto err;
  if (gpib_write_byte(GPIB_CMD_LAD | addr, 0) < 0) goto err;
  if (gpib_write_byte(GPIB_CMD_SDC, 0) < 0) goto err;

  GPIB_RELEASE(PIN_ATN);
  return 0;
err:
  GPIB_RELEASE(PIN_ATN);
  return -1;
}

// Group Execute Trigger (GET)
// Triggers the device to take a measurement
static int gpib_trigger(uint8_t addr) {
  GPIB_ASSERT(PIN_ATN);
  Delay_Us(1);

  if (gpib_write_byte(GPIB_CMD_UNL, 0) < 0) goto err;
  if (gpib_write_byte(GPIB_CMD_LAD | addr, 0) < 0) goto err;
  if (gpib_write_byte(GPIB_CMD_GET, 0) < 0) goto err;

  GPIB_RELEASE(PIN_ATN);
  return 0;
err:
  GPIB_RELEASE(PIN_ATN);
  return -1;
}

// Go To Local (GTL)
// Addresses a specific device and restores Front Panel control
// (Keeps REN asserted for other devices on the bus)
static int gpib_go_to_local(uint8_t addr) {
  GPIB_ASSERT(PIN_ATN);
  Delay_Us(1);

  if (gpib_write_byte(GPIB_CMD_UNL, 0) < 0) goto err;
  if (gpib_write_byte(GPIB_CMD_LAD | addr, 0) < 0) goto err;
  if (gpib_write_byte(GPIB_CMD_GTL, 0) < 0) goto err;

  GPIB_RELEASE(PIN_ATN);
  return 0;
err:
  GPIB_RELEASE(PIN_ATN);
  return -1;
}

// Serial Poll
// Reads the Status Byte (STB) from the device
static int gpib_serial_poll(uint8_t addr, uint8_t* status) {
  GPIB_ASSERT(PIN_ATN);
  Delay_Us(1);

  // setupo seq: UNL -> SPE -> LAD(Me) -> TAD(Target)
  if (gpib_write_byte(GPIB_CMD_UNL, 0) < 0) goto err;
  if (gpib_write_byte(GPIB_CMD_SPE, 0) < 0) goto err;
  if (gpib_write_byte(GPIB_CMD_LAD | MY_ADDR, 0) < 0) goto err;
  if (gpib_write_byte(GPIB_CMD_TAD | addr, 0) < 0) goto err;

  gpib_write_data(0x00);  // Float data lines
  GPIB_ASSERT(PIN_NDAC);  // Busy / Not Accepted
  GPIB_ASSERT(PIN_NRFD);  // Busy / Not Ready
  GPIB_RELEASE(PIN_ATN);  // Handover to data mode

  bool eoi;
  if (gpib_read_byte(status, &eoi) < 0) {
    goto err_data;
  }

  GPIB_ASSERT(PIN_ATN);
  Delay_Us(1);

  // return to cmd mode
  GPIB_RELEASE(PIN_NRFD);
  GPIB_RELEASE(PIN_NDAC);

  // end seq: SPD -> UNT
  gpib_write_byte(GPIB_CMD_SPD, 0);
  gpib_write_byte(GPIB_CMD_UNT, 0);
  GPIB_RELEASE(PIN_ATN);

  return 0;

err_data:
  // if read failed, assert ATN and release those lines
  GPIB_ASSERT(PIN_ATN);
  Delay_Us(1);
  GPIB_RELEASE(PIN_NRFD);
  GPIB_RELEASE(PIN_NDAC);

err:
  // just ensure we don't leave the device in spoll mode
  gpib_write_byte(GPIB_CMD_SPD, 0);
  gpib_write_byte(GPIB_CMD_UNT, 0);
  GPIB_RELEASE(PIN_ATN);
  return -1;
}

// Data transfer

// Send string to device (handles CRLF escape sequences)
static int gpib_send(uint8_t addr, const char* str) {
  if (gpib_start_session(addr, SESSION_WRITE) < 0) return -1;

  int len = strlen(str);

  // EOS mode determines what we ADD to the string
  const char* suffix = "";
  if (sys_cfg.eos_mode == 0)
    suffix = "\r\n";  // CRLF
  else if (sys_cfg.eos_mode == 1)
    suffix = "\r";  // CR
  else if (sys_cfg.eos_mode == 2)
    suffix = "\n";  // LF
  // EOS 3 = none

  int suffix_len = strlen(suffix);
  int total_len = len + suffix_len;

  for (int i = 0; i < total_len; i++) {
    uint8_t b = (i < len) ? str[i] : suffix[i - len];

    // assert EOI on last byte ONLY if ++eoi 1 (eoi_assert) is set
    int is_last = (i == total_len - 1);
    int trigger_eoi = (sys_cfg.eoi_assert && is_last);

    if (gpib_write_byte(b, trigger_eoi) < 0) {
      // error cleanup
      GPIB_ASSERT(PIN_ATN);
      gpib_write_byte(GPIB_CMD_UNL, 0);
      GPIB_RELEASE(PIN_ATN);
      return -1;
    }
  }

  // normal cleanup
  GPIB_ASSERT(PIN_ATN);
  gpib_write_byte(GPIB_CMD_UNL, 0);
  GPIB_RELEASE(PIN_ATN);
  return 0;
}

// Receive string from device
static int gpib_receive(uint8_t addr, char* buf, int max_len) {
  if (gpib_start_session(addr, SESSION_READ) < 0) return -1;

  int count = 0;
  bool eoi = false;
  uint8_t byte;
  int effective_max = max_len - 2;

  while (count < effective_max) {
    if (gpib_read_byte(&byte, &eoi) < 0) break;
    buf[count++] = (char)byte;

    // hw EOI always stops the read immediately
    if (eoi) break;

    bool match = false;
    char c = (char)byte;

    // buf[count-1] is 'c'. buf[count-2] is prev.
    char prev = (count >= 2) ? buf[count - 2] : 0;
    char prev2 = (count >= 3) ? buf[count - 3] : 0;

    switch (sys_cfg.eor_mode) {
      case 0:  // CR + LF
        if (prev == '\r' && c == '\n') match = true;
        break;
      case 1:  // CR
        if (c == '\r') match = true;
        break;
      case 2:  // LF
        if (c == '\n') match = true;
        break;
      case 3:  // none?
        match = false;
        break;
      case 4:  // LF + CR
        if (prev == '\n' && c == '\r') match = true;
        break;
      case 5:  // ETX (0x03)
        if (c == 0x03) match = true;
        break;
      case 6:  // CR + LF + ETX
        if (prev2 == '\r' && prev == '\n' && c == 0x03) match = true;
        break;
      case 7:  // EOI signal only
        match = false;
        break;
    }

    if (match) break;
  }

  // append USB EOT char if enabled
  if (sys_cfg.eot_enable) {
    buf[count++] = (char)sys_cfg.eot_char;
  }

  buf[count] = 0;  // null terminate

  // ensure listeners are open before asserting ATN
  GPIB_RELEASE(PIN_NDAC);
  GPIB_RELEASE(PIN_NRFD);

  // cleanup: ATN -> UNT
  GPIB_ASSERT(PIN_ATN);
  gpib_write_byte(GPIB_CMD_UNT, 0);
  GPIB_RELEASE(PIN_ATN);

  return count;
}

// does not stop on newline and does not NULL terminate
static int gpib_receive_binary(uint8_t addr, char* buf, int expected_len) {
  if (gpib_start_session(addr, SESSION_READ) < 0) return -1;

  int count = 0;
  bool eoi = false;
  uint8_t byte;

  // run until we have the expected count
  while (count < expected_len) {
    if (gpib_read_byte(&byte, &eoi) < 0) break;  // hw timeout

    buf[count++] = byte;

    // only stop on EOI. Ignore '\n' because this is binary data.
    if (eoi) break;
  }

  // ensure listeners are open before asserting ATN
  GPIB_RELEASE(PIN_NDAC);
  GPIB_RELEASE(PIN_NRFD);

  // cleanup: ATN -> UNT
  GPIB_ASSERT(PIN_ATN);
  gpib_write_byte(GPIB_CMD_UNT, 0);
  GPIB_RELEASE(PIN_ATN);

  return count;
}

static void gpib_init(void) {
  // calculate BSHR for the DIO lines
  for (int i = 0; i < 256; i++) {
    gpib_write_lut[i] =
        CALC_PIN_BSHR(i, 0, PIN_POS_D1) | CALC_PIN_BSHR(i, 1, PIN_POS_D2) |
        CALC_PIN_BSHR(i, 2, PIN_POS_D3) | CALC_PIN_BSHR(i, 3, PIN_POS_D4) |
        CALC_PIN_BSHR(i, 4, PIN_POS_D5) | CALC_PIN_BSHR(i, 5, PIN_POS_D6) |
        CALC_PIN_BSHR(i, 6, PIN_POS_D7) | CALC_PIN_BSHR(i, 7, PIN_POS_D8);
  }

  // float all control lines
  GPIB_RELEASE(PIN_EOI);
  GPIB_RELEASE(PIN_REN);
  GPIB_RELEASE(PIN_ATN);
  GPIB_RELEASE(PIN_IFC);
  GPIB_RELEASE(PIN_DAV);
  GPIB_RELEASE(PIN_NDAC);
  GPIB_RELEASE(PIN_NRFD);

  // configure control lines as open-drain outputs
  funPinMode(PIN_EOI, GPIO_Speed_50MHz | GPIO_CNF_OUT_OD);
  funPinMode(PIN_REN, GPIO_Speed_50MHz | GPIO_CNF_OUT_OD);
  funPinMode(PIN_ATN, GPIO_Speed_50MHz | GPIO_CNF_OUT_OD);
  funPinMode(PIN_IFC, GPIO_Speed_50MHz | GPIO_CNF_OUT_OD);
  funPinMode(PIN_DAV, GPIO_Speed_50MHz | GPIO_CNF_OUT_OD);
  funPinMode(PIN_NDAC, GPIO_Speed_50MHz | GPIO_CNF_OUT_OD);
  funPinMode(PIN_NRFD, GPIO_Speed_50MHz | GPIO_CNF_OUT_OD);

  // float data lines
  gpib_write_data(0x00);

  // data lines
  funPinMode(PIN_DIO1, GPIO_Speed_50MHz | GPIO_CNF_OUT_OD);
  funPinMode(PIN_DIO2, GPIO_Speed_50MHz | GPIO_CNF_OUT_OD);
  funPinMode(PIN_DIO3, GPIO_Speed_50MHz | GPIO_CNF_OUT_OD);
  funPinMode(PIN_DIO4, GPIO_Speed_50MHz | GPIO_CNF_OUT_OD);
  funPinMode(PIN_DIO5, GPIO_Speed_50MHz | GPIO_CNF_OUT_OD);
  funPinMode(PIN_DIO6, GPIO_Speed_50MHz | GPIO_CNF_OUT_OD);
  funPinMode(PIN_DIO7, GPIO_Speed_50MHz | GPIO_CNF_OUT_OD);
  funPinMode(PIN_DIO8, GPIO_Speed_50MHz | GPIO_CNF_OUT_OD);

  // SRQ is input with pull-up
  funPinMode(PIN_SRQ, GPIO_CNF_IN_PUPD);
  funDigitalWrite(PIN_SRQ, 1);

  // clr
  gpib_interface_clear();
}

// ------------------------------------

static void buzzer_init(void) {
  funPinMode(PIN_BUZZ, GPIO_Speed_50MHz | GPIO_CNF_OUT_PP);
  funDigitalWrite(PIN_BUZZ, 0);

  RCC->APB1PCENR |= RCC_TIM2EN;
  TIM2->PSC = (FUNCONF_SYSTEM_CORE_CLOCK / 1000000) - 1;
  TIM2->ATRLR = 250;
  TIM2->DMAINTENR |= TIM_UIE;

  NVIC_EnableIRQ(TIM2_IRQn);

  TIM2->CTLR1 |= TIM_CEN;
}

static void buzzer_hw_set(uint32_t freq_hz) {
  if (freq_hz == 0) {
    is_buzzer_pulsing = false;
    return;
  }

  if (current_buzz_freq != freq_hz) {
    current_buzz_freq = freq_hz;
    int reload_val = (int)(1000000UL / (2 * freq_hz));
    TIM2->ATRLR = reload_val;
    TIM2->CNT = 0;  // reset phase only on CHANGE
  }
  is_buzzer_pulsing = true;
}

void TIM2_IRQHandler(void) __attribute__((interrupt));
void TIM2_IRQHandler(void) {
  if (TIM2->INTFR & TIM_UIF) {
    // clr the flag
    TIM2->INTFR = (int)~TIM_UIF;

    if (is_buzzer_pulsing) {
      // Toggle PC13
      if (GPIOC->OUTDR & (1 << 13)) {
        GPIOC->BSHR = (1 << (16 + 13));  // Reset (Low)
      } else {
        GPIOC->BSHR = (1 << 13);  // Set (High)
      }
    } else {
      // ensure low when inactive
      if (GPIOC->OUTDR & (1 << 13)) {
        GPIOC->BSHR = (1 << (16 + 13));
      }
    }
  }
}

static void tone_nb(int freq, uint32_t duration_ms) {
  buzzer_hw_set(freq);
  if (freq > 0) {
    app.tone_start_ts = millis();
    app.tone_duration = duration_ms;
    app.tone_timer_pending = true;
  }
}

void play_tune(const uint32_t* tune) {
  // Hardcoded to 4 elements (2 notes) for speed/simplicity
  for (int i = 0; i < 4; i += 2) {
    buzzer_hw_set(tune[i]);
    Delay_Ms(tune[i + 1]);

    buzzer_hw_set(0);
    Delay_Ms(30);
  }
}

// ------
// USB

int HandleSetupCustom(struct _USBState* ctx, int setup_code) {
  if (ctx->USBFS_SetupReqType & USB_REQ_TYP_CLASS) {
    switch (setup_code) {
      case 0x20:  // CDC_SET_LINE_CODING
      case 0x21:  // CDC_GET_LINE_CODING
        ctx->pCtrlPayloadPtr = cdc_line_coding;
        return 7;
      case 0x22:  // CDC_SET_CONTROL_LINE_STATE
        return 0;
    }
  }
  return -1;
}

int HandleInRequest(struct _USBState* ctx __attribute__((unused)),
                    int endp __attribute__((unused)),
                    uint8_t* data __attribute__((unused)),
                    int len __attribute__((unused))) {
  return 0;
}

void HandleDataOut(struct _USBState* ctx, int endp, uint8_t* data, int len) {
  if (endp == 0) {
    ctx->USBFS_SetupReqLen = 0;
  } else if (endp == 2) {
    // Copy to Ring Buffer
    for (int i = 0; i < len; i++) {
      uint32_t next_head = (usb_rx_head + 1) & USB_RX_MASK;

      if (next_head != usb_rx_tail) {
        usb_rx_buffer[usb_rx_head] = data[i];
        usb_rx_head = next_head;
      } else {
        // buf overflow
      }
    }
  }
}

static void usb_process_tx(void) {
  if (!app.usb_online) return;

  // check hw busy (endp 3)
  if (USBFSCTX.USBFS_Endp_Busy[3]) return;
  // check buffer empty
  if (usb_tx_head == usb_tx_tail) return;

  // calc contiguous chunk size
  uint32_t tail = usb_tx_tail;
  uint32_t head = usb_tx_head;

  // calc contiguous size linear from tail to end of arr or head
  int len;
  if (head > tail) {
    len = head - tail;
  } else {
    len = USB_TX_BUF_SIZE - tail;
  }

  // cap at USB pkt size
  if (len > 64) len = 64;

  // send to hw, with memcpy
  USBFS_SendEndpointNEW(3, (uint8_t*)&usb_tx_buffer[tail], len, 1);
  // advance tail
  usb_tx_tail = (tail + len) & USB_TX_MASK;
}

static void usb_send_text(const char* str) {
  if (!app.usb_online) {
    // if offline, just reset buffer
    usb_tx_head = usb_tx_tail = 0;
    return;
  }

  while (*str) {
    uint32_t next = (usb_tx_head + 1) & USB_TX_MASK;

    // buffer full?
    if (next == usb_tx_tail) {
      usb_process_tx();

      uint32_t timeout = app.usb_timeout_cycles;  // ~5ms

      while (next == usb_tx_tail) {
        // this *should* be set by the ISR, so can exit immediately
        if (!USB_HW_IS_ACTIVE()) return;
        if (--timeout == 0) {
          return;  // give up and drop the packet
        }

        usb_process_tx();
      }
    }

    usb_tx_buffer[usb_tx_head] = *str++;
    usb_tx_head = next;
  }

  // tx rightg away
  usb_process_tx();
}

// pull a line from ring buffer
static int get_start_command(char* dest_buf, int max_len) {
  uint32_t head = usb_rx_head;
  uint32_t tail = usb_rx_tail;

  if (head == tail) return 0;

  int len = 0;
  bool found_newline = false;
  uint32_t scan = tail;

  while (scan != head) {
    char c = usb_rx_buffer[scan];
    if (c == '\n' || c == '\r') {
      found_newline = true;
      break;
    }
    scan = (scan + 1) & USB_RX_MASK;

    if (++len >= max_len - 1) break;
  }

  if (found_newline) {
    for (int i = 0; i < len; i++) {
      dest_buf[i] = usb_rx_buffer[tail];
      tail = (tail + 1) & USB_RX_MASK;
    }
    dest_buf[len] = 0;

    // eat limiters
    tail = scan;

    while (tail != head) {
      char c = usb_rx_buffer[tail];
      if (c == '\r' || c == '\n') {
        tail = (tail + 1) & USB_RX_MASK;
      } else {
        break;
      }
    }

    // update the global volatile tail
    usb_rx_tail = tail;
    return len;
  }

  return 0;
}

static void handle_usb_state(void) {
  int raw_status = USB_HW_IS_ACTIVE();
  uint32_t now = millis();

  // edge detection
  if (raw_status != app.usb_raw_prev) {
    app.usb_ts = now;
    app.usb_raw_prev = raw_status;
  }

  // debounce with different thresholds for connect/disconnect
  uint32_t threshold = raw_status ? sys_cfg.usb_debounce_connect_ms
                                  : sys_cfg.usb_debounce_disconnect_ms;

  if ((now - app.usb_ts) > threshold) {
    // state has been stable long enough
    if (app.usb_online != raw_status) {
      app.usb_online = raw_status;

      if (app.usb_online) {
        printf("[USB] CONNECTED\n");
        usb_rx_tail = usb_rx_head = 0;
      } else {
        printf("[USB] DISCONNECTED\n");
      }
    }
  }
}

// ------

static void handle_env_sensor(void) {
  if (!app.env_sensor_present) {
    return;
  }

  uint32_t now = millis();

  if (((now - app.env_last_read) >= sys_cfg.env_sensor_read_interval_ms) &&
      aht20_read(&app.current_env) == AHT20_OK) {
    app.env_last_read = now;
  }
}

// helper to write text to HP3478A display
static void dmm_display(const char* text, const char* mode) {
  // cmp vs shadow buf
  if (strncmp(app.last_disp_sent, text, HP_DISP_BUF_SIZE) == 0) {
    return;
  }

  // printf("Updating Display: %s\n", text);

  // update shadow buf
  strncpy(app.last_disp_sent, text, HP_DISP_BUF_SIZE - 1);
  // "D[23]" + Text + "\n"
  snprintf(scratch.disp.full_cmd, sizeof(scratch.disp.full_cmd), "%s%s\n", mode,
           app.last_disp_sent);

  // send it
  gpib_send(sys_cfg.dmm_addr, scratch.disp.full_cmd);
}

inline static void dmm_display_normal(void) {
  gpib_send(sys_cfg.dmm_addr, HP3478A_DISP_NORMAL);
  // invalidate cache (we're giving control back to DMM)
  app.last_disp_sent[0] = '\0';
}

static void save_dmm_state(void) {
  gpib_interface_clear();
  gpib_send(sys_cfg.dmm_addr, HP3478A_CMD_STATUS_BYTE);

  int len =
      gpib_receive_binary(sys_cfg.dmm_addr, (char*)app.saved_state_bytes, 5);
  app.has_saved_state = (len == 5) ? true : false;
}

static void restore_dmm_state(void) {
  if (!app.has_saved_state) {
    printf("[STATE] No saved state, applying defaults.\n");
    // default fallback if no state saved
    gpib_send(sys_cfg.dmm_addr,
              HP3478A_DISP_NORMAL HP3478A_FUNC_DC_VOLTS HP3478A_RANGE_AUTO
                  HP3478A_DIGITS_5_5 HP3478A_AUTOZERO_ON HP3478A_TRIG_INTERNAL
                      HP3478A_CMD_MASK_BTN_ONLY);
    return;
  }

  dmm_decoded_state_t state;
  decode_dmm_state_bytes(app.saved_state_bytes, &state);

  printf("[STATE] Restoring: Func=%s Range=%s Digits=%s\n", state.cmd_func,
         state.cmd_range, state.cmd_digits);

  // "D1 Fx Rx Nx Zx" + "T1 M20"
  build_restoration_string(scratch.cmd.line_buf, &state);
  strcat(scratch.cmd.line_buf, HP3478A_TRIG_INTERNAL HP3478A_CMD_MASK_BTN_ONLY);

  printf("[STATE] Restore CMD: %s\n", scratch.cmd.line_buf);
  gpib_send(sys_cfg.dmm_addr, scratch.cmd.line_buf);
}

static void exit_to_passthrough(void) {
  buzzer_hw_set(0);
  printf("[APP] Exiting to Passthrough. Restoring DMM...\n");
  restore_dmm_state();
  gpib_go_to_local(sys_cfg.dmm_addr);

  app.current_mode = MODE_PASSTHROUGH;
  app.data.menu.layer = SUBMENU_NONE;
  app.menu_pos = 0;
  app.tone_timer_pending = false;

  uint32_t now = millis();
  app.last_poll_time = now;
  app.ignore_input_start_ts = now;
}

static int init_restored_state(char* unit_dst) {
  if (!app.has_saved_state) {
    dmm_display("ERR NO STATE", HP3478A_DISP_TEXT_FAST);
    return -1;
  }

  dmm_decoded_state_t saved_cfg;
  decode_dmm_state_bytes(app.saved_state_bytes, &saved_cfg);
  build_restoration_string(scratch.cmd.line_buf, &saved_cfg);

  strcat(scratch.cmd.line_buf,
         HP3478A_TRIG_INTERNAL HP3478A_CMD_MASK_BTN_DATA HP3478A_CMD_SRQ_CLEAR);

  gpib_send(sys_cfg.dmm_addr, scratch.cmd.line_buf);
  strcpy(unit_dst, saved_cfg.unit_str);

  return 0;
}

static void enter_feature_mode(menu_item_t item) {
  printf("[APP] Enter Feature Mode: %s (%d)\n", MENU_NAMES[item], item);
  // force display refresh
  app.last_disp_sent[0] = '\0';
  // clean buffer for cmds
  scratch.cmd.line_buf[0] = '\0';

  gpib_remote_enable(1);  // make sure REN is asserted

  switch (item) {
    case MENU_AUTOHOLD:
      // Assuming you track autohold units in the REL struct or dedicated one
      if (init_restored_state(app.data.autohold.unit) == 0) {
        app.current_mode = MODE_FEAT_AUTOHOLD;
        app.data.autohold.stable_count = 0;
        app.data.autohold.is_populated = false;
        dmm_display("AUTO HOLD", HP3478A_DISP_TEXT_FAST);
      }
      break;

    case MENU_REL:
      if (init_restored_state(app.data.rel.unit) == 0) {
        app.current_mode = MODE_FEAT_REL;
        app.data.rel.offset = 0.0;
        dmm_display("REL MODE", HP3478A_DISP_TEXT_FAST);
      }
      break;

    case MENU_STATS:
      if (init_restored_state(app.data.stats.unit) == 0) {
        app.current_mode = MODE_FEAT_STATS;

        // Initialize Stats
        app.data.stats.min = DBL_MAX;   // start impossible high
        app.data.stats.max = -DBL_MAX;  // start impossible low
        app.data.stats.sum = 0.0;
        app.data.stats.count = 0;
        app.data.stats.view_mode = 0;
        app.data.stats.disp_timer = millis();

        dmm_display("STATS INIT", HP3478A_DISP_TEXT_FAST);
      }
      break;

    case MENU_DBM:
      // F1=DCV, A1=AutoRange (Critical for dBm), N5=5.5d
      gpib_send(sys_cfg.dmm_addr,
                HP3478A_MEAS_AC_VOLTS HP3478A_TRIG_INTERNAL
                    HP3478A_CMD_MASK_BTN_DATA HP3478A_CMD_SRQ_CLEAR);
      app.current_mode = MODE_FEAT_DBM;
      dmm_display("DBM 50 OHM", HP3478A_DISP_TEXT_FAST);
      break;

    case MENU_TEMP:
      app.current_mode = MODE_FEAT_TEMP;
      if (app.temp_sensor == SENS_TYPE_K) {
        // Thermocouple Setup:
        // F1: DC Volts
        // R-2: 30mV Range
        // Z1: AutoZero On
        gpib_send(sys_cfg.dmm_addr,
                  HP3478A_FUNC_DC_VOLTS HP3478A_RANGE_NEG_2 HP3478A_DIGITS_5_5
                      HP3478A_AUTOZERO_ON HP3478A_TRIG_INTERNAL
                          HP3478A_CMD_MASK_BTN_DATA HP3478A_CMD_SRQ_CLEAR);
        dmm_display("TEMP TYPE-K", HP3478A_DISP_TEXT_FAST);
      } else if (app.temp_sensor == SENS_THERMISTOR) {
        // Thermistor: 2-Wire Ohms (Usually standard for NTC)
        // Range Auto (NTC varies wildy)
        gpib_send(sys_cfg.dmm_addr,
                  HP3478A_FUNC_OHMS_2WIRE HP3478A_RANGE_AUTO HP3478A_DIGITS_5_5
                      HP3478A_AUTOZERO_ON HP3478A_TRIG_INTERNAL
                          HP3478A_CMD_MASK_BTN_DATA HP3478A_CMD_SRQ_CLEAR);

        char buf[13];
        snprintf(buf, sizeof(buf), "NTC %s",
                 NTC_DEFS[app.temp_ntc_preset].name);
        dmm_display(buf, HP3478A_DISP_TEXT_FAST);
      } else {
        // Resistor Setup (PT1000):
        // F3=2W / F4=4W
        // R4=30kOhm Range
        if (app.temp_wire_mode == WIRE_2W) {
          gpib_send(sys_cfg.dmm_addr,
                    HP3478A_FUNC_OHMS_2WIRE HP3478A_RANGE_4 HP3478A_DIGITS_5_5
                        HP3478A_AUTOZERO_ON HP3478A_TRIG_INTERNAL
                            HP3478A_CMD_MASK_BTN_DATA HP3478A_CMD_SRQ_CLEAR);
        } else {
          gpib_send(sys_cfg.dmm_addr,
                    HP3478A_FUNC_OHMS_4WIRE HP3478A_RANGE_4 HP3478A_DIGITS_5_5
                        HP3478A_AUTOZERO_ON HP3478A_TRIG_INTERNAL
                            HP3478A_CMD_MASK_BTN_DATA HP3478A_CMD_SRQ_CLEAR);
        }

        if (app.temp_sensor == SENS_PT1000)
          dmm_display("TEMP PT1000", HP3478A_DISP_TEXT_FAST);
        // else
        // dmm_display("TEMP THERM", HP3478A_DISP_TEXT_FAST);
      }
      break;

    case MENU_CONT:
      // F3: 2W Ohm, R0: 30 Ohm Range, N3: 3.5 Digits (fastest ADC), M21
      gpib_send(sys_cfg.dmm_addr,
                HP3478A_FUNC_OHMS_2WIRE HP3478A_RANGE_1 HP3478A_DIGITS_3_5
                    HP3478A_AUTOZERO_OFF HP3478A_TRIG_INTERNAL
                        HP3478A_CMD_MASK_BTN_DATA HP3478A_CMD_SRQ_CLEAR);
      app.current_mode = MODE_FEAT_CONT;
      dmm_display("CONT MODE", HP3478A_DISP_TEXT_FAST);
      break;

    case MENU_DIODE:
      // F3: 2-Wire Ohms
      // R3: 3 kOhm Range (1mA Current Through Unknown)
      // N4: 4.5 Digits
      // Z0: Auto-Zero OFF
      gpib_send(sys_cfg.dmm_addr,
                HP3478A_FUNC_OHMS_2WIRE HP3478A_RANGE_3 HP3478A_DIGITS_4_5
                    HP3478A_AUTOZERO_OFF HP3478A_TRIG_INTERNAL
                        HP3478A_CMD_MASK_BTN_DATA HP3478A_CMD_SRQ_CLEAR);

      app.current_mode = MODE_FEAT_DIODE;

      app.data.diode.connected = 0;
      app.data.diode.chirp_start = 0;

      dmm_display("DIODE TEST", HP3478A_DISP_TEXT_FAST);
      break;

    case MENU_XOHM:
      // H7: High Impedance / Extended Ohm Mode
      // The DMM puts internal 10M in parallel with input
      gpib_send(sys_cfg.dmm_addr,
                HP3478A_MEAS_OHMS_EXT HP3478A_DIGITS_5_5 HP3478A_TRIG_INTERNAL
                    HP3478A_CMD_MASK_BTN_DATA HP3478A_CMD_SRQ_CLEAR);
      app.data.xohm.r1 = 0.0;
      app.data.xohm.calibrated = false;
      app.current_mode = MODE_FEAT_XOHM;
      dmm_display("XOHM 10M REF", HP3478A_DISP_TEXT_FAST);
      break;

    case MENU_EXIT:
    default:
      exit_to_passthrough();
      break;
  }
}

static void enter_menu_mode(void) {
  // force display refresh
  app.last_disp_sent[0] = '\0';

  save_dmm_state();
  // Trigger Hold (T4) to make sure no new measurements
  // interrupt our display while we're going through the menu
  // also clear SRQ here manually so we don't increment menu entry
  // the earlier we clear status bits, the better
  gpib_send(sys_cfg.dmm_addr, HP3478A_TRIG_HOLD HP3478A_CMD_SRQ_CLEAR);

  app.current_mode = MODE_MENU;
  app.menu_pos = MENU_REL;
  app.data.menu.timer = millis();

  dmm_display("M: REL", HP3478A_DISP_TEXT_FAST);
}

static void handle_feature_logic(void) {
  uint8_t stb = 0;
  gpib_serial_poll(sys_cfg.dmm_addr, &stb);

  // exit button (SRQ)
  if (stb & HP3478A_MASK_KEYBOARD_SRQ) {
    exit_to_passthrough();
    return;
  }

  // data ready (Bit 0)
  if (!(stb & HP3478A_MASK_DATA_READY)) return;

  // read measurement
  int len = gpib_receive(sys_cfg.dmm_addr, scratch.io.raw_data,
                         sizeof(scratch.io.raw_data));
  if (len < 0) {
    // timeout or error
    printf("[FEAT] DMM read timeout in feature. Mode: %d\n", app.current_mode);
    app.current_mode = MODE_PASSTHROUGH;
    app.dmm_online = false;
    gpib_interface_clear();
    return;
  }

  double val = parse_double(scratch.io.raw_data);
  // overload (HP 3478A sends +9.99990E+9 for OL)
  bool is_overload = false;
  if (val < DMM_OL_NEG_THRESHOLD || val > DMM_OL_THRESHOLD) is_overload = true;

  // RELATIVE MODE
  if (app.current_mode == MODE_FEAT_REL) {
    if (app.data.rel.offset == 0.0) {
      // waiting to capture the NULL value
      if (is_overload) {
        app.data.rel.stable_count = 0;  // reset counter if probes are open
        dmm_display("O.VLD", HP3478A_DISP_TEXT_FAST);
      } else {
        // valid reading
        app.data.rel.stable_count++;

        if (app.data.rel.stable_count >= sys_cfg.rel_stable_count) {
          app.data.rel.offset = val;
          dmm_display("NULL SET", HP3478A_DISP_TEXT_FAST);
          tone_nb(sys_cfg.buzzer_chirp_hz, sys_cfg.buzzer_chirp_ms);
          app.data.rel.stable_count = 0;
        } else {
          dmm_display("LOCKING...", HP3478A_DISP_TEXT_FAST);
        }
      }
    } else {
      // offset is already set
      if (is_overload) {
        dmm_display("O.VLD", HP3478A_DISP_TEXT_FAST);
      } else {
        double diff = val - app.data.rel.offset;
        char eff_unit[5];
        // prepend 'D'
        snprintf(eff_unit, sizeof(eff_unit), "D%s", app.data.rel.unit);

        format_metric_value(scratch.disp.full_cmd,
                            sizeof(scratch.disp.full_cmd), diff, eff_unit, 1);
        dmm_display(scratch.disp.full_cmd, HP3478A_DISP_TEXT);
      }
    }
  } else if (app.current_mode == MODE_FEAT_AUTOHOLD) {
    bool is_signal_valid = !is_overload && val > sys_cfg.autohold_min_val;

    if (!is_signal_valid) {
      app.data.autohold.stable_count = 0;

      if (!app.data.autohold.is_populated) {
        dmm_display("-----", HP3478A_DISP_TEXT_FAST);
      }
    } else {
      // check deviation between current live val and "candidate"
      double diff_percent = 0.0;
      if (app.data.autohold.stable_count > 0) {
        diff_percent = fabs((val - app.data.autohold.candidate_val) /
                            app.data.autohold.candidate_val) *
                       100.0;
      }

      if (app.data.autohold.stable_count == 0 ||
          diff_percent <= sys_cfg.autohold_threshold) {
        // reading is within stability window

        // if this is the start of a new seq, set the candidate
        if (app.data.autohold.stable_count == 0) {
          app.data.autohold.candidate_val = val;
        }

        app.data.autohold.stable_count++;

        // latch trigger
        if (app.data.autohold.stable_count >= sys_cfg.autohold_stable_count) {
          // diff between new candidate and old latched val
          double change_from_displayed = 0.0;
          if (app.data.autohold.is_populated) {
            change_from_displayed = fabs((app.data.autohold.candidate_val -
                                          app.data.autohold.latched_val) /
                                         app.data.autohold.latched_val) *
                                    100.0;
          }

          // only update display (and beep) if:
          // 1. we haven't shown anything yet
          // 2. the new stable value is (>AUTOHOLD_CHANGE_REQ%) from the old val
          if (!app.data.autohold.is_populated ||
              change_from_displayed > sys_cfg.autohold_change_req) {
            // latch it
            app.data.autohold.latched_val = app.data.autohold.candidate_val;
            app.data.autohold.is_populated = true;

            format_metric_value(
                scratch.disp.full_cmd, sizeof(scratch.disp.full_cmd),
                app.data.autohold.latched_val, app.data.autohold.unit, 1);
            dmm_display(scratch.disp.full_cmd, HP3478A_DISP_TEXT);

            // chirp
            tone_nb(sys_cfg.buzzer_chirp_hz, sys_cfg.buzzer_chirp_ms);
          }

          // cap counter
          app.data.autohold.stable_count = sys_cfg.autohold_stable_count;
        }
      } else {
        // jitter/unstable - reset
        app.data.autohold.stable_count = 0;
        app.data.autohold.candidate_val = val;
      }
    }
  }

  // dBm MODE
  // TODO: different references
  else if (app.current_mode == MODE_FEAT_DBM) {
    if (is_overload) {
      dmm_display("O.VLD", HP3478A_DISP_TEXT_FAST);
    } else {
      // P(mW) = (V^2 / 50) * 1000 = V^2 * 20
      double p_mw = (val * val / sys_cfg.dbm_ref_z) * 1000;

      if (p_mw < 1e-9) {
        // Align -INF to look consistent
        // Display: "-INF    DBM"
        memset(scratch.disp.full_cmd, ' ', HP_DISP_LEN);
        scratch.disp.full_cmd[HP_DISP_LEN] = '\0';
        memcpy(scratch.disp.full_cmd, "-INF", 4);
        memcpy(&scratch.disp.full_cmd[8], " DBM", 4);
        dmm_display(scratch.disp.full_cmd, HP3478A_DISP_TEXT_FAST);
      } else {
        double dbm = 10.0 * log10(p_mw);
        format_metric_value(scratch.disp.full_cmd,
                            sizeof(scratch.disp.full_cmd), dbm, "DBM", 00);
        dmm_display(scratch.disp.full_cmd, HP3478A_DISP_TEXT);
      }
    }
  }
  // TEMP MODE
  else if (app.current_mode == MODE_FEAT_TEMP) {
    if (is_overload && app.temp_sensor != SENS_TYPE_K) {
      dmm_display("OPEN / ERR", HP3478A_DISP_TEXT_FAST);
    } else {
      double temp_c = 0.0;
      const char* unit_str = " C";

      // TYPE K THERMOCOUPLE
      if (app.temp_sensor == SENS_TYPE_K) {
        // 30mV range safety check (Floating input > 50mV)
        if (fabs(val) > 0.050) {
          dmm_display("CHECK PROBE", HP3478A_DISP_TEXT_FAST);
          return;
        } else {
          double t_amb;

          // Cold Junction Compensation (CJC)
          if (app.env_sensor_present) {
            t_amb = app.current_env.temp_c_x100 / 100.0;
            t_amb -= sys_cfg.cjc_self_heating_offset;
            unit_str = "C (K)";

          } else {
            t_amb = sys_cfg.cjc_fallback_temp;
            unit_str = "C (K*)";  // '*' means fallback CJC used
          }

          // Type K response is actually NOT linear, this should be a LUT or a
          // polynomial calculation
          // Temp = Ambient + (V_meas * Sensitivity)
          temp_c = t_amb + (val * sys_cfg.type_k_scale);
        }
      }
      // PT1000 RTD (Callendar-Van Dusen)
      else if (app.temp_sensor == SENS_PT1000) {
        if (val < 10.0) {
          dmm_display("SHORT", HP3478A_DISP_TEXT_FAST);
          return;
        } else {
          double c = sys_cfg.rtd_r0 - val;
          double b = sys_cfg.rtd_r0 * sys_cfg.rtd_a;
          double a = sys_cfg.rtd_r0 * sys_cfg.rtd_b;
          double disc = (b * b) - (4 * a * c);

          if (disc >= 0)
            temp_c = (-b + sqrt(disc)) / (2 * a);
          else {
            dmm_display("RANGE ERR", HP3478A_DISP_TEXT_FAST);
            return;
          }
        }
      }
      // THERMISTOR (simplified Steinhart-Hart)
      else {
        if (val < 10.0) {
          dmm_display("SHORT", HP3478A_DISP_TEXT_FAST);
          return;
        }
        if (val > 4000000.0) {
          dmm_display("OPEN", HP3478A_DISP_TEXT_FAST);
          return;
        }

        double r_meas = val;
        // constants from our preset
        double r0 = NTC_DEFS[app.temp_ntc_preset].r0;
        double beta = NTC_DEFS[app.temp_ntc_preset].beta;
        const double t0_k = 298.15;  // 25.0C in Kelvin

        // 1/T = 1/T0 + (1/B * ln(R/R0))
        double ln_ratio = log(r_meas / r0);
        double inv_t = (1.0 / t0_k) + ((1.0 / beta) * ln_ratio);

        // convert Kelvin to Celsius
        temp_c = (1.0 / inv_t) - 273.15;

        unit_str = "C NTC";
      }

      format_metric_value(scratch.disp.full_cmd, sizeof(scratch.disp.full_cmd),
                          temp_c, unit_str, 0);
      dmm_display(scratch.disp.full_cmd, HP3478A_DISP_TEXT);
    }
  }
  // CONT MODE
  else if (app.current_mode == MODE_FEAT_CONT) {
    int is_short = (!is_overload && val < sys_cfg.cont_threshold_ohms);

    // instant beep
    buzzer_hw_set(is_short ? sys_cfg.buzzer_cont_hz : 0);

    uint32_t now = millis();
    if (now - app.data.cont.last_disp_update > sys_cfg.cont_disp_update_ms) {
      app.data.cont.last_disp_update = now;

      if (is_overload) {
        dmm_display("OPEN", HP3478A_DISP_TEXT_FAST);
      } else {
        format_metric_value(scratch.disp.full_cmd,
                            sizeof(scratch.disp.full_cmd), val, "OHM", 1);
        dmm_display(scratch.disp.full_cmd, HP3478A_DISP_TEXT);
      }
    }
  }

  else if (app.current_mode == MODE_FEAT_DIODE) {
    uint32_t now = millis();
    double voltage = is_overload ? 9.9 : (val / 1000.0);
    bool is_valid_signal =
        (voltage > sys_cfg.diode_th_short && voltage < sys_cfg.diode_th_open);

    if (voltage < sys_cfg.diode_th_open) {
      format_metric_value(scratch.disp.full_cmd, sizeof(scratch.disp.full_cmd),
                          voltage, "VDC", 1);
      dmm_display(scratch.disp.full_cmd, HP3478A_DISP_TEXT);
    } else {
      dmm_display("OPEN", HP3478A_DISP_TEXT);
    }

    diode_state_t next_state = app.data.diode.connected;

    switch (app.data.diode.connected) {
      case DIODE_STATE_OPEN:
      case DIODE_STATE_SHORT:
        if (is_valid_signal) {
          next_state = DIODE_STATE_CHECKING;
          app.data.diode.chirp_start = now;
        } else {
          // strictly either open or short
          next_state = (voltage >= sys_cfg.diode_th_open) ? DIODE_STATE_OPEN
                                                          : DIODE_STATE_SHORT;
        }
        break;

      case DIODE_STATE_CHECKING:
        if (!is_valid_signal) {
          next_state = DIODE_STATE_SHORT;  // signal lost/glitch
        } else if ((now - app.data.diode.chirp_start) >=
                   sys_cfg.diode_stable_ms) {
          // has been stable long enough
          tone_nb(sys_cfg.buzzer_chirp_hz, sys_cfg.buzzer_chirp_ms);
          next_state = DIODE_STATE_DONE;
        }
        break;

      case DIODE_STATE_DONE:
        // latch until signal is clearly lost
        if (!is_valid_signal) {
          next_state = DIODE_STATE_SHORT;
        }
        break;

      default:
        next_state = DIODE_STATE_OPEN;
        break;
    }

    app.data.diode.connected = next_state;
  }

  // XOHM MODE
  else if (app.current_mode == MODE_FEAT_XOHM) {
    // cal phase, measure the internal 10M resistor
    if (!app.data.xohm.calibrated) {
      // need the probes to be open. Internal R is ~10M
      if (val > 8.0e6 && val < 12.0e6) {
        app.data.xohm.r1 = val;  // Store R1
        app.data.xohm.calibrated = true;
        tone_nb(sys_cfg.buzzer_chirp_hz, sys_cfg.buzzer_chirp_ms);
      } else {
        dmm_display("OPEN PROBES", HP3478A_DISP_TEXT_FAST);
      }
    }
    // Rx = (R1 * R2) / (R1 - R2)
    // R1 = xohm_ref (Internal)
    // R2 = val (Measured Parallel)
    else {
      if (is_overload || val >= (app.data.xohm.r1 - 1000.0)) {
        dmm_display("OPEN", HP3478A_DISP_TEXT_FAST);
      } else {
        double r1 = app.data.xohm.r1;
        double r2 = val;
        double rx = (r1 * r2) / (r1 - r2);

        format_metric_value(scratch.disp.full_cmd,
                            sizeof(scratch.disp.full_cmd), rx, "OHM", 1);
        dmm_display(scratch.disp.full_cmd, HP3478A_DISP_TEXT);
      }
    }
  } else if (app.current_mode == MODE_FEAT_STATS) {
    if (is_overload) {
      dmm_display("O.VLD", HP3478A_DISP_TEXT_FAST);
    } else {
      // accumulate math
      if (val < app.data.stats.min) app.data.stats.min = val;
      if (val > app.data.stats.max) app.data.stats.max = val;
      app.data.stats.sum += val;
      app.data.stats.count++;

      // rotate display
      uint32_t now = millis();
      if (now - app.data.stats.disp_timer > sys_cfg.stats_cycle_time_ms) {
        app.data.stats.view_mode++;
        if (app.data.stats.view_mode > 3) app.data.stats.view_mode = 0;
        app.data.stats.disp_timer = now;
      }

      // render
      const char* prefix_str = "";
      double val_to_show = val;

      switch (app.data.stats.view_mode) {
        case 0:  // Live
          val_to_show = val;
          break;
        case 1:  // Avg
          prefix_str = "AVG ";
          val_to_show = app.data.stats.sum / app.data.stats.count;
          break;
        case 2:  // Min
          prefix_str = "MIN ";
          val_to_show = app.data.stats.min;
          break;
        case 3:  // Max
          prefix_str = "MAX ";
          val_to_show = app.data.stats.max;
          break;
        default:
          app.data.stats.view_mode = 0;
          break;
      }

      // render
      if (app.data.stats.view_mode == 0) {
        // live mode
        format_metric_value(scratch.disp.full_cmd,
                            sizeof(scratch.disp.full_cmd), val_to_show,
                            app.data.stats.unit, 1);
      } else {
        // stats mode: prefix (4) + number (8)
        double abs_v = fabs(val_to_show);
        double scaled = val_to_show;
        char suffix = 0;

        if (abs_v >= 1.0e9) {
          scaled *= 1.0e-9;
          suffix = 'G';
        } else if (abs_v >= 1.0e6) {
          scaled *= 1.0e-6;
          suffix = 'M';
        } else if (abs_v >= 1.0e3) {
          scaled *= 1.0e-3;
          suffix = 'K';
        }

        char num_buf[16];
        double_to_str(num_buf, sizeof(num_buf), scaled, 4);

        // combine: prefix + number + suffix
        snprintf(scratch.disp.full_cmd, sizeof(scratch.disp.full_cmd),
                 "%s%s%c  ", prefix_str, num_buf, suffix ? suffix : ' ');
      }

      // send it
      dmm_display(scratch.disp.full_cmd, HP3478A_DISP_TEXT);
    }
  }
}

// gens the base string (e.g., "M: TEMP", "S: TYPE K") into
// scratch.disp.full_cmd
static void prepare_menu_base_string(void) {
  const char* prefix = "";
  const char* name = "???";

  if (app.data.menu.layer == SUBMENU_NONE) {
    prefix = "M: ";
    if (app.menu_pos < MENU_MAX_ITEMS) name = MENU_NAMES[app.menu_pos];
  } else if (app.data.menu.layer == SUBMENU_TEMP_SENS) {
    prefix = "S: ";
    if (app.menu_pos < SENS_MAX_ITEMS) name = SENSOR_NAMES[app.menu_pos];
  } else if (app.data.menu.layer == SUBMENU_TEMP_WIRE) {
    prefix = "T: ";
    if (app.menu_pos < WIRE_MAX_ITEMS) name = WIRE_NAMES[app.menu_pos];
  } else if (app.data.menu.layer == SUBMENU_TEMP_NTC) {
    prefix = "N: ";
    if (app.menu_pos < NTC_MAX_ITEMS) name = NTC_DEFS[app.menu_pos].name;
  }

  snprintf(scratch.disp.full_cmd, sizeof(scratch.disp.full_cmd), "%s%s", prefix,
           name);
}

inline static int get_menu_max_items(void) {
  if (app.data.menu.layer == SUBMENU_TEMP_SENS) return SENS_MAX_ITEMS;
  if (app.data.menu.layer == SUBMENU_TEMP_WIRE) return WIRE_MAX_ITEMS;
  if (app.data.menu.layer == SUBMENU_TEMP_NTC) return NTC_MAX_ITEMS;
  return MENU_MAX_ITEMS;
}

static void handle_menu_navigation(void) {
  uint32_t now = millis();
  uint32_t elapsed = now - app.data.menu.timer;

  if (elapsed > MENU_DEBOUNCE_MS) {
    // only poll GPIB if physical line is asserted
    if (gpib_check_srq()) {
      uint8_t stb = 0;

      if (gpib_serial_poll(sys_cfg.dmm_addr, &stb) != 0) return;

      // check if it was the front panel btn
      if (stb & HP3478A_MASK_KEYBOARD_SRQ) {
        // reset timer
        app.data.menu.timer = now;
        app.menu_pos++;
        if (app.menu_pos >= get_menu_max_items()) app.menu_pos = 0;

        // update display
        prepare_menu_base_string();
        dmm_display(scratch.disp.full_cmd, HP3478A_DISP_TEXT_FAST);
        return;
      }
    }
  }

  prepare_menu_base_string();

  // only calculate dots if we are past the initial delay
  if (elapsed > sys_cfg.menu_sublayer_delay_ms) {
    uint32_t dot_time = elapsed - sys_cfg.menu_sublayer_delay_ms;
    int dots = dot_time / sys_cfg.menu_dot_interval_ms;
    if (dots > 3) dots = 3;

    for (int i = 0; i < dots; i++) strcat(scratch.disp.full_cmd, ".");

    // this does a strncmp every time :c
    dmm_display(scratch.disp.full_cmd, HP3478A_DISP_TEXT_FAST);
  }

  if (elapsed <= sys_cfg.menu_commit_delay_ms) return;

  // L0: main menu
  if (app.data.menu.layer == SUBMENU_NONE) {
    if (app.menu_pos == MENU_TEMP) {
      // sensor select
      app.data.menu.layer = SUBMENU_TEMP_SENS;
      app.menu_pos = 0;  // default to first sensor
      app.data.menu.timer = now;

      prepare_menu_base_string();
      dmm_display(scratch.disp.full_cmd, HP3478A_DISP_TEXT_FAST);
      return;
    }
    // enter standard modes
    enter_feature_mode(app.menu_pos);
    return;
  }

  // L1: sensor select
  else if (app.data.menu.layer == SUBMENU_TEMP_SENS) {
    app.temp_sensor = (temp_sensor_t)app.menu_pos;

    if (app.temp_sensor == SENS_TYPE_K) {
      // Type K is voltage based so skip wire select
      enter_feature_mode(MENU_TEMP);
    } else if (app.temp_sensor == SENS_THERMISTOR) {
      app.data.menu.layer = SUBMENU_TEMP_NTC;
      app.menu_pos = 0;  // default to 10K
      app.data.menu.timer = now;
      prepare_menu_base_string();
      dmm_display(scratch.disp.full_cmd, HP3478A_DISP_TEXT_FAST);
    } else {
      // wire select (for resistive)
      app.data.menu.layer = SUBMENU_TEMP_WIRE;
      app.menu_pos = 0;  // default to 2W
      app.data.menu.timer = now;

      prepare_menu_base_string();
      dmm_display(scratch.disp.full_cmd, HP3478A_DISP_TEXT_FAST);
    }
    return;
  }
  // L2a: wire select
  else if (app.data.menu.layer == SUBMENU_TEMP_WIRE) {
    app.temp_wire_mode = (wire_mode_t)app.menu_pos;
    enter_feature_mode(MENU_TEMP);
    return;
  }  // L2b: NTC select
  else if (app.data.menu.layer == SUBMENU_TEMP_NTC) {
    app.temp_ntc_preset = (ntc_preset_t)app.menu_pos;
    enter_feature_mode(MENU_TEMP);
    return;
  }
}

static void cmd_help(void) {
  static const char* help_text =
      "\r\nHP3478A Internal USB-GPIB " FW_VERSION
      "\r\n"
      "\r\n"
      "[GPIB Setup]\r\n"
      "  ++addr <0-30>     Target Address\r\n"
      "  ++auto <0-2>      0:Off, 1:Read-After-Write, 2:Query-Only\r\n"
      "  ++read_tmo_ms <t> Timeout in ms\r\n"
      "  ++eoi <0|1>       Assert hardware EOI on write end\r\n"
      "  ++eos <0-3>       Write Term: 0:CRLF, 1:CR, 2:LF, 3:None\r\n"
      "  ++eor <0-7>       Read Stop:  0:CRLF ... 7:EOI-Only\r\n"
      "  ++eot_enable <B>  Append extra char to read output\r\n"
      "  ++eot_char <dec>  The char to append\r\n"
      "  ++dmm_loop <0|1>  Toggle HP3478A loop, if it conflicts w/ GPIB bus\r\n"
      "\r\n"
      "[System Configuration]\r\n"
      "  ++config          List all configurable parameters\r\n"
      "  ++get <name>      Get parameter value\r\n"
      "  ++set <name> <v>  Set parameter value\r\n"
      "  ++savecfg         Save config to flash\r\n"
      "  ++ver             Firmware Version\r\n"
      "  ++rst             System Reset\r\n"
      "\r\n"
      "[GPIB Bus Operations]\r\n"
      "  ++read            Read from target\r\n"
      "  ++write <data>    Write to target\r\n"
      "  ++trg             Device Trigger (GET)\r\n"
      "  ++clr             Device Clear (SDC)\r\n"
      "  ++dcl             Universal Device Clear (DCL)\r\n"
      "  ++ifc             Interface Clear (Bus Reset)\r\n"
      "  ++spoll [addr]    Serial Poll\r\n"
      "  ++srq             Query SRQ Line (0=High/Idle, 1=Low/Active)\r\n"
      "  ++loc             Local (Drop REN)\r\n"
      "  ++llo             Local Lockout\r\n"
      "\r\n"
      "[Internal HP3478A Features]\r\n"
      "  ++cont, ++hold, ++rel, ++xohm\r\n"
      "  ++dbm, ++diode, ++math, ++norm\r\n"
      "  ++temp <l1> <l2>  Temperature sensor mode\r\n"
      "  ++env [temp|hum]  Internal AHT20 Sensor\r\n"
      "  ++disp <text>     Write text to LCD\r\n"
      "\r\n";

  usb_send_text(help_text);
}

static void cmd_status(void) {
  int srq = gpib_check_srq();

  snprintf(scratch.cmd.fmt_buf, sizeof(scratch.cmd.fmt_buf),
           "ADDR: %d\r\n"
           "TMO : %lu ms\r\n"
           "AUTO: %d\r\n"
           "EOS : %d\r\n"
           "EOR : %d\r\n"
           "EOI : %d\r\n"
           "EOT : %s (%d)\r\n"
           "SRQ : %d\r\n"
           "DMM_LOOP : %d\r\n",
           sys_cfg.target_addr, sys_cfg.gpib_timeout_ms, sys_cfg.auto_read,
           sys_cfg.eos_mode, sys_cfg.eor_mode, sys_cfg.eoi_assert,
           sys_cfg.eot_enable ? "ON" : "OFF", sys_cfg.eot_char, srq,
           app.dmm_loop);

  usb_send_text(scratch.cmd.fmt_buf);
}

static void process_command(void) {
  if (!get_start_command(scratch.cmd.line_buf, sizeof(scratch.cmd.line_buf))) {
    return;
  }

  char* p_cmd = skip_spaces(scratch.cmd.line_buf);
  menu_item_t menu_item = MENU_EXIT;

  if (strncmp(p_cmd, "++", 2) == 0) {
    p_cmd += 2;
    char* p_args = p_cmd;
    while (*p_args && !isspace((unsigned char)*p_args)) {
      p_args++;
    }
    // if we found a space/separator, split
    if (*p_args) {
      *p_args = 0;
      p_args++;
      p_args = skip_spaces(p_args);
    }

    cmd_id_t cmd_id = parse_command_id(p_cmd);

    switch (cmd_id) {
      // config
      case CMD_ADDR:
        if (*p_args) {
          int addr = atoi(p_args);
          if (addr >= 0 && addr <= 30) {
            sys_cfg.target_addr = addr;
          } else
            usb_send_text("ERR: Invalid Addr\r\n");
        } else {
          // if no arg provided, show current
          snprintf(scratch.cmd.fmt_buf, sizeof(scratch.cmd.fmt_buf), "%d\r\n",
                   sys_cfg.target_addr);
          usb_send_text(scratch.cmd.fmt_buf);
        }
        break;

      case CMD_AUTO:
        if (*p_args) {
          int val = atoi(p_args);
          // Allow 0, 1, 2. Ignore 3 since we don't support it
          // it's possible but.. effort
          if (val >= 0 && val <= 2) {
            sys_cfg.auto_read = val;
          }
        } else {
          snprintf(scratch.cmd.fmt_buf, sizeof(scratch.cmd.fmt_buf), "%d\r\n",
                   sys_cfg.auto_read);
          usb_send_text(scratch.cmd.fmt_buf);
        }
        break;

      case CMD_VER:
        usb_send_text("HP3478A Internal USB-GPIB " FW_VERSION "\r\n");
        break;
      case CMD_STAT:
        cmd_status();
        break;
      case CMD_HELP:
        cmd_help();
        break;
      case CMD_RST:
        NVIC_SystemReset();
        break;

      case CMD_EOI:
        if (*p_args) {
          sys_cfg.eoi_assert = (atoi(p_args) ? true : false);
        } else {
          usb_send_text(sys_cfg.eoi_assert ? "1\r\n" : "0\r\n");
        }
        break;

      case CMD_EOS:
        if (*p_args) {
          int val = atoi(p_args);
          if (val >= 0 && val <= 3) sys_cfg.eos_mode = val;
        } else {
          snprintf(scratch.cmd.fmt_buf, sizeof(scratch.cmd.fmt_buf), "%d\r\n",
                   sys_cfg.eos_mode);
          usb_send_text(scratch.cmd.fmt_buf);
        }
        break;

      case CMD_EOT_ENABLE:
        if (*p_args) {
          sys_cfg.eot_enable = (atoi(p_args) ? true : false);
        } else {
          usb_send_text(sys_cfg.eot_enable ? "1\r\n" : "0\r\n");
        }
        break;

      case CMD_EOT_CHAR:
        if (*p_args) {
          sys_cfg.eot_char = (uint8_t)atoi(p_args);
        } else {
          snprintf(scratch.cmd.fmt_buf, sizeof(scratch.cmd.fmt_buf), "%d\r\n",
                   sys_cfg.eot_char);
          usb_send_text(scratch.cmd.fmt_buf);
        }
        break;

      case CMD_EOR:
        if (*p_args) {
          int val = atoi(p_args);
          if (val >= 0 && val <= 7) {
            sys_cfg.eor_mode = val;
          }
        } else {
          snprintf(scratch.cmd.fmt_buf, sizeof(scratch.cmd.fmt_buf), "%d\r\n",
                   sys_cfg.eor_mode);
          usb_send_text(scratch.cmd.fmt_buf);
        }
        break;

      case CMD_MODE:
        if (*p_args) {
          // int val = atoi(p_args);
          // if (val == 1) {
          //   // we are always controller
          // } else {
          //   usb_send_text("ERR: dev mode unsupp\r\n");
          // }
        } else {
          // current mode -> 1
          usb_send_text("1\r\n");
        }
        break;

      case CMD_DMM_LOOP:
        if (*p_args) {
          app.dmm_loop = (atoi(p_args) ? true : false);
          exit_to_passthrough();
          gpib_interface_clear();
        } else {
          usb_send_text(app.dmm_loop ? "1\r\n" : "0\r\n");
        }
        break;

      case CMD_SAVECFG:
        config_save();
        break;

      case CMD_CFG_LIST: {
        // dump all config values
        for (size_t i = 0; i < CONFIG_MAP_SIZE; i++) {
          const cfg_field_t* f = &CONFIG_MAP[i];
          uint8_t* base_addr = (uint8_t*)&sys_cfg;
          void* p_val = (void*)(base_addr + f->offset);

          usb_send_text(f->name);
          usb_send_text(": ");

          if (f->type == CFG_TYPE_UINT8) {
            snprintf(scratch.cmd.fmt_buf, 64, "%u\r\n", *(uint8_t*)p_val);
          } else if (f->type == CFG_TYPE_UINT32) {
            snprintf(scratch.cmd.fmt_buf, 64, "%lu\r\n", *(uint32_t*)p_val);
          } else if (f->type == CFG_TYPE_DOUBLE) {
            double_to_str(scratch.cmd.fmt_buf, 64, *(double*)p_val, 6);
            strcat(scratch.cmd.fmt_buf, "\r\n");
          }
          usb_send_text(scratch.cmd.fmt_buf);
        }
        break;
      }

      case CMD_CFG_GET: {
        char* arg_name = p_args;
        // terminate arg at next space
        char* p_end = strchr(arg_name, ' ');
        if (p_end) *p_end = 0;

        const cfg_field_t* f = find_config_field(arg_name);
        if (!f) {
          usb_send_text("ERR: Param not found\r\n");
        } else {
          uint8_t* base_addr = (uint8_t*)&sys_cfg;
          void* p_val = (void*)(base_addr + f->offset);

          if (f->type == CFG_TYPE_UINT8) {
            snprintf(scratch.cmd.fmt_buf, 64, "%u\r\n", *(uint8_t*)p_val);
          } else if (f->type == CFG_TYPE_UINT32) {
            snprintf(scratch.cmd.fmt_buf, 64, "%lu\r\n", *(uint32_t*)p_val);
          } else if (f->type == CFG_TYPE_DOUBLE) {
            double_to_str(scratch.cmd.fmt_buf, 64, *(double*)p_val,
                          6); /* precision 6 */
            strcat(scratch.cmd.fmt_buf, "\r\n");
          }

          usb_send_text(scratch.cmd.fmt_buf);
        }
        break;
      }

      case CMD_CFG_SET: {
        // syntax: ++set <param> <value>
        char* arg_name = p_args;
        char* arg_val = strchr(arg_name, ' ');

        if (!arg_val) {
          usb_send_text("ERR: Missing value\r\n");
          return;
        }
        *arg_val = 0;
        arg_val++;
        arg_val = skip_spaces(arg_val);

        const cfg_field_t* f = find_config_field(arg_name);
        if (!f) {
          usb_send_text("ERR: Param not found\r\n");
        } else {
          uint8_t* base_addr = (uint8_t*)&sys_cfg;
          void* p_dest = (void*)(base_addr + f->offset);

          if (f->type == CFG_TYPE_UINT8) {
            *(uint8_t*)p_dest = (uint8_t)atoi(arg_val);
          } else if (f->type == CFG_TYPE_UINT32) {
            *(uint32_t*)p_dest = (uint32_t)strtoul(arg_val, NULL, 10);
          } else if (f->type == CFG_TYPE_DOUBLE) {
            *(double*)p_dest = parse_double(arg_val);
          }

          config_apply_to_app();
        }
        break;
      }

      case CMD_SRQ:
        usb_send_text(gpib_check_srq() ? "1\r\n" : "0\r\n");
        break;

      // data
      case CMD_READ:
        goto do_read_operation;
      case CMD_WRITE:
        if (*p_args) {
          gpib_send(sys_cfg.target_addr, p_args);
          // if auto is 1 (Read-after-write), read now
          if (sys_cfg.auto_read == 1) goto do_read_operation;
        }
        break;

      // GPIB Bus control
      case CMD_TIMEOUT:
        if (*p_args) {
          int val = atoi(p_args);
          // min 1ms, max 60s
          if (val > 0 && val < 60000) {
            sys_cfg.gpib_timeout_ms = val;
          } else {
            usb_send_text("ERR: Range 1-60000\r\n");
          }
        } else {
          snprintf(scratch.cmd.fmt_buf, sizeof(scratch.cmd.fmt_buf), "%lu\r\n",
                   sys_cfg.gpib_timeout_ms);
          usb_send_text(scratch.cmd.fmt_buf);
        }
        break;
      case CMD_TRG:
        gpib_trigger(sys_cfg.target_addr);
        break;
      case CMD_CLR:
        gpib_device_clear(sys_cfg.target_addr);
        break;
      case CMD_DCL:
        gpib_universal_clear();
        break;
      case CMD_IFC:
        gpib_interface_clear();
        break;
      case CMD_LLO:
        gpib_local_lockout();
        break;
      case CMD_GTL:
        gpib_go_to_local(sys_cfg.target_addr);
        break;
      case CMD_LOC:
        gpib_remote_enable(0);
        break;

      case CMD_REN:
        if (*p_args) {
          gpib_remote_enable(atoi(p_args));
        } else
          usb_send_text("ERR: Usage ++ren 1|0\r\n");
        break;

      case CMD_SPOLL: {
        uint8_t poll_addr = (*p_args) ? atoi(p_args) : sys_cfg.target_addr;
        uint8_t stb;
        if (gpib_serial_poll(poll_addr, &stb) == 0) {
          snprintf(scratch.cmd.fmt_buf, sizeof(scratch.cmd.fmt_buf), "%d\r\n",
                   stb);
          usb_send_text(scratch.cmd.fmt_buf);
        } else
          usb_send_text("ERR: Poll TMO\r\n");
        break;
      }

      // HP3478A Internal Features
      case CMD_NORM:
        exit_to_passthrough();
        // ensure REN is back up for normal use
        gpib_remote_enable(1);
        usb_send_text("OK\r\n");
        break;

      case CMD_DISP: {
        int i = 0;
        while (p_args[i] != 0 && i < HP_DISP_LEN) {
          char c = p_args[i];
          if (c >= 'a' && c <= 'z') c -= 32;
          scratch.disp.full_cmd[i++] = c;
        }
        scratch.disp.full_cmd[i] = 0;
        dmm_display(scratch.disp.full_cmd, HP3478A_DISP_TEXT_FAST);
        usb_send_text("OK\r\n");
        break;
      }

      case CMD_ENV: {
        if (!app.env_sensor_present) {
          usb_send_text("ERR: No Sensor\r\n");
          return;
        }
        double t = app.current_env.temp_c_x100 / 100.0;
        double h = app.current_env.hum_p_x100 / 100.0;
        char* arg = skip_spaces(p_args);
        char out_buf[32];

        // temp only
        if (starts_with_nocase(arg, "temp")) {
          double_to_str(out_buf, sizeof(out_buf), t, 2);
          usb_send_text(out_buf);
          usb_send_text("\r\n");
        }
        // hum only
        else if (starts_with_nocase(arg, "hum")) {
          double_to_str(out_buf, sizeof(out_buf), h, 2);
          usb_send_text(out_buf);
          usb_send_text("\r\n");
        }
        // CSV format (temp,hum)
        else {
          double_to_str(out_buf, 16, t, 2);
          strcat(out_buf, ",");
          // fmt humidity into a temp buffer and append
          char h_buf[16];
          double_to_str(h_buf, sizeof(h_buf), h, 2);
          strcat(out_buf, h_buf);
          strcat(out_buf, "\r\n");

          usb_send_text(out_buf);
        }
        break;
      }

      // feat entry
      case CMD_CONT:
        menu_item = MENU_CONT;
        break;
      case CMD_AUTOHOLD:
        menu_item = MENU_AUTOHOLD;
        break;
      case CMD_TEMP: {
        // syntax: +temp <type> [config]
        menu_item = MENU_TEMP;

        if (*p_args == 0) {
          usb_send_text(
              "Syntax: ++temp <type> [config]\r\n"
              "0 PT1000\r\n"
              "1 Thermistor [0..11]\r\n"
              "2 Type K [0=2Wire, 1=4Wire]\r\n");
          return;
        }

        char* next;
        long l1 = strtol(p_args, &next, 10);
        long l2 = strtol(next, NULL, 10);

        if (l1 < 0 || l1 >= SENS_MAX_ITEMS) {
          snprintf(scratch.cmd.fmt_buf, sizeof(scratch.cmd.fmt_buf),
                   "ERR: max %d sensor idx\r\n", SENS_MAX_ITEMS - 1);
          usb_send_text(scratch.cmd.fmt_buf);
          return;
        }
        if (l1 == SENS_THERMISTOR) {
          if (l2 < 0 || l2 >= NTC_MAX_ITEMS) {
            snprintf(scratch.cmd.fmt_buf, sizeof(scratch.cmd.fmt_buf),
                     "ERR: max %d preset idx\r\n", NTC_MAX_ITEMS - 1);
            usb_send_text(scratch.cmd.fmt_buf);
            return;
          }
          app.temp_ntc_preset = (ntc_preset_t)l2;
        } else if (l1 == SENS_TYPE_K) {
          if (l2 < 0 || l2 >= WIRE_MAX_ITEMS) {
            snprintf(scratch.cmd.fmt_buf, sizeof(scratch.cmd.fmt_buf),
                     "ERR: 0 = 2W, 1 = 4W mode\r\n");
            usb_send_text(scratch.cmd.fmt_buf);
            return;
          }
          app.temp_wire_mode = (wire_mode_t)l2;
        }

        app.temp_sensor = (temp_sensor_t)l1;
        break;
      }

      case CMD_REL:
        menu_item = MENU_REL;
        break;
      case CMD_XOHM:
        menu_item = MENU_XOHM;
        break;
      case CMD_DBM:
        menu_item = MENU_DBM;
        break;
      case CMD_DIODE:
        menu_item = MENU_DIODE;
        break;
      case CMD_MATH: {
        menu_item = MENU_STATS;
        break;
      }

      default:
        usb_send_text("ERR: Unknown cmd\r\n");
        break;
    }

    // if it was an internal cmd
    if (menu_item != MENU_EXIT) {
      save_dmm_state();  // save before changing
      enter_feature_mode(menu_item);
      usb_send_text("OK\r\n");
    }

    return;
  }

  // passthrough mode (not "++")
  if (gpib_send(sys_cfg.target_addr, p_cmd) < 0) {
    usb_send_text("ERR: Send Fail\r\n");
    return;
  }

  // 2. Auto-Read Logic
  // If Auto == 1: Always Read
  // If Auto == 2: Read only if command ends with '?'
  if (sys_cfg.auto_read == 1 || (sys_cfg.auto_read == 2 && is_query(p_cmd))) {
    goto do_read_operation;
  }

  return;

do_read_operation: {
  int len = gpib_receive(sys_cfg.target_addr, scratch.io.raw_data,
                         sizeof(scratch.io.raw_data));
  if (len > 0) {
    usb_send_text(scratch.io.raw_data);
  } else {
    // 'no-data' state or EOI
    // maybe send nothing or a newline?
  }
}
}

static void dmm_loop(void) {
  uint32_t now = millis();

  if (app.tone_timer_pending) {
    if ((now - app.tone_start_ts) >= app.tone_duration) {
      buzzer_hw_set(0);
      app.tone_timer_pending = false;
    }
  }

  // Passthrough
  if (app.current_mode == MODE_PASSTHROUGH) {
    int srq_asserted = gpib_check_srq();
    int time_to_poll = (now - app.last_poll_time) >= sys_cfg.poll_interval_ms;

    // if disconnected, we only try to reconnect on timer ticks
    if (!app.dmm_online && !time_to_poll) return;
    // if online, we poll if SRQ is pulled OR timer expires
    if (app.dmm_online && !srq_asserted && !time_to_poll) return;

    app.last_poll_time = now;

    uint8_t stb = 0;
    // try to talk to DMM
    int poll_result = gpib_serial_poll(sys_cfg.dmm_addr, &stb);

    if (poll_result != 0) {
      // poll failed (Timeout/NACK)
      if (app.dmm_online) {
        // printf("DMM Lost connection.\n");
        app.dmm_online = false;
      }
      // slow down polling when offline
      sys_cfg.poll_interval_ms = sys_cfg.dmm_recovery_delay_ms;
      return;
    }

    // got a valid response, check if this is a recovery
    if (!app.dmm_online) {
      // printf("DMM Recovered.\n");
      // only assert REN here when it's online
      gpib_remote_enable(1);
      gpib_interface_clear();

      app.dmm_online = true;
#if defined(BUZZER_ONLINE_TUNE) && BUZZER_ONLINE_TUNE
      play_tune(ONLINE_NOTES);
#endif
      gpib_send(sys_cfg.dmm_addr,
                HP3478A_CMD_MASK_BTN_ONLY HP3478A_CMD_SRQ_CLEAR);
      gpib_go_to_local(sys_cfg.dmm_addr);
      sys_cfg.poll_interval_ms =
          sys_cfg.poll_interval_ms;  // restore fast polling
      return;
    }

    // valid and online, check buttons
    if (stb & HP3478A_MASK_KEYBOARD_SRQ &&
        (now - app.ignore_input_start_ts) > sys_cfg.menu_lockout_ms) {
      enter_menu_mode();
    }

    return;
  }

  // Nav
  if (app.current_mode == MODE_MENU) {
    handle_menu_navigation();
    return;
  }

  // Features
  // early exit if no SRQ
  if (!gpib_check_srq()) {
    return;
  }

  uint8_t stb;
  if (gpib_serial_poll(sys_cfg.dmm_addr, &stb) != 0) {
    // DMM crashed during feature mode
    // printf("Feature crash: DMM Lost\n");
    app.current_mode = MODE_PASSTHROUGH;
    app.dmm_online = false;
    gpib_interface_clear();
    return;
  }

  // check exit button first (priority)
  if (stb & HP3478A_MASK_KEYBOARD_SRQ) {
    exit_to_passthrough();
    return;
  }

  // handle measurement data ready
  if (stb & HP3478A_MASK_DATA_READY) {
    handle_feature_logic();
  }
}

int main() {
  SystemInit();
  systick_init();
  funGpioInitAll();

  config_init();
  buzzer_init();
  i2c_init();
  if (aht20_init() == AHT20_OK) {
    app.env_sensor_present = true;
    printf("[INIT] AHT20 Sensor OK\n");
  } else {
    app.env_sensor_present = false;
    printf("[INIT] AHT20 Sensor MISSING\n");
  }

  gpib_init();
  USBFSSetup();
  // usb_debug = 1;

  // app state
  app.usb_raw_prev = USB_HW_IS_ACTIVE();
  config_apply_to_app();
  // init timers
  uint32_t now = millis();
  app.usb_ts = now;
  app.ignore_input_start_ts = now - 2000;
  app.last_poll_time = 0;
  app.env_last_read = 0;

  while (1) {
    handle_usb_state();
    usb_process_tx();
    if (app.dmm_loop) {
      dmm_loop();
    }
    handle_env_sensor();

    if (app.usb_online) {
      process_command();
    }
  }
}
