% 3D Animation of R_e Evolution with Scaled Sphere
clc; clear; close all;

load("smc_results.mat")

% Load time-series data (Replace with actual data from your controller)
% load('controller_output.mat'); % Assuming saved simulation results

% Extract relevant variables
time = SQ_all.Time; % Time vector
Re_values = Re_all.Data; % Assuming Re_all.Data is a 3x3xN array of rotation matrices over time

% Convert R_e to a sphere representation (rotation vector)
N = length(time);
sphere_points = zeros(N, 3);
theta_values = zeros(N, 1);

for k = 1:N
    Rk = reshape(Re_values(:,:,k), [3,3]); % Extract 3x3 matrix at time k
    theta = acos((trace(Rk) - 1) / 2); % Compute rotation angle
    theta_values(k) = theta; % Store theta over time

    if abs(theta) > 1e-5 % Avoid singularities
        rot_axis = (1 / (2*sin(theta))) * [Rk(3,2) - Rk(2,3); Rk(1,3) - Rk(3,1); Rk(2,1) - Rk(1,2)];
        sphere_points(k, :) = theta * rot_axis; % Scale by theta
    else
        sphere_points(k, :) = [0, 0, 0]; % Identity rotation at (0,0,0)
    end
end

% Compute the maximum theta value
theta_max = max(theta_values);

% Create figure
figure;
hold on;
grid on;
axis equal;
view(3);
xlabel('Rotation X'); ylabel('Rotation Y'); zlabel('Rotation Z');
title('Evolution of Rotation Error R_e in SO(3)');
colorbar;
set(gca, 'FontSize', 12);

% Plot scaled sphere
[xs, ys, zs] = sphere(30); % Generate unit sphere
xs = theta_max * xs; % Scale by max theta
ys = theta_max * ys;
zs = theta_max * zs;
surf(xs, ys, zs, 'FaceAlpha', 0.1, 'EdgeColor', 'none'); % Light transparent sphere

% Mark the identity rotation (target)
scatter3(0, 0, 0, 200, 'm', 'p', 'filled'); % Identity rotation at (0,0,0)

% Initialize trajectory plot
trajectory_plot = plot3(NaN, NaN, NaN, 'k', 'LineWidth', 2); % Path of R_e
current_point = scatter3(NaN, NaN, NaN, 100, 'r', 'filled'); % Moving point

% Animation loop
for k = 1:10:N
    % Update trajectory
    set(trajectory_plot, 'XData', sphere_points(1:k,1), ...
                         'YData', sphere_points(1:k,2), ...
                         'ZData', sphere_points(1:k,3));

    % Update current point
    set(current_point, 'XData', sphere_points(k,1), ...
                       'YData', sphere_points(k,2), ...
                       'ZData', sphere_points(k,3));

    % Pause for animation effect
    pause(0.05);
end