/****************************************************************************/
/* 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 "TimeP.h"
#include "Syslog.h"
#include "DeltaT.h"

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

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();
  enable(True);
}


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

void DeltaT::enable(Boolean on_off_switch)
{
  _enabled = on_off_switch;
  _output->data.enabled = on_off_switch;
  _output->write();
}

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*)&_client_addr, 0, sizeof(_client_addr));

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

  // Bind client socket
  if ( bind(_sockfd, (struct sockaddr *)&_client_addr, sizeof(_client_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);

  
  return DeviceIF::Ok;
} 

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

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

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


  sendExternalControlSwitches();
}

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

  sendExternalControlSwitches();
}

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

  sendExternalControlSwitches();
}

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

  sendExternalControlSwitches();
}

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

  sendExternalControlSwitches();
}

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)
    sendExternalControlSwitches();
  }
}

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
    sendExternalControlSwitches();
  }
}

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

  sendExternalControlSwitches();
}

void DeltaT::set_idt_logging(Boolean on)
{
  Syslog::write("DeltaT - set 837 logging to %d", on);
  if (on)
    _send_buf[28] = 1;		//837 data logging, 0=Off, 1=On
  else
    _send_buf[28] = 0;

  sendExternalControlSwitches();
}

int DeltaT::messageHandler()
{
  int status = 0;
  //  float sos;
  long sos;

  DeltaTMessage::Message msg;
  while (_msgQ->read(&msg) > 0) {
    status++;

    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:
      Boolean on;
      on = (msg._int > 0)? True: False;
      set_gain_eq(on);
      break;

    case DeltaTMessage::SetLogging:
      on = (msg._int > 0)? True: False;
      set_idt_logging(on);
      break;

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

  return status;
}

void DeltaT::run()
{

  Boolean debug = False;

  int           nbytes;

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

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

    messageHandler();

    if (!_enabled) {
      dprintf("DeltaT logging not enabled...\n");
      sleep(1);
      continue;
    }

    // Use select to see if any packets are waiting on the app socket
    //
    fd_set reads, writes;
    FD_ZERO(&reads);
    FD_ZERO(&writes);
    FD_SET(_sockfd, &reads);
    struct timeval tv;
    tv.tv_sec = 1;
    tv.tv_usec = 0L;

    // check for data on the socket
    if (select(_sockfd+1, &reads, NULL, NULL, &tv) == -1) {
      Syslog::write("DeltaT::run() - socket select call error: %d", errno);
    }

    // Data present if _sockfd is set
    if (!FD_ISSET(_sockfd, &reads)) {
      dprintf("Nothing from idt app...");
      continue;
    }

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

    processData(nbytes);

    // If this is the first packet, make sure to send any
    // external commands that might have been made already.
    //
    if (!_xmit) {
      _xmit = True;
      sendExternalControlSwitches();
    }
  }
  return;
}

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)
   												//must be 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] = 0x00;					//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;
}


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

  if (nbytes > 10) {
    dprintf("%d bytes: buf[0:2]=>%c %c %c",
	    nbytes,
	    _receive_buf[0],
	    _receive_buf[1],
	    _receive_buf[2]
	    );
  }


  if (nbytes < 256) {
    dprintf("Insufficient Data? Only %d bytes", nbytes);
    return;
  }

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

  else {
    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;

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