// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0.If a copy of the MPL was not distributed with this
// file, You can obtain one at https ://mozilla.org/MPL/2.0/.

#include "DiscreteAngle.h"

#include "Angle.h"
#include <math.h>

template <> AngleUsing<unsigned char>::AngleUsing(unsigned char angle) {
  if (!isfinite(angle)) {
    value = 0;
    return;
  }

  // normalize to (-180..180]
  angle = Angle::Normalize(angle);

  // map (-180..180] to (0..255], which is equivaluent to 1..255
  // This means that range value 0 is not used
  this->value = (unsigned char)((angle + 180.0F) / 360.0F * 255.0F);
}

template <> float AngleUsing<unsigned char>::ToFloat() {
  float f = ((float)this->value / 255.0F) * 360.0F - 180.0F;
  return f;
}

template <> AngleUsing<unsigned short>::AngleUsing(unsigned short angle) {
  if (!isfinite(angle)) {
    value = 0;
    return;
  }

  // normalize to (-180..180]
  angle = Angle::Normalize(angle);

  // map (-180..180] to (0..65535] which is equivalent to 1..65535
  // This means that range value 0 is not used
  this->value = (unsigned short)((angle + 180.0F) / 360.0F * 65535.0F);
}

template <> float AngleUsing<unsigned short>::ToFloat() {
  float f = ((float)this->value / 65535.0F) * 360.0F - 180.0F;
  return f;
}