// 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 <math.h>
#include "Angle.h"

template <>
AngleOf<signed short>::AngleOf(float angle) {
  if (!isfinite(angle)) {
    value = 0;
    return;
  }

  // map float [-180..180) to integer [-32768..32767]
  this->value = (signed short)((angle / 360.0F) * 65536.0F);
}

template <>
AngleOf<signed short>::operator float() const {
  float f = ((this->value * 180) / 32768.0F);
  return f;
}

// This should not exist...
// template <>
// AngleOf<signed short> AngleOf<signed short>::pi = 3.1415927410125732421875F;

// template <>
// AngleOf<signed short> AngleOf<signed short>::Rad2Deg = 360.0f / (pi * 2);
// template <>
// AngleOf<signed short> AngleOf<signed short>::Deg2Rad = (pi * 2) / 360.0f;

// template <>
// AngleOf<signed short> AngleOf<signed short>::Normalize(
//     AngleOf<signed short> angle) {
//   return angle;
// }

// Not correct!!! just for syntactical compilation ATM
template <>
AngleOf<signed short> AngleOf<signed short>::CosineRuleSide(
    float a,
    float b,
    AngleOf<signed short> gamma) {
  float a2 = a * a;
  float b2 = b * b;
  float d = a2 + b2 - 2 * a * b * cos(gamma * AngleOf<float>::Deg2Rad);
  // Catch edge cases where float inacuracies lead tot nans
  if (d < 0)
    return 0;

  float c = sqrtf(d);
  return c;
}

// Not correct!!! just for syntactical compilation ATM
template <>
AngleOf<signed short> AngleOf<signed short>::CosineRuleAngle(float a,
                                                             float b,
                                                             float c) {
  float a2 = a * a;
  float b2 = b * b;
  float c2 = c * c;
  float d = (a2 + b2 - c2) / (2 * a * b);
  // Catch edge cases where float inacuracies lead tot nans
  if (d >= 1)
    return 0;
  if (d <= -1)
    return 180;

  float gamma = acos(d) * Angle::Rad2Deg;
  return gamma;
}