/****************************************************************************/
/* Copyright (c) 2000 MBARI                                                 */
/* MBARI Proprietary Information. All rights reserved.                      */
/****************************************************************************/
/* Summary  :                                                               */
/* Filename : SimulatedAhrs.cc                                               */
/* Author   :                                                               */
/* Project  :                                                               */
/* Version  : 1.0                                                           */
/* Created  : 02/07/2000                                                    */
/* Modified :                                                               */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/****************************************************************************/
//////////////////////////////////////////////////////////////////////
//
// PURPOSE:  Simulate the Ahrs Compass and Inclinometer
// AUTHOR:   Marsh, Following O'Reilly's Template
//
// $Id: SimulatedAhrs.cc,v 1.3 2001/06/02 21:33:30 hthomas Exp $
//
//////////////////////////////////////////////////////////////////////
//
#include <time.h>
#include "SimulatedAhrs.h"
#include "MathP.h"
#include "Syslog.h"
#include "VehicleConfigurationIF.h"
#include "WorkSiteIF.h"

SimulatedAhrs::SimulatedAhrs()
  : AhrsIF_SK()
{

  VehicleConfigurationIF vehicleConfig("vehicleConfig");
  //
  // Get stuff from worksite.cfg.
  //
  WorkSiteIF workSite("workSite");
  // Get magnetic variation
  _magneticVar =  workSite.magneticVariation();

  Boolean debug = True;

  _simulator = new SimulatorIF("simulator");

}


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


void SimulatedAhrs::name(DeviceIF::Name name)
{
  sprintf(name, "Ahrs");
}


void SimulatedAhrs::serialNumber(DeviceIF::Name number)
{
  strcpy(number, "XBow serialNO");
}


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


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


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


DeviceIF::Status SimulatedAhrs::status()
{
  // Dummy implementation for now
  return DeviceIF::Ok;
}


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


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

const double maxAhrsTilt = 85.*PI/180.;

DeviceIF::Status SimulatedAhrs::getAttitude(AhrsIF::Attitude *attitude, 
					    TimeIF::TimeSpec *sampleTime)
{
  double rollSF, pitchSF, magHeading;

  getState();
  //
  // The simulated AHRS tilt sensor is limited to 85 Degrees.
  //
  pitchSF = _euler[1];
  if( pitchSF >  maxAhrsTilt ) pitchSF =  maxAhrsTilt;
  if( pitchSF < -maxAhrsTilt ) pitchSF = -maxAhrsTilt;
  //
  // BEWARE:  The AHRS puts out projected, or space-fixed (SF), roll and pitch 
  // angles.  THESE ARE NOT EULER ANGLES.  The following relation converts
  // the 2-1 Euler roll into a space-fixed roll angle.  See Wertz, p.226.
  // For the 2-1 sequence the space-fixed and body-fixed pitch are the same.
  // The first rotation, yaw, is intrinsically space-fixed.
  //
  // At 20 Deg pitch, the Euler roll and the space-fixed roll differ 
  // by about 20 milli-Radians.
  //
  rollSF = atan( tan(_euler[0])/cos(pitchSF) );
  if( rollSF >  maxAhrsTilt ) rollSF =  maxAhrsTilt;
  if( rollSF < -maxAhrsTilt ) rollSF = -maxAhrsTilt;

  magHeading = PI + Math::modPi( _euler[2]  -  _magneticVar - PI);

  attitude->roll      = rollSF;
  attitude->pitch     = pitchSF;
  attitude->yaw       = magHeading;
  attitude->rollRate  = _omega[0];
  attitude->pitchRate = _omega[1];
  attitude->yawRate   = _omega[2];

  sampleTime->seconds = time(0);
  sampleTime->nanoSeconds = 0;
  return DeviceIF::Ok;
}


Boolean SimulatedAhrs::magneticCompass()
{
  return True;
}


long SimulatedAhrs::getState()
{
     Boolean debug = False;
     
     double altitude;
     //
     // Extract the vehicle state from the simulation.
     //
     _simulator->state( _position, _velocity, _euler, _omega );

     return OK;
}


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