// TODO: Transition to using https://github.com/cuspaceflight/joey-m/blob/master/firmware/gps.c requesting UBX data

#include "stm32f0xx_hal.h"

#include "config.h"
#include "gpio.h"
#include "uart.h"
#include "gps.h"


typedef struct _gps_data
{
    int32_t hdop;
    int32_t sats_in_view;
    int32_t speed;
    int32_t heading;


    int32_t latitude;
    int32_t longitude;
    int32_t altitude;

    uint8_t hour;
    uint8_t minute;
    uint8_t second;

} gps_data_t;

gps_data_t position;

// Private methods
static void gps_ubx_checksum(uint8_t* data, uint8_t len, uint8_t* cka, uint8_t* ckb);
static uint8_t _gps_verify_checksum(uint8_t* data, uint8_t len);



void gps_init()
{
    // Initialize serial port
   // done in poweron uart_init();


    // Disable GLONASS mode
    uint8_t disable_glonass[20] = {0xB5, 0x62, 0x06, 0x3E, 0x0C, 0x00, 0x00, 0x00, 0x20, 0x01, 0x06, 0x08, 0x0E, 0x00, 0x00, 0x00, 0x01, 0x01, 0x8F, 0xB2};

    //gps_sendubx(disable_glonass, 20);
    HAL_UART_Transmit(uart_gethandle(), disable_glonass, 20, 100);

    // Enable power saving
    uint8_t enable_powersave[10] = {0xB5, 0x62, 0x06, 0x11, 0x02, 0x00, 0x08, 0x01, 0x22, 0x92};
    //gps_sendubx(enable_powersave, 10);
    HAL_UART_Transmit(uart_gethandle(), enable_powersave, 10, 100);


    // Set dynamic model 6 (<1g airborne platform)
    uint8_t airborne_model[] = { 0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x2C, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x16, 0xDC };
    //gps_sendubx(airborne_model, sizeof(airborne_model)/sizeof(uint8_t));
    HAL_UART_Transmit(uart_gethandle(), airborne_model, sizeof(airborne_model)/sizeof(uint8_t), 100);


   // Disable messages
   uint8_t setGLL[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x2B};
   HAL_UART_Transmit(uart_gethandle(), setGLL, sizeof(setGLL)/sizeof(uint8_t), 100);
   HAL_Delay(1);

   uint8_t setGSA[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x32};
   HAL_UART_Transmit(uart_gethandle(), setGSA, sizeof(setGSA)/sizeof(uint8_t), 100);
   HAL_Delay(1);

   uint8_t setGSV[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03, 0x39};
   HAL_UART_Transmit(uart_gethandle(), setGSV, sizeof(setGSV)/sizeof(uint8_t), 100);
   HAL_Delay(1);

   uint8_t setRMC[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x04, 0x40};
   HAL_UART_Transmit(uart_gethandle(), setRMC, sizeof(setRMC)/sizeof(uint8_t), 100);
   HAL_Delay(1);

   uint8_t setVTG[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x46};
   HAL_UART_Transmit(uart_gethandle(), setVTG, sizeof(setRMC)/sizeof(uint8_t), 100);
   HAL_Delay(1);


}


void gps_update_position()
{
    // Request a NAV-POSLLH message from the GPS
    uint8_t request[8] = {0xB5, 0x62, 0x01, 0x02, 0x00, 0x00, 0x03,
        0x0A};
    //_gps_send_msg(request, 8);
    uint8_t flushed = uart_gethandle()->Instance->RDR;
    HAL_UART_Transmit(uart_gethandle(), request, 8, 100);

    uint8_t buf[36];
    HAL_UART_Receive(uart_gethandle(), buf, 36, 100);

    //for(uint8_t i = 0; i < 36; i++)
    //    buf[i] = _gps_get_byte();

//    // Verify the sync and header bits
//    if( buf[0] != 0xB5 || buf[1] != 0x62 )
//        led_set(LED_RED, 1);
//    if( buf[2] != 0x01 || buf[3] != 0x02 )
//        led_set(LED_RED, 1);

    // 4 bytes of longitude (1e-7)
    position.longitude = (int32_t)buf[10] | (int32_t)buf[11] << 8 |
        (int32_t)buf[12] << 16 | (int32_t)buf[13] << 24;

    // 4 bytes of latitude (1e-7)
    position.latitude = (int32_t)buf[14] | (int32_t)buf[15] << 8 |
        (int32_t)buf[16] << 16 | (int32_t)buf[17] << 24;

    // 4 bytes of altitude above MSL (mm)
    position.altitude = (int32_t)buf[22] | (int32_t)buf[23] << 8 |
        (int32_t)buf[24] << 16 | (int32_t)buf[25] << 24;

    if( !_gps_verify_checksum(&buf[2], 32) )
        led_blink(2);
}


void gps_update_time(uint8_t* hour, uint8_t* minute, uint8_t* second)
{
    // Send a NAV-TIMEUTC message to the receiver
    uint8_t request[8] = {0xB5, 0x62, 0x01, 0x21, 0x00, 0x00, 0x22, 0x67};
    uint8_t flushed = uart_gethandle()->Instance->RDR;
    volatile HAL_StatusTypeDef res = HAL_UART_Transmit(uart_gethandle(), request, 8, 100);

    // Get the message back from the GPS
    uint8_t buf[28];
    res = HAL_UART_Receive(uart_gethandle(), buf, 28, 500);

//    // Verify the sync and header bits
//    if( buf[0] != 0xB5 || buf[1] != 0x62 )
//        led_set(LED_RED, 1);
//    if( buf[2] != 0x01 || buf[3] != 0x21 )
//        led_set(LED_RED, 1);

    *hour = buf[22];
    *minute = buf[23];
    *second = buf[24];

//    if( !_gps_verify_checksum(&buf[2], 24) ) led_set(LED_RED, 1);
}

/**
 * Check the navigation status to determine the quality of the
 * fix currently held by the receiver with a NAV-STATUS message.
 */
void gps_check_lock(uint8_t* lock, uint8_t* sats)
{
    // Construct the request to the GPS
    uint8_t request[8] = {0xB5, 0x62, 0x01, 0x06, 0x00, 0x00,
        0x07, 0x16};
    uint8_t flushed = uart_gethandle()->Instance->RDR;
    HAL_UART_Transmit(uart_gethandle(), request, 8, 100);

    // Get the message back from the GPS
    uint8_t buf[60];
    HAL_UART_Receive(uart_gethandle(), buf, 60, 100);

    // Verify the sync and header bits
//    if( buf[0] != 0xB5 || buf[1] != 0x62 )
//        led_set(LED_RED, 1);
//    if( buf[2] != 0x01 || buf[3] != 0x06 )
//        led_set(LED_RED, 1);

    // Check 60 bytes minus SYNC and CHECKSUM (4 bytes)
//    if( !_gps_verify_checksum(&buf[2], 56) ) led_set(LED_RED, 1);

    // Return the value if GPSfixOK is set in 'flags'
    if( buf[17] & 0x01 )
        *lock = buf[16];
    else
        *lock = 0;

    *sats = buf[53];
}

/**
 * Verify that the uBlox 6 GPS receiver is set to the <1g airborne
 * navigaion mode.
 */
uint8_t gps_check_nav(void)
{
    uint8_t request[8] = {0xB5, 0x62, 0x06, 0x24, 0x00, 0x00,
        0x2A, 0x84};
    uint8_t flushed = uart_gethandle()->Instance->RDR;
    HAL_UART_Transmit(uart_gethandle(), request, 8, 100);

    // Get the message back from the GPS
    uint8_t buf[44];
    HAL_UART_Receive(uart_gethandle(), buf, 44, 100);

//    // Verify sync and header bytes
//    if( buf[0] != 0xB5 || buf[1] != 0x62 )
//        led_set(LED_RED, 1);
//    if( buf[2] != 0x06 || buf[3] != 0x24 )
//        led_set(LED_RED, 1);

    // Check 40 bytes of message checksum
//    if( !_gps_verify_checksum(&buf[2], 40) ) led_set(LED_RED, 1);

    // Clock in and verify the ACK/NACK
    uint8_t ack[10];
//    for(uint8_t i = 0; i < 10; i++)
//        ack[i] = _gps_get_byte();

    HAL_UART_Receive(uart_gethandle(), ack, 10, 100);

    // If we got a NACK, then return 0xFF
    if( ack[3] == 0x00 ) return 0xFF;

    // Return the navigation mode and let the caller analyse it
    return buf[8];
}

/**
 * Verify the checksum for the given data and length.
 */

static uint8_t _gps_verify_checksum(uint8_t* data, uint8_t len)
{
    uint8_t a, b;
    gps_ubx_checksum(data, len, &a, &b);
    if( a != *(data + len) || b != *(data + len + 1))
        return 0;
    else
        return 1;
}

// Calculate a UBX checksum using 8-bit Fletcher (RFC1145)
static void gps_ubx_checksum(uint8_t* data, uint8_t len, uint8_t* cka, uint8_t* ckb)
{
    *cka = 0;
    *ckb = 0;
    for( uint8_t i = 0; i < len; i++ )
    {
        *cka += *data;
        *ckb += *cka;
        data++;
    }
}


void gps_poweron(void)
{
    // NOTE: pchannel
    HAL_GPIO_WritePin(GPS_NOTEN, 0);
    uart_init();

    // Begin DMA reception
    //HAL_UART_Receive_DMA(uart_gethandle(), nmeaBuffer, NMEABUFFER_SIZE);
}


void gps_poweroff(void)
{
    // NOTE: pchannel
    uart_deinit();
    HAL_GPIO_WritePin(GPS_NOTEN, 1);
}

//
//uint8_t gps_hadfix = 0;
//uint8_t gps_hasfix()
//{
//	uint8_t hasFix = get_latitudeTrimmed()[0] != 0x00;
//	gps_hadfix = hasFix;
//	return hasFix;
//}



//void gps_sendubx(uint8_t* dat, uint8_t size)
//{
//    uint8_t sendctr;
//    for(sendctr = 0; sendctr < size; sendctr++)
//    {
//        HAL_UART_Transmit(huart1, dat[sendctr]);
//    }
//}

uint8_t gps_acquiring= 0;
uint32_t gps_lastfix_time;

void gps_acquirefix(void)
{
    gps_poweron();

    // Wait for fix
    gps_acquiring = 1;
}

uint8_t gps_getstate(void)
{
    if(gps_acquiring)
        return GPS_STATE_ACQUIRING;
    else if(gps_lastfix_time == 0)
        return GPS_STATE_NOFIX;
    else if(HAL_GetTick() - gps_lastfix_time < GPS_STALEFIX_MS)
        return GPS_STATE_FRESHFIX;
    else
        return GPS_STATE_STALEFIX;

}




// vim:softtabstop=4 shiftwidth=4 expandtab