/****************************************************************************/
/* 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 <sys/select.h>
#include <unistd.h>
#include <termios.h>
#include <sys/stat.h>
#include <sys/uio.h>
#include <fcntl.h>
#include <errno.h>

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

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

//////////////////////////////////////////////////////////////////////////////
// Ctor
DeltaT::DeltaT(unsigned int port)
  : _xmit(False), Task(DeltaTTaskName)
{
  Syslog::write("DeltaT: constructing...\n");
  _input = new DeltaTInput();
  _output = new DeltaTOutput();
  _sos = 14985;

  _msgQ = new DeltaTMessage(MessageQueue::Read, DeltaTMessageQueueName);

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

  setExternalControlSwitches();
  start();
}


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

//////////////////////////////////////////////////////////////////////////////
// Initialize comms to DeltaT beam former
DeviceIF::Status DeltaT::initialize(void)
{
  Syslog::write("DeltaT: initializing...\n");

  // Setup socket to receive the deltaT packets
  if (1) {
    if ( (_sockfd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0 ) {
      Syslog::write("can't open delta-t app socket\n", errno);
    }

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

    // Set up client info
    // Beam former sends data to us
    //
    _server_addr.sin_family      = AF_INET;
    _server_addr.sin_addr.s_addr = inet_addr("134.89.32.227");
    _server_addr.sin_port        = htons(_port);

    if (connect(_sockfd, (struct sockaddr *)&_server_addr, sizeof(_server_addr))
	< 0) {
      Syslog::write("can't connect to delta-t app socket: %d", errno);
      exit(1);
    }

    Syslog::write("DeltaT: connected to beamformer...\n");
    // Got comms, so send an enable and go
    setExternalControlSwitches();
    sendExternalControlSwitches();

  } else {

    if ( (_sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0 ) {
      Syslog::write("can't open dgram socket\n", errno);
    }

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

    // Set up client info
    // Beam former sends data to us
    //
    _client_addr.sin_family      = AF_INET;
    _client_addr.sin_addr.s_addr = htonl(INADDR_ANY);
    _client_addr.sin_port        = htons(_port);

    // For socket option SO_RCVTIMEO
    _server_tv.tv_sec = 1;
    _server_tv.tv_usec = 0L;

    // Bind client socket
    if ( bind(_sockfd, (struct sockaddr *)&_client_addr, sizeof(_client_addr)) < 0) {
      Syslog::write("can't bind dgram socket: %d", errno);
      exit(1);
    }
    else {
      if (0 > setsockopt(_sockfd, SOL_SOCKET, SO_RCVTIMEO,
			 (const void **)&_server_tv, sizeof(struct timeval))) {
	Syslog::write("DeltaT::initialize() setsockopt failed: %d", errno);
	exit(1);
      }
      unsigned int minb = 32;
      if (0 > setsockopt(_sockfd, SOL_SOCKET, SO_RCVLOWAT,
			 (const void *)&minb, sizeof(unsigned int))) {
	Syslog::write("DeltaT::initialize() setsockopt failed: %d", errno);
	exit(1);
      }
    }
  }

  // Used to index into the receive buffer when more/less than a
  // single, complete DeltaT message is received.
  // In nominal case, value will always = 0.
  //
  _offset = 0;

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

  
  return DeviceIF::Ok;
} 

#define DT_83P  1
#define DT_83B  2
#define DT_83Z  3

//////////////////////////////////////////////////////////////////////////////
// Continuously listen for data packets from the beam former. Process data
// when packets are received. Also handle any messages from the DeltaT
// server process.
//
void DeltaT::run()
{

  Boolean debug = True;

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

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

    int nbytes = 0;
    dprintf("See if IDT packet available, read and process...");
    if ((nbytes = recvfrom(_sockfd, _receive_buf+ _offset,
			   sizeof(_receive_buf) - _offset,
			   0, (struct sockaddr *)&_server_addr,
			   &serveraddrsize )) < 0) {
      if (errno == EAGAIN)
	Syslog::write("recvfrom timeout");
      else
	Syslog::write("ERROR: recvfrom failed with error %d\n", errno);

      nbytes = 0;
    }

    int msg_type = 0;
    nbytes += _offset;
    if (nbytes > 0) msg_type = processPacket(nbytes);

    // Handle messages from the server process
    //
    // According to Jeff Patterson at Imagenex, the beam forming
    // application is designed so that the optimum time to use
    // external control messages is immediately after the
    // app transmits beam data packets.
    //
    //if (messageHandler() > 0) sendExternalControlSwitches();

    if (DT_83P == msg_type) process83PData();

    //sleep(1);
    // Ready for new data
    sendExternalControlSwitches();

  }

  return;
}

int DeltaT::processPacket(int nbytes)
{
  int msg_type = 0;
  Boolean debug = False;
  int serveraddrsize = sizeof(_server_addr);

  dprintf("DeltaT::processData- %d bytes in packet: buf[0:2]=>%c %c %c",
	    nbytes, _receive_buf[0], _receive_buf[1], _receive_buf[2]);

  unsigned int expected_length = 0;
  if (0 == strncmp((char const*)_receive_buf, "83P", 3)) {
    if (nbytes >= 256) {
      // Examine the rest of the 256-byte header
    
      unsigned int nbeams = TO_UINT(_receive_buf[70], _receive_buf[71]);
      dprintf("DeltaT::processData - got header? %d bytes, %d beams", nbytes, nbeams);
      unsigned int intensities = _receive_buf[117];
      expected_length = 256 + 2*nbeams + 2*nbeams*intensities;
      dprintf("DeltaT::processData - packet size should = %d", expected_length);

      // Is this a complete and valid 83P message?
      if (nbytes >= expected_length) msg_type = DT_83P;
    }
  }
  else if (0 == strncmp((char const*)_receive_buf, "83B", 3)) {
    expected_length = 256 + TO_UINT(_receive_buf[70], _receive_buf[71]);
    msg_type = DT_83B;
  }
  else if (0 == strncmp((char const*)_receive_buf, "83Z", 3)) {
    expected_length = 32;
    msg_type = DT_83Z;
  }

  if (nbytes = expected_length) {
    _offset = 0;                      // Clean, nominal case
  }
  else if (nbytes < expected_length) {
    Syslog::write("DeltaT::processPacket() - recvd %d bytes, needed at least %d",
		  nbytes, expected_length);
		  
    _offset = nbytes;   // Need more data, do another recvfrom
  }
  else if (nbytes > expected_length) {
    Syslog::write("DeltaT::processPacket() - recvd %d bytes, expected only %d",
		  nbytes, expected_length);
    // Move the "next" message to the head of the buffer
    _offset = nbytes - expected_length;
    memcpy(_receive_buf, _receive_buf+expected_length, _offset);
  }

  
  return msg_type;
}

//////////////////////////////////////////////////////////////////////////////
// Process 83P data packets from the beam former
//
void DeltaT::process83PData()
{
  Boolean debug = True;

  // Process 83P data packet
  //
  unsigned int nsamples = TO_UINT(_receive_buf[72], _receive_buf[73]);
  unsigned int resolution = TO_UINT(_receive_buf[85], _receive_buf[86]);

  unsigned int nbeams = TO_UINT(_receive_buf[70], _receive_buf[71]);
  _output->data.nbeams = nbeams;

  unsigned int interval = TO_UINT(_receive_buf[91], _receive_buf[92]);
  _output->data.dt_interval = interval;

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

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

  // Individual beam ranges
  //
  dprintf("process83P() - extract beam ranges");
  unsigned char *ranges = _receive_buf+256;
  unsigned char *intens = _receive_buf+256+(2*nbeams);
  int i;
  for (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", 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();
  if (_enabled)
    _log->write();
  else
    dprintf("process83P() - MVC logging not enabled");

  return;
}

////////////////////////////////////////////////////////////////////////////
// Handle message from DeltaTServer
//
int DeltaT::messageHandler()
{
  int nmsgs = 0;
  long sos;

  // Extract messages from the server and handle one by one
  //
  DeltaTMessage::Message msg;
  while (_msgQ->read(&msg) > 0) {

    nmsgs++;
    _xmit = True;
    switch (msg._msg) {

    case DeltaTMessage::StartMVCLogging:
      start();
      break;

    case DeltaTMessage::StopMVCLogging:
      stop();
      break;

    case DeltaTMessage::SetSOS:
      sos = msg._float * 10.;
      set_sos(sos);
      break;

    case DeltaTMessage::SetRange:
      set_range(msg._int);
      break;

    case DeltaTMessage::SetGain:
      set_gain(msg._int);
      break;

    case DeltaTMessage::SetNBeams:
      set_nbeams(msg._int);
      break;

    case DeltaTMessage::SetSectorSize:
      set_sector_size(msg._int);
      break;

    case DeltaTMessage::SetBeamWidth:
      set_beam_width(msg._int);
      break;

    case DeltaTMessage::SetAveraging:
      set_averaging(msg._int);
      break;

    case DeltaTMessage::SetGainEq:
      set_gain_eq(msg._int);
      break;

    case DeltaTMessage::SetLogging:
      set_idt_logging(msg._int);
      break;

    default:
      nmsgs--;
      Syslog::write("DeltaT::messageHandler() - Invalid DeltaTMessage: %d", msg._msg);
      break;
    }
  }

  return nmsgs;
}


void DeltaT::start()
{
  Syslog::write("DeltaT - start mvc logging");
  set_mvc_logging(True);
}

void DeltaT::stop()
{
  Syslog::write("DeltaT - stop mvc logging");
  set_mvc_logging(False);
}

void DeltaT::set_mvc_logging(int on_off_switch)
{
  _enabled = (on_off_switch != 0);
  _output->data.enabled = _enabled;
  _output->write();
  Syslog::write("DeltaT - set MVC logging to %d", on_off_switch);
}

void DeltaT::set_idt_logging(int on_off_switch)
{
  if (on_off_switch != 0)
    _send_buf[28] = 1;		//837 data logging, 0=Off, 1=On
  else
    _send_buf[28] = 0;

  Syslog::write("DeltaT - set 837 logging to %d", on_off_switch);
}

void DeltaT::set_range(long range)
{
  Syslog::write("DeltaT - set range to %d", range);
  //must be in units of Meters
  //0 = n/a
  //1 = n/a
  //2 = 5M
  //3 = 10M
  //4 = 20M
  //5 = 30M
  //6 = 40M
  //7 = 50M
  //8 = 60M
  //9 = 80M
  //10 = 100M

  if (range <= 5) {
    _send_buf[7] = 2;		//Range
  }
  else if (range <= 10) {
    _send_buf[7] = 3;
  }
  else if (range <= 20) {
    _send_buf[7] = 4;
  }
  else if (range <= 30) {
    _send_buf[7] = 5;
  }
  else if (range <= 40) {
    _send_buf[7] = 6;
  }
  else if (range <= 50) {
    _send_buf[7] = 7;
  }
  else if (range <= 60) {
    _send_buf[7] = 8;
  }
  else if (range <= 80) {
    _send_buf[7] = 9;
  }
  else if (range <= 100) {
    _send_buf[7] = 10;
  }
}

void DeltaT::set_averaging(long avg)
{
  Syslog::write("DeltaT - setting averaging to %d", avg);
  if (avg == 0 || avg == 1  //Averaging, 0,1=Off, 3, 5, 7 = #shots to average
      || avg == 3 || avg == 5 || avg == 7)
    _send_buf[14] = avg;
  else {
    Syslog::write("DeltaT - Error! invalid setting for averaging: %d", avg);
    return;
  }
}

void DeltaT::set_beam_width(long width)
{
  Syslog::write("DeltaT - setting beam width to %d", width);
  if (width >= 0 && width <=3)
    _send_buf[12] = width; //Beamwidth, 0=Wide, 1=Normal, 2=Narrow, 3=Narrow Mixed
  else {
    Syslog::write("DeltaT - Error! invalid setting for beam width: %d", width);
    return;
  }
}

void DeltaT::set_sector_size(long size)
{
  Syslog::write("DeltaT - setting sector size to %d", size);
  if (size == 120)
    _send_buf[11] = 3; //Sector Size, 0=30, 1=60, 2=90, 3=120deg
  else if (size == 90)
    _send_buf[11] = 2;
  else if (size == 60)
    _send_buf[11] = 1;
  else if (size == 30)
    _send_buf[11] = 0;
  else {
    Syslog::write("DeltaT - Error! invalid setting for sector size: %d", size);
    return;
  }
}

void DeltaT::set_nbeams(long nb)
{
  Syslog::write("DeltaT - setting nbeams to %d", nb);
  if (nb == 120)
    _send_buf[13] = 2;		//Number of Beams, 0=480, 1=240, 2=120
  else if (nb == 240)
    _send_buf[13] = 1;
  else if (nb == 480)
    _send_buf[13] = 0;
  else {
    Syslog::write("DeltaT - Error! invalid setting for nbeams: %d", nb);
    return;
  }
}

void DeltaT::set_sos(long sos)
{
  if (sos >= 14000 && sos <= 16000) {
    _sos = sos;
    _send_buf[17] = (sos&0xFF00)>>8;;	//Sound Velocity*10 (Hi Byte), 1400.0 to 1600.0m/s
    _send_buf[18] = (sos&0x00FF);	//Sound Velocity*10 (Lo Byte)
  }
}

void DeltaT::set_gain(long gain)
{
  Syslog::write("DeltaT - set gain to %d", gain);
  if (gain > 0 && gain < 21) {
    _send_buf[8] = gain;		//Gain, 0 to 20dB
  }
}

void DeltaT::set_gain_eq(int on_off_switch)
{
  if (on_off_switch != 0)
    _send_buf[10] = 1;		//Gain Equalization, 0=Off, 1=On
  else
    _send_buf[10] = 0;
  Syslog::write("DeltaT - set gain-eq to %d", on_off_switch);
}

///////////////////////////////////////////////////////////////////////////
// Manage the external control switch packet.
// Settings are persisted because all the settings must be sent in a
// control packet. Code originated from Imagenex sample file.
//
void DeltaT::setExternalControlSwitches()
{
  for(int i=0;i<256;i++) _send_buf[i] = 0;

  _send_buf[0] = 'E';
  _send_buf[1] = 'C';
  _send_buf[2] = 0;    //ID

  _send_buf[3] = 1;   //Control Byte 1
  //Bit0: 0 = LocalControl, 1 = ExternalControl
  //Bit0 must be set to ExternalControl for Switch
  //settings to take effect

  _send_buf[4] = 0;		//Control Byte 2
  //Bit0: 0 = Transmit & Receive, 1 = Receive Only (disable transmitter)

  _send_buf[5] = 0;		//Control Byte 3
  _send_buf[6] = 0;		//Control Byte 4

  _send_buf[7] = 10;		//Range
  //must be in units of Meters
  //0 = n/a 1 = n/a
  //2 = 5M  3 = 10M  4 = 20M  5 = 30M  6 = 40M  7 = 50M  8 = 60M  9 = 80M  10 = 100M

  _send_buf[8] = 12;		//Gain, 0 to 20dB
  _send_buf[9] = 50;		//Display Gain, 1 to 100 percent
  _send_buf[10] = 0;		//Gain Equalization, 0=Off, 1=On
  _send_buf[11] = 3;		//Sector Size, 0=30, 1=60, 2=90, 3=120deg
  _send_buf[12] = 1;		//Beamwidth, 0=Wide, 1=Normal, 2=Narrow, 3=Narrow Mixed
  _send_buf[13] = 2;		//Number of Beams, 0=480, 1=240, 2=120
  _send_buf[14] = 0;		//Averaging, 0,1=Off, 2,3,4,...10 = #shots to average

  _send_buf[15] = (0&0xFF00)>>8;			//Persistence (Hi Byte), 0 to 600sec
  _send_buf[16] = (0&0x00FF);				//Persistence (Lo Byte)

  _send_buf[17] = (15000&0xFF00)>>8;; //Sound Velocity*10 (Hi Byte), 1400.0 to 1600.0m/s
  _send_buf[18] = (15000&0x00FF);     //Sound Velocity*10 (Lo Byte) in units of Meters

  _send_buf[19] = 3; //Mode, 0=Sector, 1=Linear, 2=Perspective, 3=Profile, 4=Beamtest
  _send_buf[20] = 0; //83P/83B Output Enable, 0=83P, 1=83B
                     //For 83P Output:
                     //Enable Profile Point Detection (set _send_buf[21]=1)
                     //For 83B Output:
                     //Sector Size must be 120 Deg (set _send_buf[11]=3)
                     //Number of Beams must be 120 (set _send_buf[13]=2)

   _send_buf[21] = 1;		//Profile Point Detection, 0=Disable, 1=Enable
   _send_buf[22] = 1;		//Profile Minimum Range, 0 to 100M
   							//must be in units of Meters
   _send_buf[23] = 25;		//Profile Minimum Level, 10 to 90 percent
   _send_buf[24] = 0;		//Transducer Up/Down, 0=Down, 1=Up
   _send_buf[25] = 0+180;	//Profile Tilt Angle + 180, -30 to +30deg
   _send_buf[26] = 0;		//Roll Correction, 0=Off, 1=On
   _send_buf[27] = 0;		//Measurement Units, 0=Meters, 1=Feet, 2=Yards
   _send_buf[28] = 1;		//Record Start/Start (.837)

   _send_buf[29] = 0;		//Record Start/Start (.83P)
   //Not implemented
   _send_buf[30] = 0;		//Record Start/Start (.83B)
   //Not implemented

   //The following External Trigger Control Bytes (31-33) are valid only for
   //DeltaT Sonar Heads supplied with the External Trigger Hardware Option
   _send_buf[31] = 0x03;					//External Trigger Control
   //Bit0, Edge: 	 0=NEG,     1=POS
   //Bit1, Enable: 0=Disable, 1=Enable

   _send_buf[32] = (0&0xFF00)>>8;		//External Trigger Transmit Delay (Hi Byte)
   _send_buf[33] = (0&0x00FF);		//External Trigger Transmit Delay (Lo Byte)
   //0 to 10000 in 100 microsecond increments

   _send_buf[34] = 2;		//Profile Point Filter
   //0=First Return, 1=Maximum Return, 2=Bottom Following                                    
}

void DeltaT::sendExternalControlSwitches()
{
  Boolean debug = False;
  int stat;

  Syslog::write("DeltaT::Sending... 837:%d Range:%d Gain:%d Gain_eq:%d Nbeams:%d Sector:%d Width:%d Avg:%d SOS:%ld",
		_send_buf[28], _send_buf[7], _send_buf[8], _send_buf[10], _send_buf[13],
		_send_buf[11],_send_buf[12],_send_buf[14], _sos);
  if ((stat = sendto(_sockfd, &_send_buf[0], sizeof(_send_buf), 0,
		     (struct sockaddr *)&_server_addr, sizeof(_server_addr))) < 0) {
    Syslog::write("External command comms failure: %d", errno);
  }
  return;
}