function gyroSymbolic()
tic
% tic starts a timer which ends when toc is issued (end of script). This
% enables one to analyse code performance.

%{
BODIES:
Ship -> 1
First gimbal -> 2
Second gimbal -> 4
First rotor -> 3
Second rotor -> 5
%}

%{
Declaration of symbolic variables.
All components of matrices and vectors has to be defined as symbolic
variables.

Symbolic variables can be defined as dependent variable i.e. time dependent
variables. This can be used for differentiation.

Declaration of symbolic variable:       syms <varName>
Declaration of dependent sym variable:  syms <varName(var)>
%}

syms t
syms phi2(t) dphi2(t) 
syms phi4(t) dphi4(t) 
syms psi3(t) dpsi3(t) 
syms psi5(t) dpsi5(t)

syms dx1 dx2 dx3 w11 w12 w13
syms ddx1 ddx2 ddx3 dw11 dw12 dw13

syms R1_11(t) R1_12(t) R1_13(t) 
syms R1_21(t) R1_22(t) R1_23(t) 
syms R1_31(t) R1_32(t) R1_33(t)

syms h21_1 h21_2 h21_3
syms h41_1 h41_2 h41_3

syms m1 m2 m3 m4 m5;
syms J11 J12 J13 J21 J22 J23 J31 J32 J33 J41 J42 J43 J51 J52 J53;

syms Fw1 Fw2 Fw3 Mw1 Mw2 Mw3 Mr Mg g Mb1 Mb2 Mb3 Fd2 Md1

n = 5;      % Number of bodies
nstar = 10;  % Number of essential generalized coordinates

% The two gimbals and rotors have equal and opposite nutation and spin.
% Using this, as done below, enables MATLAB to do simplifications of the 
% equations.
psi3 = -psi5;
phi2 = -phi4;
dpsi3 = -dpsi5;
dphi2 = -dphi4;

% Vectors and matrices containing non-symbolic elements should be defined
% by using the sym function.
I3 = sym(eye(3));

% e2 and e3 are unit vectors for the two and three axis. They are both used
% to simplify notation.
e2 = sym(zeros(3,1));
e2(2) = 1;
e3 = sym(zeros(3,1));
e3(3) = 1;

% Rotation matrices are defined below
% Ship
R1 = [R1_11 R1_12 R1_13; R1_21 R1_22 R1_23; R1_31 R1_32 R1_33];

% Gimbals from ship
R21 = sym(zeros(3));
R21(1,1) =  cos(phi2);
R21(1,3) =  sin(phi2);
R21(3,3) =  cos(phi2);
R21(3,1) = -sin(phi2);
R21(2,2) =  1;

R41 = sym(zeros(3));
R41(1,1) =  cos(phi4);
R41(1,3) =  sin(phi4);
R41(3,3) =  cos(phi4);
R41(3,1) = -sin(phi4);
R41(2,2) =  1;

% Rotors from gimbals
R32 = sym(zeros(3));
R32(1,1) =  cos(psi3);
R32(1,2) = -sin(psi3);
R32(2,2) =  cos(psi3);
R32(2,1) =  sin(psi3);
R32(3,3) =  1;

R54 = sym(zeros(3));
R54(1,1) =  cos(psi5);
R54(1,2) = -sin(psi5);
R54(2,2) =  cos(psi5);
R54(2,1) =  sin(psi5);
R54(3,3) =  1;

% Rotors from ship
R31 = R21*R32;

R51 = R41*R54;

% Distances from the ship CM to the gimbals CM. These distances are assumed
% to be zero.
h21_1 = 0;
h21_2 = 0;
h21_3 = 0;

h41_1 = 0;
h41_2 = 0;
h41_3 = 0;

h21(1) = h21_1;
h21(2) = h21_2;
h21(3) = h21_3;

h41(1) = h41_1;
h41(2) = h41_2;
h41(3) = h41_3;

% The distance vectors are skewed using symSkew (defined below). These
% matrices are used in the B matrix.
s21Skew = symSkew(h21);
s41Skew = symSkew(h41);

% B matrix construction
B = sym(zeros(6*n,nstar));
B(1:3, 1:3) = I3;
B(4:6, 4:6) = I3;
B(7:9, 1:3) = I3;
B(7:9, 4:6) = R1*s21Skew.';
B(10:12, 4:6) = R21.';
B(10:12, 7) = e2;
B(13:15, 1:3) = I3;
B(13:15, 4:6) = R1*s21Skew.';
B(16:18, 4:6) = R31.';
B(16:18, 7) = R32.' * e2;
B(16:18, 9) = e3;
B(19:21, 1:3) = I3;
B(19:21, 4:6) = R1*s41Skew.';
B(22:24, 4:6) = R41.';
B(22:24, 8) = e2;
B(25:27, 1:3) = I3;
B(25:27, 4:6) = R1*s41Skew.';
B(28:30, 4:6) = R51.';
B(28:30, 8) = R54.' * e2;
B(28:30, 10) = e3;

% The operator .' transposes symbolic matrices and vectors. Using the '
% operator computes complex conjugate transposes.
Bt = B.';

% Differentiating of the B matrix with respect to time using diff 
% and simplification using simplify.
dB = diff(B,t);

% Using the subs function one can replace symbolic expressions. 
% Below 'diff(phi2(t),t)' are replaced with 'dphi2(t)' and so on.
dB = subs(dB,diff(phi2(t),t),dphi2(t));
dB = subs(dB,diff(psi3(t),t),dpsi3(t));
dB = subs(dB,diff(phi4(t),t),dphi4(t));
dB = subs(dB,diff(psi5(t),t),dpsi5(t));

% Assuming that the gimbals and rotors have equal mass and inertia.
m2 = m4;
m3 = m5;
J22 = J42;
J33 = J53;

% M matrix construction
M = sym(zeros(6*n));
M(1,1) = m1;
M(2,2) = m1;
M(3,3) = m1;

M(4,4) = J11;
M(5,5) = J12;
M(6,6) = J13;

M(7,7) = m2;
M(8,8) = m2;
M(9,9) = m2;

% Assuming that the gimbal is symmetric about all three axes
M(10,10) = J22;
M(11,11) = J22;
M(12,12) = J22;

M(13,13) = m3;
M(14,14) = m3;
M(15,15) = m3;

% Assuming that the rotor is a thin disk
M(16,16) = J33/2;
M(17,17) = J33/2;
M(18,18) = J33;

M(19,19) = m4;
M(20,20) = m4;
M(21,21) = m4;

M(22,22) = J42;
M(23,23) = J42;
M(24,24) = J42;

M(25,25) = m5;
M(26,26) = m5;
M(27,27) = m5;

M(28,28) = J53/2;
M(29,29) = J53/2;
M(30,30) = J53;

% Defining the omegas
w1 = sym(zeros(3,1));
w1(1) = w11;
w1(2) = w12;
w1(3) = w13;
Omega1 = symSkew(w1);

O2 = sym(zeros(3,1));
O2(1:3,1) = R21.'*w1 + (dphi2*e2);
Omega2 = symSkew(O2);

O3 = sym(zeros(3,1));
O3(1:3,1) = R31.'*w1 + (R32.'* (dphi2*e2)+ dpsi3*e3);
Omega3 = symSkew(O3);

O4 = sym(zeros(3,1));
O4(1:3,1) = R41.'*w1 + dphi4*e2;
Omega4 = symSkew(O4);

O5 = sym(zeros(3,1));
O5(1:3,1) = R51.'*w1 + R54.'* (dphi4*e2)+ dpsi5*e3;
Omega5 = symSkew(O5);

% D matrix construction
D = sym(zeros(6*n));
D(4:6,4:6)     = Omega1(1:3,1:3);
D(10:12,10:12) = Omega2(1:3,1:3);
D(16:18,16:18) = Omega3(1:3,1:3);
D(22:24,22:24) = Omega4(1:3,1:3);
D(28:30,28:30) = Omega5(1:3,1:3);

%{
Defining forces and moments
Fw, Mw: Wave forces and moments. These will be found from sensors in the
experimental set up.
Fb, Mb: Bouyancy. Represents stiffness.
Fd, Md: Drag. Represents damping.

Assumptions:
- The rotors are moving with a prescribed angular velocity --> moment from
rotor is zero --> Mr = 0.
%}
Fw = sym(zeros(3,1));
Mw = sym(zeros(3,1));
Mb = sym(zeros(3,1));
Fd = sym(zeros(3,1));
Md = sym(zeros(3,1));

Fw(1) = Fw1;
Fw(2) = Fw2;
Fw(3) = Fw3;
Mw(1) = Mw1;
Mw(2) = Mw2;
Mw(3) = Mw3;
Fb = (m1 + m2 + m3 + m4 + m5)*g*e3;

Fd(2) = Fd2;
Md(1) = Md1;

Mb(1) = Mb1;

Mr = 0;

% Constructing the force vector
F1 = Fw + Fb - m1*g*e3 + Fd;
M1 = Mw + Mb + Md;
F2 = -m2*g*e3;
M2 = -Mr*e3 + Mg*e2;
F3 = -m3*g*e3;
M3 = Mr*e3;
F4 = -m4*g*e3;
M4 = Mr*e3 - Mg*e2;
F5 = -m5*g*e3;
M5 = -Mr*e3;
Q = sym(zeros(6*n,1));
Q(1:3,1) = F1;
Q(4:6,1) = M1;
Q(7:9,1) = F2;
Q(10:12,1) = M2;
Q(13:15,1) = F3;
Q(16:18,1) = M3;
Q(19:21,1) = F4;
Q(22:24,1) = M4;
Q(25:27,1) = F5;
Q(28:30,1) = M5;

% Vector for the generalized velocities, to be multiplied by N*.
dX = sym(zeros(nstar,1));
dX(1) = 0;
dX(2) = 0;
dX(3) = 0;
dX(4) = w11;
dX(5) = w12;
dX(6) = w13;
dX(7) = dphi2;
dX(8) = dphi4;
dX(9) = dpsi3;
dX(10) = dpsi5;

ddX = sym(zeros(nstar,1));
ddX(1) = ddx1;
ddX(2) = ddx2;
ddX(3) = ddx3;
ddX(4) = dw11;
ddX(5) = dw12;
ddX(6) = dw13;

%{
Building M*, N* and F*. See Chapter 13 of Murakami's and Impelluso's
textbook.
Equation to be solved for generalized accelerations (ddX):
(M*)(ddX) + (N*)(dX) = (F*) --> ddX = inv(M*)[(F*) - (N*)(dX)]
%}
Mstar  = (Bt*M*B); %Mstar
Nstar  = (Bt*(D*M*B + M*dB)); %Nstar
Fstar =  (Bt*Q); %Fstar


%{
There are two ways of solving the equations by using the inverse in MATLAB.
The following operations produces the same result:
1: Ax = b --> x = inv(A)b
2: Ax = b --> x = A\b.

MATLAB advices to use the second method, however in this case and on this machine
the second method seemed to be the fastest.
%}

invMstar = inv(Mstar);

%{
Solving the equation is done below. The inbuilt simplify function is used
to simplify the input argument and make it more readable.

NOTE:
Using the simplify function is time consuming and should be avoided unless
one wants to print something to the command window or a file.
%}

eq1 = simplify(Mstar*ddX)
eq2 = simplify(Nstar*dX)
eq3 = simplify(Fstar)
eq4 = simplify((Fstar - Nstar*dX)./Mstar)
myLatexInterpreter(eq1)
myLatexInterpreter(eq2)
myLatexInterpreter(eq3)
myLatexInterpreter(eq4)

%{
equation(1) = ddx1
equation(2) = ddx2
equation(3) = ddx3
equation(4) = dw11
equation(5) = dw12
equation(6) = dw13
equation(7) = ddphi2
equation(8) = ddphi4
equation(9) = ddpsi3
equation(10) = ddpsi5
%}

% In this problem the equations of interest are the ones for the angular
% accelerations of the ship (dw11, dw12, dw13).
%disp(simplify(equation(4)));
%myLatexInterpreter(simplify(equation(4)))
%disp(equation(5));
%disp(equation(6));
%disp(equation(7));
%disp(equation(8));

toc % Stop timer

%{
Final tip:
MATLAB has a inbuilt latex interpreter which displays strings as with latex
formatting. This is handy when working with latex and can be printed to
external files or to the command window.

The example below formats the equation for dw12 using latex and displays
it to the command window using disp.
%}
%Example:
%disp(latex(equation(5)));
end


function [Mat] = symSkew(vec)
% symSkew skews a symbolic 3-element vector (input) to  a 3x3 skew 
% symmetric symbolic matrix (output).
    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