#include <linux/errno.h>
#include <rtl.h>
#include <time.h>

#include <unistd.h>
/* #include <asm/io.h> */
#include <sys/io.h>

#include <rtl_sched.h>
#include <rtl_fifo.h>
#include "control.h"
#include "mbuff.h"

pthread_t tasks[2];

/* the contents of shared memory may change at any time, thus volatile */
volatile char * shm1;
static	robot_JLS_type *robot_JLS;
static	int started_flag;

#define TASK_CONTROL_FIFO_OFFSET 4

short   encoderOffsetG[6] = {0,0,0,8000,0,0};

/* This task will read start and stop commands from the fifo */
void *thread_code(void *t)
{
	int fifo = (int) t;
	int taskno = fifo - 1;
	struct my_msg_struct msg;
	short controlval[6];

	while (1) {
	  int ret;
	  int i;
	  int err;
	  ret = pthread_wait_np();
	  if ((err = rtf_get (taskno + TASK_CONTROL_FIFO_OFFSET, &msg, sizeof(msg))) == sizeof(msg)) {
	    rtl_printf("Task %d: executing the \"%d\" command to task %d; period %d\n", fifo - 1,  msg.command, msg.task, msg.period);
	    switch (msg.command) {
	    case START_TASK:
	      started_flag = 1;
	      pthread_make_periodic_np(pthread_self(), gethrtime(), msg.period * 1000);
	      
	      break;
	    case STOP_TASK:
	      pthread_suspend_np(pthread_self());
	      started_flag = 0;
	      break;
	    default:
	      rtl_printf("RTL task: bad command\n");
	      return 0;
	    }
	  }
	  /* Perform control function here */
          /* Read all the joint values */

	  if(started_flag) {
	    
	    for (i=0; i<6; i++){
	      robot_JLS->pos[i] = (inw(BASE_ADDR + ENC_COUNT_BASE + 2*i)
				   -encoderOffsetG[i]);
	      robot_JLS->pos[i] = (inw(BASE_ADDR + ENC_COUNT_BASE + 2*i)
				   -encoderOffsetG[i]);
	      
	    } /* endfor */
	    /* This function implements the simplest form of P-control for
	       each joint.  Encoders should be zeroed before beginning this
	       function */

	    for (i=0; i<6; i++){
	      controlval[i]=
		 (robot_JLS->setpt[i]-robot_JLS->pos[i])*robot_JLS->gain[i]; 
	      outw(0x0800+controlval[i],BASE_ADDR + DAC_BASE + 2*i);
	      } 
	    /* Implement an ever simpler minded trajectory generator */

	    for (i=0; i<6; i++){
	      if(robot_JLS->target[i] > robot_JLS->setpt[i])
		robot_JLS->setpt[i]++;
	      if(robot_JLS->target[i] < robot_JLS->setpt[i])
		robot_JLS->setpt[i]--;
	    }

	    }
	  

	  }
	return 0;
}

int my_handler(unsigned int fifo)
{
	struct my_msg_struct msg;
	int err;

	while ((err = rtf_get(COMMAND_FIFO, &msg, sizeof(msg))) == sizeof(msg)) {
		rtf_put (msg.task + TASK_CONTROL_FIFO_OFFSET, &msg, sizeof(msg));
		rtl_printf("FIFO handler: sending the \"%d\" command to task %d; period %d\n", msg.command,
				msg.task, msg.period);
		pthread_wakeup_np (tasks [msg.task]);
	}
	if (err != 0) {
		return -EINVAL;
	}
	return 0;
}


int init_module(void)
{
	int c[5];
	pthread_attr_t attr;
	struct sched_param sched_param;
	int ret;

	rtf_destroy(1);
	rtf_destroy(2);
	rtf_destroy(3);
	rtf_destroy(4);
	rtf_destroy(5);
 	c[0] = rtf_create(1, 4000);
	c[1] = rtf_create(2, 4000);
	c[2] = rtf_create(3, 200);	/* input control channel */
	c[3] = rtf_create(4, 100);	/* input control channel */
	c[4] = rtf_create(5, 100);	/* input control channel */
	printk("Fifo return 1=%d 2=%d 3=%d\n",c[0],c[1],c[2]);

	pthread_attr_init (&attr);
	sched_param.sched_priority = 4;
	pthread_attr_setschedparam (&attr, &sched_param);
	ret = pthread_create (&tasks[0],  &attr, thread_code, (void *)1);
	

	/* Remove second task for now MS
	pthread_attr_init (&attr);
	sched_param.sched_priority = 5;
	pthread_attr_setschedparam (&attr, &sched_param);
	ret = pthread_create (&tasks[1],  &attr, thread_code, (void *)2);
	*/
	
	rtf_create_handler(3, &my_handler);


	shm1 = (volatile char*) mbuff_alloc("control",1024);
	robot_JLS = (robot_JLS_type *) shm1;
	started_flag = 1;

	return 0;
}


void cleanup_module(void)
{

	rtf_destroy(1);
	rtf_destroy(2);
	rtf_destroy(3);
	rtf_destroy(4);
	rtf_destroy(5);

	pthread_delete_np (tasks[0]);
/*	pthread_delete_np (tasks[1]); */

	mbuff_free("control",(void*)shm1);


}
