/*  
    RT_CONTROL
    kernel module meant to sample the ni-6052e-pci device at a high frequency and 
    implement real time control on the incomming data.

    to compile:
    gcc -c -D__KERNEL__ -DMODULE -I<rtai dir> -isystem /lib/modules/`uname -r`/build/include -o <output filename> <this file>

    note: be sure the kernel version of comedilib is in /usr/include/linux 

    Originally writted by Alan Chen <alanchen@mit.edu> for

    Modifications by Artem <artems@mit.edu>:
       - multichannel functionality added
       - control algorithm is taken outside this file to simplify user implementation
       - more fifos added for on-line configuration and data streaming

*/

#include <linux/module.h> 
#include <asm/adeos.h>
#include <rtai_sched.h>
#include <math.h>
#include <linux/comedilib.h>
#include <stdio.h>
#include "rt_info.h"
#include "control.c"

MODULE_LICENSE("GPL"); 
MODULE_SUPPORTED_DEVICE("ni-6052e-pci");
MODULE_AUTHOR("alanchen@mit.edu, artems@mit.edu"); 
MODULE_DESCRIPTION("Detect signals at input CH0 and send command pulses to output CH1");

RT_TASK rttask;
struct comedi_insn_struct insns_1[3];
struct comedi_insn_struct insns_2[5];
comedi_t *comediDev;

struct data_packet_struct data_packet;

/* user globals */
//extern long long start_timestamp;

// start of the actual process
void control_process (int whatever) {

  // conversion
  double bias=BIPOLAR*(RANGE_HIGH-RANGE_LOW)/2;		
  double scale=(RANGE_HIGH-RANGE_LOW)/pow(2,PRECISION);		

  double output[2];
  double input[4];
  int i;
  
  if (VERBOSE) printk("Starting control_process\n");

  memset(&data_packet, 0, sizeof(data_packet));

  while (1) {
    
    // instructions to do before the control algorithm
    if (comedi_do_insn(comediDev, &insns_1[0]) < 0 || 
	comedi_do_insn(comediDev, &insns_1[1]) < 0 || 
	comedi_do_insn(comediDev, &insns_1[2]) < 0)
      printk("error doing insns_1\n");
    

    for (i=0 ; i<4 ; i++)
      input[i] = data_packet.ad_samples[i]*scale-bias;

    // ==============================
    control_algorithm(input, output);
    // ==============================

    data_packet.da_samples[0]=(lsampl_t)((output[0]+bias)/scale);
    data_packet.da_samples[1]=(lsampl_t)((output[1]+bias)/scale);

    // instructions to do after the control algorithm
    if (comedi_do_insn(comediDev, &insns_2[0]) < 0 || 
	comedi_do_insn(comediDev, &insns_2[1]) < 0 || 
	comedi_do_insn(comediDev, &insns_2[2]) < 0 || 
	//	comedi_do_insn(comediDev, &insns_2[3]) < 0 ||    // input channel 3 is not used, don't read it
	comedi_do_insn(comediDev, &insns_2[4]) < 0)
      printk("error doing insns_2\n");    

    // block until the next interrupt: 
    rt_task_wait_period();							
  }
  return;
}



int init_comedi() {

  if (VERBOSE) printk("Initializing rt_control\n");

  if ((comediDev=comedi_open("/dev/comedi0"))==NULL) {				// open comedi device
    printk("Failed to open comedi device\n");
  } else if (VERBOSE) printk("Opening comedi device\n");

  if (comedi_lock(comediDev,AI_SUBDEV)) {					// lock the a/d
    printk("Failed to lock subdevice 0\n");
  } else if (VERBOSE) printk("Locking sub-devce 0\n");

  if (comedi_lock(comediDev,AO_SUBDEV)) {					// lock the d/a
    printk("Failed to lock subdevice 1\n");
  } else if (VERBOSE) printk("Locking sub-device 1\n");

  /* 
     It is advised in comedi mamual that some waiting is allowed 
     between read operations from different channels.
     This is why the following strange sequence of instructions is chosen:

     before calling the control algorithm:
            input channel 0 is read
	    write to output channel 0 is performed (stored value from a previous cycle)
	    'read focus' moved to input channel 1
     after control algorithm:
            input channel 1 is read
	    write to output channel 1 is performed (this will be a freshly updated value)
	    more channels read if required
	    'read focus' is moved to input channel 0
  */

  // before the control algorithm:

  insns_1[0].insn = INSN_READ;
  insns_1[0].n = 1;
  insns_1[0].data = data_packet.ad_samples ;
  insns_1[0].subdev=AI_SUBDEV;
  insns_1[0].chanspec = CR_PACK(0,0,AREF_DIFF);  

  insns_1[1].insn = INSN_WRITE;						
  insns_1[1].n = 1;
  insns_1[1].data = data_packet.da_samples ; 
  insns_1[1].subdev = AO_SUBDEV;
  insns_1[1].chanspec = CR_PACK(0,0,AREF_GROUND);

  insns_1[2].insn = INSN_READ;
  insns_1[2].n = 0;
  insns_1[2].data = data_packet.ad_samples + 1 ;
  insns_1[2].subdev = AI_SUBDEV;
  insns_1[2].chanspec = CR_PACK(1, 0, AREF_DIFF);

  // after the control algorithm:

  insns_2[0].insn = INSN_READ;
  insns_2[0].n = 1;
  insns_2[0].data = data_packet.ad_samples + 1 ;
  insns_2[0].subdev=AI_SUBDEV;
  insns_2[0].chanspec = CR_PACK(1,0,AREF_DIFF);  

  insns_2[1].insn = INSN_WRITE;				
  insns_2[1].n = 1;
  insns_2[1].data = data_packet.da_samples + 1 ; 
  insns_2[1].subdev = AO_SUBDEV;
  insns_2[1].chanspec = CR_PACK(1,0,AREF_GROUND);

  insns_2[2].insn = INSN_READ;
  insns_2[2].n = 1;
  insns_2[2].data = data_packet.ad_samples + 2 ;
  insns_2[2].subdev=AI_SUBDEV;
  insns_2[2].chanspec = CR_PACK(2,0,AREF_DIFF);  

  insns_2[3].insn = INSN_READ;
  insns_2[3].n = 1;
  insns_2[3].data = data_packet.ad_samples + 3 ;
  insns_2[3].subdev=AI_SUBDEV;
  insns_2[3].chanspec = CR_PACK(3,0,AREF_DIFF);  

  insns_2[4].insn = INSN_READ;
  insns_2[4].n = 0;
  insns_2[4].data = data_packet.ad_samples ;
  insns_2[4].subdev = AI_SUBDEV;
  insns_2[4].chanspec = CR_PACK(0, 0, AREF_DIFF);

  return 0;
}


void clean_comedi() {

  if (comedi_unlock(comediDev,AI_SUBDEV)) 					// unlock the a/d
    printk("Failed to unlock comedi subdev 0\n");
  else if (VERBOSE) printk("Unlocked subdev 0\n");

  if (comedi_unlock(comediDev,AO_SUBDEV)) 					// unlock the d/a
    printk("Failed to unlock comedi subdev 1\n");
  else if (VERBOSE) printk("Unlocked subdev 1\n");

  if (comedi_close(comediDev))							// close the comedi device
    printk("Failed to close the comedi device\n");
  else if (VERBOSE) printk("Closed Comedi device\n");

}


 
//static int __init moduleInit(void)
int init_module(void)
{

  int tick_period;

  // call the initialize comedi routine
  init_comedi();			

  // set timer mode
  rt_set_periodic_mode();		

  // rt_task_init(struct RT_TASK, void function(int arg), int arg, int stacksize, int priorty (0=high), int usesFPU (1=yes), void signal)
  rt_task_init(&rttask, control_process, 0, STACKSIZE, PRIORITY, 1, 0);

  // create and reset fifos, use this one to uni-directionally stream any sort of data
  rtf_create(FIFO, FIFOSIZE);
  rtf_reset(FIFO);

  // this one has an associated user-defined handler, use it to interactively adjust control parameters
  rtf_create(FIFO1, FIFOSIZE);
  rtf_reset(FIFO1);
  rtf_create_handler(FIFO1, adjust_params);

  // one more fifo and semaphore-fifo for saving captured traces around detected events
  rtf_create(FIFO2, sizeof(int) * 2 * (1+MAX_STORAGE*2));
  rtf_reset(FIFO2);
  rtf_sem_init(FIFO3, 0);
 
  tick_period = start_rt_timer(nano2count(RT_PERIOD));

  init_control();
  
   rt_task_make_periodic(&rttask, rt_get_time() + tick_period, tick_period);

  return 0;
}


void cleanup_module(void)
{


   // clean up comedi leftoevers
   clean_comedi();

   // stop the timer
   stop_rt_timer();

   // delete the task
   rt_task_delete(&rttask);

   // destroy FIFOs
   rtf_destroy(FIFO);
   rtf_destroy(FIFO1);
   rtf_destroy(FIFO2);
   rtf_sem_destroy(FIFO3);

   if (VERBOSE) printk("FIFOs destroyed\n");
   if (VERBOSE) printk("Exiting Module\n");
   return;
}