//
// pulser.c : Raspberry pi utility that works with LabView program
//            AblationStation to serve as a pulse generator for the
//            MSC-101 EMG Lambda Physik excimer laser.
//
// author: richard.t.jones and brendan.pratt at uconn.edu
// version 1: april 30, 2015
// version 2: january 14, 2016
//
// notes for version 1.0:
// 1) Laser pulses are generated at a fixed rate, currently set to
//    fire at a rate of LASER_PULSE_RATE (Hz) defined below.
// 2) The AblationStation software issues commands to the ablation
//    ablation state motor stages to move at a given speed across the
//    sample, and then sets one of the GPIO pins (GO) to high. This is
//    taken as a signal that the pulser program should start pulsing at
//    the LASER_PULSE_RATE until the fixed number of pulses have expired.
// 3) The GO pin should go low around the time that the pulse pattern 
//    is completed but this does not shorten the pulse pattern, nor
//    prolong it in case it stays high for a long time.
// 4) After the preset pulse pattern is completed, the GO pin should
//    transition from low to high again to start the next row.
// 5) The actual sequence of fired/skipped pulses during a single row
//    is encoded in an input file, one pattern per line. Each pattern
//    is executed at least once, and possibly multiple times as determined
//    by the AblationStation algorithm.
// 6) Whenever it is time to advance to the next pattern (next line in
//    the input file), a second pin (ADVANCE) is asserted together with
//    the GO pin.
// 7) The pattern on each line in the input sequence file may be written
//    either forward or reverse order, and it is up to pulser.c algorithm 
//    to arrange them to maintain an order that is consistent with the
//    serpentine pattern of motor sweeps across the sample. The first row
//    is always left-to-right.
// 8) If for some reason the AblationStation algorithm tells pulser.c to
//    advance past the last line in the sequence file, it should fall back
//    to the empty pattern where no laser pulses are generated.
// 9) The pulser algorithm is designed not be interruptable in the middle
//    of one row pattern, but at the end of each row it pauses and must
//    see the GO pin go high again before any more pulses are generated.
// 10) The pulser program needs to know which direction the motors are
//    moving in, left-to-right or right-to-left. This information is
//    communicated from AlbationStation by setting (right-to-left) or
//    reseting (left-to-right) a third pin (DIRECTION) before the GO edge.
//
// notes for version 2.0:
// 1) Since the laser microprocessor controller is now working, we decided
//    to add additional capability to pulser.c to monitor the pulse data
//    recorded by the uP and make intelligent decisions based on this info.
//    The uP is given a target pulse energy which it tries to maintain by
//    regulating the HV level of the laser head. This is does autonomously
//    except that it can easily fault if the gain of the laser medium drops
//    below a sufficient level. When this happens, the uP switches off the
//    laser and quits responding to pulses. If we are going to run with the
//    uP regulation enabled, the pulser.c program needs to be able to detect
//    and handle this situation. The rest of the notes explain how.
// 2) Communication with the uP is over a serial RS232 bus. This is wired
//    to a usb-serial adaptor on one of the usb ports of the rpi. The rpi
//    library wiringPi (github.com) is used to manage this interface in the
//    code below.
// 3) Initialation takes place when pulser.c first starts up. It opens the
//    communication to the uP and takes over control from the front panel.
//    This means that the user should not attempt to control the laser from
//    the front panel once the laser is ready to run an ablation pass. To
//    prepare for initialization, the laser should have HV Off and Laser Off.
//    Just to be sure, press the RESET button on the front panel just before
//    starting pulser so that the software has full control.Software steps
//    for initialization are described in the code documentation below.
// 4) A full asynchronous handshake is now implemented between Labview and
//    the rpi pulser, with Labview->GO as a row-start command and rpi->DONE
//    as a row-complete response. The GO signal functions, as before, for
//    telling the rpi when to start pulsing a new row. However in the new
//    scheme, as soon as the last pulse is issued by pulser, rather than just
//    waiting for the next GO, the rpi queries the laser for the latest pulse
//    count and checks if it agrees with what it asked for on the last sweep.
//    If the two agree the the rpi raises the DONE and Labview knows it 
//    should add delta-Y and proceed with the next row, otherwise Labview
//    assumes that the present row is still incomplete, sets delta-Y = 0,
//    and re-sweeps over the same row in the opposite direction.
// 5) For this protocol to work, there needs to be a specific time window
//    within which the rpi MUST assert DONE in order for the row to be
//    considered complete. We will measure the transaction time for this
//    laser pulse query cycle to complete and add a delay to the end-of-row
//    sequence in Labview to make sure the DONE, if present, is seen by
//    Labview in time before the next row begins. As soon as the rpi sees
//    the GO for the next sweep, it should reset the DONE if asserted.
// 6) If the pulse count disagrees the the demand sequence then something
//    went wrong with the laser during the last sweep. Often this will be
//    a terminal condition that makes pulser exit with an error message,
//    leaving DONE permanently low. However, there is one situation which
//    we want to handle as a special case, which occurs whenever the uP
//    fails to regulate the pulse energy and flashes the error message,
//    "PRESET ENERGY TOO HIGH". This is not a fatal error. What we want to
//    to do is to turn the laser back on, re-enable the uP ILC, and redo
//    any unfinished pulses on the incomplete row, then continue as usual.
//    See comments in the code below for implementation details.
//
// usage:
// Two command-line arguments are required, as shown in the following example
//        $ sudo ./pulser input_file.seq 15
// where input_file.seq is the name of the input sequence file for this pass
// that has prepared by the user, and "15" specifies at which line in the 
// sequence file the pulser algorithm should start. Sequence file line 
// numbers start at 0.
//
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include <termios.h>
#include <wiringSerial.h>

#include "rtc.h"

#define LASER_PULSE_RATE 50
#define MAX_ROW_COUNT    2000
#define MAX_COLUMN_COUNT 2000
#define DEGLITCH_INTERVAL 6

// GPIO bus pin numbers
#define PULSE_OUT 4
#define GO_IN 18
#define DONE_OUT 16
#define ADVANCE_IN 23
#define REVERSE_IN 24

typedef uint32_t bitarray_t;

bitarray_t *parray;
double xlim[2][MAX_ROW_COUNT];
double ylim[2][MAX_ROW_COUNT];
int pcount[MAX_ROW_COUNT];

int max_lineno = 0;
int advance_pin = 0;
int reverse_pin = 0;

void usage()
{
  printf("Usage: pulser <inputfile.seq> <starting_row>\n");
  exit(1);
}

bitarray_t *mallocbit(int size)
{
   // Utility function to implement bit array semantics in c

   int w32 = ceil(size / 32.);
   return malloc(sizeof(uint32_t) * w32);
}

void setbit(bitarray_t *bitarray, const int index, const int value)
{
   // Utility function to implement bit array semantics in c
 
   int w32 = index / 32;
   int bit = index % 32;
   if (value)
      bitarray[w32] |= (1 << bit);
   else
      bitarray[w32] &= ~(1 << bit);
}

int testbit(const bitarray_t *bitarray, const int index)
{
   // Utility function to implement bit array semantics in c
 
   int w32 = index / 32;
   int bit = index % 32;
   return (bitarray[w32] >> bit) & 1;
}

void parse_input_file(FILE *fp)
{
  // Reads pulse sequence information from the input file
  // and stores the information in global arrays:
  //   parray[line * MAX_COLS + col] : 2d pulse on/off table, one per bit
  //   xlim[n][line] : x-limits of this input line, low (n=0) and high (n=1)
  //   ylim[n][line] : y-limits of this input line, low (n=0) and high (n=1)
  //   pcount[line]  : total number of pulses (bits) in this line

  char inbuff[1000];
  int lineno = 0;
  while (fgets(inbuff, sizeof(inbuff), fp)) {
    int rowno;
    int w, wmax;
    int reverse;
    int p, pmax;
    bitarray_t *workarray = mallocbit(MAX_COLUMN_COUNT);
    sscanf(strtok(inbuff, " "), "%d", &rowno) ;
    sscanf(strtok(0, " "), "%f", &ylim[0][lineno]);
    sscanf(strtok(0, " "), "%f", &ylim[1][lineno]);
    sscanf(strtok(0, " "), "%f", &xlim[0][lineno]);
    sscanf(strtok(0, " "), "%f", &xlim[1][lineno]);
    sscanf(strtok(0, " "), "%d", &pcount[lineno]);
    reverse = (xlim[0][lineno] < xlim[1][lineno])? 0 : 1;
    if (reverse) {
      double xx = xlim[0][lineno];
      xlim[0][lineno] = xlim[1][lineno];
      xlim[1][lineno] = xx;
    }
    pmax = pcount[lineno];
    wmax = ceil(pmax / 32.);
    for (w = 0; w < wmax; w++) {
      sscanf(strtok(0, " "), "%x", &workarray[w]);
    }
    for (p = 0; p < pmax; ++p) {
      int index = lineno * MAX_COLUMN_COUNT + p;
      if (reverse)
         setbit(parray, index, testbit(workarray, pmax - 1 - p));
      else
         setbit(parray, index, testbit(workarray, p));
    }
    ++lineno;
  }
  max_lineno = lineno;
  fclose(fp);
}

int recvmsg(int fd, char *msg, int maxlen)
{
   // Receive a response message from the EMG101 laser,
   // looking for a capital letter status code as a message
   // terminator. The return value is the number of characters
   // received including the status code letter but not the
   // string-terminating null. Two error exits values exist:
   //    -1) means it timed out waiting for a response (10s)
   //   -99) means the input buffer overflowed

   int len = 0;
   while (len < maxlen - 1) {
      int resp = serialGetchar(fd);
      if (resp == -1) {
         msg[len] = 0;
         return -1;
      }
      msg[len++] = resp;
      if (resp >= 'A' && resp <= 'Z') {
         msg[len] = 0;
         return len;
      }
   }
   return -9;
}

main(int argc, char **argv)
{
  FILE *finp;
  char finname[999];
  int startline;
  char inbuff[1000];
  int lineno;
  volatile int *MHzClock;

  // parse command line

  if (argc != 3) {
    usage();
  }
  else if (sscanf(argv[2], "%d", &startline) != 1) {
    usage();
  }
  else if ((finp = fopen(argv[1], "r")) == 0) {
    fprintf(stderr, "Error - cannot open input file %s\n", argv[0]);
    exit(1);
  }

  // load input pulse pattern into parray

  parray = mallocbit(MAX_ROW_COUNT * MAX_COLUMN_COUNT);
  if (parray == 0) {
    fprintf(stderr, "Error - failed to allocate parray, please resize.\n");
    exit(1);
  }
  parse_input_file(finp);

  // initialize real-time clock and gpio pins

  MHzClock = setup_rtc();
  gpio_config_output(PULSE_OUT);
  gpio_config_input(GO_IN, 1);
  gpio_config_output(DONE_OUT);
  gpio_config_input(ADVANCE_IN, 1);
  gpio_config_input(REVERSE_IN, 1);

  // initialize communication with the laser uP

  int serfd = serialOpen("/dev/ttyUSB0", 300);
  if (serfd < 0) {
    fprintf(stderr, "Error opening serial communcations with the laser uP.\n"
            "error code from serialOpen: %d\n", serfd);
    exit(2);
  }
  struct termios portset;
  tcgetattr(serfd, &portset);
  portset.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR |
                       PARMRK | INPCK | ISTRIP | IXON);
  portset.c_oflag = 0;
  portset.c_cflag &= ~(CSIZE | PARENB | CSTOPB);
  portset.c_cflag |= (CS8 | CRTSCTS);
  int ret = tcsetattr(serfd, TCSAFLUSH, &portset);
  if (ret != 0) {
    fprintf(stderr, "Error setting up serial communcations with the laser uP.\n"
            "response code from tcsetattr: %d\n", ret);
    exit(2);
  }
  
  // test communcation by requesting initial pulse counter

  int pulsecounter;
  serialPrintf(serfd, "C");
  char response[999];
  ret = recvmsg(serfd, response, 999);
  if (ret < 0 || sscanf(response, "%12dR", &pulsecounter) != 1) {
    fprintf(stderr, "Error setting up serial communcations with the laser uP.\n"
            "received response: %s\n", response);
    serialClose(serfd);
    exit(3);
  }

  // Initialization of the laser proceeds through the following steps
  // 1) turn on the HV, wait for 5 seconds
  // 2) attempt turn on the laser, check response
  // 3) if step 2 failed, return to step 1, otherwise proceed
  // 4) attempt to turn on ILC, check response and abort unless success
  // 5) read and report back the preset energy and pulse counter value

  int HVon = 0;
  while (HVon == 0) {
    serialPrintf(serfd, "M"); // HV on
    sleep(5);
    char response[999];
    ret = recvmsg(serfd, response, 999);
    if (ret < 0 || response[0] != 'R') {
      fprintf(stderr, "Error turning on the laser HV.\n"
           "received response: %s\n", response);
      serialClose(serfd);
      exit(4);
    }
    serialPrintf(serfd, "N"); // laser on
    ret = recvmsg(serfd, response, 999);
    if (ret < 0 || response[0] != 'R') {
      continue;
    }
    break;
  }
  sleep(2);
  serialPrintf(serfd, "W"); // ILC on
  ret = recvmsg(serfd, response, 999);
  if (ret < 0 || response[0] != 'R') {
    fprintf(stderr, "Error turning on the laser microprocessor ILC.\n"
           "received response: %s\n", response);
    serialClose(serfd);
    exit(5);
  }
  int preset_energy_mJ;
  serialPrintf(serfd, "B"); // read preset energy
  ret = recvmsg(serfd, response, 999);
  if (ret < 0 || sscanf(response, "%4dR", &preset_energy_mJ) != 1) {
    fprintf(stderr, "Error looking up the laser pulse preset energy.\n"
            "received response: %s\n", response);
    serialClose(serfd);
    exit(6);
  }
  printf("Laser is armed and ready with preset pulse energy %d mJ\n",
         preset_energy_mJ);

  // loop over all remaining lines in the input file

  int shortrow = 0;
  lineno = startline;
  while (1) {
    int reverse;
    int advance;
    int repcount;

    // polling loop for GO edge, leave room for keyboard interrupts

    repcount = 0;
    while (repcount < DEGLITCH_INTERVAL) {
      if (gpio_read_input(GO_IN))
        repcount = 0;
      else
        ++repcount;
    }

    printf("waiting for go at input line %d of %d...\n", lineno, max_lineno);
    repcount = 0;
    while (repcount < DEGLITCH_INTERVAL) {
      if (gpio_read_input(GO_IN)){
        ++repcount;
      } 
      else
        repcount = 0;
    }

    // reset DONE as we start the new sweep

    gpio_write_output(DONE_OUT, 0);

    // advance to next line if requested

    advance = 0;
    if (gpio_read_input(ADVANCE_IN)) {
      advance = 1;
      ++lineno;
    }
    if (lineno >= max_lineno) {
      break;
    }    

    // block interupts

    interrupts(0);

    // shift out the pulses at LASER_PULSE_RATE

    int demand_counter = 0;
    if (gpio_read_input(REVERSE_IN)) {
      int p;
      reverse = 1;
      for (p = pcount[lineno] -1; p >= 0; --p) {
        int value = testbit(parray, lineno * MAX_COLUMN_COUNT + p);
        int tstart = *MHzClock;
        while (*MHzClock < tstart + 100)
          gpio_write_output(PULSE_OUT, value);
        while (*MHzClock < tstart + 20000)
          gpio_write_output(PULSE_OUT, 0);
        demand_counter += value;
        if (shortrow && demand_counter >= shortrow)
          break;
      }
    }
    else {
      int p;
       reverse = 0;
      for (p = 0; p < pcount[lineno]; ++p) {
        int value = testbit(parray, lineno * MAX_COLUMN_COUNT + p);
        int tstart = *MHzClock;
        while (*MHzClock < tstart + 100)
          gpio_write_output(PULSE_OUT, value);
        while (*MHzClock < tstart + 20000)
          gpio_write_output(PULSE_OUT, 0);
        demand_counter += value;
        if (shortrow && demand_counter >= shortrow)
          break;
      }
    }

    // unblock interupts and continue

    interrupts(1);

    // ask laser for updated pulse counter and compare with expected

    int updated_counter;
    serialPrintf(serfd, "C"); // read pulse count
    ret = recvmsg(serfd, response, 999);
    if (ret < 0 || sscanf(response, "%12dR", &updated_counter) != 1) {
      fprintf(stderr, "Error fetching the laser pulse counter.\n"
              "received response: %s\n", response);
      serialClose(serfd);
      exit(7);
    }
    int saw = updated_counter - pulsecounter;
    printf("finished line %d, advance=%d, reverse=%d, "
           "pulses requested=%d, reported=%d\n",
           lineno, advance, reverse, demand_counter, saw);
    pulsecounter = updated_counter;

    // if comparison is ok, turn ILC on (again) and report DONE

    if (abs(saw - demand_counter) < 5 + demand_counter * 0.05) {
       serialPrintf(serfd, "W");
       ret = recvmsg(serfd, response, 999);
       if (ret < 0 || response[0] != 'R') {
         fprintf(stderr, "Error enabling the ILC at the end of a sweep.\n"
                 "received response: %s\n", response);
         serialClose(serfd);
         exit(8);
       }
       gpio_write_output(DONE_OUT, 1);
       shortrow = 0;
       continue;
    }
    else if (shortrow) {
      fprintf(stderr, "Laser pulse count falls short two sweeps in a row.\n"
              "Has the laser failed? User intervention required!\n");
      serialClose(serfd);
      exit(9);
    }

    // there was a problem with the previous row, try to recover
    //  1) turn the laser back on again (but not the ILC)
    //  2) set shortrow to the number of pulses missed

    serialPrintf(serfd, "N"); // laser on
    ret = recvmsg(serfd, response, 999);
    if (ret < 0 || response[0] != 'R') {
      fprintf(stderr, "Error turning the laser on after a trip.\n"
              "received response: %s\n", response);
      serialClose(serfd);
      exit(9);
    }
    shortrow = demand_counter - saw;
  }
}