#include <iostream>
#include <sys/mman.h>
#include <assert.h>
#include "native/task.h"
#include "native/timer.h"
#include "native/queue.h"
#include "native/intr.h"

#include <signal.h>
#include <sys/io.h>

RT_TASK maintask;
RT_TASK inttask;
RT_TASK intcreatortask;
RT_TASK queue_endtask;

RT_INTR lptint;

#define LPT_BASE		0x378
#define LPT_INT			7

#define PERIOD_TIME 	10
#define TIMEBASE 		10000
bool finish = false; 

unsigned long periods 	= 0;
unsigned long ints 		= 0;

void int_task(void* cookie){
	
	RT_QUEUE outQueue;
	rt_queue_create(&outQueue,"out-queue",10000,1024000,Q_FIFO);  
	
	int err = 0;

	err = rt_intr_create(&lptint,"parp_int",7,0);
	std::cout << "interrupt create:" << err << std::endl;
	
	err = rt_intr_enable(&lptint);
	std::cout << "interrupt enable:" << err << std::endl;
	// intr mode on
	outb_p(0x10, LPT_BASE + 2);
	
	// pin9 off
	outb_p(0x00, LPT_BASE );
	
	int counter = 0;
	
	while(!finish){
			
		err = rt_intr_wait(&lptint, 100);
		
		if(err >= 0 ){
			counter++;
			ints++;
			err = rt_queue_write(&outQueue, &counter, sizeof(int), Q_NORMAL);
		//	std::cout << "interrupt count: "  << counter << " writung queue: " << err << std::endl; 			
		}
	}
	
	err = rt_queue_delete(&outQueue);
	std::cout << "queue delete:" << err << std::endl;
	
	err = rt_intr_delete(&lptint);
	std::cout << "interrupt delete:" << err << std::endl;
	
	std::cout << "leaving interrupt task" << std::endl;
}

void int_creator_task(void* cookie){
	rt_task_set_periodic(NULL,0,PERIOD_TIME);
	while(!finish){	
			outb_p(0x00,LPT_BASE);
			outb_p(0xFF,LPT_BASE);
			rt_task_sleep(1);
			outb_p(0x00,LPT_BASE);
			rt_task_wait_period(NULL);	
			periods++;
	}
	std::cout << "leaving int creator task" << std::endl;
}

void output_task(void* cookie){
	RT_QUEUE outQueue;
	int value;
	int err;
	
	err = rt_queue_bind(&outQueue,"out-queue",TM_INFINITE);
	std::cout << "binding queue: " << err << std::endl;

	while(!finish){
			err = rt_queue_read(&outQueue, &value, sizeof(int), 1000);
			if(err > 0) std::cout << "  -->  gelesen : " << value << std::endl;
			if(value > 5000) raise(SIGTERM);
	}
	
	err = rt_queue_delete(&outQueue);
	std::cout << "queue delete:" << err << std::endl;
	
	std::cout << "leaving outtask" << std::endl;
}

void catch_signal(int sig) {
	std::cout << "got signal..." << std::endl;
	finish = true;
}

int main(void){
	std::cout << "xenomai interrupt test" << std::endl;
	mlockall(MCL_CURRENT | MCL_FUTURE);
	
	signal(SIGTERM,  catch_signal);
	signal(SIGKILL,  catch_signal);
	signal(SIGQUIT,  catch_signal);
	signal(SIGINT ,  catch_signal);	
	
	if (iopl(3)) {  
		printf("iopl err\n");
		exit(1);
	}
	
	int err;

	err = rt_task_shadow (&maintask,"maintask",10,0);
	std::cout << "task shadow:" << err << std::endl;
	
	err = rt_timer_set_mode(TIMEBASE);
	std::cout << "timer set mode:" << err << std::endl;
	
	rt_task_spawn(&inttask,"interrupt-task",1024,99,T_JOINABLE,int_task,NULL);
	
	rt_task_spawn(&intcreatortask,"interrupt-creator-task",1024,50,T_JOINABLE,int_creator_task,NULL);
	
	rt_task_spawn(&queue_endtask,"queue_endtask",1024,1,T_JOINABLE,output_task,NULL);
	
	rt_task_join(&intcreatortask);
	rt_task_join(&inttask);
	rt_task_join(&queue_endtask);
	
	std::cout << "task's finished, exiting" <<  "[" << periods << "/" << ints << "]" << std::endl;
	
	return 0;
}