#include <sys/mman.h>
#include <csignal>
#include <native/queue.h>
#include <native/timer.h>
#include <native/task.h>
#include <cmath>

bool running = true;
RT_QUEUE queue;

void exitSignal (int param)
{
	printf ("Exiting program...\n");
	running = false;
}

void mainLoop(void* dummy)
{	
	int status;
	printf("Main loop...\n");
		
	RTIME currentTime, lastTime;
	
	double result = 7.0;
	double a = 1000.;
	double b = 333.;
	unsigned int counter = 0;
	float periodTime = 500.0f;
	float processingTime = 0.0f;
	double position[3];
	
	while(running)
	{
		rt_task_wait_period(NULL);
		
		RTIME startTime = rt_timer_read();
		for(unsigned long long int i=0; i < ((unsigned long)result) % 5 + 3000; ++i)
		{
			a += sin((double)i / 500.);
			b = sqrt(a);
			result += ((double)i / 3.0 + 3.1423) * (double) i - a * b;
		}
		currentTime = rt_timer_read();
		
		processingTime = 0.9f * processingTime + 0.1f * (float)(currentTime - startTime) / 1000.f;
		periodTime = 0.9f * periodTime + 0.1f * (float)(currentTime - lastTime) / 1000.f;
		lastTime = currentTime;
		
		/*if(rt_queue_read(&queue, (void*)position, 3*sizeof(double), TM_NONBLOCK) > 0)
		{	
			printf("X: %.1f  Y: %.1f  Z: %.1f\n", position[0], position[1], position[2]);
		}*/
		
		if(counter++ > 100)
		{
			counter = 0;
			printf("Period Time: %.1fus      Result: %.1f\n", periodTime, result);
			printf("Processing Time: %.1fus\n", processingTime);
		}
	}	
}


int main (int argc , char * argv []) 
{
	mlockall(MCL_CURRENT | MCL_FUTURE);
	
	signal (SIGINT, exitSignal);
	printf("Initialization...");
	
	int status;
								
	/*if(rt_queue_bind(&queue, "camPosQueue", TM_NONBLOCK) != 0)
	{
		return 0;
	}*/
	
	rt_timer_set_mode(TM_ONESHOT);
	
	RT_TASK task;
	if(rt_task_create(&task, NULL, 0, 80, T_JOINABLE) != 0)
	{
		printf("Creation of Xenomai Task failed!\n");
		return 0;
	}
	
	rt_task_set_periodic 	(&task, TM_NOW,	4000000);

	printf("done!\n");
	
	rt_task_start(&task, &mainLoop, (void*)NULL);
  	
	
	rt_task_join(&task);	
	
	mainLoop(NULL);
	
	printf("CleanUp...");
	
	//rt_queue_unbind(&queue); 
 	
	printf("done!\n");
	return 0;
}