/**
 * @file mr16.c
 * @brief MR16 Module Implementation - SX1281 Radio Control with FreeRTOS CLI
 * 
 * This module provides comprehensive control for the MR16 radio system using
 * the SX1281 2.4GHz transceiver. Features include:
 * 
 * - Multi-protocol support: BLE, LoRa, GFSK, FLRC
 * - Flash configuration persistence
 * - FreeRTOS CLI interface for real-time control
 * - Parameter validation and error handling
 * - LCD status display integration
 * - Event-driven callback system
 * 
 * @author MR16 Development Team
 * @date 2024
 * @version 1.0
 * 
 * Hardware Requirements:
 * - STM32F103C8T6 MCU (20KB RAM, 64KB Flash)
 * - SX1281 2.4GHz Radio Transceiver
 * - LCD Display Module
 * - UART2 for CLI Communication
 * 
 * Memory Usage:
 * - Static CLI buffers: 320 bytes (64+256)
 * - Help text storage: ~4KB (const)
 * - Radio parameter structures: ~200 bytes
 */

/* Includes ----------------------------------------------------------------- */
#include "module/mr16.h"
#include "module/config.h"

/* STM32 HAL Headers */
#include <main.h>
#include "usart.h"

#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h> /* for strtoul, strtol */

/* Device Driver Headers */
#include "bsp/flash.h"
#include "device/sx1281_driver/sx1281.h"
#include "device/sx1281_driver/radio.h"
#include "device/sx1281_driver/sx1281_driver.h"
#include "device/lcd_driver/lcd.h"

/* FreeRTOS Headers */
#include "component/FreeRTOS_CLI.h"

/* Private defines --------------------------------------------------------- */

/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* MR16 finite state machine states */
typedef enum {
  MR16_FSM_NONE,
  MR16_FSM_INIT,
  MR16_FSM_TX,
  MR16_FSM_RX,
} MR16_FSM_t;

/* Private variables -------------------------------------------------------- */
/* Radio instance and configuration */
SX1281_t radio;

/* External references */
extern const unsigned char logo_M[];           /* LCD logo bitmap */
extern uint8_t uart2_data[255], uart2_datalength; /* UART data buffer */
extern MR16_t mr16;                            /* MR16 main instance */

/* Radio communication buffers */
uint8_t radioRXBuffer[BUFFER_SIZE];
uint8_t radioRXSize;

/* State machines */
static MR16_FSM_t MR16_FSM = MR16_FSM_NONE;

/* UART notification flag */
bool UART2_IdleFlag = false;

/* CLI buffers - Static allocation to prevent stack overflow */
static char cInputBuffer[64];   /* CLI input buffer */
static char cOutputBuffer[256]; /* CLI output buffer */

/* Private function prototypes ---------------------------------------------- */
int8_t MR16_CLI_Init(void);
static BaseType_t mr16CommandHandler(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
static BaseType_t radioCommandHandler(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
static BaseType_t outputHelpText(char *pcWriteBuffer, size_t xWriteBufferLen, 
                                 const char *helpText, size_t *position);

/* Helper Functions ------------------------------------------------------- */
/**
 * @brief Output help text in chunks to prevent buffer overflow
 * @param pcWriteBuffer Output buffer
 * @param xWriteBufferLen Buffer length
 * @param helpText Help text to output
 * @param position Current position in help text
 * @return pdTRUE if more output pending, pdFALSE if complete
 */
static BaseType_t outputHelpText(char *pcWriteBuffer, size_t xWriteBufferLen, 
                                 const char *helpText, size_t *position) {
    size_t help_len = strlen(helpText);
    
    if (*position >= help_len) {
        *position = 0;
        return pdFALSE;
    }
    
    size_t chunk = (xWriteBufferLen > 1) ? (xWriteBufferLen - 1) : 0;
    size_t remain = help_len - *position;
    size_t to_copy = (remain < chunk) ? remain : chunk;
    
    if (to_copy > 0) {
        memcpy(pcWriteBuffer, helpText + *position, to_copy);
        pcWriteBuffer[to_copy] = '\0';
        *position += to_copy;
        return (*position < help_len) ? pdTRUE : pdFALSE;
    }
    
    *position = 0;
    return pdFALSE;
}

/* SX1281 Radio Event Callback ----------------------------------------------- */
int8_t SX1281_Callback(SX1281_States_t source) {
  switch (source) {
  case RX_DONE: 
    printf("\r\n[RX] >>> Packet Received <<<\r\n");

    /* Extract payload from radio buffer */
    radioRXSize = 0;
    Radio.GetPayload(radioRXBuffer, &radioRXSize, BUFFER_SIZE);
    if (radioRXSize < BUFFER_SIZE) {
      radioRXBuffer[radioRXSize] = 0;  /* Null terminate */
    } else {
      radioRXBuffer[BUFFER_SIZE - 1] = 0;
    }

    /* Parse header and update packet counters */
    uint16_t RXheader = 0;
    if (radioRXSize >= 2) {
      RXheader = ((uint16_t)radioRXBuffer[1] << 8) | (uint16_t)radioRXBuffer[0];
    }
    for (int i = 0; i < 3; i++) {
      if (RXheader == mr16.param->RX_ID[i]) {
        mr16.packetCount[i]++;
        break;
      }
    }

    /* Display reception statistics */
    uint16_t totalCount = mr16.packetCount[0] + mr16.packetCount[1] + mr16.packetCount[2];
    printf("[RX] Header: 0x%04X | Size: %d bytes | Count: %d\r\n", 
         RXheader, radioRXSize, totalCount);
    break;
  

    case RX_TIMEOUT:
        /* 打印状态 */
        printf( "\r\n[RX] ... Timeout (Restarting RX)\r\n" );
		SX1281_SetRXSuccessive(&radio);
      break;

    case RX_ERROR:
        /* 打印状态 */
        printf( "\r\n[RX] !!! ERROR - Reception Failed !!!\r\n" );
        Radio.SetRx( ( TickTime_t ) { RADIO_TICK_SIZE_1000_US, 0x1000 } );
      break;

    case TX_DONE:
        /* 打印状态 */
        printf( "\r\n[TX] <<< Packet Sent Successfully >>>\r\n" );
        printf( "[TX] Length: %d bytes\r\n", mr16.txbuffer[2]+5 ); 
//		radio.param.packetParams.Params.Flrc.PayloadLength = data_length;	
//		Radio.SetPacketParams( &radio.param.packetParams );
        // a++;if (a>100) a=0;
        // uint8_t haha[5]={a,2,3,4,a};
        // SX1281_SetTX(&radio,(uint8_t*)haha,5);
        SX1281_SetTX(&radio,mr16.txbuffer,mr16.txbuffer[2]+5);
		// SX1281_SetTX(uart_data,data_length);
      break;

    case TX_TIMEOUT:
        /* 打印状态 */
        printf( "\r\n[TX] !!! TIMEOUT - Transmission Failed !!!\r\n" ); 
        
      break;

    case LORA_CAD_DONE:
        /* 打印状态 */
        printf( "\r\n[CAD] Channel Activity Detected\r\n" );
      break;
    default:
      break;
  }
  return 0;  
}

/* Exported functions ------------------------------------------------------- */
int8_t MR16_UI_PowerON() {
  LCD_DrawBitmap(logo_M,180,80,64,64,CRIMSON,MSB);
  return 0;
}

int8_t MR16_UI_Home() {
  return 0;
}

int8_t MR16_UI_Setting() {
  return 0;
}

int8_t MR16_Init(MR16_t *mr16, MR16_Param_t *param, MR16_Mode_t mr16Mode, SX1281_RadioMode_t radioMode) {
    if (!mr16 || !param) {
        return -1; /* Invalid parameters */
    }
    
    /* Smart config loading: try Flash first, fallback to defaults */
    if (!Config_LoadMR16FromFlashBool()) {
        /* Flash invalid or corrupt, use safe defaults */
        Config_SetDefaults();
        printf("[MR16] Flash config invalid, using defaults\r\n");
    } else {
        printf("[MR16] Config loaded from Flash successfully\r\n");
    }
    
    /* Initialize CLI system */
    MR16_CLI_Init();
    
    /* Initialize MR16 parameters - config should be loaded before calling this function */
    mr16->param = &Config_Get()->mr16;
    
    /* Initialize SX1281 radio using persistent parameters */
    if (SX1281_Init(&radio, &mr16->param->radioParams, radioMode) != DEVICE_OK) {
        return -1; /* Radio initialization failed */
    }
    
    /* Start continuous RX mode */
    SX1281_SetRXSuccessive(&radio);
    
    /* Initialize LCD display */
    LCD_Init(1);
    LCD_Clear(BLACK);
    LCD_DrawString(0, 0, "SX1281", MEDIUMORCHID, 32, LSB);
    LCD_DrawBitmap(logo_M, 180, 80, 64, 64, CRIMSON, MSB);
    
    return 0; /* Success */
}

void MR16_NotifyUARTIdle(void) {
    UART2_IdleFlag = true;
}

int8_t MR16_Main(MR16_t *mr16) {
    /* Handle CLI commands from UART */
    if (UART2_IdleFlag) {
        UART2_IdleFlag = false;
        
        /* Copy UART data to input buffer */
        if (uart2_datalength > 0 && uart2_datalength < sizeof(cInputBuffer)) {
            memcpy(cInputBuffer, uart2_data, uart2_datalength);
            cInputBuffer[uart2_datalength] = '\0';
        } else {
            cInputBuffer[0] = '\0';
        }
        
        /* Process CLI command */
        cOutputBuffer[0] = '\0';
        BaseType_t xMore = pdTRUE;
        do {
            xMore = FreeRTOS_CLIProcessCommand(cInputBuffer, cOutputBuffer, sizeof(cOutputBuffer));
            if (cOutputBuffer[0] != '\0') {
                printf("%s", cOutputBuffer);
            }
            cOutputBuffer[0] = '\0';
        } while (xMore == pdTRUE);
    }

    /* Handle MR16 mode operations */
    switch (mr16->param->mode) {
        case MR16_MODE_RFUART:
            /* RF UART mode - handle RF to UART bridging */
            break;
            
        case MR16_MODE_RC:
            /* Remote control mode */
            switch (radio.appMode) {
                case APPMODE_TX:
                    SX1281_Running(&radio);
                    SX1281_SetTX(&radio, mr16->txbuffer, mr16->txbuffer[2] + 5);
                    break;
                    
                case APPMODE_RXSUCCESSIVE:
                    SX1281_Running(&radio);
                    break;
                    
                default:
                    break;
            }
            break;
         
        case MR16_MODE_NONE:
        default:
            /* Idle mode */
            break;
    }
    
    return 0;
}


/*-------------------------------------------------

					  CLI

--------------------------------------------------*/
static const char radio_help_en[] =
"============================================================================\r\n"
"radio <subcmd> [args]  - SX1281 radio control\r\n"
"----------------------------------------------------------------------------\r\n"
"Mode Configuration:\r\n"
"  mode <BLE|LORA|GFSK|FLRC>        - Change working mode\r\n"
"  rffreq <Hz>                      - Set RF frequency\r\n"
"----------------------------------------------------------------------------\r\n"
"Parameter Configuration:\r\n"
"  modulation <field> <value>       - Set modulation parameters\r\n"
"  packet <field> <value>           - Set packet parameters\r\n"
"  baudrate <mode> <index>          - Set baudrate\r\n"
"  power <dBm>                      - Set TX power (-18..+13)\r\n"
"  ramptime <us>                    - Set ramp time (2|4|6|8|10|12|16|20)\r\n"
"----------------------------------------------------------------------------\r\n"
"Operation Commands:\r\n"
"  tx | rx                          - TX/RX operations\r\n"
"----------------------------------------------------------------------------\r\n"
"Help Commands:\r\n"
"  modulation help | packet help    - Show detailed help\r\n"
"============================================================================\r\n";

static const char modulation_help[] =
"============================================================================================================\r\n"
"modulation <field> <value>  - Set modulation parameters\r\n"
"  BLE/GFSK fields:\r\n"
"    br_bw    : 0x04(2.0M/2.4M)||0x28(1.6M/2.4M)||0x4C(1.0M/2.4M)||0x45(1.0M/1.2M)\r\n"
"               0x70(0.8M/2.4M)||0x69(0.8M/1.2M)||0x8D(0.5M/1.2M)||0x86(0.5M/0.6M)\r\n"
"               0xB1(0.4M/1.2M)||0xAA(0.4M/0.6M)||0xCE(0.25M/0.6M)||0xC7(0.25M/0.3M)||0xEF(0.125M/0.3M)\r\n"
"    modindex : 0(0.35)||1(0.50)||2(0.75)||3(1.00)||4(1.25)||5(1.50)||6(1.75)||7(2.00)\r\n"
"               8(2.25)||9(2.50)||10(2.75)||11(3.00)||12(3.25)||13(3.50)||14(3.75)||15(4.00)\r\n"
"    shaping  : 0x00(OFF)||0x10(BT_1_0)||0x20(BT_0_5)\r\n"
"  LORA fields:\r\n"
"    sf : 0x50(SF5)||0x60(SF6)||0x70(SF7)||0x80(SF8)||0x90(SF9)||0xA0(SF10)||0xB0(SF11)||0xC0(SF12)\r\n"
"    bw : 0x34(BW_200K)||0x26(BW_400K)||0x18(BW_800K)||0x0A(BW_1600K)\r\n"
"    cr : 0x01(4/5)||0x02(4/6)||0x03(4/7)||0x04(4/8)||0x05(LI_4/5)||0x06(LI_4/6)||0x07(LI_4/7)\r\n"
"  FLRC fields:\r\n"
"    br_bw    : 0x04(2.6M/2.4M)||0x28(2.08M/2.4M)||0x45(1.3M/1.2M)||0x69(1.04M/1.2M)\r\n"
"               0x86(0.65M/0.6M)||0xAA(0.52M/0.6M)||0xC7(0.325M/0.3M)||0xEB(0.26M/0.3M)\r\n"
"    cr       : 0x00(CR_1/2)||0x02(CR_3/4)||0x04(CR_1/0)\r\n"
"    shaping  : 0x00(OFF)||0x10(BT_1_0)||0x20(BT_0_5)\r\n"
"Examples: radio modulation br_bw 0x4C\r\n"
"          radio modulation sf 0x70\r\n"
"============================================================================================================\r\n";
static const char packet_help[] =
"================================================================================================\r\n"
"packet <field> <value>  - Set packet parameters (current PacketType based)\r\n"
"  BLE fields:\r\n"
"    ConnectionState : 0(MASTER_SLAVE)||1(ADVERTISER)||2(TX_TEST)||3(RX_TEST)||4(RXTX_TEST)\r\n"
"    CrcField        : 0(OFF)||1(CRC_3B)\r\n"
"    BlePacketType   : 0(PRBS_9)||1(PRBS_15)||2(EYELONG_1_0)||3(EYELONG_0_1)\r\n"
"                      4(EYESHORT_1_0)||5(EYESHORT_0_1)||6(ALL_1)||7(ALL_0)\r\n"
"    Whitening       : 0x00(ON)||0x08(OFF)\r\n"
"  GFSK fields:\r\n"
"    Preamble        : 0x00(4bits)||0x10(8bits)||0x20(12bits)||0x30(16bits)\r\n"
"                      0x40(20bits)||0x50(24bits)||0x60(28bits)||0x70(32bits)\r\n"
"    SyncWordLength  : 0x00(1byte)||0x02(2bytes)||0x04(3bytes)||0x06(4bytes)||0x08(5bytes)\r\n"
"    SyncWordMatch   : 0x00(OFF)||0x10(1)||0x20(2)||0x30(1_2)||0x40(3)||0x50(1_3)||0x60(2_3)||0x70(1_2_3)\r\n"
"    SyncWord1/2/3   : <hex_bytes> - Set sync word 1/2/3 (max 5 bytes for GFSK)\r\n"
"    SyncWord        : Show all sync words\r\n"
"    Header          : 0x00(VARIABLE)||0x20(FIXED)\r\n"
"    Payload         : 1-255 (bytes)\r\n"
"    Crc             : 0x00(OFF)||0x10(1byte)||0x20(2bytes)||0x30(3bytes)\r\n"
"    Whitening       : 0x00(ON)||0x08(OFF)\r\n"
"  LORA fields:\r\n"
"    Preamble        : 0-255 (symbols count)\r\n"
"    Header          : 0x00(VARIABLE/EXPLICIT)||0x80(FIXED/IMPLICIT)\r\n"
"    Payload         : 1-255 (bytes)\r\n"
"    Crc             : 0x20(ON)||0x00(OFF)\r\n"
"    InvertIQ        : 0x40(NORMAL)||0x00(INVERTED)\r\n"
"  FLRC fields: Same as GFSK (Preamble/SyncWordLength/SyncWordMatch/SyncWord1/2/3/Header/Payload/Crc/Whitening)\r\n"
"    SyncWord1/2/3   : <hex_bytes> - Set sync word 1/2/3 (max 4 bytes for FLRC)\r\n"
"Examples: radio packet Payload 64\r\n"
"          radio packet Crc 0x20\r\n"
"          radio packet SyncWord1 0x12345678\r\n"
"          radio packet SyncWord\r\n"
"================================================================================================\r\n";

static const char mr16_help[] =
"======================================================\r\n"
"mr16 <subcmd> [args] - MR16 system commands\r\n"
"------------------------------------------------------\r\n"
"Configuration Commands:\r\n"
"  save        : Save MR16 configuration to Flash\r\n"
"  get         : Load configuration from Flash\r\n"
"  mode <arg>  : Set/get operating mode (RFUART|RC)\r\n"
"  power <dBm> : Set/show TX power (-18 to +13 dBm)\r\n"
"  rxid1/2/3 <hex>     : Set/get RX ID (hex value)\r\n"
"------------------------------------------------------\r\n"
"Operation Commands:\r\n"
"  tx          : Transmit current TX buffer\r\n"
"  rx          : Start continuous receive\r\n"
"------------------------------------------------------\r\n"
"Information Commands:\r\n"
"  rxid        : Show all three RX IDs\r\n"
"------------------------------------------------------\r\n"
"Help Commands:\r\n"
"  help        : Show this help (supports paging)\r\n"
"  mode help   : Show mode details\r\n"
"======================================================\r\n";

static const char baudrate_help[] =
"=====================================================================\r\n"
"baudrate <mode> <index>  - Set baudrate\r\n"
"  BLE  : 0(250K)||1(500K)||2(1M)\r\n"
"  LORA : 0(216b)||1(1K)||2(5K)||3(10K)||4(20K)||5(61K)||6(127K)||7(203K)\r\n"
"  GFSK : 0(125K)||1(250K)||2(500K)||3(1M)\r\n"
"  FLRC : 0(130K)||1(260K)||2(520K)||3(1040K)\r\n"
"Examples: radio baudrate GFSK 2\r\n"
"          radio baudrate LORA 3\r\n"
"=====================================================================\r\n";
static BaseType_t mr16CommandHandler(char *pcWriteBuffer, size_t xWriteBufferLen, 
                                     const char *pcCommandString) {
    BaseType_t len1 = 0, len2 = 0, len3 = 0;
    const char *p1 = FreeRTOS_CLIGetParameter(pcCommandString, 1, &len1);
    const char *p2 = FreeRTOS_CLIGetParameter(pcCommandString, 2, &len2);
    const char *p3 = FreeRTOS_CLIGetParameter(pcCommandString, 3, &len3);
    
    char arg1[32] = {0};
    char arg2[32] = {0};
    char arg3[32] = {0};
    
    /* Extract first parameter */
    if (p1) {
        size_t c1 = (len1 < (BaseType_t)(sizeof(arg1)-1)) ? (size_t)len1 : sizeof(arg1)-1;
        strncpy(arg1, p1, c1);
        arg1[c1] = '\0';
    }
    
    /* Extract second parameter */
    if (p2) {
        size_t c2 = (len2 < (BaseType_t)(sizeof(arg2)-1)) ? (size_t)len2 : sizeof(arg2)-1;
        strncpy(arg2, p2, c2);
        arg2[c2] = '\0';
    }
    
    /* Extract third parameter */
    if (p3) {
        size_t c3 = (len3 < (BaseType_t)(sizeof(arg3)-1)) ? (size_t)len3 : sizeof(arg3)-1;
        strncpy(arg3, p3, c3);
        arg3[c3] = '\0';
    }
    
    /* Built-in help support */
    if (strcasecmp(arg1, "help") == 0) {
      static size_t help_pos = 0;
      return outputHelpText(pcWriteBuffer, xWriteBufferLen, mr16_help, &help_pos);
    }

    /* Command: save - Save configuration to Flash */
    if (strcasecmp(arg1, "save") == 0) {
        STMFLASH_Write(FLASH_SAVE_ADDR, (uint16_t*)&Config_Get()->mr16, sizeof(MR16_Param_t)/2);
        snprintf(pcWriteBuffer, xWriteBufferLen, "Configuration saved to Flash!\r\n");
        
    /* Command: get - Read configuration from Flash */
    } else if (strcasecmp(arg1, "get") == 0) {
        STMFLASH_Read(FLASH_SAVE_ADDR, (uint16_t*)&Config_Get()->mr16, sizeof(MR16_Param_t)/2);
        snprintf(pcWriteBuffer, xWriteBufferLen, "Configuration loaded from Flash\r\n");
        
    /* Command: mode - Set MR16 mode */
    } else if (strcasecmp(arg1, "mode") == 0) {
      if (strcasecmp(arg2, "help") == 0) {
        snprintf(pcWriteBuffer, xWriteBufferLen, "mode: 'RFUART' - bridge UART to RF|| 'RC' - remote control mode\r\n");
        return pdFALSE;
      }
      if (arg2[0] != '\0') {
            if (strcasecmp(arg2, "RFUART") == 0) {
                mr16.param->mode = MR16_MODE_RFUART;
                snprintf(pcWriteBuffer, xWriteBufferLen, "Mode set to: RFUART\r\n");
            } else if (strcasecmp(arg2, "RC") == 0) {
                mr16.param->mode = MR16_MODE_RC;
                snprintf(pcWriteBuffer, xWriteBufferLen, "Mode set to: RC\r\n");
            } else {
                snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid mode: %s\r\n", arg2);
            }
        } else {
          const char *mode_str = "UNKNOWN";
          switch (mr16.param->mode) {
            case MR16_MODE_NONE:    mode_str = "NONE"; break;
            case MR16_MODE_RFUART:  mode_str = "RFUART"; break;
            case MR16_MODE_RC:      mode_str = "RC"; break;
            default: break;
          }
          snprintf(pcWriteBuffer, xWriteBufferLen, "Current mode: %s\r\n", mode_str);
        }
        
    /* Command: tx - Transmit packet */
    } else if (strcasecmp(arg1, "tx") == 0) {
        SX1281_SetTX(&radio, mr16.txbuffer, mr16.txbuffer[2] + 5);
        snprintf(pcWriteBuffer, xWriteBufferLen, "Transmitting packet...\r\n");
        
    /* Command: rx - Start continuous receive */
    } else if (strcasecmp(arg1, "rx") == 0) {
        SX1281_SetRXSuccessive(&radio);
        snprintf(pcWriteBuffer, xWriteBufferLen, "Starting continuous receive...\r\n");
        
    /* Command: rxid1 - Set RX ID 1 */
    } else if (strcasecmp(arg1, "rxid1") == 0) {
        if (arg2[0] != '\0') {
            uint16_t rxid;
            sscanf(arg2, "%hx", &rxid);
            if (rxid > 0x0001 && rxid < 0xFFFE) {
                mr16.param->RX_ID[0] = rxid;
                snprintf(pcWriteBuffer, xWriteBufferLen, "RXID1 set to: 0x%04X\r\n", mr16.param->RX_ID[0]);
            } else {
                snprintf(pcWriteBuffer, xWriteBufferLen, "RXID1 set failed: invalid value\r\nValid range: 0x0002-0xFFFD\r\n");
            }
        } else {
            snprintf(pcWriteBuffer, xWriteBufferLen, "Current RXID1: 0x%04X\r\n", mr16.param->RX_ID[0]);
        }
        
    /* Invalid command */
    } else if (strcasecmp(arg1, "rxid2") == 0) {
        if (arg2[0] != '\0') {
            uint16_t rxid;
            sscanf(arg2, "%hx", &rxid);
            if (rxid > 0x0001 && rxid < 0xFFFE) {
                mr16.param->RX_ID[1] = rxid;
                snprintf(pcWriteBuffer, xWriteBufferLen, "RXID2 set to: 0x%04X\r\n", mr16.param->RX_ID[1]);
            } else {
                snprintf(pcWriteBuffer, xWriteBufferLen, "RXID2 set failed: invalid value\r\nValid range: 0x0002-0xFFFD\r\n");
            }
        } else {
            snprintf(pcWriteBuffer, xWriteBufferLen, "Current RXID2: 0x%04X\r\n", mr16.param->RX_ID[1]);
        }
        
    /* Invalid command */
    } else if (strcasecmp(arg1, "rxid3") == 0) {
        if (arg2[0] != '\0') {
            uint16_t rxid;
            sscanf(arg2, "%hx", &rxid);
            if (rxid > 0x0001 && rxid < 0xFFFE) {
                mr16.param->RX_ID[2] = rxid;
                snprintf(pcWriteBuffer, xWriteBufferLen, "RXID3 set to: 0x%04X\r\n", mr16.param->RX_ID[2]);
            } else {
                snprintf(pcWriteBuffer, xWriteBufferLen, "RXID3 set failed: invalid value\r\nValid range: 0x0002-0xFFFD\r\n");
            }
        } else {
            snprintf(pcWriteBuffer, xWriteBufferLen, "Current RXID3: 0x%04X\r\n", mr16.param->RX_ID[2]);
        }
        
    /* Command: rxid - Show all RX IDs */
    } else if (strcasecmp(arg1, "rxid") == 0 && arg2[0] == '\0') {
        snprintf(pcWriteBuffer, xWriteBufferLen, 
                "RX IDs:\r\n  RXID1: 0x%04X\r\n  RXID2: 0x%04X\r\n  RXID3: 0x%04X\r\n",
                mr16.param->RX_ID[0], mr16.param->RX_ID[1], mr16.param->RX_ID[2]);
                
    /* Command: power - Set/show TX power */
    } else if (strcasecmp(arg1, "power") == 0) {
      /* mr16 power [<dBm>] - show or set TX power for MR16 radio */
      if (arg2[0] == '\0') {
        /* Print current TX power (from radio parameters if available) */
        if (radio.param) {
          snprintf(pcWriteBuffer, xWriteBufferLen, "Current power: %d dBm\r\n", radio.param->txOutputPower);
        } else {
          snprintf(pcWriteBuffer, xWriteBufferLen, "Current power: (radio not initialized)\r\n");
        }
      } else {
        /* Parse integer value, allow optional suffixes like 'dBm' or 'dbm' or 'dB' */
        char *endptr = NULL;
        long v = strtol(arg2, &endptr, 0);
        if (endptr == arg2) {
          /* No conversion done */
          snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid power value: %s\r\n", arg2);
        } else {
          /* Acceptable range: -18 .. +13 dBm */
          if (v < -18 || v > 13) {
            snprintf(pcWriteBuffer, xWriteBufferLen, "Power out of range: %ld dBm (valid: -18..13)\r\n", v);
          } else {
            int8_t p = (int8_t)v;
            /* Apply to radio if available */
            if (radio.param) {
              radio.param->txOutputPower = p;
              Radio.SetTxParams(radio.param->txOutputPower, radio.param->rampTime);
            }
            snprintf(pcWriteBuffer, xWriteBufferLen, "Power set to %d dBm\r\n", p);
          }
        }
      }
        
    /* Invalid command */
    } else {
      snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid command: %s\r\nUse 'mr16 help' for available commands\r\n", arg1);
    }
    
    return pdFALSE;
}

static BaseType_t radioCommandHandler(char *pcWriteBuffer, size_t xWriteBufferLen, 
                                      const char *pcCommandString) {
    BaseType_t len1 = 0, len2 = 0, len3 = 0, len4 = 0;
    const char *p1 = FreeRTOS_CLIGetParameter(pcCommandString, 1, &len1);
    const char *p2 = FreeRTOS_CLIGetParameter(pcCommandString, 2, &len2);
    const char *p3 = FreeRTOS_CLIGetParameter(pcCommandString, 3, &len3);
    const char *p4 = FreeRTOS_CLIGetParameter(pcCommandString, 4, &len4);
    
    char arg1[32] = {0};
    char arg2[32] = {0};
    char arg3[32] = {0};
    char arg4[32] = {0};
    
    /* Extract parameters */
    if (p1) { size_t c1 = (len1 < (BaseType_t)(sizeof(arg1)-1)) ? (size_t)len1 : sizeof(arg1)-1; 
              strncpy(arg1, p1, c1); arg1[c1] = '\0'; }
    if (p2) { size_t c2 = (len2 < (BaseType_t)(sizeof(arg2)-1)) ? (size_t)len2 : sizeof(arg2)-1; 
              strncpy(arg2, p2, c2); arg2[c2] = '\0'; }
    if (p3) { size_t c3 = (len3 < (BaseType_t)(sizeof(arg3)-1)) ? (size_t)len3 : sizeof(arg3)-1; 
              strncpy(arg3, p3, c3); arg3[c3] = '\0'; }
    if (p4) { size_t c4 = (len4 < (BaseType_t)(sizeof(arg4)-1)) ? (size_t)len4 : sizeof(arg4)-1; 
              strncpy(arg4, p4, c4); arg4[c4] = '\0'; }
    
    /* Command: help - Display help information */
    if (strcasecmp(arg1, "help") == 0) {
        static size_t help_pos = 0;
        return outputHelpText(pcWriteBuffer, xWriteBufferLen, radio_help_en, &help_pos);
        
    } else if (strcasecmp(arg1, "modulation") == 0 && strcasecmp(arg2, "help") == 0) {
        static size_t mod_help_pos = 0;
        return outputHelpText(pcWriteBuffer, xWriteBufferLen, modulation_help, &mod_help_pos);
        
    } else if (strcasecmp(arg1, "packet") == 0 && strcasecmp(arg2, "help") == 0) {
        static size_t pkt_help_pos = 0;
        return outputHelpText(pcWriteBuffer, xWriteBufferLen, packet_help, &pkt_help_pos);
        
    /* Parameter processing commands */
    } else {
        /* Parse numeric value from arg3 or arg4 */
        unsigned long val = 0;
        if (arg4[0] != '\0') {
            val = strtoul(arg4, NULL, 0);
        } else if (arg3[0] != '\0') {
            val = strtoul(arg3, NULL, 0);
        }
        
        /* Command: mode - Change or show radio mode */
        if (strcasecmp(arg1, "mode") == 0) {
          if (arg2[0] == '\0') {
            /* Print current mode */
            const char *mstr = "UNKNOWN";
            switch (radio.param->radioMode) {
              case RADIOMODE_BLE: mstr = "BLE"; break;
              case RADIOMODE_LORA: mstr = "LORA"; break;
              case RADIOMODE_GFSK: mstr = "GFSK"; break;
              case RADIOMODE_FLRC: mstr = "FLRC"; break;
              default: break;
            }
            snprintf(pcWriteBuffer, xWriteBufferLen, "Current radio mode: %s\r\n", mstr);
          } else {
            SX1281_RadioMode_t proto;
            if (strcasecmp(arg2, "BLE") == 0) {
              proto = RADIOMODE_BLE;
            } else if (strcasecmp(arg2, "LORA") == 0) {
              proto = RADIOMODE_LORA;
            } else if (strcasecmp(arg2, "GFSK") == 0) {
              proto = RADIOMODE_GFSK;
            } else if (strcasecmp(arg2, "FLRC") == 0) {
              proto = RADIOMODE_FLRC;
            } else {
              snprintf(pcWriteBuffer, xWriteBufferLen, "Unknown radio mode: %s\r\n", arg2);
              return pdFALSE;
            }

            if (SX1281_SetMode(&radio, proto) == DEVICE_OK) {
              snprintf(pcWriteBuffer, xWriteBufferLen, "Radio mode set to %s\r\n", arg2);
            } else {
              snprintf(pcWriteBuffer, xWriteBufferLen, "Failed to set radio mode\r\n");
            }
          }
        
        /* Command: modulation - Set modulation parameters */
        } else if (strcasecmp(arg1, "modulation") == 0) {
          /* Show current modulation params when no field provided */
          if (arg2[0] == '\0') {
            switch (radio.param->modulationParams.PacketType) {
              case PACKET_TYPE_BLE:
                snprintf(pcWriteBuffer, xWriteBufferLen, "Modulation(BLE): br_bw=0x%X modindex=%u shaping=0x%X\r\n",
                    radio.param->modulationParams.Params.Ble.BitrateBandwidth,
                    radio.param->modulationParams.Params.Ble.ModulationIndex,
                    radio.param->modulationParams.Params.Ble.ModulationShaping);
                break;
              case PACKET_TYPE_GFSK:
                snprintf(pcWriteBuffer, xWriteBufferLen, "Modulation(GFSK): br_bw=0x%X modindex=%u shaping=0x%X\r\n",
                    radio.param->modulationParams.Params.Gfsk.BitrateBandwidth,
                    radio.param->modulationParams.Params.Gfsk.ModulationIndex,
                    radio.param->modulationParams.Params.Gfsk.ModulationShaping);
                break;
              case PACKET_TYPE_LORA:
                snprintf(pcWriteBuffer, xWriteBufferLen, "Modulation(LoRa): sf=0x%X bw=0x%X cr=0x%X\r\n",
                    radio.param->modulationParams.Params.LoRa.SpreadingFactor,
                    radio.param->modulationParams.Params.LoRa.Bandwidth,
                    radio.param->modulationParams.Params.LoRa.CodingRate);
                break;
              case PACKET_TYPE_FLRC:
                snprintf(pcWriteBuffer, xWriteBufferLen, "Modulation(FLRC): br_bw=0x%X cr=0x%X shaping=0x%X\r\n",
                    radio.param->modulationParams.Params.Flrc.BitrateBandwidth,
                    radio.param->modulationParams.Params.Flrc.CodingRate,
                    radio.param->modulationParams.Params.Flrc.ModulationShaping);
                break;
              default:
                snprintf(pcWriteBuffer, xWriteBufferLen, "Unknown modulation packet type\r\n");
                break;
            }
            return pdFALSE;
          }
            /* Handle different packet types for modulation */
            switch (radio.param->modulationParams.PacketType) {
                case PACKET_TYPE_BLE:
                    if (strcasecmp(arg2, "br_bw") == 0) {
                        if (val > 0xEF) {
                            snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid br_bw value for BLE (max: 0xEF)\r\n");
                            return pdFALSE;
                        }
                        radio.param->modulationParams.Params.Ble.BitrateBandwidth = (RadioGfskBleBitrates_t)val;
                    } else if (strcasecmp(arg2, "modindex") == 0) {
                        if (val > 15) {
                            snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid modindex value for BLE (max: 15)\r\n");
                            return pdFALSE;
                        }
                        radio.param->modulationParams.Params.Ble.ModulationIndex = (RadioGfskBleModIndexes_t)val;
                    } else if (strcasecmp(arg2, "shaping") == 0) {
                        if (val != 0x00 && val != 0x10 && val != 0x20) {
                            snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid shaping value for BLE (0x00/0x10/0x20)\r\n");
                            return pdFALSE;
                        }
                        radio.param->modulationParams.Params.Ble.ModulationShaping = (RadioModShapings_t)val;
                    } else {
                        snprintf(pcWriteBuffer, xWriteBufferLen, "Unknown modulation field for BLE: %s\r\n", arg2);
                        return pdFALSE;
                    }
                    break;
				case PACKET_TYPE_GFSK:
					if (strcasecmp(arg2, "br_bw") == 0) {
						if (val > 0xEF) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid br_bw value for GFSK\r\n"); return pdFALSE; }
						radio.param->modulationParams.Params.Gfsk.BitrateBandwidth = (RadioGfskBleBitrates_t)val;
					}
					else if (strcasecmp(arg2, "modindex") == 0) {
						if (val > 15) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid modindex value for GFSK\r\n"); return pdFALSE; }
						radio.param->modulationParams.Params.Gfsk.ModulationIndex = (RadioGfskBleModIndexes_t)val;
					}
					else if (strcasecmp(arg2, "shaping") == 0) {
						if (val != 0x00 && val != 0x10 && val != 0x20) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid shaping value for GFSK\r\n"); return pdFALSE; }
						radio.param->modulationParams.Params.Gfsk.ModulationShaping = (RadioModShapings_t)val;
					}
					else { snprintf(pcWriteBuffer, xWriteBufferLen, "Unknown modulation field\r\n"); return pdFALSE; }
					break;
				case PACKET_TYPE_LORA:
					if (strcasecmp(arg2, "sf") == 0) {
						if (val != 0x50 && val != 0x60 && val != 0x70 && val != 0x80 && val != 0x90 && val != 0xA0 && val != 0xB0 && val != 0xC0) {
						snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid sf value for LORA\r\n"); return pdFALSE;
						}
						radio.param->modulationParams.Params.LoRa.SpreadingFactor = (RadioLoRaSpreadingFactors_t)val;
					}
					else if (strcasecmp(arg2, "bw") == 0) {
						if (val != 0x34 && val != 0x26 && val != 0x18 && val != 0x0A) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid bw value for LORA\r\n"); return pdFALSE; }
						radio.param->modulationParams.Params.LoRa.Bandwidth = (RadioLoRaBandwidths_t)val;
					}
					else if (strcasecmp(arg2, "cr") == 0) {
						if (val < 0x01 || val > 0x07) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid cr value for LORA\r\n"); return pdFALSE; }
						radio.param->modulationParams.Params.LoRa.CodingRate = (RadioLoRaCodingRates_t)val;
					}
					else { snprintf(pcWriteBuffer, xWriteBufferLen, "Unknown modulation field\r\n"); return pdFALSE; }
					break;
				case PACKET_TYPE_FLRC:
					if (strcasecmp(arg2, "br_bw") == 0) {
						if (val != 0x04 && val != 0x28 && val != 0x45 && val != 0x69 && val != 0x86 && val != 0xAA && val != 0xC7 && val != 0xEB) {
						snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid br_bw value for FLRC\r\n"); return pdFALSE;
						}
						radio.param->modulationParams.Params.Flrc.BitrateBandwidth = (RadioFlrcBitrates_t)val;
					}
					else if (strcasecmp(arg2, "cr") == 0) {
						if (val != 0x00 && val != 0x02 && val != 0x04) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid cr value for FLRC\r\n"); return pdFALSE; }
						radio.param->modulationParams.Params.Flrc.CodingRate = (RadioFlrcCodingRates_t)val;
					}
					else if (strcasecmp(arg2, "shaping") == 0) {
						if (val != 0x00 && val != 0x10 && val != 0x20) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid shaping value for FLRC\r\n"); return pdFALSE; }
						radio.param->modulationParams.Params.Flrc.ModulationShaping = (RadioModShapings_t)val;
					}
					else { snprintf(pcWriteBuffer, xWriteBufferLen, "Unknown modulation field\r\n"); return pdFALSE; }
					break;
				default:
					snprintf(pcWriteBuffer, xWriteBufferLen, "Unknown current packet type\r\n");
					return pdFALSE;
			}
            Radio.SetModulationParams(&radio.param->modulationParams);
            snprintf(pcWriteBuffer, xWriteBufferLen, "Modulation updated\r\n");
            
          } else if (strcasecmp(arg1, "packet") == 0) {
          /* Show current packet params when no field provided */
          if (arg2[0] == '\0') {
            switch (radio.param->packetParams.PacketType) {
              case PACKET_TYPE_BLE:
                snprintf(pcWriteBuffer, xWriteBufferLen, "Packet(BLE): ConnectionState=%u CrcField=%u BlePacketType=%u Whitening=0x%X\r\n",
                     radio.param->packetParams.Params.Ble.ConnectionState,
                     radio.param->packetParams.Params.Ble.CrcField,
                     radio.param->packetParams.Params.Ble.BlePacketType,
                     radio.param->packetParams.Params.Ble.Whitening);
                break;
              case PACKET_TYPE_GFSK:
                snprintf(pcWriteBuffer, xWriteBufferLen, "Packet(GFSK): Preamble=0x%X SyncLen=0x%X SyncMatch=0x%X Header=0x%X Payload=%u Crc=0x%X Whitening=0x%X\r\n",
                     radio.param->packetParams.Params.Gfsk.PreambleLength,
                     radio.param->packetParams.Params.Gfsk.SyncWordLength,
                     radio.param->packetParams.Params.Gfsk.SyncWordMatch,
                     radio.param->packetParams.Params.Gfsk.HeaderType,
                     radio.param->packetParams.Params.Gfsk.PayloadLength,
                     radio.param->packetParams.Params.Gfsk.CrcLength,
                     radio.param->packetParams.Params.Gfsk.Whitening);
                break;
              case PACKET_TYPE_LORA:
                snprintf(pcWriteBuffer, xWriteBufferLen, "Packet(LoRa): Preamble=%u Header=0x%X Payload=%u Crc=0x%X InvertIQ=0x%X\r\n",
                     radio.param->packetParams.Params.LoRa.PreambleLength,
                     radio.param->packetParams.Params.LoRa.HeaderType,
                     radio.param->packetParams.Params.LoRa.PayloadLength,
                     radio.param->packetParams.Params.LoRa.CrcMode,
                     radio.param->packetParams.Params.LoRa.InvertIQ);
                break;
              case PACKET_TYPE_FLRC:
                snprintf(pcWriteBuffer, xWriteBufferLen, "Packet(FLRC): Preamble=0x%X SyncLen=0x%X SyncMatch=0x%X Header=0x%X Payload=%u Crc=0x%X Whitening=0x%X\r\n",
                     radio.param->packetParams.Params.Flrc.PreambleLength,
                     radio.param->packetParams.Params.Flrc.SyncWordLength,
                     radio.param->packetParams.Params.Flrc.SyncWordMatch,
                     radio.param->packetParams.Params.Flrc.HeaderType,
                     radio.param->packetParams.Params.Flrc.PayloadLength,
                     radio.param->packetParams.Params.Flrc.CrcLength,
                     radio.param->packetParams.Params.Flrc.Whitening);
                break;
              default:
                snprintf(pcWriteBuffer, xWriteBufferLen, "Unknown packet type\r\n");
                break;
            }
            return pdFALSE;
          }
    /* packet <proto> <field> <value>
       fields examples: payload | preamble | header | crc | whitening
    */
    /* 根据当前 packet type 设置对应字段 */
    switch (radio.param->packetParams.PacketType) {

      case PACKET_TYPE_BLE:
          /* BLE 使用 Ble 子结构 */
          if (strcasecmp(arg2, "ConnectionState") == 0) {
            if (val > 4) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid BLE ConnectionState (0-4)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.Ble.ConnectionState = (RadioBleConnectionStates_t)val;
          } else if (strcasecmp(arg2, "CrcField") == 0) {
            if (val > 1) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid BLE CrcField (0-1)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.Ble.CrcField = (RadioBleCrcFields_t)val;
          } else if (strcasecmp(arg2, "BlePacketType") == 0) {
            if (val > 7) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid BLEPacketType (0-7)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.Ble.BlePacketType = (RadioBlePacketTypes_t)val;
          } else if (strcasecmp(arg2, "Whitening") == 0) {
            if (val != 0x00 && val != 0x08) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid BLE Whitening (0x00/0x08)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.Ble.Whitening = (RadioWhiteningModes_t)val;
          } else {
            snprintf(pcWriteBuffer, xWriteBufferLen, "Unknown BLE packet field\r\n");
            return pdFALSE;
          }
          break;

      case PACKET_TYPE_GFSK:
          /* GFSK 子结构 */
          if (strcasecmp(arg2, "Preamble") == 0) {
            if (val != 0x00 && val != 0x10 && val != 0x20 && val != 0x30 && val != 0x40 && val != 0x50 && val != 0x60 && val != 0x70) {
              snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid GFSK Preamble (0x00/0x10/0x20/0x30/0x40/0x50/0x60/0x70)\r\n"); return pdFALSE;
            }
            radio.param->packetParams.Params.Gfsk.PreambleLength = (RadioPreambleLengths_t)val;
          } else if (strcasecmp(arg2, "SyncWordLength") == 0) {
            if (val != 0x00 && val != 0x02 && val != 0x04 && val != 0x06 && val != 0x08) {
              snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid GFSK SyncWordLength (0x00/0x02/0x04/0x06/0x08)\r\n"); return pdFALSE;
            }
            radio.param->packetParams.Params.Gfsk.SyncWordLength = (RadioSyncWordLengths_t)val;
          } else if (strcasecmp(arg2, "SyncWordMatch") == 0) {
            if (val != 0x00 && val != 0x10 && val != 0x20 && val != 0x30 && val != 0x40 && val != 0x50 && val != 0x60 && val != 0x70) {
              snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid GFSK SyncWordMatch (0x00/0x10/0x20/0x30/0x40/0x50/0x60/0x70)\r\n"); return pdFALSE;
            }
            radio.param->packetParams.Params.Gfsk.SyncWordMatch = (RadioSyncWordRxMatchs_t)val;
          } else if (strcasecmp(arg2, "SyncWord") == 0) {
            /* Show all GFSK sync words */
            snprintf(pcWriteBuffer, xWriteBufferLen, 
                    "GFSK Sync Words:\r\n"
                    "  Word 1: %02X%02X%02X%02X%02X\r\n"
                    "  Word 2: %02X%02X%02X%02X%02X\r\n"
                    "  Word 3: %02X%02X%02X%02X%02X\r\n",
                    mr16.param->radioParams.syncWord.gfsk.first[0],
                    mr16.param->radioParams.syncWord.gfsk.first[1],
                    mr16.param->radioParams.syncWord.gfsk.first[2],
                    mr16.param->radioParams.syncWord.gfsk.first[3],
                    mr16.param->radioParams.syncWord.gfsk.first[4],
                    mr16.param->radioParams.syncWord.gfsk.second[0],
                    mr16.param->radioParams.syncWord.gfsk.second[1],
                    mr16.param->radioParams.syncWord.gfsk.second[2],
                    mr16.param->radioParams.syncWord.gfsk.second[3],
                    mr16.param->radioParams.syncWord.gfsk.second[4],
                    mr16.param->radioParams.syncWord.gfsk.third[0],
                    mr16.param->radioParams.syncWord.gfsk.third[1],
                    mr16.param->radioParams.syncWord.gfsk.third[2],
                    mr16.param->radioParams.syncWord.gfsk.third[3],
                    mr16.param->radioParams.syncWord.gfsk.third[4]);
            return pdFALSE;
          } else if (strcasecmp(arg2, "SyncWord1") == 0) {
            if (arg3[0] != '\0') {
                /* Set GFSK sync word 1 */
                uint32_t sync_val = (uint32_t)strtoul(arg3, NULL, 0);
                mr16.param->radioParams.syncWord.gfsk.first[0] = (sync_val >> 24) & 0xFF;
                mr16.param->radioParams.syncWord.gfsk.first[1] = (sync_val >> 16) & 0xFF;
                mr16.param->radioParams.syncWord.gfsk.first[2] = (sync_val >> 8) & 0xFF;
                mr16.param->radioParams.syncWord.gfsk.first[3] = sync_val & 0xFF;
                mr16.param->radioParams.syncWord.gfsk.first[4] = 0;
                snprintf(pcWriteBuffer, xWriteBufferLen, "GFSK sync word 1 set to 0x%08lX\r\n", (unsigned long)sync_val);
            } else {
                snprintf(pcWriteBuffer, xWriteBufferLen, "GFSK Sync Word 1: %02X%02X%02X%02X%02X\r\n",
                        mr16.param->radioParams.syncWord.gfsk.first[0],
                        mr16.param->radioParams.syncWord.gfsk.first[1],
                        mr16.param->radioParams.syncWord.gfsk.first[2],
                        mr16.param->radioParams.syncWord.gfsk.first[3],
                        mr16.param->radioParams.syncWord.gfsk.first[4]);
            }
            return pdFALSE;
          } else if (strcasecmp(arg2, "SyncWord2") == 0) {
            if (arg3[0] != '\0') {
                /* Set GFSK sync word 2 */
                uint32_t sync_val = (uint32_t)strtoul(arg3, NULL, 0);
                mr16.param->radioParams.syncWord.gfsk.second[0] = (sync_val >> 24) & 0xFF;
                mr16.param->radioParams.syncWord.gfsk.second[1] = (sync_val >> 16) & 0xFF;
                mr16.param->radioParams.syncWord.gfsk.second[2] = (sync_val >> 8) & 0xFF;
                mr16.param->radioParams.syncWord.gfsk.second[3] = sync_val & 0xFF;
                mr16.param->radioParams.syncWord.gfsk.second[4] = 0;
                snprintf(pcWriteBuffer, xWriteBufferLen, "GFSK sync word 2 set to 0x%08lX\r\n", (unsigned long)sync_val);
            } else {
                snprintf(pcWriteBuffer, xWriteBufferLen, "GFSK Sync Word 2: %02X%02X%02X%02X%02X\r\n",
                        mr16.param->radioParams.syncWord.gfsk.second[0],
                        mr16.param->radioParams.syncWord.gfsk.second[1],
                        mr16.param->radioParams.syncWord.gfsk.second[2],
                        mr16.param->radioParams.syncWord.gfsk.second[3],
                        mr16.param->radioParams.syncWord.gfsk.second[4]);
            }
            return pdFALSE;
          } else if (strcasecmp(arg2, "SyncWord3") == 0) {
            if (arg3[0] != '\0') {
                /* Set GFSK sync word 3 */
                uint32_t sync_val = (uint32_t)strtoul(arg3, NULL, 0);
                mr16.param->radioParams.syncWord.gfsk.third[0] = (sync_val >> 24) & 0xFF;
                mr16.param->radioParams.syncWord.gfsk.third[1] = (sync_val >> 16) & 0xFF;
                mr16.param->radioParams.syncWord.gfsk.third[2] = (sync_val >> 8) & 0xFF;
                mr16.param->radioParams.syncWord.gfsk.third[3] = sync_val & 0xFF;
                mr16.param->radioParams.syncWord.gfsk.third[4] = 0;
                snprintf(pcWriteBuffer, xWriteBufferLen, "GFSK sync word 3 set to 0x%08lX\r\n", (unsigned long)sync_val);
            } else {
                snprintf(pcWriteBuffer, xWriteBufferLen, "GFSK Sync Word 3: %02X%02X%02X%02X%02X\r\n",
                        mr16.param->radioParams.syncWord.gfsk.third[0],
                        mr16.param->radioParams.syncWord.gfsk.third[1],
                        mr16.param->radioParams.syncWord.gfsk.third[2],
                        mr16.param->radioParams.syncWord.gfsk.third[3],
                        mr16.param->radioParams.syncWord.gfsk.third[4]);
            }
            return pdFALSE;
          } else if (strcasecmp(arg2, "Header") == 0) {
            if (val != 0x00 && val != 0x20) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid GFSK Header (0x00/0x20)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.Gfsk.HeaderType = (RadioPacketLengthModes_t)val;
          } else if (strcasecmp(arg2, "Payload") == 0) {
            if (val < 1 || val > 255) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid GFSK Payload (1-255)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.Gfsk.PayloadLength = (uint8_t)val;
          } else if (strcasecmp(arg2, "Crc") == 0) {
            if (val != 0x00 && val != 0x10 && val != 0x20 && val != 0x30) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid GFSK Crc (0x00/0x10/0x20/0x30)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.Gfsk.CrcLength = (RadioCrcTypes_t)val;
          } else if (strcasecmp(arg2, "Whitening") == 0) {
            if (val != 0x00 && val != 0x08) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid GFSK Whitening (0x00/0x08)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.Gfsk.Whitening = (RadioWhiteningModes_t)val;
          } else {
            snprintf(pcWriteBuffer, xWriteBufferLen, "Unknown GFSK packet field\r\n");
            return pdFALSE;
          }
          break;

      case PACKET_TYPE_LORA:
          /* LoRa 子结构 */
          if (strcasecmp(arg2, "Preamble") == 0) {
            if (val < 0 || val > 255) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid LoRa Preamble (0-255)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.LoRa.PreambleLength = (uint8_t)val;
          } else if (strcasecmp(arg2, "Header") == 0) {
            if (val != 0x00 && val != 0x80) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid LoRa Header (0x00/0x80)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.LoRa.HeaderType = (RadioLoRaPacketLengthsModes_t)val;
          } else if (strcasecmp(arg2, "Payload") == 0) {
            if (val < 1 || val > 255) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid LoRa Payload (1-255)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.LoRa.PayloadLength = (uint8_t)val;
          } else if (strcasecmp(arg2, "Crc") == 0) {
            if (val != 0x00 && val != 0x20) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid LoRa Crc (0x00/0x20)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.LoRa.CrcMode = (RadioLoRaCrcModes_t)val;
          } else if (strcasecmp(arg2, "InvertIQ") == 0) {
            if (val != 0x00 && val != 0x40) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid LoRa InvertIQ (0x00/0x40)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.LoRa.InvertIQ = (RadioLoRaIQModes_t)val;
          } else {
            snprintf(pcWriteBuffer, xWriteBufferLen, "Unknown LoRa packet field\r\n");
            return pdFALSE;
          }
          break;

      case PACKET_TYPE_FLRC:
          /* FLRC 子结构 */
          if (strcasecmp(arg2, "Preamble") == 0) {
            if (val != 0x00 && val != 0x10 && val != 0x20 && val != 0x30 && val != 0x40 && val != 0x50 && val != 0x60 && val != 0x70) {
              snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid FLRC Preamble (0x00/0x10/0x20/0x30/0x40/0x50/0x60/0x70)\r\n"); return pdFALSE;
            }
            radio.param->packetParams.Params.Flrc.PreambleLength = (RadioPreambleLengths_t)val;
          } else if (strcasecmp(arg2, "SyncWordLength") == 0) {
            if (val != 0x00 && val != 0x02 && val != 0x04 && val != 0x06 && val != 0x08) {
              snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid FLRC SyncWordLength (0x00/0x02/0x04/0x06/0x08)\r\n"); return pdFALSE;
            }
            radio.param->packetParams.Params.Flrc.SyncWordLength = (RadioFlrcSyncWordLengths_t)val;
          } else if (strcasecmp(arg2, "SyncWordMatch") == 0) {
            if (val != 0x00 && val != 0x10 && val != 0x20 && val != 0x30 && val != 0x40 && val != 0x50 && val != 0x60 && val != 0x70) {
              snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid FLRC SyncWordMatch (0x00/0x10/0x20/0x30/0x40/0x50/0x60/0x70)\r\n"); return pdFALSE;
            }
            radio.param->packetParams.Params.Flrc.SyncWordMatch = (RadioSyncWordRxMatchs_t)val;
          } else if (strcasecmp(arg2, "SyncWord") == 0) {
            /* Show all FLRC sync words */
            snprintf(pcWriteBuffer, xWriteBufferLen, 
                    "FLRC Sync Words:\r\n"
                    "  Word 1: %02X%02X%02X%02X\r\n"
                    "  Word 2: %02X%02X%02X%02X\r\n"
                    "  Word 3: %02X%02X%02X%02X\r\n",
                    mr16.param->radioParams.syncWord.flrc.first[0],
                    mr16.param->radioParams.syncWord.flrc.first[1],
                    mr16.param->radioParams.syncWord.flrc.first[2],
                    mr16.param->radioParams.syncWord.flrc.first[3],
                    mr16.param->radioParams.syncWord.flrc.second[0],
                    mr16.param->radioParams.syncWord.flrc.second[1],
                    mr16.param->radioParams.syncWord.flrc.second[2],
                    mr16.param->radioParams.syncWord.flrc.second[3],
                    mr16.param->radioParams.syncWord.flrc.third[0],
                    mr16.param->radioParams.syncWord.flrc.third[1],
                    mr16.param->radioParams.syncWord.flrc.third[2],
                    mr16.param->radioParams.syncWord.flrc.third[3]);
            return pdFALSE;
          } else if (strcasecmp(arg2, "SyncWord1") == 0) {
            if (arg3[0] != '\0') {
                /* Set FLRC sync word 1 */
                uint32_t sync_val = (uint32_t)strtoul(arg3, NULL, 0);
                mr16.param->radioParams.syncWord.flrc.first[0] = (sync_val >> 24) & 0xFF;
                mr16.param->radioParams.syncWord.flrc.first[1] = (sync_val >> 16) & 0xFF;
                mr16.param->radioParams.syncWord.flrc.first[2] = (sync_val >> 8) & 0xFF;
                mr16.param->radioParams.syncWord.flrc.first[3] = sync_val & 0xFF;
                snprintf(pcWriteBuffer, xWriteBufferLen, "FLRC sync word 1 set to 0x%08lX\r\n", (unsigned long)sync_val);
            } else {
                snprintf(pcWriteBuffer, xWriteBufferLen, "FLRC Sync Word 1: %02X%02X%02X%02X\r\n",
                        mr16.param->radioParams.syncWord.flrc.first[0],
                        mr16.param->radioParams.syncWord.flrc.first[1],
                        mr16.param->radioParams.syncWord.flrc.first[2],
                        mr16.param->radioParams.syncWord.flrc.first[3]);
            }
            return pdFALSE;
          } else if (strcasecmp(arg2, "SyncWord2") == 0) {
            if (arg3[0] != '\0') {
                /* Set FLRC sync word 2 */
                uint32_t sync_val = (uint32_t)strtoul(arg3, NULL, 0);
                mr16.param->radioParams.syncWord.flrc.second[0] = (sync_val >> 24) & 0xFF;
                mr16.param->radioParams.syncWord.flrc.second[1] = (sync_val >> 16) & 0xFF;
                mr16.param->radioParams.syncWord.flrc.second[2] = (sync_val >> 8) & 0xFF;
                mr16.param->radioParams.syncWord.flrc.second[3] = sync_val & 0xFF;
                snprintf(pcWriteBuffer, xWriteBufferLen, "FLRC sync word 2 set to 0x%08lX\r\n", (unsigned long)sync_val);
            } else {
                snprintf(pcWriteBuffer, xWriteBufferLen, "FLRC Sync Word 2: %02X%02X%02X%02X\r\n",
                        mr16.param->radioParams.syncWord.flrc.second[0],
                        mr16.param->radioParams.syncWord.flrc.second[1],
                        mr16.param->radioParams.syncWord.flrc.second[2],
                        mr16.param->radioParams.syncWord.flrc.second[3]);
            }
            return pdFALSE;
          } else if (strcasecmp(arg2, "SyncWord3") == 0) {
            if (arg3[0] != '\0') {
                /* Set FLRC sync word 3 */
                uint32_t sync_val = (uint32_t)strtoul(arg3, NULL, 0);
                mr16.param->radioParams.syncWord.flrc.third[0] = (sync_val >> 24) & 0xFF;
                mr16.param->radioParams.syncWord.flrc.third[1] = (sync_val >> 16) & 0xFF;
                mr16.param->radioParams.syncWord.flrc.third[2] = (sync_val >> 8) & 0xFF;
                mr16.param->radioParams.syncWord.flrc.third[3] = sync_val & 0xFF;
                snprintf(pcWriteBuffer, xWriteBufferLen, "FLRC sync word 3 set to 0x%08lX\r\n", (unsigned long)sync_val);
            } else {
                snprintf(pcWriteBuffer, xWriteBufferLen, "FLRC Sync Word 3: %02X%02X%02X%02X\r\n",
                        mr16.param->radioParams.syncWord.flrc.third[0],
                        mr16.param->radioParams.syncWord.flrc.third[1],
                        mr16.param->radioParams.syncWord.flrc.third[2],
                        mr16.param->radioParams.syncWord.flrc.third[3]);
            }
            return pdFALSE;
          } else if (strcasecmp(arg2, "Header") == 0) {
            if (val != 0x00 && val != 0x20) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid FLRC Header (0x00/0x20)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.Flrc.HeaderType = (RadioPacketLengthModes_t)val;
          } else if (strcasecmp(arg2, "Payload") == 0) {
            if (val < 1 || val > 255) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid FLRC Payload (1-255)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.Flrc.PayloadLength = (uint8_t)val;
          } else if (strcasecmp(arg2, "Crc") == 0) {
            if (val != 0x00 && val != 0x10 && val != 0x20 && val != 0x30) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid FLRC Crc (0x00/0x10/0x20/0x30)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.Flrc.CrcLength = (RadioCrcTypes_t)val;
          } else if (strcasecmp(arg2, "Whitening") == 0) {
            if (val != 0x00 && val != 0x08) { snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid FLRC Whitening (0x00/0x08)\r\n"); return pdFALSE; }
            radio.param->packetParams.Params.Flrc.Whitening = (RadioWhiteningModes_t)val;
          } else {
            snprintf(pcWriteBuffer, xWriteBufferLen, "Unknown FLRC packet field\r\n");
            return pdFALSE;
          }
          break;

      default:
        snprintf(pcWriteBuffer, xWriteBufferLen, "Unknown current packet type\r\n");
        return pdFALSE;
    }

    /* 应用并返回 */
    Radio.SetPacketParams(&radio.param->packetParams);
    snprintf(pcWriteBuffer, xWriteBufferLen, "Packet parameters updated\r\n");
  } else if (strcasecmp(arg1, "baudrate") == 0) {
    /* baudrate <proto> <value> */
    int8_t ret = -1;
    if (arg2[0] == '\0') {
        snprintf(pcWriteBuffer, xWriteBufferLen, "Baudrates: BLE=%u LORA=%u GFSK=%u FLRC=%u\r\n",
                 radio.param->baudrate.ble,
                 radio.param->baudrate.lora,
                 radio.param->baudrate.gfks,
                 radio.param->baudrate.flrc);
        return pdFALSE;
    }
    if (strcasecmp(arg2, "BLE") == 0) {
      radio.param->baudrate.ble = (SX1281_BLEBaudrate_t)val;
      ret = SX1281_SetBLEBaudrate(&radio, radio.param->baudrate.ble);
    } else if (strcasecmp(arg2, "LORA") == 0) {
      radio.param->baudrate.lora = (SX1281_LORABaudrate_t)val;
      ret = SX1281_SetLORABaudrate(&radio, radio.param->baudrate.lora);
    } else if (strcasecmp(arg2, "GFSK") == 0) {
      radio.param->baudrate.gfks = (SX1281_GFKSBaudrate_t)val;
      ret = SX1281_SetGFSKBaudrate(&radio, radio.param->baudrate.gfks);
    } else if (strcasecmp(arg2, "FLRC") == 0) {
      radio.param->baudrate.flrc = (SX1281_FLRCBaudrate_t)val;
      ret = SX1281_SetFLRCBaudrate(&radio, radio.param->baudrate.flrc);
    } else {
      snprintf(pcWriteBuffer, xWriteBufferLen, "Unknown proto: %s\r\n", arg2);
      return pdFALSE;
    }
    if (ret == DEVICE_OK) {
      snprintf(pcWriteBuffer, xWriteBufferLen, "Baudrate updated for %s -> %lu\r\n", arg2, (unsigned long)val);
    } else {
      snprintf(pcWriteBuffer, xWriteBufferLen, "Failed to update baudrate for %s\r\n", arg2);
    }
  } else if (strcasecmp(arg1, "power") == 0) {
    /* power <value> */
    if (arg2[0] == '\0') {
        snprintf(pcWriteBuffer, xWriteBufferLen, "Current TX power: %d dBm\r\n", radio.param->txOutputPower);
        return pdFALSE;
    }
    int8_t p = (int8_t)val;
    radio.param->txOutputPower = p;
    Radio.SetTxParams(radio.param->txOutputPower, radio.param->rampTime);
    snprintf(pcWriteBuffer, xWriteBufferLen, "TX power set to %d dBm\r\n", p);
  } else if (strcasecmp(arg1, "ramptime") == 0) {
    /* ramptime <us> - accepts 2/4/6/8/10/12/16/20 */
    RadioRampTimes_t rt = RADIO_RAMP_02_US;
    unsigned long us = val;
    if (arg2[0] == '\0') {
        /* print current ramptime */
        unsigned long cur_us = 2;
        switch (radio.param->rampTime) {
            case RADIO_RAMP_02_US: cur_us = 2; break;
            case RADIO_RAMP_04_US: cur_us = 4; break;
            case RADIO_RAMP_06_US: cur_us = 6; break;
            case RADIO_RAMP_08_US: cur_us = 8; break;
            case RADIO_RAMP_10_US: cur_us = 10; break;
            case RADIO_RAMP_12_US: cur_us = 12; break;
            case RADIO_RAMP_16_US: cur_us = 16; break;
            case RADIO_RAMP_20_US: cur_us = 20; break;
            default: break;
        }
        snprintf(pcWriteBuffer, xWriteBufferLen, "Current ramp time: %lu us\r\n", cur_us);
        return pdFALSE;
    }
    switch (us) {
      case 2: rt = RADIO_RAMP_02_US; break;
      case 4: rt = RADIO_RAMP_04_US; break;
      case 6: rt = RADIO_RAMP_06_US; break;
      case 8: rt = RADIO_RAMP_08_US; break;
      case 10: rt = RADIO_RAMP_10_US; break;
      case 12: rt = RADIO_RAMP_12_US; break;
      case 16: rt = RADIO_RAMP_16_US; break;
      case 20: rt = RADIO_RAMP_20_US; break;
      default: snprintf(pcWriteBuffer, xWriteBufferLen, "Invalid ramptime\r\n"); return pdFALSE;
    }
    radio.param->rampTime = rt;
    Radio.SetTxParams(radio.param->txOutputPower, radio.param->rampTime);
    snprintf(pcWriteBuffer, xWriteBufferLen, "Ramp time set to %lu us\r\n", us);
  } else if (strcasecmp(arg1, "rffreq") == 0 || strcasecmp(arg1, "rffrequency") == 0) {
    /* rffreq <value in Hz> */
    if (arg2[0] == '\0') {
        snprintf(pcWriteBuffer, xWriteBufferLen, "Current RF freq: %llu Hz\r\n", (unsigned long long)radio.param->rfFrequency);
        return pdFALSE;
    }
    uint32_t f = (uint32_t)val;
    SX1281_SetRFFrequency(&radio, f);
    snprintf(pcWriteBuffer, xWriteBufferLen, "RF freq set to %lu Hz\r\n", (unsigned long)f);
  } else {
    snprintf(pcWriteBuffer, xWriteBufferLen, "Unknown radio command\r\n");
  }
        }

  return pdFALSE;
}

/* CLI Initialization Function -------------------------------------------- */
/**
 * @brief Initialize MR16 CLI commands
 * @return 0 on success, -1 on failure
 */
int8_t MR16_CLI_Init(void) {
    static const CLI_Command_Definition_t mr16Command = {
      "mr16",
      "mr16 <subcmd> [args] - MR16 system commands. Use 'mr16 help' for details\r\n",
      mr16CommandHandler,
      -1
    };
    
    static const CLI_Command_Definition_t radioCommand = {
        "radio",
        "radio <cmd> - SX1281 radio control commands. Use 'radio help' for details\r\n",
        radioCommandHandler,
        -1
    };
    
    /* Register commands */
    if (FreeRTOS_CLIRegisterCommand(&mr16Command) != pdPASS) {
        return -1;
    }
    
    if (FreeRTOS_CLIRegisterCommand(&radioCommand) != pdPASS) {
        return -1;
    }
    
    return 0;
}
