/*******************************************************************************
 * @file    dr_piezo.c
 * @brief   Piezo Transducer Driver (2MHz Signal Generator)
 * @author  Charles KWON
 * @date    2025-12-09
 *
 * @details Uses Timer2 + GPIOTE + PPI for CPU-free 2MHz waveform generation
 *
 *          Timing Diagram (TX sequence):
 *
 *                   |<----------- PE HIGH ----------->|
 *          PE    ___/-------------------------------------\___
 *          P_OUT ___/-\_/-\_/-\_/-\_/-\________________\___
 *          N_OUT ___\_/-\_/-\_/-\_/-\_/________________\___
 *          DMP   _________________________/-----\_________
 *                   |<-- 3~5 cycles -->|  |<DMP>|
 *
 *          P_OUT and N_OUT toggle in anti-phase (complementary)
 *
 *          2MHz = 500ns period = 250ns half-period
 *          Timer @ 16MHz: 1 tick = 62.5ns
 *          Half-period = 250ns = 4 ticks
 *          Full-period = 500ns = 8 ticks
 ******************************************************************************/

/*==============================================================================
 * Module overview — Piezo ultrasound transducer driver
 *
 * Generates 2 MHz ultrasound TX signals for bladder measurement using
 * nRF52840 hardware peripherals (Timer2 + GPIOTE + PPI) for CPU-free
 * precise waveform generation.
 *
 * TX sequence:
 *   1. PE = HIGH  -> enable MOSFET driver
 *   2. P_OUT / N_OUT alternating pulses (2 MHz, 3..7 cycles)
 *      P_OUT and N_OUT are anti-phase, applying +/-20V across the piezo
 *   3. DMP = HIGH -> dump residual piezo energy
 *   4. DMP = LOW  -> discharge complete
 *   5. PE = LOW   -> return to idle
 *
 * HW architecture:
 *   Timer2 CC[0] (half-period = 4 ticks) -> PPI CH8,9  -> GPIOTE CH4,5 toggle
 *   Timer2 CC[1] (full-period = 8 ticks) -> PPI CH10,11 -> GPIOTE CH4,5 toggle
 *   Timer2 CC[2] (full-period = 8 ticks) -> auto-clear + IRQ (cycle counter)
 *
 * Power: DR_PIEZO_PWR_EN (P1.9) -> DC/DC -> +/-20V
 *
 * SW burst mode (dr_piezo_burst_sw_* family):
 *   Direct GPIO via port register (NRF_P1->OUT), interrupts disabled,
 *   NOP-based timing. Per-frequency NOP count differs.
 *   NOP timing: CPU 64 MHz -> 1 NOP = 15.625 ns
 *   half_period_ns = 1 000 000 / (freq_MHz * 2)
 *   NOP_count = (half_period - register_write ~30 ns) / 15.625
 *   loop overhead (~47 ns = 3 NOP) subtracted from second half-period
 *============================================================================*/

#include "dr_piezo.h"
#include "nrf_gpio.h"
#include "nrf_timer.h"
#include "nrf_gpiote.h"
#include "nrf_ppi.h"
#include "nrf_delay.h"
#include "nrfx_timer.h"
#include "nrf_pwr_mgmt.h"
#include "power_control.h"
#include "app_util_platform.h"

/* Debug output: enabled when FEATURE_PRINTF is defined */
#ifdef FEATURE_PRINTF
#include "debug_print.h"
#else
#define DBG_PRINTF(...)
#endif

/*==============================================================================
 * Hardware resource allocation
 *   Timer2: 16 MHz clock, time base for 2 MHz waveform
 *   GPIOTE CH4/5: P_OUT / N_OUT hardware toggle
 *   PPI CH8..11: timer compare events -> GPIOTE toggle tasks
 *============================================================================*/
#define PIEZO_TIMER             NRF_TIMER2
#define PIEZO_TIMER_IRQn        TIMER2_IRQn
#define PIEZO_TIMER_IRQ_PRIORITY 6

#define GPIOTE_CH_P_OUT         4   /* P_OUT toggle */
#define GPIOTE_CH_N_OUT         5   /* N_OUT toggle */

#define PPI_CH_P_OUT_TOGGLE_0   8   /* CC[0] (half-period) -> P_OUT */
#define PPI_CH_N_OUT_TOGGLE_0   9   /* CC[0] (half-period) -> N_OUT */
#define PPI_CH_P_OUT_TOGGLE_1   10  /* CC[1] (full-period) -> P_OUT */
#define PPI_CH_N_OUT_TOGGLE_1   11  /* CC[1] (full-period) -> N_OUT */

/*==============================================================================
 * Timing constants
 *   Timer2 @ 16 MHz: 1 tick = 62.5 ns
 *   2 MHz signal: period 500 ns (8 ticks), half-period 250 ns (4 ticks)
 *============================================================================*/
#define TIMER_FREQ_MHZ          16
#define TICK_NS                 (1000 / TIMER_FREQ_MHZ)  /* 62.5 ns */

/* 2 MHz reference timing: period = 500 ns, half-period = 250 ns */
#define PERIOD_TICKS_2MHZ       8       /* full period: 500 ns / 62.5 ns = 8 ticks */
#define HALF_PERIOD_TICKS       4       /* half period: 250 ns / 62.5 ns = 4 ticks */

/*==============================================================================
 * SW burst port register bitmasks
 *
 * Pre-calculated from pin definitions in dr_piezo.h.
 * Used for direct writes to NRF_P1->OUT, enabling simultaneous multi-pin control.
 * PE is on P0 port, controlled separately via OUTSET/OUTCLR.
 *============================================================================*/
/* Extract bit position within port from pin number (lower 5 bits = 0~31) */
#define PIN_NUM(pin)    ((pin) & 0x1F)

#define P_OUT_MASK      (1UL << PIN_NUM(DR_PIEZO_PIN_P_OUT))   /* P1.07 positive output */
#define N_OUT_MASK      (1UL << PIN_NUM(DR_PIEZO_PIN_N_OUT))   /* P1.06 negative output */
#define PE_MASK         (1UL << PIN_NUM(DR_PIEZO_PIN_PE))      /* P0.25 pulse enable */
#define DMP_MASK        (1UL << PIN_NUM(DR_PIEZO_PIN_DMP))     /* P1.00 dump control */

/* Combined mask of piezo control pins on P1 port (excludes channel select pins) */
#define P1_CTRL_MASK    (P_OUT_MASK | N_OUT_MASK | DMP_MASK)

/*==============================================================================
 * Static variables
 *============================================================================*/
static volatile bool     m_tx_active = false;           /* TX in progress (modified in IRQ) */
static volatile uint8_t  m_remaining_cycles = 0;        /* remaining pulse cycles (decremented in IRQ) */
static uint32_t          m_period_ticks = PERIOD_TICKS_2MHZ; /* current period (timer ticks) */
static bool              m_power_enabled = false;       /* DC/DC converter state */
static bool              m_initialized = false;         /* driver initialised flag */
static nrfx_timer_t      m_mux_settle_timer = NRFX_TIMER_INSTANCE(4);
static volatile bool     m_mux_settle_done = true;
static bool              m_mux_settle_timer_ready = false;

/*==============================================================================
 * Timer2 IRQ handler
 *
 * Called every period (CC[2]) to decrement the remaining cycle count.
 * When zero:
 *   1) Stop and clear timer
 *   2) Disable GPIOTE (stop P_OUT/N_OUT toggling)
 *   3) Reconfigure GPIOs as push-pull output, drive LOW
 *   4) Generate DMP pulse (discharge residual piezo energy)
 *   5) PE = LOW (disable MOSFET driver)
 *============================================================================*/
void TIMER2_IRQHandler(void)
{
    if (nrf_timer_event_check(PIEZO_TIMER, NRF_TIMER_EVENT_COMPARE2))
    {
        nrf_timer_event_clear(PIEZO_TIMER, NRF_TIMER_EVENT_COMPARE2);
        
        if (m_remaining_cycles > 0)
        {
            m_remaining_cycles--;
        }
        
        if (m_remaining_cycles == 0)
        {
            /* Step 1: Stop timer */
            nrf_timer_task_trigger(PIEZO_TIMER, NRF_TIMER_TASK_STOP);
            nrf_timer_task_trigger(PIEZO_TIMER, NRF_TIMER_TASK_CLEAR);
            
            /* Step 2: Disable GPIOTE (return GPIO to SW control) */
            nrf_gpiote_task_disable(GPIOTE_CH_P_OUT);
            nrf_gpiote_task_disable(GPIOTE_CH_N_OUT);
            
            /* Step 3: Reconfigure GPIOs and set idle state */
            nrf_gpio_cfg_output(DR_PIEZO_PIN_P_OUT);
            nrf_gpio_cfg_output(DR_PIEZO_PIN_N_OUT);
            nrf_gpio_pin_clear(DR_PIEZO_PIN_P_OUT);
            nrf_gpio_pin_clear(DR_PIEZO_PIN_N_OUT);
            
            /* Step 4: DMP pulse */
            nrf_gpio_pin_set(DR_PIEZO_PIN_DMP);
            __NOP(); __NOP(); __NOP(); __NOP();
            __NOP(); __NOP(); __NOP(); __NOP();
            __NOP(); __NOP(); __NOP(); __NOP();
            __NOP(); __NOP(); __NOP(); __NOP();
            nrf_gpio_pin_clear(DR_PIEZO_PIN_DMP);
            
            /* Step 5: PE = LOW */
            nrf_gpio_pin_clear(DR_PIEZO_PIN_PE);
            
            m_tx_active = false;
        }
    }
}

/*==============================================================================
 * Power control
 * Switches the DC/DC converter (+/-20V) for piezo driving voltage.
 * ~10 ms needed for power stabilisation.
 *============================================================================*/
void dr_piezo_power_on(void)
{
    //nrf_delay_ms(20);
    
    nrf_gpio_cfg_output(DR_PIEZO_PWR_EN);
    nrf_gpio_pin_set(DR_PIEZO_PWR_EN);
    
    /* Wait for power stabilization */
    nrf_delay_ms(10);
    
    m_power_enabled = true;
    
    //DBG_PRINTF("[PIEZO] TX/RX Active: +/-20V ready\r\n");
}

/* Power OFF: disable TX -> disable MUX -> shut down DC/DC */
void dr_piezo_power_off(void)
{
    dr_piezo_disable();

    /* clear MUX enable pins left HIGH by select_channel() */
    nrf_gpio_pin_clear(DR_PIEZO_EN_MUXA);
    nrf_gpio_pin_clear(DR_PIEZO_EN_MUXB);
    nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL0);
    nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL1);

    nrf_gpio_pin_clear(DR_PIEZO_PWR_EN);

    m_power_enabled = false;

    //DBG_PRINTF("[PIEZO] Power OFF\r\n");
}

/* Check piezo power state */
bool dr_piezo_is_power_on(void)
{
    return m_power_enabled;
}

/*==============================================================================
 * Internal (private) initialisation
 *============================================================================*/

/* Configure all signal pins as push-pull output, drive LOW (idle) */
static void dr_piezo_gpio_init(void)
{
    nrf_gpio_cfg_output(DR_PIEZO_PIN_PE);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_DMP);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_P_OUT);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_N_OUT);

    /* All LOW = idle */
    nrf_gpio_pin_clear(DR_PIEZO_PIN_PE);
    nrf_gpio_pin_clear(DR_PIEZO_PIN_DMP);
    nrf_gpio_pin_clear(DR_PIEZO_PIN_P_OUT);
    nrf_gpio_pin_clear(DR_PIEZO_PIN_N_OUT);

    /* Initialize MUX control pins */
    dr_piezo_mux_init();
}

/* GPIOTE init: configure P_OUT/N_OUT in toggle mode (PPI targets) */
static void dr_piezo_gpiote_init(void)
{
    /* P_OUT: Toggle mode, initial LOW */
    nrf_gpiote_task_configure(
        GPIOTE_CH_P_OUT,
        DR_PIEZO_PIN_P_OUT,
        NRF_GPIOTE_POLARITY_TOGGLE,
        NRF_GPIOTE_INITIAL_VALUE_LOW
    );
    nrf_gpiote_task_enable(GPIOTE_CH_P_OUT);
    
    /* N_OUT: Toggle mode, initial LOW */
    nrf_gpiote_task_configure(
        GPIOTE_CH_N_OUT,
        DR_PIEZO_PIN_N_OUT,
        NRF_GPIOTE_POLARITY_TOGGLE,
        NRF_GPIOTE_INITIAL_VALUE_LOW
    );
    nrf_gpiote_task_enable(GPIOTE_CH_N_OUT);
}

/*
 * Timer init: 16 MHz clock, 16-bit mode
 * CC[0] = half-period (4 ticks): toggle event at half-period
 * CC[1] = full-period (8 ticks): toggle event at full-period
 * CC[2] = full-period (8 ticks): IRQ + auto-clear (SHORT)
 */
static void dr_piezo_timer_init(void)
{
    nrf_timer_task_trigger(PIEZO_TIMER, NRF_TIMER_TASK_STOP);
    nrf_timer_task_trigger(PIEZO_TIMER, NRF_TIMER_TASK_CLEAR);
    
    nrf_timer_mode_set(PIEZO_TIMER, NRF_TIMER_MODE_TIMER);
    nrf_timer_bit_width_set(PIEZO_TIMER, NRF_TIMER_BIT_WIDTH_16);
    nrf_timer_frequency_set(PIEZO_TIMER, NRF_TIMER_FREQ_16MHz);
    
    /*
     * Compare channel assignment:
     * - CC[0] = 4 (half period): toggle P_OUT, toggle N_OUT
     * - CC[1] = 8 (full period): toggle P_OUT, toggle N_OUT
     * - CC[2] = 8 (full period): cycle count IRQ + CLEAR
     *
     * Initial state: P=HIGH, N=LOW
     * t=4:  1st toggle -> P=LOW, N=HIGH
     * t=8:  2nd toggle -> P=HIGH, N=LOW (+ CLEAR)
     * t=12: 1st toggle -> P=LOW, N=HIGH
     * ...
     */
    
    /* CC[0]: half-period toggle */
    nrf_timer_cc_write(PIEZO_TIMER, NRF_TIMER_CC_CHANNEL0, HALF_PERIOD_TICKS);  // 4
    
    /* CC[1]: full-period toggle */
    nrf_timer_cc_write(PIEZO_TIMER, NRF_TIMER_CC_CHANNEL1, m_period_ticks);     // 8
    
    /* CC[2]: full-period - cycle count IRQ source */
    nrf_timer_cc_write(PIEZO_TIMER, NRF_TIMER_CC_CHANNEL2, m_period_ticks);     // 8
    
    /* Auto-CLEAR on CC[2] match */
    nrf_timer_shorts_enable(PIEZO_TIMER, NRF_TIMER_SHORT_COMPARE2_CLEAR_MASK);
    
    /* CC[2] interrupt enable (cycle counter) */
    nrf_timer_int_enable(PIEZO_TIMER, NRF_TIMER_INT_COMPARE2_MASK);
    
    NVIC_SetPriority(PIEZO_TIMER_IRQn, PIEZO_TIMER_IRQ_PRIORITY);
    NVIC_EnableIRQ(PIEZO_TIMER_IRQn);
}

/*
 * PPI init: connect timer compare events to GPIOTE toggle tasks (4 channels)
 * CC[0] event (half-period) -> P_OUT toggle + N_OUT toggle
 * CC[1] event (full-period) -> P_OUT toggle + N_OUT toggle
 * This ensures P_OUT and N_OUT always operate in anti-phase.
 */
static void dr_piezo_ppi_init(void)
{
    /*
     * Connection map:
     * CC[0] (t=4): P_OUT toggle, N_OUT toggle
     * CC[1] (t=8): P_OUT toggle, N_OUT toggle
     */
    
    /* CC[0] -> P_OUT toggle */
    nrf_ppi_channel_endpoint_setup(
        (nrf_ppi_channel_t)PPI_CH_P_OUT_TOGGLE_0,
        (uint32_t)nrf_timer_event_address_get(PIEZO_TIMER, NRF_TIMER_EVENT_COMPARE0),
        (uint32_t)&NRF_GPIOTE->TASKS_OUT[GPIOTE_CH_P_OUT]
    );
    nrf_ppi_channel_enable((nrf_ppi_channel_t)PPI_CH_P_OUT_TOGGLE_0);
    
    /* CC[0] -> N_OUT toggle */
    nrf_ppi_channel_endpoint_setup(
        (nrf_ppi_channel_t)PPI_CH_N_OUT_TOGGLE_0,
        (uint32_t)nrf_timer_event_address_get(PIEZO_TIMER, NRF_TIMER_EVENT_COMPARE0),
        (uint32_t)&NRF_GPIOTE->TASKS_OUT[GPIOTE_CH_N_OUT]
    );
    nrf_ppi_channel_enable((nrf_ppi_channel_t)PPI_CH_N_OUT_TOGGLE_0);
    
    /* CC[1] -> P_OUT toggle */
    nrf_ppi_channel_endpoint_setup(
        (nrf_ppi_channel_t)PPI_CH_P_OUT_TOGGLE_1,
        (uint32_t)nrf_timer_event_address_get(PIEZO_TIMER, NRF_TIMER_EVENT_COMPARE1),
        (uint32_t)&NRF_GPIOTE->TASKS_OUT[GPIOTE_CH_P_OUT]
    );
    nrf_ppi_channel_enable((nrf_ppi_channel_t)PPI_CH_P_OUT_TOGGLE_1);
    
    /* CC[1] -> N_OUT toggle */
    nrf_ppi_channel_endpoint_setup(
        (nrf_ppi_channel_t)PPI_CH_N_OUT_TOGGLE_1,
        (uint32_t)nrf_timer_event_address_get(PIEZO_TIMER, NRF_TIMER_EVENT_COMPARE1),
        (uint32_t)&NRF_GPIOTE->TASKS_OUT[GPIOTE_CH_N_OUT]
    );
    nrf_ppi_channel_enable((nrf_ppi_channel_t)PPI_CH_N_OUT_TOGGLE_1);
}

static void dr_piezo_mux_settle_timeout_handler(nrf_timer_event_t event_type, void *p_context)
{
    (void)p_context;

    if (event_type != NRF_TIMER_EVENT_COMPARE0)
    {
        return;
    }

    nrfx_timer_disable(&m_mux_settle_timer);
    m_mux_settle_done = true;
}

static void dr_piezo_mux_timer_init(void)
{
    nrfx_err_t err;
    nrfx_timer_config_t config = NRFX_TIMER_DEFAULT_CONFIG;

    config.frequency = NRF_TIMER_FREQ_1MHz;
    config.mode = NRF_TIMER_MODE_TIMER;
    config.bit_width = NRF_TIMER_BIT_WIDTH_16;
    config.interrupt_priority = APP_IRQ_PRIORITY_LOWEST;

    err = nrfx_timer_init(&m_mux_settle_timer, &config, dr_piezo_mux_settle_timeout_handler);
    if (err == NRFX_SUCCESS)
    {
        m_mux_settle_timer_ready = true;
    }
    else
    {
        m_mux_settle_timer_ready = false;
        DBG_PRINTF("[DR_PIEZO] MUX settle timer init failed: %lu\r\n", (unsigned long)err);
    }
}

static void dr_piezo_mux_timer_start(void)
{
    if (!m_mux_settle_timer_ready)
    {
        nrf_delay_us(DR_PIEZO_MUX_SETTLING_US);
        m_mux_settle_done = true;
        return;
    }

    m_mux_settle_done = false;
    nrfx_timer_disable(&m_mux_settle_timer);
    nrfx_timer_clear(&m_mux_settle_timer);
    nrfx_timer_extended_compare(
        &m_mux_settle_timer,
        NRF_TIMER_CC_CHANNEL0,
        nrfx_timer_us_to_ticks(&m_mux_settle_timer, DR_PIEZO_MUX_SETTLING_US),
        NRF_TIMER_SHORT_COMPARE0_CLEAR_MASK,
        true
    );
    nrfx_timer_enable(&m_mux_settle_timer);
}

/*==============================================================================
 * TX driver public functions
 *============================================================================*/

/* TX driver init: configure GPIO -> GPIOTE -> Timer -> PPI in order */
void dr_piezo_init(void)
{
    dr_piezo_gpio_init();
    dr_piezo_gpiote_init();
    dr_piezo_timer_init();
    dr_piezo_ppi_init();
    dr_piezo_mux_timer_init();
    
    m_tx_active = false;
    m_remaining_cycles = 0;
    m_initialized = true;
}

/* TX driver deinit: stop timer, disable PPI/GPIOTE, all pins LOW */
void dr_piezo_uninit(void)
{
    nrf_timer_task_trigger(PIEZO_TIMER, NRF_TIMER_TASK_STOP);
    nrf_timer_int_disable(PIEZO_TIMER, NRF_TIMER_INT_COMPARE2_MASK);
    NVIC_DisableIRQ(PIEZO_TIMER_IRQn);
    
    nrf_ppi_channel_disable((nrf_ppi_channel_t)PPI_CH_P_OUT_TOGGLE_0);
    nrf_ppi_channel_disable((nrf_ppi_channel_t)PPI_CH_N_OUT_TOGGLE_0);
    nrf_ppi_channel_disable((nrf_ppi_channel_t)PPI_CH_P_OUT_TOGGLE_1);
    nrf_ppi_channel_disable((nrf_ppi_channel_t)PPI_CH_N_OUT_TOGGLE_1);
    
    nrf_gpiote_task_disable(GPIOTE_CH_P_OUT);
    nrf_gpiote_task_disable(GPIOTE_CH_N_OUT);
    
    nrf_gpio_pin_clear(DR_PIEZO_PIN_PE);
    nrf_gpio_pin_clear(DR_PIEZO_PIN_DMP);
    nrf_gpio_pin_clear(DR_PIEZO_PIN_P_OUT);
    nrf_gpio_pin_clear(DR_PIEZO_PIN_N_OUT);

    if (m_mux_settle_timer_ready)
    {
        nrfx_timer_disable(&m_mux_settle_timer);
        nrfx_timer_uninit(&m_mux_settle_timer);
        m_mux_settle_timer_ready = false;
    }
    
    m_tx_active = false;
    m_initialized = false;
}

/*
 * Hardware-based burst transmit (using Timer + PPI + GPIOTE)
 * 1) Reconfigure GPIOTE: P_OUT starts HIGH, N_OUT starts LOW (anti-phase)
 * 2) PE = HIGH -> enable MOSFET driver
 * 3) Start timer -> PPI automatically toggles P_OUT/N_OUT
 * 4) IRQ handler manages remaining cycles and shutdown sequence
 */
void dr_piezo_burst(uint8_t cycles)
{
    if (m_tx_active)
    {
        DBG_PRINTF("[DR_PIEZO] TX busy!\r\n");
        return;
    }
    
    if (!m_initialized)
    {
        DBG_PRINTF("[DR_PIEZO] ERROR: Not initialized!\r\n");
        return;
    }
    
    if (cycles < DR_PIEZO_MIN_CYCLES) cycles = DR_PIEZO_MIN_CYCLES;
    if (cycles > DR_PIEZO_MAX_CYCLES) cycles = DR_PIEZO_MAX_CYCLES;
    
    DBG_PRINTF("[DR_PIEZO] Burst: %d cycles\r\n", cycles);
    
    m_remaining_cycles = cycles;
    m_tx_active = true;
    
    /* Reconfigure GPIOTE */
    nrf_gpiote_task_disable(GPIOTE_CH_P_OUT);
    nrf_gpiote_task_disable(GPIOTE_CH_N_OUT);
    
    nrf_gpiote_task_configure(
        GPIOTE_CH_P_OUT,
        DR_PIEZO_PIN_P_OUT,
        NRF_GPIOTE_POLARITY_TOGGLE,
        NRF_GPIOTE_INITIAL_VALUE_HIGH   /* P_OUT starts HIGH */
    );
    nrf_gpiote_task_enable(GPIOTE_CH_P_OUT);
    
    nrf_gpiote_task_configure(
        GPIOTE_CH_N_OUT,
        DR_PIEZO_PIN_N_OUT,
        NRF_GPIOTE_POLARITY_TOGGLE,
        NRF_GPIOTE_INITIAL_VALUE_LOW    /* N_OUT starts LOW */
    );
    nrf_gpiote_task_enable(GPIOTE_CH_N_OUT);
    
    /* DMP = LOW */
    nrf_gpio_pin_clear(DR_PIEZO_PIN_DMP);
    
    /* Timer clear */
    nrf_timer_task_trigger(PIEZO_TIMER, NRF_TIMER_TASK_CLEAR);
    
    /* Clear pending events */
    nrf_timer_event_clear(PIEZO_TIMER, NRF_TIMER_EVENT_COMPARE0);
    nrf_timer_event_clear(PIEZO_TIMER, NRF_TIMER_EVENT_COMPARE1);
    nrf_timer_event_clear(PIEZO_TIMER, NRF_TIMER_EVENT_COMPARE2);
    
    /* PE = HIGH (enable driver) */
    nrf_gpio_pin_set(DR_PIEZO_PIN_PE);
    
    /* Settling delay before starting timer */
    __NOP(); __NOP(); __NOP(); __NOP();
    
    /* Timer START -> PPI automatically toggles P_OUT/N_OUT */
    nrf_timer_task_trigger(PIEZO_TIMER, NRF_TIMER_TASK_START);
}

/* Burst transmit with default cycle count (5) */
void dr_piezo_pulse(void)
{
    dr_piezo_burst(DR_PIEZO_DEFAULT_CYCLES);
}

/* Enable TX output: manually set initial state (PE=HIGH, P_OUT=HIGH, N_OUT=LOW) */
void dr_piezo_enable(void)
{
    nrf_gpio_pin_set(DR_PIEZO_PIN_P_OUT);
    nrf_gpio_pin_clear(DR_PIEZO_PIN_N_OUT);
    nrf_gpio_pin_clear(DR_PIEZO_PIN_DMP);
    nrf_gpio_pin_set(DR_PIEZO_PIN_PE);
    DBG_PRINTF("[DR_PIEZO] TX enabled\r\n");
}

/* Disable TX output: all signal pins return to LOW (idle) */
void dr_piezo_disable(void)
{
    nrf_gpio_pin_clear(DR_PIEZO_PIN_DMP);
    nrf_gpio_pin_clear(DR_PIEZO_PIN_PE);
    nrf_gpio_pin_clear(DR_PIEZO_PIN_P_OUT);
    nrf_gpio_pin_clear(DR_PIEZO_PIN_N_OUT);
}

/* Check if TX is in progress (cleared to false in IRQ handler) */
bool dr_piezo_is_busy(void)
{
    return m_tx_active;
}

/* Change operating frequency (100kHz~4MHz): reconfigure timer CC registers */
void dr_piezo_set_frequency(uint32_t freq_hz)
{
    if (freq_hz < 100000 || freq_hz > 4000000)
    {
        DBG_PRINTF("[DR_PIEZO] Invalid freq: %d Hz\r\n", freq_hz);
        return;
    }
    
    uint32_t period_ns = 1000000000UL / freq_hz;
    m_period_ticks = period_ns * TIMER_FREQ_MHZ / 1000;
    
    nrf_timer_cc_write(PIEZO_TIMER, NRF_TIMER_CC_CHANNEL0, m_period_ticks / 2);
    nrf_timer_cc_write(PIEZO_TIMER, NRF_TIMER_CC_CHANNEL1, m_period_ticks);
    nrf_timer_cc_write(PIEZO_TIMER, NRF_TIMER_CC_CHANNEL2, m_period_ticks);
    
    DBG_PRINTF("[DR_PIEZO] Freq: %d Hz (period=%d ticks)\r\n", freq_hz, m_period_ticks);
}

/*==============================================================================
 * MUX control functions
 * 8-channel analog MUX selects piezo echo signal path.
 * MUXA: CH0~CH3, MUXB: CH4~CH7
 * SEL0, SEL1: MUX internal channel address selection
 * High Drive (H0H1) mode: strong output drive for fast MUX IC switching
 *============================================================================*/

/* MUX control pin init: all 4 pins as High Drive output, default LOW */
void dr_piezo_mux_init(void)
{
    /* Configure pins as output with high drive strength */
    nrf_gpio_cfg(
        DR_PIEZO_MUX_SEL0,
        NRF_GPIO_PIN_DIR_OUTPUT,
        NRF_GPIO_PIN_INPUT_DISCONNECT,
        NRF_GPIO_PIN_NOPULL,
        NRF_GPIO_PIN_H0H1,  /* High drive */
        NRF_GPIO_PIN_NOSENSE
    );

    nrf_gpio_cfg(
        DR_PIEZO_MUX_SEL1,
        NRF_GPIO_PIN_DIR_OUTPUT,
        NRF_GPIO_PIN_INPUT_DISCONNECT,
        NRF_GPIO_PIN_NOPULL,
        NRF_GPIO_PIN_H0H1,  /* High drive */
        NRF_GPIO_PIN_NOSENSE
    );

    /* GPIO PIN Setting */
    nrf_gpio_cfg(
        DR_PIEZO_EN_MUXA,                  // PIN
        NRF_GPIO_PIN_DIR_OUTPUT,           // DIR : OUTPUT
        NRF_GPIO_PIN_INPUT_DISCONNECT,     // INPUT BUFFER X (DIR : OUTPUT)
        NRF_GPIO_PIN_NOPULL,               // PULL UP, PULL DOWN X
        NRF_GPIO_PIN_H0H1,                 // HIGH DRIVE(STRONG OUTPUT) !!
        NRF_GPIO_PIN_NOSENSE               // INTERRUPT X
    );

    nrf_gpio_cfg(
        DR_PIEZO_EN_MUXB, 
        NRF_GPIO_PIN_DIR_OUTPUT, 
        NRF_GPIO_PIN_INPUT_DISCONNECT,
        NRF_GPIO_PIN_NOPULL,
        NRF_GPIO_PIN_H0H1,
        NRF_GPIO_PIN_NOSENSE
    );

    nrf_gpio_pin_clear(DR_PIEZO_EN_MUXA);     /* P0.21 = LOW, Select Channel -> HIGH */
    nrf_gpio_pin_clear(DR_PIEZO_EN_MUXB);     /* P0.23 = LOW, Select Channel -> HIGH */
    nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL0);    /* P1.10 = LOW */
    nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL1);    /* P0.28 = LOW */
}


/*
 * Select piezo channel (0~7)
 * Channel mapping (EN_MUXA, EN_MUXB, SEL0, SEL1):
 *   CH0 = MUXA input0 (1,0,0,0)   CH4 = MUXB input0 (0,1,0,0)
 *   CH1 = MUXA input2 (1,0,1,0)   CH5 = MUXB input1 (0,1,1,0)
 *   CH2 = MUXA input1 (1,0,0,1)
 *   CH3 = MUXA input3 (1,0,1,1)
 * MUX settling time (~1.3ms) required after channel switch.
 */
void dr_piezo_select_channel_start(uint8_t channel)
{
    channel = channel & 0x07;  /* Mask to 0~7 range */

    switch (channel)
    {
        case 0: // A0: EN_MUXA=1, EN_MUXB=0, SEL0=0, SEL1=0
            nrf_gpio_pin_clear(DR_PIEZO_EN_MUXB); nrf_gpio_pin_set(DR_PIEZO_EN_MUXA);
            nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL0); nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL1);
            break;
        case 1: // A1: EN_MUXA=1, EN_MUXB=0, SEL0=1, SEL1=0
            nrf_gpio_pin_clear(DR_PIEZO_EN_MUXB); nrf_gpio_pin_set(DR_PIEZO_EN_MUXA);
            nrf_gpio_pin_set(DR_PIEZO_MUX_SEL0);   nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL1);
            break;
        case 2: // A2: EN_MUXA=1, EN_MUXB=0, SEL0=0, SEL1=1 
            nrf_gpio_pin_clear(DR_PIEZO_EN_MUXB); nrf_gpio_pin_set(DR_PIEZO_EN_MUXA);
            nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL0); nrf_gpio_pin_set(DR_PIEZO_MUX_SEL1);
            break;
        case 3: // A3: EN_MUXA=1, EN_MUXB=0, SEL0=1, SEL1=1
            nrf_gpio_pin_clear(DR_PIEZO_EN_MUXB); nrf_gpio_pin_set(DR_PIEZO_EN_MUXA);
            nrf_gpio_pin_set(DR_PIEZO_MUX_SEL0);   nrf_gpio_pin_set(DR_PIEZO_MUX_SEL1);
            break;
        case 4: // B0: EN_MUXA=0, EN_MUXB=1, SEL0=0, SEL1=0
            nrf_gpio_pin_clear(DR_PIEZO_EN_MUXA); nrf_gpio_pin_set(DR_PIEZO_EN_MUXB);
            nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL0); nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL1);
            break;
        case 5: // B1: EN_MUXA=0, EN_MUXB=1, SEL0=1, SEL1=0
            nrf_gpio_pin_clear(DR_PIEZO_EN_MUXA); nrf_gpio_pin_set(DR_PIEZO_EN_MUXB);
            nrf_gpio_pin_set(DR_PIEZO_MUX_SEL0); nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL1);
            break;
    }

    /* Start one-shot timer for MUX settling (> 1.2ms). */
    dr_piezo_mux_timer_start();
}

void dr_piezo_wait_mux_settled(void)
{
    while (!m_mux_settle_done)
    {
        nrf_pwr_mgmt_run();
    }
}

bool dr_piezo_is_mux_settled(void)
{
    return m_mux_settle_done;
}

void dr_piezo_select_channel(uint8_t channel)
{
    dr_piezo_select_channel_start(channel);
    dr_piezo_wait_mux_settled();
}

/* Pin test: toggle each signal pin HIGH/LOW in sequence (for oscilloscope verification) */
void dr_piezo_test_pins(void)
{
    DBG_PRINTF("[DR_PIEZO] Pin test...\r\n");
    
    nrf_gpiote_task_disable(GPIOTE_CH_P_OUT);
    nrf_gpiote_task_disable(GPIOTE_CH_N_OUT);
    
    nrf_gpio_cfg_output(DR_PIEZO_PIN_PE);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_DMP);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_P_OUT);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_N_OUT);
    
    DBG_PRINTF("  PE (P1.05)...\r\n");
    nrf_gpio_pin_set(DR_PIEZO_PIN_PE);
    nrf_delay_ms(100);
    nrf_gpio_pin_clear(DR_PIEZO_PIN_PE);
    nrf_delay_ms(100);
    
    DBG_PRINTF("  DMP (P1.09)...\r\n");
    nrf_gpio_pin_set(DR_PIEZO_PIN_DMP);
    nrf_delay_ms(100);
    nrf_gpio_pin_clear(DR_PIEZO_PIN_DMP);
    nrf_delay_ms(100);
    
    DBG_PRINTF("  P_OUT (P1.03)...\r\n");
    nrf_gpio_pin_set(DR_PIEZO_PIN_P_OUT);
    nrf_delay_ms(100);
    nrf_gpio_pin_clear(DR_PIEZO_PIN_P_OUT);
    nrf_delay_ms(100);
    
    DBG_PRINTF("  N_OUT (P1.02)...\r\n");
    nrf_gpio_pin_set(DR_PIEZO_PIN_N_OUT);
    nrf_delay_ms(100);
    nrf_gpio_pin_clear(DR_PIEZO_PIN_N_OUT);
    nrf_delay_ms(100);
    
    DBG_PRINTF("[DR_PIEZO] Pin test done\r\n");
}

/*==============================================================================
 * System functions (power + TX driver unified control)
 *============================================================================*/

/* Full system init: TX driver init -> power ON (call once at boot) */
void dr_piezo_system_init(void)
{
    dr_piezo_init();      /* Set GPIO/GPIOTE/Timer/PPI to safe state first */
    dr_piezo_power_on();  /* Then apply DC/DC power */
}

/* Full system shutdown: deinit TX driver -> power OFF */
void dr_piezo_system_uninit(void)
{
    dr_piezo_uninit();
    dr_piezo_power_off();
}

/* Burst transmit with power check (blocking: waits until TX complete) */
void dr_piezo_transmit(uint8_t cycles)
{
    if (!m_power_enabled)
    {
        DBG_PRINTF("[DR_PIEZO] ERROR: Power not enabled!\r\n");
        return;
    }
    
    dr_piezo_burst(cycles);
    
    while (dr_piezo_is_busy()) { }
}

/*==============================================================================
 * Software-based burst mode
 * 2025-12-11 Charles KWON
 *==============================================================================
 *
 * Generates ultrasound pulses by directly manipulating GPIO registers from
 * the CPU, instead of using Timer/PPI/GPIOTE hardware. Interrupts are
 * disabled to guarantee precise timing.
 *
 * === Timing diagram ===
 *
 *          |<-margin->|<---- pulses ----->|<-- DMP -->|<-margin->|
 *
 * PE    ___/--------------------------------------------------\___
 * P_OUT ___________/-\_/-\_/-\_/-\_/-\____________________\_______
 * N_OUT ___________\_/-\_/-\_/-\_/-\_/____________________\_______
 * DMP   __________________________________/----------\____________
 *
 * === Signal description ===
 * - PE (Pulse Enable): activation signal wrapping the entire sequence (with margins)
 * - P_OUT: positive output, toggles at 2MHz in anti-phase with N_OUT
 * - N_OUT: negative output, toggles at 2MHz in anti-phase with P_OUT
 * - DMP (Dump): discharges residual piezo energy after pulse completion
 *
 * === Operating principle ===
 * 1) __disable_irq() blocks interrupts -> prevents timing jitter
 * 2) PE ON (via P0.25 OUTSET register -> no effect on other P0 pins)
 * 3) NOP margin, then for-loop alternates P_OUT/N_OUT
 * 4) DMP pulse (32 NOP ~ 500ns)
 * 5) PE OFF, then __enable_irq() restores interrupts
 *
 * === Direct port register access ===
 * Pre-calculated bitmasks are written directly to NRF_P1->OUT register.
 * This allows simultaneous control of P_OUT, N_OUT, DMP while preserving
 * channel select pins (MUX SEL) via P1_CTRL_MASK (only control pins change).
 * PE is on P0 port, so it is controlled separately via OUTSET/OUTCLR registers.
 *============================================================================*/
/*
 * Software burst - default frequency 2.1MHz
 *
 * NOP timing calculation:
 *   2.1MHz -> period 476ns, half-period 238ns
 *   CPU 64MHz -> 1 NOP = 15.625ns
 *   1st half: 14 NOP (~219ns) + register write (~30ns) = ~249ns
 *   2nd half: 9 NOP (~141ns) + loop overhead (~47ns) + register write (~30ns) = ~218ns
 *   Total: ~436-476ns (~2.1MHz)
 */
void dr_piezo_burst_sw(uint8_t cycles)
{
    /* Clamp cycles to valid range (1-20) */
    if (cycles < 1) cycles = 1;
    if (cycles > 20) cycles = 20;

    /* RTT: snapshot then single print */
    uint32_t _d0 = NRF_P1->OUT;

    /* Disable GPIOTE hardware control to prevent conflicts */
    nrf_gpiote_task_disable(GPIOTE_CH_P_OUT);
    nrf_gpiote_task_disable(GPIOTE_CH_N_OUT);

    uint32_t _d1 = NRF_P1->OUT;

    /* Save ENTIRE P1 port state BEFORE any GPIO config */
    uint32_t saved_p1_out = NRF_P1->OUT;
    uint32_t saved_p0_out = NRF_P0->OUT;

    /* Configure all signal pins as outputs */
    nrf_gpio_cfg_output(DR_PIEZO_PIN_PE);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_DMP);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_P_OUT);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_N_OUT);

    uint32_t _d2 = NRF_P1->OUT;

    //DBG_PRINTF("[B]S0:%u%u%u\r\n", (_d0>>10)&1, (_d1>>10)&1, (_d2>>10)&1);

    /* Restore P1 port state (nrf_gpio_cfg_output may clear bits) */
    NRF_P1->OUT = saved_p1_out;
    NRF_P0->OUT = (NRF_P0->OUT & ~PE_MASK) | (saved_p0_out & PE_MASK);
    
    /*--------------------------------------------------------------------------
     * Pre-calculate all output states for fast switching
     * Each state represents a specific combination of pin levels
     * IMPORTANT: Channel select pins (P1.11, P1.12) are preserved in all states
     *------------------------------------------------------------------------*/

    // PE uses OUTSET/OUTCLR (prevents affecting other P0 pins)

    uint32_t p1_all_low      = saved_p1_out & ~P1_CTRL_MASK;
    uint32_t p1_P_high_N_low = p1_all_low | P_OUT_MASK;
    uint32_t p1_P_low_N_high = p1_all_low | N_OUT_MASK;
    uint32_t p1_DMP_high     = p1_all_low | DMP_MASK;
    
    /*--------------------------------------------------------------------------
     * Critical timing section - interrupts disabled
     *------------------------------------------------------------------------*/
    __disable_irq();
    
    /* Initialize: Set all signals to LOW */
    NRF_P0->OUTCLR = PE_MASK;   // PE OFF (OUTCLR does not affect other pins)
    NRF_P1->OUT = p1_all_low;

    /* PE rises first with margin before pulses start */
    NRF_P0->OUTSET = PE_MASK;   // PE ON (OUTSET does not affect other pins)

    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    
            
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */


    
    /*--------------------------------------------------------------------------
     * Generate 2.1MHz pulse burst (hardcoded NOP timing for stability)
     *
     * 2.1 MHz: 476ns period, 238ns half-period
     * At 64MHz CPU (1 NOP = 15.625ns):
     *   First half:  13 NOPs (~203ns) + register write (~30ns) = ~233ns
     *   Second half: 10 NOPs (~156ns) + loop overhead (~47ns) = ~203ns
     *   Total: ~436-476ns per cycle
     *------------------------------------------------------------------------*/
    for (uint8_t i = 0; i < cycles; i++)
    {
        /* First half-period: P_OUT=HIGH, N_OUT=LOW */
        NRF_P1->OUT = p1_P_high_N_low;
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();__NOP();
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP(); __NOP();


        /* Second half-period: P_OUT=LOW, N_OUT=HIGH */
        NRF_P1->OUT = p1_P_low_N_high;
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP(); __NOP(); __NOP();
			
    }
    
    /*--------------------------------------------------------------------------
     * DMP (Dump) pulse
     * - Starts simultaneously with N_OUT falling edge
     * - Duration: ~500ns (32 NOPs)
     *------------------------------------------------------------------------*/

    NRF_P1->OUT = p1_DMP_high;
    
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    
    /* DMP falls, PE remains HIGH for margin period */
    NRF_P1->OUT = p1_all_low;
    __NOP(); __NOP(); __NOP();  /* ~47ns margin */

    NRF_P0->OUTCLR = PE_MASK;   // PE OFF

    __enable_irq();
}

/**
 * @brief Software-based burst at 1.8 MHz
 * @param cycles Number of cycles (1~20)
 *
 * Timing:
 *   1.8 MHz: 556ns period, 278ns half-period
 *   At 64MHz CPU (1 NOP = 15.625ns):
 *     First half:  15 NOPs (~234ns)
 *     Second half: 12 NOPs (~188ns) + loop overhead
 */
/*
 * Software burst - 1.8MHz
 * NOP timing: half-period 278ns
 *   1st half: 17 NOP (~266ns) + register write (~30ns) = ~296ns
 *   2nd half: 11 NOP (~172ns) + loop overhead (~47ns) + register write (~30ns) = ~249ns
 *   Total: ~499-556ns (~1.8MHz)
 */
void dr_piezo_burst_sw_18mhz(uint8_t cycles)
{
    /* Clamp cycles to valid range (1-20) */
    if (cycles < 1) cycles = 1;
    if (cycles > 20) cycles = 20;

    /* Disable GPIOTE hardware control to prevent conflicts */
    nrf_gpiote_task_disable(GPIOTE_CH_P_OUT);
    nrf_gpiote_task_disable(GPIOTE_CH_N_OUT);

    /* Save port states BEFORE any GPIO config */
    uint32_t saved_p1_out = NRF_P1->OUT;
    uint32_t saved_p0_out = NRF_P0->OUT;

    /* Configure all signal pins as outputs */
    nrf_gpio_cfg_output(DR_PIEZO_PIN_PE);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_DMP);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_P_OUT);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_N_OUT);

    /* Restore port states (nrf_gpio_cfg_output may clear bits) */
    NRF_P1->OUT = saved_p1_out;
    NRF_P0->OUT = (NRF_P0->OUT & ~PE_MASK) | (saved_p0_out & PE_MASK);

    //NRF_P0->OUT = saved_p0_out;

    /* Pre-calculate P1 output states (PE on P0.25 - controlled via OUTSET/OUTCLR) */
    uint32_t p1_all_low      = saved_p1_out & ~P1_CTRL_MASK;
    uint32_t p1_P_high_N_low = p1_all_low | P_OUT_MASK;
    uint32_t p1_P_low_N_high = p1_all_low | N_OUT_MASK;
    uint32_t p1_DMP_high     = p1_all_low | DMP_MASK;

    __disable_irq();

    /* Initialize: Set all signals to LOW */
    NRF_P0->OUTCLR = PE_MASK;
    NRF_P1->OUT = p1_all_low;

    /* PE rises first with margin before pulses start */
    NRF_P0->OUTSET = PE_MASK;
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();

    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();

    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();

    /*--------------------------------------------------------------------------
     * Generate 1.8MHz pulse burst
     *
     * 1.8 MHz: 556ns period, 278ns half-period
     * At 64MHz CPU (1 NOP = 15.625ns):
     *   First half:  15 NOPs (~234ns) + register write (~30ns) = ~264ns
     *   Second half: 12 NOPs (~188ns) + loop overhead (~47ns) = ~235ns
     *   Total: ~499-556ns per cycle
     *------------------------------------------------------------------------*/
    for (uint8_t i = 0; i < cycles; i++)
    {
        /* First half-period: P_OUT=HIGH, N_OUT=LOW */
        NRF_P1->OUT = p1_P_high_N_low;
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP();

        /* Second half-period: P_OUT=LOW, N_OUT=HIGH */
        NRF_P1->OUT = p1_P_low_N_high;
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); 
    }

    /* DMP (Dump) pulse */
    NRF_P1->OUT = p1_DMP_high;

    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();

    /* DMP falls, PE remains HIGH for margin period */
    NRF_P1->OUT = p1_all_low;
    __NOP(); __NOP(); __NOP();

    /* End of sequence: PE OFF */
    NRF_P0->OUTCLR = PE_MASK;

    __enable_irq();
}

/**
 * @brief Software-based burst at 2.0 MHz
 * @param cycles Number of cycles (1~20)
 *
 * Timing:
 *   2.0 MHz: 500ns period, 250ns half-period
 *   At 64MHz CPU (1 NOP = 15.625ns):
 *     First half:  14 NOPs (~219ns) + register write (~30ns) = ~249ns
 *     Second half: 11 NOPs (~172ns) + loop overhead (~47ns) = ~219ns
 *     Total: ~468-500ns per cycle
 */
/*
 * Software burst - 2.0MHz
 * NOP timing: half-period 250ns
 *   1st half: 15 NOP (~234ns) + register write (~30ns) = ~264ns
 *   2nd half: 10 NOP (~156ns) + loop overhead (~47ns) + register write (~30ns) = ~233ns
 *   Total: ~468-500ns (~2.0MHz)
 */
void dr_piezo_burst_sw_20mhz(uint8_t cycles)
{
    /* Clamp cycles to valid range (1-20) */
    if (cycles < 1) cycles = 1;
    if (cycles > 20) cycles = 20;

    /* Disable GPIOTE hardware control to prevent conflicts */
    nrf_gpiote_task_disable(GPIOTE_CH_P_OUT);
    nrf_gpiote_task_disable(GPIOTE_CH_N_OUT);

    /* Save port states BEFORE any GPIO config */
    uint32_t saved_p1_out = NRF_P1->OUT;
    uint32_t saved_p0_out = NRF_P0->OUT;

    /* Configure all signal pins as outputs */
    nrf_gpio_cfg_output(DR_PIEZO_PIN_PE);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_DMP);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_P_OUT);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_N_OUT);

    /* Restore port states (nrf_gpio_cfg_output may clear bits) */
    NRF_P1->OUT = saved_p1_out;
    //NRF_P0->OUT = saved_p0_out;
    NRF_P0->OUT = (NRF_P0->OUT & ~PE_MASK) | (saved_p0_out & PE_MASK);

    /* Pre-calculate P1 output states (PE on P0.25 - controlled via OUTSET/OUTCLR) */
    uint32_t p1_all_low      = saved_p1_out & ~P1_CTRL_MASK;
    uint32_t p1_P_high_N_low = p1_all_low | P_OUT_MASK;
    uint32_t p1_P_low_N_high = p1_all_low | N_OUT_MASK;
    uint32_t p1_DMP_high     = p1_all_low | DMP_MASK;

    __disable_irq();

    /* Initialize: Set all signals to LOW */
    NRF_P0->OUTCLR = PE_MASK;
    NRF_P1->OUT = p1_all_low;

    /* PE rises first with margin before pulses start */
    NRF_P0->OUTSET = PE_MASK;
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();

    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();

    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();

    /*--------------------------------------------------------------------------
     * Generate 2.0MHz pulse burst
     *
     * 2.0 MHz: 500ns period, 250ns half-period
     * At 64MHz CPU (1 NOP = 15.625ns):
     *   First half:  14 NOPs (~219ns) + register write (~30ns) = ~249ns
     *   Second half: 11 NOPs (~172ns) + loop overhead (~47ns) = ~219ns
     *   Total: ~468-500ns per cycle
     *------------------------------------------------------------------------*/
    for (uint8_t i = 0; i < cycles; i++)
    {
        /* First half-period: P_OUT=HIGH, N_OUT=LOW */
        NRF_P1->OUT = p1_P_high_N_low;
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP(); __NOP(); __NOP();
			  __NOP();

        /* Second half-period: P_OUT=LOW, N_OUT=HIGH */
        NRF_P1->OUT = p1_P_low_N_high;
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        
    }

    /* DMP (Dump) pulse */
    NRF_P1->OUT = p1_DMP_high;

    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();

    /* DMP falls, PE remains HIGH for margin period */
    NRF_P1->OUT = p1_all_low;
    __NOP(); __NOP(); __NOP();

    /* End of sequence: PE OFF */
    NRF_P0->OUTCLR = PE_MASK;

    __enable_irq();
}



/**
 * @brief Software-based burst at 2.0 MHz
 * @param cycles Number of cycles (1~20)
 *
 * Timing:
 *   1.9 MHz: 500ns period, 250ns half-period
 *   At 64MHz CPU (1 NOP = 15.625ns):
 *     First half:  14 NOPs (~219ns) + register write (~30ns) = ~249ns
 *     Second half: 11 NOPs (~172ns) + loop overhead (~47ns) = ~219ns
 *     Total: ~468-500ns per cycle
 */
/*
 * Software burst - 1.9MHz
 * NOP timing: half-period 263ns
 *   1st half: 15 NOP (~234ns) + register write (~30ns) = ~264ns
 *   2nd half: 9 NOP (~141ns) + loop overhead (~47ns) + register write (~30ns) = ~218ns
 *   Total: ~468-500ns (~1.9MHz)
 */
void dr_piezo_burst_sw_19mhz(uint8_t cycles)
{
    /* Clamp cycles to valid range (1-20) */
    if (cycles < 1) cycles = 1;
    if (cycles > 20) cycles = 20;

    /* Disable GPIOTE hardware control to prevent conflicts */
    nrf_gpiote_task_disable(GPIOTE_CH_P_OUT);
    nrf_gpiote_task_disable(GPIOTE_CH_N_OUT);

    /* Save port states BEFORE any GPIO config */
    uint32_t saved_p1_out = NRF_P1->OUT;
    uint32_t saved_p0_out = NRF_P0->OUT;

    /* Configure all signal pins as outputs */
    nrf_gpio_cfg_output(DR_PIEZO_PIN_PE);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_DMP);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_P_OUT);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_N_OUT);

    /* Restore port states (nrf_gpio_cfg_output may clear bits) */
    NRF_P1->OUT = saved_p1_out;
    //NRF_P0->OUT = saved_p0_out;
    NRF_P0->OUT = (NRF_P0->OUT & ~PE_MASK) | (saved_p0_out & PE_MASK);

    /* Pre-calculate P1 output states (PE on P0.25 - controlled via OUTSET/OUTCLR) */
    uint32_t p1_all_low      = saved_p1_out & ~P1_CTRL_MASK;
    uint32_t p1_P_high_N_low = p1_all_low | P_OUT_MASK;
    uint32_t p1_P_low_N_high = p1_all_low | N_OUT_MASK;
    uint32_t p1_DMP_high     = p1_all_low | DMP_MASK;

    __disable_irq();

    /* Initialize: Set all signals to LOW */
    NRF_P0->OUTCLR = PE_MASK;
    NRF_P1->OUT = p1_all_low;

    /* PE rises first with margin before pulses start */
    NRF_P0->OUTSET = PE_MASK;
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();

    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();

    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();

    /*--------------------------------------------------------------------------
     * Generate 2.0MHz pulse burst
     *
     * 2.0 MHz: 500ns period, 250ns half-period
     * At 64MHz CPU (1 NOP = 15.625ns):
     *   First half:  14 NOPs (~219ns) + register write (~30ns) = ~249ns
     *   Second half: 11 NOPs (~172ns) + loop overhead (~47ns) = ~219ns
     *   Total: ~468-500ns per cycle
     *------------------------------------------------------------------------*/
    for (uint8_t i = 0; i < cycles; i++)
    {
        /* First half-period: P_OUT=HIGH, N_OUT=LOW */
        NRF_P1->OUT = p1_P_high_N_low;
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP(); __NOP(); __NOP();
			  __NOP();

        /* Second half-period: P_OUT=LOW, N_OUT=HIGH */
        NRF_P1->OUT = p1_P_low_N_high;
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP(); __NOP(); __NOP(); 
        
    }

    /* DMP (Dump) pulse */
    NRF_P1->OUT = p1_DMP_high;

    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();

    /* DMP falls, PE remains HIGH for margin period */
    NRF_P1->OUT = p1_all_low;
    __NOP(); __NOP(); __NOP();

    /* End of sequence: PE OFF */
    NRF_P0->OUTCLR = PE_MASK;

    __enable_irq();
}





/**
 * @brief Software-based burst at 2.2 MHz
 * @param cycles Number of cycles (1~20)
 *
 * Timing:
 *   2.2 MHz: 454ns period, 227ns half-period
 *   At 64MHz CPU (1 NOP = 15.625ns):
 *     First half:  13 NOPs (~203ns) + register write (~30ns) = ~233ns
 *     Second half: 11 NOPs (~172ns) + loop overhead (~47ns) = ~219ns
 *     Total: ~452ns per cycle (~2.21 MHz)
 */
/*
 * Software burst - 2.2MHz
 * NOP timing: half-period 227ns
 *   1st half: 13 NOP (~203ns) + register write (~30ns) = ~233ns
 *   2nd half: 8 NOP (~125ns) + loop overhead (~47ns) + register write (~30ns) = ~202ns
 *   Total: ~435-454ns (~2.2MHz)
 */
void dr_piezo_burst_sw_22mhz(uint8_t cycles)
{
    /* Clamp cycles to valid range (1-20) */
    if (cycles < 1) cycles = 1;
    if (cycles > 20) cycles = 20;

    /* Disable GPIOTE hardware control to prevent conflicts */
    nrf_gpiote_task_disable(GPIOTE_CH_P_OUT);
    nrf_gpiote_task_disable(GPIOTE_CH_N_OUT);

    /* Save port states BEFORE any GPIO config */
    uint32_t saved_p1_out = NRF_P1->OUT;
    uint32_t saved_p0_out = NRF_P0->OUT;

    /* Configure all signal pins as outputs */
    nrf_gpio_cfg_output(DR_PIEZO_PIN_PE);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_DMP);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_P_OUT);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_N_OUT);

    /* Restore port states (nrf_gpio_cfg_output may clear bits) */
    NRF_P1->OUT = saved_p1_out;
    NRF_P0->OUT = (NRF_P0->OUT & ~PE_MASK) | (saved_p0_out & PE_MASK);

    /* Pre-calculate P1 output states */
    uint32_t p1_all_low      = saved_p1_out & ~P1_CTRL_MASK;
    uint32_t p1_P_high_N_low = p1_all_low | P_OUT_MASK;
    uint32_t p1_P_low_N_high = p1_all_low | N_OUT_MASK;
    uint32_t p1_DMP_high     = p1_all_low | DMP_MASK;

    __disable_irq();

    /* Initialize: Set all signals to LOW */
    NRF_P0->OUTCLR = PE_MASK;
    NRF_P1->OUT = p1_all_low;

    /* PE rises first with margin before pulses start */
    NRF_P0->OUTSET = PE_MASK;
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();

    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();

    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();

    /*--------------------------------------------------------------------------
     * Generate 2.2MHz pulse burst
     *
     * 2.2 MHz: 454ns period, 227ns half-period
     * At 64MHz CPU (1 NOP = 15.625ns):
     *   First half:  11 NOPs - positive pulse width
     *   Second half: 10 NOPs - negative pulse width
     *   Total: 21 NOPs per cycle (~2.2 MHz)
     *------------------------------------------------------------------------*/
    for (uint8_t i = 0; i < cycles; i++)
    {
        /* First half-period: P_OUT=HIGH, N_OUT=LOW  (positive pulse width) */
        NRF_P1->OUT = p1_P_high_N_low;
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP();
			  __NOP(); 

        /* Second half-period: P_OUT=LOW, N_OUT=HIGH  (negative pulse width) */
        NRF_P1->OUT = p1_P_low_N_high;
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP(); __NOP(); 
    }

    /* DMP (Dump) pulse */
    NRF_P1->OUT = p1_DMP_high;

    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();

    /* DMP falls, PE remains HIGH for margin period */
    NRF_P1->OUT = p1_all_low;
    __NOP(); __NOP(); __NOP();

    /* End of sequence: PE OFF */
    NRF_P0->OUTCLR = PE_MASK;

    __enable_irq();
}

/**
 * @brief Software-based burst at 1.7 MHz
 * @param cycles Number of cycles (1~20)
 *
 * Timing:
 *   1.7 MHz: 588ns period, 294ns half-period
 *   At 64MHz CPU (1 NOP = 15.625ns):
 *     First half:  17 NOPs (~266ns) + register write (~30ns) = ~296ns
 *     Second half: 14 NOPs (~219ns) + loop overhead (~47ns) = ~266ns
 *     Total: ~532-588ns per cycle
 */
/*
 * Software burst - 1.7MHz
 * NOP timing: half-period 294ns
 *   1st half: 18 NOP (~281ns) + register write (~30ns) = ~311ns
 *   2nd half: 10 NOP (~156ns) + loop overhead (~47ns) + register write (~30ns) = ~233ns
 *   Total: ~532-588ns (~1.7MHz)
 */
void dr_piezo_burst_sw_17mhz(uint8_t cycles)
{
    /* Clamp cycles to valid range (1-20) */
    if (cycles < 1) cycles = 1;
    if (cycles > 20) cycles = 20;

    /* Disable GPIOTE hardware control to prevent conflicts */
    nrf_gpiote_task_disable(GPIOTE_CH_P_OUT);
    nrf_gpiote_task_disable(GPIOTE_CH_N_OUT);

    /* Save port states BEFORE any GPIO config */
    uint32_t saved_p1_out = NRF_P1->OUT;
    uint32_t saved_p0_out = NRF_P0->OUT;

    /* Configure all signal pins as outputs */
    nrf_gpio_cfg_output(DR_PIEZO_PIN_PE);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_DMP);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_P_OUT);
    nrf_gpio_cfg_output(DR_PIEZO_PIN_N_OUT);

    /* Restore port states (nrf_gpio_cfg_output may clear bits) */
    NRF_P1->OUT = saved_p1_out;
    NRF_P0->OUT = (NRF_P0->OUT & ~PE_MASK) | (saved_p0_out & PE_MASK);

    /* Pre-calculate P1 output states (PE on P0 - controlled via OUTSET/OUTCLR) */
    uint32_t p1_all_low      = saved_p1_out & ~P1_CTRL_MASK;
    uint32_t p1_P_high_N_low = p1_all_low | P_OUT_MASK;
    uint32_t p1_P_low_N_high = p1_all_low | N_OUT_MASK;
    uint32_t p1_DMP_high     = p1_all_low | DMP_MASK;

    __disable_irq();

    /* Initialize: Set all signals to LOW */
    NRF_P0->OUTCLR = PE_MASK;   // PE OFF
    NRF_P1->OUT = p1_all_low;

    /* PE rises first with margin before pulses start */
    NRF_P0->OUTSET = PE_MASK;   // PE ON
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();

    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();

    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP();

    /*--------------------------------------------------------------------------
     * Generate 1.7MHz pulse burst
     *
     * 1.7 MHz: 588ns period, 294ns half-period
     * At 64MHz CPU (1 NOP = 15.625ns):
     *   First half:  17 NOPs (~266ns) + register write (~30ns) = ~296ns
     *   Second half: 14 NOPs (~219ns) + loop overhead (~47ns) = ~266ns
     *   Total: ~532-588ns per cycle
     *------------------------------------------------------------------------*/
    for (uint8_t i = 0; i < cycles; i++)
    {
        /* First half-period: P_OUT=HIGH, N_OUT=LOW */
        NRF_P1->OUT = p1_P_high_N_low;
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP();
        __NOP();


        /* Second half-period: P_OUT=LOW, N_OUT=HIGH */
        NRF_P1->OUT = p1_P_low_N_high;
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        __NOP(); __NOP(); __NOP(); __NOP(); __NOP();
        //__NOP(); __NOP(); __NOP(); __NOP();
    }

    /* DMP (Dump) pulse */
    NRF_P1->OUT = p1_DMP_high;

    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();
    __NOP(); __NOP(); __NOP(); __NOP();

    /* DMP falls, PE remains HIGH for margin period */
    NRF_P1->OUT = p1_all_low;
    __NOP(); __NOP(); __NOP();

    /* End of sequence: PE OFF */
    NRF_P0->OUTCLR = PE_MASK;   // PE OFF

    __enable_irq();
}
