#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 	1
#define TIMEBASE 	1000 * 1000

bool finish 			= false; 
unsigned long periods 	= 0;
unsigned long ints 		= 0;

void int_task(void* cookie){
//	RT_QUEUE outQ;

//	rt_queue_create(&outQ, "out-queue", 1000, 1000, 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++;
			if (counter == 1000) raise(SIGTERM);
				
			std::cout << "interrupt count: "  << counter  << 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 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_join(&intcreatortask);
	rt_task_join(&inttask);
	//rt_task_join(&queue_endtask);
	
	std::cout << "task's finished, exiting" <<  "[" << periods << "/" << ints << "]" << std::endl;
	
	return 0;
}