function getOmegaRK4()

%{
To accommodate for changing angular velocities, the reconstruction can be 
performed at every time step of the Runge-Kutta integration, assuming the 
assuming the angular velocity stays constant from t to t + delta-t.
%}
    dt = 0.05; % Small timestep 
    t = 0.01:dt:150; % Timevector to 150 secounds.
   
    % Declaration of omegas as a vector of zeros.
    w1 = zeros(1,length(t)); 
    w2 = zeros(1,length(t)); 
    w3 = zeros(1,length(t)); 
    
for i = 2:(length(t)) % i counter starts at 2 and has the same numbers of elements as timevector
    
	K1 = f1( t(i-1), w1(i-1), w2(i-1), w3(i-1))*dt;
	L1 = f2( t(i-1), w1(i-1), w2(i-1), w3(i-1))*dt;
	M1 = f3( t(i-1), w1(i-1), w2(i-1), w3(i-1))*dt;
		
	K2 = f1( t(i-1), (w1(i-1) + K1/2) ,(w2(i-1)+ L1/2), (w3(i-1)+ M1/2))*dt;
	L2 = f2( t(i-1), (w1(i-1) + K1/2) ,(w2(i-1)+ L1/2), (w3(i-1)+ M1/2))*dt;	
	M2 = f3( t(i-1), (w1(i-1) + K1/2) ,(w2(i-1)+ L1/2), (w3(i-1)+ M1/2))*dt;

	K3 = f1( t(i-1), (w1(i-1) + K2/2) ,(w2(i-1)+ L2/2), (w3(i-1)+ M2/2))*dt;
	L3 = f2( t(i-1), (w1(i-1) + K2/2) ,(w2(i-1)+ L2/2), (w3(i-1)+ M2/2))*dt;
	M3 = f3( t(i-1), (w1(i-1) + K2/2) ,(w2(i-1)+ L2/2), (w3(i-1)+ M2/2))*dt;

	K4 = f1( t(i-1), (w1(i-1) + K3) ,(w2(i-1)+ L3), (w3(i-1)+ M3))*dt;
	L4 = f2( t(i-1), (w1(i-1) + K3) ,(w2(i-1)+ L3), (w3(i-1)+ M3))*dt;
	M4 = f3( t(i-1), (w1(i-1) + K3) ,(w2(i-1)+ L3), (w3(i-1)+ M3))*dt;

	w1(i) = w1(i-1) + (K1 + 2*K2 + 2*K3 + K4)/6;
	w2(i) = w2(i-1) + (L1 + 2*L2 + 2*L3 + L4)/6;
	w3(i) = w3(i-1) + (M1 + 2*M2 + 2*M3 + M4)/6;
    
    % This is the nature of runge kutta fourth order
end
% Boom motion
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 = A*sin(w*t); %The exitation of the Boom

    figure(1);                  % Figure number
    hold on                     % Holds on to the plot to plot the next one
    plot(t, w1,'k');            % Plots omega1 with black solid line
    plot(t,dzeta2/20,'-.k');    % Plots the exitation of the Boom
    ylabel('Omega 1 [rad/s]');  % Makes Label on y-axis
    xlabel('time [s]');         % Makes Label on x-axis
    title(['\fontsize{22}Omega 1, Roll']); %Title of plot
    hold off
    
    figure(2);                  
    hold on
    plot(t, w2,'k');            %Plots omega2
    plot(t,dzeta2/20,'-.k'); 
    ylabel('Omega 2 [rad/s]');
    xlabel('time [s]');
    title(['\fontsize{22}Omega 2, Pitch']);
    hold off
    
    figure(3); 
    hold on
    plot(t, w3,'k'); 
    plot(t,dzeta2/20,'-.k');
    ylim([-0.15001 0.15001])
    ylabel('Omega 3 [rad/s]');
    xlabel('time [s]');
    title(['\fontsize{22}Omega 3, Yaw']);  
    hold off
end

function f1 = f1(t,w1,w2,w3)
         
    % This is the function for omega1 which comes from craneOnShip.m sctipt

    f1 = (1.16e-16*(1.71e42*w2*w3 ...
        + 6.38e41*cos(0.15*t)*sin(0.314*t) ...
        - 1.1e39*w2*cos(0.15*t)^2 ...
        + 3.09e43*w2*w3*cos((1.41*cos(0.314*t))/t)^2 ...
        + 1.07e41*w2*w3*cos(0.15*t)^2 ...
        - 1.53e41*cos((1.41*cos(0.314*t))/t)^2*cos(0.15*t)*sin(0.314*t) ...
        - 1.08e41*w2*cos((1.41*cos(0.314*t))/t)^2*cos(0.15*t)^2 ...
        + 1.1e39*w1*cos(0.15*t)*sin(0.15*t) ...
        + 4.25e42*w3*cos(0.15*t)*sin(0.314*t) ...
        - 7.22e41*w2*w3*cos((1.41*cos(0.314*t))/t)^2*cos(0.15*t)^2 ...
        - 1.82e41*w1*w3*cos(0.15*t)*sin(0.15*t) ...
        + 1.08e41*w1*cos((1.41*cos(0.314*t))/t)^2*cos(0.15*t)*sin(0.15*t) ...
        - 1.02e42*w3*cos((1.41*cos(0.314*t))/t)^2*cos(0.15*t)*sin(0.314*t) ...
        + 1.44e42*w1*w2*cos((1.41*cos(0.314*t))/t)*cos(0.15*t)^3*sin((1.41*cos(0.314*t))/t) ...
        + 7.22e41*w1*w3*cos((1.41*cos(0.314*t))/t)^2*cos(0.15*t)*sin(0.15*t) ...
        + 1.02e42*w1*cos((1.41*cos(0.314*t))/t)*cos(0.15*t)^2*sin((1.41*cos(0.314*t))/t)*sin(0.314*t) ...
        - 7.22e41*w1^2*cos((1.41*cos(0.314*t))/t)*cos(0.15*t)^2*sin((1.41*cos(0.314*t))/t)*sin(0.15*t) ...
        + 7.22e41*w2^2*cos((1.41*cos(0.314*t))/t)*cos(0.15*t)^2*sin((1.41*cos(0.314*t))/t)*sin(0.15*t) ...
        - 7.22e41*w1*w2*cos((1.41*cos(0.314*t))/t)*cos(0.15*t)*sin((1.41*cos(0.314*t))/t) ...
        + 1.02e42*w2*cos((1.41*cos(0.314*t))/t)*cos(0.15*t)*sin((1.41*cos(0.314*t))/t)*sin(0.15*t)*sin(0.314*t)))/(3.7e27*cos((1.41*cos(0.314*t))/t)^2 ...
        + 8.75e24*cos(0.15*t)^2 ...
        + 2.04e26);
 end

function f2 = f2(t,w1,w2,w3)

    % This is the function for omega2 which comes from craneOnShip.m sctipt

     f2 = -(2.91e-15*(6.83e40*w1*w3 ...
         - 4.17e37*w1 ...
         - 2.43e40*sin(0.15*t)*sin(0.314*t) ...
         - 4.12e39*w1*cos((1.41*cos(0.314*t))/t)^2 ...
         + 4.17e37*w1*cos(0.15*t)^2 ...
         - 1.62e41*w3*sin(0.15*t)*sin(0.314*t) ...
         + 1.08e42*w1*w3*cos((1.41*cos(0.314*t))/t)^2 ...
         - 4.29e39*w1*w3*cos(0.15*t)^2 ...
         + 5.82e39*cos((1.41*cos(0.314*t))/t)^2*sin(0.15*t)*sin(0.314*t) ...
         + 4.12e39*w1*cos((1.41*cos(0.314*t))/t)^2*cos(0.15*t)^2 ...
         + 4.17e37*w2*cos(0.15*t)*sin(0.15*t) ...
         + 2.75e40*w1*w3*cos((1.41*cos(0.314*t))/t)^2*cos(0.15*t)^2 ...
         - 2.75e40*w1^2*cos((1.41*cos(0.314*t))/t)*cos(0.15*t)^3*sin((1.41*cos(0.314*t))/t) ...
         + 2.75e40*w2^2*cos((1.41*cos(0.314*t))/t)*cos(0.15*t)^3*sin((1.41*cos(0.314*t))/t) ...
         - 1.29e39*w2*w3*cos(0.15*t)*sin(0.15*t) ...
         - 3.88e40*w2*cos((1.41*cos(0.314*t))/t)*sin((1.41*cos(0.314*t))/t)*sin(0.314*t) ...
         + 2.75e40*w1^2*cos((1.41*cos(0.314*t))/t)*cos(0.15*t)*sin((1.41*cos(0.314*t))/t) ...
         - 2.75e40*w2^2*cos((1.41*cos(0.314*t))/t)*cos(0.15*t)*sin((1.41*cos(0.314*t))/t) ...
         + 4.12e39*w2*cos((1.41*cos(0.314*t))/t)^2*cos(0.15*t)*sin(0.15*t) ...
         + 3.88e40*w3*cos((1.41*cos(0.314*t))/t)^2*sin(0.15*t)*sin(0.314*t) ...
         + 2.75e40*w2*w3*cos((1.41*cos(0.314*t))/t)^2*cos(0.15*t)*sin(0.15*t) ...
         + 3.88e40*w2*cos((1.41*cos(0.314*t))/t)*cos(0.15*t)^2*sin((1.41*cos(0.314*t))/t)*sin(0.314*t) ...
         + 2.75e40*w1*w2*cos((1.41*cos(0.314*t))/t)*sin((1.41*cos(0.314*t))/t)*sin(0.15*t) ...
         - 3.88e40*w1*cos((1.41*cos(0.314*t))/t)*cos(0.15*t)*sin((1.41*cos(0.314*t))/t)*sin(0.15*t)*sin(0.314*t) ...
         - 5.49e40*w1*w2*cos((1.41*cos(0.314*t))/t)*cos(0.15*t)^2*sin((1.41*cos(0.314*t))/t)*sin(0.15*t)))/(3.7e27*cos((1.41*cos(0.314*t))/t)^2 ...
         + 8.75e24*cos(0.15*t)^2 ...
         + 2.04e26);
 end

function f3 = f3(t,w1,w2,w3)

    % This is the function for omega1 which comes from craneOnShip.m sctipt
    
    f3 = -0.658*w1*w2;
end