import numpy as np
from scipy import signal

def integer_lfilter(b_int, a_int, x_float, shift_in, shift_out, shift_b, shift_a):
    x_int = np.round(x_float * (2**shift_in)).astype(np.int64)
    y_int = np.zeros(len(x_int), dtype=np.int64)

    q_bx = shift_in + shift_b
    q_ay = shift_out + shift_a
    q_acc = max(q_bx, q_ay)

    s_bx = q_acc - q_bx
    s_ay = q_acc - q_ay

    A0 = a_int[0]
    nb = len(b_int)
    na = len(a_int)

    for n in range(len(x_int)):
        acc = 0
        for i in range(nb):
            if n - i >= 0:
                acc += (b_int[i] * x_int[n - i]) << s_bx
        for j in range(1, na):
            if n - j >= 0:
                acc -= (a_int[j] * y_int[n - j]) << s_ay

        # 原本的邏輯：(acc // A0) >> s_ay
        # 修正後的邏輯：直接計算總位移，減少除法誤差
        # y_int[n] = (acc // A0) >> s_ay
        
        # 模擬 DSP 的 A0 通常是 2^shift_a
        # 所以 acc / 2^shift_a >> (q_acc - shift_a - shift_out)
        # 等於 acc >> (q_acc - shift_out)
        total_shift = q_acc - shift_out
        y_int[n] = acc >> total_shift

    return y_int.astype(float) / (2**shift_out)

# 測試 1: 低通濾波器
fs = 1000
fc = 10
b, a = signal.butter(1, fc / (fs/2), 'low')
# b = [0.03046, 0.03046], a = [1.0, -0.939]
shift = 14
b_int = [int(round(x * (2**shift))) for x in b]
a_int = [int(round(x * (2**shift))) for x in a]

x = np.ones(100) # Step input
y_float = signal.lfilter(b, a, x)
y_fixed = integer_lfilter(b_int, a_int, x, shift, shift, shift, shift)

print("Float Final:", y_float[-1])
print("Fixed Final:", y_fixed[-1])
print("Diff:", y_float[-1] - y_fixed[-1])