%% MAIN FILE FOR CRUISE CONTROLLER SYSTEM
clc; clear; close all
% PID gains
P = 2.5;
I = 0.8;
D = 0.2;
%% (2) Implementation parameters
Ts = 0.05;
% sensor encoding
m_velocityS = 0;   
M_velocityS = 150;   
n_bits_sensor_signal = 16;

% demand encoding
m_demand = 30;      
M_demand = 110;      
n_bits_demand_signal = 16;

% actuator encoding
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;
% PID variables
integral_error = 0;
prev_error = 0;

[u, control] = deal(50);
velocity_real = 60;

error = 0;
demand = 60;

%% (4) Simulation loop
for j = 1:length(T)

    % get vehicle velocity
    [velocity_raw, velocity_real, throttle_setting] = getRawVelocity(...
        n_bits_sensor_signal, velocity_real, throttle_setting, sim_step, u);

    % decode velocity
    velocity = ((bin2dec(velocity_raw) * (M_velocityS - m_velocityS)) / ...
                (2^n_bits_sensor_signal)) + m_velocityS;

    if mod((j-1)*sim_step, Ts) == 0

        % driver demand
        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;

        % PID controller
        error = demand - velocity;
        integral_error = integral_error + error * Ts;
        derivative = (error - prev_error) / Ts;

        control = P*error + I*integral_error + D*derivative;
        % saturation
        if abs(control) > 100
            control = max(-100, min(100, control));
            integral_error = integral_error - error * Ts;
        end

        prev_error = error;

        % encode actuator signal
        control_raw_dec = round((control - m_ControlSig) * ...
            (2^n_bits_actuator_signal) / (M_ControlSig - m_ControlSig));

        control_raw = dec2bin(control_raw_dec, n_bits_actuator_signal);

        % send command
        u = sendControlCommand(control_raw, n_bits_actuator_signal, ...
                               actuator_signal_delay, j, sim_step, u_vec);
    end

    % store values
    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

subplot(2,1,1)
plot(T, d_vec, 'g-'); hold on
plot(T, y_vec, 'b-')
plot(T, d_vec - y_vec, 'r--')
grid
title('Velocity response')
ylabel('Velocity (mph)')
xlabel('Time (s)')
legend('Driver Demand','Real Velocity','Error')

subplot(2,1,2)
stairs(T, u_vec, 'g-')
hold on
plot(T, Th_vec, 'r-')
grid
title('Throttle / Brake effort')
ylabel('% throttle/brake')
xlabel('Time (s)')
legend('Control %','Throttle position')