/****************************************************************************/
/* Copyright (c) 2014 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : Interface to the Cathx M12 camera.                            */
/* Filename : CathxM12.h                                                    */
/* Author   : Henthorn                                                      */
/* Project  : Iceberg AUV                                                   */
/* Version  : 1.0                                                           */
/* Created  : 05/13/2015                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/

#include <time.h>
#include "Syslog.h"
#include "CathxM12.h"
#include "CathxM12Msg.h"
#include "CathxM12Log.h"
#include "CathxM12Output.h"

#define RAD_2_DEG 57.295779513
#define URL  "http://%s/api/data_item_value/%s/0"


static pid_t _ppid;

static char   CathxM12::_response[2000];
static size_t CathxM12::_response_size;

// interface for use with the Cathx M12 Camera.
//
CathxM12::CathxM12(const char* name, const char* server_ip,
                   unsigned long port, int period)
  : PeriodicTask(name), _port(port), _period(period), _sockfd(-1),
    _tcpfd(-1), _errmsg(False), _profile(-1), _capture_state(UNDEFINED),
    _total(0), _session(0), _available_space(0), _unresponsive(0)
{
  _ppid = getppid();

   _name = strdup(name);
   _server_ip = strdup(server_ip);

   addPeriodicCallback(_period, (CallbackMethod)CathxM12::m12_callback);

  _msgQ   = new CathxM12Msg();
  _log    = new CathxM12Log(this, DataLog::BinaryFormat, "CathxM12");

   if (0 != initialize_sockets())
      exit(1);

   Syslog::write("CathxM12 - Opening NavigationIF..."); fflush(stdout);
   try {
      _navif = new NavigationIF("NavigationServerCathx", 10);
   }
   catch (Exception e) {
      fprintf(stderr, "CathxM12 Caught exception: %s\n", e.msg);
      Syslog::write("CathxM12 - Navigation server not found");
      _navif = 0;
   }
   catch (...) {
      fprintf(stderr, "CathxM12 Caught some exception...\n");
      Syslog::write("CathxM12 - Navigation server not found");
      _navif = 0;
   }

   // Trying it without curl
   //curl_setup();
  _log->write();
}

CathxM12::~CathxM12()
{
  if (_name) delete _name;
  if (_server_ip) delete _server_ip;
  if (_msgQ) delete _msgQ;
  if (_log) delete _log;
  close(_sockfd);
  close(_tcpfd);
}

// Returns zero if all went well
//
int CathxM12::initialize_sockets()
{
  Syslog::write("CathxM12: initializing UDP socket to port %d...\n", _port);

  // Setup UDP socket to send Navigation update packets
  if ( (_sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0 ) {
    Syslog::write("CathxM12 can't open dgram socket\n",errno);
    perror("CathxM12 can't open dgram socket");
    return -1;
  }

  memset((void*)&_server_addr, 0, sizeof(_server_addr));

  // Set up client info
  //
  inet_aton(_server_ip, &_server_addr.sin_addr);
  _server_addr.sin_family      = AF_INET;
  _server_addr.sin_port        = htons(_port);


  int tcp_port = 8861;

  Syslog::write("CathxM12: initializing TCP socket on port %d...\n", tcp_port);

  // Setup TCP socket to send commands
  if ( (_tcpfd = socket(AF_INET, SOCK_STREAM, 0)) < 0 ) {
    Syslog::write("CathxM12 can't open stream socket\n",errno);
    perror("CathxM12 can't open stream socket");
    return -1;
  }

  memset((void*)&_tcp_addr, 0, sizeof(_tcp_addr));

  // Set up client info
  //
  inet_aton(_server_ip, &_tcp_addr.sin_addr);
  _tcp_addr.sin_family      = AF_INET;
  _tcp_addr.sin_port        = htons(tcp_port);

  if (0>connect(_tcpfd, (struct sockaddr const*)&_tcp_addr, sizeof(_tcp_addr)))
  {
    perror("CathxM12 can't connect stream socket");
    return -1;
  }


  // For socket option SO_RCVTIMEO
  struct timeval tv;
  tv.tv_sec =  1;
  tv.tv_usec = 500000L;

  if (0 > setsockopt(_tcpfd, SOL_SOCKET, SO_RCVTIMEO,
           (const void **)&tv, sizeof(struct timeval))) {
    perror("CathxM12::initialize_sockets() setsockopt failed");
    return -1;
  }

  Syslog::write("CathxM12 sockets open and ready");

  profile(-1);    // Get the current camera profile
  return 0;
} 

void CathxM12::m12_callback()
{
   Boolean debug = False;

   // Exit if parent dies
   //
   pid_t ppid = getppid();
   if (ppid != _ppid)
   {
     Syslog::write("CathxM12 - Parent stopped, so I am too");
     exit(0);
   }

   // Send camera nav position
   send_nav_data();

   // Process messages in queue
   //
   int nmsgs = 0;
   int pause_time = 1; // one second pause after a message to the Cathx
   CathxM12Msg::Message msg;
   while (_msgQ->read(&msg) > 0) {

     nmsgs++;
     pause_time = 1;
     switch (msg._msg) {

     case CathxM12Msg::Action:
       if (msg._capture >= 0)
       {
         char *cap = msg._capture == 0? "false" : "true";
         capture(cap);
         while (pause_time = sleep(pause_time));
       }

       if (msg._profile >= 0)
       {
         int c = _capture_state;

         capture("false");
         while (pause_time = sleep(pause_time));

         profile(msg._profile);
         while (pause_time = sleep(pause_time));

         // Resume imagecapture if required
         if (c)
         {
            capture("true");
            while (pause_time = sleep(pause_time));
         }
       }

       break;

       default:
         nmsgs--;
         Syslog::write("CathxM12 - Invalid message: %d", msg._msg);
         break;
     }
  }

  // Get camera status
  status();
  _log->write();
}


void CathxM12::send_nav_data()
{
  Boolean debug = False;

  if (!_navif)
  {
     return;
  }

  //
  // Send NAV data for camera to include in images
  //
  // Gather data from Navigation
  // Assemble into NAV packet for Cathx
  // Push it
  //
  char navdata[150];
  char *nav_fmt = "$PNAVALL,"                       // preamble
                  "%4d%02d%02dT%02d%02d%02d.%03dZ," // ISO 8601 time
                  "A,"                              // valid data
                  "%02.7f,"                         // lat,degrees,zero-padded
                  "%03.7f,"                         // long,deg,zero-padded
                  "%.2f,"                           // depth,meters,not padded
                  "%.2f,"                           // alt,meters,not padded
                  "%.3f,"                           // surge,m/s,not padded
                  "%.3f,"                           // sway,m/s,not padded
                  "%.3f,"                           // heave,m/s,not padded
                  "%03.2f,"                         // roll,deg,padded
                  "%03.2f,"                         // pitch,deg,padded
                  "%03.2f,"                         // yaw,deg,padded
                  "%03.2f,"                         // roll-rate,deg/s,padded
                  "%03.2f,"                         // pitch-rate,deg/s,padded
                  "%03.2f,"                         // yaw-rate,deg/s,padded
                  "*";                              // end of nav data

  unsigned int year, month, day, hour, min, sec, millis;
  float lat, lon, depth, alt, surge, sway, heave;
  float roll, pitch, yaw, roll_r, pitch_r, yaw_r;

  // Get the latest navigation state
  //
  NavigationIF::Position pos;
  NavigationIF::Attitude att;
  _navif->state(&pos, &att);

  // Fill time data with position update time
  //
  struct tm *gmt;
  gmt = gmtime(&pos.updateTime.seconds);

  year = 1900 + gmt->tm_year;
  month = 1 + gmt->tm_mon;
  day = gmt->tm_mday;
  hour = gmt->tm_hour;
  min = gmt->tm_min;
  sec = gmt->tm_sec;
  millis = (int)(pos.updateTime.nanoSeconds / 1000000L);

  // Fill position and attitude data
  //
  lat = pos.latitude*RAD_2_DEG;
  lon = pos.longitude*RAD_2_DEG;
  depth = pos.z;
  alt = pos.altitude;
  surge = sway = heave = 0.;
  roll = att.roll*RAD_2_DEG;
  pitch = att.pitch*RAD_2_DEG;
  yaw = att.yaw*RAD_2_DEG;
  roll_r = att.omega_B_x*RAD_2_DEG;
  pitch_r = att.omega_B_y*RAD_2_DEG;
  yaw_r = att.omega_B_z*RAD_2_DEG;

  sprintf(navdata, nav_fmt,
          year, month, day, hour, min, sec, millis,
          lat, lon, depth, alt, surge, sway, heave,
          roll, pitch, yaw, roll_r, pitch_r, yaw_r);

  // Calculate checksum.
  // Do not include the initial '$' or the final '*'
  char checksum;
  int len, star;
  for (checksum = 0, len = 1, star = strlen(navdata) - 1; len < star; len++)
    checksum ^= navdata[len];

  // variable len is now indexed to '*', so place checksum
  // after that followed by cr and lf
  //
  navdata[++len] = checksum;
  navdata[++len] = 0x0d;
  navdata[++len] = 0x0a;
  navdata[++len] = '\0';
  
  dprintf("CathxM12::send_nav_data - %s", navdata);

  if (sendto(_sockfd, navdata, len, 0, (struct sockaddr*)&_server_addr,
      sizeof(_server_addr)) < 0) {
    Syslog::write("CathxM12: failed to send nav data to %s: %d",
                   _server_ip, errno);
    perror("CathxM12: failed to send nav data to Cathx");
  }
}


// Central place to handel sending commands to and parsing response from
// the CathxM12 camera server. Returns < 0 on error, 0 when the response
// timed-out, otherwise the length of the string response.
//
int CathxM12::cmd_and_response(const char* cmd)
{
  Boolean debug = True;

  dprintf("CathxM12::cmd_and_response() sending %s ", cmd);

  int sent, response_size = 0;
  if ((sent = send(_tcpfd, cmd, strlen(cmd), 0)) < 0)
  {
    perror("CathxM12::cmd_and_response() - TCP comms failed");
    return -1;
  }
  dprintf("\t\tSent %d bytes to Cathx\n", sent);


  // Try a few times to get the response
  //
  for (int j = 0; response_size <= 0 && j < 6; j++)
  {
    response_size = recv(_tcpfd, _buf, BUF_SIZE, 0);
    dprintf("\t\tReceived %d bytes from Cathx\n", response_size);
  }

  if (response_size <= 0)
  {
    perror("CathxM12::cmd_and_response() - no response or timed-out");
    _unresponsive = 1;
    return response_size;
  }
  else
    _unresponsive = 0;


  // We're gonna use sscanf to extract the interested values,
  // so lets strip the buffer of messy characters while we copy
  // it to make parsing more straightforward.
  //
  char mybuf[BUF_SIZE+1];
  for (int i = 0; i < response_size+1; i++)
  {
     switch (_buf[i])
     {
        // replace with a space if one of these
        case '!':
        case '{':
        case '}':
        case ':':
        case ',':
        case '"':
        case '\r':
        case '\n':
          mybuf[i] = ' ';
          break;
        
        // Otherwise copy
        default:
          mybuf[i] = _buf[i];
     }
  }
  _buf[response_size+1] = '\0';
  dprintf("CathxM12::parse_status() modified string: %s", mybuf);

  // Parse response in _buf. May contain responses from more than one command
  //
  char *response;
  char value[100];
  if (response = strstr(mybuf, "camera_status"))
  {
    // Extract camera status values - too many to process here
    parse_status(response);
  }

  if (response = strstr(mybuf, "camera_active_profile"))
  {
    sscanf(response, "%*s %d", &_profile);
    dprintf("\t\tparsed profile: %d", _profile);
  }

  if (response = strstr(mybuf, "acq_enable"))
  {
    sscanf(response, "%*s %s", value);
  }

  return response_size;

}

int CathxM12::capture(char *t_or_f)
{
  // Test with TCP comms
  //
  sprintf(_buf, "%s %s", "acq_enable", t_or_f);
  return cmd_and_response(_buf);

}


int CathxM12::profile(int profile_number)
{
   if (profile_number < 0)
   {
      cmd_and_response("camera_active_profile");
   }
   else
   {
      sprintf(_buf, "%s %d", "camera_active_profile", profile_number);
      cmd_and_response(_buf);

      if (profile_number != _profile)
         Syslog::write("CathxM12::profile() - new profile not accepted? Could be latency.");
   }
   return 0;

}


int CathxM12::status()
{
  // Try with TCP comms
  //
  return cmd_and_response("camera_status");

}

// Parse the Cathx Status message.
//
int CathxM12::parse_status(const char* status_msg)
{
   Boolean debug = False;

   // Nominal status message form:
   // `!camera_status: {"state" : "Acquiring", "count" : "1",
   //  "wtd" : "1450222605", "let" : "1450222600", "lst" : "1450222605",
   //  "image_saving" : {"state" : "enabled", "mode" : "internal", 
   //  "location" : "/media/sda/images/dir", "total" : "1", "session" : "1",
   //  "failed" : "0", "free" : "421873"}}
   //
   // The location value is sometimes empty, skip to total

   if (!strstr(status_msg, "camera_status "))
      return -1;

   // Lets scanf the values out of the buffer
   // Most of the stuff is fluff.
   // String size limits in the 3rd and 14th element must match
   // STATE_SIZE (20) and IMGSV_SIZE (10) defined in the header file.
   // 
   int scanned = sscanf(status_msg,
      " %*s %*s %20s %*s %*s %*s %*s %*s %*s %*s %*s %*s %*s %10s",
      _state, _img_saving);

   dprintf("CathxM12::parse_status() extracted %s, %s", _state, _img_saving);


   // Jump ahead to "total" because the items in between vary in number
   //
   char *total, *session, *space;
   if (total = strstr(status_msg, "total"))
   {
      scanned += sscanf(total, "%*s %d %*s %d %*s %*s %*s %ld",
         &_total, &_session, &_available_space);

      dprintf("CathxM12::parse_status() extracted %d, %d, %ld",
         _total, _session, _available_space);

   }

   // Interpret the string data for logging
   //
   if (!strncmp("enabled", _img_saving, strlen("enabled")))
      _saving = 1;
   else
      _saving = 0;

   if (!strncmp("Idle", _state, strlen("Idle")))
      _capture_state = IDLE;
   else if (!strncmp("Acqui", _state, strlen("Acqui")))
      _capture_state = ACQUIRE;
   else
      _capture_state = OTHER;

   return scanned;
}


size_t CathxM12::read_curl_response(void *buffer, size_t size, size_t nmemb, void *userp)
{
   Boolean debug = False;

   dprintf("**** curl buffer len = %d, buffer = %s\n", size*nmemb, (char*)buffer);

   memcpy((void*)_response, (void*)buffer, size*nmemb);
   _response_size = size;

   return size*nmemb;
}

// This function should be called before setting the URL or post options
// 
int CathxM12::curl_setup()
{
    curl_cleanup();
   _curl = curl_easy_init();
   if (_curl) {
      curl_easy_setopt(_curl, CURLOPT_WRITEFUNCTION, read_curl_response);
      /* complete within TIMEOUT seconds */
      curl_easy_setopt(_curl, CURLOPT_TIMEOUT, 2L);
      curl_easy_setopt(_curl, CURLOPT_NOSIGNAL, 1L);
      curl_easy_setopt(_curl, CURLOPT_ERRORBUFFER, _errorbuf);
      return 0;
    }
    else
    {
       Syslog::write("%s CathxM12::curl_setup() - CURL initialization failed %d", _name);
       return -1;
    }
}

int CathxM12::curl_execute()
{
   Boolean debug = True;

   _response[0] = '\0';
   int c = curl_easy_perform(_curl);
   if (c != CURLE_OK)
      dprintf("CathxM12::curl_execute() curl response - %d => %s\n",
         c, _errorbuf);

   return c;
}

int CathxM12::curl_cleanup()
{
   if (_curl)
   {
      curl_easy_cleanup(_curl);
      _curl = NULL;
      return 0;
   }
   return 1;   
}