import matplotlib.pyplot as plt
import numpy as np
import mdt

def remove_useless_data(data, idx):
    new_data = []
    new_idx = []
    for i in range(len(data)-1):
        if i == 0:
            new_data.append(data[i])
            new_idx.append(idx[i])
        elif i == len(data)-1:
            new_data.append(data[i])
            new_idx.append(idx[i])
            new_data.append(data[i+1])
            new_idx.append(idx[i+1])
        elif(not(data[i-1]==data[i]==data[i+1])):
            new_data.append(data[i])
            new_idx.append(idx[i])
    return new_data, new_idx


def remove_useless_data2(data, idx):
    data = np.asarray(data)
    idx  = np.asarray(idx)

    # Identify flat regions of length >= 3
    mid = np.ones(len(data), dtype=bool)
    mid[1:-1] = ~((data[:-2] == data[1:-1]) & (data[1:-1] == data[2:]))

    # Apply the mask
    return data[mid], idx[mid]

def theoretische_ADU_kennlinie(save_path=None, u_min=-5, u_max=5, N=3, alone=True):
    L = 2**N
    U_lsb = (u_max - u_min) / (L-1)

    x = [u_min]
    y = [0]

    for i in range(L):
        if i == 0:
            y.append(0)
            x.append(u_min+(U_lsb/2))
        elif i == L-1:
            y.append(y[i]+1)
            x.append(u_max)
        else:
            y.append(y[i]+1)
            x.append(x[i]+U_lsb)

    
    print(f"x: {x}")
    print(f"y: {y}")

    if alone:
        plt.figure()

    plt.step(x, y, marker="", linewidth=0.5, label=f"{N}-Bit AD-Umsetzer, $U_{{LSB}}$ = {U_lsb:.3f} V (Theorie)")

    if alone:
        plt.xlabel("Eingangsspannung U [V]")
        plt.ylabel("Ausgangscode")
        plt.grid(True)
        plt.legend()
        if save_path:
            plt.savefig(save_path, dpi=300)
        plt.show()



# def theoretische_ADU_kennlinie(save_path, u_min=-5, u_max=5, N=3):
#     y = []
#     y.append(np.int64(0))
#     for value in np.arange((N**2)-1):
#         y.append(value)
#     y.append(y[len(y)-1]+1)

#     U_lsb = (u_max-u_min)/((2**N)-1)

#     x = []
#     x.append(u_min)
#     x.append(u_min+U_lsb/2)
#     for _ in range(len(y)-4):
#         x.append(x[len(x)-1]+U_lsb)
#     x.append(x[len(x)-1]+U_lsb)
#     x.append(u_max)

#     # x_values = []
#     # double_inner(x, x_values, len(x))

#     # y_values = []
#     # double_inner(y, y_values, len(y))


#     # print(f"step_length: {U_lsb}")
#     # print(f"x[{len(x)}]: {x}")
#     # print(f"y[{len(y)}]: {y}")

#     # plot something
#     plt.step(x, y, marker="", label=f"{N}"+"-Bit AD-Umsetzers mit $U_{LSB}$ = " + str(np.round(U_lsb, 3))+ " (Theorie)")

#     # format ticks as binary
#     # ax.set_yticks(y)
#     # ax.set_yticklabels([format(i, "03b") for i in y])
#     # plt.xticks(np.arange(int(np.floor(np.min(x))), int(np.ceil(np.max(x)))+1, 1))

#     # plt.xlim(u_min-U_lsb/2, u_max+U_lsb/2)

#     # plt.xlabel("U [V]")
#     # plt.ylabel("Codes [bit]")

#     # plt.title(f"Kennlinie einer {N}Bit-ADU")
#     # plt.grid()
#     # plt.legend()
#     # plt.tight_layout()
#     # if save_path:
#     #     plt.savefig(save_path, format="svg")
#     # plt.show()

def manuell_adu(data_file, save_path):
    matrix = np.loadtxt(data_file, delimiter=",", skiprows=1, dtype=str)
    matrix = np.char.strip(matrix)   # remove leading/trailing spaces

    von  = matrix[:, 0].astype(float)
    bis  = matrix[:, 1].astype(float)
    code = matrix[:, 2]

    x = []

    for i in range(len(von)):
        if(i !=0):
            x.append(bis[i])
        else:
            x.append(von[i])
            x.append(bis[i])
    y = [np.int64(0)]
    y.extend(np.arange(len(code)))

    # print(f"x: {x}")
    # print(f"y: {y}")

    plt.step(x, y, marker="x", linewidth=0.5, label=f"4"+"-Bit AD-Umsetzers mit $U_{LSB}$ = " + "0.66 V (Manuell)")

    # plt.yticks(y, [format(i, "03b") for i in y])

    # plt.xlim(np.min(x)-(x[1]-x[0]), np.max(x)+(x[1]-x[0]))

    # plt.xlabel("U [V]")
    # plt.ylabel("Codes [bit]")

    # plt.title(f"Kennlinie einer 4Bit-ADU")
    # plt.grid()
    # plt.legend()
    # plt.tight_layout()
    # plt.savefig(save_path, format="svg")
    # plt.show()

def automatisch_adu(data_file, save_path, u_min=-5.5, u_max=5.5):
    data = np.loadtxt(data_file, dtype=np.float64)
    # print(data)
    start = 0
    multiple = 0
    max_mult=10
    for value in data:
        start+=1
        if(value==0):
            multiple+=1
        else:
            multiple=0
        if multiple==max_mult:
            start-=max_mult
            break

    data = data[start:]

    end = 0
    multiple = 0
    for value in data:
        end+=1
        if(value>0):
            multiple+=1
        else:
            multiple=0
        if multiple==max_mult:
            end-=max_mult
            break

    data = data[np.int64(end/2):]

    start = 0
    for value in data:
        start+=1
        if(value>=15):
            break

    data = data[:np.int64(start+end/2)]

    points=0
    correct=False
    for value in data:
        if value > 1 or correct==True:
            correct = True
            points +=1
        if correct==True and value >=3:
            break

    step_time=(points)/40000

    U_LSB = (u_max-u_min)*2 * step_time

    # print(f"U_LSB: {U_LSB}")

    y = np.int64(data)
    idx = np.arange(0,len(data))

    y, idx = remove_useless_data2(y, idx)

    # x=np.subtract(np.multiply(np.divide(idx, 39500),22), 5.5)
    x=np.subtract(np.multiply(np.divide(idx, 40000),22), 5.5)

    plt.step(x, y, marker="", linewidth=0.5, label=f"4"+"-Bit AD-Umsetzers mit $U_{LSB}$ = " + f"{np.round(U_LSB,3)} V (Automatisiert)")

    # plt.yticks(y, [format(i, "03b") for i in y])
    # plt.xticks(np.arange(int(np.floor(x.min())), int(np.ceil(x.max()))+1, 1))

    # plt.xlim(-6, 6)

    # plt.xlabel("t [s]")
    # plt.ylabel("Codes [bit]")

    # plt.title(f"Kennlinie einer 4Bit-ADU")
    # plt.grid()
    # plt.legend()
    # plt.tight_layout()
    # plt.savefig(save_path, format="svg")
    # plt.show()

def quantisierungsrauschen_plot(data_file):
    data = np.loadtxt(data_file, dtype=np.float64)
    x = np.arange(0, len(data))
    # # print(x)
    # plt.plot(x, data)
    return data, x
    
def plot_sine_wave(amplitude=1, frequency=1, phase=0, t_start=0, t_end=2*np.pi, points=1000,
                   x_shift=0, y_shift=0, color='blue', linewidth=1.5, title='Sine Wave', xlabel='Time', ylabel='Amplitude'):
    """
    Plots a sine wave with customizable parameters.

    Parameters:
    - amplitude: Amplitude of the sine wave
    - frequency: Frequency in Hz
    - phase: Phase shift in radians
    - t_start: Start of x-axis
    - t_end: End of x-axis
    - points: Number of points in the plot
    - x_shift: Shift the wave along x-axis
    - y_shift: Shift the wave along y-axis
    - color: Line color
    - linewidth: Line thickness
    - title: Plot title
    - xlabel, ylabel: Axis labels
    """

    # Generate x-axis values
    t = np.linspace(t_start, t_end, points)

    # Compute sine wave with phase and shifts
    y = amplitude * np.sin(2 * np.pi * frequency * t + phase) + y_shift
    t = t + x_shift  # shift x-axis if needed

    return y, t


def vorbereitung():
    theoretische_ADU_kennlinie(save_path="prak4/3-bit-adu.svg")

def task1():
    automatisch_adu("prak4/prak4/ADU-Kennlinie_automatisch.csv", f"prak4/4-bit-adu-auto.svg")
    theoretische_ADU_kennlinie(N=4, save_path="prak4/prak4/4-bit-adu-theorie.svg", alone=False)
    manuell_adu("prak4/prak4/ADU-Kennlinie_manuell.csv", f"prak4/4-bit-adu-manuell.svg")
    
    plt.plot([-5,5],[0,15], marker="", linewidth=0.5, label=r"$\infty$-Bit ADU (Theoretisch)")

    plt.yticks(np.arange(0,15), [format(i, "03b") for i in np.arange(0,15)])
    plt.xticks(np.arange(int(np.floor(-6)), int(np.ceil(6))+1, 1))

    plt.xlim(-5.5, 5.5)

    plt.xlabel("U [V]")
    plt.ylabel("Codes [bit]")

    plt.title(f"Kennlinie einer 4Bit-ADU")
    plt.grid()
    plt.legend(loc='upper left')
    plt.tight_layout()
    plt.savefig(f"prak4/4bit-adu-kennlinie.svg", format="svg")
    plt.show()

def cut_fitting(idx_target, data, idx_source):
    start = np.where(idx_source == idx_target[0])[0]
    end = np.where(idx_source == idx_target[len(idx_target)-1])[0][0]

    if len(start)>1:
        for v in start:
            if v + len(idx_target) == end:
                start = v
                break
    
    idx_source = idx_source[start:end]
    data = data[start:end]


    return data, idx_source

def get_smallest_sin_dif(data, idx):
    x_shift = 0
    sum = 50000000
    for i in np.arange(-1700, -1500):
        ideal, t = plot_sine_wave(amplitude=5, frequency=15, t_start=0, t_end=1.5*40000, points=60000, x_shift=i)
        ideal, t = cut_fitting(idx, ideal, np.int64(t))
        if np.sum(np.abs(np.abs(ideal)-np.abs(data)))<sum:
            sum = np.sum(np.abs(np.abs(ideal)-np.abs(data)))
            x_shift = i
    print(f"x_shift: {x_shift}")
    print(f"sum: {sum}")

    ideal, t = plot_sine_wave(amplitude=5, frequency=15, t_start=0, t_end=1.5*40000, points=60000, x_shift=x_shift)
    ideal, t = cut_fitting(idx, ideal, np.int64(t))

    return ideal, t

def plot_both_sin(data, idx, ideal):
    plt.plot(idx, data, label=f'Gemessener Sinus')
    plt.plot(idx, ideal, label=f'Idealer Sinus')
    
    plt.xlim(left=0, right=1)

    plt.xlabel("t [s]")
    plt.ylabel("u [V]")

    plt.title(f"Gemessenes und Ideales Signal")
    plt.grid()
    plt.legend(loc='upper left')
    plt.tight_layout()
    plt.savefig(f"prak4/sin_normal.svg", format="svg")
    plt.show()

def plot_diff(data, idx, ideal):

    plt.plot(idx, data-ideal)
    plt.xlabel("t [s]")
    plt.ylabel("u [V]")

    plt.title(f"Quantisierungsrauschen im Beispiel einer Sinus-Funktion")
    plt.grid()
    # plt.legend(loc='upper left')
    plt.tight_layout()
    plt.savefig(f"prak4/sin_all.svg", format="svg")
    plt.show()

def plot_all_sin(data, idx, ideal):
    plt.plot(idx, data, label=f'Gemessener Sinus')
    plt.plot(idx, ideal, label=f'Idealer Sinus')
    N=len(idx)
    U_Rauschen = (data-ideal)
    plt.plot(idx, U_Rauschen, label=f'Quantisierungsrauschen')
    U_eff = np.sqrt(np.multiply(np.divide(1, N), np.sum(np.square(U_Rauschen))))

    print(f"U_eff: {U_eff}")
    plt.plot([idx[0], idx[len(idx)-1]], [U_eff, U_eff], label=f'Effektivwert des Quantisierungsrauschens')
    plt.xlabel("t [s]")
    plt.ylabel("u [V]")
    
    plt.xlim(left=0, right=0.2)

    plt.title(f"Quantisierungsrauschen im Beispiel einer Sinus-Funktion")
    plt.grid()
    plt.legend(loc='upper left')
    plt.tight_layout()
    plt.savefig(f"prak4/sin_all.svg", format="svg")
    plt.show()

def task2():
    plt.figure(figsize=(8,4))
    data, idx = quantisierungsrauschen_plot(f"prak4/prak4/Quantisierungsrauschen.csv")
    ideal, t = get_smallest_sin_dif(data, idx)
    # plot_both_sin(data, np.divide(idx, 40000), ideal)
    # plot_diff(data, np.divide(idx, 40000), ideal)
    plot_all_sin(data, np.divide(idx, 40000), ideal)
    
def plot_data_spektrum(data_file, rate, save_path, xlim_start=0, xlim_end=0):
    data = np.loadtxt(data_file, dtype=np.float64)
    f,u = mdt.spectrum(data, rate)
    fig, (ax1,ax2) = plt.subplots(2)
    ax1.plot(np.divide(np.arange(0, len(data)), rate),data)
    ax2.plot(f,u)

    if xlim_end !=0:
        ax1.set_xlim(xlim_start, xlim_end)

    # Titles for each subplot
    ax1.set_title("Zeitsignal")
    ax2.set_title("Frequenz Spektrum")

    # X/Y labels for each subplot
    ax1.set_xlabel("t [s]")
    ax1.set_ylabel("U [V]")

    ax2.set_xlabel("Frequency [Hz]")
    ax2.set_ylabel("Magnitude")

    # Overall title
    # fig.suptitle(f"Signal analyse bei {np.divide(rate, 1000)} kHz Abtastrate", fontsize=16)

    # Layout to prevent overlap
    plt.tight_layout()
    fig.subplots_adjust(top=0.88)   # make space for suptitle
    plt.savefig(save_path, format="svg")
    plt.show()

def task3():
    rates = [10000, 12500, 15000, 17500, 20000, 25000, 30000, 35000, 40000]
    for rate in rates:
        plot_data_spektrum(f"prak4/prak4/Aliasing_{rate}.csv", rate, f"prak4/aliasing_{rate}.svg")

def task4():
    rates = [10000, 44000]
    for rate in rates:
        plot_data_spektrum(f"prak4/prak4/Aliasingfilter_mit_box_{rate}.csv", rate, f"prak4/aliasing_mit_filter_{rate}.svg", 0, 0.01)
        plot_data_spektrum(f"prak4/prak4/Aliasingfilter_ohne_box_{rate}.csv", rate, f"prak4/aliasing_ohne_filter_{rate}.svg", 0, 0.01)

if __name__ == '__main__':
    task3()