% x1 = Beta "side slip angle of the center of gravity"
% x2 = velocity
% x3 = yaw
% f1 = x1_dot
% f2 = x2_dot
% f3 = x3_dot
% y1 = Beta
% y2 = velocity
% y3 = yaw
% y4 = alpha_f "tire side slip angle front"
% y5 = alpha_r
% y6 = lambda_f "tire slip ratio front"
% y7 = lambda_r
% u1 = delta "steering angle"
% u2 = omega_f "angular velosity front"
% u3 = omega_r
%%
data = load('Linearization/Fixed_point_20.mat');
data = data.simout;
% data = simout;
lf  = car.lf;
lr  = car.lr;
m   = car.m;
Fzf = car.Fzf;
Fzr = car.Fzr;
p   = car.p;
I   = car.I;
Cdy = car.Tire.Cdy;
Cby = car.Tire.Cby;
Ccy = car.Tire.Ccy;
Cey = car.Tire.Cey;
Cdx = car.Tire.Cdx;
Cbx = car.Tire.Cbx;
Ccx = car.Tire.Ccx;
Cex = car.Tire.Cex;
syms x1 x2 x3 f1 f2 f3 y1 y2 y3 y4 y5 y6 y7 u1 u2 u3

y1 = x1;
y2 = x2;
y3 = x3;

vf = [cos(u1) sin(u1); -sin(u1) cos(u1)]*...
     [x2*cos(x1); x2*sin(x1) + lf*x3];
vr = [1 0; 0 1]*...
     [x2*cos(x1); x2*sin(x1) - lr*x3];
vxf = vf(1);
vyf = vf(2);
vxr = vr(1);
vyr = vr(2); 

y4 = -atan2(vyf, abs(vxf));
y5 = -atan2(vyr, abs(vxr));
y6 = (u2*p - vxf)/abs(u2*p);
y7 = (u3*p - vxr)/abs(u3*p);



Fyf = Cdy * Fzf * sin(Ccy * atan(Cby*y4 - Cey*(Cby*y4 - atan(Cby*y4))));
Fyr = Cdy * Fzr * sin(Ccy * atan(Cby*y5 - Cey*(Cby*y5 - atan(Cby*y5))));

Fxf = Cdx * Fzf * sin(Ccx * atan(Cbx*y6 - Cex*(Cbx*y6 - atan(Cbx*y6))));
Fxr = Cdx * Fzr * sin(Ccx * atan(Cbx*y7 - Cex*(Cbx*y7 - atan(Cbx*y7))));

F = [Fxf; Fyf; Fxr; Fyr];

SAP = [cos(u1), -sin(u1), 1, 0;...
    sin(u1), cos(u1), 0, 1;...
    lf*sin(u1), lf*cos(u1), 0, -lr];

RotDyn = [-sin(x1), cos(x1), 0;...
    cos(x1), sin(x1), 0;...
    0, 0, 1];

C = diag([1/m/x2, 1/m, 1/I]);

Dyn = C * RotDyn * SAP * F - [x3;0;0];
f1 = Dyn(1);
f2 = Dyn(2);
f3 = Dyn(3); 

A = [diff(f1, x1) diff(f1, x2) diff(f1, x3);
    diff(f2, x1) diff(f2, x2) diff(f2, x3);
    diff(f3, x1) diff(f3, x2) diff(f3, x3)];
B = [diff(f1, u1) diff(f1, u2) diff(f1, u3);
    diff(f2, u1) diff(f2, u2) diff(f2, u3);
    diff(f3, u1) diff(f3, u2) diff(f3, u3)];
C = [diff(y1, x1) diff(y1, x2) diff(y1, x3);
    diff(y2, x1) diff(y2, x2) diff(y2, x3);
    diff(y3, x1) diff(y3, x2) diff(y3, x3);
    diff(y4, x1) diff(y4, x2) diff(y4, x3);
    diff(y5, x1) diff(y5, x2) diff(y5, x3);
    diff(y6, x1) diff(y6, x2) diff(y6, x3);
    diff(y7, x1) diff(y7, x2) diff(y7, x3)];
D = [diff(y1, u1) diff(y1, u2) diff(y1, u3);
    diff(y2, u1) diff(y2, u2) diff(y2, u3);
    diff(y3, u1) diff(y3, u2) diff(y3, u3);
    diff(y4, u1) diff(y4, u2) diff(y4, u3);
    diff(y5, u1) diff(y5, u2) diff(y5, u3);
    diff(y6, u1) diff(y6, u2) diff(y6, u3);
    diff(y7, u1) diff(y7, u2) diff(y7, u3)];
initU = [0 20 20];
initBeta = data.Data(end,1);
initV = data.Data(end,2);
initR = data.Data(end,3);
P = data.Data(end,:); 
A = double(subs(A, [x1 x2 x3 y1 y2 y3 y4 y5 y6 y7 u1 u2 u3], [data.Data(end,1:3) P initU]));
B = double(subs(B, [x1 x2 x3 y1 y2 y3 y4 y5 y6 y7 u1 u2 u3], [data.Data(end,1:3) P initU]));
C = double(subs(C, [x1 x2 x3 y1 y2 y3 y4 y5 y6 y7 u1 u2 u3], [data.Data(end,1:3) P initU]));
D = double(subs(D, [x1 x2 x3 y1 y2 y3 y4 y5 y6 y7 u1 u2 u3], [data.Data(end,1:3) P initU]));

SS = ss(A,B,C,D);
TF = tf(SS);



