#include <math.h>

#include "Angle.h"
#include "Polar.h"

Polar::Polar() {
  angle = 0.0F;
  distance = 0.0F;
}

Polar::Polar(float newAngle, float newDistance) {
  // distance should always be 0 or greater
  if (newDistance < 0) {
    angle = angle - 180;
    distance = -newDistance;
  } else {
    angle = newAngle;
    distance = newDistance;
  }
}

const Polar Polar::zero = Polar(0, 0);

float Polar::Distance(const Polar& v1, const Polar& v2) {
  float d =
      Angle::CosineRuleSide(v1.distance, v2.distance, v2.angle - v1.angle);
  return d;
}

Polar Polar::operator+(const Polar& v2) const {
  float deltaAngle = Angle::Normalize(v2.angle - this->angle);
  float rotation = deltaAngle < 0 ? 180 + deltaAngle : 180 - deltaAngle;

  if (rotation == 180 && v2.distance > 0)
    return Polar::zero;

  // return Polar(this->angle + v2.angle, this->distance + v2.distance);
  float newDistance =
      Angle::CosineRuleSide(v2.distance, this->distance, rotation);

  float angle = rotation;
  if (newDistance > 0.001F &&
      this->distance > 0.001F)  // nonzero because float inaccuracies may still
                                // lead to problems with small number
                                // plus, it is more efficient....
    angle = Angle::CosineRuleAngle(newDistance, this->distance, v2.distance);
  float newAngle = deltaAngle < 0 ? Angle::Normalize(this->angle - angle)
                                  : Angle::Normalize(this->angle + angle);
  Polar vector = Polar(newAngle, newDistance);
  return vector;
}

Polar Polar::operator-() {
  Polar vector = Polar(this->angle - 180, this->distance);
  return vector;
}

Polar Polar::operator-(const Polar& v2) const {
  Polar vector = *this + (Polar(v2.angle - 180, v2.distance));
  return vector;
}

Polar Polar::operator*(float f) const {
  return Polar(this->angle, this->distance * f);
}

Polar Polar::operator/(const float& f) {
  return Polar(this->angle, this->distance / f);
}