#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,
              uint32_t gps_time, float latitude, float longitude, float altitude)
{
    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;
    
    // 设置GPS数据
    packet->gps_time = gps_time;
    packet->gps_latitude = latitude;
    packet->gps_longitude = longitude;
    packet->gps_altitude = altitude;
}

uint8_t ValidatePacket(const DataPacket_t *packet)
{
    if (packet == NULL) return 0;
    
    // 检查包头
    if (packet->start_byte != PACKET_START_BYTE) 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;
    
    // 精简版数据包无校验和，仅检查包头
    return 1; // 包头正确，认为有效
}

void PackCorrectedData(CorrectedDataPacket_t *packet, float x, float y, float z,
                       uint32_t gps_time, float latitude, float longitude, float altitude)
{
    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;
    
    // 设置GPS数据
    packet->gps_time = gps_time;
    packet->gps_latitude = latitude;
    packet->gps_longitude = longitude;
    packet->gps_altitude = altitude;
}

void PackCorrectedDataWithGPS(CorrectedDataPacketWithGPS_t *packet, float x, float y, float z,
                              uint32_t gps_time, float latitude, float longitude, float altitude)
{
    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;
    
    // 设置GPS数据
    packet->gps_time = gps_time;
    packet->gps_latitude = latitude;
    packet->gps_longitude = longitude;
    packet->gps_altitude = altitude;
}

uint8_t ValidateCorrectedPacketWithGPS(const CorrectedDataPacketWithGPS_t *packet)
{
    if (packet == NULL) return 0;
    
    // 检查包头
    if (packet->start_byte != PACKET_START_BYTE) return 0;
    
    // 精简版数据包无校验和，仅检查包头
    // 可以添加简单的数据合理性检查
    // 例如：检查GPS坐标是否在有效范围内等
    
    return 1; // 包头正确，认为有效
}