#include "NavTelemetry.h"
#include "Math.h"

static const int NavTelemetry::LatitudeFieldWidth = 7;
static const int NavTelemetry::LongitudeFieldWidth = 8;
static const int NavTelemetry::DepthFieldWidth = 4;
static const int NavTelemetry::AltitudeFieldWidth = 4;
static const int NavTelemetry::HeadingFieldWidth = 5;
static const int NavTelemetry::SpeedFieldWidth = 3;
static const int NavTelemetry::PitchFieldWidth = 4;

NavTelemetry::NavTelemetry(int maxPacketBytes)
  : AcousticTelemetry(maxPacketBytes),
    _integerData("int")
{
  addField(LatitudeFieldWidth);
  addField(LongitudeFieldWidth);
  addField(DepthFieldWidth);
  addField(AltitudeFieldWidth);
  addField(HeadingFieldWidth);
  addField(SpeedFieldWidth);
  addField(PitchFieldWidth);
}


NavTelemetry::~NavTelemetry()
{
}


void NavTelemetry::encode()
{
  // Encode latitude, in ten-thousandths of a degree
  _integerData.setValue((int )(data.latitude * Math::DegsPerRad * 10000));
  set(&_integerData);

  // Encode longitude, in ten-thousandths of a degree
  _integerData.setValue((int )(data.longitude * Math::DegsPerRad * 10000));
  set(&_integerData);

  // Depth in meters
  _integerData.setValue(Math::round(data.depth));
  set(&_integerData);

  // Altitude in meters
  _integerData.setValue(Math::round(data.altitude));
  set(&_integerData);

  // Heading in tenths of a degree
  _integerData.setValue(Math::round(Math::DegsPerRad * data.heading * 10));
  set(&_integerData);

  // Speed in tenths of a meter per second
  _integerData.setValue(Math::round(data.speed * 10));
  set(&_integerData);

  // Pitch in degrees
  _integerData.setValue(Math::round(Math::DegsPerRad * data.pitch));
  set(&_integerData);
}


void NavTelemetry::decode()
{
  get(&_integerData);
  data.latitude = _integerData.value() * Math::RadsPerDeg / 10000.;

  get(&_integerData);
  data.longitude = _integerData.value() * Math::RadsPerDeg / 10000.;

  get(&_integerData);
  data.depth = _integerData.value();

  get(&_integerData);
  data.altitude = _integerData.value();

  get(&_integerData);
  data.heading = Math::RadsPerDeg * _integerData.value() / 10.;

  get(&_integerData);
  data.speed = _integerData.value() / 10.;

  get(&_integerData);
  data.pitch = Math::RadsPerDeg * _integerData.value();
}