
% vehicle params 
vehicle.Cf = 160000;
vehicle.Cr = 180000;
vehicle.m = 1500;
vehicle.a = 1.04;
vehicle.b = 1.42; 
vehicle.Iz = 2250;

% controller params 
kp = .053;
xla = 14.2;

% speed params 
Ux = 10;
ay = 3;

% path params 
kMax = ay / Ux ^ 2;
s = [0 10 20 120 130 150];
k = [0 0 kMax kMax 0 0];

s = [0 100];
k = [kMax kMax];

% sim time 
dt = .001;
t = 0:dt:s(end)/Ux-0.5;
n = length(t);

% preallocate 
xCG = zeros(4, n);
xRA = zeros(4, n);
deltaFB = zeros(1, n);
deltaFFW = zeros(1, n);
sCG = zeros(1, n);
kCG = zeros(1, n);
sRA = zeros(1, n);
kRA = zeros(1, n);
Bss = zeros(1, n);
poseCG = zeros(3, n);
poseRA = zeros(3, n);
poseRAcg = zeros(3, n);

% define starting location of RA relative to CG 
poseRA(:,1) = [poseCG(1,1) - vehicle.b*cos(poseCG(3,1));
               poseCG(2,1) - vehicle.b*sin(poseCG(3,1));
               poseCG(3,1)];
poseRAcg(:,1) = poseRA(:,1); % rear axle of CG version

% starting position along path(so RA still on path) 
s0 = vehicle.b*2;
xRA(2, 1) = k(1)*vehicle.b; % need to offset for dPsi so e is correct

% simulate
for i=1:n-1
    % get linear model
    [Acg, Bcg, Ccg] = getLinearModel(1, vehicle, Ux);
    [Ara, Bra, Cra] = getLinearModel(2, vehicle, Ux);

    % get path position 
    sCG(i) = Ux * t(i) + s0;
    kCG(i) = interp1(s, k, sCG(i));
    sRA(i) = Ux * t(i) + s0 - vehicle.b;
    % this correct ? 
    kRA(i) = interp1(s, k, sRA(i));

    % control law CG 
    % deltaFFW(i) = (vehicle.a + vehicle.b + ... 
    % Ux^2*vehicle.m*vehicle.b/((vehicle.a+vehicle.b)*vehicle.Cf) - ... 
    % Ux^2*vehicle.m*vehicle.a/((vehicle.a+vehicle.b)*vehicle.Cr))*kCG(i);
    % Bss(i) = vehicle.b*kCG(i)-vehicle.m*vehicle.a*Ux^2*kCG(i)/((vehicle.a+vehicle.b)*vehicle.Cr);
    % Bss(i) = 0;
    % deltaFB(i) = -kp * (xCG(1, i) + xla * (xCG(2, i) + Bss(i)));

    % control law RA 
    deltaFFW(i) = (vehicle.a + vehicle.b + ... 
        Ux^2*vehicle.m*vehicle.b/((vehicle.a+vehicle.b)*vehicle.Cf) - ... 
        Ux^2*vehicle.m*vehicle.a/((vehicle.a+vehicle.b)*vehicle.Cr))*kRA(i);
    Bss(i) = vehicle.b*kRA(i)-vehicle.m*vehicle.a*Ux^2*kRA(i)/((vehicle.a+vehicle.b)*vehicle.Cr);
    Bss(i) = -vehicle.m*vehicle.a*Ux^ 2*kRA(i)/((vehicle.a+vehicle.b)*vehicle.Cr);
    % Bss(i) = 0;
    deltaFB(i) = -kp*(xRA(1,i)+xla*(xRA(2,i)+Bss(i)));

    % update states 
    dxCG = Acg * xCG(:,i) + Bcg*(deltaFFW(i)+deltaFB(i)) + Ccg*kCG(i);
    xCG(:,i+1) = xCG(:,i) + dxCG*dt;
    dxRA = Ara*xRA(:,i) + Bra*(deltaFFW(i)+deltaFB(i)) + Cra*kRA(i);
    xRA(:,i+1) = xRA(:,i) + dxRA*dt;

    yawCG = poseCG(3, i);
    Uy = xCG(4,i)*Ux;
    dposeCG = [Ux*cos(yawCG) - Uy*sin(yawCG);
               Ux*sin(yawCG) + Uy*cos(yawCG);
               xCG(3,i)];
    poseCG(:,i+1) = poseCG(:,i) + dposeCG*dt;
    poseRAcg(:,i+1) = [poseCG(1,i+1) - vehicle.b*cos(poseCG(3,i+1));
                       poseCG(2,i+1) - vehicle.b*sin(poseCG(3,i+1));
                       poseCG(3,i+1)];

    yawRA = poseRA(3,i);
    Uy = xRA(4,i)*Ux;
    % yawRA = yawCG;
    dposeRA = [Ux*cos(yawRA) - Uy*sin(yawRA);
               Ux*sin(yawRA) + Uy*cos(yawRA);
               xRA(3,i)];
    poseRA(:,i+1) = poseRA(:,i) + dposeRA*dt;
end

figure;hold on 
plot(t,xCG(1,:),'b') 
plot(t,xCG(2,:),'r') 
plot(t,xRA(1,:),'--') 
plot(t,xRA(2,:),'r--') 
legend('e', 'dpsi') 

%% 
figure;
hold on 
plot(t,xCG(3,:),'g') 
plot(t,xCG(4,:),'k') 
plot(t,xRA(3,:),'g--') 
plot(t,xRA(4,:),'k--') 
legend('r', 'beta')

figure;
hold on 
plot(t,xCG(1,:),'k.-') 
plot(t,xRA(1,:),'r.-') 
eRAcg = xCG(1,:) - vehicle.b*xCG(2, :);
plot(t,eRAcg, 'g.-') 
legend('CG e', 'RA e', 'RA e from CG')

figure;
hold on 
plot(t,xCG(4,:), 'k.-') 
plot(t,xRA(4,:),'r.-') 
betaRAcg = xCG(4,:) - xCG(3,:)*vehicle.b/Ux;
plot(t, betaRAcg, 'g.-') 
legend('CG beta', 'RA beta', 'RA beta from CG')

figure;
hold on 
plot(t, deltaFFW, '.-') 
plot(t, deltaFB, 'r.-') 
legend('FFW', 'FB')

figure;
hold on;
plot(t, Bss) 
legend('Bss')
heading = cumtrapz(sCG(1:end-1), kCG(1:end-1));
xPath = cumtrapz(sCG(1:end-1), cos(heading));
yPath = cumtrapz(sCG(1:end-1), sin(heading));

figure;
hold on;
axis equal 
plot(poseCG(1,:),poseCG(2,:),'.') 
plot(poseRAcg(1,:),poseRAcg(2,:),'r.') 
plot(poseRA(1,:),poseRA(2,:),'g.') 
plot(xPath, yPath, 'k')

figure;
hold on 
plot(t,poseCG(3,:)) 
plot(t,poseRAcg(3,:),'r') 
plot(t,poseRA(3,:), 'g')
