#include "Spherical16.h"

#include "Quaternion.h"
#include "Spherical.h"

#include <math.h>

Spherical16::Spherical16() {
  this->distance = 0.0f;
  this->horizontalAngle = Angle16(0);
  this->verticalAngle = Angle16(0);
}

Spherical16::Spherical16(Polar polar) {
  this->distance = polar.distance;
  this->horizontalAngle = Angle16(polar.angle);
  this->verticalAngle = Angle16(0);
}

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

Spherical16::Spherical16(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 Spherical16 Spherical16::zero = Spherical16(0.0f, 0.0f, 0.0f);
const Spherical16 Spherical16::forward = Spherical16(1.0f, 0.0f, 0.0f);
const Spherical16 Spherical16::back = Spherical16(1.0f, 180.0f, 0.0f);
const Spherical16 Spherical16::right = Spherical16(1.0f, 90.0f, 0.0f);
const Spherical16 Spherical16::left = Spherical16(1.0f, -90.0f, 0.0f);
const Spherical16 Spherical16::up = Spherical16(1.0f, 0.0f, 90.0f);
const Spherical16 Spherical16::down = Spherical16(1.0f, 0.0f, -90.0f);

bool Spherical16::operator==(const Spherical16& v) const {
  return (this->distance == v.distance &&
          this->horizontalAngle == v.horizontalAngle &&
          this->verticalAngle == v.verticalAngle);
}

Spherical16 Spherical16::Normalize(const Spherical16& v) {
  Spherical16 r = Spherical16(1, v.horizontalAngle, v.verticalAngle);
  return r;
}
Spherical16 Spherical16::normalized() const {
  Spherical16 r = Spherical16(1, this->horizontalAngle, this->verticalAngle);
  return r;
}

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

Spherical16 Spherical16::operator-(const Spherical16& s2) const {
  Spherical thisSpherical = Spherical(
      this->distance, (Angle)this->horizontalAngle, (Angle)this->verticalAngle);
  Spherical spherical2 = Spherical(s2.distance, (Angle)s2.horizontalAngle,
                                   (Angle)s2.verticalAngle);

  // let's do it the easy way...
  Vector3 v1 = Vector3(thisSpherical);
  Vector3 v2 = Vector3(spherical2);
  Vector3 v = v1 - v2;
  Spherical16 r = Spherical16(v);
  return r;
}
Spherical16 Spherical16::operator-=(const Spherical16& v) {
  *this = *this - v;
  return *this;
}

Spherical16 Spherical16::operator+(const Spherical16& s2) const {
  // let's do it the easy way...
  Vector3 v1 = Vector3(Spherical(this->distance, (float)this->horizontalAngle,
                                 (float)this->verticalAngle));
  Vector3 v2 = Vector3(Spherical(s2.distance, (float)s2.horizontalAngle,
                                 (float)s2.verticalAngle));
  Vector3 v = v1 + v2;
  Spherical16 r = Spherical16(v);
  return r;
  /*
  // This is the hard way...
  if (v2.distance <= 0)
    return Spherical(this->distance, this->horizontalAngle,
                     this->verticalAngle);
  if (this->distance <= 0)
    return v2;

  float deltaHorizontalAngle =
      (float)Angle::Normalize(v2.horizontalAngle - this->horizontalAngle);
  float horizontalRotation = deltaHorizontalAngle < 0
                                 ? 180 + deltaHorizontalAngle
                                 : 180 - deltaHorizontalAngle;
  float deltaVerticalAngle =
      Angle::Normalize(v2.verticalAngle - this->verticalAngle);
  float verticalRotation = deltaVerticalAngle < 0 ? 180 + deltaVerticalAngle
                                                  : 180 - deltaVerticalAngle;

  if (horizontalRotation == 180 && verticalRotation == 180)
    // angle is too small, take this angle and add the distances
    return Spherical(this->distance + v2.distance, this->horizontalAngle,
                     this->verticalAngle);

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

  // Now we have to project the angle to the horizontal and vertical planes...
  // The axis for the angle is the cross product of the two spherical vectors
  // (which function we do not have either...)
  float horizontalAngle = 0;
  float verticalAngle = 0;

  float newHorizontalAngle =
      deltaHorizontalAngle < 0
          ? Angle::Normalize(this->horizontalAngle - horizontalAngle)
          : Angle::Normalize(this->horizontalAngle + horizontalAngle);
  float newVerticalAngle =
      deltaVerticalAngle < 0
          ? Angle::Normalize(this->verticalAngle - verticalAngle)
          : Angle::Normalize(this->verticalAngle + verticalAngle);

  Spherical v = Spherical(newDistance, newHorizontalAngle, newVerticalAngle);
  */
}
Spherical16 Spherical16::operator+=(const Spherical16& v) {
  *this = *this + v;
  return *this;
}

// Spherical Passer::LinearAlgebra::operator*(const Spherical &v, float f) {
//   return Spherical(v.distance * f, v.horizontalAngle, v.verticalAngle);
// }
// Spherical Passer::LinearAlgebra::operator*(float f, const Spherical &v) {
//   return Spherical(v.distance * f, v.horizontalAngle, v.verticalAngle);
// }
Spherical16 Spherical16::operator*=(float f) {
  this->distance *= f;
  return *this;
}

// Spherical Passer::LinearAlgebra::operator/(const Spherical &v, float f) {
//   return Spherical(v.distance / f, v.horizontalAngle, v.verticalAngle);
// }
// Spherical Passer::LinearAlgebra::operator/(float f, const Spherical &v) {
//   return Spherical(v.distance / f, v.horizontalAngle, v.verticalAngle);
// }
Spherical16 Spherical16::operator/=(float f) {
  this->distance /= f;
  return *this;
}

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

#include "AngleUsing.h"
#include "FloatSingle.h"
#include "Vector3.h"

const float epsilon = 1E-05f;
const float Rad2Deg = 57.29578F;

Angle Spherical16::AngleBetween(const Spherical16& v1, const Spherical16& v2) {
  // float denominator = sqrtf(v1_3.sqrMagnitude() * v2_3.sqrMagnitude());
  float denominator =
      v1.distance * v2.distance;  // sqrtf(v1.distance * v1.distance *
                                  // v2.distance * v2.distance);
  if (denominator < epsilon)
    return 0.0f;

  Vector3 v1_3 = Vector3(Spherical(v1.distance, (Angle)v1.horizontalAngle,
                                   (Angle)v1.verticalAngle));
  Vector3 v2_3 = Vector3(Spherical(v2.distance, (Angle)v2.horizontalAngle,
                                   (Angle)v2.verticalAngle));
  float dot = Vector3::Dot(v1_3, v2_3);
  float fraction = dot / denominator;
  if (isnan(fraction))
    return fraction;  // short cut to returning NaN universally

  float cdot = Float::Clamp(fraction, -1.0, 1.0);
  float r = ((float)acos(cdot)) * Rad2Deg;
  return r;
}

Spherical16 Spherical16::Rotate(const Spherical16& v,
                                Angle horizontalAngle,
                                Angle verticalAngle) {
  Spherical16 r = Spherical16(v.distance, v.horizontalAngle + horizontalAngle,
                              v.verticalAngle + verticalAngle);
  return r;
}
Spherical16 Spherical16::RotateHorizontal(const Spherical16& v, Angle a) {
  Spherical16 r =
      Spherical16(v.distance, v.horizontalAngle + a, v.verticalAngle);
  return r;
}
Spherical16 Spherical16::RotateVertical(const Spherical16& v, Angle a) {
  Spherical16 r =
      Spherical16(v.distance, v.horizontalAngle, v.verticalAngle + a);
  return r;
}