close all;
clear;
clc;

%% Define constants and initial conditions
%Constants
lambda1 = 1; % kg*m^2
lambda3 = 5; % kg*m^2
w3 = 2; % rad/s
L3 = lambda3*w3; % kg*m^2/s
M = 2; % kg
g = 9.81; % m/s^2
R = 0.5; % m

% Initial conditions
init_th = 0.1;
init_th_prime = 0;
init_phi = 3*pi/4;
Lz = L3*cos(init_th);
init_phi_prime = (((Lz-L3*cos(init_th)) /(lambda1*(sin(init_th)^2))));
init_psi = 0;
init_psi_prime = (L3 - (lambda3*(((Lz-L3*cos(init_th)) /(lambda1*(sin(init_th)^2))))*cos(init_th)))/lambda3;

%Define Top axis length (for visualization)
length = 2;


%% Solve System using ode45 
odefun = @(t, y) [y(2); ((lambda1* ((((Lz-L3*cos(y(1))) /(lambda1*(sin(y(1))^2))))^2) * sin(y(1)) * cos(y(1))) - (L3 * (((Lz-L3*cos(y(1))) /(lambda1*(sin(y(1))^2)))) *sin(y(1))) + (M*g*R*sin(y(1))))/lambda1; (((Lz-L3*cos(y(1))) /(lambda1*(sin(y(1))^2)))); (L3 - (lambda3*(((Lz-L3*cos(y(1))) /(lambda1*(sin(y(1))^2))))*cos(y(1))))/lambda3];

tspan = [0,100];

init_conds = [init_th, init_th_prime, init_phi, init_psi];

[t,y] = ode45(odefun, tspan,init_conds);

%% Uncomment to see plots of 
% Num_pts = 300;
% plot(t(1:Num_pts),y(1:Num_pts,1), t(1:Num_pts), y(1:Num_pts,3), t(1:Num_pts), y(1:Num_pts,4))
% legend("\theta", "\phi", "\psi","Location","northeastoutside")
% ylabel("Generalized Coordinate (rad)")
% xlabel("Time(s)")
% title('Generalized Coordinates vs. Time(s)')
% grid on

%% Visualization
th = y(:,1);
phi = y(:,3);
psi = y(:,4);

i = 1;
figure('Position', [400, 100, 1000, 800]); % Adjust the values as needed
while i <= max(size(t))

    x = length*cos(phi(1:i,1)).*sin(th(1:i,1));
    y = length*sin(phi(1:i,1)).*sin(th(1:i,1));
    z = length*cos(th(1:i,1));

    xi = length*cos(phi(i,1)).*sin(th(i,1));
    yi = length*sin(phi(i,1)).*sin(th(i,1));
    zi = length*cos(th(i,1));

    plot3(x,y,z,'LineWidth', 3, "Color","k")
    grid on
    hold on
    line([0, xi], [0, yi], [0, zi], 'Color', 'k', 'LineWidth', 2);
    xlabel("X")
    ylabel("Y")
    zlabel("Z")
    title("Dynamics of a Symmetric Top" + newline + "initial conditions: " + "  \theta_0 = " +num2str(init_th)+ " rad,  \theta_0' = " +num2str(init_th_prime)+ " rad/s,  \phi_0 = " +num2str(init_psi)+ " rad, \phi'_0 = " + num2str(init_phi_prime) + " rad,  \psi_0' = " +num2str(init_psi_prime)+ " rad/s , \psi_0 = " + num2str(init_psi) + " rad,  L_3 = " + num2str(L3) + " kg\cdotm^2/s,  L_z = " + num2str(Lz) + "  kg\cdotm^2/s")
  
    axis equal


    % Create a grid of points for the sphere's surface
    [x, y, z] = sphere(50); % Adjust the number of points for smoother or coarser surface
    
    % Define the level of opacity (transparency) for the sphere's surface
    opacity = 0.2; % Adjust the opacity level (0.0 to 1.0)
    
    % Create the sphere with transparency
    hold on
    surf(length*x, length*y, length*z, 'FaceAlpha', opacity, 'EdgeColor', 'none');


    % Calculate the x, y, and z coordinates of the circle points
    % Define the number of points to create a smooth circle
    Rot_mat = euler2mat(th(i), phi(i), psi(i));
    numPoints = 100;
    theta = linspace(0, 2 * pi, numPoints);
    xCircle =  cos(theta);
    yCircle =  sin(theta);
    zCircle = zeros(size(xCircle));
    CD = [xCircle; yCircle; zCircle];
    new_CD = (Rot_mat*CD)';


    % Create the initial circle at the center of the spinning axis
    c = fill3((new_CD(:,1)+R*xi), (new_CD(:,2)+R*yi), (new_CD(:,3)+R*zi), 'b'); % 'b' for blue color

    % Add a black dot at edge of the circlular plane to visualize psi'
    hold on
    plot3((new_CD(1,1)+R*xi), (new_CD(1,2)+R*yi), (new_CD(1,3)+R*zi), 'k.', 'MarkerSize', 20);
    pause(0.0001);

    % Add a red dot at the center of mass of rigid body
    hold on
    plot3(R*xi, R*yi, R*zi, 'r.', 'MarkerSize', 20);
    pause(0.0001);

    i = i+1;
    clf
    
end

