#include "Spherical.h"

#include "Angle.h"
#include "Quaternion.h"

#include <math.h>

Spherical::Spherical() {
  this->distance = 0.0f;
  this->horizontalAngle = 0.0f;
  this->verticalAngle = 0.0f;
}

Spherical::Spherical(Polar polar) {
  this->distance = polar.distance;
  this->horizontalAngle = polar.angle;
  this->verticalAngle = 0.0f;
}

Spherical::Spherical(float distance, Angle horizontalAngle,
                     Angle verticalAngle) {
  if (distance < 0) {
    this->distance = -distance;
    this->horizontalAngle = Angle::Normalize(horizontalAngle - 180);
    this->verticalAngle = verticalAngle;
  } else {
    this->distance = distance;
    this->horizontalAngle = Angle::Normalize(horizontalAngle);
    this->verticalAngle = Angle::Normalize(verticalAngle);
  }
}

Spherical::Spherical(Vector3 v) {
  this->distance = v.magnitude();
  if (distance == 0.0f) {
    this->verticalAngle = 0.0f;
    this->horizontalAngle = 0.0f;
  } else {
    this->verticalAngle =
        (90.0f - acosf(v.Up() / this->distance) * Angle::Rad2Deg);
    this->horizontalAngle = atan2f(v.Right(), v.Forward()) * Angle::Rad2Deg;
  }
}

const Spherical Spherical::zero = Spherical(0.0f, 0.0f, 0.0f);

Spherical Spherical::operator-() {
  Spherical v = Spherical(this->distance, this->horizontalAngle + 180.0f,
                          this->verticalAngle + 180.0f);
  return v;
}

// float Spherical::GetSwing() {
//   // Not sure if this is correct
//   return sqrtf(horizontalAngle * horizontalAngle +
//                verticalAngle * verticalAngle);
// }

// float Spherical::Distance(const Spherical &s1, const Spherical &s2) {
//   float d = 0;
//   return d;
// }