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

// Quaternion.hpp
// 四元数テンプレートクラス
// lib++20 四元数.h からの移植 (2026-02-15)
//
// q = w + xi + yj + zk (スカラー部 w, ベクトル部 x,y,z)
// Hamilton 積: ij=k, jk=i, ki=j, ji=-k, kj=-i, ik=-j
//
// 用途: 3D 回転、球面補間 (SLERP)、物理シミュレーション

#pragma once

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

namespace calx {

// ComplexScalar concept は Complex.hpp で定義済みだが、
// 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> クラス
// ================================================================
// メモリレイアウト: w (スカラー), x, y, z (ベクトル)
// 数学表記: q = w + xi + yj + zk

template<QuaternionScalar T>
class Quaternion {
public:
    using value_type = T;  // 成分の型

    T w;  // スカラー部
    T x;  // i 成分
    T y;  // j 成分
    T z;  // k 成分

    // ============================================================
    // コンストラクタ
    // ============================================================

    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_) {}

    // ============================================================
    // 比較演算子
    // ============================================================

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

    // ============================================================
    // 単項演算子
    // ============================================================

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

    // ============================================================
    // 四則演算
    // ============================================================

    [[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 積 (非可換)
    [[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();
    }

    // スカラー乗算
    [[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);
    }

    // ============================================================
    // 複合代入演算子
    // ============================================================

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

    // ============================================================
    // 四元数固有の操作
    // ============================================================

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

    // ノルムの二乗: |q|^2 = w^2 + x^2 + y^2 + z^2
    [[nodiscard]] constexpr T normSq() const {
        return w * w + x * x + y * y + z * z;
    }

    // ノルム: |q|
    [[nodiscard]] T norm() const {
        using std::sqrt;
        return sqrt(normSq());
    }

    // 逆元: 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);
    }

    // 単位四元数に正規化
    [[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 回転
    // ============================================================

    // 軸ベクトル (ax, ay, az) と角度 angle [rad] から回転四元数を作る
    // 軸は正規化されている前提
    static Quaternion fromAxisAngle(const T& ax, const T& ay, const T& az,
                                     const T& angle) {
        using std::sin; using std::cos;
        T halfAngle = angle / T(2);
        T s = sin(halfAngle);
        return Quaternion(cos(halfAngle), ax * s, ay * s, az * s);
    }

    // ベクトル (vx, vy, vz) を回転: v' = q * v * q^(-1)
    // this は単位四元数であること
    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;
    }

    // ============================================================
    // 球面線形補間 (SLERP)
    // ============================================================

    // q0 と q1 を t (0~1) で補間
    static Quaternion slerp(const Quaternion& q0, const Quaternion& q1, const T& t) {
        using std::acos; using std::sin; using std::abs;

        T dot = q0.w * q1.w + q0.x * q1.x + q0.y * q1.y + q0.z * q1.z;

        // 最短経路を取るため、内積が負なら q1 を反転
        Quaternion q1adj = q1;
        if (dot < T(0)) {
            q1adj = -q1;
            dot = -dot;
        }

        // ほぼ同じ四元数なら線形補間にフォールバック
        if (dot > T(1) - T(1e-6)) {
            Quaternion result = q0 * (T(1) - t) + q1adj * t;
            return result.normalized();
        }

        T theta = acos(dot);
        T sinTheta = sin(theta);
        T w0 = sin((T(1) - t) * theta) / sinTheta;
        T w1 = sin(t * theta) / sinTheta;

        return q0 * w0 + q1adj * w1;
    }

    // ============================================================
    // 指数・対数
    // ============================================================

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

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

    // ============================================================
    // 文字列変換・出力
    // ============================================================

    [[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."; };

        // 全て虚部が 0 ならスカラーのみ
        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");

        // 先頭の '+' を除去
        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();
    }
};

// ================================================================
// 自由関数
// ================================================================

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

// 型特性
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 calx
