#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::Normalize(newAngle - 180);
    distance = -newDistance;
  } else {
    angle = Angle::Normalize(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 {
  if (v2.distance == 0)
    return Polar(this->angle, this->distance);
  if (this->distance == 0)
    return v2;

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

  if (rotation == 180 && v2.distance > 0) {
    //  angle is too small, take this angle and add the distances
    return Polar(this->angle, this->distance + v2.distance);
  }

  float newDistance =
      Angle::CosineRuleSide(v2.distance, this->distance, rotation);

  float 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);
}

Polar Polar::Rotate(Polar v, float angle) {
  v.angle = Angle::Normalize(v.angle + angle);
  return v;
}