%% Close Figures, Clear Workspace, and Clear Terminal
close all;
clear;
clc;

%% Call simulation functions
[Y,U,t_total,t_update,Xobs] = forwardIntegrate();
info = getTrajectoryInfo(Y,U,Xobs)

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

load('TestTrack.mat')
load('ROB535_ControlProject_part1_Team3.mat')

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

for i = 1:length(Xobs)
    xvals = Xobs{i}(:,1);
    yvals = Xobs{i}(:,2);
    fill(xvals, yvals, 'm');
end

plot(ROB535_ControlProject_part1_state(:,1), ROB535_ControlProject_part1_state(:,3), 'k--');
plot(Y(:,1), Y(:,3), 'b-');