%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%CRUISE CONTROL PERFORMANCE

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc; clear; close all;

% --- STEP 1: CHOOSE YOUR SCENARIO ---
% 1 = Baseline PID (After) | 2 = P-Only (Before) | 3 = 4-bit Quantization 
% 4 = Long Sampling (0.5s) | 5 = Network Delay (0.2s) | 6 = Disturbance (Hill)
scenario = 1; 

% Default Parameters
Kp = 1.9; Ki = 0.55; Kd = 0.1; % Optimized gains
Ts = 0.05; bits = 16; delay = 0.00; hill_slope = 0;

% --- STEP 2: APPLY SCENARIO BREAKPOINTS ---
if scenario == 2 % BEFORE: Poor Tuning
    Kp = 1.0; Ki = 0; Kd = 0; 
elseif scenario == 3 % BIT RESOLUTION TEST
    bits = 4;
elseif scenario == 4 % SAMPLING TIME TEST
    Ts = 0.5; 
elseif scenario == 5 % NETWORK DELAY TEST
    delay = 0.25; 
elseif scenario == 6 % REAL-WORLD DISTURBANCE
    hill_slope = 1; % Activates a hill at t=40s
end

% Simulation Settings
dt = 0.01; T_end = 70; time = 0:dt:T_end;
R = 2^bits; [v_vec, d_vec, u_vec] = deal(zeros(size(time)));
v_act = 60; u_cmd = 50; act_pos = 50; int_err = 0; last_err = 0; current_u = 50;

%% --- STEP 3: SIMULATION ENGINE ---
for i = 1:length(time)
    % External Disturbance (Hill at 40s-50s)
    dist = 0; if hill_slope == 1 && time(i) > 40 && time(i) < 55, dist = -6; end
    
    [v_raw, v_act, act_pos] = getRawVelocity(bits, v_act + dist, act_pos, dt, u_cmd);
    v_fb = (bin2dec(v_raw) * (150 - 0) / R) + 0;
    
    if mod((i-1)*dt, Ts) == 0
        d_raw = getRawVelocityDemand(bits, dt, i);
        v_ref = (bin2dec(d_raw) * (110 - 30) / R) + 30;
        
        % PID Calculation
        err = v_ref - v_fb;
        int_err = int_err + (err * Ts);
        der_err = (err - last_err) / Ts;
        current_u = (Kp * err) + (Ki * int_err) + (Kd * der_err);
        last_err = err;
        current_u = max(min(current_u, 100), -100); % Saturation
        
        % Encoding/Sending
        u_raw_dec = ((current_u - (-100)) * R) / (100 - (-100));
        u_raw_str = dec2bin(round(u_raw_dec), bits);
        u_cmd = sendControlCommand(u_raw_str, bits, delay, i, dt, u_vec);
    end
    v_vec(i) = v_act; d_vec(i) = v_ref; u_vec(i) = u_cmd;
end

%% --- STEP 4: PLOTTING ---
figure('Color', 'w', 'Name', ['Scenario ', num2str(scenario)]);
subplot(2,1,1); plot(time, d_vec, 'r--', time, v_vec, 'b', 'LineWidth', 1.5);
ylabel('Velocity (mph)'); title(['Scenario ', num2str(scenario), ': Velocity Response']); grid on;
subplot(2,1,2); stairs(time, u_vec, 'k');
ylabel('Throttle %'); xlabel('Time (s)'); title('Control Effort'); grid on;