#include <stdio.h>
#include <errno.h>
#include <sys/time.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <rtl_fifo.h>
#include <rtl_time.h>
#include "control.h"
#include "mbuff.h"

#include "rtcontrol.h"

#define BUFSIZE 70

char buf[BUFSIZE];

/* the contents of shared memory may change at any time, thus volatile */
volatile char * shm1;
struct my_msg_struct msg;
int fd0, fd1, ctl;
robot_JLS_type *robot_JLS;


int init_rtcontrol()
{



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

	if ((fd0 = open("/dev/rtf1", O_RDONLY)) < 0) {
		fprintf(stderr, "Error opening /dev/rtf1\n");
		exit(1);
	}

	if ((fd1 = open("/dev/rtf2", O_RDONLY)) < 0) {
		fprintf(stderr, "Error opening /dev/rtf2\n");
		exit(1);
	}

	if ((ctl = open("/dev/rtf3", O_WRONLY)) < 0) {
		fprintf(stderr, "Error opening /dev/rtf3\n");
		exit(1);
	}

	
	return 0;
}

int quit_rtcontrol()
{
	fprintf(stderr, "control_app: now sending commands to stop RT-tasks\n");
	/* stop the tasks */
	msg.command = STOP_TASK;
	msg.task = 0;
	if (write(ctl, &msg, sizeof(msg)) < 0) {
		fprintf(stderr, "Can't send a command to RT-task\n");
		return(1);
	}

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


}


int stop_rtcontrol()
{

	fprintf(stderr, "control_app: now sending commands to stop RT-tasks\n");
	msg.command = STOP_TASK;
	msg.task = 0;
	if (write(ctl, &msg, sizeof(msg)) < 0) {
		fprintf(stderr, "Can't send a command to RT-task\n");
		exit(1);
	}
	return(0);
}

int start_rtcontrol()
{

	fprintf(stderr, "control_app: now sending commands to re-start RT-tasks\n");
	/* Restart the task */
	msg.command = START_TASK;
	msg.task = 0;
	msg.period = 1000;  /* microseconds */
	if (write(ctl, &msg, sizeof(msg)) < 0) {
		fprintf(stderr, "Can't send a command to RT-task\n");
		return(1);
	}
	return(0);

}

int write_setpt(short *val)
{
  short i;
  for (i = 0; i < 6; i++) {
    robot_JLS->setpt[i] = val[i];
  }
return(0);
}

int write_gain(short *gain)
{
  short i;
  for (i = 0; i < 6; i++) {
    robot_JLS->gain[i] = gain[i];
  }
return(0);
}

int write_target(short *target)
{
  short i;
  for (i = 0; i < 6; i++) {
    robot_JLS->target[i] = target[i];
  }
return(0);
}
int read_position(short *pos)
{
  short i;
  for (i = 0; i < 6; i++) {
    pos[i]= robot_JLS->pos[i]; 
  }
return(0);
}
