
import os
import sys
import numpy as np
import matplotlib.pyplot as plt

R = 4 # Decimation factor
M = 1 # Delay in CIC filter
N = 4 # Order of the filter

# Open the input file and read the impulse response data
def read_impulse_response(file_path):
    if not os.path.exists(file_path):
        print(f"Error: File {file_path} does not exist.")
        sys.exit(1)

    with open(file_path, 'r') as file:
        lines = file.readlines()

    impulse_response = []
    for line in lines:
        try:
            value = float(line.strip())
            impulse_response.append(value)
        except ValueError:
            print(f"Warning: Skipping invalid line '{line.strip()}' in {file_path}")

    return np.array(impulse_response)

def convert_to_frequency_domain(impulse_response):
    # Perform FFT
    frequency_response = np.fft.fft(impulse_response)
    
    frequency_response /= np.max(np.abs(frequency_response))
    
    return frequency_response

def get_theoretical_freq_resp(decimation_factor, delay, order, num_points=1024, fs=50e6):
    freq = np.fft.fftfreq(num_points, d=1/fs)
    # Вычисляем нормализованные частоты
    f_norm = np.abs(freq) / fs
    # Расчет по формуле
    R = decimation_factor
    M = delay
    N = order
    # Избегаем деления на ноль при f=0
    mag = np.ones(num_points)
    nonzero = f_norm != 0
    # Угол для sin(π*f*R*M)
    angle_num = np.pi * f_norm[nonzero] * R * M
    # Угол для sin(π*f*M)
    angle_den = np.pi * f_norm[nonzero] 
    # |H(f)| = |sin(π*f*R*M)/(R*sin(π*f*M))|^N
    mag[nonzero] = np.abs(np.sin(angle_num) / ( np.sin(angle_den))) ** N
    # Нормализуем амплитудную характеристику
    mag /= np.max(mag)
    return freq, mag

# Plot the frequency response
def plot_frequency_response(frequency_response, input_signal_time_domain, fs=50e6):
    normalized_freq = np.fft.fftfreq(len(input_signal_time_domain), 1/fs) / fs
    
    plt.figure(figsize=(10, 6))
    half_len = len(normalized_freq) // 2
    plt.plot(normalized_freq[:half_len], np.abs(frequency_response[:half_len]), 
             label='Signal Frequency Response')
    plt.title('Frequency Response')
    plt.xlabel('Normalized Frequency (f/fs)')
    # Magnitude in dB
    plt.plot(normalized_freq[:half_len], 20 * np.log10(np.abs(frequency_response[:half_len])), label='Magnitude (dB)')
    plt.ylabel('Magnitude (dB)')
    plt.ylim(-150, 10) 
    plt.grid(True)
    plt.legend()
    plt.xlim(0, 0.5)
    plt.show()

def convert_to_fixed_point(R):
    # Read File with coefficients
    with open(f'C:\\S5243_FFT_REPO\\src\\src\\Sim\\fir_filter_R{R}.coe', 'r') as file:
        lines = file.readlines()
        # skip first 3 lines
        coeffs = []
        for line in lines[3:]:
            # Удалить запятую в конце строки, если есть или точку с запятой
            line = line.strip().rstrip(',;')
            if line.strip():
                coeffs.append(float(line.strip()))
    coeffs = np.array(coeffs)
    # Get rid of the exponent notation. Show in the normal float format
    np.set_printoptions(suppress=True, precision=6)
    print("Coefficients in float format:")
    print(coeffs)
    # find the maximum absolute value and it's width
    max_abs_value = np.max(np.abs(coeffs))
    print(f"Maximum absolute value of coefficients: {max_abs_value}")
    #number of bits needed to represent the coefficients
    num_bits = int(np.ceil(np.log2(max_abs_value + 1)))
    print(f"Number of bits needed to represent the coefficients: {num_bits}")
    # Convert to integer format Q.1.12.3 - 1 bit for sign, 12 bits for integer part, 3 bits for fractional part
    fixed_point_coeffs = np.round(coeffs * (2 ** (16-(num_bits + 1)))).astype(np.int16)  # Q.1.12.3
    print(f"Fixed point coefficients (Q.1.{15-(16-(num_bits + 1))}.{(16-(num_bits + 1))}):")
    # Вставляем запятую после каждого значения
    print(", ".join(str(coeff) for coeff in fixed_point_coeffs))

    # Convert to hexadecimal format
    hex_coeffs = [f"{int(coeff) & 0xFFFF:04X}" for coeff in fixed_point_coeffs]
    print("Fixed point coefficients in hexadecimal format:")
    print(hex_coeffs)
    # Edit the Coeffs package file 
    with open(f'C:\\S5243_FFT_REPO\\src\\src\\FirFilter\\CoeffsR{R}pkg.sv', 'w') as file:
        # Запись заголовка и объявления массива
        file.write(f"package CoeffsR{R}pkg;\n")
        file.write(f"localparam signed [15:0] COEFFS[{len(fixed_point_coeffs)}] = '{{\n")

        # Запись коэффициентов в формате 16'sh<HEX_VALUE>
        for i, hex_coeff in enumerate(hex_coeffs):
            comment = ""
            # Добавляем комментарий для отрицательных чисел
            if fixed_point_coeffs[i] < 0:
                comment = f" // 0x{hex_coeff} is {fixed_point_coeffs[i]} in signed 16-bit"

            # Добавляем запятую для всех строк кроме последней
            comma = "," if i < len(hex_coeffs) - 1 else ""

            # Записываем строку с коэффициентом
            file.write(f"    16'sh{hex_coeff}{comma}{comment}\n")
        # Skip the line with MSB_POS
        file.write("};\n\n")
        file.write(f"localparam integer MSB_POS = $ceil(4*$clog2(1*{R}))+14;")
        file.write("\n\n\nendpackage\n")

# Main function to execute the conversion
def main():
    impulse_response_file = 'C:\S5243_FFT_REPO\src\src\Sim\FilteredData.txt'

    convert_to_fixed_point(4)
    convert_to_fixed_point(2)
    convert_to_fixed_point(8)
    filt_data_impulse_response = read_impulse_response(impulse_response_file)
    filt_data_frequency_response = convert_to_frequency_domain(filt_data_impulse_response)
    plot_frequency_response(filt_data_frequency_response,filt_data_impulse_response)
    print("Frequency response conversion completed successfully.")
    # Show the input signal 
    input_signal_time_domain = read_impulse_response('C:\S5243_FFT_REPO\src\src\Sim\InputSignal.txt')
    input_signal_frequency_response = convert_to_frequency_domain(input_signal_time_domain)
    fs = 50e6  
    # freq_axis = np.fft.fftfreq(len(input_signal_time_domain), 1/fs) / 1e6
    freq_axis = np.fft.fftfreq(len(input_signal_time_domain), 1/fs)
    
    # plt.figure(figsize=(10, 6))
    Denominator = R if R > 1 else 2  
    half_len = len(freq_axis) // Denominator
    # plt.plot(freq_axis[:half_len], np.abs(input_signal_frequency_response[:half_len]), label='Input Signal Frequency Response')
    # plt.title('Input Signal Frequency Response')
    # plt.xlabel('Frequency (MHz)')
    # plt.ylabel('Magnitude')
    # plt.grid()
    # plt.legend()
    # plt.xlim(0, fs/(2*1e6)) 
    # # plt.show()


    cic_filter_impulse_response = read_impulse_response('C:\S5243_FFT_REPO\src\src\Sim\ImpResp1.txt')
    cic_filter_frequency_response = convert_to_frequency_domain(cic_filter_impulse_response)

    
    # Получаем теоретическую частотную характеристику CIC фильтра ( весь диапазон)
    theory_freq, theory_mag = get_theoretical_freq_resp(R, M, N, num_points=len(cic_filter_impulse_response), fs=fs)


    plt.figure(figsize=(12, 8))

    # Выделяем положительные частоты для теоретической АЧХ
    mask_pos = theory_freq >= 0
    theory_freq_pos = theory_freq[mask_pos]
    theory_mag_pos = theory_mag[mask_pos]
    
    # Новая частота Найквиста после децимации
    nyquist_new = fs/(2*R)

      # Выделяем частоты для АЧХ после децимации только до новой частоты Найквиста
    freq_after_dec = np.fft.fftfreq(len(cic_filter_impulse_response), d=1/fs*R)
    resp_after_dec = cic_filter_frequency_response
    mask_nyquist = (freq_after_dec >= 0) & (freq_after_dec <= nyquist_new)
    freq_pos_after = freq_after_dec[mask_nyquist]
    resp_pos_after = resp_after_dec[mask_nyquist]

    db_theory = 20 * np.log10(np.maximum(theory_mag_pos, 1e-10))
    plt.plot(theory_freq_pos, db_theory, 'r-', label='Теоретическая АЧХ (весь диапазон)')
    plt.plot(freq_axis[:half_len], 20 * np.log10(np.abs(cic_filter_frequency_response[:half_len])), 'b-', linewidth=2, label='АЧХ после децимации')

    # Отмечаем новую частоту Найквиста
    plt.axvline(x=nyquist_new, color='k', linestyle=':',
                label=f'Новая частота Найквиста ({nyquist_new/1e6:.2f} МГц)')
    
    # Затеняем область выше новой частоты Найквиста
    plt.axvspan(nyquist_new, fs/2, alpha=0.2, color='gray',
                label='Область наложения спектра (aliasing)')
    plt.title(f'Сравнение частотных характеристик CIC-фильтра R={R}, D={M}, N={N}')
    plt.xlabel('Частота [Гц]')
    plt.ylabel('Амплитуда [дБ]')
    plt.grid(True)
    plt.legend()
    plt.xlim(0, fs/2)  # Отображаем только до fs/2
    plt.ylim(-300, 10)
    plt.tight_layout()
    plt.show()

    # # 2nd stage CIC filter
    # cic_filter_impulse_response_2nd = read_impulse_response('C:\S5243_FFT_REPO\src\src\Sim\ImpResp2.txt')
    # cic_filter_frequency_response_2nd = convert_to_frequency_domain(cic_filter_impulse_response_2nd)
    # # plot the frequency response of the CIC filter in dB
    # plt.figure(figsize=(10, 6))
    # plt.plot(freq_axis[:half_len], 20 * np.log10(np.abs(cic_filter_frequency_response_2nd[:half_len])), label='CIC Filter Frequency Response 2nd Stage (dB)')
    # plt.title(f'CIC Filter Frequency Response 2nd Stage (R={np.cbrt(R)*np.cbrt(R)})')
    # plt.xlabel('Frequency (MHz)')
    # plt.ylabel('Magnitude (dB)')
    # plt.ylim(-300, 10)
    # plt.grid()
    # plt.legend()
    # plt.xlim(0, fs/(2*(np.cbrt(R)**2)*1e6))
    # plt.show()

    # # 3rd stage CIC filter
    # cic_filter_impulse_response_3rd = read_impulse_response('C:\S5243_FFT_REPO\src\src\Sim\ImpResp3.txt')
    # cic_filter_frequency_response_3rd = convert_to_frequency_domain(cic_filter_impulse_response_3rd)
    # # plot the frequency response of the CIC filter in dB
    # plt.figure(figsize=(10, 6))
    # plt.plot(freq_axis[:half_len], 20 * np.log10(np.abs(cic_filter_frequency_response_3rd[:half_len])), label='CIC Filter Frequency Response 3rd Stage (dB)')
    # plt.title(f'CIC Filter Frequency Response 3rd Stage (R={R})')
    # plt.xlabel('Frequency (MHz)')
    # plt.ylabel('Magnitude (dB)')
    # plt.ylim(-400, 10)
    # plt.grid()
    # plt.legend()
    # plt.xlim(0, fs/(2*R*1e6))
    # plt.show()

    # fir_filter_impulse_response = read_impulse_response('C:\S5243_FFT_REPO\src\src\Sim\AfterFir.txt')
    # fir_filter_frequency_response = convert_to_frequency_domain(fir_filter_impulse_response)
    # # plot the frequency response of the FIR filter in dB
    # plt.figure(figsize=(10, 6))
    # plt.plot(freq_axis[:half_len], 20 * np.log10(np.abs(fir_filter_frequency_response[:half_len])), label='FIR Filter Frequency Response (dB)')
    # plt.title('FIR Filter Frequency Response')
    # plt.xlabel('Frequency (MHz)')
    # plt.ylabel('Magnitude (dB)')
    # plt.ylim(-300, 10)
    # plt.grid()
    # plt.legend()
    # plt.xlim(0, fs/(2*R*1e6))
    # plt.show()

    fir_comp_resp = read_impulse_response('C:\S5243_FFT_REPO\src\src\Sim\FirFilt.txt')
    # Plot in time domain
    # plt.figure(figsize=(10, 6))
    # plt.plot(fir_comp_resp, label='FIR Compensator Impulse Response')
    # plt.title('FIR Compensator Impulse Response')
    # plt.xlabel('Sample Index')
    # plt.ylabel('Amplitude')
    # plt.grid()
    # plt.legend()
    # plt.xlim(0, len(fir_comp_resp))
    # plt.show()

    # After FIR time domain
    after_fir_resp = read_impulse_response('C:\S5243_FFT_REPO\src\src\Sim\AfterFir.txt')
    # plt.figure(figsize=(10, 6))
    # plt.plot(after_fir_resp, label='After FIR Impulse Response')
    # plt.title('After FIR Impulse Response')
    # plt.xlabel('Sample Index')
    # plt.ylabel('Amplitude')
    # plt.grid()
    # plt.legend()
    # plt.xlim(0, len(after_fir_resp))
    # plt.show()
    # Plot the frequency response of the FIR compensator
    fir_compensation_response = convert_to_frequency_domain(after_fir_resp)
    plt.figure(figsize=(10, 6))
    plt.plot(freq_axis[:half_len], 20 * np.log10(np.abs(fir_compensation_response[:half_len])), label='FIR Compensation Frequency Response (dB)')
    # Plot the original CIC filter frequency response for comparison
    plt.plot(freq_axis[:half_len], 20 * np.log10(np.abs(cic_filter_frequency_response[:half_len])), 'b-', linewidth=2, label='АЧХ после децимации')
    plt.title('FIR Compensation Frequency Response')
    plt.xlabel('Frequency (Hz)')
    plt.ylabel('Magnitude (dB)')
    plt.ylim(-100, 10)
    plt.grid()
    plt.legend()
    plt.xlim(0, fs/(2*R))
    plt.show()
  
    
if __name__ == "__main__":
    main()

