import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
from scipy.signal import chirp

#------------------------------------------------

# Note: If you want to use zero for frequencies f_x, f_y, and f_z, use a very 
# small value instead of zero, as the 'chirp' function can have trouble 
# with zero value in frequencies.

#------------------------------------------------
# Constant values
FT = 200  # Sampling Frequency [Hz]
t_signal = 60  # Duration of the constant signal [s]
t_ramp_start = 10  # Duration of the starting ramp [s]
t_ramp_end = 10  # Duration of the ending ramp [s]

t_tot = t_ramp_start + t_signal + t_ramp_end
t = np.arange(0, t_tot + 1/FT, 1/FT)  # Time vector [s]
g = 9.81  # Gravity acceleration [m/s^2]

#------------------------------------------------
# Vectors of parameters for the FOR LOOP with importation from .xlsx file
file = (r'C:\Users\vitog\OneDrive\Desktop\Tesi\Lab\Input_horizontal_excitation\Harmonic_values.xlsx')  # Replace with the actual file path
sheet = 'X_direction'

# Replace 'your_file.xlsx' with the actual file path
file_path = file

# Replace 'Sheet1' with the name of the sheet containing your data
sheet_name = sheet

# Import f_x_vec, f_x_file_name_vec
df = pd.read_excel(file_path, sheet_name=sheet_name)
f_x_vec = df['f_[Hz]']

# Import Amp_x_acc_vec and Amp_x_poss_name_vec
Amp_x_acc_vec = df['a_acc_x_[m/s^2]']

#------------------------------------------------
# Initialize an empty matrix to store the position vector in one matrix
matrix_of_Poss = np.empty((len(t), 0))

#------------------------------------------------
# Starting FOR LOOP
# generating N-sine signals along x-direction
total_number_of_signals = len(f_x_vec)

for i in range(total_number_of_signals):
#for i in (0,2):
    # Assign Parameters
    f_x = f_x_vec[i]
    Amp_x_acc = Amp_x_acc_vec[i]

    # Assembling Acceleration Amplitude
    Amp_array_x = np.full_like(t, Amp_x_acc)
# Building of the starting and ending ramping in amplitude of the sine signal
    value_to_extract_up_to_start = t_ramp_start
    t_start = t[t <= value_to_extract_up_to_start]
    m_start = Amp_x_acc / t_ramp_start
    Amp_array_x[:len(t_start)] = m_start * t_start

    value_to_extract_up_to_end = t_ramp_end
    t_end = t[t >= (t_tot - value_to_extract_up_to_end)]
    m_end = Amp_x_acc / t_ramp_end
    Amp_array_x[-len(t_end):] = -m_end * (t_end - (t_tot - t_ramp_end))
    Amp_array_x[-len(t_end):] = -Amp_array_x[-len(t_end):][::-1]
    
    # Sine sweep in X direction
    beta = 0  # because it's a constant signal
    f_t = f_x + beta * t
    sine_signal_x = chirp(t, f0=f_x, f1=f_x, t1=t_tot, method='linear')
    sine_signal_x = sine_signal_x * Amp_array_x
    Amp_x_pos = -sine_signal_x / (4 * np.pi ** 2 * f_t ** 2)

    # Converting from [m] to [mm]
    Poss = Amp_x_pos * 1000
    #------------------------------------------------
    # ORGANIZING MATRIX OF POSITION IN TIME ACCORDING TO SHAKESPEARE CONTROL
    # UNIT INPUT FILE:
    # Round the elements of the Poss array to 3 decimal places
    Poss = np.round(Poss, 3)
    # Set the first row to zeros - LIMITATION ON SHAKESPEARE CONTROL UNIT
    Poss[0] = 0
    #------------------------------------------------
    # Assembling poistion matrix
    matrix_of_Poss = np.column_stack((matrix_of_Poss, Poss))
    #------------------------------------------------
    # Plotting of the signals
    plt.figure()
    plt.subplot(2, 1, 1)
    # Plot the position along X
    plt.plot(t, Poss/1000)
    plt.plot(t, np.zeros(len(Poss)), 'r--')  # Plot the zero line
    plt.title('Position along X')
    plt.xlabel('time [s]')
    plt.ylabel('Position [m]')
    plt.grid(True)
    plt.minorticks_on()
    plt.grid(which='minor', linestyle=':', linewidth=0.5, color='black')
    plt.subplot(2, 1, 2)
    # Plot the acceleration along X
    plt.plot(t, sine_signal_x)
    plt.plot(t, np.zeros(len(Poss)), 'r--')  # Plot the zero line
    plt.title('Acceleration along X')  # Corrected title placement
    plt.xlabel('time [s]')
    plt.ylabel('Acceleration [m/s^2]')
    plt.grid(True)
    plt.minorticks_on()
    plt.grid(which='minor', linestyle=':', linewidth=0.5, color='black')
    plt.tight_layout()  # Add this line for proper layout
    # Show the plot
    plt.show()
