/****************************************************************************/
/* 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 "TimeP.h"
#include "Syslog.h"
#include "System.h"
#include "VehicleConfigurationIF.h"
#include "FloatAttribute.h"
#include "AngleAttribute.h"

#define MAXRANGE 99.99
#define MAXSIMRANGE 110.

SimulatedRangeFinder::SimulatedRangeFinder()
  : RangeFinderIF_SK(),   _attributes("psa916Server")
{
  //
  // Read in parameters from the .cfg file:
  //
  char configFileName[256];
  strcpy(configFileName, System::configurationFile("psa916.cfg"));
  _attributes.add(new AngleAttribute("xzAngle", "x-z plane mounting angle",
				    &_xzAngle));

  _attributes.add(new FloatAttribute("maxRange", "Max measurable range",
				     &_maxRange, 99.));
  _maxRange = MAXSIMRANGE;

  AttributeParser::parse(configFileName, &_attributes);
  //
  // Make the vehicle parameters available in the constructor. This is a
  // local variable.
  //
  VehicleConfigurationIF vehicleConfig("vehicleConfig");
  //
  // Extract some into class variables.
  //
  // This should come from psa916.cfg.  But I can't figure out how to read it
  // in here using the existing framework.
  //_mountAngle = 60.*PI/180.;   //Between body-fixed z axis and sonar los
  _mountAngle = _xzAngle;
  //
  // Compute the unit vector:
  _los_B[0] = sin( _mountAngle );
  _los_B[1] = 0.;
  _los_B[2] = cos( _mountAngle );
  //
  // 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(0.,0.);                         

  triggerEvent(DeviceIF::Ok);

  try 
  {
     _log = new PSA916Log( this, DataLog::BinaryFormat );
  }
  catch ( ... ) 
  {
     throw Exception("SimulatedRangeFinder - PSA916Log contructor failed\n");
  }

}


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

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 = _mountAngle;
}


void SimulatedRangeFinder::maxRange(double *meters)
{
  *meters = _maxRange;
}
double rangeTol    = 0.2;            //Range accuracy to 20 cm.
//
// Note: Later, read this in from the psa.cfg. It will require adding "len"
// to the vehicle.cfg, and then computing the 2.62.  The number in psa.cfg
// should be the distance back from the nose to the sonar head.
//
double offset_B[3] = {2.62, 0., 0.};   //Distance to psa916 in body coords.

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

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

  *echoReceived = 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. )
  {
     // *** Original Stuff, circa Dec. 1999.
     //
     // 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] + _mountAngle )/cos( euler[0] ));
     //
     // *** End original stuff.
     //
     // Use new beamRange computation using bisect().
     //
     double grazing;
     Boolean surfaceDetect;
     _range = _simulator->beamRange( offset_B, _los_B, 0., _maxRange, 
                                     rangeTol, bisectCnt, &grazing,
	                             &surfaceDetect);
     if( _range != 0. )
     {
	*echoReceived = True;
     }
     else
     {
	*echoReceived = False;
	_range = MAXSIMRANGE;
     }
     *value = _range;
  }
  else
  {
    //
    // The bottom is not within sight.  Also, don't divide by zero.
    //
    *echoReceived = False;
    //
    //Give _range a reasonable value, but not much more than it's range:
    _range = MAXSIMRANGE;  
  }
  k = _simulator->nTs();
  //
  // Print out debug info
  //
  if( (k % 5) == 0 && (k != 0) )
  dprintf(" SimulatedRangeFinder::range() \n"
	  "               waterDepth(0.,0.) = %.1f \n"
	  "               range = %.1f, \n"
	  "               iterations = %d \n",
	  _simulator->waterDepth(0.,0.),
	  _range, *bisectCnt );
  //
  // Implement these later.
  //
  //sampleTime->seconds = time(0);
  //sampleTime->nanoSeconds = 0.;

  TimeIF::TimeSpec ts;
  Time::gettime(&ts);
  sampleTime->seconds = ts.seconds;
  sampleTime->nanoSeconds = ts.nanoSeconds;

  _log->write();

  return DeviceIF::Ok;
}


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


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


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