clear all; close all; clc;

% US guidelines for building roads
%km
speed_km   = [0.1, 10, 20, 30, 40,  50,  60,  70,  80,  90, 100, 110, 120, 130, 140];
%m
min_radius = [40, 70, 100, 150, 200, 260, 320, 400, 600, 900, 1200];
speed = speed_km./3.6; %m/s

slip_span = 1:0.5:6;
for sl = 1:length(slip_span)
    fprintf('---processing slip %d---\n', slip_span(sl));
    slip_des = deg2rad(slip_span(sl));
    for s = 1:length(speed)
        fprintf('speed: %d\n', speed_km(s));
        u_des = speed(s);

        % simulation parameters
        dt = 0.002;
        ts = 0;
        tf = 60;
        tspan = ts:dt:tf;
        T = 0;
        Xi = [0,0,u_des,0,0,0];
        X = zeros(6, length(tspan));
        X(:,1) = Xi;
        U = zeros(2, length(tspan));
        delta_i = 0;

        for i = 2:length(tspan)
            %retreive states
            x = X(1,i-1);
            y = X(2,i-1);
            u = X(3,i-1);
            v = X(4,i-1);
            psi = X(5,i-1);
            w = X(6,i-1);
            d = U(2,i-1);

            %control
            slip(i) = - atan((v + (2.67/2) * w) /  (eps + sqrt(u^2))) + d * tanh(u * 10);

            acc = (u_des-u)*0.1;
            delta_i = delta_i + dt*(slip_des-slip(i));
            delta = (slip_des-slip(i))*0.5 + delta_i*1.2;
            U(:,i) = [acc,delta]';

            %simulate
            [T(i),X(:,i)] = car_model_bicycle_GS(X(:,i-1), dt, U(:,i));
        end

        %retrieve simulation states
        x = X(1,:);
        y = X(2,:);
        u = X(3,:);
        v = X(4,:);
        psi = X(5,:);
        w = X(6,:);
        acc = U(1,:);
        delta = U(2,:);

%         figure(1*s)
%         plot(x, y, 'LineWidth', 2);
%         title('position')
%
%         figure(s)
%         plot(tspan, slip, ...
%              tspan, delta, ...
%              tspan, slip_des*ones(1,length(w)), ...
%              'LineWidth', 2)
%         legend('slip', 'delta', 'desired slip')

        if (abs(slip(end)-slip_des) > 0.001)
            fprintf('WARNING: slip not met!\n')
        end
        if (rad2deg(delta(end))*16 > 480)
            req_steer_angle(s, sl) = 480;
        else
            req_steer_angle(s, sl) = rad2deg(delta(end))*16;
        end
        lateral_acc(s,sl) = w(end)*u(end);

    end
end

figure(1)
surf(speed_km', slip_span', req_steer_angle')
xlabel('speed [km]')
ylabel('slip angle [deg]')
zlabel('steering angle limit [deg]')
title('slip based steering angle limits')

figure(2)
surf(speed_km', slip_span', lateral_acc')
xlabel('speed [km]')
ylabel('slip angle [deg]')
zlabel('lateral acceleration [m/s^2]')
title('lateral acceleration at slip limits')

% figure(100)
% plot(speed_km, req_steer_angle, 'LineWidth', 2)
% xlabel('speed [m/s]')
% ylabel('steering wheel angle [deg]')
% title('steering angle limits using US roadway guidelines')

save('generate_files/slip_limit.mat')
