#include <ArduinoJson.h>
#include "IRIS_Method.h"
#include "vsmd_parser.h"
#include "VSMD_command_handler.h"
#include "bluetooth_handler.h"  // 添加蓝牙模块

VSMDParser parser;

#define DEBUG_TX_PIN 17  // TX
#define DEBUG_RX_PIN 16  // RX
#define BUFFER_SIZE 1024
#define JSON_BUFFER_SIZE 512

HardwareSerial DebugSerial(1);  // 使用UART1

// 全局变量
uint8_t receiveBuffer[BUFFER_SIZE];     // 接收缓冲区
uint8_t sendBuffer[BUFFER_SIZE];        // 发送缓冲区
uint8_t responseBuffer[BUFFER_SIZE];    // 响应缓冲区
uint8_t command;                        // 存储命令
int32_t bytesProcessed;                 // 处理的数据字节数
int32_t unpackResult;                   // 解包结果
int32_t packedLength;                   // 打包后的数据长度

// 设备状态变量
bool deviceEnabled = false;
int32_t pulsePerSecond;

void printDeviceStatus();
void handleSerialData();

void printDeviceStatus() {
    DebugSerial.println("\n=== Device Status ===");
    DebugSerial.print("Enabled: ");
    DebugSerial.println(deviceEnabled ? "Yes" : "No");
    DebugSerial.print("Bluetooth: ");
    DebugSerial.println(isBluetoothConnected() ? "Connected" : "Disconnected");
    DebugSerial.println("==================\n");
}

void setup() {
    // 调试串口
    DebugSerial.begin(9600, SERIAL_8N1, DEBUG_RX_PIN, DEBUG_TX_PIN);
    DebugSerial.setTimeout(1);
    
    // 电机数据通信串口
    Serial.begin(9600);
    Serial.setTimeout(100);
    
    DebugSerial.println("VSMD INIT");
    DebugSerial.println("CMD: dev, sts, cfg, ena, off, mov, pos, rmv, pps, org, stp, zero, sav");
    DebugSerial.println("cfg CMD: bdr, mcs, spd, acc, dec, cra, crn, crh, s1f, s1r, s2f, s2r, zmd, snr, osv, zsd, zsp, dmd, dar, msr, msv, psr, psv");
    
    // 初始化蓝牙
    initBluetooth();
    
    printDeviceStatus();
}

void loop() {
    // 处理串口数据
    handleSerialData();
    
    // 处理蓝牙数据
    handleBluetoothData();
    
    // 短暂延迟
    delay(10);
}

void handleSerialData() {
    if (DebugSerial.available() > 0) {
        int bytesRead = DebugSerial.readBytes(receiveBuffer, sizeof(receiveBuffer));
        
        bytesProcessed = IRIS_Cut_Befor_Header(receiveBuffer, bytesRead);
        
        if (bytesProcessed > 0) {
            unpackResult = IRIS_STM32_Protocol_Unpack(receiveBuffer, bytesProcessed, &command, responseBuffer);
            
            if (unpackResult >= 0) {
                // 根据命令类型处理数据
                switch (command) {
                    case 0x00: { // JSON数据
                        char jsonString[JSON_BUFFER_SIZE];
                        memcpy(jsonString, responseBuffer, unpackResult);
                        jsonString[unpackResult] = '\0';
                        //printSerialRawData(responseBuffer, unpackResult);
                        processJsonCommand(jsonString);
                        //BLE Notify
                        sendBluetoothData(sendBuffer, packedLength);
                        break;
                    }
                    
                    case 0x01: { // 字符串数据
                        char stringData[BUFFER_SIZE];
                        memcpy(stringData, responseBuffer, unpackResult);
                        stringData[unpackResult] = '\0';
                        
                        DebugSerial.print("Received String: ");
                        DebugSerial.println(stringData);
                        
                        processStringCommand(stringData);
                        break;
                    }
                    
                    default: { // 自定义二进制数据
                        DebugSerial.print("Received Binary Data, Command: 0x");
                        DebugSerial.print(command, HEX);
                        DebugSerial.print(", Length: ");
                        DebugSerial.println(unpackResult);
                        
                        // 回显
                        packedLength = IRIS_Protocol_Pack(command, unpackResult, responseBuffer, sendBuffer);
                        DebugSerial.write(sendBuffer, packedLength);
                        break;
                    }
                }
            } else {
                DebugSerial.print("Unpack failed, error code: ");
                DebugSerial.println(unpackResult);
            }
        }
    }
}