#include <math.h>

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

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

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

Polar::Polar(Vector2 v) {
  angle = Vector2::SignedAngle(
      Vector2::forward,
      v); // atan2(v.x, sqrt(v.z * v.z + v.y * v.y)) * Angle::Rad2Deg;
  distance = v.magnitude();
}

Polar::Polar(Spherical s) {
  angle = s.horizontalAngle;
  distance = s.distance * cosf(s.verticalAngle * Angle::Deg2Rad);
}

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

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

Polar Polar::operator+(Polar &v2) {
  if (v2.distance == 0)
    return Polar(this->distance,
                 this->angle); // Polar(this->angle, this->distance);
  if (this->distance == 0)
    return v2;

  float deltaAngle = Angle::Normalize(v2.angle - (float)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->distance + v2.distance,
        this->angle); // 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 ? (float)this->angle - angle : (float)this->angle + angle;
  newAngle = Angle::Normalize(newAngle);
  Polar vector = Polar(newDistance, newAngle); // Polar(newAngle, newDistance);
  return vector;
}

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

Polar Polar::operator-(Polar &v2) {
  // Polar vector = *this + Polar(v2.distance, (float)v2.angle - 180);
  //(Polar(v2.angle - 180, v2.distance));
  Polar vector = -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);
}

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