%% MAIN FILE FOR CRUISE CONTROLLER SYSTEM %%

% (1) initialize
% (2) set implementation parameters
% (3) set simulation parameters
% (4) simulation loop
% (5) plot results

%% (1) Initialize %%

clc 
clear all
close all % house keeping

% define global variables
global sim_step u j u_vec 
% initialize vector where to store results for plots
d_vec = []; u_vec = []; y_vec = [];e_vec = [] ; T_vec = [];
P = 15.0;
I = 1.00; %.0045;
D = 0.0;

%% (2) set implementation parameters %%

% sampling time must be an integer multiple of sim_step
Ts = 0.05; %sim_step.*5; %0.05; 

% number of bits used to encode velocity sensor signal. velocity sensor
% range in mph [0,150]
m_velocityS =0;
M_velocityS = 150;
n_bits_sensor_signal = 16;

% number of bits used to encode velocity demand signal. demand
% range in mph [30,110]
m_demand = 30; M_demand = 110;
n_bits_demand_signal = 16;

% number of bits used to encode throttle/brake actuator signal. actuator
% range in mph [-100,100] where [-100,0) is percentage of braking and
% (0,100] is percentage of throttle
n_bits_actuator_signal = 16;
R = 2.^n_bits_actuator_signal;

%% (3) set simulation parameters %%

sim_step = 0.01; % simulation time step in seconds
sim_time = 70; % simulation time length in seconds
T = 0:sim_step:sim_time; % simulation steps vector
throttle_setting = 50; % initialize control action (throttle/brake setting)

% communication delay between controller and actuator in seconds.
% must be an integer multiple of sim_step
actuator_signal_delay = 0.00;

%% remove when in function
m_ControlSig = -100;
M_ControlSig = 100;
u = 50;
control = 50;

%% (4) simulation loop %%
velocity_real = 60;

for j = 1:length(T) % for every simulation step
    
    loops_remaining = length(T)-j % display progression
    
    % get vehicle raw veocity sensor measurement from the communication bus
    [velocity_raw, velocity_real,throttle_setting] = getRawVelocity(n_bits_sensor_signal,velocity_real,P,throttle_setting);
    
    % decode raw bus signal (from x-bit binary to mph)
    velocity = ((bin2dec(velocity_raw).*(M_velocityS-m_velocityS))/R)+m_velocityS;

    if mod((j-1)*sim_step,Ts) == 0 % every sampling time   
        
        % get driver's velocity demand raw signal from the communication bus
        demand_raw = getRawVelocityDemand(n_bits_sensor_signal); 
        
        % decode raw signal (from x-bit binary to mph)
        demand = 0;% INCORRECT DECODING! INSERT YOUR ENCODING FUNCTION HERE
        
        % calculate control signal (range [-100,100])
        [control, error] = deal(0);
        
        % encode control signal into x-bit binary number
        
        control_raw = num2str(1000000000000000); % INCORRECT ENCODING! INSERT YOUR ENCODING FUNCTION HERE
        
        % send control command to throttle/brake actuator via communication bus
        sendControlCommand(control_raw,n_bits_actuator_signal,actuator_signal_delay);
    end 
    
    % store values for plotting
    e_vec = [e_vec error];
    d_vec = [d_vec demand];
    u_vec = [u_vec control];
    y_vec = [y_vec velocity_real];
    T_vec = [T_vec throttle_setting];
end  


%% (5) plot results %%

figure

% plot velocity response
subplot(2,1,1)
plot(T,d_vec,'g-');hold on
plot(T,y_vec,'b-'); 
plot(T,d_vec-y_vec); grid;
title('Velocity response'); 
subtitle([' Kp = ', num2str(P),' Ki = ', num2str(I),' Kd = ',...
    num2str(D)]); 
ylabel('Velocity (mph)'); xlabel('Time (s)');
legend('Driver Demmand','Real Velocity','error')

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