#include "DistanceSensor.h"

#include "Messages/LowLevelMessages.h"

namespace RoboidControl {

DistanceSensor::DistanceSensor(Thing* parent) : Thing(parent) {
  this->type = Type::DistanceSensor;
  this->name = "Distance sensor";
}

float DistanceSensor::GetDistance() {
  if (this->externalDistance < this->internalDistance)
    return this->externalDistance;
  else
    return this->internalDistance;
}

int DistanceSensor::GenerateBinary(char* bytes, unsigned char* ix) {
  LowLevelMessages::SendFloat16(bytes, ix, this->internalDistance);
  return *ix;
}

void DistanceSensor::ProcessBinary(char* bytes) {
  unsigned char ix = 0;
  this->externalDistance = LowLevelMessages::ReceiveFloat16(bytes, &ix);
}

}  // namespace RoboidControl