#include "vsmd_parser.h"
#include "ble.h"

extern HardwareSerial DebugSerial;

// BCC 校验函数
uint8_t VSMDParser::bcc_checksum(uint8_t* data, int size) {
    uint8_t sum = 0;
    for (int index = 0; index < size; index++) {
        sum ^= data[index];
    }
    return sum;
}

// 构造函数
VSMDParser::VSMDParser() {
    // 可选初始化
}

// 数据转换函数
void VSMDParser::convert(uint8_t* data, value_info& info) {
    info.udata = data[0];
    info.udata <<= 7;
    info.udata |= data[1];
    info.udata <<= 7;
    info.udata |= data[2];
    info.udata <<= 7;
    info.udata |= data[3];
    info.udata <<= 7;
    info.udata |= data[4];
}

// VSMD 解析函数
void VSMDParser::parse(uint8_t* data, int size) {
    // if (size < 10) {
    //     DebugSerial.println("data too short");
    //     return;
    // }

    if (data[0] == 0xFF && data[size - 1] == 0xFE) {
        uint8_t checksum = bcc_checksum(&data[1], size - 3);
        if (checksum == data[size - 2]) {
            switch (data[2]) {
                case 1: { // dev 01
                    DebugSerial.print("dev:");
                    int start = 0;
                    while (start < size && data[start] != 'V') start++;
                    if (start < size) {
                        int end = start;
                        while (end < size && data[end] >= 32 && data[end] < 127) end++;
                        for (int i = start; i < end; i++) {
                            DebugSerial.print((char)data[i]);
                        }
                        DebugSerial.println();
                    } else {
                        DebugSerial.println("dev err");
                    }
                    break;
                }
                case 2: { // sts 02
                    float spd = 0;
                    int pos = 0;
                    uint32_t sts = 0;
                    value_info info;

                    convert(&data[3], info);
                    spd = info.fdata;

                    convert(&data[8], info);
                    pos = info.idata;

                    convert(&data[13], info);
                    sts = info.udata;
            
                    DebugSerial.print("Speed: ");
                    DebugSerial.print(spd);
                    DebugSerial.print(", Position: ");
                    DebugSerial.print(pos);
                    DebugSerial.print(", Status: ");
                    DebugSerial.println(sts);

                    break;
                }
                case 3: { // cfg 03
                    DebugSerial.print("cfg:");
                    int payloadStart = 3;
                    int payloadEnd = size - 3;

                    if (payloadStart < payloadEnd) {
                        for (int i = payloadStart; i < payloadEnd; i++) {
                            DebugSerial.print((char)data[i]);
                        }
                        DebugSerial.println();
                    } else {
                        DebugSerial.println(" 数据长度不足");
                    }
                    break;
                }
                case 4: { // demo 04
                    DebugSerial.print("demo:");
                    int payloadStart = 3;
                    int payloadEnd = size - 3;

                    if (payloadStart < payloadEnd) {
                        for (int i = payloadStart; i < payloadEnd; i++) {
                            DebugSerial.print((char)data[i]);
                        }
                        DebugSerial.println();
                    } else {
                        DebugSerial.println(" 数据长度不足");
                    }
                    break;
                }
                default:
                    DebugSerial.print("Unknow cmd: ");
                    DebugSerial.println(data[2]);
                    break;
            }
        } else {
            return;
            DebugSerial.println("BCC ERROR");
            DebugSerial.print("CHECK BCC: ");
            DebugSerial.println(checksum);
            DebugSerial.print("RES BCC: ");
            DebugSerial.println(data[size - 2], HEX);
        }
    } else {
        DebugSerial.println("Head or  Tail ERROR");
        DebugSerial.print("Receive Data: ");
        for (int i = 0; i < size; i++) {
            DebugSerial.print(data[i], HEX);
            DebugSerial.print(" ");
        }
        DebugSerial.println();
    }
}


// 新增parseInfo函数,返回解析结果
String VSMDParser::parseInfo(uint8_t* data, int size) {
    String result = "";
    
    if (data[0] == 0xFF && data[size - 1] == 0xFE) {
        uint8_t checksum = bcc_checksum(&data[1], size - 3);
        if (checksum == data[size - 2]) {
            switch (data[2]) {
                case 1: { // dev 01
                    int start = 0;
                    while (start < size && data[start] != 'V') start++;
                    if (start < size) {
                        int end = start;
                        while (end < size && data[end] >= 32 && data[end] < 127) end++;
                        for (int i = start; i < end; i++) {
                            result += (char)data[i];
                        }
                    } else {
                        result = "dev err";
                    }
                    break;
                }
                case 2: { // sts 02
                    float spd = 0;
                    int pos = 0;
                    uint32_t sts = 0;
                    value_info info;

                    convert(&data[3], info);
                    spd = info.fdata;

                    convert(&data[8], info);
                    pos = info.idata;

                    convert(&data[13], info);
                    sts = info.udata;
            
                    result = "Speed: " + String(spd) + ", Position: " + String(pos) + ", Status: " + String(sts);
                    break;
                }
                case 3: { // cfg 03
                    int payloadStart = 3;
                    int payloadEnd = size - 3;

                    if (payloadStart < payloadEnd) {
                        for (int i = payloadStart; i < payloadEnd; i++) {
                            result += (char)data[i];
                        }
                    } else {
                        result = "Not enough data";

                    }
                    break;
                }
                case 4: { // demo 04
                    int payloadStart = 3;
                    int payloadEnd = size - 3;

                    if (payloadStart < payloadEnd) {
                        for (int i = payloadStart; i < payloadEnd; i++) {
                            result += (char)data[i];
                        }
                    } else {
                        result = "Not enough data";
                    }
                    break;
                }
                default:
                    result = "Unknown cmd: " + String(data[2]);
                    break;
            }
        } else {
            result = "BCC ERROR";
        }
    } else {
        result = "Head or Tail ERROR";
    }
    
    return result;
}

// ... existing code ...
// /**************************** ble read ************************************ */
// void VSMDParser::parse(uint8_t* data, int size) {
//     if (data[0] == 0xFF && data[size - 1] == 0xFE) {
//         uint8_t checksum = bcc_checksum(&data[1], size - 3);
//         if (checksum == data[size - 2]) {
//             switch (data[2]) {
//                 case 1: { // dev
//                     String response = "dev:";
//                     int start = 0;
//                     while (start < size && data[start] != 'V') start++;
//                     if (start < size) {
//                         int end = start;
//                         while (end < size && data[end] >= 32 && data[end] < 127) end++;
//                         for (int i = start; i < end; i++) {
//                             response += (char)data[i];
//                         }
//                         response += "\n";
//                     } else {
//                         response += "dev err\n";
//                     }

//                     // 发送通过蓝牙
//                     if (pCharacteristic) {
//                         pCharacteristic->setValue(response.c_str());
//                         pCharacteristic->notify();
//                     }
//                     break;
//                 }
//                 case 2: { // sts
//                     float spd = 0;
//                     int pos = 0;
//                     uint32_t sts = 0;
//                     value_info info;

//                     convert(&data[3], info);
//                     spd = info.fdata;

//                     convert(&data[8], info);
//                     pos = info.idata;

//                     convert(&data[13], info);
//                     sts = info.udata;

//                     String response = "Speed: " + String(spd) + ", Position: " + String(pos) + ", Status: " + String(sts) + "\n";

//                     // 发送通过蓝牙
//                     if (pCharacteristic) {
//                         pCharacteristic->setValue(response.c_str());
//                         pCharacteristic->notify();
//                     }
//                     break;
//                 }
//                 case 3: { // cfg
//                     String response = "cfg:";
//                     int payloadStart = 3;
//                     int payloadEnd = size - 3;

//                     if (payloadStart < payloadEnd) {
//                         for (int i = payloadStart; i < payloadEnd; i++) {
//                             response += (char)data[i];
//                         }
//                         response += "\n";
//                     } else {
//                         response += " 数据长度不足\n";
//                     }

//                     // 发送通过蓝牙
//                     if (pCharacteristic) {
//                         pCharacteristic->setValue(response.c_str());
//                         pCharacteristic->notify();
//                     }
//                     break;
//                 }
//                 case 4: { // demo
//                     String response = "demo:";
//                     int payloadStart = 3;
//                     int payloadEnd = size - 3;

//                     if (payloadStart < payloadEnd) {
//                         for (int i = payloadStart; i < payloadEnd; i++) {
//                             response += (char)data[i];
//                         }
//                         response += "\n";
//                     } else {
//                         response += " 数据长度不足\n";
//                     }

//                     // 发送通过蓝牙
//                     if (pCharacteristic) {
//                         pCharacteristic->setValue(response.c_str());
//                         pCharacteristic->notify();
//                     }
//                     break;
//                 }
//                 default:
//                     String response = "未知命令: ";
//                     response += data[2];
//                     response += "\n";

//                     // 发送通过蓝牙
//                     if (pCharacteristic) {
//                         pCharacteristic->setValue(response.c_str());
//                         pCharacteristic->notify();
//                     }
//                     break;
//             }
//         } else {
//             // BCC 校验失败
//             String response = "BCC ERROR\nCHECK BCC: ";
//             response += checksum;
//             response += "\nRES BCC: ";
//             response += data[size - 2], HEX;
//             response += "\n";

//             // 发送通过蓝牙
//             if (pCharacteristic) {
//                 pCharacteristic->setValue(response.c_str());
//                 pCharacteristic->notify();
//             }
//         }
//     } else {
//         String response = "Head or Tail ERROR\nReceive Data: ";
//         for (int i = 0; i < size; i++) {
//             response += data[i], HEX;
//             response += " ";
//         }
//         response += "\n";

//         // 发送通过蓝牙
//         if (pCharacteristic) {
//             pCharacteristic->setValue(response.c_str());
//             pCharacteristic->notify();
//         }
//     }
// }