#include "Spherical.h"

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

#include <math.h>

// using Angle = float;

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

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

Spherical::Spherical(float distance, Angle horizontalAngle, Angle verticalAngle)
    : distance(distance), horizontalAngle(horizontalAngle),
      verticalAngle(verticalAngle) {}

Spherical::Spherical(Vector3 v) {
  distance = v.magnitude();
  if (distance == 0.0f) {
    verticalAngle = 0;
    horizontalAngle = 0;
  } else {
    verticalAngle = (90 - acosf(v.y / distance) * Angle::Rad2Deg);
    horizontalAngle = atan2f(v.x, v.z) * Angle::Rad2Deg;
  }
}

const Spherical Spherical::zero = Spherical(0.0F, (Angle)0.0F, (Angle)0.0F);

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

// Polar Spherical::ProjectOnHorizontalPlane() {
//   return Polar(horizontalAngle, distance);
// }

// Vector3 Spherical::ToVector3() {
//   float verticalRad = (90 - verticalAngle) * Angle::Deg2Rad;
//   float horizontalRad = horizontalAngle * Angle::Deg2Rad;
//   float cosVertical = cosf(verticalRad);
//   float sinVertical = sinf(verticalRad);
//   float cosHorizontal = cosf(horizontalRad);
//   float sinHorizontal = sinf(horizontalRad);
//   Vector3 v = Vector3(this->distance * sinVertical * sinHorizontal,
//                       this->distance * cosVertical,
//                       this->distance * sinVertical * cosHorizontal);
//   return v;
// }