%% Part 1 Testing Xenia's Code
close all;
figure(1)
hold on
plot(Y_ref(1,:), Y_ref(3,:), 'g-')
plot(Y(1,:), Y(3,:), 'b-')

[Y_test, T_test] = forwardIntegrateControlInput(U', init);
traj_info = getTrajectoryInfo(Y_test(1:439,:), U(:,1:439)')
plot(Y_test(:,1), Y_test(:,3), 'k-')

plot(TestTrack.bl(1,:), TestTrack.bl(2,:), 'r.')
plot(TestTrack.br(1,:), TestTrack.br(2,:), 'r.')

%% Part 2 Symbolic Jacobians for Linearized System
close all;
clear all;
clc;

Nw  = 2;
f   = 0.01;
Iz  = 2667;
a   = 1.35;
b   = 1.45;
By  = 0.27;
Cy  = 1.2;
Dy  = 0.7;
Ey  = -1.6;
Shy = 0;
Svy = 0;
m   = 1400;
g   = 9.806;

syms x_var u_var y_var v_var psi_var r_var ... % states
                 delta_f_var F_x_var ...                   % inputs
                 F_yf_var F_yr_var                         % lateral forces

% Vehicle Dynamics (Equation 1)
dx   = u_var*cos(psi_var) - v_var*sin(psi_var);
du   = (1/m)*(-f*m*g + Nw*F_x_var - F_yf_var*sin(delta_f_var)) + v_var*r_var;
dy   = u_var*sin(psi_var) + v_var*cos(psi_var);
dv   = (1/m)*(F_yf_var*cos(delta_f_var) + F_yr_var) - u_var*r_var;
dpsi = r_var;
dr   = (1/Iz)*(a*F_yf_var*cos(delta_f_var) - b*F_yr_var);

% Jacobians of continuous-time linearized system
% TODO: Change A_c & B_c computation to regular functions (e.g.,
% using `matlabFunction`) if speed is an issue
A_c_symb = [ ...
    diff(dx,  x_var), diff(dx,  u_var), diff(dx,  y_var), diff(dx,  v_var), diff(dx,  psi_var), diff(dx,  r_var);
    diff(du,  x_var), diff(du,  u_var), diff(du,  y_var), diff(du,  v_var), diff(du,  psi_var), diff(du,  r_var);
    diff(dy,  x_var), diff(dy,  u_var), diff(dy,  y_var), diff(dy,  v_var), diff(dy,  psi_var), diff(dy,  r_var);
    diff(dv,  x_var), diff(dv,  u_var), diff(dv,  y_var), diff(dv,  v_var), diff(dv,  psi_var), diff(dv,  r_var);
    diff(dpsi,x_var), diff(dpsi,u_var), diff(dpsi,y_var), diff(dpsi,v_var), diff(dpsi,psi_var), diff(dpsi,r_var);
    diff(dr,  x_var), diff(dr,  u_var), diff(dr,  y_var), diff(dr,  v_var), diff(dr,  psi_var), diff(dr,  r_var);
];

B_c_symb = [ ...
    diff(dx,  delta_f_var), diff(dx,  F_x_var);
    diff(du,  delta_f_var), diff(du,  F_x_var);
    diff(dy,  delta_f_var), diff(dy,  F_x_var);
    diff(dv,  delta_f_var), diff(dv,  F_x_var);
    diff(dpsi,delta_f_var), diff(dpsi,F_x_var);
    diff(dr,  delta_f_var), diff(dr,  F_x_var);
];

A_c = matlabFunction(A_c_symb, 'File', 'A_c_func');
B_c = matlabFunction(B_c_symb, 'File', 'B_c_func');

%% Part 2 Symbolic Jacobians for Track Constraint
close all;
clear all;
clc;

syms x_err y_err x_ref y_ref v1_x v1_y v2_x v2_y
x = x_err + x_ref;
y = y_err + y_ref;

pt = [x; y; 0];
v1 = [v1_x; v1_y; 0];
v2 = [v2_x; v2_y; 0];

% Compute vectors
a       = v2 - v1; % vector from v1 to v2
b       = pt - v1; % vector from v1 to pt
cross_p = cross(a,b);

% Compute distance & direction
dist        = norm(cross_p) / norm(a);
direction   = sign(cross_p(3));
signed_dist = dist * direction;

temp1 = matlabFunction(jacobian(signed_dist, x_err), 'File', 'signed_dist_x_err');
temp2 = matlabFunction(jacobian(signed_dist, y_err), 'File', 'signed_dist_y_err');

%% Part 2 Symbolic Jacobians for Obstacle Constraint
close all;
clear all;
clc;

syms x_err y_err x_ref y_ref rad_obstacle cen_obstacle_x cen_obstacle_y
x = x_err + x_ref;
y = y_err + y_ref;

obstacle_func = ...
    (rad_obstacle)^2 ...
    - (x - cen_obstacle_x)^2 ...
    - (y - cen_obstacle_y)^2;

temp1 = matlabFunction(jacobian(obstacle_func, x_err), 'File', 'obstacle_x_err');
temp2 = matlabFunction(jacobian(obstacle_func, y_err), 'File', 'obstacle_y_err');