/*
|==============================================================================
| Copyright (C) 2009-2010 MBARI.  All Rights Reserved.
|
|==============================================================================
|
| Interface application to TRI-M HESC board installed on the Benthic Rover
|
|==============================================================================
*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <strings.h>
#include <ctype.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <fcntl.h>

#include <termios.h>
#include <unistd.h>
#include <time.h>
#include <arpa/inet.h>

#include <PvApi.h>

static char* g_port_name = "/dev/ttyAM0";
static int   g_hesc_fd = 0;
static int   g_verbose = 0;
static int   g_ret     = -1;

// Command XXXX
// 
void Sleep(unsigned int time)
{
    struct timespec t,r;
    
    t.tv_sec    = time / 1000;
    t.tv_nsec   = (time % 1000) * 1000000;    
    
    while(nanosleep(&t,&r)==-1)
        t = r;
}


void showMainUsage()
{
    printf("usage: hesc [-p <port>] command [options]");
    printf("-p\tport used to communicate with HESC (default: /dev/hesc\n");
}


// Prepare the comm port for serial comms
//
int port_setup(void)
{
  g_hesc_fd = open(g_port_name, O_RDWR|O_NOCTTY);
  if (g_hesc_fd == -1) {
    if (g_verbose) 
      printf("hesc: Could not open %s\n", g_port_name);
    return -1;
  }
  if (!isatty(g_hesc_fd)) {
    if (g_verbose) 
      printf("hesc: %s is not a serial port\n", g_port_name);
    return -1;
  }
  struct termios config;
  if (tcgetattr(g_hesc_fd, &config)) {
    if (g_verbose) 
      printf("hesc: Could not acquire termios for %s", g_port_name);
    return -1;
  }

  // Turn off input processing
  config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK |
                      INPCK  | ISTRIP | IXON);

  // Turn off output processing
  config.c_oflag &= ~(ONLRET | OCRNL | ONLCR | ONOCR |
                      OFILL  | OLCUC | OPOST);

  // Turn off line processing
  config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);

  // Turn off input processing
  config.c_cflag &= ~(CSIZE | PARENB);
  config.c_cflag |= CS8;

  config.c_cc[VMIN] = 1;
  config.c_cc[VTIME] = 0;

  if (cfsetispeed(&config, B9600) < 0 || cfsetospeed(&config, B9600) < 0) {
    if (g_verbose) 
      printf("hesc: Could not set speed for %s", g_port_name);
    return -1;
  }

  if (tcsetattr(g_hesc_fd, TCSAFLUSH, &config) < 0) {
    if (g_verbose) 
      printf("hesc: Could not set attributes for %s", g_port_name);
    return -1;
  }

  return 0;
}

// Read response from HESC. Return 0 if proper acknowledge.
//
int read_ack()
{
  // Define response buffer
  //
  char response[4];
  memset(response, 0x00, sizeof(response));

  // Read response if any
  //
  int n = read(g_hesc_fd, response, sizeof(response));
  if (g_verbose) {
    printf("hesc: %d of %d bytes read from %s\n", n, sizeof(response),
           g_port_name);
    for (int i = 0; i < n; i++)
      printf("response[%d] = %X\n", i, response[i]);
  }

  // Didn't receive the expected response
  if (n != sizeof(response)) {
    printf("hesc: size of ack response invalid\n");
    return -1;
  }

  if (response[0] != 0x00 || response[1] != 0x01 ||
      response[2] != 0x02 || response[3] != 0xFF) {
    printf("hesc: substance of response invalid\n");
    return -1;
  }

  return 0;
}

// Ask for the firmware version.
// Return the number of bytes read (non-zero) if board is there.
// Otherwise, return 0.
//
int ver(void)
{
  // Define the command buffer
  //
  char cmd[4];
  cmd[0] = 0x13;
  cmd[1] = 0x3E;
  cmd[2] = 0x02;
  cmd[3] = 0xFF;

  // Define response buffer
  //
  char response[3];
  memset(response, 0x00, sizeof(response));

  // Send the command
  //
  int n = write(g_hesc_fd, cmd, sizeof(cmd));

  if (g_verbose) 
    printf("hesc: %d bytes written to %s\n", n, g_port_name);

  // Failed to write the command.
  if (n != sizeof(cmd))
    return -1;

  // Read response if any
  //
  n = read(g_hesc_fd, response, sizeof(response));
  if (g_verbose) 
    printf("hesc: %d of %d bytes read from %s\n", n, sizeof(response),
           g_port_name);

  // Didn't receive the expected response
  if (n != sizeof(response))
    return -1;


  for (int i = 0; i < n; i++)
    printf("%X", response[i]);
  printf("\n");

  return 0;
}

// Enter eeprom RAM access mode as described by Tri-M
//
int enter_ram()
{
  int n = 0;

  //  Command buffer
  //
  char cmd[4];
  memset(cmd, 0x00, sizeof(cmd));

  // Send the command sequence
  //
  cmd[0] = 0x12;         // Write
  cmd[1] = 0x00;
  cmd[2] = 0x05;
  cmd[3] = 0x00;

  n = write(g_hesc_fd, cmd, sizeof(cmd));
  if (g_verbose) {
    printf("hesc: enter_ram - %d bytes written to %s\n", n, g_port_name);
    for (unsigned int i = 0; i < sizeof(cmd); i++)
      printf("cmd[%d] = %X\n", i, cmd[i]);
  }

  // Failed to write the command.
  if (n != sizeof(cmd))
    return -1;

  Sleep(250);

  // Command acknowledged?
  //
  if (0 != read_ack())
    return -1;

  // Global return val is good
  g_ret = 0;

  return 0;
}


// Index eeprom RAM as described by Tri-M
//
int index_ram()
{
  int n = 0;

  //  Command buffer
  //
  char cmd[4];
  memset(cmd, 0x00, sizeof(cmd));

  // Set the RAM index pointer
  //
  cmd[0] = 0x12;         // Write
  cmd[1] = 0xA0;
  cmd[2] = 0x0C;
  cmd[3] = 0x00;

  n = write(g_hesc_fd, cmd, sizeof(cmd));

  if (g_verbose) {
    printf("hesc: index_ram - %d bytes written to %s\n", n, g_port_name);
    for (unsigned int i = 0; i < sizeof(cmd); i++)
      printf("cmd[%d] = %X\n", i, cmd[i]);
  }

  // Failed to write the command.
  if (n != sizeof(cmd))
    return -1;

  Sleep(250);

  // Command acknowledged?
  //
  if (0 != read_ack())
    return -1;

  // Global return val is good
  g_ret = 0;

  return 0;
}


// Write the startup interval into eeprom
//
int set_interval(unsigned long interval)
{
  int n = 0;

  //  Command buffer
  //
  char cmd[4];
  memset(cmd, 0x00, sizeof(cmd));

  // Split time into hi and lo words
  //
  unsigned char hi = interval / 256;
  unsigned char lo = interval % 256;

  // Write the start-up interval
  //
  cmd[0] = 0x12;         // Write
  cmd[1] = 0xA1;
  cmd[2] = lo;
  cmd[3] = hi;

  n = write(g_hesc_fd, cmd, sizeof(cmd));

  if (g_verbose) {
    printf("hesc: set_interval - %d bytes written to %s\n", n, g_port_name);
    for (unsigned int i = 0; i < sizeof(cmd); i++)
      printf("cmd[%d] = %X\n", i, cmd[i]);
  }

  // Failed to write the command.
  if (n != sizeof(cmd))
    return -1;

  Sleep(250);

  // Command acknowledged?
  //
  if (0 != read_ack())
    return -1;

  // Global return val is good
  g_ret = 0;

  return 0;
}

// Exit eeprom RAM access mode
//
int exit_ram()
{
  int n = 0;

  //  Command buffer
  //
  char cmd[4];
  memset(cmd, 0x00, sizeof(cmd));

  // Reset access mode
  //
  cmd[0] = 0x12;         // Write
  cmd[1] = 0x00;
  cmd[2] = 0x01;
  cmd[3] = 0x00;

  n = write(g_hesc_fd, cmd, sizeof(cmd));

  if (g_verbose) {
    printf("hesc: reset_access - %d bytes written to %s\n", n, g_port_name);
    for (unsigned int i = 0; i < sizeof(cmd); i++)
      printf("cmd[%d] = %X\n", i, cmd[i]);
  }

  // Failed to write the command.
  if (n != sizeof(cmd))
    return -1;

  Sleep(250);

  // Command acknowledged?
  //
  if (0 != read_ack())
    return -1;

  // Global return val is good
  g_ret = 0;

  return 0;
}


// Tell the HESC board to turn on power after a given interval
//
int startup()
{
  int n = 0;

  //  Command buffer
  //
  char cmd[4];
  memset(cmd, 0x00, sizeof(cmd));

  // Tell the HESC to turn the power off
  //
  cmd[0] = 0x12;         // Write
  cmd[1] = 0x98;
  cmd[2] = 0xD3;
  cmd[3] = 0x59;

  n = write(g_hesc_fd, cmd, sizeof(cmd));

  if (g_verbose) {
    printf("hesc: shutdown/startup - %d bytes written to %s\n",
           n, g_port_name);
    for (unsigned int i = 0; i < sizeof(cmd); i++)
      printf("cmd[%d] = %X\n", i, cmd[i]);
  }

  // Failed to write the command.
  if (n != sizeof(cmd))
    return -1;

  Sleep(250);

  // Command acknowledged?
  //
  if (0 != read_ack())
    return -1;

  // Global return val is good
  g_ret = 0;

  //
  return 0;
}

// Main
//
// Parse command-line and dispatch
//
int main(int argc, char* argv[])
{
  int c;
  long time_delay = 0L;

  // Initialize global return val to bad
  g_ret = -1;

  // Get options and dispatch
  //
  while ((c = getopt (argc, argv, "vt:p:")) != -1)
  {
    switch(c)
    {
      case 't':
        time_delay = atol(optarg);
        break;

      case 'v':
        g_verbose = 1;
        break;

      case 'p':
        g_port_name = optarg;

      default:
        showMainUsage();
        return g_ret;
    }
  }

  // Prepare the HESC port
  //
  if (0 != port_setup()) {
    return g_ret;
  }

  int val = -1;
  if (0 == strcasecmp(argv[0], "ver"))
    val = ver();
  else if (0 == strcasecmp(argv[0], "enter_ram"))
    val = enter_ram();
  else if (0 == strcasecmp(argv[0], "index_ram"))
    val = index_ram();
  else if (0 == strcasecmp(argv[0], "set_interval"))
    val = set_interval(time_delay);
  else if (0 == strcasecmp(argv[0], "exit_ram"))
    val = exit_ram();
  else if (0 == strcasecmp(argv[0], "startup"))
    val = startup();

  close(g_hesc_fd);

  // Write the answer to stdout
  //
  fprintf(stdout, "%d\n", val);
  return g_ret;
}