%% MAIN FILE FOR CRUISE CONTROLLER SYSTEM
clc; clear; close all

% PID gains 
P = 2.5; I = 1.5; D = 0.2;     

%% (2) Implementation parameters
Ts = 0.05;    % Sampling time (seconds)
% Encoding parameters
m_velocityS = 0; M_velocityS = 150; n_bits_sensor_signal = 16;
m_demand = 30; M_demand = 110; n_bits_demand_signal = 16;
n_bits_actuator_signal = 16; m_ControlSig = -100; M_ControlSig = 100;

%% (3) Simulation parameters
sim_step = 0.01; sim_time = 70;
T = 0:sim_step:sim_time;
[d_vec, u_vec, y_vec, e_vec, Th_vec] = deal(nan(size(T)));

throttle_setting = 50; actuator_signal_delay = 0.00;
integral_error = 0; prev_error = 0;
[u, control] = deal(50); velocity_real = 60;
error = 0; demand = 60;
acc_vec = zeros(size(T)); velocity_prev = 60;

%% (4) Simulation loop
for j = 1:length(T)
    [velocity_raw, velocity_real, throttle_setting] = getRawVelocity(...
        n_bits_sensor_signal, velocity_real, throttle_setting, sim_step, u);
    velocity = ((bin2dec(velocity_raw) * (M_velocityS - m_velocityS)) / ...
                (2^n_bits_sensor_signal)) + m_velocityS;
    
    velocity_ms = velocity_real * 0.44704;
    velocity_prev_ms = velocity_prev * 0.44704;
    acc_vec(j) = (velocity_ms - velocity_prev_ms) / sim_step;
    velocity_prev = velocity_real;

    if mod((j-1)*sim_step, Ts) == 0
        demand_raw = getRawVelocityDemand(n_bits_demand_signal, sim_step, j);
        demand_dec = bin2dec(demand_raw);
        demand = demand_dec * (M_demand - m_demand) / (2^n_bits_demand_signal) + m_demand;
        
        error = demand - velocity;
        integral_error = integral_error + (error * Ts);
        derivative = (error - prev_error) / Ts;
        control = P*error + I*integral_error + D*derivative;
        
        if control > 100; control = 100; if error * (control - 100) > 0; integral_error = integral_error - error * Ts; end
        elseif control < -100; control = -100; if error * (control + 100) < 0; integral_error = integral_error - error * Ts; end; end
        
        integral_error = max(-500, min(500, integral_error));
        prev_error = error;
        
        control_raw_dec = round((control - m_ControlSig) * (2^n_bits_actuator_signal) / (M_ControlSig - m_ControlSig));
        control_raw_dec = max(0, min(2^n_bits_actuator_signal - 1, control_raw_dec));
        control_raw = dec2bin(control_raw_dec, n_bits_actuator_signal);
        u = sendControlCommand(control_raw, n_bits_actuator_signal, actuator_signal_delay, j, sim_step, u_vec);
    end
    e_vec(j) = error; d_vec(j) = demand; u_vec(j) = control; y_vec(j) = velocity_real; Th_vec(j) = throttle_setting;
end

%% (5) Plot results
figure('Position', [100, 100, 900, 600])
subplot(2,1,1)
plot(T, d_vec, 'g-', 'LineWidth', 1.5); hold on; plot(T, y_vec, 'b-', 'LineWidth', 1.5); plot(T, d_vec - y_vec, 'r--', 'LineWidth', 1)
grid on; title('Velocity Response with PID Controller', 'FontSize', 12); ylabel('Velocity (mph)'); xlabel('Time (s)')
legend('Driver Demand', 'Real Velocity', 'Error', 'Location', 'best'); xlim([0 sim_time])
subplot(2,1,2)
stairs(T, u_vec, 'g-', 'LineWidth', 1.5); hold on; plot(T, Th_vec, 'r-', 'LineWidth', 1.5)
grid on; title('Throttle / Brake Control Effort'); ylabel('% throttle/brake'); xlabel('Time (s)')
legend('Control Signal', 'Throttle Position', 'Location', 'best'); xlim([0 sim_time])

%% (6) Performance metrics
fprintf('\n=== PERFORMANCE METRICS ===\n');
fprintf('Final Steady-State Error: %.3f mph\n', abs(e_vec(end)));
fprintf('Control Range: %.1f to %.1f %%\n', min(u_vec), max(u_vec));

%% ============================================
% QUANTIZATION EFFECTS (16,8,6,4 bits)
% ============================================
fprintf('\n=== Running Quantization Analysis ===\n');
bit_levels = [16, 8, 6, 4]; quant_velocities = zeros(length(bit_levels), length(T));
for b = 1:length(bit_levels)
    bits = bit_levels(b); fprintf('Testing %d bits...\n', bits);
    throttle_setting = 50; integral_error = 0; prev_error = 0; u = 50; velocity_real = 60; error = 0; demand = 60;
    for j = 1:length(T)
        [velocity_raw, velocity_real, throttle_setting] = getRawVelocity(bits, velocity_real, throttle_setting, sim_step, u);
        velocity = ((bin2dec(velocity_raw) * (M_velocityS - m_velocityS)) / (2^bits)) + m_velocityS;
        if mod((j-1)*sim_step, Ts) == 0
            demand_raw = getRawVelocityDemand(bits, sim_step, j);
            demand_dec = bin2dec(demand_raw);
            demand = demand_dec * (M_demand - m_demand) / (2^bits) + m_demand;
            error = demand - velocity;
            integral_error = integral_error + (error * Ts);
            derivative = (error - prev_error) / Ts;
            control = P*error + I*integral_error + D*derivative;
            if control > 100; control = 100; if error * (control - 100) > 0; integral_error = integral_error - error * Ts; end
            elseif control < -100; control = -100; if error * (control + 100) < 0; integral_error = integral_error - error * Ts; end; end
            integral_error = max(-500, min(500, integral_error)); prev_error = error;
            control_raw_dec = round((control - m_ControlSig) * (2^bits) / (M_ControlSig - m_ControlSig));
            control_raw_dec = max(0, min(2^bits - 1, control_raw_dec));
            control_raw = dec2bin(control_raw_dec, bits);
            u = sendControlCommand(control_raw, bits, actuator_signal_delay, j, sim_step, u_vec);
        end
        quant_velocities(b, j) = velocity_real;
    end
end

figure('Position', [100, 100, 1000, 600]); colors = {'b', [0 0.7 0], [0.8 0.8 0], 'r'}; hold on
for b = 1:length(bit_levels); plot(T, quant_velocities(b, :), 'Color', colors{b}, 'LineWidth', 1.2, 'DisplayName', sprintf('%d-bit', bit_levels(b))); end
plot(T, d_vec, 'k--', 'LineWidth', 1.5, 'DisplayName', 'Demand'); grid on
title('Effect of Bit Resolution on Velocity Response', 'FontSize', 12, 'FontWeight', 'bold')
xlabel('Time (s)'); ylabel('Velocity (mph)'); legend('Location', 'best'); xlim([0 sim_time]); ylim([0 100])
axes('Position', [0.62 0.62 0.25 0.25]); hold on
zoom_start = 4500; zoom_end = 5500;
for b = 1:length(bit_levels); plot(T(zoom_start:zoom_end), quant_velocities(b, zoom_start:zoom_end), 'Color', colors{b}, 'LineWidth', 1.2); end
plot(T(zoom_start:zoom_end), d_vec(zoom_start:zoom_end), 'k--', 'LineWidth', 1); grid on
title('Zoom (45-55s) - Quantization Noise', 'FontSize', 9); xlabel('Time (s)'); ylabel('Velocity (mph)'); xlim([45 55])

%% ============================================
% SAMPLING TIME EFFECTS (0.05,0.2,0.5,0.8,1.0 s)
% ============================================
fprintf('\n=== Running Sampling Time Analysis ===\n');
Ts_levels = [0.05, 0.2, 0.5, 0.8, 1.0]; Ts_velocities = zeros(length(Ts_levels), length(T)); Ts_stable = zeros(length(Ts_levels), 1);
for t = 1:length(Ts_levels)
    Ts_test = Ts_levels(t); fprintf('Testing Ts = %.2f s...\n', Ts_test);
    throttle_setting = 50; integral_error = 0; prev_error = 0; u = 50; velocity_real = 60; error = 0; demand = 60;
    for j = 1:length(T)
        [velocity_raw, velocity_real, throttle_setting] = getRawVelocity(n_bits_sensor_signal, velocity_real, throttle_setting, sim_step, u);
        velocity = ((bin2dec(velocity_raw) * (M_velocityS - m_velocityS)) / (2^n_bits_sensor_signal)) + m_velocityS;
        if mod((j-1)*sim_step, Ts_test) == 0
            demand_raw = getRawVelocityDemand(n_bits_demand_signal, sim_step, j);
            demand_dec = bin2dec(demand_raw);
            demand = demand_dec * (M_demand - m_demand) / (2^n_bits_demand_signal) + m_demand;
            error = demand - velocity;
            integral_error = integral_error + (error * Ts_test);
            derivative = (error - prev_error) / Ts_test;
            control = P*error + I*integral_error + D*derivative;
            if control > 100; control = 100; if error * (control - 100) > 0; integral_error = integral_error - error * Ts_test; end
            elseif control < -100; control = -100; if error * (control + 100) < 0; integral_error = integral_error - error * Ts_test; end; end
            integral_error = max(-500, min(500, integral_error)); prev_error = error;
            control_raw_dec = round((control - m_ControlSig) * (2^n_bits_actuator_signal) / (M_ControlSig - m_ControlSig));
            control_raw_dec = max(0, min(2^n_bits_actuator_signal - 1, control_raw_dec));
            control_raw = dec2bin(control_raw_dec, n_bits_actuator_signal);
            u = sendControlCommand(control_raw, n_bits_actuator_signal, actuator_signal_delay, j, sim_step, u_vec);
        end
        if velocity_real > 200 || velocity_real < 0 || isnan(velocity_real); Ts_stable(t) = 0; Ts_velocities(t, j:end) = NaN; break;
        else; Ts_stable(t) = 1; Ts_velocities(t, j) = velocity_real; end
    end
end

figure('Position', [100, 100, 1000, 600]); Ts_colors = {'b', [0 0.7 0], [0.8 0.8 0], 'm', 'r'};
subplot(2,1,1); hold on
for t = 1:length(Ts_levels)
    if Ts_stable(t) == 1; plot(T, Ts_velocities(t, :), 'Color', Ts_colors{t}, 'LineWidth', 1.2, 'DisplayName', sprintf('Ts = %.2f s', Ts_levels(t)));
    else; plot(T, Ts_velocities(t, :), 'Color', Ts_colors{t}, 'LineWidth', 1.2, 'LineStyle', '--', 'DisplayName', sprintf('Ts = %.2f s (UNSTABLE)', Ts_levels(t))); end
end
plot(T, d_vec, 'k--', 'LineWidth', 1.5, 'DisplayName', 'Demand'); grid on
title('Effect of Sampling Time on Velocity Response'); ylabel('Velocity (mph)'); xlabel('Time (s)'); legend('Location', 'best'); xlim([0 sim_time])
subplot(2,1,2); hold on; step_start = 3500; step_end = 4500;
for t = 1:length(Ts_levels); if Ts_stable(t) == 1; plot(T(step_start:step_end), Ts_velocities(t, step_start:step_end), 'Color', Ts_colors{t}, 'LineWidth', 1.2); end; end
plot(T(step_start:step_end), d_vec(step_start:step_end), 'k--', 'LineWidth', 1.5); grid on
title('Step Response at 35s (Demand 55-->70 mph)'); ylabel('Velocity (mph)'); xlabel('Time (s)'); legend('Ts = 0.05s', 'Ts = 0.2s', 'Ts = 0.5s', 'Location', 'best'); xlim([35 45])

%% ============================================
% COMMUNICATION DELAY EFFECTS (0,0.1,0.2,0.25,0.3 s)
% ============================================
fprintf('\n=== Running Communication Delay Analysis ===\n');
delay_levels = [0.00, 0.10, 0.20, 0.25, 0.30]; delay_velocities = zeros(length(delay_levels), length(T)); delay_stable = zeros(length(delay_levels), 1);
for dly = 1:length(delay_levels)
    delay_test = delay_levels(dly); fprintf('Testing delay = %.2f s...\n', delay_test);
    throttle_setting = 50; integral_error = 0; prev_error = 0; u = 50; velocity_real = 60; error = 0; demand = 60;
    delay_buffer = []; delay_steps = round(delay_test / sim_step);
    for j = 1:length(T)
        [velocity_raw, velocity_real, throttle_setting] = getRawVelocity(n_bits_sensor_signal, velocity_real, throttle_setting, sim_step, u);
        velocity = ((bin2dec(velocity_raw) * (M_velocityS - m_velocityS)) / (2^n_bits_sensor_signal)) + m_velocityS;
        if mod((j-1)*sim_step, Ts) == 0
            demand_raw = getRawVelocityDemand(n_bits_demand_signal, sim_step, j);
            demand_dec = bin2dec(demand_raw);
            demand = demand_dec * (M_demand - m_demand) / (2^n_bits_demand_signal) + m_demand;
            error = demand - velocity;
            integral_error = integral_error + (error * Ts);
            derivative = (error - prev_error) / Ts;
            control = P*error + I*integral_error + D*derivative;
            if control > 100; control = 100; if error * (control - 100) > 0; integral_error = integral_error - error * Ts; end
            elseif control < -100; control = -100; if error * (control + 100) < 0; integral_error = integral_error - error * Ts; end; end
            integral_error = max(-500, min(500, integral_error)); prev_error = error;
            control_raw_dec = round((control - m_ControlSig) * (2^n_bits_actuator_signal) / (M_ControlSig - m_ControlSig));
            control_raw_dec = max(0, min(2^n_bits_actuator_signal - 1, control_raw_dec));
            control_raw = dec2bin(control_raw_dec, n_bits_actuator_signal);
            if delay_steps > 0; delay_buffer = [delay_buffer; u];
                if length(delay_buffer) > delay_steps; u = delay_buffer(1); delay_buffer(1) = [];
                else; u = 50; end
            else; u = sendControlCommand(control_raw, n_bits_actuator_signal, delay_test, j, sim_step, u_vec); end
        end
        if velocity_real > 200 || velocity_real < 0 || isnan(velocity_real); delay_stable(dly) = 0; delay_velocities(dly, j:end) = NaN; break;
        else; delay_stable(dly) = 1; delay_velocities(dly, j) = velocity_real; end
    end
end

figure('Position', [100, 100, 1000, 600]); delay_colors = {'b', [0 0.7 0], [0.8 0.8 0], 'm', 'r'}; hold on
for dly = 1:length(delay_levels)
    if delay_stable(dly) == 1; plot(T, delay_velocities(dly, :), 'Color', delay_colors{dly}, 'LineWidth', 1.2, 'DisplayName', sprintf('Delay = %.2f s', delay_levels(dly)));
    else; plot(T, delay_velocities(dly, :), 'Color', delay_colors{dly}, 'LineWidth', 1.2, 'LineStyle', '--', 'DisplayName', sprintf('Delay = %.2f s (UNSTABLE)', delay_levels(dly))); end
end
plot(T, d_vec, 'k--', 'LineWidth', 1.5, 'DisplayName', 'Demand'); grid on
title('Effect of Communication Delay on Velocity Response'); ylabel('Velocity (mph)'); xlabel('Time (s)'); legend('Location', 'best'); xlim([0 sim_time])

%% ============================================
% ISO 2631 PASSENGER COMFORT ANALYSIS
% ============================================
fprintf('\n=== ISO 2631 Comfort Analysis ===\n');
velocity_store = zeros(size(T)); throttle_setting = 50; integral_error = 0; prev_error = 0; u = 50; velocity_real = 60; error = 0; demand = 60;
for j = 1:length(T)
    [velocity_raw, velocity_real, throttle_setting] = getRawVelocity(n_bits_sensor_signal, velocity_real, throttle_setting, sim_step, u);
    velocity = ((bin2dec(velocity_raw) * (M_velocityS - m_velocityS)) / (2^n_bits_sensor_signal)) + m_velocityS;
    velocity_store(j) = velocity_real;
    if mod((j-1)*sim_step, Ts) == 0
        demand_raw = getRawVelocityDemand(n_bits_demand_signal, sim_step, j);
        demand_dec = bin2dec(demand_raw);
        demand = demand_dec * (M_demand - m_demand) / (2^n_bits_demand_signal) + m_demand;
        error = demand - velocity;
        integral_error = integral_error + (error * Ts);
        derivative = (error - prev_error) / Ts;
        control = P*error + I*integral_error + D*derivative;
        if control > 100; control = 100; if error * (control - 100) > 0; integral_error = integral_error - error * Ts; end
        elseif control < -100; control = -100; if error * (control + 100) < 0; integral_error = integral_error - error * Ts; end; end
        integral_error = max(-500, min(500, integral_error)); prev_error = error;
        control_raw_dec = round((control - m_ControlSig) * (2^n_bits_actuator_signal) / (M_ControlSig - m_ControlSig));
        control_raw_dec = max(0, min(2^n_bits_actuator_signal - 1, control_raw_dec));
        control_raw = dec2bin(control_raw_dec, n_bits_actuator_signal);
        u = sendControlCommand(control_raw, n_bits_actuator_signal, actuator_signal_delay, j, sim_step, u_vec);
    end
end

velocity_ms = velocity_store * 0.44704;
acc_diff = diff(velocity_ms) / sim_step;
acc_simple = [0; acc_diff(:)]'; 
acc_smooth = movmean(acc_simple, 300);
start_idx = round(10 / sim_step);
acc_analysis = max(min(acc_smooth(start_idx:end), 2.5), -2.5);
rms_acc = rms(acc_analysis); max_acc = max(abs(acc_analysis));
jerk_smooth = movmean(diff(acc_smooth) / sim_step, 200);
max_jerk = max(abs(jerk_smooth(start_idx:end)));

if rms_acc < 0.315; comfort = 'Comfortable'; elseif rms_acc < 0.63; comfort = 'A little uncomfortable';
elseif rms_acc < 1.0; comfort = 'Fairly uncomfortable'; else; comfort = 'Very uncomfortable'; end

fprintf('RMS: %.2f m/s² | Max: %.2f m/s² | Jerk: %.0f m/s³ | Classification: %s\n', rms_acc, max_acc, max_jerk, comfort);

figure('Position', [100, 100, 1200, 450]);
subplot(1,3,1); plot(T, acc_smooth, 'b-', 'LineWidth', 1.5); hold on; yline(0.315, 'g--'); yline(0.63, 'r--'); yline(0, 'k-'); grid on
title('Vehicle Acceleration'); xlabel('Time (s)'); ylabel('Acceleration (m/s²)'); xlim([0 sim_time]); ylim([-2 2.5])
subplot(1,3,2); histogram(acc_analysis, 40, 'FaceColor', [0.3 0.6 0.9], 'EdgeColor', 'none'); grid on
title('Acceleration Distribution'); xlabel('Acceleration (m/s²)'); ylabel('Frequency'); xlim([-1.5 2]); hold on; xline(rms_acc, 'r--', 'LineWidth', 2)
subplot(1,3,3); axis off; text(0.1, 0.7, sprintf('ISO 2631 Results\n\nRMS: %.2f m/s²\nMax: %.2f m/s²\nJerk: %.0f m/s³\n\nClassification: %s', rms_acc, max_acc, max_jerk, comfort), 'FontSize', 12, 'FontWeight', 'bold')
sgtitle('ISO 2631 Passenger Comfort Analysis', 'FontSize', 14, 'FontWeight', 'bold')