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

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

// ─── 内部辅助宏：填充GPS位置字段（仅在宏开启时） ─────────────────────────────
#ifdef ENABLE_GPS_POSITION
#define FILL_GPS_POSITION(pkt, lat, lon, alt) \
    do { (pkt)->gps_latitude = (lat); (pkt)->gps_longitude = (lon); (pkt)->gps_altitude = (alt); } while(0)
#else
#define FILL_GPS_POSITION(pkt, lat, lon, alt)  // 空操作
#endif

// ─── 原始ADC数据包 ────────────────────────────────────────────────────────────

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_TYPE_STANDARD;
    packet->adc_data1  = adc1;
    packet->adc_data2  = adc2;
    packet->adc_data3  = adc3;
    packet->gps_time   = gps_time;
    FILL_GPS_POSITION(packet, latitude, longitude, altitude);
}

void PackDataWithOptic(DataPacketWithOptic_t *packet,
                       int32_t adc1, int32_t adc2, int32_t adc3,
                       uint32_t gps_time, float latitude, float longitude, float altitude,
                       uint32_t optical_mag)
{
    if (packet == NULL) return;
    packet->start_byte  = PACKET_TYPE_WITH_OPTIC;
    packet->adc_data1   = adc1;
    packet->adc_data2   = adc2;
    packet->adc_data3   = adc3;
    packet->gps_time    = gps_time;
    FILL_GPS_POSITION(packet, latitude, longitude, altitude);
    packet->optical_mag = optical_mag;
}

// ─── 校正数据包 ───────────────────────────────────────────────────────────────

void PackCorrectedData(CorrectedDataPacket_t *packet,
                       float x, float y, float z,
                       uint32_t gps_time, float latitude, float longitude, float altitude)
{
    PackCorrectedDataWithGPS(packet, x, y, z, gps_time, latitude, longitude, 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_TYPE_STANDARD;
    packet->corrected_x = x;
    packet->corrected_y = y;
    packet->corrected_z = z;
    packet->gps_time    = gps_time;
    FILL_GPS_POSITION(packet, latitude, longitude, altitude);
}

void PackCorrectedDataWithOptic(CorrectedDataPacketWithOptic_t *packet,
                                float x, float y, float z,
                                uint32_t gps_time, float latitude, float longitude, float altitude,
                                uint32_t optical_mag)
{
    if (packet == NULL) return;
    packet->start_byte  = PACKET_TYPE_WITH_OPTIC;
    packet->corrected_x = x;
    packet->corrected_y = y;
    packet->corrected_z = z;
    packet->gps_time    = gps_time;
    FILL_GPS_POSITION(packet, latitude, longitude, altitude);
    packet->optical_mag = optical_mag;
}

// ─── 验证函数 ─────────────────────────────────────────────────────────────────

uint8_t ValidatePacket(const DataPacket_t *packet)
{
    if (packet == NULL) return 0;
    return (packet->start_byte == PACKET_TYPE_STANDARD) ? 1 : 0;
}

uint8_t ValidateCorrectedPacket(const CorrectedDataPacket_t *packet)
{
    if (packet == NULL) return 0;
    return (packet->start_byte == PACKET_TYPE_STANDARD) ? 1 : 0;
}