function reduced

syms t
syms th1t(t) th2t(t)
syms dth1t(t) dth2t(t)
syms ddth1t(t) ddth2t(t)
syms th1 th2
syms dth1 dth2
syms ddth1 ddth2

syms dx1 dx2 dx3 w1 w2 w3 dw1 dw2 dw3
syms ddx1 ddx2 ddx3

syms L
syms m1 m2 m3
syms J11 J12 J13 J21 J22 J23 J31 J32 J33

e1 = sym([1;0;0]);
e3 = sym([0;0;1]);

R2_1 = sym(eye(3,3));
R2_1(1,1) = cos(th1t);
R2_1(2,2) = cos(th1t);
R2_1(1,2) = -sin(th1t);
R2_1(2,1) = sin(th1t);

R3_2 = sym(eye(3,3));
R3_2(2,2) = cos(th2t);
R3_2(3,3) = cos(th2t);
R3_2(2,3) = -sin(th2t);
R3_2(3,2) = sin(th2t);

I3 = sym(eye(3,3));

B_matrix = sym(zeros(9,5));
B_matrix(1:3,1:3) = I3;
B_matrix(4:6,1:3) = R2_1.';
B_matrix(4:6,4) = e3;
B_matrix(7:9,1:3) = R3_2.' * R2_1.';
B_matrix(7:9,4) = R3_2.' * e3;
B_matrix(7:9,5) = e1;

Bt_matrix = B_matrix.';
dB_matrix = diff(B_matrix,t);

%Some assumptions about the masses...
% m1 = 2000;
% m2 = 0;
% m3 = 100;

% J11 = m1 * 4 / 6;
J13 = J11;
J12 = J11;

% J21 = m2 / 2;
J21 = 0;
J22 = 0;
J23 = J21;

% J31 = m3 / 3; 
J32 = 0;
J33 = J31;

M_matrix = sym(diag([J11,J12,J13,J21,J22,J23,J31,J32,J33]));

%Define the omegas
w = [w1;w2;w3];
O2 = sym(zeros(3,1));
O2(1:3,1) = (R2_1.' * w) + (dth1t * e3);
O3 = sym(zeros(3,1));
O3(1:3,1) = R3_2.' * R2_1.' * w + R3_2.' * dth1t * e3 + dth2t * e1;
Omega1 = symSkew(w);
Omega2 = symSkew(O2);
Omega3 = symSkew(O3);

D_matrix = sym(zeros(9,9));
D_matrix(1:3,1:3) = Omega1(1:3,1:3);
D_matrix(4:6,4:6) = Omega2(1:3,1:3);
D_matrix(7:9,7:9) = Omega3(1:3,1:3);

Mstar = simplify(Bt_matrix * M_matrix * B_matrix);
Nstar = simplify(Bt_matrix * (M_matrix * dB_matrix + D_matrix * M_matrix * B_matrix));


dqr = sym(zeros(5,1));
dqr(1,1) = dw1;
dqr(2,1) = dw2;
dqr(3,1) = dw3;
dqr(4,1) = ddth1t;
dqr(5,1) = ddth2t;

qr = sym(zeros(5,1));
qr(1,1) = w1;
qr(2,1) = w2;
qr(3,1) = w3;
qr(4,1) = dth1t;
qr(5,1) = dth2t;

ess = sym(zeros(3,1));
for a= 1:3
    for j=1:5
        ess(a) = ess(a) - Nstar(a,j)*qr(j);
        if(j~= a)
            ess(a) = ess(a) - Mstar(a,j)*dqr(j);
        end
    end
    ess(a) = ess(a) / Mstar(a,a);
end


%SETTING UP FOR GAUSS ELIMINATION

syms ga gb gc gd gf gg gh gi

ga = sym(ess(1));
gb = sym(ess(1));
gc = sym(ess(1));
gd = sym(ess(2));
ge = sym(ess(2));
gf = sym(ess(2));
gg = sym(ess(3));
gh = sym(ess(3));
gi = sym(ess(3));

ga = subs(ga,dw2,0);
ga = subs(ga,dw3,0);

gb = subs(gb,dw2,1);
gb = subs(gb,dw3,0);
gb = gb - ga;

gc = subs(gc,dw2,0);
gc = subs(gc,dw3,1);
gc = gc - ga;

gd = subs(gd,dw1,0);
gd = subs(gd,dw3,0);

ge = subs(ge,dw1,1);
ge = subs(ge,dw3,0);
ge = ge - gd;

gf = subs(gf,dw1,0);
gf = subs(gf,dw3,1);
gf = gf - gd;

gg = subs(gg,dw1,0);
gg = subs(gg,dw2,0);

gh = subs(gh,dw1,1);
gh = subs(gh,dw2,0);
gh = gh - gg;

gi = subs(gi,dw1,0);
gi = subs(gi,dw2,1);
gi = gi - gg;

gauss_matrix = [-1,gb,gc,-ga;ge,-1,gf,-gd;gh,gi,-1,-gg];
gauss_matrix = rref(gauss_matrix);
q_unc = sym(zeros(3,1));
q_unc(1:3) = gauss_matrix(1:3,4);

q_unc = subs(q_unc,diff(th1t(t), t),dth1t);
q_unc = subs(q_unc,diff(th2t(t), t),dth2t);

time = 0;
step = 1;
step_max = 2000;
while step < step_max
    step = step * 2;
    parfor a=1:3
        q_unc(a) = simplify(q_unc(a),'Steps',step);
    end
    clc;
    disp(step);
end

disp(q_unc);
end

function [Mat] = symSkew(vec)
    Mat = sym(zeros(3));
    Mat(1,2) = -vec(3);
    Mat(1,3) =  vec(2);
    Mat(2,3) = -vec(1);
    Mat(2,1) = -Mat(1,2);
    Mat(3,1) = -Mat(1,3);
    Mat(3,2) = -Mat(2,3);
end