/*******************************************************************************
 * @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 (???):
 *          
 *                   |<----------- PE HIGH ----------->|
 *          PE    ___/?????????????????????????????????\___
 *          P_OUT ___/?\_/?\_/?\_/?\_/?\________________\___
 *          N_OUT ___\_/?\_/?\_/?\_/?\_/________________\___
 *          DMP   _________________________/?????\_________
 *                   |<-- 3~5 cycles -->|  |<DMP>|
 *          
 *          P_OUT? N_OUT? ?? ?? ?? (???)
 *          
 *          2MHz = 500ns period = 250ns half-period
 *          Timer @ 16MHz: 1 tick = 62.5ns
 *          Half-period = 250ns = 4 ticks
 *          Full-period = 500ns = 8 ticks
 ******************************************************************************/

#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 "power_control.h"
#include "app_util_platform.h"

#ifdef FEATURE_PRINTF
#include "debug_print.h"
#else
#define DBG_PRINTF(...)
#endif

/*==============================================================================
 * HARDWARE RESOURCE ALLOCATION
 *============================================================================*/
#define PIEZO_TIMER             NRF_TIMER2
#define PIEZO_TIMER_IRQn        TIMER2_IRQn
#define PIEZO_TIMER_IRQ_PRIORITY 6

/* GPIOTE channels */
#define GPIOTE_CH_P_OUT         4
#define GPIOTE_CH_N_OUT         5

/* PPI channels */
#define PPI_CH_P_OUT_TOGGLE_0   8
#define PPI_CH_N_OUT_TOGGLE_0   9
#define PPI_CH_P_OUT_TOGGLE_1   10
#define PPI_CH_N_OUT_TOGGLE_1   11

/*==============================================================================
 * TIMING CONSTANTS
 *============================================================================*/
#define TIMER_FREQ_MHZ          16
#define TICK_NS                 (1000 / TIMER_FREQ_MHZ)  /* 62.5ns */

/* 2MHz: period = 500ns, half = 250ns */
#define PERIOD_TICKS_2MHZ       8       /* 500ns / 62.5ns = 8 */
#define HALF_PERIOD_TICKS       4       /* 250ns / 62.5ns = 4 */

/*==============================================================================
 * PIEZO OPERATING FREQUENCY CONFIGURATION
 *============================================================================*/
/*
 * Target piezo frequency: 1.8 MHz
 *
 * Timing (hardcoded in dr_piezo_burst_sw for stability):
 *   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
 *
 * To change frequency, modify the NOP counts in dr_piezo_burst_sw():
 *   2.0 MHz: First=14, Second=11
 *   2.1 MHz: First=13, Second=10
 *   1.5 MHz: First=18, Second=15
 */
#define PIEZO_FREQ_MHZ          2.1f

/*==============================================================================
 * STATIC VARIABLES
 *============================================================================*/
static volatile bool     m_tx_active = false;
static volatile uint8_t  m_remaining_cycles = 0;
static uint32_t          m_period_ticks = PERIOD_TICKS_2MHZ;
static bool              m_power_enabled = false;
static bool              m_initialized = false;

/*==============================================================================
 * INTERRUPT HANDLER
 *============================================================================*/
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 (GPIO ??? ??) */
            nrf_gpiote_task_disable(GPIOTE_CH_P_OUT);
            nrf_gpiote_task_disable(GPIOTE_CH_N_OUT);
            
            /* Step 3: GPIO? ?? ? idle ?? */
            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 FUNCTIONS
 *============================================================================*/

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("[DR_PIEZO] Power ON: +/-20V ready\r\n");
}

void dr_piezo_power_off(void)
{
    dr_piezo_disable();
    
    nrf_gpio_pin_clear(DR_PIEZO_PWR_EN);
    
    m_power_enabled = false;
    
    DBG_PRINTF("[DR_PIEZO] Power OFF\r\n");
}

/*==============================================================================
 * PRIVATE FUNCTIONS
 *============================================================================*/

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();

    DBG_PRINTF("[DR_PIEZO] GPIO init done\r\n");
}

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);
    
    DBG_PRINTF("[DR_PIEZO] GPIOTE init done\r\n");
}

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);
    
    /*
     * ??? ??? ?? ???:
     * - CC[0] = 4 (half period): P_OUT ??, N_OUT ??
     * - CC[1] = 8 (full period): P_OUT ??, N_OUT ??
     * - CC[2] = 8 (full period): ??? ??? + CLEAR
     * 
     * ?? ??: P=HIGH, N=LOW
     * t=4:  ? ? ?? -> P=LOW, N=HIGH
     * t=8:  ? ? ?? -> P=HIGH, N=LOW (+ CLEAR)
     * t=12: ? ? ?? -> P=LOW, N=HIGH
     * ...
     */
    
    /* CC[0]: ??? ?? */
    nrf_timer_cc_write(PIEZO_TIMER, NRF_TIMER_CC_CHANNEL0, HALF_PERIOD_TICKS);  // 4
    
    /* CC[1]: ? ?? ?? */
    nrf_timer_cc_write(PIEZO_TIMER, NRF_TIMER_CC_CHANNEL1, m_period_ticks);     // 8
    
    /* CC[2]: ? ?? - ??? ???? */
    nrf_timer_cc_write(PIEZO_TIMER, NRF_TIMER_CC_CHANNEL2, m_period_ticks);     // 8
    
    /* CC[2]?? ?? CLEAR */
    nrf_timer_shorts_enable(PIEZO_TIMER, NRF_TIMER_SHORT_COMPARE2_CLEAR_MASK);
    
    /* CC[2] ???? (??? ???) */
    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);
    
    DBG_PRINTF("[DR_PIEZO] Timer init done (period=%d ticks)\r\n", m_period_ticks);
}

static void dr_piezo_ppi_init(void)
{
    /*
     * ??? ??:
     * CC[0] (t=4): P_OUT ??, N_OUT ??
     * CC[1] (t=8): P_OUT ??, N_OUT ??
     */
    
    /* 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);
    
    DBG_PRINTF("[DR_PIEZO] PPI init done (4 channels)\r\n");
}

/*==============================================================================
 * TX DRIVER FUNCTIONS
 *============================================================================*/

void dr_piezo_init(void)
{
    DBG_PRINTF("[DR_PIEZO] Initializing TX driver...\r\n");
    
    dr_piezo_gpio_init();
    dr_piezo_gpiote_init();
    dr_piezo_timer_init();
    dr_piezo_ppi_init();
    
    m_tx_active = false;
    m_remaining_cycles = 0;
    m_initialized = true;
    
    DBG_PRINTF("[DR_PIEZO] TX driver ready (2MHz)\r\n");
}

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);
    
    m_tx_active = false;
    m_initialized = false;
    
    DBG_PRINTF("[DR_PIEZO] TX driver uninitialized\r\n");
}

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;
    
    /* 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 (??? ??) */
    nrf_gpio_pin_set(DR_PIEZO_PIN_PE);
    
    /* ??? ?? ? ??? ?? */
    __NOP(); __NOP(); __NOP(); __NOP();
    
    /* Timer START -> PPI? ???? P_OUT/N_OUT ?? */
    nrf_timer_task_trigger(PIEZO_TIMER, NRF_TIMER_TASK_START);
}

void dr_piezo_pulse(void)
{
    dr_piezo_burst(DR_PIEZO_DEFAULT_CYCLES);
}

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");
}

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);
    DBG_PRINTF("[DR_PIEZO] TX disabled\r\n");
}

bool dr_piezo_is_busy(void)
{
    return m_tx_active;
}

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 FUNCTION
 *============================================================================*/

void dr_piezo_mux_init(void)
{
    /* Configure MUX control pins as outputs */
    /*nrf_gpio_cfg_output(DR_PIEZO_MUX_SEL1);
    nrf_gpio_cfg_output(DR_PIEZO_MUX_SEL2);
    nrf_gpio_cfg_output(DR_PIEZO_MUX_SEL3);*/

    /*nrf_gpio_cfg_output(DR_PIEZO_EN_MUXA);
    nrf_gpio_cfg_output(DR_PIEZO_EN_MUXB);
    nrf_gpio_cfg_output(DR_PIEZO_MUX_SEL0);
    nrf_gpio_cfg_output(DR_PIEZO_MUX_SEL1);*/

    /* 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     jhChun 0129 */
    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
    );

    /* Set MUX selection: P1.13=HIGH, P1.12=LOW, P1.11=LOW */
    //nrf_gpio_pin_set(DR_PIEZO_MUX_SEL1);    /* P1.13 = HIGH */
    //nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL2);  /* P1.12 = LOW */
    //nrf_gpio_pin_clear(DR_PIEZO_MUX_SEL3);  /* P1.11 = LOW */

    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 */

    DBG_PRINTF("[DR_PIEZO] MUX init done\r\n");
}


void dr_piezo_select_channel(uint8_t channel)
{
    channel = channel & 0x07;  /* Mask to 0-7 */

    switch (channel) {
        //           EN_A  EN_B  SEL0  SEL1
        case 0: // A0: 1    0     0     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: // A2: 1    0     1     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: // A1: 1    0     0     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: 1    0     1     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: 0    1     1     1
            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_set(DR_PIEZO_MUX_SEL1);
            break;
        case 5: // B1: 0    1     1     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_set(DR_PIEZO_MUX_SEL1);
            break;
        case 6: // B2: 0    1     0     1
            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;
        case 7: // B3: 0    1     0     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;
    }

    /* Delay for MUX settling (analog path stabilization) */
    nrf_delay_us(DR_PIEZO_MUX_SETTLING_US);
}

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
 *============================================================================*/

void dr_piezo_system_init(void)
{
    DBG_PRINTF("[DR_PIEZO] System init...\r\n");
    dr_piezo_power_on();
    dr_piezo_init();
    DBG_PRINTF("[DR_PIEZO] System ready\r\n");
}

void dr_piezo_system_uninit(void)
{
    dr_piezo_uninit();
    dr_piezo_power_off();
    DBG_PRINTF("[DR_PIEZO] System shutdown\r\n");
}

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 
 * 2025-12-11 Charles KWON
 *==============================================================================
 * 
 * Timing Diagram:
 * 
 *          |<-margin->|<----- pulses ----->|<-- DMP -->|<-margin->|
 * 
 * PE    ___/--------------------------------------------------\___
 * P_OUT ___________/-\_/-\_/-\_/-\_/-\____________________\_______
 * N_OUT ___________\_/-\_/-\_/-\_/-\_/____________________\_______
 * DMP   __________________________________/----------\____________
 * 
 * Signal Description:
 * - PE (Pulse Enable): Wraps entire sequence with margin before and after
 * - P_OUT: Positive output, starts LOW, toggles at 2MHz, opposite phase to N_OUT
 * - N_OUT: Negative output, starts LOW, toggles at 2MHz, opposite phase to P_OUT
 * - DMP (Dump): Activates after pulses complete, simultaneous with N_OUT falling edge
 * 
 * Timing Specifications:
 * - Frequency: 2MHz (500ns period, 250ns half-period)
 * - CPU Clock: 64MHz (1 NOP = 15.625ns)
 * - Duty Cycle: 50:50
 * - DMP Pulse Width: ~500ns
 * - PE Margin: ~3 NOPs before pulses and after DMP
 * 
 * Pin Mapping (Port P1):
 * - P1.02: N_OUT (Negative output)
 * - P1.03: P_OUT (Positive output)
 * - P1.05: PE (Pulse Enable)
 * - P1.09: DMP (Dump control)
 * 
 * Note: Direct register access (NRF_P1->OUT) is used for simultaneous
 *       multi-pin control to ensure precise timing and phase alignment.
 *============================================================================*/
#define PIN_NUM(pin)    ((pin) & 0x1F)


/* Bit masks for P1 port pins - derived from dr_piezo.h definitions
 * 
 * WARNING: Never hardcode pin numbers!
 * Hardcoding may save a developer's time momentarily,
 * but it will also shorten their lifespan
 * - Charles KWON
*/

#define P_OUT_MASK      (1UL << PIN_NUM(DR_PIEZO_PIN_P_OUT))   /* P1.03 -> P1.07 */
#define N_OUT_MASK      (1UL << PIN_NUM(DR_PIEZO_PIN_N_OUT))   /* P1.02 -> P1.06 */
#define PE_MASK         (1UL << PIN_NUM(DR_PIEZO_PIN_PE))      /* P1.05 -> P0.25 */
#define DMP_MASK        (1UL << PIN_NUM(DR_PIEZO_PIN_DMP))     /* P1.09 -> P1.00 */

/* Combined mask for all piezo control signals (excluding channel select) */
#define P1_CTRL_MASK    (P_OUT_MASK | N_OUT_MASK | DMP_MASK)

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는 OUTSET/OUTCLR 사용 (다른 P0 핀 영향 방지)

    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로 다른 핀 영향 없음)
    NRF_P1->OUT = p1_all_low;

    /* PE rises first with margin before pulses start */
    NRF_P0->OUTSET = PE_MASK;   // PE ON (OUTSET로 다른 핀 영향 없음)

    __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
 */
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는 P0.25 - 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
 */
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는 P0.25 - 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
 */
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는 P0.25 - 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)
 */
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
 */
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는 P0에서 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();
}

