#include <comedilib.h>
#include <sys/mman.h>

#define HARD_LXRT
#define KEEP_STATIC_INLINE
#include <rtai_lxrt_user.h>
#include <rtai_lxrt.h>

#include "datos.h"
#include "neuroident.h"
#include "control_module.h"
#include "lxrt_wrapper.h"

comedi_t *dev;
int read_subdev, write_subdev;
comedi_range *read_range, *write_range;
lsampl_t read_maxdata, write_maxdata;

#define CM_BP 1000000

int configure_adc(comedi_data_str *dat);

void*
control_thread(void* arg)
{
  lsampl_t volt, u;
  double y, du;
  RT_TASK *mytask;
  RTIME tick_period;
  unsigned char lstop, stop;
  unsigned int period, myperiod;
  unsigned long mytask_name= nam2num(CONTROL_TASK);
  int iter= (int) arg;
  int cnt= 0, com_ret;
  MBX *period_mbx= exist(PERIOD_MBX);
  MBX *data_mbx= exist(DATA_MBX);
  MBX *stop_mbx= exist(STOP_MBX);
  MBX *input_mbx= exist(INPUT_MBX);
  int rec;
  comedi_data_str comd;
  neuro_data_str neurod;
  input_data_str linpd, inpd;
  lsampl_t osc_maxdata, osc_data;
  int osc_cnt= 0;

  inicializar_alg();

#ifdef DEBUG
  printf("control: Inicializando task...\n");
#endif
  mytask= rt_task_init_schmod(mytask_name, 1, 0, 0, SCHED_FIFO, 0);
  if(!mytask)
  {
    printf("No se puede crear el task de control\n");
    return NULL;
  }
#ifdef DEBUG
  printf("control: Task %p iniciado\n", mytask);
#endif
  mlockall(MCL_CURRENT | MCL_FUTURE);
  
#ifdef DEBUG
  printf("control: Pasando a \"hard real time\"\n");
#endif
  rt_make_hard_real_time();

  rt_send(exist(UI_TASK), 0);

#ifdef DEBUG
  printf("control: recibiendo datos comedi\n");
#endif
  rec= rt_mbx_receive(data_mbx, &comd, sizeof(comd));
#ifdef DEBUG
  printf("control: faltaron %d de %d.\n"
	 "control: comd.dev %d\n"
	 "control: comd.rchan %d\n"
	 "control: comd.wchan %d\n"
	 "control: comd.rrange %d\n"
	 "control: comd.wrange %d\n"
	 "control: comd.rref %d\n"
	 "control: comd.wref %d\n"
	 "control: Recibiendo datos input\n", rec,
	 sizeof(comd),
	 comd.dev, comd.rchan, comd.wchan, comd.rrange, comd.wrange,
	 comd.rref, comd.wref);
#endif
  rec= rt_mbx_receive(data_mbx, &inpd, sizeof(inpd));
  linpd.ref= inpd.ref;
  linpd.ident= inpd.ident;
#ifdef DEBUG
  printf("control: faltaron %d de %d.\n"
	 "control: inpd.ref %g\n"
	 "control: inpd.ident %d\n"
	 "control: recibiendo periodo\n", rec,
	 sizeof(inpd),
	 inpd.ref, inpd.ident);
#endif
  rec= rt_mbx_receive(period_mbx, &period, sizeof(period));
  myperiod= period;
  
#ifdef DEBUG
  printf("control: faltaron %d de %d.\n"
	 "control: perido %d\n"
	 "control: configurando comedi\n", rec,
	 sizeof(period), period);
#endif
  if(configure_adc(&comd) != 0)
    return 0;

#ifdef DEBUG
  printf("control: enviando read_range\n");
#endif
  rec= rt_mbx_send(data_mbx, read_range, sizeof(comedi_range));
#ifdef DEBUG
  printf("control: faltaron %d de %d. enviando write_range\n", rec,
	 sizeof(comedi_range));
#endif
  rec= rt_mbx_send(data_mbx, write_range, sizeof(comedi_range));
#ifdef DEBUG
  printf("control: faltaron %d de %d\n", rec,
	 sizeof(comedi_range));
#endif
  osc_maxdata= comedi_get_maxdata(dev, read_subdev, 1);

  tick_period = myperiod * nano2count(CM_BP);
  rt_task_make_periodic(mytask, rt_get_time(), tick_period);

  lstop= 0;
  while(!lstop)
  {
    /* Mostrar en osciloscopio */
    if(osc_cnt)
    {
      osc_cnt= 0;
      osc_data= osc_maxdata;
    }
    else
    {
      osc_cnt= 1;
      osc_data= 0;
    }
    comedi_data_write(dev, write_subdev, 1, 0, AREF_GROUND, osc_data);
    
    com_ret= comedi_data_read(dev, read_subdev,
			      comd.rchan,
			      comd.rrange,
			      comd.rref,
			      &volt);
    if(com_ret < 0)
      rt_printk("%s: %s\n", __PRETTY_FUNCTION__,
		comedi_strerror(comedi_errno()));
    else
    {
      y= comedi_to_phys(volt, read_range, read_maxdata);
      du= procesar(y, myperiod, &linpd, &neurod);
      u= comedi_from_phys(du, write_range, write_maxdata);
      com_ret= comedi_data_write(dev, write_subdev, comd.wchan,
				 comd.wrange, comd.wref, u);
      if(com_ret < 0)
	rt_printk("%s: %s\n", __PRETTY_FUNCTION__,
		  comedi_strerror(comedi_errno()));
    }
    /* Mostrar en osciloscopio */
    /*comedi_data_write(dev, write_subdev, 1, 0, AREF_GROUND, 0); */

#ifdef DEBUG
    //printf("control: Enviando condicional neurod\n");
#endif
    rec= rt_mbx_send_if(data_mbx, &neurod, sizeof(neurod));
    rt_task_wait_period();

#ifdef DEBUG
    /*printf("Faltaron %d de %d. Recibiendo condicional period\n", rec,
      sizeof(neurod));*/
#endif
    rec= rt_mbx_receive_if(period_mbx, &period, sizeof(period));
    if(rec == 0)
    {
      myperiod= period;
      tick_period = period * nano2count(CM_BP);
      rt_task_make_periodic(mytask, rt_get_time(), tick_period);
    }

#ifdef DEBUG
    /*printf("Faltaron %d de %d. Recibiendo condicional stop\n", rec,
      sizeof(period));*/
#endif
    rec= rt_mbx_receive_if(stop_mbx, &stop, sizeof(stop));
    if(rec == 0)
      lstop= stop;

#ifdef DEBUG
    /*printf("Faltaron %d de %d. Recibiendo condicional input\n", rec,
      sizeof(stop));*/
#endif
    rec= rt_mbx_receive_if(input_mbx, &inpd, sizeof(inpd));
    if(rec == 0)
    {
      linpd.ref= inpd.ref;
      linpd.ident= inpd.ident;
    }
#ifdef DEBUG
    //printf("Faltaron %d de %d\n", rec, sizeof(inpd));
#endif

    if(iter > 0)
    {
      ++cnt;
      if(cnt == iter)
	lstop= 1;
    }
  }

  rt_make_soft_real_time();
  rt_task_delete(mytask);

  u= comedi_from_phys(0, write_range, write_maxdata);

  com_ret= comedi_data_write(dev, write_subdev, comd.wchan,
			     comd.wrange, comd.wref, u);

  if(com_ret < 0)
    printf("%s: Saliendo del hilo de control: %s\n", __PRETTY_FUNCTION__,
	      comedi_strerror(comedi_errno()));
  comedi_close(dev);
  return NULL;
}

int
configure_adc(comedi_data_str *dat)
{
  char dev_str[20];

  sprintf(dev_str, "/dev/comedi%d", dat->dev);
#ifdef DEBUG
  printf("Abriendo dispositivo %s...\n", dev_str);
#endif
  dev= comedi_open(dev_str);
  if(!dev)
  {
    printf("%s: No se pudo abrir %s: %s\n", __PRETTY_FUNCTION__, dev_str,
	      comedi_strerror(comedi_errno()));
    return 1;
  }
#ifdef DEBUG
  printf("Buscando subdispositivo de entrada... ");
#endif
  read_subdev= comedi_find_subdevice_by_type(dev, COMEDI_SUBD_AI, 0);
  if(read_subdev < 0)
  {
    printf("%s: No se pudo encontrar subdispositivo de lectura: %s\n",
	      __PRETTY_FUNCTION__, comedi_strerror(comedi_errno()));
    return 1;
  }
#ifdef DEBUG
  printf("encontrado: %d\n", read_subdev);
  printf("Buscando subdispositivo de salida... ");
#endif
  
  write_subdev= comedi_find_subdevice_by_type(dev, COMEDI_SUBD_AO, 0);
  if(write_subdev < 0)
  {
    printf("%s: No se pudo encontrar subdispositivo de escritura: %s\n",
	      __PRETTY_FUNCTION__, comedi_strerror(comedi_errno()));
    return 1;
  }
#ifdef DEBUG
  printf("encontrado: %d\n", write_subdev);
#endif  
  if(comedi_lock(dev, read_subdev) < 0)
  {
    printf("%s: No se pudo bloquear subdispositivo de lectura: %s\n",
	      __PRETTY_FUNCTION__, comedi_strerror(comedi_errno()));
    return 1;
  }
  if(comedi_lock(dev, write_subdev) < 0)
  {
    printf("%s: No se pudo bloquear subdispositivo de escritura: %s\n",
	      __PRETTY_FUNCTION__, comedi_strerror(comedi_errno()));
    return 1;
  }
  read_maxdata= comedi_get_maxdata(dev, read_subdev, 0);
  if(read_maxdata == 0)
  {
    printf("%s: %s\n", __PRETTY_FUNCTION__,
	      comedi_strerror(comedi_errno()));
    return 1;
  }
  write_maxdata= comedi_get_maxdata(dev, read_subdev, 0);
  if(write_maxdata == 0)
  {
    printf("%s: %s\n", __PRETTY_FUNCTION__,
	      comedi_strerror(comedi_errno()));
    return 1;
  }
  read_range= comedi_get_range(dev, read_subdev, dat->rchan, dat->rrange);
  if(!read_range)
  {
    printf("%s: %s\n", __PRETTY_FUNCTION__,
	      comedi_strerror(comedi_errno()));
    return 1;
  }
  write_range= comedi_get_range(dev, write_subdev, dat->wchan, dat->wrange);
  if(!write_range)
  {
    printf("%s: %s\n", __PRETTY_FUNCTION__,
	      comedi_strerror(comedi_errno()));
    return 1;
  }

  return 0;
}