#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);

bool Polar::operator==(const Polar &v) {
  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+(Polar &v2) {
  if (v2.distance == 0)
    return Polar(this->distance, this->angle);
  if (this->distance == 0.0f)
    return v2;

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

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

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

  float angle =
      Angle::CosineRuleAngle(newDistance, this->distance, v2.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-() {
  Polar v = Polar(this->distance, this->angle + 180);
  return v;
}

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

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

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

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