%% load all parameters
run("parameters.m")
N = 1000;
%% create centerline trajectory
%% implement continuous nonlinear dynamics
% x = [n mu vx vy omega]
% u = [delta Flong]

% load racetrack
load('racetrack.mat', 't_r'); % load right boundary from *.mat file
load('racetrack.mat', 't_l'); % load left boundary from *.mat file
t_r_x = t_r(:, 1); % x coordinate of right racetrack boundary
t_r_y = t_r(:, 2); % y coordinate of right racetrack boundary
t_l_x = t_l(:, 1); % x coordinate of left racetrack boundary
t_l_y = t_l(:, 2); % y coordinate of left racetrack boundary

x_m = (t_l(:, 1) + t_r(:, 1)) / 2;
y_m = (t_l(:, 2) + t_r(:, 2)) / 2;

[a, ~] = find(diff(x_m) == 0 & diff(y_m) == 0);
x_m(a) = [];
y_m(a) = [];
refPose = [x_m, y_m];

distances = sqrt(diff(x_m).^2 + diff(y_m).^2);

cumulative_distances = [0; cumsum(distances)];

Delta_s = linspace(0, cumulative_distances(end), N+1);

s = Delta_s(end);

x_interp = interp1(cumulative_distances, x_m, Delta_s, 'linear')';
y_interp = interp1(cumulative_distances, y_m, Delta_s, 'linear')';

phi_ref = zeros(N+1, 1);
phi_ref_dot = zeros(N+1, 1);

phi_ref(1) = 90;
for j = 2:N+1
    phi_ref(j) = wrapTo360(atan2d(y_interp(j) - y_interp(j - 1), x_interp(j) - x_interp(j - 1)));
    if abs(phi_ref(j) - phi_ref(j-1)) >= 180
       phi_ref(j) = phi_ref(j) - 360;
    end
end

phi_ref = pi/180*phi_ref;

%phi_ref = phi_ref * pi / 180; % calculate phi in rad
for j = 2:N+1
    phi_ref_dot(j) = (phi_ref(j) - phi_ref(j - 1)) / Delta_s(j);
end

dx_dt = diff(x_interp) ./ diff(Delta_s)';
dy_dt = diff(y_interp) ./ diff(Delta_s)';

d2x_dt2 = diff(dx_dt) ./ diff(Delta_s(2:end))';
d2y_dt2 = diff(dy_dt) ./ diff(Delta_s(2:end))';

kappa = zeros(N+1,1);

kappa(2:end-1) = (dx_dt(2:end) .* d2y_dt2 - dy_dt(2:end) .* d2x_dt2) ./ (dx_dt(2:end).^2 + dy_dt(2:end).^2).^(3/2);
kappa(1) = kappa(2);
kappa(end) = kappa(end-1);
%%
nx = 5; % no of states
m = 2; % no of control inputs

%%
% System dynamics constraints
ds = Delta_s(2)-Delta_s(1);
vx0 = 2;

% try initial guesses

% Initial guess for input
u01 = phi_ref;
% u01 = zeros(N+1,1);
v_ref = 3*ones(N+1,1);

vx_ref = v_ref.*cos(phi_ref);
vy_ref = v_ref.*sin(phi_ref);

%%
% Initial guess for states by simulation
X0 = [0; 0; vx_ref(1); vy_ref(1); 0]; % [n0,mu0, vx0, vy0, omega0]

x0 = zeros(nx,N+1);
x0(:,1) = X0;

sdot0 = zeros(N+1,1);

k = 0;

while k < N
     k = k+1;

     %Dynamics(n, mu, vx, vy, omega, delta, Flong, kappa, sdot);
     n = x0(1,k);
     mu = x0(2,k);
     vx = vx_ref(k);
     vy = vy_ref(k);
     omega = x0(5,k);

     delta = u01(k);
    
     sdot = (vx*cos(mu)-vy*sin(mu))/(1-n*kappa(k));
     sdot0(k) = sdot;
     x0(:,k+1) = x0(:,k)+ds/sdot*Dynamics(n, mu, vx, vy, omega, delta, kappa(k), sdot);

     x0(3,k+1) = vx_ref(k+1);
     x0(4,k+1) = vx_ref(k+1);
end

sdot0(end) = sdot0(end-1);
Delta_s(end) 

%%
clf
figure()
plot(sdot0)

%%
figure()
hold on
plot(x0(1,:))
figure()
plot(x0(2,:))
figure()
plot(x0(5,:))

%% Dynamics function
function dn = Dynamics(n, mu, vx, vy, omega, delta, kappa, sdot)
    run("parameters.m")
    
    % Compute tire slip angles 
    alphaf = -delta + atan((l_f*omega + vy) / vx);
    alphar = atan((vy-l_r*omega) / vx);
    
    %Compute lateral tire forces
    Fyf = D_f * sin(C_f*atan(B_f*alphaf - E_f*(B_f*alphaf - atan(B_f*alphaf))));
    Fyr = D_r * sin(C_r*atan(B_r*alphar - E_r*(B_r*alphar - atan(B_r*alphar))));
    
    % Compute state derivatives
    dn = [(vx*sin(mu) + vy*cos(mu)),                        % dn
          (omega - sdot*kappa),                              % dmu
           0,        % dvx
           0,       % dvy
          (1/I_z) * (Fyf*l_f*cos(delta) - Fyr*l_r)];         % domega

end