clear all
clc

syms theta(t) x(t) tau(t) Ibody Iwheel Mbody Mwheel l r b g

% commonly seen coeff
coeff_Ib_mbl2 = Ibody + Mbody*(l^2)
coeff_mb_Iw_mw = Mbody + 2*Iwheel/(r^2) + 2*Mwheel
coeff_mb_l = Mbody*l
coeff_mb2_l2 = coeff_mb_l^2

%eq1 = diff(x(t),t,2)*coeff_mb_Iw_mw + coeff_mb_l*diff(theta(t), t, 2) == tau(t)/r - diff(x(t), t, 1)*b/(r^2)
%eq2 = diff(theta(t), t, 2)*(I_body+M_body*(l^2)) + coeff_mb_l*diff(x(t), t, 2) - g*coeff_mb_l*theta(t) == -tau(t)

eq1 = coeff_mb_Iw_mw * diff(x(t), t, 2) ==  ( -coeff_mb_l*diff(theta(t), t, 2)*cos(theta(t))) + coeff_mb_l*(diff(theta(t), t, 1)^2)*sin(theta(t)) + tau(t)/r - diff(x(t), t, 1)*b*(r^2)
eq2 = coeff_Ib_mbl2 * diff(theta(t), t, 2) == -coeff_mb_l*diff(x(t), t, 2)*cos(theta(t)) + coeff_mb_l*g*sin(theta(t)) - tau(t)


disp('method 1 - substituition in time')
syms thetaddot thetadot xddot xdot torque
eq1_1 = subs(eq1, [diff(theta, t, 2) diff(theta, t, 1) theta(t) diff(x, t, 2) diff(x, t, 1) x(t) tau], [thetaddot thetadot theta xddot xdot x torque])
eq1_2 = subs(eq2, [diff(theta, t, 2) diff(theta, t, 1) theta(t) diff(x, t, 2) diff(x, t, 1) x(t) tau], [thetaddot thetadot theta xddot xdot x torque])
thetaddot_partial = solve(eq1_1, thetaddot, 'IgnoreAnalyticConstraints', true)
xddot_partial = subs(eq1_2, thetaddot, thetaddot_partial)
xddot_sol = solve(xddot_partial, xddot, 'IgnoreAnalyticConstraints', true)
thetaddot_partial = subs(eq1_1, xddot, xddot_sol)
thetaddot_sol = solve(thetaddot_partial, thetaddot, 'IgnoreAnalyticConstraints', true)


xddot = vpa(xddot_sol, 3)
thetaddot = vpa(thetaddot_sol ,3)

