import numpy as np

# ------------------------------------------------------------
# 按论文公式 (9)(10)(11) 生成 Muntz–Laguerre 正交有理基 (解析形式)：
#
# 给定稳定极点集合 {p_k} (Re(p_k)<0)。论文记法中使用 -a_k，其中 Re(a_k)>0。
# 对应关系：p_k = -a_k  ⇒  a_k = -p_k,  Re(a_k)= -Re(p_k) >0
#
# 连续内积意义下（沿 jω 轴积分）这些 φ_k 解析正交。离散频率采样后数值上
# 可能偏离，可再用加权 QR 做数值再正交（可选）。
#
# 公式在“稳定极点 p 表达”下的改写：
#   (原) 实极点:  φ_p(s) = sqrt(2 Re(a_p)) / (s + a_p) * Π (s - a_i^*)/(s + a_i)
#   变换 a_p = -p ⇒ Re(a_p)= -Re(p) = σ >0 且 (s + a_p) = (s - p)
#   且乘积 (s - a_i^*)/(s + a_i) = (s + p_i^*)/(s - p_i)
#   ⇒ φ_p(s) = sqrt(-2 Re(p)) / (s - p) * Π_{i<p} (s + p_i^*)/(s - p_i)
#
# 复对 (p, p*)，取 imag(p)>0 的 p 作为首：
#   (原) φ_p   = sqrt(2 Re(a_p)) (s - |a_p|)/[(s + a_p)(s + a_p^*)] * Π(...)
#   (原) φ_{p+1}= sqrt(2 Re(a_p)) (s + |a_p|)/[(s + a_p)(s + a_p^*)] * Π(...)
#   代入 a_p=-p：
#       Re(a_p)= -Re(p)=σ>0,  (s + a_p) = (s - p), (s + a_p^*)=(s - p^*)
#       |a_p| = |p|
#       乘积同上 ⇒ Π_{i<p} (s + p_i^*)/(s - p_i)
#
#   ⇒ φ_p(s)   = sqrt(-2 Re(p)) (s - |p|)/[(s - p)(s - p^*)] * Π_{i<p} (s + p_i^*)/(s - p_i)
#      φ_{p^*}(s)= sqrt(-2 Re(p)) (s + |p|)/[(s - p)(s - p^*)] * Π_{i<p} (s + p_i^*)/(s - p_i)
#
# product 递推维护：prod_k = Π_{i≤k} (s + p_i^*)/(s - p_i)
# 对复对要顺序乘两次（p 与 p*）。
# ------------------------------------------------------------

class MuntzLaguerreIterator:
    def __init__(self, s: np.ndarray, stable_poles: list | np.ndarray):
        """
        s: 复频率数组 (Nf,), s = j 2π f
        stable_poles: 稳定极点列表 (Re<0). 复共轭对要求正虚部在前 (p, p*).
        """
        self.s = np.asarray(s, dtype=complex)
        self.poles = list(stable_poles)
        self.N = len(self.poles)
        self.k = 0
        # 初始化乘积 Π_{i<p} (s + p_i^*)/(s - p_i)
        self.product = np.ones_like(self.s, dtype=complex)

    def __iter__(self):
        return self

    def __next__(self):
        if self.k >= self.N:
            raise StopIteration

        p = self.poles[self.k]
        if np.real(p) >= 0:
            raise ValueError(f"极点必须在左半平面: {p}")

        # 复对首 (正虚部)
        if np.iscomplex(p) and np.imag(p) > 0:
            if self.k + 1 >= self.N:
                raise ValueError("复极点缺少共轭")
            pc = self.poles[self.k + 1]
            if not np.isclose(pc, np.conj(p)):
                raise ValueError("复极点未按 (p, p*) 顺序排列 (正虚部在前)")
            sigma = -np.real(p)           # >0
            scale = np.sqrt(2 * sigma)
            r = np.abs(p)
            denom = (self.s - p) * (self.s - pc)

            # 两个基函数
            phi_p  = scale * (self.s - r) / denom * self.product
            phi_pc = scale * (self.s + r) / denom * self.product

            # product 先乘 (s + p^*)/(s - p)，再乘 (s + p)/(s - p^*)
            self.product = self.product * (self.s + pc) / (self.s - p)
            self.product = self.product * (self.s + p)  / (self.s - pc)

            self.k += 2
            return [phi_p, phi_pc]

        # 复对次 (负虚部) —— 应该被首元素处理，出现表示顺序错误
        if np.iscomplex(p) and np.imag(p) < 0:
            raise ValueError("检测到负虚部复极点但其共轭尚未处理，请将正虚部成员放在前面。")

        # 实极点
        sigma = -np.real(p)
        if sigma <= 0:
            raise ValueError("实极点实部应为负 (稳定)。")
        scale = np.sqrt(2 * sigma)
        phi = scale / (self.s - p) * self.product
        # 更新乘积
        self.product = self.product * (self.s + p) / (self.s - p)

        self.k += 1
        return [phi]

def generate_muntz_laguerre_basis(s: np.ndarray, stable_poles: list | np.ndarray):
    """
    生成完整基函数列表: [φ_0=1, φ_1, φ_2, ...]
    """
    basis = [np.ones_like(s, dtype=complex)]
    for block in MuntzLaguerreIterator(s, stable_poles):
        basis.extend(block)
    return basis

# ---------- 可选：离散再正交 (加权 QR) ----------
# def trapezoid_weights(freqs: np.ndarray):
#     if len(freqs) == 1:
#         return np.ones(1)
#     df = np.diff(freqs)
#     w = np.zeros_like(freqs, dtype=float)
#     w[0] = 0.5 * df[0]
#     w[-1] = 0.5 * df[-1]
#     if len(freqs) > 2:
#         w[1:-1] = 0.5 * (df[:-1] + df[1:])
#     return w

def weighted_qr_from_basis(basis_cols: list[np.ndarray], weights: np.ndarray | None = None):
    A = np.column_stack(basis_cols)  # (Nf, M)
    if weights is None:
        sw = np.ones(A.shape[0])
    else:
        sw = np.sqrt(weights.real)
    Aw = sw[:, None] * A
    Qw, R = np.linalg.qr(Aw)
    Phi = Qw / (sw[:, None] + 1e-30)
    return Phi, R  # Raw = Phi R

def check_discrete_orthogonality(Phi: np.ndarray, w: np.ndarray):
    G = Phi.conj().T @ (w[:, None] * Phi)
    off = np.max(np.abs(G - np.eye(G.shape[0])))
    return G, off

def verify_orthonormal(Phi: np.ndarray, w: np.ndarray, atol=1e-10, rtol=1e-8):
    """
    返回:
        G        : Gram 矩阵 (Φ^H W Φ)
        max_off  : 最大非对角幅值
        diag_err : max |diag(G)-1|
        passed   : 是否满足阈值
    """
    G = Phi.conj().T @ (w[:, None] * Phi)
    I = np.eye(G.shape[0])
    diag_err = np.max(np.abs(np.diag(G) - 1.0))
    max_off = np.max(np.abs(G - I + np.diag(np.diag(G) - 1.0)))
    passed = (diag_err <= atol) and (max_off <= rtol)
    return G, max_off, diag_err, passed

def omega_weights(freqs_hz: np.ndarray):
    """
    基于 ω=2πf 的梯形法得到 w_ω = Δω/(2π)，使得
    (1/2π) ∫_{-∞}^{∞} → Σ w_ω,k
    """
    f = freqs_hz
    if len(f) == 1:
        return np.ones(1)
    df = np.diff(f)
    w_f = np.zeros_like(f)
    w_f[0] = 0.5 * df[0]
    w_f[-1] = 0.5 * df[-1]
    if len(f) > 2:
        w_f[1:-1] = 0.5 * (df[:-1] + df[1:])
    # dω = 2π df,  (1/2π) * dω = df  ⇒ 直接 w_f 就是 w_ω
    return w_f  # 已等价于 Δω/(2π)

# ------------------ 示例 ------------------
if __name__ == "__main__":
    # 示例稳定极点 (复对正虚部在前)
    stable_poles = [
        -0.8e9,
        -1.0e9 + 2.5e9j,
        -1.0e9 - 2.5e9j,
        -2.2e9
    ]
    freqs = np.linspace(1e8, 8e9, 400)
    s = 1j * 2 * np.pi * freqs

    basis = generate_muntz_laguerre_basis(s, stable_poles)
    print("解析基函数数量 =", len(basis))
    print("前5个基函数示例 (每个前10个频点):")
    for i in range(5):
        print(f"φ_{i}:", basis[i][:10])

    w = omega_weights(freqs)
    Phi_num, R = weighted_qr_from_basis(basis, w)
    Gram, off = check_discrete_orthogonality(Phi_num, w)
    print("离散 Gram 最大非对角元素 =", off)
    print("R 形状:", R.shape)
    # 验证 Raw ≈ Phi R
    raw = np.column_stack(basis)
    err = np.max(np.abs(raw - Phi_num @ R))
    print("重构误差 ||Raw - Phi R||_∞ =", err)

    # 验证正交性
    print("离散 Gram 矩阵 (前5x5):")
    print(Gram[:5, :5])

    Gcheck, max_off, diag_err, ok = verify_orthonormal(Phi_num, w)
    print(f"Diag 误差={diag_err:.3e}, Max off={max_off:.3e}, Orthonormal={ok}")
    # 额外: 验证 R
    # raw = Φ R  =>  R ≈ Φ^H W raw  (因为 Φ^H W Φ = I)
    R_alt = Phi_num.conj().T @ (w[:,None] * raw)
    print("R 差异 ||R - R_alt||_max =", np.max(np.abs(R - R_alt)))