#include <pic16f689.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <stdlib.h>
#include "configuration.h"
#include "tickcounter.h"
#include "softuart.h"

char GPS_LINE[75];
bool gps_getline()
{
    uint8_t count = 0;
    uint16_t start = get_ticks16();
    do
    {
        while (GPS_RXREG == 0)
        {
            if ((uint16_t)get_ticks16() - start > 10)
                return false; // Timeout
        }
        GPS_LINE[count++] = GPS_RXREG;
        GPS_RXREG = 0;
    } while (count < 75 && GPS_LINE[count-1] != '\n');
    GPS_LINE[count-1] = '\0';
    return true;
}

uint16_t time_to_fix = 0;
bool gps_waitforfix(uint16_t seconds, bool update_time_to_fix)
{
    uint16_t start = get_ticks16();
    
    if (update_time_to_fix)
        time_to_fix = 0;
    
    while (get_ticks16() - start < seconds + 10)
    {
        clrwdt();
        
        if (!gps_getline())
            return false;
        
        if (strncmp(GPS_LINE, "$GPGGA", 6) == 0)
        {
            uint8_t commas = 0, pos = 0;
            while (commas < 6 && pos < 40)
            {
                if (GPS_LINE[pos] == ',') commas++;
                pos++;
            }
                
            if (update_time_to_fix)
                time_to_fix = get_ticks16() - start;
            
            if (strncmp(GPS_LINE + pos, "1,", 2) == 0)
            {
                return true;
            }
            else if (get_ticks16() - start > seconds)
            {
                // If we can't get a proper fix, return atleast a partial one
                return false;
            }
        }
    }
    
    if (update_time_to_fix)
        time_to_fix = get_ticks16() - start;
    
    return false;
}