//Written by Tiziano Wehrli
//With library, compile with: 
//gccarm -g -o gps_demo.out gps_demo.c -lgps -lm ../Libaries/gps_library.o 
#include "eyebot.h"
#include <gps.h>

#define SECOND 1000000

int main() {

	struct gps_data_t gps_data;

    int initReturn = initializeGPS(&gps_data);
    if (initReturn != 0) {
        return 1;
    }

    //Init the GPS data variables
    float latitude;
    float longitude;
    float speed;

    printf("Running main loop now");
    //Main Run Time Loop
    while (true) {

        //Quickly poll to check if we have GPS data, 0.001ms max wait time
        if (isDataGPS(&gps_data)) {
            //Data is available
            printf("GPS data available for us to read\n");
            int readReturn = readFromGPS(&gps_data, &latitude, &longitude, &speed);
            if (readReturn == 0) {
                printf("Latitude: \t %.6f \n Longitude: \t %.6f \n Speed: \t %.6f \n ", latitude, longitude, speed);
            }
        }
        printf("Dropped into main loop, doing something for 0.3 seconds \n \n");
        usleep(0.3 * SECOND); 

    }
    
    // When you are done...
    (void)gps_stream(&gps_data, WATCH_DISABLE, NULL);
    (void)gps_close(&gps_data);
    return 0;
}