/* example of periodic task that drives gizmo */

#include <stdio.h>
#include <unistd.h>
#include <time.h>
#include <sys/time.h>
#include <fcntl.h>
#include <string.h>
#include <sched.h>

#define DAQ_NAME "/dev/daq"

void timespec_add_ns(struct timespec *t, unsigned long long ns) {
  int tmp = ns / 1000000000;
  t->tv_sec += tmp;
  ns -= tmp * 1000000000;
  t->tv_nsec += ns;

  tmp = t->tv_nsec / 1000000000;
  t->tv_sec += tmp;
  t->tv_nsec -= tmp * 1000000000;
}

int main(const int argc, const char * argv[])
{
  int
    gizmo_fd,    /* gizmo open file descriptor */
    voltage,     /* voltage to activate solenoid */
    frame,       /* current frame number in cyclic executive */
    frame_ms,    /* number of milliseconds per frame */
    major_cycle, /* number of frames in major cycle */
    off_frame;   /* frame number to turn off solenoid */

  struct timespec timer;  /* next time to wake up */
  struct sched_param param;

  if ((gizmo_fd = open (DAQ_NAME, O_RDWR)) < 0) {
    perror ("open: " DAQ_NAME);
    return -1;
  }

  frame_ms = 3;
  if (argc > 1) {
    major_cycle = (int) strtol(argv[1], NULL, 10)/ frame_ms; 
    off_frame = (int) strtol(argv[2], NULL, 10) / frame_ms; 
  } else {
    major_cycle = 280 / frame_ms;
    off_frame = 10 / frame_ms;
  }

  /* set the priority of the task (high priority) */
  param.sched_priority = sched_get_priority_max(SCHED_FIFO);
  sched_setscheduler(0, SCHED_FIFO, &param);

  frame = 0;
  clock_gettime(CLOCK_REALTIME, &timer);
  for (;;) {
    if (frame == 0) {
      voltage = 4;
      write(gizmo_fd, &voltage, 2);
    } else if (frame == off_frame) {
      voltage = 0;
      write(gizmo_fd, &voltage, 2);
    }
    timespec_add_ns(&timer, frame_ms * 1000000);
    clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &timer, NULL);
    frame++;
    if (frame == major_cycle) frame = 0;
   }
  return 0;
}