/****************************************************************************/
/* 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"

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(0),
    _errmsg(False)
{
  _ppid = getppid();

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

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

  _msgQ   = new CathxM12Msg();
  _output = new CathxM12Output();
  _log    = new CathxM12Log(this, DataLog::BinaryFormat, "CathxM12");
  _output->write(); _log->write();
  initialize_socket();

   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;
   }


   curl_setup();
}

CathxM12::~CathxM12()
{
  if (_name) delete _name;
  if (_server_ip) delete _server_ip;
  if (_msgQ) delete _msgQ;
  if (_output) delete _output;
  if (_log) delete _log;
}

int CathxM12::initialize_socket()
{

  if (_sockfd >= 0)
     Syslog::write("CathxM12: initializing UDP socket to port %d...\n", _port);

  // Setup socket to receive the deltaT packets
  //if ( (_sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0 ) {
  if ( (_sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0 ) {
    if (_sockfd >= 0) Syslog::write("CathxM12 can't open dgram socket\n",errno);
    if (_sockfd >= 0) perror("CathxM12 can't open dgram socket");
    return (_sockfd = -1);
  }

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

  // Set up client info
  // Beam former sends data to us
  //
  inet_aton(_server_ip, &_server_addr.sin_addr);
  _server_addr.sin_family      = AF_INET;
  _server_addr.sin_port        = htons(_port);

  return _sockfd;
} 

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
   if (_sockfd > 0)
   {
     send_nav_data();
   }
   else
   {
     Syslog::write("CathxM12 - initializing socket");
     initialize_socket();
   }

   // Process messages in queue
   //
   int nmsgs = 0;
   CathxM12Msg::Message msg;
   while (_msgQ->read(&msg) > 0) {

     nmsgs++;
     switch (msg._msg) {

     case CathxM12Msg::Action:
       if (msg._capture > 0)
       {
         capture("true");
         sleep(1);
       }
       else if (msg._capture == 0)
       {
         capture("false");
         sleep(1);
       }

       if (msg._profile >= 0)
       {
         profile(msg._profile);
         sleep(1);
       }

       break;

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

  // Get camera status every 5 seconds
  if (time(NULL) % 5 == 0) status();

}

#define RAD_2_DEG 57.295779513

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");
  }
}

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

int CathxM12::capture(char *t_or_f)
{
  // We want to emulate the curl command line:
  // curl -i -H "Accept: application/json" -X PUT -d "value=true"
  // http://134.89.32.15/api/data_item_value/acq_enable/0
  //
  char json[20];
  sprintf(json, "value=%s", t_or_f);
  sprintf(_buf, URL,  _server_ip, "acq_enable");

  Syslog::write(
    "CathxM12::capture(%s)- curl -i -H \"Accept: application/json\" -X PUT -d \"%s\" %s",
     t_or_f, json, _buf);

  struct curl_slist *headers = NULL;
  headers = curl_slist_append(headers, "Accept: application/json");

  // Used the advice here:
  // http://stackoverflow.com/questions/7569826/send-string-in-put-request-with-libcurl
  //
  curl_setup();
  curl_easy_setopt(_curl, CURLOPT_HEADER, 1L); 
  curl_easy_setopt(_curl, CURLOPT_HTTPHEADER, headers); 
  curl_easy_setopt(_curl, CURLOPT_CUSTOMREQUEST, "PUT"); /* !!! */
  curl_easy_setopt(_curl, CURLOPT_POSTFIELDS, json); /* data goes here */
  curl_easy_setopt(_curl, CURLOPT_URL, _buf);

  int c = curl_execute();
  //Syslog::write("CathxM12::capture(%s) errors: %s", t_or_f, _errorbuf);

  curl_slist_free_all(headers);
  curl_cleanup();

  Boolean debug = False;
  dprintf("CathxM12::profile(%s) response: %s\n", t_or_f, _response);

  _output->data.req_capture = strcmp(t_or_f, "true")? 0 : 1;
  status();

  return c;
}


int CathxM12::profile(int profile_number)
{
  // We want to emulate the curl command line:
  // curl -i -H "Accept: application/json" -X PUT -d "value=N"
  // http://134.89.32.15/api/data_item_value/camera_active_profile/0
  //
  char json[20];
  sprintf(json, "value=%d", profile_number);
  sprintf(_buf, URL,  _server_ip, "camera_active_profile");

  Syslog::write(
    "CathxM12::profile(%d)- curl -i -H \"Accept: application/json\" -X PUT -d \"%s\" %s",
     profile_number, json, _buf);

  struct curl_slist *headers = NULL;
  headers = curl_slist_append(headers, "Accept: application/json");

  // Used the advice here:
  // http://stackoverflow.com/questions/7569826/send-string-in-put-request-with-libcurl
  //
  curl_setup();
  curl_easy_setopt(_curl, CURLOPT_HEADER, 1L); 
  curl_easy_setopt(_curl, CURLOPT_HTTPHEADER, headers); 
  curl_easy_setopt(_curl, CURLOPT_CUSTOMREQUEST, "PUT"); /* !!! */
  curl_easy_setopt(_curl, CURLOPT_POSTFIELDS, json); /* data goes here */
  curl_easy_setopt(_curl, CURLOPT_URL, _buf);

  int c = curl_execute();
  //Syslog::write("CathxM12::profile(%d) errors: %s", profile_number, _errorbuf);

  curl_slist_free_all(headers);
  curl_cleanup();

  Boolean debug = False;
  dprintf("CathxM12::profile(%d) response: %s\n",profile_number,_response);

  _output->data.req_profile = profile_number;
  status();

  return c;
}

int CathxM12::status()
{
  // We want to emulate the curl command line:
  // curl http://134.89.32.15/api/data_item_value/camera_status/0/value.json
  //
  sprintf(_buf, "http://%s/api/data_item_value/camera_status/0/value.json",
    _server_ip);

  curl_setup();
  curl_easy_setopt(_curl, CURLOPT_URL, _buf);

  int c = curl_execute();
  curl_cleanup();

  Boolean debug = False;
  dprintf("CathxM12::status() response: %s\n", _response);

  _output->data.act_capture = 0;
  if (_response_size > 0)
  {
    _output->data.act_capture = strstr(_response, "Acquiring")? 1 : 0;
  }

  _output->write();
  _log->write();

  return c;
}

size_t CathxM12::read_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_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;   
}