/****************************************************************************/
/* Copyright (c) 2006 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : MtiDriver.cc                                                   */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  :                                                               */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include "SerialDevice.h"
#include "Syslog.h"
#include "MtiDriver.h"
#include "MtiOutput.h"
#include "MtiLog.h"
#include "MTComm.h"

MtiDriver::MtiDriver(const char *devicename):
  Task("MtiDriver")
{
    Boolean debug = True;
    _deviceReady = False;
    
    _devicename = strdup(devicename);
    _mtComm = new CMTComm();
         
    //set up output areas and logging
    _mtiOutput = new MtiOutput();
    _mtiLog = new MtiLog( this, DataLog::BinaryFormat );

    // Initialize the device itself
    initialize();
}


MtiDriver::~MtiDriver()
{
  delete _mtComm;
  delete _mtiOutput;
  delete _mtiLog;
  free(_devicename);
}

// Set-up Mti with proper settings
//
DeviceIF::Status MtiDriver::initialize()
{
  // Connect to MTi
  //
  if (_mtComm->openPort(_devicename) != MTRV_OK)
  {
    Syslog::write("MtiDriver::initialize() - cannot open %s", _devicename);
    throw Exception("Error in MtiDriver serial parameters");
  }

  // Put MTi/MTx in Config State
  //
  if(_mtComm->writeMessage(MID_GOTOCONFIG) != MTRV_OK){
    Syslog::write("MtiDriver::initialize() - MTi device not responding");
    throw Exception("Error in MtiDriver set-up");
  }

  // Request DID
  //
  unsigned long value = 0xFFFFFFFF;
  _mtComm->reqSetting(MID_REQDID, value);
  if ((value & DID_TYPEH_MASK) != DID_TYPEH_MTI_MTX) {
    Syslog::write("MtiDriver::initialize() - MTi/MTx not detected");
    throw Exception("Error in MtiDriver set-up");
  }

  // Set MTi to output in calibration mode and orient to quaternion
  //
  if (_mtComm->setDeviceMode((OUTPUTMODE_CALIB | OUTPUTMODE_ORIENT),
                              OUTPUTSETTINGS_ORIENTMODE_QUATERNION) != MTRV_OK)
  {
    Syslog::write("MtiDriver::initialize() - Could not set device mode");
    throw Exception("Error in MtiDriver set-up");
  }
  
  // Set MTi to output at 10 Hz.
  // According to the documentation, lowest possible rate is 10Hz.
  // Use 11520 for 100ms period
  //
  if (_mtComm->setSetting(MID_SETPERIOD, 11520, LEN_PERIOD) != MTRV_OK)
  {
    Syslog::write("MtiDriver::initialize() - Could not set device period");
    throw Exception("Error in MtiDriver set-up");
  }
  
  // Put MTi/MTx in Measurement State
  //
  _mtComm->writeMessage(MID_GOTOMEASUREMENT);

  _deviceReady = True;
  
  return DeviceIF::Ok;
}


void MtiDriver::run()
{
  float fdata[18];
  unsigned char data[MAXMSGLEN];
  short datalen;
  
  if (!_deviceReady)
    return;
    
  while (_mtComm->readDataMessage(data, datalen) == MTRV_OK) {
    _mtComm->getValue(VALUE_CALIB_ACC, fdata, data);
    _mtiOutput->data.accX = fdata[0];
    _mtiOutput->data.accY = fdata[1];
    _mtiOutput->data.accZ = fdata[2];
   
    _mtComm->getValue(VALUE_CALIB_GYR, fdata, data);
    _mtiOutput->data.gyrX = fdata[0];
    _mtiOutput->data.gyrY = fdata[1];
    _mtiOutput->data.gyrZ = fdata[2];
   
    _mtComm->getValue(VALUE_CALIB_MAG, fdata, data);
    _mtiOutput->data.magX = fdata[0];
    _mtiOutput->data.magY = fdata[1];
    _mtiOutput->data.magZ = fdata[2];
   
    _mtComm->getValue(VALUE_ORIENT_QUAT, fdata, data);
    _mtiOutput->data.quat0 = fdata[0];
    _mtiOutput->data.quat1 = fdata[1];
    _mtiOutput->data.quat2 = fdata[2];
    _mtiOutput->data.quat3 = fdata[3];
    
    _mtiOutput->write();
    _mtiLog->write();
    
    //printf("%f\t%f\r", _mtiOutput->data.accX, _mtiOutput->data.quat0);
  }

}