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

#define MAXRANGE 99.99

SimulatedRangeFinder::SimulatedRangeFinder()
  : RangeFinderIF_SK()
{
  //
  // Make the vehicle parameters available in the constructor. This is a
  // local variable.
  //
  VehicleConfigurationIF vehicleConfig("vehicleConfig");
  //
  // Extract some into class variables.
  //
  pointingAngle = 0.;
  //
  // Now read in constants from simulator.cfg.  The best way to do this
  // is to have Simulator's constructor read them in, and then pass them
  // through SimulatorIF. (Generated from the .idl).
  //
  _simulator = new SimulatorIF("simulator");

  //
  // Read zMax from simulation.cfg, someday.
  //
  zMax = _simulator->waterDepth();                         
  _maxRange = 200.;

  triggerEvent(DeviceIF::Ok);
}


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

void SimulatedRangeFinder::name(DeviceIF::Name name)
{
  strcpy(name, "Simulated RangeFinder");
}


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


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


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


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


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


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


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


void SimulatedRangeFinder::xzAngle(double *radians)
{
  *radians = pointingAngle;
}


void SimulatedRangeFinder::maxRange(double *meters)
{
  *meters = _maxRange;
}


DeviceIF::Status SimulatedRangeFinder::range(Boolean *validRange, 
					     double *value, 
					     TimeIF::TimeSpec *sampleTime)
{
  Boolean debug = False;

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

  *validRange = True;

  double altitude;
  //
  // Extract the vehicle state from the simulation.
  //
  _simulator->state(position, vel_B_N_B, euler, omega_B_N_B);
  //
  // Compute the range, given vehicle orientation, depth, and bottom depth.
  // ASSUME that the bottom is flat and level.
  //
  if( fabs( euler[0] )            < PI*9./20. &&
      fabs( euler[1] + pointingAngle ) < PI*9./20.   )
  {
    //
    // See Notebook #3, p.3.  I think this should be
    //
    // range = alt / ( c(theta)*c(phi)*c(thetaP) - s(theta)*s(thetaP) )
    //
    // But, in the interest of consistancy, I ended up just inverting the eqn 
    // in sensor_load.c.
    //
    altitude = (zMax - position[Z]);
    *value   = (altitude / cos( euler[1] + pointingAngle )/cos( euler[0] ));
    *validRange = True;
  }
  else
  {
    //
    // The bottom is not within sight.  Also, don't divide by zero.
    //
    *validRange = False;
  }
  dprintf("SimulatedRangeFinder::range =%.2f\n", *value);
  //
  // Implement these later.
  //
  sampleTime->seconds = time(0);
  sampleTime->nanoSeconds = 0.;

  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedRangeFinder::enable() 
{
  return DeviceIF::Ok;
}


DeviceIF::Status SimulatedRangeFinder::disable() 
{
  return DeviceIF::Ok;
}


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