% // clang-format off

% vehicle params 
vehicle.Cf = 150000;
vehicle.Cr = 150000;
vehicle.m = 2000;
vehicle.a = 1.2;
vehicle.b = 1.2; 
vehicle.Iz = vehicle.m*vehicle.a*vehicle.b;
g = 9.81

%% modify vehicle params

% assume this already has 1/g, so rad/(m/s^2)
Karray = [0.00149, -0.000568]
colors = ['b', 'r', 'g', 'k']

for i = 1:length(Karray)
    K = Karray(i); 

    L = vehicle.a + vehicle.b;
    wf = vehicle.b/L * vehicle.m*g;
    wr = vehicle.a/L * vehicle.m*g;
    vehicle.Cr = wr / (wf/vehicle.Cf - K*g);
    disp('Diff, should be zero:')
    (wf/vehicle.Cf - wr/vehicle.Cr)/g - K
    disp('critical speed')
    sqrt((vehicle.a+vehicle.b)/abs(K))

    %vary speeds
    Ux = linspace(1,70,50);
    dist = zeros(length(Ux),1);
    for j = 1:length(Ux)

        % get linear model
        [A, B, C] = getLinearModel(1, vehicle, Ux(j));

        % add steering feedback
        p = 0.0;
        xla = 20;
        deltaGains = -[p p*xla 0 0];
        Acl = A + B*deltaGains;

        pVehicle = eig(Acl(3:4,3:4)); % or set gains to 0
        pDelta = eig(Acl);

        c = [j/length(Ux) 0 1-j/length(Ux)];

        figure(i); hold on;
        plot(real(pVehicle),imag(pVehicle),'x','color',c)
        dist(j) = max(real(pVehicle));
        % figure(52); hold on;
        % plot(real(pDelta),imag(pDelta),'x','color',c)
    end

    figure(52); hold on
    plot(Ux,dist,[colors(i) 'o'])
    xlabel('Ux [m/s]'); ylabel('Sigma')
end