//=========================================================================
// Summary  : Main file for Reson 6046
// Filename : Reson6046Driver.cc
// Author   : Reed Christenson
// Project  : Reson 6046 Driver
// Revision : 0.0
// Created  : 2002.02.26
// Modified : 2002.02.26
// 	$Id: Reson6046Driver.cc,v 1.72 2011/11/08 16:05:06 hthomas Exp $	
//=========================================================================
// Description : Reson 6046 Driver
//=========================================================================

#include "stdafx.h"
#include <stdio.h>
#include <sys/timeb.h>
#include "Reson7K.h"

#define POSITION_MSG 1003
#define ATTITUDE_MSG 1004
// #define ALTITUDE_MSG 1006
// #define MOTION_MSG   1007
// #define DEPTH_MSG    1008
// #define CTD_MSG      1010


Reson7K::Reson7K(char *address)
{   
SOCKET sock;
	// Create another instance of a simple socket for sending bluefin data frames
    _mbDataSocket = NULL;
    _mbDataSocket = new SimpleSocket();
   

    // Make a data socket connection for Multibeam remote control
    while(!_mbDataSocket->ready())
    {
		sock = // port 7000 - according to Reson tester
		_mbDataSocket->createBroadcastDatagramSend(7000, address);
								 
		if(!_mbDataSocket->ready()) {
			Sleep(1000);
		}
    }
   
}

/// Reson 6046 Driver Destructor.
Reson7K::~Reson7K(void)
{
	if (_mbDataSocket) {
		_mbDataSocket->destroy();
	}
}


// Build and send a 7K Remote Command to the 7K multibeam
// Parameters have default value of 0.0
//
int Reson7K::MB_RemoteCommand(int cmdIn, char *recordData)
{
	int data_size = 0;
	int rc_id, rth;

	rc_id = -1;
	rth = cmdIn;

	switch (cmdIn) {
		case Reson6046Command::Position : //1003
			data_size += sizeof(U32) + sizeof(F32) + 3*sizeof(F64) + 5*sizeof(U8);
			break;

		case Reson6046Command::Altitude : //1006
			data_size += sizeof(F32);
			break;

		case Reson6046Command::Depth : //1008
			data_size += 2*sizeof(U8) + sizeof(U16) + sizeof(F32);
			break;

		case Reson6046Command::RollPitchHeave : //1012
			data_size += 3*sizeof(F32);
			break;

		case Reson6046Command::Heading : //1013
			data_size += sizeof(F32);
			break;

		default :
			return -1;
	}
	
	//Build message network frame
  //
  unsigned drf_size;
  if (rth == 7500) {
     drf_size = sizeof(Reson6046DataRecordFrame) + sizeof(MBRemoteControl)
                      + data_size;
  } else {
     drf_size = sizeof(Reson6046DataRecordFrame) + data_size;
  }
  unsigned totalSize = sizeof(struct Reson6046NetworkFrame) + drf_size; 
  char *buf = new char[totalSize];
  struct Reson6046NetworkFrame *nf = (struct Reson6046NetworkFrame*)buf;

  nf->version = MB7K_PROTOCOL_VERSION;
  nf->offset = sizeof(struct Reson6046NetworkFrame);
  nf->totalPackets = 1;
  nf->totalRecords = 1;
  nf->transmissionIdentifier = 0;
  nf->packetSize = totalSize;
  nf->totalSize = drf_size;
  nf->sequenceNumber = 0;
  nf->destDeviceIdentifier = 0;
  nf->destEnumerator = 0;
  nf->srcEnumerator = 0;
  nf->srcDeviceIdentifier = 0;

  // Build Data Record Frame
  //
  struct Reson6046DataRecordFrame *drf = (struct Reson6046DataRecordFrame*)
                            (buf + sizeof(struct Reson6046NetworkFrame));

  drf->offset = sizeof(Reson6046DataRecordFrame) - 2*sizeof(U16) - sizeof(U32);
  drf->syncPattern = 0x0000FFFF;
  drf->size = drf_size;
  drf->optionalDataOffset = 0;
  drf->optionalDataIdentifier = 0;

  //set time
  long sec = 0;
  time(&sec);
  struct tm *fullTime = gmtime(&sec);
  struct timeb tmb;
  ftime(&tmb);
  drf->time7k.year = fullTime->tm_year + 1900;
  drf->time7k.day =  fullTime->tm_yday + 1;
  drf->time7k.seconds = (float)fullTime->tm_sec + (float)tmb.millitm/1000.;
  drf->time7k.minutes = fullTime->tm_min;
  drf->time7k.hours = fullTime->tm_hour;

  drf->recordVersion = MB7K_RECORD_VERSION;
  drf->recordTypeIdentifier = rth;
  drf->deviceIdentifier = 7125;
  //drf->reserved1 = 0;
  drf->systemEnumerator = 0;
  drf->sequentialRecordCounter = 0;
  drf->flags = 0;
  drf->reserved2 = 0;
  drf->reserved3 = rc_id;
  drf->totalRecords = 0;
  drf->fragmentNumber = 0;
  drf->version = MB7K_PROTOCOL_VERSION;

  // Place remote control info in meesage buffer
  //
  if (rth == 7500) {
  	MBRemoteControl *pRC = (MBRemoteControl*)
       	                  (buf + sizeof(struct Reson6046NetworkFrame) +
       	                         sizeof(struct Reson6046DataRecordFrame) -
       	                         sizeof(U32));
  	pRC->rc_id = rc_id;
  	pRC->ticket = 0;
  	memcpy((char*)(pRC) + sizeof(MBRemoteControl), recordData, data_size);

  	// For debug purposes use #if 1, otherwise use #if 0
#if 0  
  	printf("Dumping MB record of size %u, drf size of %u...\n", totalSize, drf_size);
  	for (int ii = 0; ii < totalSize;) {
  	  printf("%02X:", buf[ii++]);
  	  if ((ii % 8) == 0) printf("\n");
  	}
  	printf("\n");
#endif
  } else {
	memcpy((char*)(buf + sizeof(struct Reson6046NetworkFrame) +
			     sizeof(struct Reson6046DataRecordFrame)-sizeof(U32)),
	       recordData, data_size);
//	Syslog::write("sending speed of sound of %f\n", *((float *)recordData));
  }

  int bytesSent = _mbDataSocket->broadcast(buf, totalSize);

  delete buf;

  return 0;
}

int Reson7K::MB_RemoteCommand(int cmdIn, float cmdParam, float cmdParam1,
				      float cmdParam2, float cmdParam3, int iParam)
{
  F32 param = cmdParam;
  F32 param1 = cmdParam1;
  F32 param2 = cmdParam2;
  F32 param3 = cmdParam3;
  U32 iparam = iParam;
  
  // Determine which command and build the Remote Control message section
  char recordData[512];  
  char *ptrToData = recordData;
  char *ptrs = ptrToData;
  int  data_size = 0;

  int rc_id, rth;
  switch (cmdIn) {
    case Reson6046Command::MBStopPing  :  // No params
      rc_id = 1101;
      rth = 7500;
      break;

    case Reson6046Command::MBStartPing :
      rc_id = 1100;
      rth = 7500;
      break;

    case Reson6046Command::SetMBRange :  // Single params
      rc_id = 1003;
      rth = 7500;
      data_size += sizeof(F32);
      memcpy(ptrToData, &param, sizeof(F32));
      break;

    case Reson6046Command::SetMBPingRate :
      rc_id = 1004;
      rth = 7500;
      data_size += sizeof(F32);
      memcpy(ptrToData, &param, sizeof(F32));
      break;

    case Reson6046Command::SetMBTxPower :
      rc_id = 1005;
      rth = 7500;
      data_size += sizeof(F32);
//      Syslog::write("setting multibeam transmit power\n");
      memcpy(ptrToData, &param, sizeof(F32));
      break;

    case Reson6046Command::SetMBRxGain :
      rc_id = 1008;
      rth = 7500;
      data_size += sizeof(F32);
      memcpy(ptrToData, &param, sizeof(F32));
      break;

    case Reson6046Command::SetMBPulseWidth :
      rc_id = 1006;
      rth = 7500;
      data_size += sizeof(F32);
      memcpy(ptrToData, &param, sizeof(F32));
      break;

    case Reson6046Command::SetMBSoundVelocity :
      if (param > -0.01 && param < 0.01) // param is effectively zero
		{
		float sos = 1500.0;
		param = sos;
		//
		rc_id = -1; //no rc_id for this command
		rth = 7610;
		data_size += sizeof(F32);
		if (param < 100.) param = 1484.0;
		memcpy(ptrToData, &param, sizeof(F32));
		}
      break;

    case Reson6046Command::SetMBAbsorptionLoss :
      rc_id = -1;
      rth = 7611;
      data_size += sizeof(F32);
      memcpy(ptrToData, &param, sizeof(F32));
      break;

    case Reson6046Command::SetMBSpreadingLoss :
      rc_id = -1;
      rth = 7612;
      data_size += sizeof(F32);
      memcpy(ptrToData, &param, sizeof(F32));
      break;

    case Reson6046Command::SetMBBottomDetectionFilters :  // Multiple params
      rc_id = 1010;
      rth = 7500;
      data_size += 4*sizeof(F32);
      memcpy((void*)ptrs, &param, sizeof(F32));  // minRange
      ptrs += sizeof(F32);
      memcpy((void*)ptrs, &param1, sizeof(F32)); // maxRange
      ptrs += sizeof(F32);
      memcpy((void*)ptrs, &param2, sizeof(F32)); // minDepth
      ptrs += sizeof(F32);
      memcpy((void*)ptrs, &param3, sizeof(F32)); // maxDepth
      ptrs += sizeof(F32);
      break;

    case Reson6046Command::UseDepthRangeMBBottomDetect :
      rc_id = 1009;
      rth = 7500;
      data_size += sizeof(U32);
      iparam = 0x0;
      if (iParam == 1) iparam = 0x200;
      if (iParam == 2) iparam = 0x100;
      if (iParam == 3) iparam = 0x200 | 0x100;
        
      memcpy(ptrToData, &iparam, sizeof(U32));
      break;

    case Reson6046Command::SetMBSnippetControl :
      rc_id = 1103;
      rth = 7500;
      data_size += 2*sizeof(U32);
      
      memcpy(ptrToData, &iparam, sizeof(U32));
      iparam = (U32)param; // Window size
      memcpy(ptrToData+sizeof(U32), &iparam, sizeof(U32));
      break;

    case Reson6046Command::Start7KCenterLogging :
      char fileName[256];
      time_t rawtime;
      struct tm *timeinfo;
      size_t length;

      rc_id = 1200;
      rth = 7500;
      data_size += (sizeof(U32) + sizeof(U8)*256);
      memset(ptrToData, 0x00, data_size);
      iparam = 0; //always append to existing file, never overwrite
      memcpy(ptrs, &iparam, sizeof(U32)); ptrs += sizeof(U32);
      time(&rawtime);
      timeinfo = gmtime(&rawtime);
      length = strftime(fileName, 256, "%Y%m%d_%H%M%S\0",timeinfo);
      memcpy(ptrs,fileName,length); 
      break;

    case Reson6046Command::Stop7KCenterLogging :
      rc_id = 1201;
      rth = 7500;
      data_size += 0; //no parameters
      break;


    default:
      return 0;
      break;

  }

  // Build message network frame
  //
  unsigned drf_size;
  if (rth == 7500) {
     drf_size = sizeof(Reson6046DataRecordFrame) + sizeof(MBRemoteControl)
                      + data_size;
  } else {
     drf_size = sizeof(Reson6046DataRecordFrame) + data_size;
  }
  unsigned totalSize = sizeof(struct Reson6046NetworkFrame) + drf_size; 
  char *buf = new char[totalSize];
  struct Reson6046NetworkFrame *nf = (struct Reson6046NetworkFrame*)buf;

  nf->version = MB7K_PROTOCOL_VERSION;
  nf->offset = sizeof(struct Reson6046NetworkFrame);
  nf->totalPackets = 1;
  nf->totalRecords = 1;
  nf->transmissionIdentifier = 0;
  nf->packetSize = totalSize;
  nf->totalSize = drf_size;
  nf->sequenceNumber = 0;
  nf->destDeviceIdentifier = 0;
  nf->destEnumerator = 0;
  nf->srcEnumerator = 0;
  nf->srcDeviceIdentifier = 0;

  // Build Data Record Frame
  //
  struct Reson6046DataRecordFrame *drf = (struct Reson6046DataRecordFrame*)
                            (buf + sizeof(struct Reson6046NetworkFrame));

  drf->offset = sizeof(Reson6046DataRecordFrame) - 2*sizeof(U16) - sizeof(U32);
  drf->syncPattern = 0x0000FFFF;
  drf->size = drf_size;
  drf->optionalDataOffset = 0;
  drf->optionalDataIdentifier = 0;

  //set time
  long sec = 0;
  time(&sec);
  struct tm *fullTime = gmtime(&sec);
  struct timeb tmb;
  ftime(&tmb);
  drf->time7k.year = fullTime->tm_year + 1900;
  drf->time7k.day =  fullTime->tm_yday + 1;
  drf->time7k.seconds = (float)fullTime->tm_sec + (float)tmb.millitm/1000.;
  drf->time7k.minutes = fullTime->tm_min;
  drf->time7k.hours = fullTime->tm_hour;

  drf->recordVersion = MB7K_RECORD_VERSION;
  drf->recordTypeIdentifier = rth;
  drf->deviceIdentifier = 7125;
//  drf->reserved1 = 0;
  drf->systemEnumerator = 0;
  drf->sequentialRecordCounter = 0;
  drf->flags = 0;
  drf->reserved2 = 0;
  drf->reserved3 = rc_id;
  drf->totalRecords = 0;
  drf->fragmentNumber = 0;
  drf->version = MB7K_PROTOCOL_VERSION;

  // Place remote control info in meesage buffer
  //
  if (rth == 7500) {
  	MBRemoteControl *pRC = (MBRemoteControl*)
       	                  (buf + sizeof(struct Reson6046NetworkFrame) +
       	                         sizeof(struct Reson6046DataRecordFrame) -
       	                         sizeof(U32));
  	pRC->rc_id = rc_id;
  	pRC->ticket = 0;
  	memcpy((char*)(pRC) + sizeof(MBRemoteControl), recordData, data_size);

  	// For debug purposes use #if 1, otherwise use #if 0
#if 0  
  	printf("Dumping MB record of size %u, drf size of %u...\n", totalSize, drf_size);
  	for (int ii = 0; ii < totalSize;) {
  	  printf("%02X:", buf[ii++]);
  	  if ((ii % 8) == 0) printf("\n");
  	}
  	printf("\n");
#endif
  } else {
	memcpy((char*)(buf + sizeof(struct Reson6046NetworkFrame) +
			     sizeof(struct Reson6046DataRecordFrame)-sizeof(U32)),
	       recordData, data_size);
//	Syslog::write("sending speed of sound of %f\n", *((float *)recordData));
  }

  int bytesSent = _mbDataSocket->broadcast(buf, totalSize);

  delete buf;

  return 0;
}

