#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <time.h>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/msg.h>
#include <pthread.h>
#include "speedo.h"
#include "compass.h"
#include "gps.h"
#include "compute_globals.h"


int main(void)
{
    pthread_t speedo_thread;
    pthread_t compass_thread;
    pthread_t gps_thread;
    int rc; 
    int t;


//Start thread that collects and processes compass data. 
    rc = pthread_create(&compass_thread,NULL,compute_compass,(void*)t);
    if(rc)
	{
	printf("ERRORl return code from pthread_create() for compass is %d\n",rc);
	exit(-1);
	}

//Start thread that collects and processes speedo data. 
    rc = pthread_create(&speedo_thread,NULL,compute_speedo,(void*)t);
    if(rc)
	{
	printf("ERRORl return code from pthread_create() for speedo is %d\n",rc);
	exit(-1);
	}

//Start thread that collects and processes gps data. 
    rc = pthread_create(&gps_thread,NULL,compute_gps,(void*)t);
    if(rc)
	{
	printf("ERRORl return code from pthread_create() for gps is %d\n",rc);
	exit(-1);
	}

while(1)
{
pthread_mutex_lock(&speedo_data_mutex);
printf("SPEEDO: %ld %ld %g    %g \n",speedo_data.time_sec,speedo_data.time_usec,speedo_data.delta_t,speedo_data.speed);
pthread_mutex_unlock(&speedo_data_mutex);

pthread_mutex_lock(&compass_data_mutex);
printf("COMPASS: %ld %ld %g    %g \n",compass_data.time_sec,compass_data.time_usec,compass_data.delta_t,compass_data.heading);
pthread_mutex_unlock(&compass_data_mutex);

pthread_mutex_lock(&gps_data_mutex);
printf("GPS: %ld %ld %g    %g \n",gps_data.time_sec,gps_data.time_usec,gps_data.delta_t,gps_data.lat);
pthread_mutex_unlock(&gps_data_mutex);


sleep(4);
//usleep(100000);
}


pthread_exit(NULL);
return 0;
}