#include <sched.h>
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <assert.h>
#include <rtai_lxrt.h>
#include <signal.h>
#include <sys/io.h>
#include <sys/types.h>
#include <unistd.h>
#include <pthread.h>
#include <FL/Fl.H>
#include <FL/Fl_Window.H>
#include <FL/Fl_Value_Slider.H>

volatile unsigned int on_time[2] = {1500000, 1500000};
unsigned int  off_time = 17000000;
enum {SERVO_A, SERVO_B};
volatile int loop_again = 1;

void* servo_task(void *p)
{
	RT_TASK *task;
	int pin = (int)p;
	
	iopl(3);
	task = rt_task_init(rt_get_name(0), 0, 4096, 0);
	if(!task) {
		printf("unable to create slave task...\n");
		return 0;
	}

	mlockall(MCL_CURRENT | MCL_FUTURE);
	rt_make_hard_real_time();
	while(loop_again) {
		outb(inb(0x378)|(1 << pin), 0x378);
		rt_sleep(nano2count(on_time[pin]));
		outb(inb(0x378)&~(1 << pin), 0x378);
		rt_sleep(nano2count(off_time));
	}
	rt_make_soft_real_time();
	rt_task_delete(task);
	return 0;
}

void slider_event(Fl_Widget *w, void *servo)
{
	double curr;
	curr = ((Fl_Value_Slider*)w)->value();
	on_time[(int)servo] = (unsigned int)(curr * 1000000);
	
}

void slider_config(Fl_Value_Slider *s, double value, double *range,
                   double step,  void (*handler)(Fl_Widget*, void*), 
                   int servo)
{
	s->textcolor(FL_RED);
	s->value(value);
	s->range(range[0], range[1]);
	s->step(step);
	s->callback(handler, (void*)servo);
}
	

int main(int argc, char **argv)
{
	RT_TASK *task;
	pthread_t th1, th2;
	double range[2] = {0.5, 2.2};
	Fl_Window *root;
	Fl_Value_Slider *slider_a, *slider_b;

	task = rt_task_init(nam2num("main"), 0, 4096, 0);
	assert(task != 0);
	rt_set_oneshot_mode();
	start_rt_timer(0);
	
	pthread_create(&th1, 0, servo_task, (void*)SERVO_A);
	pthread_create(&th2, 0, servo_task, (void*)SERVO_B);

	root = new Fl_Window(300, 180);
	slider_a = new Fl_Value_Slider(100, 10, 25, 150, "Servo A");
	slider_b = new Fl_Value_Slider(180, 10, 25, 150, "Servo B");
	slider_config(slider_a, 0.5, range, 0.085, slider_event, SERVO_A);
	slider_config(slider_b, 0.5, range, 0.085, slider_event, SERVO_B);
	
	root->end();
	root->show(argc, argv);
	Fl::run();
	loop_again = 0;
	pthread_join(th1, 0);
	pthread_join(th2, 0);
	stop_rt_timer();
	rt_task_delete(task);
	return 0; 

}
