/****************************************************************************/
/* Copyright (c) 2007 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : AtsDriver.h                                                   */
/* Author   :                                                               */
/* Project  : Seafloor Mapping                                              */
/* Version  : 1.0                                                           */
/* Created  : 06/07/2007                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <math.h>
#include <unix.h>
#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/ioctl.h> // IO Control of socket.
#include "AtsDriver.h"
#include "AtsOutput.h"
#include "AtsLog.h"
#include "System.h"
#include "Syslog.h"

AtsDriver::AtsDriver(const short port)
   : Task("atsDriver"), _atsPort(port), _atsSocket(-1),
   _verbose(False), _lastLoggedDelta(0.), _baseline(0.)
{
  _output = new AtsOutput();
  _log    = new AtsLog(this, DataLog::BinaryFormat);
}


AtsDriver::~AtsDriver()
{
  delete _output;
}

int AtsDriver::init()
{
  Boolean debug = _verbose;

  // Create socket
  //
  setupSocket();
  if (_atsSocket < 0)
  {
    Syslog::write("AtsDriver: Unable to initialize interface to Ats");
    return -1;
  }

  // Initialize the baseline system time difference
  //
  if (calcTimeDiff(&_baseline) != 0)
  {
    Syslog::write("AtsDriver: Failed establishing baseline, errno = %d", errno);
  }
  Syslog::write("AtsDriver: Established baseline of %.3f seconds", _baseline);
  _output->data.baseline = _baseline;
  _output->write();
  
  return 0;
}

int AtsDriver::setupSocket()
{
  Boolean debug = _verbose;
  
  int  sockfd = _atsSocket = -1;

  // Initialize our socket to an unuseable value
  //
  if ( (sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
  {
    Syslog::write("AtsDriver: socket init failed: ret = %d, errno = %d)",
                  sockfd, errno);
    return -1;
  }


  // Attempt to setup a UDP socket on the Ats port
  // We will read time packets from the Ats
  //
  dprintf("AtsDriver: Socket params - Port:%d", _atsPort);
  
  bzero((char*) &_servaddr, sizeof(_servaddr));
  _servaddr.sin_family      = AF_INET;
  _servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
  _servaddr.sin_port        = htons(_atsPort);

  int err;
  if ((err = bind(sockfd, (struct sockaddr*)&_servaddr, sizeof(_servaddr))) < 0)
  {
    dprintf("AtsDriver: binding socket failed: ret = %d, errno = %d", err, errno);
    Syslog::write("AtsDriver: Failed to bind socket on port %d, ret=%d, errno =%d",
      _atsPort, err, errno);
    return -1;
  }

  // Socket is working
  //
  Syslog::write("AtsDriver: Socket created on fd %d", sockfd);
  _atsSocket = sockfd;  
  
  return 0;
}


int AtsDriver::calcTimeDiff(float *diff)
{
  Boolean debug = _verbose;
  
  if (_atsSocket < 0) return -1;
  
  // Read a time packet. Should be a blocking read, so Auv time server
  // determines the rate at which this driver runs and time data is
  // processed with minimal latency.
  //
  double resonTime = 0;
  int n = recv(_atsSocket, (char*)&resonTime, sizeof(resonTime), 0);
  
  // Now get our system time immediately after
  //
  struct timespec now;
  clock_gettime(CLOCK_REALTIME, &now);

  // Convert to a straight number value
  //
  double sec  = (double)now.tv_sec;
  double nsec = (double)now.tv_nsec;
  double auvTime = sec + (nsec/1.e+09);
  dprintf("AtsDriver: auvTime = %.3f , timespec: %ld, %ld, %.3f + %.3f", auvTime,
          now.tv_sec, now.tv_nsec,
                 sec,        nsec/1.e+09);
  
  // Now look at what was read from the ats connection
  //
  if (n < 0)
  {
    dprintf("AtsDriver: read nothing from Ats. Returned %d, errno = %d",
            n, errno);
    return -1;
  }
  dprintf("AtsDriver: read %d bytes to Ats: %.3f", n, resonTime);
  
  // Here's the difference
  //
  double dd = resonTime - auvTime;
  *diff = (float)dd;
  dprintf("AtsDriver: time diff = %.3f (%.3f - %.3f)", *diff, resonTime, auvTime);
  
  return 0;   // OK
}
  

void AtsDriver::run()
{
  Boolean debug = _verbose;

  Syslog::write("AtsDriver: Tracking time drift");
  while (True)
  {
    float currentDiff;
    if (calcTimeDiff(&currentDiff) != 0)
    {
      Syslog::write("AtsDriver: Error recving time data, errno = %d", errno);
      return;
    }

    // Place delta and drift in the output for the AtsServer
    //
    float drift = currentDiff - _baseline;
    dprintf("AtsDriver: drift = %f", drift);
    
    _output->data.delta = currentDiff;  
    _output->data.drift = drift;
    _output->write();

    // Log data
    logData();
  }
}

// Log the delta and the drift only when the delts has changed
// by a millisecond or more since the last logged value.
//
void AtsDriver::logData()
{
  if (fabs(_lastLoggedDelta - _output->data.delta) >= 0.001)
  {
    _lastLoggedDelta = _output->data.delta;
    _log->write();
  }
}