static char State_id[] = "$Header: State.cc,v 1.2 97/03/20 12:24:55 oreilly Exp $";

/*
$Log:	State.cc,v $
Revision 1.2  97/03/20  12:24:55  12:24:55  oreilly (Thomas C. O'Reilly)
*** empty log message ***

Revision 1.1  96/10/28  09:10:12  09:10:12  oreilly (Thomas C. O'Reilly)
Initial revision

*/

#include "State.h"
#include "DmErrno.h"
#include "Math.h"

DmStateObject::DmStateObject(const char *name)
  : DmObject(name, 1)
{
  Errno status = dm_create((char *)name, 1, 
			   &_handle,
			   DM_FLT32, NStateElements, 
			   DM_ENDT, 0);

  char buf[100];
  char errorBuf[errorMsgSize];
    
  switch (status)
  {
    case SUCCESS:
    case EDM_NAME_EXISTS:
    break;
    
    default:
    sprintf(buf, "DmStateObject::DmStateObject(%s)", name);
    sprintDmError(status, buf, errorBuf);
    setError(errorBuf);
    return;
  }
}


DmStateObject::~DmStateObject()
{
}



Errno DmStateObject::read(StateValue *values, DM_Time *t)
{
  MBool debug = TRUE;
  
  return dm_read(_handle, values, sizeof(StateValue) * NStateElements, t);
}


Errno DmStateObject::write(StateValue *values, DM_Time *t)
{
  return dm_write(_handle, values, sizeof(StateValue) * NStateElements, t);
}


DmAttitudeObject::DmAttitudeObject(MBool useRaw)
  : DmStateObject(ATTITUDE_DM)
{
  _useRaw = useRaw;
  if (_useRaw)
  {
    char *itemName;
    char errorBuf[errorMsgSize];
    
    itemName = "TIBURON.SENSOR.COMPASS.DEGREES";
    _rawHeading = new DmFlt32Object(itemName);
    if (_rawHeading->error())
    {
      _rawHeading->errorMsg(errorBuf);
      setError(errorBuf);
      return;
    }

    itemName = "TIBURON.SENSOR.PITCH.DEGREES";
    _rawPitch = new DmFlt32Object(itemName);
    if (_rawPitch->error())
    {
      _rawPitch->errorMsg(errorBuf);
      setError(errorBuf);
      return;
    }
    
    itemName = "TIBURON.SENSOR.ROLL.DEGREES";
    _rawRoll = new DmFlt32Object(itemName);
    if (_rawRoll->error())
    {
      _rawRoll->errorMsg(errorBuf);
      setError(errorBuf);
      return;
    }
  }
}


DmAttitudeObject::~DmAttitudeObject()
{
}


Errno DmAttitudeObject::read(StateValue *values, DM_Time *t)
{
  if (!_useRaw)
    return DmStateObject::read(values, t);
  else
  {
    Flt32 pitch, roll, heading;
    Errno status;
    
    if ((status = _rawHeading->read(&heading, t)) != SUCCESS)
      return status;

    if ((status = _rawPitch->read(&pitch, t)) != SUCCESS)
      return status;

    if ((status = _rawRoll->read(&roll, t)) != SUCCESS)
      return status;

    values[HeadingIndex] = heading * Math::RadsPerDeg;
    values[PitchIndex] = pitch * Math::RadsPerDeg;
    values[RollIndex] = roll * Math::RadsPerDeg;

    values[HeadingRateIndex] = values[PitchRateIndex] = 
      values[RollRateIndex] = BadStateValue;
    
    return SUCCESS;
  }
}


Errno DmAttitudeObject::write(StateValue *values, DM_Time *t)
{
  if (!_useRaw)
    return DmStateObject::write(values, t);
  else
  {
    Flt32 pitch, roll, heading;
    Errno status;
    
    heading = values[HeadingIndex] * Math::RadsPerDeg;
    pitch = values[PitchIndex] * Math::RadsPerDeg;
    roll = values[RollIndex] * Math::RadsPerDeg;
    
    if ((status =  _rawHeading->write(heading, t)) != SUCCESS)
      return status;

    if ((status = _rawPitch->write(pitch, t)) != SUCCESS)
      return status;

    if ((status = _rawRoll->write(roll, t)) != SUCCESS)
      return status;

    return SUCCESS;
  }
}


DmPositionObject::DmPositionObject(MBool useRaw)
  : DmStateObject(POSITION_DM)
{
  _useRaw = useRaw;
  if (_useRaw)
  {
    char *itemName;
    char errorBuf[errorMsgSize];
    
    itemName = "TIBURON.SENSOR.DEPTH.METERS";
    _rawDepth = new DmFlt64Object(itemName);
    if (_rawDepth->error())
    {
      _rawDepth->errorMsg(errorBuf);
      setError(errorBuf);
      return;
    }
  }
}


DmPositionObject::~DmPositionObject()
{
}



Errno DmPositionObject::read(StateValue *values, DM_Time *t)
{
  if (!_useRaw)
    return DmStateObject::read(values, t);
  else
  {
    Flt64 depth;
    Errno status;
    
    if ((status = _rawDepth->read(&depth, t)) != SUCCESS)
      return status;

    values[2] = depth;
    values[0] = values[1] = values[3] = values[4] = values[5] = BadStateValue;
    
    return SUCCESS;
  }
}



Errno DmPositionObject::write(StateValue *values, DM_Time *t)
{
  if (!_useRaw)
    return DmStateObject::write(values, t);
  else
  {
    Errno status;
    
    if ((status =  _rawDepth->write(values[2], t)) != SUCCESS)
      return status;

    return SUCCESS;
  }
}