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


#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 handleConfigCommand(JsonObject& doc);
void handleEnableCommand();
void handleOffCommand();
void handleMoveCommand();
void handlePositionCommand(int value);
void handleRelativeMoveCommand(int value);
void handlePulsePerSecondCommand(int value = -1);
void handleOriginCommand();
void handleStopCommand();
void handleZeroCommand(const char* value);
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: cfg, ena, off, mov, pos, rmv, pps, org, stp, zero");
    printDeviceStatus();
}

void loop() {
    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'; // 确保字符串以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;
    }
    
    const char* cmd = doc["cmd"];
    if (cmd == nullptr) {
        sendJsonResponse("error", "NO_CMD_FIELD");
        return;
    }
    
    // 根据cmd执行相应操作
    if (strcmp(cmd, "cfg") == 0) {
        JsonObject configObj = doc.as<JsonObject>();
        handleConfigCommand(configObj);
    } else if (strcmp(cmd, "ena") == 0) {
        handleEnableCommand();
    } else if (strcmp(cmd, "off") == 0) {
        handleOffCommand();
    } else if (strcmp(cmd, "mov") == 0) {
        handleMoveCommand();
    } else if (strcmp(cmd, "pos") == 0) {
        int value = doc["value"] | 0;
        handlePositionCommand(value);
    } else if (strcmp(cmd, "rmv") == 0) {
        int value = doc["value"] | 0;
        handleRelativeMoveCommand(value);
    } else if (strcmp(cmd, "pps") == 0) {
        if (doc.containsKey("value")) {
            int value = doc["value"];
            handlePulsePerSecondCommand(value);
        } else {
            handlePulsePerSecondCommand();
        }
    } else if (strcmp(cmd, "org") == 0) {
        handleOriginCommand();
    } else if (strcmp(cmd, "stp") == 0) {
        handleStopCommand();
    } else if (strcmp(cmd, "zero") == 0) {
        const char* value = doc["value"] | "";
        handleZeroCommand(value);
    } else {
        sendJsonResponse(cmd, "UNKNOWN_COMMAND");
    }
}

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

void sendJsonResponse(const char* cmd, const char* status, int value) {
    StaticJsonDocument<JSON_BUFFER_SIZE> responseDoc;
    responseDoc["cmd"] = cmd;
    responseDoc["status"] = status;
    
    if (value != -1) {
        responseDoc["value"] = value;
    }
    
    // 设备状态信息
    responseDoc["enabled"] = deviceEnabled;
    responseDoc["position"] = currentPosition;
    responseDoc["moving"] = isMoving;
    
    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 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 handleConfigCommand(JsonObject& doc) {
    DebugSerial.println("Processing configuration command...");
    
    // 创建响应JSON对象
    StaticJsonDocument<JSON_BUFFER_SIZE> responseDoc;
    responseDoc["cmd"] = "cfg";
    responseDoc["status"] = "OK";
    
    // 添加设备状态信息
    responseDoc["enabled"] = deviceEnabled;
    responseDoc["position"] = currentPosition;
    responseDoc["moving"] = isMoving;
    
    // 创建配置参数对象
    JsonObject configParams = responseDoc.createNestedObject("config");
    
    // 处理配置参数
    if (doc.containsKey("params")) {
        JsonObject params = doc["params"];
        
        // 更新配置参数并添加到响应中
        if (params.containsKey("cfg zmd")) {
            deviceConfig.zmd = params["cfg zmd"];
            configParams["cfg zmd"] = deviceConfig.zmd;
        }
        if (params.containsKey("cfg snr")) {
            deviceConfig.snr = params["cfg snr"];
            configParams["cfg snr"] = deviceConfig.snr;
        }
        if (params.containsKey("cfg osv")) {
            deviceConfig.osv = params["cfg osv"];
            configParams["cfg osv"] = deviceConfig.osv;
        }
        if (params.containsKey("cfg zsd")) {
            deviceConfig.zsd = params["cfg zsd"];
            configParams["cfg zsd"] = deviceConfig.zsd;
        }
        if (params.containsKey("cfg zsp")) {
            deviceConfig.zsp = params["cfg zsp"];
            configParams["cfg zsp"] = deviceConfig.zsp;
        }
        if (params.containsKey("cfg dmd")) {
            deviceConfig.dmd = params["cfg dmd"];
            configParams["cfg dmd"] = deviceConfig.dmd;
        }
        if (params.containsKey("cfg dar")) {
            deviceConfig.dar = params["cfg dar"];
            configParams["cfg dar"] = deviceConfig.dar;
        }
        if (params.containsKey("cfg msr")) {
            deviceConfig.msr = params["cfg msr"];
            configParams["cfg msr"] = deviceConfig.msr;
        }
        if (params.containsKey("cfg msv")) {
            deviceConfig.msv = params["cfg msv"];
            configParams["cfg msv"] = deviceConfig.msv;
        }
        if (params.containsKey("cfg psr")) {
            deviceConfig.psr = params["cfg psr"];
            configParams["cfg psr"] = deviceConfig.psr;
        }
        if (params.containsKey("cfg psv")) {
            deviceConfig.psv = params["cfg psv"];
            configParams["cfg psv"] = deviceConfig.psv;
        }
        if (params.containsKey("cfg bdr")) {
            deviceConfig.bdr = params["cfg bdr"];
            configParams["cfg bdr"] = deviceConfig.bdr;
        }
        if (params.containsKey("cfg cid")) {
            deviceConfig.cid = params["cfg cid"];
            configParams["cfg cid"] = deviceConfig.cid;
        }
        if (params.containsKey("cfg mcs")) {
            deviceConfig.mcs = params["cfg mcs"];
            configParams["cfg mcs"] = deviceConfig.mcs;
        }
        if (params.containsKey("cfg spd")) {
            deviceConfig.spd = params["cfg spd"];
            configParams["cfg spd"] = deviceConfig.spd;
        }
        if (params.containsKey("cfg acc")) {
            deviceConfig.acc = params["cfg acc"];
            configParams["cfg acc"] = deviceConfig.acc;
        }
        if (params.containsKey("cfg dec")) {
            deviceConfig.dec = params["cfg dec"];
            configParams["cfg dec"] = deviceConfig.dec;
        }
        if (params.containsKey("cfg cra")) {
            deviceConfig.cra = params["cfg cra"];
            configParams["cfg cra"] = deviceConfig.cra;
        }
        if (params.containsKey("cfg crn")) {
            deviceConfig.crn = params["cfg crn"];
            configParams["cfg crn"] = deviceConfig.crn;
        }
        if (params.containsKey("cfg crh")) {
            deviceConfig.crh = params["cfg crh"];
            configParams["cfg crh"] = deviceConfig.crh;
        }
        
        DebugSerial.println("Configuration updated successfully");
    } else {
        // 如果没有params，返回所有当前配置
        configParams["cfg zmd"] = deviceConfig.zmd;
        configParams["cfg snr"] = deviceConfig.snr;
        configParams["cfg osv"] = deviceConfig.osv;
        configParams["cfg zsd"] = deviceConfig.zsd;
        configParams["cfg zsp"] = deviceConfig.zsp;
        configParams["cfg dmd"] = deviceConfig.dmd;
        configParams["cfg dar"] = deviceConfig.dar;
        configParams["cfg msr"] = deviceConfig.msr;
        configParams["cfg msv"] = deviceConfig.msv;
        configParams["cfg psr"] = deviceConfig.psr;
        configParams["cfg psv"] = deviceConfig.psv;
        configParams["cfg bdr"] = deviceConfig.bdr;
        configParams["cfg cid"] = deviceConfig.cid;
        configParams["cfg mcs"] = deviceConfig.mcs;
        configParams["cfg spd"] = deviceConfig.spd;
        configParams["cfg acc"] = deviceConfig.acc;
        configParams["cfg dec"] = deviceConfig.dec;
        configParams["cfg cra"] = deviceConfig.cra;
        configParams["cfg crn"] = deviceConfig.crn;
        configParams["cfg crh"] = deviceConfig.crh;
    }
    
    // 序列化并发送响应
    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.print("Sent Config Response: ");
    DebugSerial.println(jsonResponse);
}

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

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

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

void handlePositionCommand(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 handleRelativeMoveCommand(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 handlePulsePerSecondCommand(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 handleOriginCommand() {
    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 handleStopCommand() {
    isMoving = false;
    DebugSerial.println("Movement stopped");
    Serial.print("stp\n");
    sendJsonResponse("stp", "OK");
}

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