function gyroNumeric()

clc
close all

% Scale model inertia
J11 = .5564;
J12 = 2.9217;
J13 = 3.3592;

% Gimbal inertia
J42 = 100.32e-6;

% Disk inertia
J53 = 74.44e-6;  

% Wave forces
Mw1 = 0; 
Mw2 = 0;
Mw3 = 0; % These has to be estimated

psiDot = 1100; % Angular velocity of rotor (constant)

amplitude = 0.3; 
period = 1;
phi = @(t) amplitude*sin(2*pi*t/period);  % this equation for the gimbal is taken from Oscar's second paper
phiDot = @(t) amplitude*2*pi*cos(2*pi*t/period)/period;

% Our three equations:
dw1dt = @(t,w11,w12,w13) (Mw1 + J12*w12*w13 - J13*w12*w13 - J53*w12*w13*cos(phi(t))^2 - 2*J53*cos(phi(t))*phiDot(t)*psiDot)/(J11 + 2*J42 + J53*cos(phi(t))^2);                     

dw2dt = @(t,w11,w12,w13) (Mw2 - J11*w11*w13 + J13*w11*w13)/J12;

dw3dt = @(t,w11,w12,w13) (Mw3 + J11*w11*w12 - J12*w11*w12 + J53*w11*w12*sin(phi(t))^2 + 2*J53*w12*sin(phi(t))*psiDot)/(J13 + 2*J42 + J53*sin(phi(t))^2);

w11_0 = 0;
w12_0 = 0; % Starting values
w13_0 = 0;

timespan = [0,10];
n = 1000; % Intervals for Runga-Kutta

[tRu,w1,w2,w3] = RungaKutta(dw1dt, dw2dt, dw3dt, timespan , w11_0, w12_0, w13_0, n); % Calls the RK function

% Compute angles calculates the rotationmatrix to obtain the euler angles
angles = computeAngles(tRu, [w1' w2' w3']);

% Plot of angular roll velocity and roll angle
figure(1);
subplot(2,1,1);
plot(tRu,w1);
title('Angular roll velocity [rad/s]');
subplot(2,1,2);
plot(tRu,180/pi()*angles(:,1));
title('Roll angle [deg]');
end

% The RungaKutta method below is for coupled differential equations
% of first order with three equations and three variables

function [tv,w1,w2,w3] = RungaKutta(F, G, H, xLim, w11_0 , w12_0 , w13_0 , n)
 %{ 
    F is the first function
    G is the second function
    H is the third function
    xLim is the timespan
    w11_0, w12_0 and w13_0 are the starting values
    n is how many intervals we chose
 %} 

        h=(xLim(2)-xLim(1))/n; % Makes the stepsize
        
        w1(1)= w11_0;
        w2(1)= w12_0; % Starting values
        w3(1)= w13_0;
        
        w1(n+1)= 0;
        w2(n+1)= 0; % This just decides the length of the vector and fills it with 0s
        w3(n+1)= 0;

        tv=xLim(1):h:xLim(2); 
% Creates the timespan vector, starting from xLim(1), ending at
% xLim(2), and going in steps of h.
        
        for i=2:n+1
            % Runge Kutta
            
            k1 = h*F( tv(i-1), w1(i-1),w2(i-1), w3(i-1));
            m1 = h*G( tv(i-1), w1(i-1),w2(i-1), w3(i-1));
            n1 = h*H( tv(i-1), w1(i-1),w2(i-1), w3(i-1));

            
            k2 = h*F( tv(i-1)+h/2, w1(i-1)+k1/2, w2(i-1)+m1/2, w3(i-1)+n1/2);
            m2 = h*G( tv(i-1)+h/2, w1(i-1)+k1/2, w2(i-1)+m1/2, w3(i-1)+n1/2);
            n2 = h*H( tv(i-1)+h/2, w1(i-1)+k1/2, w2(i-1)+m1/2, w3(i-1)+n1/2);

            k3 = h*F( tv(i-1)+h/2, w1(i-1)+k2/2, w2(i-1)+m2/2, w3(i-1)+n2/2);
            m3 = h*G( tv(i-1)+h/2, w1(i-1)+k2/2, w2(i-1)+m2/2, w3(i-1)+n2/2);
            n3 = h*H( tv(i-1)+h/2, w1(i-1)+k2/2, w2(i-1)+m2/2, w3(i-1)+n2/2);

            k4 = h*F( tv(i-1)+h, w1(i-1)+k3, w2(i-1)+m3, w3(i-1)+n3);
            m4 = h*G( tv(i-1)+h, w1(i-1)+k3, w2(i-1)+m3, w3(i-1)+n3);
            n4 = h*H( tv(i-1)+h, w1(i-1)+k3, w2(i-1)+m3, w3(i-1)+n3);

            w1(i)= w1(i-1) + 1/6*(k1+2*k2+2*k3+k4);
            w2(i)= w2(i-1) + 1/6*(m1+2*m2+2*m3+m4);
            w3(i)= w3(i-1) + 1/6*(n1+2*n2+2*n3+n4);
        end 
end

function angles = computeAngles(t, w)
% computeAngles computes the rotation matrix and euler angles at every
% omega and returns a matrix containing the euler angles.

startangle = 0*pi()/180;        % Ship roll angle at t = 0

R = [1 0 0; ...
    0 cos(startangle) -sin(startangle);... % Rotation matrix at t = 0
    0 sin(startangle) cos(startangle)]; 

n = length(t);
dt = t(2) - t(1);
angles = zeros(n,3);            % Allocate matrix memory
for i = 1:n
    wi = [w(i,1) w(i,2) w(i,3)];
    R = reconstructR(R, dt, wi);% Calls function to compute rotation matrix
    angles(i,:) = rot2eul(R);   % Calls function to compute euler angles
end
end

function isRotMat(R)
% isRotMat checks if input is a rotation matrix
dotprod = R*R';
I = eye(3);
if not(norm(I - dotprod) < 1e-6)
    error('Matrix is not rotation matrix');
end
end

function ang = rot2eul(R)
% rot2eul converts the rotation matrix to euler angles

isRotMat(R);

singchk = sqrt(R(1,1)^2 + R(2,1)^2);

if singchk < 1e-6
    rotx = atan2(-R(2,3), R(2,2));
    roty = atan2(-R(3,1), singchk);
    rotz = 0;
else
    rotx = atan2(R(3,2), R(3,3));
    roty = atan2(-R(3,1), singchk);
    rotz = atan2(R(2,1), R(1,1));
end
ang = [rotx roty rotz];
end

function Rout = reconstructR(R, dt, w)
% reconstructR uses the reconstruction 
% formula to calculate the rotation matrix
Id = eye(3);
wskew = skew3(w);
wnorm = norm(w);
if wnorm == 0
    Rout = R;
    return
end
exptw = Id + wskew/wnorm*sin(dt*wnorm) + (wskew/wnorm)*(wskew/wnorm)*(1 - cos(dt*wnorm));
Rout = R*exptw;
end

function M = skew3(v)
% skew3 skews a vector containing 3 elements
M = zeros(3);
for i = 1:3
    inds = [1 2 3];
    inds(i == inds) = [];
    M(inds(1),inds(2)) = v(i)*(-1)^i;
    M(inds(2),inds(1)) = v(i)*(-1)^(i+1);
end
end