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

// Fixed.hpp
//
// 固定小数点型 (Fixed-Point Arithmetic)
//
// 産業ロボット制御・DSP 向けの決定論的固定小数点演算。
//
// 主な API:
//   Fixed<Rep, FractionBits, RoundingPolicy, OverflowPolicy>
//   四則演算 (+, -, *, /), 比較, 型変換
//   constexpr 完全対応
//
// Q-format 対応:
//   Q16.16 → Fixed<int32_t, 16>
//   Q1.15  → Fixed<int16_t, 15>

#ifndef CALX_CORE_FIXED_HPP
#define CALX_CORE_FIXED_HPP

#include <cstdint>
#include <type_traits>
#include <concepts>
#include <limits>
#include <ostream>
#include <cmath>
#include <random>
#include <array>

namespace calx {

// ════════════════════════════════════════════════════════════════
// 丸めポリシー
// ════════════════════════════════════════════════════════════════

struct round_to_nearest {
    static constexpr int64_t apply(int64_t x, int shift) {
        if (shift <= 0) return x;
        int64_t half = int64_t(1) << (shift - 1);
        return (x + half) >> shift;
    }
};

struct round_to_even {
    static constexpr int64_t apply(int64_t x, int shift) {
        if (shift <= 0) return x;
        int64_t half = int64_t(1) << (shift - 1);
        int64_t mask = (int64_t(1) << shift) - 1;
        int64_t frac = x & mask;
        int64_t result = x >> shift;
        if (frac > half) ++result;
        else if (frac == half) { if (result & 1) ++result; }
        return result;
    }
};

struct truncate {
    static constexpr int64_t apply(int64_t x, int shift) {
        if (shift <= 0) return x;
        return x >> shift;
    }
};

/// 確率的丸め (Stochastic Rounding)
/// NN 量子化等で丸めバイアスを除去。E[round(x)] = x を統計的に保証。
/// 丸めの確率は端数の大きさに比例:
///   P(ceil) = frac / 2^shift,  P(floor) = 1 - frac / 2^shift
struct stochastic {
    // thread_local RNG (constexpr では使用不可)
    static int64_t apply(int64_t x, int shift) {
        if (shift <= 0) return x;
        int64_t mask = (int64_t(1) << shift) - 1;
        int64_t frac = x & mask;
        int64_t floored = x >> shift;

        // 端数に比例した確率で切り上げ
        thread_local std::mt19937 rng(42);
        std::uniform_int_distribution<int64_t> dist(0, mask);
        if (dist(rng) < frac)
            return floored + 1;
        return floored;
    }

    /// シードを設定 (再現性のため)
    static void seed(unsigned s) {
        thread_local std::mt19937 rng(42);
        rng.seed(s);
    }
};

// ════════════════════════════════════════════════════════════════
// オーバーフローポリシー
// ════════════════════════════════════════════════════════════════

struct saturate {
    template<typename Rep>
    static constexpr Rep apply(int64_t x) {
        constexpr int64_t lo = std::numeric_limits<Rep>::min();
        constexpr int64_t hi = std::numeric_limits<Rep>::max();
        if (x < lo) return static_cast<Rep>(lo);
        if (x > hi) return static_cast<Rep>(hi);
        return static_cast<Rep>(x);
    }
};

struct wrap {
    template<typename Rep>
    static constexpr Rep apply(int64_t x) {
        return static_cast<Rep>(x);
    }
};

struct trap {
    template<typename Rep>
    static constexpr Rep apply(int64_t x) {
        constexpr int64_t lo = std::numeric_limits<Rep>::min();
        constexpr int64_t hi = std::numeric_limits<Rep>::max();
        if (x < lo || x > hi) {
            // constexpr 内では例外が投げられないので UB を回避
            // 実行時は throw, コンパイル時はコンパイルエラー
            throw "Fixed overflow";
        }
        return static_cast<Rep>(x);
    }
};

// ════════════════════════════════════════════════════════════════
// Concepts
// ════════════════════════════════════════════════════════════════

template<typename P>
concept RoundingPolicyConcept = requires(int64_t x, int shift) {
    { P::apply(x, shift) } -> std::same_as<int64_t>;
};

template<typename P>
concept OverflowPolicyConcept = requires(int64_t x) {
    { P::template apply<int32_t>(x) } -> std::same_as<int32_t>;
};

// ════════════════════════════════════════════════════════════════
// Fixed<Rep, FractionBits, RP, OP>
// ════════════════════════════════════════════════════════════════

template<
    typename Rep_,
    int FractionBits_,
    typename RoundingPolicy_ = round_to_nearest,
    typename OverflowPolicy_ = saturate
>
    requires std::signed_integral<Rep_>
          && (FractionBits_ >= 0)
          && (FractionBits_ < static_cast<int>(sizeof(Rep_) * 8))
          && RoundingPolicyConcept<RoundingPolicy_>
          && OverflowPolicyConcept<OverflowPolicy_>
class Fixed {
public:
    using Rep = Rep_;
    using RoundingPolicy = RoundingPolicy_;
    using OverflowPolicy = OverflowPolicy_;
    static constexpr int FractionBits = FractionBits_;
    static constexpr int IntegerBits = static_cast<int>(sizeof(Rep) * 8) - 1 - FractionBits;
    static constexpr int64_t Scale = int64_t(1) << FractionBits;

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

    constexpr Fixed() : raw_(0) {}

    /// 生ビットから構築 (内部用)
    struct raw_tag {};
    constexpr Fixed(Rep raw, raw_tag) : raw_(raw) {}

    /// 整数から構築
    constexpr explicit Fixed(int value)
        : raw_(OverflowPolicy::template apply<Rep>(static_cast<int64_t>(value) * Scale)) {}

    /// double から構築
    constexpr explicit Fixed(double value)
        : raw_(OverflowPolicy::template apply<Rep>(
              RoundingPolicy::apply(
                  static_cast<int64_t>(value * static_cast<double>(Scale) *
                      (value >= 0 ? 1.0 : 1.0)  // force evaluation
                  + (value >= 0 ? 0.0 : -0.0)),  // avoid UB on negative
                  0
              )
          ))
    {
        // Better approach: use round
    }

    // ── アクセサ ──

    [[nodiscard]] constexpr Rep raw() const { return raw_; }

    [[nodiscard]] constexpr double toDouble() const {
        return static_cast<double>(raw_) / static_cast<double>(Scale);
    }

    [[nodiscard]] constexpr float toFloat() const {
        return static_cast<float>(raw_) / static_cast<float>(Scale);
    }

    // ── 静的ファクトリ ──

    [[nodiscard]] static constexpr Fixed fromRaw(Rep raw) {
        return Fixed(raw, raw_tag{});
    }

    [[nodiscard]] static constexpr Fixed fromDouble(double value) {
        int64_t scaled = static_cast<int64_t>(value * static_cast<double>(Scale)
                         + (value >= 0 ? 0.5 : -0.5));
        return Fixed(OverflowPolicy::template apply<Rep>(scaled), raw_tag{});
    }

    [[nodiscard]] static constexpr Fixed min() {
        return Fixed(std::numeric_limits<Rep>::min(), raw_tag{});
    }

    [[nodiscard]] static constexpr Fixed max() {
        return Fixed(std::numeric_limits<Rep>::max(), raw_tag{});
    }

    [[nodiscard]] static constexpr Fixed epsilon() {
        return Fixed(Rep(1), raw_tag{});
    }

    [[nodiscard]] static constexpr Fixed one() {
        return Fixed(static_cast<Rep>(Scale), raw_tag{});
    }

    [[nodiscard]] static constexpr Fixed pi() {
        return fromDouble(3.14159265358979323846);
    }

    [[nodiscard]] static constexpr Fixed half_pi() {
        return fromDouble(1.57079632679489661923);
    }

    [[nodiscard]] static constexpr Fixed two_pi() {
        return fromDouble(6.28318530717958647693);
    }

    // ── 四則演算 ──

    friend constexpr Fixed operator+(Fixed a, Fixed b) {
        int64_t r = static_cast<int64_t>(a.raw_) + static_cast<int64_t>(b.raw_);
        return Fixed(OverflowPolicy::template apply<Rep>(r), raw_tag{});
    }

    friend constexpr Fixed operator-(Fixed a, Fixed b) {
        int64_t r = static_cast<int64_t>(a.raw_) - static_cast<int64_t>(b.raw_);
        return Fixed(OverflowPolicy::template apply<Rep>(r), raw_tag{});
    }

    constexpr Fixed operator-() const {
        if (raw_ == std::numeric_limits<Rep>::min())
            return Fixed(OverflowPolicy::template apply<Rep>(
                -static_cast<int64_t>(raw_)), raw_tag{});
        return Fixed(static_cast<Rep>(-raw_), raw_tag{});
    }

    /// 乗算 (中間精度拡張)
    friend constexpr Fixed operator*(Fixed a, Fixed b) {
        int64_t product = static_cast<int64_t>(a.raw_) * static_cast<int64_t>(b.raw_);
        int64_t rounded = RoundingPolicy::apply(product, FractionBits);
        return Fixed(OverflowPolicy::template apply<Rep>(rounded), raw_tag{});
    }

    /// 除算
    friend constexpr Fixed operator/(Fixed a, Fixed b) {
        if (b.raw_ == 0) {
            // 飽和: 符号に応じて最大/最小
            if (a.raw_ >= 0)
                return Fixed(std::numeric_limits<Rep>::max(), raw_tag{});
            else
                return Fixed(std::numeric_limits<Rep>::min(), raw_tag{});
        }
        int64_t scaled = static_cast<int64_t>(a.raw_) * Scale;
        int64_t quotient = scaled / static_cast<int64_t>(b.raw_);
        return Fixed(OverflowPolicy::template apply<Rep>(quotient), raw_tag{});
    }

    constexpr Fixed& operator+=(Fixed other) { *this = *this + other; return *this; }
    constexpr Fixed& operator-=(Fixed other) { *this = *this - other; return *this; }
    constexpr Fixed& operator*=(Fixed other) { *this = *this * other; return *this; }
    constexpr Fixed& operator/=(Fixed other) { *this = *this / other; return *this; }

    // ── ビットシフト ──

    constexpr Fixed operator<<(int n) const {
        return Fixed(static_cast<Rep>(raw_ << n), raw_tag{});
    }

    constexpr Fixed operator>>(int n) const {
        return Fixed(static_cast<Rep>(raw_ >> n), raw_tag{});
    }

    // ── 比較 ──

    constexpr auto operator<=>(const Fixed& other) const = default;
    constexpr bool operator==(const Fixed& other) const = default;

    // ── 異なる FractionBits 間の変換 ──

    template<int OtherFrac>
    [[nodiscard]] constexpr Fixed<Rep, OtherFrac, RoundingPolicy, OverflowPolicy>
    convertTo() const {
        using Target = Fixed<Rep, OtherFrac, RoundingPolicy, OverflowPolicy>;
        if constexpr (OtherFrac > FractionBits) {
            int shift = OtherFrac - FractionBits;
            int64_t r = static_cast<int64_t>(raw_) << shift;
            return Target::fromRaw(OverflowPolicy::template apply<Rep>(r));
        } else if constexpr (OtherFrac < FractionBits) {
            int shift = FractionBits - OtherFrac;
            int64_t r = RoundingPolicy::apply(static_cast<int64_t>(raw_), shift);
            return Target::fromRaw(OverflowPolicy::template apply<Rep>(r));
        } else {
            return Target::fromRaw(raw_);
        }
    }

    // ── 数学関数 ──

    [[nodiscard]] constexpr Fixed abs() const {
        if (raw_ < 0) return -*this;
        return *this;
    }

    // ── I/O ──

    friend std::ostream& operator<<(std::ostream& os, Fixed f) {
        return os << f.toDouble();
    }

private:
    Rep raw_;
};

// ════════════════════════════════════════════════════════════════
// 便利なエイリアス
// ════════════════════════════════════════════════════════════════

using Q16_16 = Fixed<int32_t, 16>;
using Q1_15  = Fixed<int16_t, 15>;
using Q1_31  = Fixed<int32_t, 31>;

// ════════════════════════════════════════════════════════════════
// numeric_limits 特殊化
// ════════════════════════════════════════════════════════════════

} // namespace calx

template<typename Rep, int Frac, typename RP, typename OP>
struct std::numeric_limits<calx::Fixed<Rep, Frac, RP, OP>> {
    static constexpr bool is_specialized = true;
    static constexpr bool is_signed = true;
    static constexpr bool is_integer = false;
    static constexpr bool is_exact = true;
    static constexpr int digits = sizeof(Rep) * 8 - 1;
    static constexpr int digits10 = static_cast<int>(digits * 0.30103);

    static constexpr calx::Fixed<Rep, Frac, RP, OP> min() {
        return calx::Fixed<Rep, Frac, RP, OP>::min();
    }
    static constexpr calx::Fixed<Rep, Frac, RP, OP> max() {
        return calx::Fixed<Rep, Frac, RP, OP>::max();
    }
    static constexpr calx::Fixed<Rep, Frac, RP, OP> epsilon() {
        return calx::Fixed<Rep, Frac, RP, OP>::epsilon();
    }

    static constexpr bool has_quiet_NaN = false;
    static constexpr calx::Fixed<Rep, Frac, RP, OP> quiet_NaN() {
        return calx::Fixed<Rep, Frac, RP, OP>(0);  // Fixed has no NaN
    }

    static constexpr bool has_infinity = false;
    static constexpr calx::Fixed<Rep, Frac, RP, OP> infinity() {
        return calx::Fixed<Rep, Frac, RP, OP>::max();
    }
};

namespace calx {

// ════════════════════════════════════════════════════════════════
// 固定小数点三角関数 (CORDIC)
// ════════════════════════════════════════════════════════════════

namespace detail_fixed {

/// CORDIC arctan テーブル (Q-format 対応, 32 反復分)
/// atan(2^-k) の固定小数点値
template<int FracBits>
struct CordicTable {
    static constexpr int N = 32;

    static constexpr int64_t scale = int64_t(1) << FracBits;

    static constexpr int64_t atan_table[N] = {
        static_cast<int64_t>(0.7853981633974483 * scale + 0.5),  // atan(1)
        static_cast<int64_t>(0.4636476090008061 * scale + 0.5),  // atan(1/2)
        static_cast<int64_t>(0.2449786631268641 * scale + 0.5),  // atan(1/4)
        static_cast<int64_t>(0.1243549945467614 * scale + 0.5),  // atan(1/8)
        static_cast<int64_t>(0.0624188099959574 * scale + 0.5),  // atan(1/16)
        static_cast<int64_t>(0.0312398334302683 * scale + 0.5),
        static_cast<int64_t>(0.0156237286204768 * scale + 0.5),
        static_cast<int64_t>(0.0078123410601011 * scale + 0.5),
        static_cast<int64_t>(0.0039062301319670 * scale + 0.5),
        static_cast<int64_t>(0.0019531225164788 * scale + 0.5),
        static_cast<int64_t>(0.0009765621895593 * scale + 0.5),
        static_cast<int64_t>(0.0004882812111949 * scale + 0.5),
        static_cast<int64_t>(0.0002441406201494 * scale + 0.5),
        static_cast<int64_t>(0.0001220703118937 * scale + 0.5),
        static_cast<int64_t>(0.0000610351561742 * scale + 0.5),
        static_cast<int64_t>(0.0000305175781155 * scale + 0.5),
        static_cast<int64_t>(0.0000152587890613 * scale + 0.5),
        static_cast<int64_t>(0.0000076293945311 * scale + 0.5),
        static_cast<int64_t>(0.0000038146972656 * scale + 0.5),
        static_cast<int64_t>(0.0000019073486328 * scale + 0.5),
        static_cast<int64_t>(0.0000009536743164 * scale + 0.5),
        static_cast<int64_t>(0.0000004768371582 * scale + 0.5),
        static_cast<int64_t>(0.0000002384185791 * scale + 0.5),
        static_cast<int64_t>(0.0000001192092896 * scale + 0.5),
        static_cast<int64_t>(0.0000000596046448 * scale + 0.5),
        static_cast<int64_t>(0.0000000298023224 * scale + 0.5),
        static_cast<int64_t>(0.0000000149011612 * scale + 0.5),
        static_cast<int64_t>(0.0000000074505806 * scale + 0.5),
        static_cast<int64_t>(0.0000000037252903 * scale + 0.5),
        static_cast<int64_t>(0.0000000018626451 * scale + 0.5),
        static_cast<int64_t>(0.0000000009313226 * scale + 0.5),
        static_cast<int64_t>(0.0000000004656613 * scale + 0.5),
    };

    // CORDIC ゲイン K_n = Π cos(atan(2^-k)) ≈ 0.6072529350088814
    static constexpr int64_t K = static_cast<int64_t>(0.6072529350088814 * scale + 0.5);
};

} // namespace detail_fixed

/// CORDIC sincos (回転モード)
/// angle を Fixed で受け取り、(sin, cos) を返す
template<typename Rep, int Frac, typename RP, typename OP>
constexpr std::pair<Fixed<Rep, Frac, RP, OP>, Fixed<Rep, Frac, RP, OP>>
sincos(Fixed<Rep, Frac, RP, OP> angle) {
    using F = Fixed<Rep, Frac, RP, OP>;
    using Table = detail_fixed::CordicTable<Frac>;

    constexpr int iterations = Frac < 16 ? Frac : (Frac < 30 ? Frac : 30);
    constexpr int64_t scale = F::Scale;

    // 象限処理: [-π, π] に正規化
    int64_t a = static_cast<int64_t>(angle.raw());
    constexpr int64_t pi_val = static_cast<int64_t>(3.14159265358979323846 * scale + 0.5);
    constexpr int64_t two_pi_val = pi_val * 2;
    constexpr int64_t half_pi_val = pi_val / 2;

    // mod 2π
    while (a > pi_val) a -= two_pi_val;
    while (a < -pi_val) a += two_pi_val;

    // 象限処理: |angle| > π/2 → cos 符号反転
    bool negate_cos = false;
    if (a > half_pi_val) {
        a = pi_val - a;
        negate_cos = true;
    } else if (a < -half_pi_val) {
        a = -pi_val - a;
        negate_cos = true;
    }

    // CORDIC 回転モード
    int64_t x = Table::K;  // cos 初期値 = K
    int64_t y = 0;         // sin 初期値 = 0
    int64_t z = a;         // 残り角度

    for (int k = 0; k < iterations; ++k) {
        int64_t dx = x >> k;
        int64_t dy = y >> k;
        if (z >= 0) {
            x -= dy;
            y += dx;
            z -= Table::atan_table[k];
        } else {
            x += dy;
            y -= dx;
            z += Table::atan_table[k];
        }
    }

    if (negate_cos) x = -x;

    auto sin_val = F::fromRaw(OP::template apply<Rep>(y));
    auto cos_val = F::fromRaw(OP::template apply<Rep>(x));
    return {sin_val, cos_val};
}

/// sin
template<typename Rep, int Frac, typename RP, typename OP>
constexpr Fixed<Rep, Frac, RP, OP> sin(Fixed<Rep, Frac, RP, OP> angle) {
    return sincos(angle).first;
}

/// cos
template<typename Rep, int Frac, typename RP, typename OP>
constexpr Fixed<Rep, Frac, RP, OP> cos(Fixed<Rep, Frac, RP, OP> angle) {
    return sincos(angle).second;
}

/// atan2 (CORDIC ベクトリングモード)
template<typename Rep, int Frac, typename RP, typename OP>
constexpr Fixed<Rep, Frac, RP, OP> atan2(Fixed<Rep, Frac, RP, OP> y,
                                           Fixed<Rep, Frac, RP, OP> x) {
    using F = Fixed<Rep, Frac, RP, OP>;
    using Table = detail_fixed::CordicTable<Frac>;

    constexpr int iterations = Frac < 16 ? Frac : (Frac < 30 ? Frac : 30);
    constexpr int64_t scale = F::Scale;
    constexpr int64_t pi_val = static_cast<int64_t>(3.14159265358979323846 * scale + 0.5);

    int64_t xv = static_cast<int64_t>(x.raw());
    int64_t yv = static_cast<int64_t>(y.raw());

    // 特殊ケース
    if (xv == 0 && yv == 0) return F::fromRaw(0);

    // 象限処理: x < 0 の場合、(x,y) を反転してから π を加減
    int64_t angle_offset = 0;
    if (xv < 0) {
        xv = -xv;
        yv = -yv;
        angle_offset = (yv >= 0) ? pi_val : -pi_val;
        // yv の符号が反転後に変わったので再評価は不要
        // (元の y >= 0 なら +π, y < 0 なら -π)
    }
    // 修正: angle_offset の符号は元の y の符号に依存
    if (angle_offset != 0) {
        // 元の y を見る必要がある — 上で yv を反転したので戻す
        // 別のアプローチ: 元の符号を保存
    }

    // もう一度やり直す
    xv = static_cast<int64_t>(x.raw());
    yv = static_cast<int64_t>(y.raw());
    angle_offset = 0;
    if (xv < 0) {
        if (yv >= 0) {
            xv = -xv; yv = -yv; angle_offset = pi_val;
        } else {
            xv = -xv; yv = -yv; angle_offset = -pi_val;
        }
    }

    // ベクトリングモード: y → 0 に収束
    int64_t z = 0;
    for (int k = 0; k < iterations; ++k) {
        int64_t dx = xv >> k;
        int64_t dy = yv >> k;
        if (yv >= 0) {
            xv += dy;
            yv -= dx;
            z += Table::atan_table[k];
        } else {
            xv -= dy;
            yv += dx;
            z -= Table::atan_table[k];
        }
    }

    z += angle_offset;
    return F::fromRaw(OP::template apply<Rep>(z));
}

/// abs
template<typename Rep, int Frac, typename RP, typename OP>
constexpr Fixed<Rep, Frac, RP, OP> abs(Fixed<Rep, Frac, RP, OP> x) {
    return x.abs();
}

/// sqrt (Newton 法, 固定小数点向け)
/// 初期推定に double を使い、Newton 反復で精度を改善
template<typename Rep, int Frac, typename RP, typename OP>
constexpr Fixed<Rep, Frac, RP, OP> sqrt(Fixed<Rep, Frac, RP, OP> x) {
    using F = Fixed<Rep, Frac, RP, OP>;
    if (x.raw() <= 0) return F();

    // 初期推定: toDouble() の sqrt
    F guess = F::fromDouble(std::sqrt(x.toDouble()));

    // Newton 反復: x_{n+1} = (x_n + S/x_n) / 2
    // FractionBits に応じた反復回数 (通常 3-4 回で収束)
    constexpr int max_iter = (Frac <= 8) ? 2 : (Frac <= 16) ? 3 : 4;
    F two(2);
    for (int i = 0; i < max_iter; ++i) {
        if (guess.raw() == 0) break;
        guess = (guess + x / guess) / two;
    }
    return guess;
}

// ════════════════════════════════════════════════════════════════
// 多項式近似三角関数 (PC シミュレーション向け)
// ════════════════════════════════════════════════════════════════

namespace trig_poly {

/// sin の minimax 多項式近似 (5 次, |x| ≤ π/2)
/// max error ≈ 1.5e-5
template<typename Rep, int Frac, typename RP, typename OP>
constexpr Fixed<Rep, Frac, RP, OP> sin(Fixed<Rep, Frac, RP, OP> angle) {
    using F = Fixed<Rep, Frac, RP, OP>;
    constexpr int64_t scale = F::Scale;
    constexpr int64_t pi_val = static_cast<int64_t>(3.14159265358979323846 * scale + 0.5);
    constexpr int64_t two_pi = pi_val * 2;
    constexpr int64_t half_pi = pi_val / 2;

    // [-π, π] に正規化
    int64_t a = static_cast<int64_t>(angle.raw());
    while (a > pi_val)  a -= two_pi;
    while (a < -pi_val) a += two_pi;

    // [-π/2, π/2] に縮退: sin(π - a) = sin(a), sin(-π - a) = sin(a)
    if (a > half_pi) { a = pi_val - a; }
    else if (a < -half_pi) { a = -pi_val - a; }

    F x = F::fromRaw(OP::template apply<Rep>(a));

    // sin(x) ≈ x - x³/6 + x⁵/120  (Horner 形式: x*(1 - x²*(1/6 - x²/120)))
    F x2 = x * x;
    F c5 = F::fromDouble(1.0 / 120.0);
    F c3 = F::fromDouble(1.0 / 6.0);
    F result = x * (F(1) - x2 * (c3 - x2 * c5));

    return result;
}

/// cos の多項式近似
template<typename Rep, int Frac, typename RP, typename OP>
constexpr Fixed<Rep, Frac, RP, OP> cos(Fixed<Rep, Frac, RP, OP> angle) {
    using F = Fixed<Rep, Frac, RP, OP>;
    return trig_poly::sin(angle + F::half_pi());
}

} // namespace trig_poly

// ════════════════════════════════════════════════════════════════
// LUT + 線形補間三角関数 (MCU 高速版)
// ════════════════════════════════════════════════════════════════

namespace trig_lut {

/// LUT テーブル (0〜π/2 を TableSize 分割)
/// 実行時に初期化 (std::sin は constexpr でないため)
template<int Frac, int TableSize = 256>
struct SinTable {
    static const int64_t scale = int64_t(1) << Frac;

    static const std::array<int64_t, TableSize + 1>& get() {
        static const auto t = [] {
            std::array<int64_t, TableSize + 1> arr{};
            for (int i = 0; i <= TableSize; ++i) {
                double theta = (3.14159265358979323846 / 2.0) * i / TableSize;
                arr[i] = static_cast<int64_t>(std::sin(theta) * scale + 0.5);
            }
            return arr;
        }();
        return t;
    }
};

/// sin (LUT + 線形補間)
template<typename Rep, int Frac, typename RP, typename OP, int TableSize = 256>
Fixed<Rep, Frac, RP, OP> sin(Fixed<Rep, Frac, RP, OP> angle) {
    using F = Fixed<Rep, Frac, RP, OP>;
    constexpr int64_t scale = F::Scale;
    constexpr int64_t pi_val = static_cast<int64_t>(3.14159265358979323846 * scale + 0.5);
    constexpr int64_t two_pi = pi_val * 2;
    constexpr int64_t half_pi = pi_val / 2;

    int64_t a = static_cast<int64_t>(angle.raw());
    while (a > pi_val)  a -= two_pi;
    while (a < -pi_val) a += two_pi;

    bool negate = (a < 0);
    if (negate) a = -a;
    bool mirror = (a > half_pi);
    if (mirror) a = pi_val - a;

    // テーブルインデックスの計算
    // a ∈ [0, π/2] → index ∈ [0, TableSize]
    int64_t idx_scaled = a * TableSize;  // * TableSize
    int64_t idx = idx_scaled / half_pi;
    int64_t frac = idx_scaled - idx * half_pi;  // 端数

    if (idx >= TableSize) idx = TableSize - 1;

    const auto& tab = SinTable<Frac, TableSize>::get();
    int64_t v0 = tab[static_cast<std::size_t>(idx)];
    int64_t v1 = tab[static_cast<std::size_t>(idx + 1)];

    // 線形補間
    int64_t result = v0 + (v1 - v0) * frac / half_pi;

    if (negate) result = -result;
    return F::fromRaw(OP::template apply<Rep>(result));
}

/// cos (LUT)
template<typename Rep, int Frac, typename RP, typename OP, int TableSize = 256>
Fixed<Rep, Frac, RP, OP> cos(Fixed<Rep, Frac, RP, OP> angle) {
    using F = Fixed<Rep, Frac, RP, OP>;
    return trig_lut::sin<Rep, Frac, RP, OP, TableSize>(angle + F::half_pi());
}

} // namespace trig_lut

// ════════════════════════════════════════════════════════════════
// TrigPolicy — 三角関数方式の切り替え機構
// ════════════════════════════════════════════════════════════════

/// TrigPolicy concept: sin(angle), cos(angle), sincos(angle) を提供
template<typename Policy, typename F>
concept TrigPolicy = requires(F angle) {
    { Policy::sin(angle) } -> std::convertible_to<F>;
    { Policy::cos(angle) } -> std::convertible_to<F>;
};

/// CORDIC ポリシー (デフォルト: 産業制御向け, 乗算不要)
template<int Iterations = 0>
struct cordic_policy {
    template<typename Rep, int Frac, typename RP, typename OP>
    static constexpr Fixed<Rep, Frac, RP, OP> sin(Fixed<Rep, Frac, RP, OP> angle) {
        return calx::sin(angle);
    }

    template<typename Rep, int Frac, typename RP, typename OP>
    static constexpr Fixed<Rep, Frac, RP, OP> cos(Fixed<Rep, Frac, RP, OP> angle) {
        return calx::cos(angle);
    }

    template<typename Rep, int Frac, typename RP, typename OP>
    static constexpr auto sincos(Fixed<Rep, Frac, RP, OP> angle) {
        return calx::sincos(angle);
    }
};

/// 多項式近似ポリシー (PC シミュレーション向け, 5次 Horner)
struct polynomial_policy {
    template<typename Rep, int Frac, typename RP, typename OP>
    static constexpr Fixed<Rep, Frac, RP, OP> sin(Fixed<Rep, Frac, RP, OP> angle) {
        return trig_poly::sin(angle);
    }

    template<typename Rep, int Frac, typename RP, typename OP>
    static constexpr Fixed<Rep, Frac, RP, OP> cos(Fixed<Rep, Frac, RP, OP> angle) {
        return trig_poly::cos(angle);
    }

    template<typename Rep, int Frac, typename RP, typename OP>
    static constexpr auto sincos(Fixed<Rep, Frac, RP, OP> angle) {
        return std::pair{trig_poly::sin(angle), trig_poly::cos(angle)};
    }
};

/// LUT + 線形補間ポリシー (MCU 高速版, テーブルサイズ指定可能)
template<int TableSize = 256>
struct lut_policy {
    template<typename Rep, int Frac, typename RP, typename OP>
    static Fixed<Rep, Frac, RP, OP> sin(Fixed<Rep, Frac, RP, OP> angle) {
        return trig_lut::sin<Rep, Frac, RP, OP, TableSize>(angle);
    }

    template<typename Rep, int Frac, typename RP, typename OP>
    static Fixed<Rep, Frac, RP, OP> cos(Fixed<Rep, Frac, RP, OP> angle) {
        return trig_lut::cos<Rep, Frac, RP, OP, TableSize>(angle);
    }

    template<typename Rep, int Frac, typename RP, typename OP>
    static auto sincos(Fixed<Rep, Frac, RP, OP> angle) {
        return std::pair{
            trig_lut::sin<Rep, Frac, RP, OP, TableSize>(angle),
            trig_lut::cos<Rep, Frac, RP, OP, TableSize>(angle)
        };
    }
};

/// デフォルト TrigPolicy の型エイリアス
using default_trig_policy = cordic_policy<>;

/// ポリシー指定版の sin/cos/sincos
template<typename Policy, typename Rep, int Frac, typename RP, typename OP>
    requires TrigPolicy<Policy, Fixed<Rep, Frac, RP, OP>>
constexpr auto trig_sin(Fixed<Rep, Frac, RP, OP> angle) {
    return Policy::sin(angle);
}

template<typename Policy, typename Rep, int Frac, typename RP, typename OP>
    requires TrigPolicy<Policy, Fixed<Rep, Frac, RP, OP>>
constexpr auto trig_cos(Fixed<Rep, Frac, RP, OP> angle) {
    return Policy::cos(angle);
}

template<typename Policy, typename Rep, int Frac, typename RP, typename OP>
    requires TrigPolicy<Policy, Fixed<Rep, Frac, RP, OP>>
constexpr auto trig_sincos(Fixed<Rep, Frac, RP, OP> angle) {
    return Policy::sincos(angle);
}

// ════════════════════════════════════════════════════════════════
// std 互換の数学関数 (ADL 対応)
// ════════════════════════════════════════════════════════════════

/// floor (整数部のみ残す)
template<typename Rep, int Frac, typename RP, typename OP>
constexpr Fixed<Rep, Frac, RP, OP> floor(Fixed<Rep, Frac, RP, OP> x) {
    using F = Fixed<Rep, Frac, RP, OP>;
    constexpr int64_t mask = ~((int64_t(1) << Frac) - 1);
    int64_t r = static_cast<int64_t>(x.raw()) & mask;
    return F::fromRaw(static_cast<Rep>(r));
}

/// fabs
template<typename Rep, int Frac, typename RP, typename OP>
constexpr Fixed<Rep, Frac, RP, OP> fabs(Fixed<Rep, Frac, RP, OP> x) {
    return abs(x);
}

} // namespace calx

#endif // CALX_CORE_FIXED_HPP
