% // 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 = 1*pi/180; % mimics SBG heading offset
steerDelay = .3; % sec

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

% speed params
UxPath = 10;
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];

useData = 0;

% % comment this out thru 'useData' if not using
% d = csvread('20161018T191010_kappa_ux.csv');
% idx = 1000:5300;
% kPath = d(idx,1);
% UxPath = d(idx,2);
% tPath = 0:.02:(length(kPath)-1)*.02;
% recordDataDtKappa = 0.25;
% useData = 1;

% sim time
dt = .001;
if useData
t = 0:dt:tPath(end);
else
t = 0:dt:sPath(end)/UxPath-0.5;
end
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);

lastEi = zeros(1, n); % only for testing linear integral below

% simulate
for i=1:n-1

if useData
Ux(i) = interp1(tPath, UxPath, t(i));
Ux(i) = max(Ux(i),.1);
else
Ux(i) = UxPath;
end

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

% get path position
if useData
if i>1
s(i) = s(i-1) + Ux(i)*dt;
end
if t(i)-recordDataDtKappa < 0
k(i) = kPath(1);
else
k(i) = interp1(tPath, kPath, t(i)-recordDataDtKappa); % minus .25 for lookahead on tracker
end
if t(i)-recordDataDtKappa+dtKappa < 0
kLookahead = kPath(1);
else
kLookahead = interp1(tPath, kPath, t(i)-recordDataDtKappa + dtKappa);
end
else
s(i) = Ux(i) * t(i);
k(i) = interp1(sPath, kPath, s(i));
kLookahead = interp1(sPath, kPath, s(i)+Ux(i)*dtKappa);
end

% 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
if i>1
xFuture = x(:,i);
for ii=1:ceil(dtError/dt)
dxFuture = Ara*xFuture + Bra*steerActual(i-1) + Cra*k(i);
xFuture = xFuture + dxFuture*dt;
end
else
xFuture = x(:,1);
end
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;

% % TEST WIHTOUT INTEGRATOR
% % add steering feedback
% p = 0.05;
% xla = 20;
% deltaGains = -[p p*xla 0 0];
% Acl = Ara + Bra*deltaGains;
% dx = Acl*x(:,i);
% x(:,i+1) = x(:,i) + dx*dt;

% % TEST WITH INTEGRATOR
% iGain = .1;
% deltaGains = -[p p*xla 0 0 iGain];
% Ai = [Ara zeros(4,1); 1 0 0 0 0];
% Bi = [Bra; 0];
% Acli = Ai + Bi*deltaGains;
% dxi = Acli*[x(:,i); lastEi(i)];
% deltaI2 = deltaGains(5)*lastEi(i);
% x(:,i+1) = x(:,i) + dxi(1:4)*dt;
% lastEi(i+1) = lastEi(i) + dxi(5)*dt;

% update position states
yaw = pose(3,i);
Uy = x(4,i)*Ux(i);
dpose = [Ux(i)*cos(yaw) - Uy*sin(yaw);
Ux(i)*sin(yaw) + Uy*cos(yaw);
x(3,i)];
pose(:,i+1) = pose(:,i) + dpose*dt;
end

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]')

%%
figure;
hold on
plot(t,x(3,:))
plot(t,x(4,:),'r')
legend('r', 'beta')

figure;
hold on
plot(t, deltaFFW*180/pi, 'b')
plot(t, deltaFB*180/pi, 'r')
plot(t, deltaI*180/pi, 'g')
legend('FFW', 'FB', 'I')

figure; hold on
plot(t, steerCmd)
plot(t, steerActual,'r')
legend('SteerCmd','SteerActual')

figure;
hold on;
plot(t, Bss*180/pi)
legend('Bss')

heading = cumtrapz(s(1:end-1), k(1:end-1));
xPath = cumtrapz(s(1:end-1), cos(heading));
yPath = cumtrapz(s(1:end-1), sin(heading));
figure;
hold on;
axis equal
plot(pose(1,:),pose(2,:),'g.')
plot(xPath, yPath, 'k')

figure;
hold on
plot(t,pose(3,:), 'g')
legend('yaw')

figure; hold on
plot(t,Ux)
legend('Ux')

figure
hold on
plot(t,xFutureE,'.')
plot(t,x(1,:),'r')

figure; hold on
plot(t,xFutureDpsi,'.')
plot(t,x(2,:),'r')