/****************************************************************************/
/* Copyright (c) 2012 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : DeltaT.cc                                                */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 09/17/2012                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <unistd.h>
#include <termios.h>
#include <sys/stat.h>
#include <sys/uio.h>
#include <fcntl.h>

#include "TimeP.h"
#include "DeltaT.h"
#include "Syslog.h"

#define TO_UINT(upper,lower) ((upper)<<8 | (lower))

DeltaT::DeltaT(unsigned int port)
  : _sendCmd(True), Task(DeltaTTaskName)
{
  Syslog::write("DeltaT: constructing...\n");
  _input = new DeltaTInput();
  _output = new DeltaTOutput();

  try {
  _log = new DeltaTLog( this, DataLog::BinaryFormat );
  }
  catch ( ... ) {
    throw Exception("DeltaT::DeltaT() - DeltaTLog contructor failed\n");
  }
  _port = port;
  _sockfd = -1;

  _enabled = True;
}


DeltaT::~DeltaT()
{
  delete _input;
  delete _output;
}


DeviceIF::Status DeltaT::initialize(void)
{
  Syslog::write("DeltaT: initializing...\n");

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

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

  // Set up client info
  _idtapp_addr.sin_family      = AF_INET;
  _idtapp_addr.sin_addr.s_addr = htonl(INADDR_ANY);
  _idtapp_addr.sin_port        = htons(_port);

  // Bind client socket
  if ( bind(_sockfd, (struct sockaddr *)&_idtapp_addr, sizeof(_idtapp_addr)) < 0) {
    Syslog::write("can't bind dgram socket\n", errno);
  }

  Syslog::write("We are ready to receive datagrams from any interface on port %d...\n",
	 _port);

  setExternalControlSwitches();
  //  sendExternalControlSwitches();
  
  return DeviceIF::Ok;
} 


void DeltaT::setExternalControlSwitches()
{
  memset((void*)_xcmd_buf, 0, sizeof(_xcmd_buf));

  // Set up header bytes
  _xcmd_buf[0] = 'E'; _xcmd_buf[1] = 'C'; _xcmd_buf[2] = '0';
  // Set for external control  (0=>Local Control, 1=>ExternalControl)
  _xcmd_buf[3] = 1;
  // Xmit and receive (0=>xmit+receive, 1=>receive only)
  _xcmd_buf[4] = 0;
  // Set range (Values 2-10 => 5m,10m,20m,30m,40m,50m,60m,80m,100m)
  _xcmd_buf[7] = 10;
  // Set gain (0 - 20)
  _xcmd_buf[8] = 4;
  // Set display gain (1 - 100 %)
  _xcmd_buf[9] = 40;
  // Set gain equalization (0=>Off, 1=>On)
  _xcmd_buf[10] = 1;
  // Set Sector size (0=>30, 1=>60, 2=>90, 3=>120)
  _xcmd_buf[11] = 3;
  // Beam width (0=>Wide, 1=>Normal, 2=>Narrow, 3=>Narrow Mixed)
  _xcmd_buf[12] = 1;
  // Number of beams (0=>480, 1=>240, 2=>120)
  _xcmd_buf[13] = 1;
  // Averaging (0,1=>Off, 2..10)
  _xcmd_buf[14] = 3;
  // Persistence HI Byte
  _xcmd_buf[15] = (0&0xFF00)>>8;
  // Persistence LO Byte
  _xcmd_buf[16] = (0&0x00FF);
  // Sound Velocity*10 HI Byte
  _xcmd_buf[17] = (14920&0xFF00)>>8;
  // Sound Velocity*10 LO Byte
  _xcmd_buf[18] = (14920&0x00FF);
  // Mode (0=>Sector, 1=>Linear, 2=>Perspective, 3=>Profile, 4=>Beam test)
  _xcmd_buf[19] = 2;

}

void DeltaT::sendExternalControlSwitches()
{
  int stat;

  if ((stat = sendto(_sockfd, &_xcmd_buf[0], sizeof(_xcmd_buf), 0,
		     (struct sockaddr *)&_idtapp_addr, sizeof(_idtapp_addr))) < 0) {
    Syslog::write("External command comms failure: %d", errno);
  }
  return;
}

void DeltaT::start()
{
  _enabled = True;
  _input->data.enabled = _enabled;
  _input->write();
}

void DeltaT::stop()
{
  _enabled = False;
  _input->data.enabled = _enabled;
  _input->write();
}

void DeltaT::run()
{

  Boolean debug = True;

  int           nbytes;

  dprintf("Running DeltaT driver...\n");

  while (1)
  {
    int serveraddrsize = sizeof(_idtapp_addr);

    _input->read();
    _enabled = _input->data.enabled;
    if (!_enabled) {
      dprintf("DeltaT driver not enabled...\n");
      sleep(1);
      continue;
    }

    dprintf("Get packet from idt app...\n");
    if ((nbytes = recvfrom(_sockfd, _receive_buf, sizeof(_receive_buf), 0,
		  (struct sockaddr *)&_idtapp_addr, &serveraddrsize )) < 0) {
      Syslog::write("ERROR: recvfrom failed with error %d\n", errno);
      break;
    }

    dprintf("Process packet from idt app...\n");
    processData(nbytes);

    if (_sendCmd) {
      //      sendExternalControlSwitches();
      _sendCmd = False;
    }
  }
  return;
}

void DeltaT::processData(int nbytes)
{
  Boolean debug = True;

  dprintf("Processing %d bytes, buf[0] = %c", nbytes, _receive_buf[0]);

  if (nbytes < 500) {
    dprintf("Insufficient Data?");
    return;
  }

  if(_receive_buf[2]=='Z') {
    dprintf("No Data\n");
    return;
  }

  unsigned int nbeams = TO_UINT(_receive_buf[70], _receive_buf[71]);
  unsigned int nsamples = TO_UINT(_receive_buf[72], _receive_buf[73]);
  unsigned int resolution = TO_UINT(_receive_buf[85], _receive_buf[86]);
  unsigned int interval = TO_UINT(_receive_buf[91], _receive_buf[92]);

  _output->data.nbeams = nbeams;
  _output->data.dt_interval = interval;
  _output->data.enabled = _input->data.enabled;

  TimeIF::TimeSpec ts;
  Time::gettime(&ts);
  _output->data.update_time.seconds = ts.seconds;

  memcpy((void*)&_output->data.dt_range, (const void*)&_receive_buf[133], sizeof(float));

  unsigned char *ranges = _receive_buf+256;
  unsigned char *intens = _receive_buf+256+(2*nbeams);
  for (int i = 0; i < nbeams; i++) {
    _output->data.beam_ranges[i] = TO_UINT(ranges[i*2],ranges[i*2+1]) * resolution/1000.;
    _output->data.intensities[i] = TO_UINT(intens[i*2],intens[i*2+1]);
    if (_output->data.beam_ranges[i] > 0)
      dprintf("beam[%d]: range= %f, intensity= %d\n", i, _output->data.beam_ranges[i],
	      _output->data.intensities[i]);
  }
  dprintf("%d Beams, %d Samples/Beam, Interval: %dms, Range: %f", nbeams, nsamples,
	  interval, _output->data.dt_range);

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

  return;
}