#include "data_packet.h"
#include "stddef.h"
#include "stm32f4xx_hal.h"

// CRC16-MODBUS算法实现
uint16_t Calculate_CRC16(const uint8_t *data, uint16_t len) {
    uint16_t crc = 0xFFFF;
    for (uint16_t i = 0; i < len; i++) {
        crc ^= data[i];
        for (uint8_t j = 0; j < 8; j++) {
            if (crc & 0x0001) {
                crc = (crc >> 1) ^ 0xA001;
            } else {
                crc = crc >> 1;
            }
        }
    }
    return crc;
}

void PackData(DataPacket_t *packet, int32_t adc1, int32_t adc2, int32_t adc3)
{
    if (packet == NULL) return;
    
    // 设置包头
    packet->start_byte = PACKET_START_BYTE;
    
    // 设置时间戳
    packet->timestamp = HAL_GetTick();
    
    // 设置ADC数据
    packet->adc_data1 = adc1;
    packet->adc_data2 = adc2;
    packet->adc_data3 = adc3;
    
    // 计算校验和 (不包括校验和字段本身和包尾)
    uint16_t checksum = Calculate_CRC16((uint8_t*)packet, 
                                       sizeof(DataPacket_t) - sizeof(packet->checksum) - sizeof(packet->end_byte));
    packet->checksum = checksum;
    
    // 设置包尾
    packet->end_byte = PACKET_END_BYTE;
}

uint8_t ValidatePacket(const DataPacket_t *packet)
{
    if (packet == NULL) return 0;
    
    // 检查包头
    if (packet->start_byte != PACKET_START_BYTE) return 0;
    
    // 检查包尾
    if (packet->end_byte != PACKET_END_BYTE) return 0;
    
    // 验证校验和
    uint16_t calculated_crc = Calculate_CRC16((uint8_t*)packet,
                                             sizeof(DataPacket_t) - sizeof(packet->checksum) - sizeof(packet->end_byte));
    
    if (calculated_crc != packet->checksum) return 0;
    
    return 1; // 验证通过
}

uint8_t ValidateCorrectedPacket(const CorrectedDataPacket_t *packet)
{
    if (packet == NULL) return 0;
    
    // 检查包头
    if (packet->start_byte != PACKET_START_BYTE) return 0;
    
    // 检查包尾
    if (packet->end_byte != PACKET_END_BYTE) return 0;
    
    // 验证校验和
    uint16_t calculated_crc = Calculate_CRC16((uint8_t*)packet,
                                             sizeof(CorrectedDataPacket_t) - sizeof(packet->checksum) - sizeof(packet->end_byte));
    
    if (calculated_crc != packet->checksum) return 0;
    
    return 1; // 验证通过
}

void PackCorrectedData(CorrectedDataPacket_t *packet, float x, float y, float z)
{
    if (packet == NULL) return;
    
    // 设置包头
    packet->start_byte = PACKET_START_BYTE;
    
    // 设置时间戳
    packet->timestamp = HAL_GetTick();
    
    // 设置校正后数据
    packet->corrected_x = x;
    packet->corrected_y = y;
    packet->corrected_z = z;
    
    // 计算校验和
    uint16_t checksum = Calculate_CRC16((uint8_t*)packet,
                                       sizeof(CorrectedDataPacket_t) - sizeof(packet->checksum) - sizeof(packet->end_byte));
    packet->checksum = checksum;
    
    // 设置包尾
    packet->end_byte = PACKET_END_BYTE;
}