#include <string.h>
#include <stdbool.h>
#include <time.h>
#include "oem7_gps.h"
#include "exo_common.h"
#include "exo_osal.h"
#include "minmea.h"


#ifndef LINUX_TEMP_PORT
#include "exo_io_al_uart_common.h"
#include "exo_ahw_al_spi_to_uart_common.h"


#define OBC_GPS_UART_PORT MAX14830_CH0

static uint8_t bt_prints[50];


extern ioal_uart_hdle ioal_huart6;
ahw_al_gen_info gps_uart;
static os_sem_handle_ptr gps_rcv_sem;

void gps_rx_complete_cb()
{
     os_sem_give(gps_rcv_sem);
}

static uint8_t oem7_get_data(uint8_t* cmd , uint16_t cmd_len, uint8_t* response, uint16_t rsp_len)
{
    uint8_t cmd_ack[15];
    UNUSED(cmd_ack);
    uint8_t ret=OEM7_FAIL;
       os_memset(cmd_ack,0,15);
#if defined (JANUS2) || defined (GEN2)
    ahw_al_uart_conv_receive(UART_EXPANDER_MAX14830_OBC,OBC_GPS_UART_PORT,OEM719_ACK_LEN,gps_rx_complete_cb,cmd_ack);
    ahw_al_uart_conv_transmit(UART_EXPANDER_MAX14830_OBC,OBC_GPS_UART_PORT,(uint8_t*)cmd,cmd_len);
    if((os_sem_take(gps_rcv_sem, 2000) == ral_success))
    {
        ahw_al_uart_conv_receive(UART_EXPANDER_MAX14830_OBC,OBC_GPS_UART_PORT,rsp_len,gps_rx_complete_cb,response);
#else
#ifdef STM32F769xx
#ifdef GPS_UART6_ENB
        io_hal_uart_receive_it(&ioal_huart6,&gps_uart,cmd_ack,OEM719_ACK_LEN);
        io_hal_uart_transmit(&ioal_huart6,(uint8_t*)cmd,cmd_len,500);
        if((os_sem_take(gps_rcv_sem, 2000) == ral_success))
        {
            io_hal_uart_receive_it(&ioal_huart6,&gps_uart,response,rsp_len);
#else
        if(0)
        {
#endif
#endif
#endif
#ifdef GPS_DEBUG_PRINT
            for(int i =0;i<OEM719_ACK_LEN;i++)
            {
                DEBUG_CPRINT(("GPS ACK data[%d] = 0x%x\n\r",i,cmd_ack[i]));
            }
#endif
            if((os_sem_take(gps_rcv_sem, 3000) == ral_success))
            {
#ifdef GPS_DEBUG_PRINT
                for(int i =0;i<rsp_len;i++)
                {
                    DEBUG_CPRINT(("GPS RX data[%d] = 0x%x\n\r",i,response[i]));
                }
#endif
                ret =OEM7_SCS;
            }
            else
            {
                DEBUG_CPRINT(("********* GPS READ RESPONSE FAIL ***********\n"));
                ret =OEM7_FAIL;
            }
        }
        else
        {
            DEBUG_CPRINT(("********* GPS READ ACK FAIL ***********\n"));
            ret = OEM7_FAIL;
        }
        return ret;
}

void oem7_gps_init()
{
#ifdef STM32F769xx
#ifdef GPS_UART6_ENB
    gps_uart.io_intf_id = IOAL_INST_UART6;
    ahal_register_io_cb(&gps_uart, UART_RX_CPLT_CB,gps_rx_complete_cb, HISR);
    ahal_register_io_cb(&gps_uart, UART_ERROR_CB,gps_rx_complete_cb, HISR);
#endif
#endif
    os_sem_create_bin(&gps_rcv_sem,"GPS_RCV_SEM");
    os_sem_take(gps_rcv_sem, 5000);
    ahw_al_uart_conv_receive(UART_EXPANDER_MAX14830_OBC,OBC_GPS_UART_PORT,50,NULL,bt_prints);
    os_delay(100);
#ifdef GPS_DEBUG_PRINT
    for(int i =0;i<50;i++)
    {
    DEBUG_CPRINT(("GPS BOOT data[%d] = 0x%x\n\r",i,bt_prints[i]));
    }
#endif
}
#endif
uint8_t oem7_get_gpgga(gps_location_data1* loc_data)
{
    uint8_t ret;
    struct minmea_sentence_gga frame;
#ifndef LINUX_TEMP_PORT
    //uint8_t cmd[15] ={0x6C, 0x6F, 0x67, 0x20, 0x67, 0x70, 0x67, 0x67, 0x61, 0x20, 0x6F, 0x6E, 0x63, 0x65, 0x0D };
    uint8_t* rcv_ptr = os_malloc_eram(NMEA_RSP_LEN);
    os_memset(rcv_ptr,0,NMEA_RSP_LEN);
    char* cmd = "log gpgga once\r";
    uint16_t cmd_len = (uint16_t)strlen(cmd);
        ret = oem7_get_data((uint8_t*)cmd, cmd_len, rcv_ptr, NMEA_RSP_LEN);
#else
        const char* rcv_ptr = "$GPGGA,202530.00,5109.0262,N,11401.8407,W,5,40,0.5,1097.36,M,-17.00,M,18,TSTR*61";
    ret = OEM7_SCS;
#endif
        if(ret == OEM7_SCS)
        {
            ret = minmea_parse_gga(&frame, (const char*)rcv_ptr);
            if(ret == true)
            {
                loc_data->altitude = minmea_tofloat(&frame.altitude);
                loc_data->altitude_units =frame.altitude_units;
                loc_data->dgps_age =minmea_tofloat(&frame.dgps_age);
                loc_data->fix_quality =frame.fix_quality;
                loc_data->hdop =minmea_tofloat(&frame.hdop);
                loc_data->height =minmea_tofloat(&frame.height);
                loc_data->height_units =frame.height_units;
                loc_data->latitude =minmea_tofloat(&frame.latitude);
                loc_data->longitude =minmea_tofloat(&frame.longitude);
                loc_data->satellites_tracked =frame.satellites_tracked;
                os_memcpy(&loc_data->time,&frame.time,sizeof(gps_time));
                ret=OEM7_SCS;
            }
            else
            {
                ret =OEM7_FAIL;
            }
        }
#ifndef LINUX_TEMP_PORT
    os_free(rcv_ptr);
#endif
    return ret;
}


uint8_t oem7_get_gprmc(gps_location_data2* loc_data)
{
    uint8_t ret;
    struct minmea_sentence_rmc frame;
#ifndef LINUX_TEMP_PORT
    uint8_t* rcv_ptr = os_malloc_eram(NMEA_RSP_LEN);
    os_memset(rcv_ptr,0,NMEA_RSP_LEN);
    char* cmd = "log gprmc once\r";
    uint16_t cmd_len = (uint16_t)strlen(cmd);
    ret = oem7_get_data((uint8_t*)cmd, cmd_len, rcv_ptr, NMEA_RSP_LEN);
#else
    const char* rcv_ptr = "$GPRMC,203522.00,A,5109.0262308,N,11401.8407342,W,0.004,133.4,130522,0.0,E,D*2B";
    ret = OEM7_SCS;
#endif
    if(ret == OEM7_SCS)
    {
        ret = minmea_parse_rmc(&frame, (const char*)rcv_ptr);
        if(ret == true)
        {
            loc_data->course = minmea_tofloat(&frame.course);
            loc_data->latitude = minmea_tofloat(&frame.latitude);
            loc_data->longitude = minmea_tofloat(&frame.longitude);
            loc_data->speed =minmea_tofloat(&frame.speed);
            loc_data->valid = frame.valid;
            loc_data->variation =minmea_tofloat(&frame.variation);
            os_memcpy(&loc_data->date,&frame.date,sizeof(gps_date));
            os_memcpy(&loc_data->time,&frame.time,sizeof(gps_time));
            ret=OEM7_SCS;
        }
        else
        {
            ret =OEM7_FAIL;
        }
    }
#ifndef LINUX_TEMP_PORT
    os_free(rcv_ptr);
#endif
        return ret;
}

uint8_t oem7_get_gpzda(gps_time_data* loc_data)
{
    uint8_t ret;
    struct minmea_sentence_zda frame;
#ifndef LINUX_TEMP_PORT
    uint8_t* rcv_ptr = os_malloc_eram(NMEA_RSP_LEN);
    os_memset(rcv_ptr,0,NMEA_RSP_LEN);
    char* gps_cmd = "log gpzda once\r";
    uint16_t cmd_len = (uint16_t)strlen(gps_cmd);
    ret = oem7_get_data((uint8_t*)gps_cmd, cmd_len, rcv_ptr, NMEA_RSP_LEN);
#else
    const char* rcv_ptr = "$GPZDA,204007.00,13,05,2022,,*62";
    ret = OEM7_SCS;
#endif
    if(ret == OEM7_SCS)
    {
         ret = minmea_parse_zda(&frame, (const char*)rcv_ptr);
         if(ret == true)
         {
            loc_data->hour_offset = frame.hour_offset;
            loc_data->minute_offset =frame.minute_offset;
            os_memcpy(&loc_data->date,&frame.date,sizeof(gps_date));
            os_memcpy(&loc_data->time,&frame.time,sizeof(gps_time));
            ret=OEM7_SCS;
         }
         else
         {
             ret =OEM7_FAIL;
         }
    }
#ifndef LINUX_TEMP_PORT
    os_free(rcv_ptr);
#endif
 return ret;
}

#ifndef LINUX_TEMP_PORT
/**
 * @brief This function get the best xyz information from the GPS
 */
uint8_t oem7_get_bestxyz(s_oem7_best_xyz *best_xyz)
{
    uint8_t ret;
    //Length of BestXYZ Binary Length
    char* gps_cmd = "log bestxyzb once\r";
    uint16_t cmd_len = (uint16_t)strlen(gps_cmd);
    uint8_t* rcv_ptr = (uint8_t*)os_malloc(BEST_RSP_LEN);
    uint16_t msg_id = OEM719_MSG_ID_BEST_XYZ;

    os_memset(rcv_ptr,0,BEST_RSP_LEN);
    ret = oem7_get_data((uint8_t*)gps_cmd, cmd_len, rcv_ptr, BEST_RSP_LEN);
    if(ret == OEM7_SCS)
    {
        ret = parse_bestxyz(msg_id, best_xyz, rcv_ptr);
        if(ret == OEM7_SCS)
        {
            DEBUG_CPRINT(("GPS BESTxyz decode success\n"));
            ret=OEM7_SCS;
        }
        else
        {
            DEBUG_CPRINT(("GPS BESTxyz decode Fail\n"));
            ret =OEM7_FAIL;
        }
    }
    os_free(rcv_ptr);
    return ret;
}
#else
uint8_t oem7_get_bestxyz(s_oem7_best_xyz *best_xyz)
{
    best_xyz->p_sts = 0;
    best_xyz->p_type = 0;
    best_xyz->p_x = 101.1;
    best_xyz->p_y = 201.5;
    best_xyz->p_z = 303.24;
    best_xyz->v_sts = 0;
    best_xyz->v_type = 0;
    best_xyz->v_x = 121.3;
    best_xyz->v_y = 321.5;
    best_xyz->v_z = 234.56;

    return OEM7_SCS;
}
#endif

#ifndef LINUX_TEMP_PORT
/**
 * @brief This function get the GPS time information from the GPS
 */
uint8_t oem7_get_time_info(s_oem7_time* time_info)
{
    uint8_t ret;
    char* gps_cmd = "log timeb once\r";
    uint16_t cmd_len = (uint16_t)strlen(gps_cmd);
    uint16_t msg_id = OEM719_MSG_ID_TIME;
    uint8_t* rcv_ptr = (uint8_t*)os_malloc(OEM719_MAX_RSP_LEN);

    os_memset(rcv_ptr,0,TIME_RSP_LEN);
    ret = oem7_get_data((uint8_t*)gps_cmd, cmd_len, rcv_ptr, TIME_RSP_LEN);
    if(ret == OEM7_SCS)
    {
        ret = parse_timeinfo(msg_id, time_info, rcv_ptr);
        if(ret == OEM7_SCS)
        {
            DEBUG_CPRINT(("GPS Get time info success\n"));
            ret = OEM7_SCS;
        }
        else
        {
            DEBUG_CPRINT(("GPS Get time info Fail\n"));
            ret = OEM7_FAIL;
        }
    }
    os_free(rcv_ptr);
    return ret;

}
#else
uint8_t oem7_get_time_info(s_oem7_time *time_info)
{
	time_info->clock_status = 0;
	time_info->offset = 0;
	time_info->offset_std = 0;
	time_info->utc_day = 12;
	time_info->utc_year = 2024;
	time_info->utc_hour = 5;
	time_info->utc_min = 20;
	time_info->utc_month = 5;
	time_info->utc_ms = 2500;
	time_info->utc_offset = 0;
	time_info->utc_status = 1;

    return OEM7_SCS;
}
#endif


/**
 * @brief This API encode the GPS header data's
 */
void cmd_encode(uint16_t msg_id, uint8_t* cmd, uint16_t msg_len,
        uint8_t* msg_buff)
{
    uint8_t gps_sync[3] = {0xaa, 0x44, 0x12};   //Sync 1 to 3 Command
    uint32_t cal_crc = 0;

    if(cmd!=NULL)
    {
        os_memset(cmd, 0, sizeof(32));   //Initially set all as 0
        os_memcpy(cmd, &gps_sync[0], 3);
        cmd[3] = OEM7_HEADER_LEN;
        os_memcpy(&cmd[4], &msg_id, sizeof(msg_id));
        cmd[7] = OEM7_PORT_ADDR;

        if((msg_len > 0) && (msg_buff == NULL))
        {
            os_memcpy(&cmd[8], &msg_len, sizeof(msg_len));
            os_memcpy(&cmd[10], &msg_buff, msg_len);
            cal_crc = CalculateBlockCRC32(28 + msg_len, (unsigned char*)cmd);
            os_memcpy(&cmd[28 + msg_len], &cal_crc, sizeof(cal_crc));
        }
        else
        {
            cal_crc = CalculateBlockCRC32(28, (unsigned char*)cmd);
            os_memcpy(&cmd[28], &cal_crc, sizeof(cal_crc));
        }

#ifdef GPS_DEBUG_PRINT
    for(int i =0;i<32;i++)
    {
        DEBUG_CPRINT(("GPS CMD enc [%d] = 0x%x\n\r",i,cmd[i]));
    }
#endif


    }
    else
    {
        //No Action
    }
}

/**
 * @brief This function decode the GPS best xyz information
 */
uint8_t parse_bestxyz(uint16_t msg_id, s_oem7_best_xyz *frame, uint8_t* recv_data)
{
    uint8_t status = OEM7_FAIL;
    uint8_t* data_ptr = (uint8_t*)frame;
    if(frame != NULL && recv_data != NULL)
    {
        status = header_check(recv_data,msg_id);
        if(status == OEM7_SCS)
        {
            os_memcpy(data_ptr, &recv_data[28], 32);
            os_memcpy(data_ptr+32, &recv_data[72], 32);
        }
        else
        {
            status = OEM7_FAIL;
        }
    }
    else
    {
        status = OEM7_FAIL;
    }
    return status;
}

/**
 * @brief This function decode the current GPS Time information and pack into
 * time structure
 */
uint8_t parse_timeinfo(uint16_t msg_id,s_oem7_time *frame, uint8_t* recv_data)
{
    uint8_t status = OEM7_FAIL;

    if(frame != NULL && recv_data != NULL)
    {
        status = header_check(recv_data,msg_id);
        if(status == OEM7_SCS)
        {
            os_memcpy(frame, &recv_data[OEM7_HEADER_LEN], sizeof(s_oem7_time));
            status = OEM7_SCS;
        }
        else
        {
            status = OEM7_FAIL;
        }
    }
    else
    {
        status = OEM7_FAIL;
    }
    return status;
}

/**
 * @brief This API check the GPS header
 */
uint8_t header_check(uint8_t* recv_data, uint16_t msg_id)
{
    uint8_t sync[3] = {0xaa, 0x44, 0x12};   //Sync 1 to 3 Command
    uint16_t msg_len = 0; //Message type is 0-Binary Type
    uint8_t status = 0;
    uint8_t i = 0;
    uint32_t cal_crc = 0;
    uint32_t recv_crc = 0;

    while(i < 3)
    {
        if(recv_data[i] == sync[i])
        {
            status = OEM7_SCS;
        }
        else
        {
            status = OEM7_FAIL;
        }
        i++;
    }

    if((status == OEM7_SCS) && (recv_data[3] == OEM7_HEADER_LEN))
    {
        DEBUG_CPRINT(("GPS HDR LEN MATCH SCS\n\r"));
        status = OEM7_SCS;
    }
    else
    {
        status = OEM7_FAIL;
    }

    if((status == OEM7_SCS) && (recv_data[4] == msg_id))
    {
        DEBUG_CPRINT(("GPS HDR MSG ID MATCH SCS\n\r"));
        status = OEM7_SCS;
    }
    else
    {
        status = OEM7_FAIL;
    }
#if 0
    if((status == OEM7_SCS) && ((recv_data[6] & 0x80)>0))
    {
        status = OEM7_SCS;
    }
    else
    {
        status = OEM7_FAIL;
    }
#endif

    os_memcpy(&msg_len,&recv_data[8], 2);
        DEBUG_CPRINT(("GPS HDR RECEIVED MSG LEN %d\n\r",msg_len));
    os_memcpy(&recv_crc, &recv_data[OEM7_HEADER_LEN + msg_len],\
            OEM7_CRC_LEN);
    cal_crc = CalculateBlockCRC32(OEM7_HEADER_LEN + msg_len,recv_data);
    if(cal_crc == recv_crc)
    {
        DEBUG_CPRINT(("*********GPS HDR CRC SCS***********\n\r"));
        status = OEM7_SCS;
    }
    else
    {
        DEBUG_CPRINT(("*********GPS HDR CRC FAIL***********\n\r"));
        status = OEM7_FAIL;
    }
    return status;
}

/* --------------------------------------------------------------------------
Calculate a CRC value to be used by CRC calculation functions.
-------------------------------------------------------------------------- */
unsigned long CRC32Value(int i)
{
    int j;
    unsigned long ulCRC;
    ulCRC = i;
    for ( j = 8 ; j > 0; j-- ) {
        if ( ulCRC & 1 )
            ulCRC = ( ulCRC >> 1 ) ^ CRC32_POLYNOMIAL;
        else
            ulCRC >>= 1;
    }
    return ulCRC;
}
/* --------------------------------------------------------------------------
Calculates the CRC-32 of a block of data all at once
ulCount - Number of bytes in the data block
ucBuffer - Data block
-------------------------------------------------------------------------- */
unsigned long CalculateBlockCRC32( unsigned long ulCount, unsigned char
        *ucBuffer )
{
    unsigned long ulTemp1;
    unsigned long ulTemp2;
    unsigned long ulCRC = 0;
    while ( ulCount-- != 0 )
    {
        ulTemp1 = ( ulCRC >> 8 ) & 0x00FFFFFFL;
        ulTemp2 = CRC32Value( ((int) ulCRC ^ *ucBuffer++ ) & 0xFF );
        ulCRC = ulTemp1 ^ ulTemp2;
    }
    return( ulCRC );
}
