/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : SimulatedDepthSensor.cc                                           */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
//////////////////////////////////////////////////////////////////////
//
// PURPOSE:  Simulate the DepthSensor measurement
// AUTHOR:   McEwen, Following O'Reilly's Template
// DATE:     99/12/15
//
//////////////////////////////////////////////////////////////////////
//
#include <time.h>
#include "SimulatedDepthSensor.h"
#include "MathP.h"
#include "Syslog.h"
#include "VehicleConfigurationIF.h"

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

  _simulator = new SimulatorIF("simulator");

  triggerEvent(DeviceIF::Ok);
}


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


void SimulatedDepthSensor::name(DeviceIF::Name name)
{
  strcpy(name, "Simulated DepthSensor");
}


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


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


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


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


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


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


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

const double GLITCHMAG  = 11.;    //Magnitude of Parosci glitch in meters.
const int    GLITCHCNT  = 0;      //Number of consecutive glitches in a cycle
const double GLITCHINTERVAL = 10.;//Amount of time between glitch cycles

double glitchTime = 23.;   //Time of initial simulated Parosci glitches cycle
int glitchCnt = GLITCHCNT; //Number of consecutive glitches in a cycle
int glitchCycles = 3;      //Number of glitch cycles to execute

DeviceIF::Status SimulatedDepthSensor::depth( double *value, 
					      TimeIF::TimeSpec *t )
{
  Boolean debug = False;

  SimulatorIF::Vector position, euler;
  SimulatorIF::Vector vel_B_N_B, omega_B_N_B;

  //
  // Extract the vehicle state from the simulation.
  //
  _simulator->state(position, vel_B_N_B, euler, omega_B_N_B);

  *value = position[Z];
  //
  // Hardcode in a Parosci glitch to test the outlier rejection code.
  //
  if( _simulator->simTime() > glitchTime)
  {
    if (glitchCnt > 0)
    {
      //
      // Perform next consecutive glitch in this cycle
      //
      *value = position[Z] + GLITCHMAG;
      glitchCnt--;
    }
    else if (--glitchCycles > 0)
    {
      //
      // Reset glitch triggers if we're on for another cycle
      //
      glitchCnt = GLITCHCNT;
      glitchTime += GLITCHINTERVAL;
    }
  }

  dprintf("SimulatedDepthSensor::range =%.2f\n", *value);
  //
  // Implement these later.
  //
  t->seconds = time(0);
  t->nanoSeconds = 0;

  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedDepthSensor::temp(double *value, 
					    TimeIF::TimeSpec *t)
{
  *value = 0.;
  t->seconds = time(0);
  t->nanoSeconds = 0;
  return DeviceIF::Ok;
}

DeviceIF::Status SimulatedDepthSensor::pressure(double *value, 
						TimeIF::TimeSpec *t)
{
  *value = 0.;
  t->seconds = time(0);
  t->nanoSeconds = 0;
  return DeviceIF::Ok;  
}


DeviceIF::Status SimulatedDepthSensor::calibrate()
{
  return DeviceIF::Ok;
}


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