%%
data_all = load('LongPacejka/lfs_data_slip_ratio.mat');
data_all = data_all.lfs_data;

data = load('LongPacejka/lfs_data_slip_ratio_selected.mat');
data = data.lfs_data;
%%
Time = data_all.Time.Time;
Time_selected = data(:,1)/1000;
W3_SlipRatio = (data_all.W3_SlipRatio.Data(data(:,1)));
W4_SlipRatio = (data_all.W4_SlipRatio.Data(data(:,1)));
W3_YForce = data_all.W3_YForce.Data(data(:,1));
W4_YForce = data_all.W4_YForce.Data(data(:,1)); 
W3_VerticalLoad = data_all.W3_VerticalLoad.Data(data(:,1));
W4_VerticalLoad = data_all.W4_VerticalLoad.Data(data(:,1));
% %%
% plot(Time, (data_all.W3_SlipRatio.Data))
% hold on
% plot(Time, (data_all.W4_SlipRatio.Data))
% scatter(Time_selected, W3_SlipRatio)
% scatter(Time_selected, W4_SlipRatio)
% 
% figure;
% plot(Time, data_all.W3_YForce.Data)
% hold on
% plot(Time, data_all.W4_YForce.Data)
% scatter(Time_selected, W3_YForce)
% scatter(Time_selected, W4_YForce)
% 
% 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)
%%
SlipRatio = (W3_SlipRatio + W4_SlipRatio)/2;
% tmp = SlipRatio(find(SlipRatio > 0 && YForce > 0));
YForce = W3_YForce + W4_YForce;
% YForce = YForce(find(SlipRatio > 0 && YForce > 0));
SlipRatio = SlipRatio(YForce>0);
YForce = YForce(YForce>0);
% SlipRatio = tmp;
SlipRatio = [SlipRatio; -SlipRatio];
YForce = [YForce; -YForce];
VerticalLoad = W3_VerticalLoad + W4_VerticalLoad;
%%
Func = @(Pacejka, SlipRatio)Pacejka(1).*sin((Pacejka(2).*atan(Pacejka(3).*SlipRatio - Pacejka(4).*(Pacejka(3).*SlipRatio - atan(Pacejka(3).*SlipRatio)))));
x0 = [3000 0.1 10 1];
opts = statset('nlinfit');
opts.RobustWgtFun = 'huber';
Pacejka = nlinfit(SlipRatio,YForce,Func,x0,opts);
% Pacejka_func = @(h, event)(disp(get(h,'Value')));
%%
f = figure;
set(f,'Position',[200 200 600 600]);
ax = axes('Parent',f,'position',[0.13 0.39  0.77 0.54]);
scatter(SlipRatio, YForce);
hold on
alpha = linspace(-10,10, 10000);
h = plot(alpha, Func(Pacejka, alpha), 'LineStyle', '--', 'LineWidth', 2);
%%
SliderD = uicontrol('Parent',f,'Style','slider','Position',[81,180,419,23],...
              'value',x0(1), 'min',0, 'max',20000, 'Callback', {@Pacejka_func, 'D'});
bgcolor = f.Color;
uicontrol('Parent',f,'Style','text','Position',[50,180,23,23],...
                'String','0','BackgroundColor',bgcolor);
uicontrol('Parent',f,'Style','text','Position',[500,180,40,23],...
                'String','20000','BackgroundColor',bgcolor);
LabelD = uicontrol('Parent',f,'Style','text','Position',[240,150,100,23],...
                'String',['D_0 = ' num2str(x0(1))],'BackgroundColor',bgcolor);
            
SliderC = uicontrol('Parent',f,'Style','slider','Position',[81,130,419,23],...
              'value',x0(2), 'min',0, 'max',10, 'Callback', {@Pacejka_func, 'C'});
uicontrol('Parent',f,'Style','text','Position',[50,130,23,23],...
                'String','0','BackgroundColor',bgcolor);
uicontrol('Parent',f,'Style','text','Position',[500,130,40,23],...
                'String','10','BackgroundColor',bgcolor);
LabelC = uicontrol('Parent',f,'Style','text','Position',[240,100,100,23],...
                'String',['C_0 = ' num2str(x0(2))],'BackgroundColor',bgcolor);
            
SliderB = uicontrol('Parent',f,'Style','slider','Position',[81,80,419,23],...
              'value',x0(3), 'min',0, 'max',10, 'Callback', {@Pacejka_func, 'B'});
uicontrol('Parent',f,'Style','text','Position',[50,80,23,23],...
                'String','0','BackgroundColor',bgcolor);
uicontrol('Parent',f,'Style','text','Position',[500,80,40,23],...
                'String','10','BackgroundColor',bgcolor);
LabelB = uicontrol('Parent',f,'Style','text','Position',[240,50,100,23],...
                'String',['B_0 = ' num2str(x0(3))],'BackgroundColor',bgcolor);

SliderE = uicontrol('Parent',f,'Style','slider','Position',[81,30,419,23],...
              'value',x0(4), 'min',0, 'max',10, 'Callback', {@Pacejka_func, 'E'});
uicontrol('Parent',f,'Style','text','Position',[50,30,23,23],...
                'String','0','BackgroundColor',bgcolor);
uicontrol('Parent',f,'Style','text','Position',[500,30,40,23],...
                'String','10','BackgroundColor',bgcolor);
LabelE = uicontrol('Parent',f,'Style','text','Position',[240,0,100,23],...
                'String',['E_0 = ' num2str(x0(4))],'BackgroundColor',bgcolor);
function Pacejka_func(slider, EventData, param)
    tmp = num2str(get(slider,'Value'));
    switch param
        case 'D'
            evalin('base',['x0(1) = ' tmp ';']);
            label = evalin('caller', 'LabelD');
            label.String = ['D_0 = ' tmp];
        case 'C'
            evalin('base',['x0(2) = ' tmp ';']);
            label = evalin('caller', 'LabelD');
            label.String = ['C_0 = ' tmp];
        case 'B'
            evalin('base',['x0(3) = ' tmp ';']);
            label = evalin('caller', 'LabelD');
            label.String = ['B_0 = ' tmp];
        case 'E'
            evalin('base',['x0(4) = ' tmp ';']);
            label = evalin('caller', 'LabelD');
            label.String = ['E_0 = ' tmp];
    end
    SlipRatio = evalin('caller', 'SlipRatio');
    YForce = evalin('caller', 'YForce');
    h = evalin('caller', 'h');
    f = evalin('caller', 'f');
    x0 = evalin('caller', 'x0');
    Func = @(Pacejka, SlipRatio)Pacejka(1).*sin((Pacejka(2).*atan(Pacejka(3).*SlipRatio - Pacejka(4).*(Pacejka(3).*SlipRatio - atan(Pacejka(3).*SlipRatio)))));
    opts = statset('nlinfit');
    opts.RobustWgtFun = 'huber';
    alpha = linspace(-10,10, 10000);
    Pacejka = nlinfit(SlipRatio,YForce,Func,x0,opts);
    disp(Pacejka)
    set(h,'ydata', Func(Pacejka, alpha));
    refreshdata(f, 'caller');
end