#include <ArduinoJson.h>
#include "IRIS_Method.h"
#include "vsmd_parser.h"

VSMDParser parser;

#define DEBUG_TX_PIN 17  // TX
#define DEBUG_RX_PIN 16  // RX


HardwareSerial DebugSerial(1);  // 使用UART1

#define BUFFER_SIZE 1024
#define JSON_BUFFER_SIZE 512

// 全局变量声明
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 currentPosition = 0;
int32_t targetPosition = 0;
int32_t pulsePerSecond = 1200;
bool isMoving = false;
bool isBackZero = false;

// 设备配置参数
struct DeviceConfig {
    int zmd = 1;        // cfg zmd
    int snr = 0;        // cfg snr
    int osv = 0;        // cfg osv
    int zsd = 1200;     // cfg zsd
    int zsp = 2400;     // cfg zsp
    int dmd = 2;        // cfg dmd
    int dar = 5;        // cfg dar
    int msr = 1;        // cfg msr
    int msv = 1;        // cfg msv
    int psr = 1;        // cfg psr
    int psv = 1;        // cfg psv
    int bdr = 115200;   // cfg bdr
    int cid = 1;        // cfg cid
    int mcs = 5;        // cfg mcs
    int spd = 1200;     // cfg spd
    int acc = 12000;    // cfg acc
    int dec = 12000;    // cfg dec
    float cra = 0.8;    // cfg cra
    float crn = 0.4;    // cfg crn
    float crh = 0.0;    // cfg crh
} deviceConfig;


void processJsonCommand(const char* jsonString);
void processStringCommand(const char* stringData);
void sendJsonResponse(const char* cmd, const char* status, int value = -1);
void sendStringResponse(const char* response);
void handle_cfg_cmd();
void handle_ena_cmd();
void handle_off_cmd();
void handle_mov_cmd();
void handle_pos_cmd(int value);
void handle_rmv_cmd(int value);
void handle_pps_cmd(int value = -1);
void handle_org_cmd();
void handle_stp_cmd();
void handle_zero_cmd(const char* value);
void handle_dev_cmd();
void printDeviceStatus();

void setup() {
    // 调试串口
    DebugSerial.begin(115200, SERIAL_8N1, DEBUG_RX_PIN, DEBUG_TX_PIN);
    
    // 主串口（用于数据通信）
    Serial.begin(115200);
    
    DebugSerial.println("VSMD INIT");
    DebugSerial.println("Commands:dev, cfg, ena, off, mov, pos, rmv, pps, org, stp, zero");
    printDeviceStatus();
}

void loop() {
    if (DebugSerial.available() > 0) {
        int bytesRead = DebugSerial.readBytes(receiveBuffer, sizeof(receiveBuffer));
        
        // // 添加调试信息
        // DebugSerial.print("Bytes read: ");
        // DebugSerial.println(bytesRead);
        // DebugSerial.print("Raw data: ");
        // for(int i = 0; i < bytesRead; i++) {
        //     DebugSerial.print(receiveBuffer[i], HEX);
        //     DebugSerial.print(" ");
        // }
        // DebugSerial.println();
        
        // bytesProcessed = IRIS_Cut_Befor_Header(receiveBuffer, bytesRead);
        
        // // 添加更多调试信息
        // DebugSerial.print("Bytes after header cut: ");
        // DebugSerial.println(bytesProcessed);

        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'; // 确保字符串以null结尾
                        
                        DebugSerial.print("Received JSON: ");
                        DebugSerial.println(jsonString);
                        
                        processJsonCommand(jsonString);
                        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: { // 自定义二进制数据 (0x02-0xff)
                        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);
            }
        }
    }
}

void processJsonCommand(const char* jsonString) {
    StaticJsonDocument<JSON_BUFFER_SIZE> doc;
    DeserializationError error = deserializeJson(doc, jsonString);
    
    if (error) {
        DebugSerial.print("JSON parsing failed: ");
        DebugSerial.println(error.c_str());
        sendJsonResponse("error", "JSON_PARSE_ERROR");
        return;
    }
    
    // 使用 command 字段替代 cmd
    const char* command = doc["command"];
    if (command == nullptr) {
        sendJsonResponse("error", "NO_COMMAND_FIELD");
        return;
    }
    
    // 获取 PA 参数对象（可选）
    JsonObject params = doc["PA"];

    // 根据command执行相应操作
    if (strcmp(command, "cfg") == 0) {
        handle_cfg_cmd();
    } else if (strcmp(command, "ena") == 0) {
        handle_ena_cmd();
    } else if (strcmp(command, "off") == 0) {
        handle_off_cmd();
    } else if (strcmp(command, "mov") == 0) {
        handle_mov_cmd();
    } else if (strcmp(command, "pos") == 0) {
        // 修正：正确的类型转换
        int value = params.containsKey("value") ? params["value"].as<int>() : 0;
        handle_pos_cmd(value);
    } else if (strcmp(command, "rmv") == 0) {
        int value = params.containsKey("value") ? params["value"].as<int>() : 0;
        handle_rmv_cmd(value);
    } else if (strcmp(command, "pps") == 0) {
        if (params.containsKey("value")) {
            int value = params["value"].as<int>();
            handle_pps_cmd(value);
        } else {
            handle_pps_cmd();
        }
    } else if (strcmp(command, "org") == 0) {
        handle_org_cmd();
    } else if (strcmp(command, "stp") == 0) {
        handle_stp_cmd();
    } else if (strcmp(command, "zero") == 0) {
        // 修正：正确的字符串类型转换
        const char* value = params.containsKey("value") ? params["value"].as<const char*>() : "";
        handle_zero_cmd(value);
    } else if (strcmp(command, "dev") == 0) {
        handle_dev_cmd();
    } else {
        sendJsonResponse(command, "UNKNOWN_COMMAND");
    }
}

void processStringCommand(const char* stringData) {
    // 处理字符串命令
    String response = "Echo: " + String(stringData);
    sendStringResponse(response.c_str());
}

void sendJsonResponse(const char* command, const char* status, int value) {
    StaticJsonDocument<JSON_BUFFER_SIZE> responseDoc;
    
    // 修改：使用新的JSON格式
    responseDoc["command"] = command;  // 使用 command 替代 cmd
    responseDoc["ST"] = status;        // 使用 ST 替代 status
    
    // 如果有参数值，添加到 PA 对象中
    if (value != -1) {
        JsonObject params = responseDoc.createNestedObject("PA");
        params["value"] = value;
    }
    
    // 可选：添加其他信息
    responseDoc["Other"] = "";
    
    char jsonResponse[JSON_BUFFER_SIZE];
    serializeJson(responseDoc, jsonResponse, sizeof(jsonResponse));
    
    // 打包并发送JSON响应
    DebugSerial.print("JSON Response: ");
    packedLength = IRIS_Protocol_Pack(0x00, strlen(jsonResponse), (uint8_t*)jsonResponse, sendBuffer);
    DebugSerial.write(sendBuffer, packedLength);

    // DebugSerial.println();
    
    // DebugSerial.print("Sent JSON Response: ");
    // DebugSerial.println(jsonResponse);
}

void sendStringResponse(const char* response) {
    // 打包并发送字符串响应
    packedLength = IRIS_Protocol_Pack(0x01, strlen(response), (uint8_t*)response, sendBuffer);
    DebugSerial.write(sendBuffer, packedLength);
    
    DebugSerial.print("Sent String Response: ");
    DebugSerial.println(response);
}

void handle_cfg_cmd() {
    DebugSerial.println("Configuration requested");
    Serial.println("cfg\n");
    // 返回配置相关信息
    sendJsonResponse("cfg", "OK");
}

// void handle_dev_cmd() {
//     DebugSerial.println("dev_requested");
//     Serial.println("dev\n");

//     //test VSMD数据
//     uint8_t testData[] = {
//         0xFF, 0x00, 0x01, 0x56, 0x53, 0x4D, 0x44, 0x31, 0x34, 0x32, 0x2D, 0x30, 0x32, 0x35, 0x54, 0x2D, 0x31, 0x2E, 0x31, 0x2E, 0x30, 0x32, 0x30, 0x2E, 0x32, 0x31, 0x31, 0x31, 0x31, 0x35, 0x00, 0x42, 0xFE
//     };
//     // 解析
//     DebugSerial.println("Parsing VSMD test data:");
//     parser.parse(testData, sizeof(testData));
//     // 返回设备相关信息
//     sendJsonResponse("dev", "OK");
// }
void handle_dev_cmd() {
    DebugSerial.println("dev_requested");
    Serial.println("dev\n");
    
    // vsmd test data
    uint8_t testData[] = {
        0xFF, 0x00, 0x01, 0x56, 0x53, 0x4D, 0x44, 0x31, 0x34, 0x32, 0x2D, 0x30, 0x32, 0x35, 0x54, 0x2D,0x31, 0x2E, 0x31, 0x2E, 0x30, 0x32, 0x30, 0x2E, 0x32, 0x31, 0x31, 0x31, 0x31, 0x35, 0x00, 0x42, 0xFE
    };
    
    // parseInfo获取解析结果
    String deviceInfo = parser.parseInfo(testData, sizeof(testData));
    
    // 创建带有解析数据的JSON响应
    StaticJsonDocument<JSON_BUFFER_SIZE> responseDoc;
    responseDoc["command"] = "dev";
    responseDoc["ST"] = "OK";
    responseDoc["Other"] = deviceInfo;
    
    char jsonResponse[JSON_BUFFER_SIZE];
    serializeJson(responseDoc, jsonResponse, sizeof(jsonResponse));
    
    // 打包并发送JSON响应
    packedLength = IRIS_Protocol_Pack(0x00, strlen(jsonResponse), (uint8_t*)jsonResponse, sendBuffer);
    DebugSerial.write(sendBuffer, packedLength);
    
    DebugSerial.println();

    DebugSerial.print("Sent JSON Response: ");
    DebugSerial.println(jsonResponse);
}

void handle_ena_cmd() {
    deviceEnabled = true;
    DebugSerial.println("Device enabled");
    Serial.print("ena\n");
    sendJsonResponse("ena", "OK");
}

void handle_off_cmd() {
    deviceEnabled = false;
    isMoving = false;
    DebugSerial.println("Device disabled");
    Serial.print("off\n");
    sendJsonResponse("off", "OK");
}

void handle_mov_cmd() {
    if (!deviceEnabled) {
        sendJsonResponse("mov", "DEVICE_DISABLED");
        return;
    }
    
    isMoving = true;
    DebugSerial.println("Starting movement");
    Serial.print("mov\n");
    sendJsonResponse("mov", "OK");
}

void handle_pos_cmd(int value) {
    if (!deviceEnabled) {
        sendJsonResponse("pos", "DEVICE_DISABLED");
        return;
    }
    
    targetPosition = value;
    currentPosition = value; // 模拟立即到达
    DebugSerial.print("Position set to: ");
    DebugSerial.println(value);
    sendJsonResponse("pos", "OK", value);
}

void handle_rmv_cmd(int value) {
    if (!deviceEnabled) {
        sendJsonResponse("rmv", "DEVICE_DISABLED");
        return;
    }
    
    currentPosition += value;
    DebugSerial.print("Relative move by: ");
    DebugSerial.print(value);
    DebugSerial.print(", new position: ");
    DebugSerial.println(currentPosition);
    sendJsonResponse("rmv", "OK", currentPosition);
}

void handle_pps_cmd(int value) {
    if (value == -1) {
        // SET PPS
        Serial.print("pps\n");
        sendJsonResponse("pps", "OK", pulsePerSecond);
    } else {
        // 设置PPS值
        pulsePerSecond = value;
        DebugSerial.print("Pulse per second set to: ");
        DebugSerial.println(value);
        sendJsonResponse("pps", "OK", value);
    }
}

void handle_org_cmd() {
    if (!deviceEnabled) {
        sendJsonResponse("org", "DEVICE_DISABLED");
        return;
    }
    
    currentPosition = 0;
    targetPosition = 0;
    DebugSerial.println("Returned to origin");
    Serial.print("org\n");
    sendJsonResponse("org", "OK", 0);
}

void handle_stp_cmd() {
    isMoving = false;
    DebugSerial.println("Movement stopped");
    Serial.print("stp\n");
    sendJsonResponse("stp", "OK");
}

void handle_zero_cmd(const char* value) {
    if (strcmp(value, "start") == 0) {
        Serial.print("zero start\n");
        isBackZero = true;
        DebugSerial.println("Zero started");
        sendJsonResponse("zero", "ZERO_STARTED");
    } else if (strcmp(value, "stop") == 0) {
        Serial.print("zero stop\n");
        isBackZero = false;
        currentPosition = 0;
        DebugSerial.println("Zero completed");
        sendJsonResponse("zero", "ZERO_COMPLETED");
    } else {
        sendJsonResponse("zero", "INVALID_VALUE");
    }
}

void printDeviceStatus() {
    DebugSerial.println("\n=== Device Status ===");
    DebugSerial.print("Enabled: ");
    DebugSerial.println(deviceEnabled ? "Yes" : "No");
    DebugSerial.print("Current Position: ");
    DebugSerial.println(currentPosition);
    DebugSerial.print("Target Position: ");
    DebugSerial.println(targetPosition);
    DebugSerial.print("Pulse Per Second: ");
    DebugSerial.println(pulsePerSecond);
    DebugSerial.print("Moving: ");
    DebugSerial.println(isMoving ? "Yes" : "No");
    DebugSerial.print("Zero Backing: ");
    DebugSerial.println(isBackZero ? "Yes" : "No");
    DebugSerial.println("==================\n");
}