// Copyright (C) 2026 Kiyotsugu Arai
// SPDX-License-Identifier: LGPL-3.0-or-later

// Quaternion.hpp
// Quaternion template class
//
// q = w + xi + yj + zk (scalar part w, vector part x, y, z)
// Hamilton product: ij=k, jk=i, ki=j, ji=-k, kj=-i, ik=-j
//
// Uses: 3D rotation, spherical interpolation (SLERP), physics simulation

#pragma once

#include <iosfwd>
#include <sstream>
#include <string>
#include <cmath>
#include <concepts>
#include <limits>

#include <math/core/generic_math.hpp>

namespace sangi {

// The ComplexScalar concept is already defined in Complex.hpp, but it is
// redefined here for standalone use of Quaternion
template<typename T>
concept QuaternionScalar = requires(T a, T b) {
    { a + b } -> std::convertible_to<T>;
    { a - b } -> std::convertible_to<T>;
    { a * b } -> std::convertible_to<T>;
    { a / b } -> std::convertible_to<T>;
    { -a } -> std::convertible_to<T>;
    { T(0) };
    { T(1) };
};

// ================================================================
// Quaternion<T> class
// ================================================================
// Memory layout: w (scalar), x, y, z (vector)
// Mathematical notation: q = w + xi + yj + zk

template<QuaternionScalar T>
class Quaternion {
public:
    using value_type = T;  // component type

    T w;  // scalar part
    T x;  // i component
    T y;  // j component
    T z;  // k component

    // ============================================================
    // Constructors
    // ============================================================

    constexpr Quaternion() noexcept(noexcept(T(0)))
        : w(T(0)), x(T(0)), y(T(0)), z(T(0)) {}

    constexpr Quaternion(const T& w_, const T& x_ = T(0),
                         const T& y_ = T(0), const T& z_ = T(0)) noexcept
        : w(w_), x(x_), y(y_), z(z_) {}

    // ============================================================
    // Comparison operators
    // ============================================================

    [[nodiscard]] constexpr bool operator==(const Quaternion& rhs) const {
        return w == rhs.w && x == rhs.x && y == rhs.y && z == rhs.z;
    }
    [[nodiscard]] constexpr bool operator!=(const Quaternion& rhs) const {
        return !(*this == rhs);
    }

    // ============================================================
    // Unary operators
    // ============================================================

    [[nodiscard]] constexpr Quaternion operator+() const { return *this; }
    [[nodiscard]] constexpr Quaternion operator-() const {
        return Quaternion(-w, -x, -y, -z);
    }

    // ============================================================
    // Arithmetic
    // ============================================================

    [[nodiscard]] constexpr Quaternion operator+(const Quaternion& rhs) const {
        return Quaternion(w + rhs.w, x + rhs.x, y + rhs.y, z + rhs.z);
    }

    [[nodiscard]] constexpr Quaternion operator-(const Quaternion& rhs) const {
        return Quaternion(w - rhs.w, x - rhs.x, y - rhs.y, z - rhs.z);
    }

    // Hamilton product (non-commutative)
    [[nodiscard]] constexpr Quaternion operator*(const Quaternion& rhs) const {
        return Quaternion(
            w * rhs.w - x * rhs.x - y * rhs.y - z * rhs.z,
            w * rhs.x + x * rhs.w + y * rhs.z - z * rhs.y,
            w * rhs.y - x * rhs.z + y * rhs.w + z * rhs.x,
            w * rhs.z + x * rhs.y - y * rhs.x + z * rhs.w
        );
    }

    // q1 / q2 = q1 * q2^(-1)
    [[nodiscard]] constexpr Quaternion operator/(const Quaternion& rhs) const {
        return *this * rhs.inverse();
    }

    // Scalar multiplication
    [[nodiscard]] constexpr Quaternion operator*(const T& s) const {
        return Quaternion(w * s, x * s, y * s, z * s);
    }
    [[nodiscard]] constexpr Quaternion operator/(const T& s) const {
        return Quaternion(w / s, x / s, y / s, z / s);
    }
    friend constexpr Quaternion operator*(const T& s, const Quaternion& q) {
        return Quaternion(s * q.w, s * q.x, s * q.y, s * q.z);
    }

    // ============================================================
    // Compound assignment operators
    // ============================================================

    constexpr Quaternion& operator+=(const Quaternion& rhs) {
        w += rhs.w; x += rhs.x; y += rhs.y; z += rhs.z;
        return *this;
    }
    constexpr Quaternion& operator-=(const Quaternion& rhs) {
        w -= rhs.w; x -= rhs.x; y -= rhs.y; z -= rhs.z;
        return *this;
    }
    constexpr Quaternion& operator*=(const Quaternion& rhs) {
        *this = *this * rhs; return *this;
    }
    constexpr Quaternion& operator/=(const Quaternion& rhs) {
        *this = *this / rhs; return *this;
    }
    constexpr Quaternion& operator*=(const T& s) {
        w *= s; x *= s; y *= s; z *= s; return *this;
    }
    constexpr Quaternion& operator/=(const T& s) {
        w /= s; x /= s; y /= s; z /= s; return *this;
    }

    // ============================================================
    // Quaternion-specific operations
    // ============================================================

    // Conjugate: conj(q) = w - xi - yj - zk
    [[nodiscard]] constexpr Quaternion conj() const {
        return Quaternion(w, -x, -y, -z);
    }

    // Squared norm: |q|^2 = w^2 + x^2 + y^2 + z^2
    [[nodiscard]] constexpr T normSq() const {
        return w * w + x * x + y * y + z * z;
    }

    // Norm: |q|
    [[nodiscard]] T norm() const {
        return detail::generic_sqrt(normSq());
    }

    // Whether all components are 0 (true => norm() == 0)
    [[nodiscard]] constexpr bool isZero() const {
        return w == T(0) && x == T(0) && y == T(0) && z == T(0);
    }

    // Whether the imaginary part is entirely 0 (true => real-valued, q = w + 0i + 0j + 0k)
    [[nodiscard]] constexpr bool isReal() const {
        return x == T(0) && y == T(0) && z == T(0);
    }

    // Whether the real part is 0 (true => pure-imaginary quaternion, q = 0 + xi + yj + zk)
    [[nodiscard]] constexpr bool isPureImaginary() const {
        return w == T(0);
    }

    // Inverse: q^(-1) = conj(q) / |q|^2
    [[nodiscard]] constexpr Quaternion inverse() const {
        T ns = normSq();
        if (ns == T(0)) {
            T nan = std::numeric_limits<T>::quiet_NaN();
            return Quaternion(nan, nan, nan, nan);
        }
        T d = T(1) / ns;
        return Quaternion(w * d, -x * d, -y * d, -z * d);
    }

    // Normalize to a unit quaternion
    [[nodiscard]] Quaternion normalized() const {
        T n = norm();
        if (n == T(0)) {
            T nan = std::numeric_limits<T>::quiet_NaN();
            return Quaternion(nan, nan, nan, nan);
        }
        return Quaternion(w / n, x / n, y / n, z / n);
    }

    // ============================================================
    // 3D rotation
    // ============================================================

    // Build a rotation quaternion from an axis vector (ax, ay, az) and an angle [rad]
    // Assumes the axis is already normalized
    static Quaternion fromAxisAngle(const T& ax, const T& ay, const T& az,
                                     const T& angle) {
        T halfAngle = angle / T(2);
        T s = detail::generic_sin(halfAngle);
        return Quaternion(detail::generic_cos(halfAngle), ax * s, ay * s, az * s);
    }

    // Rotate vector (vx, vy, vz): v' = q * v * q^(-1)
    // this must be a unit quaternion
    void rotateVector(T& vx, T& vy, T& vz) const {
        Quaternion v(T(0), vx, vy, vz);
        Quaternion result = *this * v * conj();
        vx = result.x;
        vy = result.y;
        vz = result.z;
    }

    // ============================================================
    // Spherical linear interpolation (SLERP)
    // ============================================================

    // Interpolate between q0 and q1 by t in [0, 1]
    static Quaternion slerp(const Quaternion& q0, const Quaternion& q1, const T& t) {
        T dot = q0.w * q1.w + q0.x * q1.x + q0.y * q1.y + q0.z * q1.z;

        // Take the shortest path: negate q1 when the inner product is negative
        Quaternion q1adj = q1;
        if (dot < T(0)) {
            q1adj = -q1;
            dot = -dot;
        }

        // Fall back to linear interpolation when the quaternions are nearly identical
        if (dot > T(1) - T(1e-6)) {
            Quaternion result = q0 * (T(1) - t) + q1adj * t;
            return result.normalized();
        }

        T theta = detail::generic_acos(dot);
        T sinTheta = detail::generic_sin(theta);
        T w0 = detail::generic_sin((T(1) - t) * theta) / sinTheta;
        T w1 = detail::generic_sin(t * theta) / sinTheta;

        return q0 * w0 + q1adj * w1;
    }

    // ============================================================
    // Exponential / logarithm
    // ============================================================

    // exp(q) = e^w * (cos|v| + v/|v| * sin|v|)  where v = (x,y,z)
    [[nodiscard]] Quaternion exp() const {
        T vNorm = detail::generic_sqrt(x * x + y * y + z * z);
        T ew = detail::generic_exp(w);
        if (vNorm == T(0)) {
            return Quaternion(ew);
        }
        T s = ew * detail::generic_sin(vNorm) / vNorm;
        return Quaternion(ew * detail::generic_cos(vNorm), x * s, y * s, z * s);
    }

    // log(q) = log|q| + v/|v| * acos(w/|q|)
    [[nodiscard]] Quaternion log() const {
        T n = norm();
        T vNorm = detail::generic_sqrt(x * x + y * y + z * z);
        if (vNorm == T(0)) {
            return Quaternion(detail::generic_log(n));
        }
        T angle = detail::generic_acos(w / n) / vNorm;
        return Quaternion(detail::generic_log(n), x * angle, y * angle, z * angle);
    }

    // ============================================================
    // String conversion / output
    // ============================================================

    [[nodiscard]] std::string toString() const {
        auto fmt = [](const T& v) -> std::string {
            std::ostringstream os;
            os << ((v == T(0)) ? T(0) : v);
            return os.str();
        };

        std::string sw = fmt(w), sx = fmt(x), sy = fmt(y), sz = fmt(z);
        auto isZero = [](const std::string& s) { return s == "0" || s == "0."; };

        // If the imaginary part is all 0, output only the scalar part
        if (isZero(sx) && isZero(sy) && isZero(sz))
            return sw;

        std::string s;
        if (!isZero(sw)) s = sw;

        auto appendComponent = [&](const std::string& val, const char* unit) {
            if (isZero(val)) return;
            if (val == "1") { s += "+"; s += unit; }
            else if (val == "-1") { s += "-"; s += unit; }
            else if (!val.empty() && val[0] == '-') { s += val; s += unit; }
            else { s += "+"; s += val; s += unit; }
        };

        appendComponent(sx, "i");
        appendComponent(sy, "j");
        appendComponent(sz, "k");

        // Strip a leading '+'
        if (!s.empty() && s[0] == '+') s.erase(0, 1);

        return s;
    }

    friend std::ostream& operator<<(std::ostream& os, const Quaternion& q) {
        return os << q.toString();
    }
};

// ================================================================
// Free functions
// ================================================================

template<QuaternionScalar T>
[[nodiscard]] constexpr Quaternion<T> conj(const Quaternion<T>& q) { return q.conj(); }

template<QuaternionScalar T>
[[nodiscard]] constexpr T normSq(const Quaternion<T>& q) { return q.normSq(); }

template<QuaternionScalar T>
[[nodiscard]] T norm(const Quaternion<T>& q) { return q.norm(); }

template<QuaternionScalar T>
[[nodiscard]] constexpr Quaternion<T> inverse(const Quaternion<T>& q) { return q.inverse(); }

template<QuaternionScalar T>
[[nodiscard]] Quaternion<T> normalize(const Quaternion<T>& q) { return q.normalized(); }

template<QuaternionScalar T>
[[nodiscard]] Quaternion<T> exp(const Quaternion<T>& q) { return q.exp(); }

template<QuaternionScalar T>
[[nodiscard]] Quaternion<T> log(const Quaternion<T>& q) { return q.log(); }

template<QuaternionScalar T>
[[nodiscard]] Quaternion<T> slerp(const Quaternion<T>& q0, const Quaternion<T>& q1, const T& t) {
    return Quaternion<T>::slerp(q0, q1, t);
}

// Type traits
template<typename T>
struct is_quaternion : std::false_type {};

template<QuaternionScalar T>
struct is_quaternion<Quaternion<T>> : std::true_type {};

template<typename T>
inline constexpr bool is_quaternion_v = is_quaternion<T>::value;

} // namespace sangi