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

#include "TimeP.h"
#include "Multibeamer.h"
#include "Syslog.h"
#include "Reson7K.h"

Multibeamer::Multibeamer(char* ip)
  : Task(MultibeamerTaskName)
{
  _ip = strdup(ip);
  _input = new MultibeamerInput();
  _output = new MultibeamerOutput();

  _reson7k = new Reson7K(_ip);

  //_log = new MultibeamerLog( this, DataLog::BinaryFormat );

#if 0
  try {
    _log = new MultibeamerLog( this, DataLog::BinaryFormat );
  }
  catch ( ... ) {
    throw Exception("Multibeamer::Multibeamer() - MultibeamerLog contructor failed\n");
  }
#endif

  initialize();
}


Multibeamer::~Multibeamer()
{
  free(_ip);
  delete _input;
  delete _output;
  delete _reson7k;
}


DeviceIF::Status Multibeamer::initialize(void)
{
  Syslog::write("Multibeamer: initializing with 7kCenter at %s...\n", _ip);

  // Subscribe to reson messages
  U32 subscriptionRequest[2];
  subscriptionRequest[0] = 1;                  // the number of records
  subscriptionRequest[1] = 7006;               // 7k bathymetric data
  int nb = _reson7k->MB_RemoteCommand(7500,    // 7500 => Remote control
                                      1051,    // 1051 => record subscription
			                             (char *)subscriptionRequest,
			                            sizeof(subscriptionRequest)); 
  return DeviceIF::Ok;
} 

//   Record Data          Optional data         Headers and incidentals
//  5*512*4bytes    +     5*512*4bytes    +         2000;
#define MAX_BUF_SIZE  25000

struct RTH7006 {
  U64 sn;
  U32 pn;
  U16 mps;
  U32 nbeams;
  U8  flags;
  U8  svf;
  F32 sos;

};

void Multibeamer::run()
{
  Boolean debug = False;
  Boolean ready = False;

  char buffer[MAX_BUF_SIZE];

  // For socket option SO_RCVTIMEO
  struct timeval tv;
  tv.tv_sec = 3;
  tv.tv_usec = 500000L;

  //_reson7k->_mbDataSocket->setOptions(SOL_SOCKET, SO_RCVTIMEO,
  //         (const void **)&tv, sizeof(struct timeval));

  Syslog::write("Multibeamer: getting data from reson...\n");

  while (True)
  {
    sleep(1);
    // Check for reson message
    //
    int mlen = _reson7k->_mbDataSocket->receiveDatagram(buffer, MAX_BUF_SIZE);
    if (mlen > 0) dprintf("Multibeamer: Recv'd %d bytes", mlen);

    // Decode and process the record
    //
    int header_len = sizeof(struct Reson6046NetworkFrame)+sizeof(struct Reson6046DataRecordFrame);
    if (mlen > header_len)
    {
      dprintf("Multibeamer: Got at least the header");
      fflush(stdout);
      ready = True;

      // Ensure that this is a bathy record
      //
      struct Reson6046DataRecordFrame *drf = (struct Reson6046DataRecordFrame*)
                            (buffer + sizeof(struct Reson6046NetworkFrame));
      dprintf("Multibeamer: Got Record %d", drf->recordTypeIdentifier);

      if (drf->recordTypeIdentifier != 7006)
      {
        dprintf("Multibeamer: Record %d isn't what I think I asked for (7006)",
         drf->recordTypeIdentifier);
        continue;
      }


      // Get the ranges data from the record
      //
      char *rth = buffer + sizeof(struct Reson6046NetworkFrame) + sizeof(struct Reson6046DataRecordFrame) - sizeU32;

      struct RTH7006 *r = (struct RTH7006*)rth;
      dprintf("pn = %d", r->pn);
      dprintf("nbeams = %d", r->nbeams);
      dprintf("sos = %.2f", r->sos);

      Time::gettime(&_output->data.mb_data.updateTime);
      _output->data.mb_data.nbeams = r->nbeams;
      _output->data.mb_data.pingNum = r->pn;

      F32* ranges = (F32*)(rth + sizeU64 + 2*sizeU32 + sizeU16 + 2*sizeU8 + sizeF32);

      for (int i = 0; i < r->nbeams; i++) _output->data.mb_data.ranges[i] = (ranges[i]*r->sos)/2;
      //for (int i = 0; i < r->nbeams; i++) dprintf("beam %d range = %.3f\n", i, (ranges[i]*r->sos)/2);
      dprintf("mid = %.2f", _output->data.mb_data.ranges[255]);

      // Post data to output
      _output->write();
    }
    else
    {
      if (!ready)
      {
        Syslog::write("Multibeamer: Is 7kCenter running on %s?", _ip);
      }
    }
  }

  return;
}