#include "Spherical.h"

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

#include <math.h>

template <typename T>
SphericalOf<T>::SphericalOf() {
  this->distance = 0.0f;
  this->horizontal = AngleOf<T>(0);
  this->vertical = AngleOf<T>(0);
}

// template <>
// SphericalOf<signed short>::SphericalOf() {
//   this->distance = 0.0f;
//   this->horizontal = AngleOf<signed short>(0);
//   this->vertical = AngleOf<signed short>(0);
// }

template <typename T>
SphericalOf<T>::SphericalOf(float distance,
                            AngleOf<T> horizontal,
                            AngleOf<T> vertical) {
  this->distance = distance;
  this->horizontal = horizontal;
  this->vertical = vertical;
}

// template <>
// SphericalOf<float>::SphericalOf(float distance,
//                                 AngleOf<float> horizontal,
//                                 AngleOf<float> vertical) {
//   this->distance = distance;
//   this->horizontal = horizontal;
//   this->vertical = vertical;
// }

// template <>
// SphericalOf<signed short>::SphericalOf(float distance,
//                                        AngleOf<signed short> horizontal,
//                                        AngleOf<signed short> vertical) {
//   this->distance = distance;
//   this->horizontal = horizontal;
//   this->vertical = vertical;
// }

template <typename T>
SphericalOf<T> SphericalOf<T>::FromPolar(PolarOf<T> polar) {
  AngleOf<T> horizontal = polar.angle;
  AngleOf<T> vertical = AngleOf<T>(0);
  SphericalOf<T> r = SphericalOf(polar.distance, horizontal, vertical);
  return r;
}

template <typename T>
SphericalOf<T> SphericalOf<T>::FromVector3(Vector3 v) {
  float distance = v.magnitude();
  if (distance == 0.0f) {
    return SphericalOf(distance, 0, 0);
  } else {
    float verticalAngle =
        (90.0f - acosf(v.Up() / distance) * Passer::LinearAlgebra::Rad2Deg);
    float horizontalAngle =
        atan2f(v.Right(), v.Forward()) * Passer::LinearAlgebra::Rad2Deg;
    return SphericalOf(distance, horizontalAngle, verticalAngle);
  }
}

template <typename T>
Vector3 SphericalOf<T>::ToVector3() const {
  float verticalRad =
      (90.0f - this->vertical.ToFloat()) * Passer::LinearAlgebra::Deg2Rad;
  float horizontalRad =
      this->horizontal.ToFloat() * Passer::LinearAlgebra::Deg2Rad;
  float cosVertical = cosf(verticalRad);
  float sinVertical = sinf(verticalRad);
  float cosHorizontal = cosf(horizontalRad);
  float sinHorizontal = sinf(horizontalRad);

  float x = this->distance * sinVertical * sinHorizontal;
  float y = this->distance * cosVertical;
  float z = this->distance * sinVertical * cosHorizontal;

  Vector3 v = Vector3(x, y, z);
  return v;
}

template <typename T>
const SphericalOf<T> SphericalOf<T>::zero =
    SphericalOf<T>(0.0f, AngleOf<T>(0), AngleOf<T>(0));
template <typename T>
const SphericalOf<T> SphericalOf<T>::forward = SphericalOf<T>(1.0f, 0.0f, 0.0f);
template <typename T>
const SphericalOf<T> SphericalOf<T>::back = SphericalOf<T>(1.0f, 180.0f, 0.0f);
template <typename T>
const SphericalOf<T> SphericalOf<T>::right = SphericalOf<T>(1.0f, 90.0f, 0.0f);
template <typename T>
const SphericalOf<T> SphericalOf<T>::left = SphericalOf<T>(1.0f, -90.0f, 0.0f);
template <typename T>
const SphericalOf<T> SphericalOf<T>::up = SphericalOf<T>(1.0f, 0.0f, 90.0f);
template <typename T>
const SphericalOf<T> SphericalOf<T>::down = SphericalOf<T>(1.0f, 0.0f, -90.0f);

template <>
const SphericalOf<signed short> SphericalOf<signed short>::zero =
    SphericalOf(0.0f, AngleOf<signed short>(0), AngleOf<signed short>(0));

template <typename T>
SphericalOf<T> SphericalOf<T>::operator-() const {
  SphericalOf<T> v =
      SphericalOf<T>(this->distance, this->horizontal.ToFloat() + 180.0f,
                     this->vertical.ToFloat() + 180.0f);
  return v;
}

template <typename T>
SphericalOf<T> SphericalOf<T>::operator-(const SphericalOf<T>& s2) const {
  // let's do it the easy way...
  Vector3 v1 = this->ToVector3();
  Vector3 v2 = s2.ToVector3();
  Vector3 v = v1 - v2;
  SphericalOf<T> r = SphericalOf<T>::FromVector3(v);
  return r;
}
template <typename T>
SphericalOf<T> SphericalOf<T>::operator-=(const SphericalOf<T>& v) {
  *this = *this - v;
  return *this;
}

template <typename T>
SphericalOf<T> SphericalOf<T>::operator+(const SphericalOf<T>& s2) const {
  // let's do it the easy way...
  Vector3 v1 = this->ToVector3();
  Vector3 v2 = s2.ToVector3();
  Vector3 v = v1 + v2;
  SphericalOf<T> r = SphericalOf<T>::FromVector3(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);
  */
}
template <typename T>
SphericalOf<T> SphericalOf<T>::operator+=(const SphericalOf<T>& v) {
  *this = *this + v;
  return *this;
}

template <typename T>
SphericalOf<T> SphericalOf<T>::operator*=(float f) {
  this->distance *= f;
  return *this;
}

template <typename T>
SphericalOf<T> SphericalOf<T>::operator/=(float f) {
  this->distance /= f;
  return *this;
}

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

const float epsilon = 1E-05f;

template <typename T>
AngleOf<T> SphericalOf<T>::AngleBetween(const SphericalOf& v1,
                                        const SphericalOf& v2) {
  float denominator = v1.distance * v2.distance;
  if (denominator < epsilon)
    return 0.0f;

  Vector3 v1_3 = v1.ToVector3();
  Vector3 v2_3 = v2.ToVector3();
  // 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;
  Angle r = Vector3::Angle(v1_3, v2_3);
  return AngleOf<T>(r.ToFloat());
}

template <typename T>
SphericalOf<T> SphericalOf<T>::Rotate(const SphericalOf<T>& v,
                                      AngleOf<T> horizontalAngle,
                                      AngleOf<T> verticalAngle) {
  SphericalOf<T> r = SphericalOf(v.distance, v.horizontal + horizontalAngle,
                                 v.vertical + verticalAngle);
  return r;
}
template <typename T>
SphericalOf<T> SphericalOf<T>::RotateHorizontal(const SphericalOf<T>& v,
                                                AngleOf<T> a) {
  SphericalOf<T> r = SphericalOf(v.distance, v.horizontal + a, v.vertical);
  return r;
}
template <typename T>
SphericalOf<T> SphericalOf<T>::RotateVertical(const SphericalOf<T>& v,
                                              AngleOf<T> a) {
  SphericalOf<T> r = SphericalOf(v.distance, v.horizontal, v.vertical + a);
  return r;
}

template class SphericalOf<float>;
template class SphericalOf<signed short>;

//---------------------------------------
/*
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.ToFloat() - 180.0f);
    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) *
                                       Passer::LinearAlgebra::Rad2Deg);
    this->horizontalAngle =
        atan2f(v.Right(), v.Forward()) * Passer::LinearAlgebra::Rad2Deg;
  }
}

const Spherical Spherical::zero = Spherical(0.0f, 0.0f, 0.0f);
const Spherical Spherical::forward = Spherical(1.0f, 0.0f, 0.0f);
const Spherical Spherical::back = Spherical(1.0f, 180.0f, 0.0f);
const Spherical Spherical::right = Spherical(1.0f, 90.0f, 0.0f);
const Spherical Spherical::left = Spherical(1.0f, -90.0f, 0.0f);
const Spherical Spherical::up = Spherical(1.0f, 0.0f, 90.0f);
const Spherical Spherical::down = Spherical(1.0f, 0.0f, -90.0f);

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

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

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

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

Spherical Spherical::operator+(const Spherical& s2) const {
  // let's do it the easy way...
  Vector3 v1 = Vector3(*this);
  Vector3 v2 = Vector3(s2);
  Vector3 v = v1 + v2;
  Spherical r = Spherical(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);

}
Spherical Spherical::operator+=(const Spherical& 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);
// }
Spherical Spherical::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);
// }
Spherical Spherical::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 Spherical::AngleBetween(const Spherical& v1, const Spherical& 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(v1);
  Vector3 v2_3 = Vector3(v2);
  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;
}

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