% Run the main script for the cartpole simulation setup.
run('cartpole_main.m') 

% Open and load the Simulink model for the cartpole.
open_system('cartpoleF')
load_system('cartpoleF')

% Initialize a SimulationInput object for Simulink simulation.
in = Simulink.SimulationInput();

% Define simulation parameters: end time, simulation time step, and sample time.
T_end = 20;   % Total simulation time in seconds.
Tsim = 1e-3;  % Simulation time step (1 millisecond).
Ts = 1e-2;    % Sample time for the controller.

% Number of simulations to run.
n_sim = 1;

% Initialize an array to store output data for each simulation.
output = zeros(T_end/Tsim+1, 4, n_sim);

% Run simulations in a loop.
for i = 1:n_sim
    % Generate random constants for the simulation parameters.
    const = rand(4,1) * 2e-4; % Four random numbers scaled by 2e-4.
    r1 = const(1); % Random constant 1.
    r2 = const(2); % Random constant 2.
    q1 = const(3); % Random constant 3.
    q2 = const(4); % Random constant 4.

    % Run the simulation.
    out = sim('cartpoleF');
    
    % Extract time and output data from the simulation results.
    time = out.simout.Time;
    output = out.simout.Data;
    
    % Monitoring property with Taliro: check if theta stays within a range.
    st_spec2 = '[] (a1 /\ a2)'; % Temporal logic formula.

    % a1: (theta < 0.5)
    st_spec2_Pred(1).str = 'a1';
    st_spec2_Pred(1).A = [0 0 1 0];
    st_spec2_Pred(1).b = 0.5;

    % a2:  (theta >-0.5)
    st_spec2_Pred(2).str = 'a2';
    st_spec2_Pred(2).A = [0 0 -1 0];
    st_spec2_Pred(2).b = 0.5;

    % Compute the robustness for the specification using Taliro.
    rob_tal = fw_taliro(st_spec2, st_spec2_Pred, output, time)

    %Plot
    plot(time, output(:,3), 'LineWidth',3)
    title('theta','FontSize',fontsize )
end