/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  : Simulate the velocity measurement from the RDI DVL            */
/* Filename : SimulatedDvl.h                                                */
/* Author   : McEwen, following O'Reilly's template.                        */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 04/03/2001                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
#include <time.h>
#include "SimulatedDvl.h"
#include "MathP.h"
#include "Syslog.h"
#include "VehicleConfigurationIF.h"

SimulatedDvl::SimulatedDvl()
  : DvlIF_SK()
{
  //
  // Make the vehicle parameters available in the constructor. This is a
  // local variable.
  //
  VehicleConfigurationIF vehicleConfig("vehicleConfig");

  _simulator = new SimulatorIF("simulator");
  m_log = new DvlLog(this, DataLog::BinaryFormat);

  triggerEvent(DeviceIF::Ok);

  for( int i=0; i<NXE; i++ )
  {
    bottomTrackVelocity[i] = 0.;  
    waterMassVelocity[i]   = 0.;    
  }

}


SimulatedDvl::~SimulatedDvl()
{
  delete _simulator;
}


void SimulatedDvl::name(DeviceIF::Name name)
{
  strcpy(name, "Simulated Dvl");
}


void SimulatedDvl::serialNumber(DeviceIF::Name number)
{
  strcpy(number, "XXX");
}


DeviceIF::Status SimulatedDvl::initialize()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedDvl::powerOn()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedDvl::powerOff()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedDvl::dataLoggingOn()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedDvl::dataLoggingOff()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedDvl::status()
{
  return DeviceIF::Ok;
}

const double maxDvlRange = 200.;
const double maxAngle    = 9.*PI/20.;  //81 Degrees

DeviceIF::Status SimulatedDvl::get(Data *data, Boolean *newData)
{
  double position[3], velocity[3], backDiff[3], simTime;
  double EulerAngles[3], angularVelocity[3], roll, pitch, yaw;
  long k;                         //Sample period counter
  DeviceIF::Status status;
  Boolean debug = True;
  //
  // Copy the state.  waterMassVelocity = vel_Bo_W_B.
  //
  _simulator->state( position, data->waterMassVelocity, 
		     EulerAngles, angularVelocity );
  //
  // This returns vel_Bo_N_W from the sim.
  //
  _simulator->translationalBottomVelocity( data->bottomTrackVelocity );
  //
  // Rob:  Change pitch and roll to space-fixed, limited.
  //
  data->temp       = 15.;
  data->roll       = EulerAngles[0];
  data->pitch      = EulerAngles[1];
  data->heading    = EulerAngles[2];

  roll  = Math::limit( EulerAngles[0], maxAngle, -maxAngle );
  pitch = Math::limit( EulerAngles[1], maxAngle, -maxAngle );
  yaw   = Math::limit( EulerAngles[2], maxAngle, -maxAngle );

  //
  // Copy simTime
  //
  k = _simulator->nTs();
  //
  // Post a DVL reading at 1 Hz.  
  //
  *newData = False;
  if( (k % 5) == 0 ) *newData = True;
  //
  // Fill in the "data" array:
  //
  data->pingTime = _simulator->simTime();
  
  data->range    = ( _simulator->waterDepth() - position[2] )/
                   ( cos(pitch) * cos(roll) );
  //
  // Water tracking:
  //
  data->waterStatus = 0;

  if( data->range < maxDvlRange )
  {
    data->bottomStatus = 0;
    data->dataStatus = data->bottomStatus;
    status = DeviceIF::Ok;
  }
  else
  {
    data->bottomStatus = 0xFF;    //Set all correlation and intensity bits
    data->dataStatus = data->bottomStatus;
    status = DeviceIF::Error;
  }
  //
  // Fill in the local data for the log->write.
  //
  for( int i=0; i<3; i++ )
  {
    bottomTrackVelocity[i] = data->bottomTrackVelocity[i];
    waterMassVelocity[i]   = data->waterMassVelocity[i];
  }
  *dvlTemp    = data->temp;
  *dvlRoll    = data->roll;
  *dvlPitch   = data->pitch;
  *dvlHeading = data->heading;
  *dvlRange   = data->range;

  *bottomStatus = data->bottomStatus;
  *waterStatus  = data->waterStatus;

  m_log->write();

  return status;
}

int SimulatedDvl::spawnAuxTasks()
{
  return 0;
}