%% Set up basic symbols.
% Use sprintf() and char(sym()) to automate.

syms x a dx da ddx dda
q = [ x; a ];
dq = [ dx; da ];
ddq = [ ddx; dda ];

%% Physical parameters of the segway - masses, inertias, lengths, gravity
syms Mw Iw Rw Hr Ir Mr g

%% Wheel kinetic and potential energy
Tw = (1/2) * Mw * dx^2 + (1/2)*Iw*(dx/Rw)^2
Vw = sym('0')

%% Rider kinetic and potential energy
Tr = (1/2)*Ir*(da^2) + (1/2)*Mr*((dx+Hr*da*cos(a))^2+(Hr*da*sin(a))^2)
Vr = Mr * g * Hr * cos(a)

%% The Lagrangian of the segway
T = Tw + Tr
V = Vw + Vr
L = T - V

%------- From here on the program is completely generic. Just
% make sure you have q, dq, ddq, and L.

%% Differentiate the second term of the lagrangian.
dLdq = transpose(jacobian(L, q));

%% Inner differentiation of the first term of the lagrangian.
dLdqdot = transpose(jacobian(L, dq));

%% Differentiate first terms w.r.t. time using chain rule
dtdLdqdot = jacobian( dLdqdot, [ q; dq ] ) * [dq; ddq];

%% Form expressions for generalized forces
Q = dtdLdqdot - dLdq

%% Gather into matrix form
% Forces are linear in ddx, dda
Q = expand(simplify(Q))
MM = sym(zeros(length(q)));
rest = sym(zeros(length(q),1));

%% Use coeffs to get mass matrix
Qcopy = Q;
for i = 1:length(q)
    for j = 1:length(q)
        [Cc,Tc] = coeffs(Qcopy(i), ddq(j));
        if length(Cc) == 2 && Tc(2) == ddq(j)
          MM(i,j) = simplify(Cc(2));
          Qcopy(i) = expand(Qcopy(i) - Cc(2) * ddq(j));
        end
    end
    rest(i) = simplify(Qcopy(i));
end