#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];
}

// 解析函数
// 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
//                     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
//                     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
//                     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
//                     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("未知命令: ");
//                     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();
//     }
// }

/**************************** 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();
        }
    }
}