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

const float Angle::Rad2Deg = 57.29578F;
const float Angle::Deg2Rad = 0.0174532924F;

float Angle::Normalize(float angle) {
  if (!isfinite(angle))
    return angle;

  while (angle <= -180)
    angle += 360;
  while (angle > 180)
    angle -= 360;
  return angle;
}

float Angle::Clamp(float angle, float min, float max) {
  float normalizedAngle = Normalize(angle);
  float r = Float::Clamp(normalizedAngle, min, max);
  return r;
}

float Angle::Difference(float a, float b) {
  float r = Normalize(b - a);
  return r;
}

float Angle::MoveTowards(float fromAngle, float toAngle, float maxAngle) {
  float d = toAngle - fromAngle;
  float sign = signbit(d) ? -1 : 1;
  d = sign * Float::Clamp(fabs(d), 0, maxAngle);
  return fromAngle + d;
}

float Angle::CosineRuleSide(float a, float b, float gamma) {
  float a2 = a * a;
  float b2 = b * b;
  float d = a2 + b2 - 2 * a * b * cos(gamma * Angle::Deg2Rad);
  // Catch edge cases where float inacuracies lead tot nans
  if (d < 0)
    return 0;

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

float Angle::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;
}

float Angle::SineRuleAngle(float a, float beta, float b) {
  float alpha = asin(a * sin(beta * Angle::Deg2Rad) / b);
  return alpha;
}