#include <math.h>

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

Polar::Polar() {
  this->distance = 0.0f;
  this->angle = 0.0f;
}
Polar::Polar(float distance, Angle angle) {
  // distance should always be 0 or greater
  if (distance < 0.0f) {
    this->distance = -distance;
    this->angle = Angle::Normalize(angle - 180);
  } else {
    this->distance = distance;
    if (this->distance == 0.0f)
      // angle is always 0 if distance is 0
      this->angle = 0.0f;
    else
      this->angle = Angle::Normalize(angle);
  }
}
Polar::Polar(Vector2 v) {
  this->distance = v.magnitude();
  this->angle = Vector2::SignedAngle(Vector2::forward, v);
}
Polar::Polar(Spherical v) {
  this->distance = v.distance * cosf(v.verticalAngle * Angle::Deg2Rad);
  this->angle = v.horizontalAngle;
}

const Polar Polar::zero = Polar(0.0f, 0.0f);
const Polar Polar::forward = Polar(1.0f, 0.0f);
const Polar Polar::back = Polar(1.0, 180.0f);
const Polar Polar::right = Polar(1.0, 90.0f);
const Polar Polar::left = Polar(1.0, -90.0f);

bool Polar::operator==(const Polar &v) const {
  return (this->distance == v.distance && this->angle == v.angle);
}

Polar Polar::Normalize(const Polar &v) {
  Polar r = Polar(1, v.angle);
  return r;
}
Polar Polar::normalized() const {
  Polar r = Polar(1, this->angle);
  return r;
}

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

Polar Polar::operator-(const Polar &v) const {
  Polar r = -v;
  return *this + r;
}
Polar Polar::operator-=(const Polar &v) {
  *this = *this - v;
  return *this;
}
Polar Polar::operator+(const Polar &v) const {
  if (v.distance == 0)
    return Polar(this->distance, this->angle);
  if (this->distance == 0.0f)
    return v;

  float deltaAngle = Angle::Normalize(v.angle - (float)this->angle);
  float rotation =
      deltaAngle < 0.0f ? 180.0f + deltaAngle : 180.0f - deltaAngle;

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

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

  float angle = Angle::CosineRuleAngle(newDistance, this->distance, v.distance);

  float newAngle = deltaAngle < 0.0f ? (float)this->angle - angle
                                     : (float)this->angle + angle;
  newAngle = Angle::Normalize(newAngle);
  Polar vector = Polar(newDistance, newAngle);
  return vector;
}
Polar Polar::operator+=(const Polar &v) {
  *this = *this + v;
  return *this;
}

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

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

Polar Polar::Rotate(const Polar &v, Angle angle) {
  Angle a = Angle::Normalize(v.angle + angle);
  Polar r = Polar(v.distance, a);
  return r;
}