%%
data_all = load('LateralPacejka/lfs_data_slip_angle.mat');
data_all = data_all.lfs_data;
data = load('LateralPacejka/lfs_data_slip_angle_selected.mat');
data = data.lfs_data;
%%
Time = data_all.Time.Time;
Time_selected = data(:,1)/100;
W3_XForce = data_all.W3_XForce.Data(data(:,1));
W4_XForce = data_all.W4_XForce.Data(data(:,1)); 
W3_VerticalLoad = data_all.W3_VerticalLoad.Data(data(:,1));
W4_VerticalLoad = data_all.W4_VerticalLoad.Data(data(:,1));
W3_SlipAngle = atan(data_all.W3_TanSlipAngle.Data(data(:,1)));
W4_SlipAngle = atan(data_all.W4_TanSlipAngle.Data(data(:,1)));
%%
plot(Time, atan(data_all.W3_TanSlipAngle.Data))
hold on
plot(Time, atan(data_all.W4_TanSlipAngle.Data))
scatter(Time_selected, W3_SlipAngle)
scatter(Time_selected, W4_SlipAngle)

figure;

plot(Time, data_all.W3_XForce.Data)
hold on
plot(Time, data_all.W4_XForce.Data)
scatter(Time_selected, W3_XForce)
scatter(Time_selected, W4_XForce)

figure 
plot(Time, data_all.W3_VerticalLoad.Data)
hold on
plot(Time, data_all.W4_VerticalLoad.Data)
scatter(Time_selected, W3_VerticalLoad)
scatter(Time_selected, W4_VerticalLoad)
%%
SlipAngle = (W3_SlipAngle + W4_SlipAngle)/2;
XForce = W3_XForce + W4_XForce;
VerticalLoad = W3_VerticalLoad + W4_VerticalLoad;
%%
Func = @(Pacejka, SlipAngle)Pacejka(1).*sin((Pacejka(2).*atan(Pacejka(3).*SlipAngle - Pacejka(4).*(Pacejka(3).*SlipAngle - atan(Pacejka(3).*SlipAngle)))));
% x0 = [3000 10 1 1];
% opts = statset('nlinfit');
% opts.RobustWgtFun = 'huber';
% Pacejka = nlinfit(SlipAngle,XForce,Func,x0,opts); % (D*F_z) C B E
Pacejka = [car.Tire.Cdy .*car.Fzf, car.Tire.Ccy, car.Tire.Cby, car.Tire.Cey];
%%
figure;
scatter(SlipAngle, XForce);
hold on
alpha = linspace(-0.5,0.5, 1000);
plot(alpha, Func(Pacejka , alpha), 'LineStyle', '-.', 'LineWidth', 4);