#include "stdio.h"
#include "stdarg.h"
#include "stdlib.h"
#include "string.h"
#include "stm32h7xx_hal.h"
#include "math.h"
#include "HIDO_Util.h"
#include "HIDO_Debug.h"
#include "FreeRTOS.h"
#include "task.h"
#include "GPS.h"
#include "GPIO.h"
#include "Uart.h"
// #include "global_param.h"

#define GPS_UART_RX_BUF_SIZE 1024
#define GPS_UART_TX_BUF_SIZE (2048 + 512)

#define IM23A_HEADER_LEN                (4U)
#define IM23A_NAV_FRAME_LEN             (100U)   /* 包含头、负载、校验和及尾部 */
#define IM23A_IMU_FRAME_LEN             (52U)
#define IM23A_MAX_FRAME_LEN             IM23A_NAV_FRAME_LEN
#define IM23A_CHECKSUM_LEN              (2U)
#define IM23A_TAIL_LEN                  (2U)
#define IM23A_NAV_HEADER                "fmin"
#define IM23A_IMU_HEADER                "fmim"
#define IM23A_GPS_TIME_SCALE            (100.0)  /* hhmmss.ss -> centisecond */
#define IM23A_IMU_TIME_SCALE            (1000.0) /* hhmmss.ss -> millisecond */

typedef struct
{
    HIDO_UINT8 m_au8Buffer[IM23A_MAX_FRAME_LEN];
    HIDO_UINT32 m_u32RecvLen;
    HIDO_UINT32 m_u32ExpectedLen;
} ST_GPSRecv;

static HIDO_UINT8 l_au8GPSUartRxBuf[GPS_UART_RX_BUF_SIZE];
static HIDO_UINT8 l_au8GPSUartTxBuf[GPS_UART_TX_BUF_SIZE];

static ST_GPIO l_astGPSPin[GPS_PIN_LAST];
static ST_GPSRecv l_stGPSRecv;

static HIDO_UINT8 l_u8PosState = 0;
static HIDO_UINT32 l_u32QXTick = 0;
HIDO_UINT32 getRTK_Tick = 0;

/* 存储最新的GPRMI和GPIMU数据 */
static ST_GPRMI l_stGPRMI;
static ST_GPIMU l_stGPIMU;
static HIDO_UINT32 s_gprmi_log_idx = 0U;
/* GPS���� */
/*******************************************************************************
 *                          IM23A Helper Declarations                          *
 *******************************************************************************/
static HIDO_VOID IM23A_ResetParser(ST_GPSRecv *parser);
static HIDO_BOOL IM23A_ValidateFrame(const HIDO_UINT8 *frame, HIDO_UINT32 len);
static HIDO_VOID IM23A_HandleFrame(const HIDO_UINT8 *frame, HIDO_UINT32 len);
static HIDO_VOID IM23A_HandleNavFrame(const HIDO_UINT8 *frame);
static HIDO_VOID IM23A_HandleImuFrame(const HIDO_UINT8 *frame);
static HIDO_DOUBLE IM23A_ReadDouble(const HIDO_UINT8 *ptr);
static HIDO_FLOAT IM23A_ReadFloat(const HIDO_UINT8 *ptr);
static HIDO_UINT32 IM23A_ReadU32(const HIDO_UINT8 *ptr);
static HIDO_UINT16 IM23A_ReadU16(const HIDO_UINT8 *ptr);
static HIDO_UINT32 IM23A_ConvertTime(double utc_val, double scale);

/* 旧版NMEA解析函数已移除，以下为IM23A专用解析工具函数 */

static HIDO_DOUBLE IM23A_ReadDouble(const HIDO_UINT8 *ptr)
{
    HIDO_DOUBLE val = 0.0;
    memcpy(&val, ptr, sizeof(val));
    return val;
}

static HIDO_FLOAT IM23A_ReadFloat(const HIDO_UINT8 *ptr)
{
    HIDO_FLOAT val = 0.0f;
    memcpy(&val, ptr, sizeof(val));
    return val;
}

static HIDO_UINT32 IM23A_ReadU32(const HIDO_UINT8 *ptr)
{
    HIDO_UINT32 val = 0U;
    memcpy(&val, ptr, sizeof(val));
    return val;
}

static HIDO_UINT16 IM23A_ReadU16(const HIDO_UINT8 *ptr)
{
    HIDO_UINT16 val = 0U;
    memcpy(&val, ptr, sizeof(val));
    return val;
}

static HIDO_UINT32 IM23A_ConvertTime(double utc_val, double scale)
{
    if (!isfinite(utc_val) || utc_val < 0.0)
    {
        return 0U;
    }
    double scaled = utc_val * scale;
    if (scaled < 0.0)
    {
        scaled = 0.0;
    }
    return (HIDO_UINT32)(scaled + 0.5);
}

static HIDO_VOID IM23A_ResetParser(ST_GPSRecv *parser)
{
    memset(parser->m_au8Buffer, 0, sizeof(parser->m_au8Buffer));
    parser->m_u32RecvLen = 0U;
    parser->m_u32ExpectedLen = 0U;
}

static HIDO_BOOL IM23A_ValidateFrame(const HIDO_UINT8 *frame, HIDO_UINT32 len)
{
    if (frame == HIDO_NULL)
    {
        return HIDO_FALSE;
    }

    if ((len != IM23A_NAV_FRAME_LEN) && (len != IM23A_IMU_FRAME_LEN))
    {
        return HIDO_FALSE;
    }

    if (frame[len - 2U] != 'e' || frame[len - 1U] != 'd')
    {
        return HIDO_FALSE;
    }

    const HIDO_UINT32 sum_len = len - IM23A_TAIL_LEN - IM23A_CHECKSUM_LEN;
    HIDO_UINT16 calc = 0U;
    for (HIDO_UINT32 i = 0U; i < sum_len; ++i)
    {
        calc = (HIDO_UINT16)(calc + frame[i]);
    }

    const HIDO_UINT16 expect = IM23A_ReadU16(&frame[sum_len]);
    return (calc == expect);
}

static HIDO_VOID IM23A_HandleFrame(const HIDO_UINT8 *frame, HIDO_UINT32 len)
{
    if (len == IM23A_NAV_FRAME_LEN && memcmp(frame, IM23A_NAV_HEADER, IM23A_HEADER_LEN) == 0)
    {
        IM23A_HandleNavFrame(frame);
    }
    else if (len == IM23A_IMU_FRAME_LEN && memcmp(frame, IM23A_IMU_HEADER, IM23A_HEADER_LEN) == 0)
    {
        IM23A_HandleImuFrame(frame);
    }
}

static HIDO_VOID IM23A_HandleNavFrame(const HIDO_UINT8 *frame)
{
    if (IM23A_ValidateFrame(frame, IM23A_NAV_FRAME_LEN) == HIDO_FALSE)
    {
        return;
    }

    const HIDO_UINT8 *p = frame + IM23A_HEADER_LEN;

    double utc = IM23A_ReadDouble(p); p += 8U;
    double latitude = IM23A_ReadDouble(p); p += 8U;
    double longitude = IM23A_ReadDouble(p); p += 8U;
    double altitude = IM23A_ReadDouble(p); p += 8U;
    float vel_n = IM23A_ReadFloat(p); p += 4U;
    float vel_e = IM23A_ReadFloat(p); p += 4U;
    float vel_d = IM23A_ReadFloat(p); p += 4U;
    float roll = IM23A_ReadFloat(p); p += 4U;
    float pitch = IM23A_ReadFloat(p); p += 4U;
    float heading = IM23A_ReadFloat(p); p += 4U;
    float pos_accuracy = IM23A_ReadFloat(p); p += 4U;
    float accel_bias_x = IM23A_ReadFloat(p); p += 4U;
    float accel_bias_y = IM23A_ReadFloat(p); p += 4U;
    float accel_bias_z = IM23A_ReadFloat(p); p += 4U;
    float gyro_bias_x = IM23A_ReadFloat(p); p += 4U;
    float gyro_bias_y = IM23A_ReadFloat(p); p += 4U;
    float gyro_bias_z = IM23A_ReadFloat(p); p += 4U;
    float sensor_temp = IM23A_ReadFloat(p); p += 4U;
    HIDO_UINT32 status = IM23A_ReadU32(p);

    memset(&l_stGPRMI, 0, sizeof(ST_GPRMI));
    l_stGPRMI.m_u32UTCTime = IM23A_ConvertTime(utc, IM23A_GPS_TIME_SCALE);
    l_stGPRMI.m_dLatitude = latitude;
    l_stGPRMI.m_dLongitude = longitude;
    l_stGPRMI.m_fAltitude = (HIDO_FLOAT)altitude;
    l_stGPRMI.m_fNorthVelocity = vel_n;
    l_stGPRMI.m_fEastVelocity = vel_e;
    l_stGPRMI.m_fUpVelocity = (HIDO_FLOAT)(-vel_d);
    l_stGPRMI.m_fRollAngle = roll;
    l_stGPRMI.m_fPitchAngle = pitch;
    l_stGPRMI.m_fHeadingAngle = heading;
    l_stGPRMI.m_fHorizontalVelStdDev = pos_accuracy;
    l_stGPRMI.m_fAccelBiasX = accel_bias_x;
    l_stGPRMI.m_fAccelBiasY = accel_bias_y;
    l_stGPRMI.m_fAccelBiasZ = accel_bias_z;
    l_stGPRMI.m_fGyroBiasX = gyro_bias_x;
    l_stGPRMI.m_fGyroBiasY = gyro_bias_y;
    l_stGPRMI.m_fGyroBiasZ = gyro_bias_z;
    l_stGPRMI.m_fImuTemperature = sensor_temp;
    l_stGPRMI.m_u32StatusFlags = status;
    l_stGPRMI.m_u8PositionQuality = (HIDO_UINT8)(status & 0xFFU);
    l_stGPRMI.m_bValid = HIDO_TRUE;

    l_u8PosState = l_stGPRMI.m_u8PositionQuality;
}

static HIDO_VOID IM23A_HandleImuFrame(const HIDO_UINT8 *frame)
{
    if (IM23A_ValidateFrame(frame, IM23A_IMU_FRAME_LEN) == HIDO_FALSE)
    {
        return;
    }

    const HIDO_UINT8 *p = frame + IM23A_HEADER_LEN;

    double utc = IM23A_ReadDouble(p); p += 8U;
    float accel_x = IM23A_ReadFloat(p); p += 4U;
    float accel_y = IM23A_ReadFloat(p); p += 4U;
    float accel_z = IM23A_ReadFloat(p); p += 4U;
    float gyro_x = IM23A_ReadFloat(p); p += 4U;
    float gyro_y = IM23A_ReadFloat(p); p += 4U;
    float gyro_z = IM23A_ReadFloat(p); p += 4U;
    (void)IM23A_ReadFloat(p); p += 4U;
    (void)IM23A_ReadFloat(p); p += 4U;
    (void)IM23A_ReadFloat(p);

    memset(&l_stGPIMU, 0, sizeof(ST_GPIMU));
    l_stGPIMU.m_u32UTCTime = IM23A_ConvertTime(utc, IM23A_IMU_TIME_SCALE);
    l_stGPIMU.m_fAccelX = accel_x;
    l_stGPIMU.m_fAccelY = accel_y;
    l_stGPIMU.m_fAccelZ = accel_z;
    l_stGPIMU.m_fGyroX = gyro_x;
    l_stGPIMU.m_fGyroY = gyro_y;
    l_stGPIMU.m_fGyroZ = gyro_z;
    l_stGPIMU.m_fTemperature = l_stGPRMI.m_fImuTemperature;
    l_stGPIMU.m_bValid = HIDO_TRUE;
}


static HIDO_VOID GPS_RecvFsm(HIDO_UINT8 _u8RecvChar)
{
    ST_GPSRecv *parser = &l_stGPSRecv;

    if (parser->m_u32ExpectedLen == 0U)
    {
        if (parser->m_u32RecvLen == 0U)
        {
            if (_u8RecvChar == 'f')
            {
                parser->m_au8Buffer[parser->m_u32RecvLen++] = _u8RecvChar;
            }
            return;
        }

        parser->m_au8Buffer[parser->m_u32RecvLen++] = _u8RecvChar;

        if (parser->m_u32RecvLen == 2U && _u8RecvChar != 'm')
        {
            if (_u8RecvChar == 'f')
            {
                parser->m_au8Buffer[0] = 'f';
                parser->m_u32RecvLen = 1U;
            }
            else
            {
                IM23A_ResetParser(parser);
            }
            return;
        }

        if (parser->m_u32RecvLen == 3U && _u8RecvChar != 'i')
        {
            if (_u8RecvChar == 'f')
            {
                parser->m_au8Buffer[0] = 'f';
                parser->m_u32RecvLen = 1U;
            }
            else
            {
                IM23A_ResetParser(parser);
            }
            return;
        }

        if (parser->m_u32RecvLen == IM23A_HEADER_LEN)
        {
            if (memcmp(parser->m_au8Buffer, IM23A_NAV_HEADER, IM23A_HEADER_LEN) == 0)
            {
                parser->m_u32ExpectedLen = IM23A_NAV_FRAME_LEN;
            }
            else if (memcmp(parser->m_au8Buffer, IM23A_IMU_HEADER, IM23A_HEADER_LEN) == 0)
            {
                parser->m_u32ExpectedLen = IM23A_IMU_FRAME_LEN;
            }
            else
            {
                HIDO_UINT8 restart = parser->m_au8Buffer[IM23A_HEADER_LEN - 1U];
                IM23A_ResetParser(parser);
                if (restart == 'f')
                {
                    parser->m_au8Buffer[0] = 'f';
                    parser->m_u32RecvLen = 1U;
                }
            }
        }
        return;
    }

    if (parser->m_u32RecvLen >= IM23A_MAX_FRAME_LEN)
    {
        IM23A_ResetParser(parser);
        return;
    }

    parser->m_au8Buffer[parser->m_u32RecvLen++] = _u8RecvChar;

    if (parser->m_u32RecvLen == parser->m_u32ExpectedLen)
    {
        IM23A_HandleFrame(parser->m_au8Buffer, parser->m_u32ExpectedLen);
        IM23A_ResetParser(parser);
    }
}

/*******************************************************************************
 * Function Name     : GPS_Rest
 * Description       : GPS��λ
 * Input             : None
 * Output            : None
 * Return            : None
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021��1��8��
 *******************************************************************************/
static HIDO_VOID GPS_Rest(void)
{
    GPIO_SET(&l_astGPSPin[GPS_PIN_REST]);
    vTaskDelay(pdMS_TO_TICKS(10));
    GPIO_RESET(&l_astGPSPin[GPS_PIN_REST]);
    vTaskDelay(pdMS_TO_TICKS(10));
    GPIO_SET(&l_astGPSPin[GPS_PIN_REST]);
}

/*******************************************************************************
 * Function Name     : GPS_PowerOn
 * Description       : GPS�ϵ�
 * Input             : None
 * Output            : None
 * Return            : None
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021��1��8��
 *******************************************************************************/
static HIDO_VOID GPS_PowerOn(void)
{
    GPIO_SET(&l_astGPSPin[GPS_PIN_EN]);
}

/*******************************************************************************
 * Function Name     : GPS_PowerOff
 * Description       : GPS����
 * Input             : None
 * Output            : None
 * Return            : None
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021��1��8��
 *******************************************************************************/
HIDO_VOID GPS_PowerOff(void)
{
    GPIO_RESET(&l_astGPSPin[GPS_PIN_EN]);
}

/*******************************************************************************
 *                             Global Function                                 *
 *******************************************************************************/

/*******************************************************************************
 * Function Name     : GPS_GetState
 * Description       : ��ȡGPS��λ״̬
 * Input             : None
 * Output            : None
 * Return            : ��λ״̬ 0 1 2 3
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021��1��8��
 *******************************************************************************/
HIDO_UINT8 GPS_GetState(HIDO_VOID)
{
    return l_u8PosState;
}

/*******************************************************************************
 * Function Name     : GPS_PinRegister
 * Description       : GPSģ��ܽ�ע��
 * Input             : _ePin �ܽŶ���
 *                   : _pstGPIOx GPIOx
 *                   : _u16GPIOPin GPIO_PIN_x
 * Output            : None
 * Return            : None
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021��1��8��
 *******************************************************************************/
HIDO_VOID GPS_PinRegister(E_GPSPin _ePin, GPIO_TypeDef *_pstGPIOx, HIDO_UINT16 _u16GPIOPin)
{
    l_astGPSPin[_ePin].m_pstGPIOx = _pstGPIOx;
    l_astGPSPin[_ePin].m_u16GPIOPin = _u16GPIOPin;
}

/*******************************************************************************
 * Function Name     : GPS_Poll
 * Description       : GPS轮询，读取UART数据并送入IM23A解析器
 *******************************************************************************/
HIDO_VOID GPS_Poll(void)
{
    HIDO_UINT8 u8RecvChar = 0U;

    while (Uart_GetChar(UART_ID_GPS, &u8RecvChar) == HIDO_OK)
    {
        GPS_RecvFsm(u8RecvChar);
    }
}

/*******************************************************************************
 * Function Name     : GPS_Init
 * Description       : GPSģ���ʼ��
 * Input             : None
 * Output            : None
 * Return            : None
 * Author            : www.hido-studio.com
 * Modified Date:    : 2021��1��8��
 *******************************************************************************/
HIDO_VOID GPS_Init(void)
{
    GPS_PowerOn();
    GPS_Rest();

    ST_UartInit stInit;

    stInit.m_eRxMode = UART_RX_MODE_DMA;
    stInit.m_eTxMode = UART_TX_MODE_DMA;
    stInit.m_pu8RxBuf = l_au8GPSUartRxBuf;
    stInit.m_u32RxBufSize = GPS_UART_RX_BUF_SIZE;
    stInit.m_pu8TxBuf = l_au8GPSUartTxBuf;
    stInit.m_u32TxBufSize = GPS_UART_TX_BUF_SIZE;
    stInit.m_u32TxQueueMemberCnt = 2;
    Uart_Init(UART_ID_GPS, &stInit);

    IM23A_ResetParser(&l_stGPSRecv);
    Uart_ReConfigBaudRate(UART_ID_GPS, 115200);
}

/*******************************************************************************
 * Function Name     : GPS_GetGPRMI
 * Description       : 获取最新的GPRMI数据
 * Input             : _pstGPRMI - 存储GPRMI数据的结构体指针
 * Output            : None
 * Return            : HIDO_OK - 成功, HIDO_ERR - 失败或数据无效
 * Author            : www.hido-studio.com
 * Modified Date:    : 2025年11月11日
 *******************************************************************************/
HIDO_INT32 GPS_GetGPRMI(ST_GPRMI *_pstGPRMI)
{
    if (_pstGPRMI == HIDO_NULL)
    {
        return HIDO_ERR;
    }
    
    if (l_stGPRMI.m_bValid == HIDO_FALSE)
    {
        return HIDO_ERR;
    }
    
    memcpy(_pstGPRMI, &l_stGPRMI, sizeof(ST_GPRMI));
    
    return HIDO_OK;
}

/*******************************************************************************
 * Function Name     : GPS_GetGPIMU
 * Description       : 获取最新的GPIMU数据
 * Input             : _pstGPIMU - 存储GPIMU数据的结构体指针
 * Output            : None
 * Return            : HIDO_OK - 成功, HIDO_ERR - 失败或数据无效
 * Author            : www.hido-studio.com
 * Modified Date:    : 2025年11月11日
 *******************************************************************************/
HIDO_INT32 GPS_GetGPIMU(ST_GPIMU *_pstGPIMU)
{
    if (_pstGPIMU == HIDO_NULL)
    {
        return HIDO_ERR;
    }
    
    if (l_stGPIMU.m_bValid == HIDO_FALSE)
    {
        return HIDO_ERR;
    }
    
    memcpy(_pstGPIMU, &l_stGPIMU, sizeof(ST_GPIMU));
    
    return HIDO_OK;
}