/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd and Vincent Hayward.	    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *      Modified by Matthew Stein 2002                                      *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/*-=-=-=-=-=-=-=-=-=-=-=- Description -=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-*
  This module is an supplement to RCI/RCCL for RTLINUX using a single 
  processor to perform both the joint-level servo, the control level and
  the planning level.

  Under RTLinux, this program will be loaded into the kernel using insmod
  and run on a real-time interrupt.  The interrupt scheduling is *not -
  preemptive* and this means the control level formerly implemented by
  rci will have to share interrupts with the JLS interrupt.  The control
  level must execute on an integral number of JLS interrupt periods and 
  execution will be scheduled by the JLS task.

  This module implements the RCIdevice for simulation.  It is separate
  from moperx.c so that programs can be run in simulation (with robotsim)
  without interfereing with the Joint Level Servo.  This program is based
  on moperx.c, but *does not* run the JLS.  Instead it
  communicates of different RTFIFOs and simply sends packes on this FIFO
  at regular 28ms intervals. 
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

/* These included for Real-Time Linux */
#include <linux/errno.h>
#include <rtl.h>
#include <time.h>
#include <unistd.h>
#include <rtl_sched.h>
#include <rtl_fifo.h>
#include "mbuff.h"

/* Definition of the real-time fifo numbers */
#include "../h/fifos.h"

pthread_t simtask;

static	int started_flag;
static  int started_cl=0;
static  int Alarm_count=0;
static  int Beat_count=0;


/* Number of .9 ms intervals to run the control level */
static  int jlsBeatInterval = 1; /* This sets the rci system clock to a
				     9 ms interval.  Can be changed by
				     contol level
				  */
static  int jlsAlarmInterval = 0; /* This should be set by the control level
				     requesting an alarm signal */


/* This task will read start and stop commands from the TASK_CONTROL_FIFO  */
void *thread_code(void *t)
{
  jlsMessageType msg;
  RciTimerEvent tev;

  while (1) {
    int ret;
    int err;
    
    /* This function waits until the next time period to return, i.e. this is the
       function that makes this thread periodic.  When this returns, perform all the
       tasks necessary and then return to this line due to the while(1) above.
    */
    ret = pthread_wait_np();
    /* Fist check if new commands have been sent over the fifo, and process 
       as necessary
    */
    if ((err = 
     rtf_get ( SIM_TASK_CONTROL_FIFO, &msg, sizeof(msg))) == sizeof(msg)) {
      rtl_printf("Executing the \"%d\" command \n",  msg.command);
      switch (msg.command) {
      case START_TASK:
	started_flag = 1;
	/* period is expressed in nanoseconds, thus the multiplier of
	   100000.  My how times change.*/
	pthread_make_periodic_np(pthread_self(), gethrtime(), 
				 280 * 100000);
	

	break;
      case STOP_TASK:
	started_flag = 0;
	/* Suspend right here until my_handler wakes us up with a call to
	   pthread_wakeup_np.  This happens when data arrives on the fifo, and
	   thus we do not set started_flag until the the fifo is read and the
	   command is START_TASK
	*/ 

	pthread_suspend_np(pthread_self());
	break;
      case START_ALRM_MSG:
	/* Send am alarm message to the CONTROL_LEVEL_FIFO to
	   activate the alarm task task
	*/
	
	if(msg.arg > 0 && msg.arg < 1000 ) /* This 1000 is arbitrary */
	  jlsAlarmInterval = msg.arg; 
	break;
      case STOP_ALRM_MSG:
	/* Stop sending alarm messages to the CONTROL_LEVEL_FIFO to
	   activate the alarm task.   However,
	   stopping beat messages does not stop the beat messages. 
	*/ 
	jlsAlarmInterval =0;
	break;

	/* MS Beat messages are activated by the planning task 
	   when creating monitor tasks.  These are the main heartbeat
	   of the planning task.
	*/
      case START_BEAT_MSG:
	/* Start sending periodic beat messages to the CONTROL_LEVEL_FIFO to
	   activate the rci control task.  Alarm messages must already be active
	   for beat messages to be activated.
	*/ 
	started_cl = 1;

	if(msg.arg > 0 && msg.arg < 1000 ) /* This 1000 is arbitrary */
	  jlsBeatInterval = msg.arg;
	
	pthread_make_periodic_np(pthread_self(), gethrtime(), 
				 280 * 100000);
	
	break;
      case STOP_BEAT_MSG:
	
	/* Stop sending periodic beat messages to the CONTROL_LEVEL_FIFO. 
	   Halting these should also stop the
	   periodic task from running
	*/ 
	started_cl = 0;
	
	pthread_suspend_np(pthread_self());
	
	break;
      default:
	rtl_printf("RTL task: bad command\n");
      }
    }
    

    if(started_cl) {
      if(jlsAlarmInterval != 0) {
	if(++Alarm_count >= jlsAlarmInterval) {
	  
	  tev.req=RCI_ALARM_READY;
	  rtf_put(SIM_CONTROL_LEVEL_FIFO,&tev, sizeof(tev));
	  Alarm_count=0;
	  jlsAlarmInterval=0; /* Alarms occur once */
	}
	
      }
      if(++Beat_count >= jlsBeatInterval) {
	
	tev.req=RCI_BEAT_READY;
	rtf_put(SIM_CONTROL_LEVEL_FIFO,&tev, sizeof(tev));
	Beat_count=0;
      }
    }
    
  } /* End of while(1) */
}

int my_handler(unsigned int fifo)
{
  jlsMessageType msg;
  int err;
	
  while ((err = rtf_get(SIM_ACTIVATION_FIFO, &msg, sizeof(msg))) == sizeof(msg)) {
    /* I have been activated because message is available on ACTIVATION_FIFO,
       Immediately pass the message to TASK_CONTROL_FIFO and then wake up 
       the control task 
    */
    rtl_printf("FIFO handler: sending the \"%d\" command to task \n", msg.command);

    rtf_put (SIM_TASK_CONTROL_FIFO, &msg, sizeof(msg));
    
    if((msg.command == START_BEAT_MSG) || (msg.command == START_TASK))
      {
	pthread_wakeup_np (simtask);
      }
  }
  if (err != 0) {
    rtl_printf("FIFO handler: error in \"%d\" \n", msg.command);
    return -EINVAL;
  }
  return 0;
}


int init_module(void)
{
  /* This function called by system function insmod when module is loaded 
   */ 
     
	pthread_attr_t attr;
	struct sched_param sched_param;
	int ret;

	Alarm_count = 0;
	Beat_count = 0;
	rtf_destroy(SIM_ACTIVATION_FIFO);
	rtf_destroy(SIM_TASK_CONTROL_FIFO);
	rtf_destroy(SIM_CONTROL_LEVEL_FIFO);


	 rtf_create(SIM_ACTIVATION_FIFO, 200);	/* Control-Level control channel */
	 rtf_create(SIM_TASK_CONTROL_FIFO, 200);	
	 rtf_create(SIM_CONTROL_LEVEL_FIFO, 200);	/* JLS control channel */

	/* Debugging rtf for dumping data *//*  Pipe for data dump */
	  /* MS Debugging 

     	rtf_destroy(6);
         dump_pipe = rtf_create(6, 4000);  
	  */
	pthread_attr_init (&attr);
	/* Set the JLS as the highest priority task */
	sched_param.sched_priority = 1;
	pthread_attr_setschedparam (&attr, &sched_param);
	/* Specify the routine thread_code  (above) as a real-time thread */
	ret = pthread_create (&simtask,  &attr, thread_code, (void *)1);
	
	/* specify my_handler as the function to be called when data is read or 
	   written to real-time fifo #3 
	*/
	
	rtf_create_handler(SIM_ACTIVATION_FIFO, &my_handler);


	started_flag = 0;

	return 0;
}


void cleanup_module(void)
{
  /* This function called by system call rmmod when the module is removed.  */

	rtf_destroy(SIM_TASK_CONTROL_FIFO);
	rtf_destroy(SIM_CONTROL_LEVEL_FIFO);
	rtf_destroy(SIM_ACTIVATION_FIFO);

	pthread_delete_np (simtask);

}
