#include "Spherical.h"

#include "Angle.h"

#include <math.h>

// using Angle = float;

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

// Spherical::Spherical(float polarAngle, float elevationAngle, float distance)
// {
//   this->horizontalAngle = polarAngle;
//   this->verticalAngle = elevationAngle;
//   this->distance = distance;
// }

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) {
  float signZ = (v.z >= 0) - (v.z < 0);
  horizontalAngle =
      atan2(v.y, signZ * sqrt(v.z * v.z + v.x * v.x)) * Angle::Rad2Deg;
  verticalAngle = -atan2(v.x, sqrt(v.z * v.z + v.y * v.y)) * Angle::Rad2Deg;
  distance = v.magnitude();
}

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