#include "Spherical.h"

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

#include <math.h>

template <typename T> SphericalOf<T>::SphericalOf() {
  this->distance = 0.0f;
  this->direction = DirectionOf<T>();
}

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

template <typename T>
SphericalOf<T>::SphericalOf(float distance, DirectionOf<T> direction) {
  if (distance < 0) {
    this->distance = -distance;
    this->direction = -direction;
  } else {
    this->distance = distance;
    this->direction = direction;
  }
}

template <typename T>
SphericalOf<T> SphericalOf<T>::FromPolar(PolarOf<T> polar) {
  AngleOf<T> horizontal = polar.angle;
  AngleOf<T> vertical = AngleOf<T>();
  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, AngleOf<T>(), AngleOf<T>());
  } else {
    AngleOf<T> verticalAngle =
        AngleOf<T>::Radians((pi / 2 - acosf(v.Up() / distance)));
    AngleOf<T> horizontalAngle =
        AngleOf<T>::Radians(atan2f(v.Right(), v.Forward()));
    return SphericalOf(distance, horizontalAngle, verticalAngle);
  }
}

/**
 * @brief Converts spherical coordinates to a 3D vector.
 *
 * This function converts the spherical coordinates represented by the
 * SphericalOf object to a 3D vector (Vector3). The conversion is based
 * on the distance and direction (vertical and horizontal angles) of the
 * spherical coordinates.
 *
 * @tparam T The type of the distance and direction values.
 * @return Vector3 The 3D vector representation of the spherical coordinates.
 */
template <typename T> Vector3 SphericalOf<T>::ToVector3() const {
  float verticalRad = (pi / 2) - this->direction.vertical.InRadians();
  float horizontalRad = this->direction.horizontal.InRadians();

  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>(), AngleOf<T>());
template <typename T>
const SphericalOf<T> SphericalOf<T>::forward =
    SphericalOf<T>(1.0f, AngleOf<T>(), AngleOf<T>());
template <typename T>
const SphericalOf<T> SphericalOf<T>::back =
    SphericalOf<T>(1.0f, AngleOf<T>::Degrees(180), AngleOf<T>());
template <typename T>
const SphericalOf<T> SphericalOf<T>::right =
    SphericalOf<T>(1.0f, AngleOf<T>::Degrees(90), AngleOf<T>());
template <typename T>
const SphericalOf<T> SphericalOf<T>::left =
    SphericalOf<T>(1.0f, AngleOf<T>::Degrees(-90), AngleOf<T>());
template <typename T>
const SphericalOf<T> SphericalOf<T>::up =
    SphericalOf<T>(1.0f, AngleOf<T>(), AngleOf<T>::Degrees(90));
template <typename T>
const SphericalOf<T> SphericalOf<T>::down =
    SphericalOf<T>(1.0f, AngleOf<T>(), AngleOf<T>::Degrees(-90));

template <typename T>
SphericalOf<T> SphericalOf<T>::WithDistance(float distance) {
  SphericalOf<T> v = SphericalOf<T>(distance, this->direction);
  return SphericalOf<T>();
}

template <typename T> SphericalOf<T> SphericalOf<T>::operator-() const {
  SphericalOf<T> v = SphericalOf<T>(
      this->distance, this->direction.horizontal + AngleOf<T>::Degrees(180),
      this->direction.vertical + AngleOf<T>::Degrees(180));
  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>
float SphericalOf<T>::DistanceBetween(const SphericalOf<T> &v1,
                                      const SphericalOf<T> &v2) {
  // SphericalOf<T> difference = v1 - v2;
  // return difference.distance;
  Vector3 vec1 = v1.ToVector3();
  Vector3 vec2 = v2.ToVector3();
  float distance = Vector3::Distance(vec1, vec2);
  return distance;
}

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>::Degrees(r.InDegrees());
}

template <typename T>
AngleOf<T> Passer::LinearAlgebra::SphericalOf<T>::SignedAngleBetween(
    const SphericalOf<T> &v1, const SphericalOf<T> &v2,
    const SphericalOf<T> &axis) {
  Vector3 v1_vector = v1.ToVector3();
  Vector3 v2_vector = v2.ToVector3();
  Vector3 axis_vector = axis.ToVector3();
  Angle r = Vector3::SignedAngle(v1_vector, v2_vector, axis_vector);
  return AngleOf<T>::Degrees(r.InDegrees());
}

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.direction.horizontal + horizontalAngle,
                  v.direction.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.direction.horizontal + a, v.direction.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.direction.horizontal, v.direction.vertical + a);
  return r;
}

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