function craneOnShip()
tic % tic starts the timer

%{
BODIES:
Ship: 1
Crane: 2
Boom: 3

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 phi3(t)  dphi3(t) ddphi3(t)
syms zeta2(t) dzeta2(t) ddzeta2(t)

syms w1 w2 w3 dw1 dw2 dw3

syms dx1 dx2 dx3 ddx1 ddx2 ddx3

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 h22 h23
syms d31 d32 d33

% 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;

% Pescribed rates:

%Turret
% Angular acceleration is equal to zero
ddphi3(t) = 0;
% Angular velocity:
dphi3(t) = 0.15; %rad/s --> v=v0+at where a=0--> v=v0 aka pescribed rate.

%Position: x=x0+v0t+1/2at^2 --> x=v0t (and if nessesary, an initial
%condition is set x=x0+v0t) This is ploted in the Rotation matrices above:
phi3(t) = dphi3(t)*t;

%Boom 
% Angular acceleration is equal to zero
ddzeta2(t) = 0;

% Angular velocity:
A = 1.414; %81*3.1416/180; %Amplitude deg*pi/180
f = 0.05; %frequency
w = 2*3.1416*f; %Angular velocity 2*pi*f
dzeta2(t) = A*sin(w*t); %rad/s --> v=v0+at where a=0--> v=v0 aka pescribed rate.

%Position: x=x0+v0t+1/2at^2 --> x=v0t (and if nessesary, an initial
%condition is set x=x0+v0t) This is ploted in the Rotation matrices above:
zeta2(t) = -A*cos(w*t)/t;

% 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 ];

% Crane from ship (z-axis)
R21 = sym(zeros(3));
R21(1,1) =  cos(phi3(t));
R21(1,2) =  -sin(phi3(t));
R21(2,1) =  sin(phi3(t));
R21(2,2) =  cos(phi3(t));
R21(3,3) =  1;

% Boom from crane (x-axis)
R32 = sym(zeros(3));
R32(1,1) =  cos(zeta2(t));
R32(1,3) = sin(zeta2(t));
R32(2,2) =  1;
R32(3,1) =  -sin(zeta2(t));
R32(3,3) =  cos(zeta2(t));

% Boom from ship
R31 = R21*R32;

% Distances from the ship CM to the crane CM. These distances are assumed
% to be zero.
h21 = 0;
h22 = 0;
h23 = 0; % heigth to the crane's CM is 13208 millimeters

h2_1 = sym(zeros(3,1));
h2_1(1) = h21;
h2_1(2) = h22;
h2_1(3) = h23;

%Distances from the boom CM to the crane CM.
d31 = 0;
d32 = 0; % outreach of 23.899 meters
d33 = 0; % heigth to top of crane is 17.012 meters

d3_2 = sym(zeros(3,1));
d3_2(1) = d31;
d3_2(2) = d32;
d3_2(3) = d33;

% The distance vectors are skewed using symSkew (defined below). These
% matrices are used in the B matrix.
s21Skew = symSkew(h2_1);
s32Skew = symSkew(d3_2);

% B matrix construction
B = sym(zeros(18, 8));
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)   = e3;
B(13:15, 1:3) = I3;
B(13:15, 4:6) = R1*R31*s32Skew.'*R31.' + R1*R21*s21Skew.'*R21.' + R1*s21Skew.';
B(13:15, 7)   = R1*R31*s32Skew.'*R32.'*e3 + R1*R21*s21Skew.'*e3;
B(13:15, 8)   = R1*R31*s32Skew.'*e2;
B(16:18, 4:6) = R31.';
B(16:18, 7)   = R32.'*e3;
B(16:18, 8)   = e2;
Bn = simplify(B); %We simplify to help MatLab solve it faster.

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

% Differentiating of the B matrix with respect to time using diff 
% and simplification using simplify.
dB = diff(Bn,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(phi3(t), t), dphi3(t));
dB = subs(dB,diff(zeta2(t), t),dzeta2(t));

% Defining the omegas
w1b = sym(zeros(3,1));
w1b(1) = w1;
w1b(2) = w2;
w1b(3) = w3;
Omega1 = symSkew(w1b);

O2 = sym(zeros(3,1)); 
O2 = R21.'*w1b+dphi3(t)*e3;
Omega2 = symSkew(O2);

O3 = sym(zeros(3,1));
O3 = R31.'*w1b+R32.'*dphi3(t)*e3+dzeta2(t)*e2;
Omega3 = symSkew(O3);

% D matrix construction
D = sym(zeros(18));
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);

% dR = R*omegaskew
dR1 = R1*Omega1;

dB = subs(dB,diff(R1, t),dR1);

% M matrix construction
M = sym(zeros(18));

%Ship J11 ~= J12 -- The mass of the ship is assumed, and the inertia is
%calculated like a cuboid.
%The inertia of the ship has to be different numbers
M(1,1) = 4100; %tonne
M(2,2) = 4100; %tonne
M(3,3) = 4100; %tonne

wi = 22; %m
h = 10;  %m
d = 85; %m

M(4,4) = 1/12*M(1,1)*(h^2+d^2);  %tonne m^2
M(5,5) = 1/12*M(2,2)*(wi^2+d^2); %tonne m^2
M(6,6) = 1/12*M(3,3)*(wi^2+h^2); %tonne m^2

% Turret all J's are zero
M(7,7) = 0;
M(8,8) = 0;
M(9,9) = 0;

M(10,10) = 0;
M(11,11) = 0;
M(12,12) = 0;

% Boom J31 ~= J32 ~= J33
M(13,13) = 78+3.8+3.4; %tonne
M(14,14) = 78+3.8+3.4; %tonne
M(15,15) = 78+3.8+3.4; %tonne

%The inertis is calculated using Creo
M(16,16) = 1.1763498e+5; %tonne m^2
M(17,17) = 1.1800369e+7; %tonne m^2
M(18,18) = 1.1739636e+7; %tonne m^2

%Assumptions:
%text.

% Vector for the generalized velocities, to be multiplied by N*.
dq = sym(zeros(8,1));      
dq(1)  = dx1;
dq(2)  = dx2;
dq(3)  = dx3;
dq(4)  = w1;
dq(5)  = w2;
dq(6)  = w3;
dq(7)  = dphi3(t);
dq(8)  = dzeta2(t);

% Vector for the generalized acceleration, to be multiplied by M*.
ddq = sym(zeros(8,1));      
ddq(1)  = ddx1;
ddq(2)  = ddx2;
ddq(3)  = ddx3;
ddq(4)  = dw1;
ddq(5)  = dw2;
ddq(6)  = dw3;
ddq(7)  = ddphi3(t);
ddq(8)  = ddzeta2(t);

%{
Building M*, N* -> See Chapter 13 of Murakami's and Impelluso's
textbook.
Equation to be solved for generalized accelerations (ddq):
(M*)(ddq) + (N*)(dq) = 0 --> ddq = inv(M*)[-(N*)(dX)]
%}
Mstar  = simplify((Bt*M*Bn)); %Mstar
Nstar  = simplify((Bt*(D*M*Bn + M*dB))); %Nstar
%{
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.
%}
fprintf('%4.2f seconds. Inverse of Mstar has started.\n',toc);
invMstar = inv(Mstar);
fprintf('%4.2f seconds. Inverse of Mstar has ended.\n',toc);
%{
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.
In this script, we simplify 1000 times.
%}   
ddq = simplify(invMstar*(-Nstar*dq));

fprintf('%4.2f seconds. ddq has been calculated.\n',toc);

parfor i = 4:6;
    ddq(i) = simplify(ddq(i),'Steps',1000);
end
%{
ddq(1) = ddx1
ddq(2) = ddx2
ddq(3) = ddx3
ddq(4) = dw1  !
ddq(5) = dw2  !
ddq(6) = dw3  !
ddq(7) = ddphi3
ddq(8) = ddzeta2

In this problem the equations of interest are the ones for the angular
accelerations of the ship (ddw1, ddw2, ddw3).
Vpa, gives the option of choosing how many significant numbers to print.
%}

disp(vpa(ddq(4),3));
disp(vpa(ddq(5),3));
disp(vpa(ddq(6),3));
toc % Stop timer
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