% // clang-format off

% vehicle params 
vehicle.Cf = 123000;
vehicle.Cr = 230000;
vehicle.m = 2342;
vehicle.a = 1.37;
vehicle.b = 1.41; 
vehicle.Iz = vehicle.m*vehicle.a*vehicle.b;


%% vary speeds

Ux = [1 5 10 15 20 25 30];
Ux = linspace(1,30,50);
%Ux = [5 15 25 35]/2.2;

for j = 1:length(Ux)

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

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

    % add steering feedback with integral
    iGain = 0;
    deltaGains = -[p p*xla 0 0 iGain];
    Ai = [A zeros(4,1); 1 0 0 0 0];
    Bi = [B; 0];
    Acli = Ai + Bi*deltaGains;

%     sys = ss(A,B,ones(1,4),0)
%     sys = ss(A,B,[0 0 1 1],0)
%     sys = ss(A(3:4,3:4),B(3:4),ones(1,2),0)
%     pzmap(sys)

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

    c = [j/length(Ux) 0 1-j/length(Ux)];
    
    figure(51); hold on;
    plot(real(pVehicle),imag(pVehicle),'x','color',c)
    figure(52); hold on;
    plot(real(pDelta),imag(pDelta),'x','color',c)
    figure(53); hold on;
    plot(real(pInt),imag(pInt),'x','color',c)
end


%% instead, vary i gain over a fixed speed

iGainArray = linspace(0,.1,50);
%iGainArray = [0,.01,0.025,.05,.1];

Ux = 45/2.2;
Ux = 22.75

for j = 1:length(iGainArray)

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

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

    % add steering feedback with integral
    iGain = iGainArray(j);
    deltaGains = -[p p*xla 0 0 iGain];
    Ai = [A zeros(4,1); 1 0 0 0 0];
    Bi = [B; 0];
    Acli = Ai + Bi*deltaGains;

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

    c = [j/length(iGainArray) 0 1-j/length(iGainArray)];
    
%     figure(51); hold on;
%     plot(real(pVehicle),imag(pVehicle),'x','color',c)
%     figure(52); hold on;
%     plot(real(pDelta),imag(pDelta),'x','color',c)
    figure(66); hold on;
    plot(real(pInt),imag(pInt),'x','color',c)
end

%% calculate i gain to achieve desired poles, across speeds

iGainArray = linspace(0,.1,1000);
UxArray = linspace(1,30,30);
iGainDesired = zeros(length(UxArray),1);


p = 0.05;
xla = 20;

for j = 1:length(UxArray)
    
    % get linear model
    [A, B, C] = getLinearModel(2, vehicle, UxArray(j));

    for jj = 1:length(iGainArray)-1
        % add steering feedback with integral
        iGain = iGainArray(jj);
        deltaGains = -[p p*xla 0 0 iGain];
        Ai = [A zeros(4,1); 1 0 0 0 0];
        Bi = [B; 0];
        Acli = Ai + Bi*deltaGains;
        
        % add steering feedback with integral
        iGain2 = iGainArray(jj+1);
        deltaGains = -[p p*xla 0 0 iGain2];
        Ai = [A zeros(4,1); 1 0 0 0 0];
        Bi = [B; 0];
        Acli2 = Ai + Bi*deltaGains;

        % roots
        pInt = esort(eig(Acli));
        pInt2 = esort(eig(Acli2));
        theta = angle(pInt(1:3));
        theta2 = angle(pInt(1:3));
        pImag = imag(pInt(1:3));
        pImag2 = imag(pInt2(1:3));
        criteria = sum(theta>0 & theta<180*pi/180); % should have magnitude between
        criteria2 = max(pImag2) - max(pImag) >= 0; % should be increasing
        criteria3 = max(pImag) >= .6;
        %criteria4 = 
        %if criteria && criteria2
        if criteria3 && criteria2
            iGainDesired(j) = iGain;
            iGain;
            break
        end
    end
end

figure
plot(UxArray,iGainDesired,'o-')
xlabel('Ux [m/s]'); ylabel('Integral Gain')

%% vary p gain over speeds

f = figure; hold on;
pGainArray = [linspace(0,.1,50) .05 .075];
%pGainArray = [0,.01,0.025,.05,.1];

xlaArray = linspace(5,30,25);

Ux = 15/2.2

for j = 1:length(pGainArray)

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

    % add steering feedback
    %p = pGainArray(j);
    p = .1;
    %xla = 20;
    xla = xlaArray(j);
    deltaGains = -[p p*xla 0 0];
    Acl = A + B*deltaGains;

    % add steering feedback with integral
    %iGain = pGainArray(j);
    %deltaGains = -[p p*xla 0 0 iGain];
    %Ai = [A zeros(4,1); 1 0 0 0 0];
    %Bi = [B; 0];
    %Acli = Ai + Bi*deltaGains;

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

    c = [j/length(pGainArray) 0 1-j/length(pGainArray)];
    
    %figure(1); hold on;
    %plot(real(pVehicle),imag(pVehicle),'x','color',c)
    %figure(figNum); hold on;
    %plot(real(pDelta),imag(pDelta),'x','color',c)
    %figure(3); hold on;
    %plot(real(pInt),imag(pInt),'x','color',c)
    
    plot(real(pDelta),imag(pDelta),'x','color',c)
end

%% vary multiple things


%f = figure; hold on;

pGainArray = linspace(0,.1,50);
xlaArray = linspace(5,30,25);
UxArray = linspace(1,30,30);

pGainArray = 0.1;
%xlaArray = 20;
UxArray = 65/2.2; 


for j = 1:length(UxArray)
    for jj = 1:length(xlaArray)
        for jjj = 1:length(pGainArray)
            
            % get linear model
            [A, B, C] = getLinearModel(2, vehicle, UxArray(j));
            
            % add steering feedback
            p = pGainArray(jjj);
            xla = xlaArray(jj);
            deltaGains = -[p p*xla 0 0];
            Acl = A + B*deltaGains;
            
            % add steering feedback with integral
            %iGain = pGainArray(j);
            %deltaGains = -[p p*xla 0 0 iGain];
            %Ai = [A zeros(4,1); 1 0 0 0 0];
            %Bi = [B; 0];
            %Acli = Ai + Bi*deltaGains;
            
            % calc poles
            %pVehicle = eig(Acl(3:4,3:4)); % or set gains to 0
            pDelta = eig(Acl);
            %pInt = eig(Acli);

            n = length(pGainArray)*length(xlaArray)*length(UxArray);
            t = j*jj*jjj;
            c = [t/n 0 1-t/n];

            %figure(1); hold on;
            %plot(real(pVehicle),imag(pVehicle),'x','color',c)
            %figure(figNum); hold on;
            %plot(real(pDelta),imag(pDelta),'x','color',c)
            %figure(3); hold on;
            %plot(real(pInt),imag(pInt),'x','color',c)

            plot(real(pDelta),imag(pDelta),'x','color',c)
        end
    end
end

