% // 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;

% non-idealialities
headingOffset = 0*pi/180; % mimics SBG heading offset
steerDelay = .2; % sec

% controller params 
dtControl = .02; % make divisible by dt
dtKappa = .23;.25; % lookahead time for ffw
%kp = .05;
%xla = 20;
ki = 0;.025; % val/s
iFreezeK = .008;

% speed params 
UxPath = 65/2.2;
ay = 2;

% path params 
kMax = ay / UxPath ^ 2;
sPath = [0 100 110 160 170 240];
kPath = [0 0 kMax kMax 0 0];

%sPath = [0 200];
%kPath = [0 0];

dt = .001;

pArray = [.05 .075 .1];
xlaArray = [10 30];

pArray = linspace(.05,.1,21);
xlaArray = linspace(5,30,19);

n1 = length(xlaArray);
n2 = length(pArray);
numInflection = zeros(n1,n2);
e1 = zeros(n1,n2);
e2 = zeros(n1,n2);

for ii = 1:n1
    for iii = 1:n2
        % LOOP STARTING HERE
        tic
        % sim time 
        t = 0:dt:sPath(end)/UxPath-0.5;
        n = length(t);

        % preallocate 
        x = zeros(4, n);
        deltaI = zeros(1, n);
        deltaFB = zeros(1, n);
        deltaFFW = zeros(1, n);
        s = zeros(1, n);
        k = zeros(1, n);
        Bss = zeros(1, n);
        pose = zeros(3, n);
        Ux = zeros(1, n);
        steerCmd = zeros(1, n);
        steerActual = zeros(1, n);
        steerArray = zeros(1, floor(steerDelay/dt));

        xFutureE = zeros(1,n);
        xFutureDpsi = zeros(1,n);

        x(1,1) =.5;
        lastEi = zeros(1, n); % only for testing linear integral below
        numPts = 0;
        infIdx = [];

        % simulate
        for i=1:n-1

            Ux(i) = UxPath;
            kp = pArray(iii);
            xla = xlaArray(ii);

            % get linear model
            [Ara, Bra, Cra] = getLinearModel(2, vehicle, Ux(i));

            % get path position 
            s(i) = Ux(i) * t(i);
            k(i) = interp1(sPath, kPath, s(i));
            kLookahead = interp1(sPath, kPath, s(i)+Ux(i)*dtKappa);

            % control law RA 
            if ~mod(t(i),dtControl)
                deltaFFW(i) = (vehicle.a + vehicle.b + ... 
                    Ux(i)^2*vehicle.m*vehicle.b/((vehicle.a+vehicle.b)*vehicle.Cf) - ... 
                    Ux(i)^2*vehicle.m*vehicle.a/((vehicle.a+vehicle.b)*vehicle.Cr))*kLookahead;
                Bss(i) = vehicle.b*kLookahead-vehicle.m*vehicle.a*Ux(i)^2*kLookahead/((vehicle.a+vehicle.b)*vehicle.Cr);
                Bss(i) = -vehicle.m*vehicle.a*Ux(i)^2*kLookahead/((vehicle.a+vehicle.b)*vehicle.Cr);
                % Bss(i) = 0;

                % estimate future e, dpsi (removed)
                xFuture = x(:,i);
                xFutureE(i) = xFuture(1);
                xFutureDpsi(i) = xFuture(2);

                lookahead_error = xFuture(1)+xla*(xFuture(2)+Bss(i)+headingOffset); % has heading offset
                deltaFB(i) = -kp*lookahead_error; 
                if i>1 
                    %deltaI(i) = deltaI(i-1) - ki*dtControl*(xRA(2,i)+Bss(i)+headingOffset);
                    %deltaI(i) = deltaI(i-1) - ki*dtControl*lookahead_error;
                    if abs(kLookahead) < iFreezeK
                        deltaI(i) = deltaI(i-1) - ki*dtControl*x(1,i);
                    else 
                        deltaI(i) = deltaI(i-1);
                    end
                end
            else
                deltaFFW(i) = deltaFFW(i-1);
                Bss(i) = Bss(i-1);
                deltaFB(i) = deltaFB(i-1);
                deltaI(i) = deltaI(i-1);
            end

            % update vehicle states  
            steerCmd(i) = deltaFFW(i)+deltaFB(i)+deltaI(i);
            if steerDelay>0
                steerActual(i) = steerArray(1);
                steerArray = [steerArray(2:end) steerCmd(i)]; 
            else
                steerActual(i) = steerCmd(i);
            end
            dx = Ara*x(:,i) + Bra*steerActual(i) + Cra*k(i);
            x(:,i+1) = x(:,i) + dx*dt;

            % update heading inflection points
            if i>2
                if (x(2,i-1) > x(2,i-2) && x(2,i-1) > x(2,i)) || ...
                   (x(2,i-1) < x(2,i-2) && x(2,i-1) < x(2,i)) 
                %if min(x(2,i-2:i))==x(2,i-1) || max(x(2,i-2:i))==x(2,i-1)
                    numPts = numPts + 1;
                    infIdx = [infIdx i-1];
                end
            end

        end
        toc
        
        figure(101); hold on 
        plot(t,x(1,:)) 
        %plot(t,x(2,:),'r') 
        %legend('e', 'dpsi') 
        xlabel('Time [s]')
        ylabel('Lateral Error [m]')

        figure(102); hold on; 
        plot(t,x(2,:)*180/pi)
        xlabel('Time [s]')
        ylabel('Heading Error [deg]')
        
        idx = find(s>=100,1);
        e1(ii,iii) = sum(abs(x(1,1:idx)))/idx;
        e2(ii,iii) = sum(abs(x(1,idx:end)))/(n-idx);
        numInflection(ii,iii) = numPts;
        
        disp([num2str(ii) ' ' num2str(iii)])       
    end
end

figure; imagesc(pArray,xlaArray,e1); colorbar; title(num2str(Ux(1)*2.2))
figure; imagesc(pArray,xlaArray,e2); colorbar; title(num2str(Ux(1)*2.2))
figure; imagesc(pArray,xlaArray,numInflection); colorbar; title(num2str(Ux(1)*2.2))

save(['data2v' num2str(Ux(1)*2.2)])

%%
%e1Min = min(e1(:));
%e1Max = min(e1,.1);
%figure; imagesc(pArray,xlaArray,e1Max); colorbar

%e2Min = min(e2(:));
%e2Max = min(e2,.1);
%figure; imagesc(pArray,xlaArray,e2Max); colorbar


%figure; imagesc(pArray,xlaArray,e2Max); colorbar

%%

% speed params 
UxPath = 45/2.2;
ay = 2;

% path params 
kMax = ay / UxPath ^ 2;
sPath = [0 100 110 160 170 240];
kPath = [0 0 kMax kMax 0 0];

pArray = [.05 .075 .1];
xlaArray = [10 30];

pArray = linspace(.05,.1,21);
xlaArray = linspace(5,30,19);

n1 = length(xlaArray);
n2 = length(pArray);
numInflection = zeros(n1,n2);
e1 = zeros(n1,n2);
e2 = zeros(n1,n2);

for ii = 1:n1
    for iii = 1:n2
        % LOOP STARTING HERE
        tic
        % sim time 
        t = 0:dt:sPath(end)/UxPath-0.5;
        n = length(t);

        % preallocate 
        x = zeros(4, n);
        deltaI = zeros(1, n);
        deltaFB = zeros(1, n);
        deltaFFW = zeros(1, n);
        s = zeros(1, n);
        k = zeros(1, n);
        Bss = zeros(1, n);
        pose = zeros(3, n);
        Ux = zeros(1, n);
        steerCmd = zeros(1, n);
        steerActual = zeros(1, n);
        steerArray = zeros(1, floor(steerDelay/dt));

        xFutureE = zeros(1,n);
        xFutureDpsi = zeros(1,n);

        x(1,1) =.5;
        lastEi = zeros(1, n); % only for testing linear integral below
        numPts = 0;
        infIdx = [];

        % simulate
        for i=1:n-1

            Ux(i) = UxPath;
            kp = pArray(iii);
            xla = xlaArray(ii);

            % get linear model
            [Ara, Bra, Cra] = getLinearModel(2, vehicle, Ux(i));

            % get path position 
            s(i) = Ux(i) * t(i);
            k(i) = interp1(sPath, kPath, s(i));
            kLookahead = interp1(sPath, kPath, s(i)+Ux(i)*dtKappa);

            % control law RA 
            if ~mod(t(i),dtControl)
                deltaFFW(i) = (vehicle.a + vehicle.b + ... 
                    Ux(i)^2*vehicle.m*vehicle.b/((vehicle.a+vehicle.b)*vehicle.Cf) - ... 
                    Ux(i)^2*vehicle.m*vehicle.a/((vehicle.a+vehicle.b)*vehicle.Cr))*kLookahead;
                Bss(i) = vehicle.b*kLookahead-vehicle.m*vehicle.a*Ux(i)^2*kLookahead/((vehicle.a+vehicle.b)*vehicle.Cr);
                Bss(i) = -vehicle.m*vehicle.a*Ux(i)^2*kLookahead/((vehicle.a+vehicle.b)*vehicle.Cr);
                % Bss(i) = 0;

                % estimate future e, dpsi (removed)
                xFuture = x(:,i);
                xFutureE(i) = xFuture(1);
                xFutureDpsi(i) = xFuture(2);

                lookahead_error = xFuture(1)+xla*(xFuture(2)+Bss(i)+headingOffset); % has heading offset
                deltaFB(i) = -kp*lookahead_error; 
                if i>1 
                    %deltaI(i) = deltaI(i-1) - ki*dtControl*(xRA(2,i)+Bss(i)+headingOffset);
                    %deltaI(i) = deltaI(i-1) - ki*dtControl*lookahead_error;
                    if abs(kLookahead) < iFreezeK
                        deltaI(i) = deltaI(i-1) - ki*dtControl*x(1,i);
                    else 
                        deltaI(i) = deltaI(i-1);
                    end
                end
            else
                deltaFFW(i) = deltaFFW(i-1);
                Bss(i) = Bss(i-1);
                deltaFB(i) = deltaFB(i-1);
                deltaI(i) = deltaI(i-1);
            end

            % update vehicle states  
            steerCmd(i) = deltaFFW(i)+deltaFB(i)+deltaI(i);
            if steerDelay>0
                steerActual(i) = steerArray(1);
                steerArray = [steerArray(2:end) steerCmd(i)]; 
            else
                steerActual(i) = steerCmd(i);
            end
            dx = Ara*x(:,i) + Bra*steerActual(i) + Cra*k(i);
            x(:,i+1) = x(:,i) + dx*dt;

            % update heading inflection points
            if i>2
                if (x(2,i-1) > x(2,i-2) && x(2,i-1) > x(2,i)) || ...
                   (x(2,i-1) < x(2,i-2) && x(2,i-1) < x(2,i)) 
                %if min(x(2,i-2:i))==x(2,i-1) || max(x(2,i-2:i))==x(2,i-1)
                    numPts = numPts + 1;
                    infIdx = [infIdx i-1];
                end
            end

        end
        toc
        
        
        idx = find(s>=100,1);
        e1(ii,iii) = sum(abs(x(1,1:idx)))/idx;
        e2(ii,iii) = sum(abs(x(1,idx:end)))/(n-idx);
        numInflection(ii,iii) = numPts;
        
        disp([num2str(ii) ' ' num2str(iii)])       
    end
end

figure; imagesc(pArray,xlaArray,e1); colorbar; title(num2str(Ux(1)*2.2))
figure; imagesc(pArray,xlaArray,e2); colorbar; title(num2str(Ux(1)*2.2))
figure; imagesc(pArray,xlaArray,numInflection); colorbar; title(num2str(Ux(1)*2.2))

save(['data2v' num2str(Ux(1)*2.2)])

%%

% speed params 
UxPath = 35/2.2;
ay = 2;

% path params 
kMax = ay / UxPath ^ 2;
sPath = [0 100 110 160 170 240];
kPath = [0 0 kMax kMax 0 0];

pArray = [.05 .075 .1];
xlaArray = [10 30];

pArray = linspace(.05,.1,21);
xlaArray = linspace(5,30,19);

n1 = length(xlaArray);
n2 = length(pArray);
numInflection = zeros(n1,n2);
e1 = zeros(n1,n2);
e2 = zeros(n1,n2);

for ii = 1:n1
    for iii = 1:n2
        % LOOP STARTING HERE
        tic
        % sim time 
        t = 0:dt:sPath(end)/UxPath-0.5;
        n = length(t);

        % preallocate 
        x = zeros(4, n);
        deltaI = zeros(1, n);
        deltaFB = zeros(1, n);
        deltaFFW = zeros(1, n);
        s = zeros(1, n);
        k = zeros(1, n);
        Bss = zeros(1, n);
        pose = zeros(3, n);
        Ux = zeros(1, n);
        steerCmd = zeros(1, n);
        steerActual = zeros(1, n);
        steerArray = zeros(1, floor(steerDelay/dt));

        xFutureE = zeros(1,n);
        xFutureDpsi = zeros(1,n);

        x(1,1) =.5;
        lastEi = zeros(1, n); % only for testing linear integral below
        numPts = 0;
        infIdx = [];

        % simulate
        for i=1:n-1

            Ux(i) = UxPath;
            kp = pArray(iii);
            xla = xlaArray(ii);

            % get linear model
            [Ara, Bra, Cra] = getLinearModel(2, vehicle, Ux(i));

            % get path position 
            s(i) = Ux(i) * t(i);
            k(i) = interp1(sPath, kPath, s(i));
            kLookahead = interp1(sPath, kPath, s(i)+Ux(i)*dtKappa);

            % control law RA 
            if ~mod(t(i),dtControl)
                deltaFFW(i) = (vehicle.a + vehicle.b + ... 
                    Ux(i)^2*vehicle.m*vehicle.b/((vehicle.a+vehicle.b)*vehicle.Cf) - ... 
                    Ux(i)^2*vehicle.m*vehicle.a/((vehicle.a+vehicle.b)*vehicle.Cr))*kLookahead;
                Bss(i) = vehicle.b*kLookahead-vehicle.m*vehicle.a*Ux(i)^2*kLookahead/((vehicle.a+vehicle.b)*vehicle.Cr);
                Bss(i) = -vehicle.m*vehicle.a*Ux(i)^2*kLookahead/((vehicle.a+vehicle.b)*vehicle.Cr);
                % Bss(i) = 0;

                % estimate future e, dpsi (removed)
                xFuture = x(:,i);
                xFutureE(i) = xFuture(1);
                xFutureDpsi(i) = xFuture(2);

                lookahead_error = xFuture(1)+xla*(xFuture(2)+Bss(i)+headingOffset); % has heading offset
                deltaFB(i) = -kp*lookahead_error; 
                if i>1 
                    %deltaI(i) = deltaI(i-1) - ki*dtControl*(xRA(2,i)+Bss(i)+headingOffset);
                    %deltaI(i) = deltaI(i-1) - ki*dtControl*lookahead_error;
                    if abs(kLookahead) < iFreezeK
                        deltaI(i) = deltaI(i-1) - ki*dtControl*x(1,i);
                    else 
                        deltaI(i) = deltaI(i-1);
                    end
                end
            else
                deltaFFW(i) = deltaFFW(i-1);
                Bss(i) = Bss(i-1);
                deltaFB(i) = deltaFB(i-1);
                deltaI(i) = deltaI(i-1);
            end

            % update vehicle states  
            steerCmd(i) = deltaFFW(i)+deltaFB(i)+deltaI(i);
            if steerDelay>0
                steerActual(i) = steerArray(1);
                steerArray = [steerArray(2:end) steerCmd(i)]; 
            else
                steerActual(i) = steerCmd(i);
            end
            dx = Ara*x(:,i) + Bra*steerActual(i) + Cra*k(i);
            x(:,i+1) = x(:,i) + dx*dt;

            % update heading inflection points
            if i>2
                if (x(2,i-1) > x(2,i-2) && x(2,i-1) > x(2,i)) || ...
                   (x(2,i-1) < x(2,i-2) && x(2,i-1) < x(2,i)) 
                %if min(x(2,i-2:i))==x(2,i-1) || max(x(2,i-2:i))==x(2,i-1)
                    numPts = numPts + 1;
                    infIdx = [infIdx i-1];
                end
            end

        end
        toc
       
        
        idx = find(s>=100,1);
        e1(ii,iii) = sum(abs(x(1,1:idx)))/idx;
        e2(ii,iii) = sum(abs(x(1,idx:end)))/(n-idx);
        numInflection(ii,iii) = numPts;
        
        disp([num2str(ii) ' ' num2str(iii)])       
    end
end

figure; imagesc(pArray,xlaArray,e1); colorbar; title(num2str(Ux(1)*2.2))
figure; imagesc(pArray,xlaArray,e2); colorbar; title(num2str(Ux(1)*2.2))
figure; imagesc(pArray,xlaArray,numInflection); colorbar; title(num2str(Ux(1)*2.2))

save(['data2v' num2str(Ux(1)*2.2)])

%%

% speed params 
UxPath = 25/2.2;
ay = 2;

% path params 
kMax = ay / UxPath ^ 2;
sPath = [0 100 110 160 170 240];
kPath = [0 0 kMax kMax 0 0];

pArray = [.05 .075 .1];
xlaArray = [10 30];

pArray = linspace(.05,.1,21);
xlaArray = linspace(5,30,19);

n1 = length(xlaArray);
n2 = length(pArray);
numInflection = zeros(n1,n2);
e1 = zeros(n1,n2);
e2 = zeros(n1,n2);

for ii = 1:n1
    for iii = 1:n2
        % LOOP STARTING HERE
        tic
        % sim time 
        t = 0:dt:sPath(end)/UxPath-0.5;
        n = length(t);

        % preallocate 
        x = zeros(4, n);
        deltaI = zeros(1, n);
        deltaFB = zeros(1, n);
        deltaFFW = zeros(1, n);
        s = zeros(1, n);
        k = zeros(1, n);
        Bss = zeros(1, n);
        pose = zeros(3, n);
        Ux = zeros(1, n);
        steerCmd = zeros(1, n);
        steerActual = zeros(1, n);
        steerArray = zeros(1, floor(steerDelay/dt));

        xFutureE = zeros(1,n);
        xFutureDpsi = zeros(1,n);

        x(1,1) =.5;
        lastEi = zeros(1, n); % only for testing linear integral below
        numPts = 0;
        infIdx = [];

        % simulate
        for i=1:n-1

            Ux(i) = UxPath;
            kp = pArray(iii);
            xla = xlaArray(ii);

            % get linear model
            [Ara, Bra, Cra] = getLinearModel(2, vehicle, Ux(i));

            % get path position 
            s(i) = Ux(i) * t(i);
            k(i) = interp1(sPath, kPath, s(i));
            kLookahead = interp1(sPath, kPath, s(i)+Ux(i)*dtKappa);

            % control law RA 
            if ~mod(t(i),dtControl)
                deltaFFW(i) = (vehicle.a + vehicle.b + ... 
                    Ux(i)^2*vehicle.m*vehicle.b/((vehicle.a+vehicle.b)*vehicle.Cf) - ... 
                    Ux(i)^2*vehicle.m*vehicle.a/((vehicle.a+vehicle.b)*vehicle.Cr))*kLookahead;
                Bss(i) = vehicle.b*kLookahead-vehicle.m*vehicle.a*Ux(i)^2*kLookahead/((vehicle.a+vehicle.b)*vehicle.Cr);
                Bss(i) = -vehicle.m*vehicle.a*Ux(i)^2*kLookahead/((vehicle.a+vehicle.b)*vehicle.Cr);
                % Bss(i) = 0;

                % estimate future e, dpsi (removed)
                xFuture = x(:,i);
                xFutureE(i) = xFuture(1);
                xFutureDpsi(i) = xFuture(2);

                lookahead_error = xFuture(1)+xla*(xFuture(2)+Bss(i)+headingOffset); % has heading offset
                deltaFB(i) = -kp*lookahead_error; 
                if i>1 
                    %deltaI(i) = deltaI(i-1) - ki*dtControl*(xRA(2,i)+Bss(i)+headingOffset);
                    %deltaI(i) = deltaI(i-1) - ki*dtControl*lookahead_error;
                    if abs(kLookahead) < iFreezeK
                        deltaI(i) = deltaI(i-1) - ki*dtControl*x(1,i);
                    else 
                        deltaI(i) = deltaI(i-1);
                    end
                end
            else
                deltaFFW(i) = deltaFFW(i-1);
                Bss(i) = Bss(i-1);
                deltaFB(i) = deltaFB(i-1);
                deltaI(i) = deltaI(i-1);
            end

            % update vehicle states  
            steerCmd(i) = deltaFFW(i)+deltaFB(i)+deltaI(i);
            if steerDelay>0
                steerActual(i) = steerArray(1);
                steerArray = [steerArray(2:end) steerCmd(i)]; 
            else
                steerActual(i) = steerCmd(i);
            end
            dx = Ara*x(:,i) + Bra*steerActual(i) + Cra*k(i);
            x(:,i+1) = x(:,i) + dx*dt;

            % update heading inflection points
            if i>2
                if (x(2,i-1) > x(2,i-2) && x(2,i-1) > x(2,i)) || ...
                   (x(2,i-1) < x(2,i-2) && x(2,i-1) < x(2,i)) 
                %if min(x(2,i-2:i))==x(2,i-1) || max(x(2,i-2:i))==x(2,i-1)
                    numPts = numPts + 1;
                    infIdx = [infIdx i-1];
                end
            end

        end
        toc
        
        
        idx = find(s>=100,1);
        e1(ii,iii) = sum(abs(x(1,1:idx)))/idx;
        e2(ii,iii) = sum(abs(x(1,idx:end)))/(n-idx);
        numInflection(ii,iii) = numPts;
        
        disp([num2str(ii) ' ' num2str(iii)])       
    end
end

figure; imagesc(pArray,xlaArray,e1); colorbar; title(num2str(Ux(1)*2.2))
figure; imagesc(pArray,xlaArray,e2); colorbar; title(num2str(Ux(1)*2.2))
figure; imagesc(pArray,xlaArray,numInflection); colorbar; title(num2str(Ux(1)*2.2))

save(['data2v' num2str(Ux(1)*2.2)])

%%

% speed params 
UxPath = 15/2.2;
ay = 2;

% path params 
kMax = ay / UxPath ^ 2;
sPath = [0 100 110 160 170 240];
kPath = [0 0 kMax kMax 0 0];

pArray = [.05 .075 .1];
xlaArray = [10 30];

pArray = linspace(.05,.1,21);
xlaArray = linspace(5,30,19);

n1 = length(xlaArray);
n2 = length(pArray);
numInflection = zeros(n1,n2);
e1 = zeros(n1,n2);
e2 = zeros(n1,n2);

for ii = 1:n1
    for iii = 1:n2
        % LOOP STARTING HERE
        tic
        % sim time 
        t = 0:dt:sPath(end)/UxPath-0.5;
        n = length(t);

        % preallocate 
        x = zeros(4, n);
        deltaI = zeros(1, n);
        deltaFB = zeros(1, n);
        deltaFFW = zeros(1, n);
        s = zeros(1, n);
        k = zeros(1, n);
        Bss = zeros(1, n);
        pose = zeros(3, n);
        Ux = zeros(1, n);
        steerCmd = zeros(1, n);
        steerActual = zeros(1, n);
        steerArray = zeros(1, floor(steerDelay/dt));

        xFutureE = zeros(1,n);
        xFutureDpsi = zeros(1,n);

        x(1,1) =.5;
        lastEi = zeros(1, n); % only for testing linear integral below
        numPts = 0;
        infIdx = [];

        % simulate
        for i=1:n-1

            Ux(i) = UxPath;
            kp = pArray(iii);
            xla = xlaArray(ii);

            % get linear model
            [Ara, Bra, Cra] = getLinearModel(2, vehicle, Ux(i));

            % get path position 
            s(i) = Ux(i) * t(i);
            k(i) = interp1(sPath, kPath, s(i));
            kLookahead = interp1(sPath, kPath, s(i)+Ux(i)*dtKappa);

            % control law RA 
            if ~mod(t(i),dtControl)
                deltaFFW(i) = (vehicle.a + vehicle.b + ... 
                    Ux(i)^2*vehicle.m*vehicle.b/((vehicle.a+vehicle.b)*vehicle.Cf) - ... 
                    Ux(i)^2*vehicle.m*vehicle.a/((vehicle.a+vehicle.b)*vehicle.Cr))*kLookahead;
                Bss(i) = vehicle.b*kLookahead-vehicle.m*vehicle.a*Ux(i)^2*kLookahead/((vehicle.a+vehicle.b)*vehicle.Cr);
                Bss(i) = -vehicle.m*vehicle.a*Ux(i)^2*kLookahead/((vehicle.a+vehicle.b)*vehicle.Cr);
                % Bss(i) = 0;

                % estimate future e, dpsi (removed)
                xFuture = x(:,i);
                xFutureE(i) = xFuture(1);
                xFutureDpsi(i) = xFuture(2);

                lookahead_error = xFuture(1)+xla*(xFuture(2)+Bss(i)+headingOffset); % has heading offset
                deltaFB(i) = -kp*lookahead_error; 
                if i>1 
                    %deltaI(i) = deltaI(i-1) - ki*dtControl*(xRA(2,i)+Bss(i)+headingOffset);
                    %deltaI(i) = deltaI(i-1) - ki*dtControl*lookahead_error;
                    if abs(kLookahead) < iFreezeK
                        deltaI(i) = deltaI(i-1) - ki*dtControl*x(1,i);
                    else 
                        deltaI(i) = deltaI(i-1);
                    end
                end
            else
                deltaFFW(i) = deltaFFW(i-1);
                Bss(i) = Bss(i-1);
                deltaFB(i) = deltaFB(i-1);
                deltaI(i) = deltaI(i-1);
            end

            % update vehicle states  
            steerCmd(i) = deltaFFW(i)+deltaFB(i)+deltaI(i);
            if steerDelay>0
                steerActual(i) = steerArray(1);
                steerArray = [steerArray(2:end) steerCmd(i)]; 
            else
                steerActual(i) = steerCmd(i);
            end
            dx = Ara*x(:,i) + Bra*steerActual(i) + Cra*k(i);
            x(:,i+1) = x(:,i) + dx*dt;

            % update heading inflection points
            if i>2
                if (x(2,i-1) > x(2,i-2) && x(2,i-1) > x(2,i)) || ...
                   (x(2,i-1) < x(2,i-2) && x(2,i-1) < x(2,i)) 
                %if min(x(2,i-2:i))==x(2,i-1) || max(x(2,i-2:i))==x(2,i-1)
                    numPts = numPts + 1;
                    infIdx = [infIdx i-1];
                end
            end

        end
        toc
        
        idx = find(s>=100,1);
        e1(ii,iii) = sum(abs(x(1,1:idx)))/idx;
        e2(ii,iii) = sum(abs(x(1,idx:end)))/(n-idx);
        numInflection(ii,iii) = numPts;
        
        disp([num2str(ii) ' ' num2str(iii)])       
    end
end

figure; imagesc(pArray,xlaArray,e1); colorbar; title(num2str(Ux(1)*2.2))
figure; imagesc(pArray,xlaArray,e2); colorbar; title(num2str(Ux(1)*2.2))
figure; imagesc(pArray,xlaArray,numInflection); colorbar; title(num2str(Ux(1)*2.2))

save(['data2v' num2str(Ux(1)*2.2)])


%%

% speed params 
UxPath = 5/2.2;
ay = 2;

% path params 
kMax = ay / UxPath ^ 2;
sPath = [0 100 110 160 170 240];
kPath = [0 0 kMax kMax 0 0];

pArray = [.05 .075 .1];
xlaArray = [10 30];

pArray = linspace(.05,.1,21);
xlaArray = linspace(5,30,19);

n1 = length(xlaArray);
n2 = length(pArray);
numInflection = zeros(n1,n2);
e1 = zeros(n1,n2);
e2 = zeros(n1,n2);

for ii = 1:n1
    for iii = 1:n2
        % LOOP STARTING HERE
        tic
        % sim time 
        t = 0:dt:sPath(end)/UxPath-0.5;
        n = length(t);

        % preallocate 
        x = zeros(4, n);
        deltaI = zeros(1, n);
        deltaFB = zeros(1, n);
        deltaFFW = zeros(1, n);
        s = zeros(1, n);
        k = zeros(1, n);
        Bss = zeros(1, n);
        pose = zeros(3, n);
        Ux = zeros(1, n);
        steerCmd = zeros(1, n);
        steerActual = zeros(1, n);
        steerArray = zeros(1, floor(steerDelay/dt));

        xFutureE = zeros(1,n);
        xFutureDpsi = zeros(1,n);

        x(1,1) =.5;
        lastEi = zeros(1, n); % only for testing linear integral below
        numPts = 0;
        infIdx = [];

        % simulate
        for i=1:n-1

            Ux(i) = UxPath;
            kp = pArray(iii);
            xla = xlaArray(ii);

            % get linear model
            [Ara, Bra, Cra] = getLinearModel(2, vehicle, Ux(i));

            % get path position 
            s(i) = Ux(i) * t(i);
            k(i) = interp1(sPath, kPath, s(i));
            kLookahead = interp1(sPath, kPath, s(i)+Ux(i)*dtKappa);

            % control law RA 
            if ~mod(t(i),dtControl)
                deltaFFW(i) = (vehicle.a + vehicle.b + ... 
                    Ux(i)^2*vehicle.m*vehicle.b/((vehicle.a+vehicle.b)*vehicle.Cf) - ... 
                    Ux(i)^2*vehicle.m*vehicle.a/((vehicle.a+vehicle.b)*vehicle.Cr))*kLookahead;
                Bss(i) = vehicle.b*kLookahead-vehicle.m*vehicle.a*Ux(i)^2*kLookahead/((vehicle.a+vehicle.b)*vehicle.Cr);
                Bss(i) = -vehicle.m*vehicle.a*Ux(i)^2*kLookahead/((vehicle.a+vehicle.b)*vehicle.Cr);
                % Bss(i) = 0;

                % estimate future e, dpsi (removed)
                xFuture = x(:,i);
                xFutureE(i) = xFuture(1);
                xFutureDpsi(i) = xFuture(2);

                lookahead_error = xFuture(1)+xla*(xFuture(2)+Bss(i)+headingOffset); % has heading offset
                deltaFB(i) = -kp*lookahead_error; 
                if i>1 
                    %deltaI(i) = deltaI(i-1) - ki*dtControl*(xRA(2,i)+Bss(i)+headingOffset);
                    %deltaI(i) = deltaI(i-1) - ki*dtControl*lookahead_error;
                    if abs(kLookahead) < iFreezeK
                        deltaI(i) = deltaI(i-1) - ki*dtControl*x(1,i);
                    else 
                        deltaI(i) = deltaI(i-1);
                    end
                end
            else
                deltaFFW(i) = deltaFFW(i-1);
                Bss(i) = Bss(i-1);
                deltaFB(i) = deltaFB(i-1);
                deltaI(i) = deltaI(i-1);
            end

            % update vehicle states  
            steerCmd(i) = deltaFFW(i)+deltaFB(i)+deltaI(i);
            if steerDelay>0
                steerActual(i) = steerArray(1);
                steerArray = [steerArray(2:end) steerCmd(i)]; 
            else
                steerActual(i) = steerCmd(i);
            end
            dx = Ara*x(:,i) + Bra*steerActual(i) + Cra*k(i);
            x(:,i+1) = x(:,i) + dx*dt;

            % update heading inflection points
            if i>2
                if (x(2,i-1) > x(2,i-2) && x(2,i-1) > x(2,i)) || ...
                   (x(2,i-1) < x(2,i-2) && x(2,i-1) < x(2,i)) 
                %if min(x(2,i-2:i))==x(2,i-1) || max(x(2,i-2:i))==x(2,i-1)
                    numPts = numPts + 1;
                    infIdx = [infIdx i-1];
                end
            end

        end
        toc
        
        idx = find(s>=100,1);
        e1(ii,iii) = sum(abs(x(1,1:idx)))/idx;
        e2(ii,iii) = sum(abs(x(1,idx:end)))/(n-idx);
        numInflection(ii,iii) = numPts;
        
        disp([num2str(ii) ' ' num2str(iii)])       
    end
end

figure; imagesc(pArray,xlaArray,e1); colorbar; title(num2str(Ux(1)*2.2))
figure; imagesc(pArray,xlaArray,e2); colorbar; title(num2str(Ux(1)*2.2))
figure; imagesc(pArray,xlaArray,numInflection); colorbar; title(num2str(Ux(1)*2.2))

save(['data2v' num2str(Ux(1)*2.2)])

%%

