%% ROB 535 Team 3 Control Project
close all;
clear;
clc;

%% Model Parameters
% Vehicle Parameters
delta_lim = [-0.5, 0.5];
F_x_lim = [-5000, 5000];
m = 1400;
N_w = 2;
f = 0.01;
I_z = 2667;
a = 1.35;
b = 1.45;
B_y = 0.27;
C_y = 1.2;
D_y = 0.7;
E_y = -1.6;
S_hy = 0;
S_vy = 0;
g = 9.806;
% Note: The Pacejka parameters are for the slip angle in degrees

% Initial Conditions
x_0 = 287; % meters
u_0 = 5; % meters/second
y_0 = -176; % meters
v_0 = 0; % meters/second
psi_0 = 2; % radians
r_0 = 0; % radians/second
state_0 = [x_0, u_0, y_0, v_0, psi_0, r_0];

load('TestTrack.mat')

%% Trajectory Synthesis
% Segment 1
segment_num = 1;
num_pts(segment_num) = 3e2;
delta_vals(segment_num) = -0.010;
F_x_vals(segment_num) = 2500;

% Segment 2
segment_num = 2;
num_pts(segment_num) = 4e2;
delta_vals(segment_num) = 0.001;
F_x_vals(segment_num) = 2500;

% Segment 3
segment_num = 3;
num_pts(segment_num) = 5.5e2;
delta_vals(segment_num) = -0.0255;
F_x_vals(segment_num) = 0;

% Segment 4
segment_num = 4;
num_pts(segment_num) = 1e2;
delta_vals(segment_num) = 0.017;
F_x_vals(segment_num) = 0;

% Segment 5
segment_num = 5;
num_pts(segment_num) = 3e2;
delta_vals(segment_num) = -0.002;
F_x_vals(segment_num) = 2500;

% Segment 6
segment_num = 6;
num_pts(segment_num) = 3.5e2;
delta_vals(segment_num) = 0.0175;
F_x_vals(segment_num) = -800;

% Segment 7
segment_num = 7;
num_pts(segment_num) = 2e2;
delta_vals(segment_num) = -0.025;
F_x_vals(segment_num) = 0;

% Segment 8
segment_num = 8;
num_pts(segment_num) = 1.85e2;
delta_vals(segment_num) = -0.025;
F_x_vals(segment_num) = -2000;

% Segment 9
segment_num = 9;
num_pts(segment_num) = 3e2;
delta_vals(segment_num) = 0.04;
F_x_vals(segment_num) = -500;

% Segment 10
segment_num = 10;
num_pts(segment_num) = 3e2;
delta_vals(segment_num) = 0.0175;
F_x_vals(segment_num) = 0;

% Segment 11
segment_num = 11;
num_pts(segment_num) = 5.5e2;
delta_vals(segment_num) = -0.03;
F_x_vals(segment_num) = 0;

% Segment 12
segment_num = 12;
num_pts(segment_num) = 3e2;
delta_vals(segment_num) = -0.02;
F_x_vals(segment_num) = 0;

% Segment 13
segment_num = 13;
num_pts(segment_num) = 2e2;
delta_vals(segment_num) = -0.03;
F_x_vals(segment_num) = 0;

% Segment 14
segment_num = 14;
num_pts(segment_num) = 4e2;
delta_vals(segment_num) = -0.0125;
F_x_vals(segment_num) = 0;

% Segment 15
segment_num = 15;
num_pts(segment_num) = 4e2;
delta_vals(segment_num) = 0.05;
F_x_vals(segment_num) = 0;

% Segment 16
segment_num = 16;
num_pts(segment_num) = 6e2;
delta_vals(segment_num) = 0.015;
F_x_vals(segment_num) = 0;

% Segment 17
segment_num = 17;
num_pts(segment_num) = 7e2;
delta_vals(segment_num) = -0.045;
F_x_vals(segment_num) = 0;

% Segment 18
segment_num = 18;
num_pts(segment_num) = 2e2;
delta_vals(segment_num) = -0.07;
F_x_vals(segment_num) = -1000;

% Segment 19
segment_num = 19;
num_pts(segment_num) = 5.5e2;
delta_vals(segment_num) = 0.071;
F_x_vals(segment_num) = 0;

% Segment 20
segment_num = 20;
num_pts(segment_num) = 1e2;
delta_vals(segment_num) = 0;
F_x_vals(segment_num) = 2500;

% Segment 21
segment_num = 21;
num_pts(segment_num) = 5e2;
delta_vals(segment_num) = 0.001;
F_x_vals(segment_num) = 2500;

% Segment 22
segment_num = 22;
num_pts(segment_num) = 2e2;
delta_vals(segment_num) = 0.02;
F_x_vals(segment_num) = -800;

% Segment 23
segment_num = 23;
num_pts(segment_num) = 2e2;
delta_vals(segment_num) = 0.0165;
F_x_vals(segment_num) = 0;

% Segment 24
segment_num = 24;
num_pts(segment_num) = 1e2;
delta_vals(segment_num) = -0.005;
F_x_vals(segment_num) = 0;

% Segment 25
segment_num = 25;
num_pts(segment_num) = 8e2;
delta_vals(segment_num) = 0;
F_x_vals(segment_num) = 2500;

%% Load Inputs from File
load('ROB535_ControlProject_part1_Team3.mat');

%% Simulate Trajectory
for i = 1:length(num_pts)
    [start_idx, end_idx] = get_indices(i, num_pts);

    delta = delta_vals(i);
    F_x = F_x_vals(i);
    U(start_idx:end_idx,:) = [delta * ones(num_pts(i),1), F_x * ones(num_pts(i),1)];
end

[Y, T] = forwardIntegrateControlInput(U, state_0);
info = getTrajectoryInfo(Y,U)

[Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, state_0);
info = getTrajectoryInfo(Y_submission,ROB535_ControlProject_part1_input)

%% Figures
% Plot segmented trajectory for debugging purposes
figure(1)
hold on;
grid on;

for i = 1:length(num_pts)
    [start_idx, end_idx] = get_indices(i, num_pts);
    plot(Y(start_idx:end_idx,1), Y(start_idx:end_idx,3), '-');
end

plot(TestTrack.bl(1,:), TestTrack.bl(2,:), 'r.-');
plot(TestTrack.br(1,:), TestTrack.br(2,:), 'r.-');
plot(TestTrack.cline(1,:), TestTrack.cline(2,:), 'g.-');

% Plot final trajectory from submission inputs
figure(2)
hold on;
grid on;

plot(Y_submission(:,1), Y_submission(:,3), 'k--');
plot(TestTrack.bl(1,:), TestTrack.bl(2,:), 'r.-');
plot(TestTrack.br(1,:), TestTrack.br(2,:), 'r.-');
plot(TestTrack.cline(1,:), TestTrack.cline(2,:), 'g.-');

%% Functions
function [start_idx, end_idx] = get_indices(segment_num, num_pts)
    if segment_num == 1
        start_idx = 1;
        end_idx = num_pts(segment_num);
    else
        start_idx = sum(num_pts(1:segment_num-1)) + 1;
        end_idx = sum(num_pts(1:segment_num));
    end
end