#include <stdio.h>
#include <semaphore.h>
#include <sys/timeb.h>
#include <sys/time.h>
#include <errno.h>
#include <string.h>
#include <pthread.h>
#include <time.h>


#define NUM_THREADS		200
#define INTERVAL		2

void* worker_thread(void *p_argument)
{
	int iIteration = 0;
	sem_t wake_semaphore;
	sem_init(&wake_semaphore, 0, 0);

	while (1)
	{
		iIteration++;
		struct timeb current_time;
		struct timeval current_timeofday;
		struct timespec clock_realtime;

		//Calculate next wake time
		ftime(&current_time);
		time_t wake_time = current_time.time + (INTERVAL - (current_time.time % INTERVAL));

		//Setup abstime struct for sem_timedwait
		struct timespec timeout;
		timeout.tv_sec = (long)wake_time;
		timeout.tv_nsec = 0;

		int result = sem_timedwait(&wake_semaphore, &timeout);

		//Get various time sources
		time_t time_time = time(NULL);
		ftime(&current_time);
		gettimeofday(&current_timeofday, NULL);
		clock_gettime(CLOCK_REALTIME, &clock_realtime);


		if (result != -1 && errno != ETIMEDOUT)
		{
			printf("%p: Error: sem_timedwait returned %u (%s)\n", (void*)pthread_self(), result, strerror(errno));
		}
		else
		{
			if (current_time.time != wake_time ||
				time(NULL) != wake_time ||
				current_timeofday.tv_sec != wake_time ||
				clock_realtime.tv_sec != wake_time)
			{
				printf("%p: Woken up at\n"
					"\titeration                     = %u\n"
					"\twake_time                     = %lu\n"
					"\ttime()                        = %lu\n"
					"\tftime()                       = %lu.%03u\n"
					"\tgettimeofday()                = %lu.%06lu\n"
					"\tclock_gettime(CLOCK_REALTIME) = %lu.%09lu\n", 
					(void*)pthread_self(), 
					iIteration, 
					wake_time,
					time_time,
					current_time.time,
					current_time.millitm,
					current_timeofday.tv_sec,
					current_timeofday.tv_usec,
					clock_realtime.tv_sec,
					clock_realtime.tv_nsec);
			}
		}
	}
	pthread_exit(0);
	return 0;
}



int main(int argc, char **ppargv)
{
	int i;
	pthread_t thread_handler_list[NUM_THREADS];

	for (i = 0; i < NUM_THREADS; i++)
	{
		if (pthread_create(&thread_handler_list[i], NULL, &worker_thread, NULL))
		{
			printf("Error creating thread no %u. %s\n", i, strerror(errno));
			return -1;
		}
	}

	pthread_join(thread_handler_list[0], NULL);


	return 0;
}