#include "StatusTelemetry.h"
#include "Math.h"

static const int StatusTelemetry::MissionTimeFieldWidth = 6;
static const int StatusTelemetry::MissionStateFieldWidth = 2;
static const int StatusTelemetry::BehaviorFieldWidth = 15;
static const int StatusTelemetry::BehaviorIndexFieldWidth = 3;
static const int StatusTelemetry::BatteryCurrentFieldWidth = 5;
static const int StatusTelemetry::BatteryVoltageFieldWidth = 5;

StatusTelemetry::StatusTelemetry(int maxPacketBytes)
  : AcousticTelemetry(maxPacketBytes),
    _integerData("int"),
    _stringData("string")
{
  addField(MissionTimeFieldWidth);
  addField(MissionStateFieldWidth);
  addField(BehaviorIndexFieldWidth);
  addField(BehaviorFieldWidth);
  addField(BatteryCurrentFieldWidth);
  addField(BatteryVoltageFieldWidth);
}


StatusTelemetry::~StatusTelemetry()
{
}


void StatusTelemetry::encode()
{
  _integerData.setValue(Math::round(data.elapsedMissionTime));
  set(&_integerData);
  _integerData.setValue((int )data.state);
  set(&_integerData);
  _integerData.setValue((int )data.behaviorIndex);
  set(&_integerData);
  _stringData.setValue(data.behaviorName);
  set(&_stringData);
  _integerData.setValue(Math::round(data.batteryCurrent));
  set(&_integerData);
  _integerData.setValue(Math::round(data.batteryVoltage * 10));
  set(&_integerData);
}


void StatusTelemetry::decode()
{
  get(&_integerData);
  data.elapsedMissionTime = (double )_integerData.value();

  get(&_integerData);
  data.state = (LayeredControlIF::MissionState )_integerData.value();

  get(&_integerData);
  data.behaviorIndex = (short )_integerData.value();

  get(&_stringData);
  strncpy(data.behaviorName, _stringData.value(), 
	  sizeof(LayeredControlIF::Name));

  data.behaviorName[sizeof(LayeredControlIF::Name)-1] = '\0';

  get(&_integerData);
  data.batteryCurrent = (double )(_integerData.value());

  get(&_integerData);
  data.batteryVoltage = (double )(_integerData.value() / 10.);
}