/****************************************************************************/
/* 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();

  //_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 for reson at %s...\n", _ip);

  // Setup socket to receive the reson data
  _reson7k = new Reson7K(_ip);

   Syslog::write("reson socket ready"); 

  // 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)); 
  printf("subscribed after sending packet of size %d\n", nb);
  return DeviceIF::Ok;
} 


void Multibeamer::run()
{
  Boolean debug = True;

  _output->data.range = 30.;
  _output->data.goodComms = True;
  _output->data.ready = True;
 
  char buffer[2048];
 
  Syslog::write("Multibeamer: getting data from reson...\n");

  while (1)
  {
    // Check for reson message
    //
    int mlen = _reson7k->_mbDataSocket->receiveDatagram(buffer, 2048);

    // Decode and process the record
    //
    int header_len = sizeof(struct Reson6046NetworkFrame)+sizeof(struct Reson6046DataRecordFrame);
    if (mlen > header_len)
    {
      // Ensure that this is a bathy record
      //
      struct Reson6046DataRecordFrame *drf = (struct Reson6046DataRecordFrame*)
                            (buffer + sizeof(struct Reson6046NetworkFrame));
      if (drf->recordTypeIdentifier != 7006)
      {
        dprintf("Multibeamer: Record is %d which is not what I asked for",
         drf->recordTypeIdentifier);
        continue;
      }

      // Get the ranges data from the record
      //
      char *rth = buffer + header_len;
      unsigned int offset = sizeof(U64) + sizeof(U32) + sizeof(U16);
      U32 nbeams = (U32)rth[offset];
      offset += sizeof(U32) + 2*sizeof(U8);
      F32 sos = (F32)rth[offset];
      dprintf("Multibeamer: Record has %d ranges calculated with %.2f sos",
         nbeams, sos);
      offset += sizeof(F32);
      F32 *ranges = (F32*)(rth + offset);

      // range beam 0 = ranges[0];
      // for (U32 i = 0; i < nbeams; i++) dprintf("beam %d range = %.3f m\n", i, ranges[i]);

      Time::gettime(&_output->data.updateTime);

      // Post data to output
      _output->write();
    }
    else
    {
      sleep(1);
    }
  }

  return;
}