%% ILDA Forward Kinematics Visualization and Animation
% This script demonstrates the forward kinematics of the ILDA finger model
% by creating 3D visualizations and generating animated GIF files.
%
% Author: GenAbyss
% Date: 2025.8.11
% Reference: Integrated linkage-driven dexterous anthropomorphic robotic hand

close all;
clear;

%% Initialize Data Structure
data_FK = struct();

%% Geometric Parameters
% Please refer to the following parameters for specific meanings: https://mp.weixin.qq.com/s/CysLC0w8-a0Tx0XXKnmZPQ
data_FK.ax = 7.0;          
data_FK.ay = 12.0;         
data_FK.bx = (18.15 - 2.5) / 2; 
data_FK.by = 14.14;        
data_FK.p = 14.48;         

data_FK.l1 = 24.19;        
data_FK.l2 = 24.19;        
data_FK.l3 = 13.75;        
data_FK.l4 = 13.07;        
data_FK.cy = 0.0;          

data_FK.r1 = 10.3;         
data_FK.r2 = 26.10;        
data_FK.r3 = 5.15;         
data_FK.r4 = 30.59;        
data_FK.r5 = 40.59;        

data_FK.u1 = 26.1;         
data_FK.u2 = 6.08;         
data_FK.u3 = 6.0;          
data_FK.u4 = 27.0;                

data_FK.beta1 = deg2rad(52.61);  
data_FK.beta2 = deg2rad(47.84);  
data_FK.beta3 = deg2rad(60.95);  
data_FK.beta4 = deg2rad(90);     
data_FK.theta4 = deg2rad(56.2);     

%% Animation Parameters
% Define actuator displacement range for animation
x = 0:0.5:10;             
nImages = length(x);      
im = cell(size(x));       

%% Generate Animation Frames
for j = 1:nImages
    % Calculate forward kinematics for current actuator displacement
    [q1, q2, q3, data_FK] = ILDA_FK(0, 0, x(j), data_FK);
    
    % Define visualization lines for finger structure
    O = [0; 0; 0];        
    
    % Base frame lines
    lines_FK{1} = [O, data_FK.A1];        
    lines_FK{2} = [O, data_FK.A2];        
    lines_FK{3} = [O, data_FK.P_mcp];     
    
    % MCP mechanism lines
    lines_FK{4} = [data_FK.P_mcp, data_FK.B1]; 
    lines_FK{5} = [data_FK.P_mcp, data_FK.B2]; 
    lines_FK{6} = [data_FK.B1, data_FK.B2];    
    
    % Actuator lines
    lines_FK{7} = [data_FK.A1, data_FK.C1];    
    lines_FK{8} = [data_FK.A2, data_FK.C2];    
    lines_FK{9} = [data_FK.C1, data_FK.B1];    
    lines_FK{10} = [data_FK.C2, data_FK.B2];   
    
    % PIP mechanism lines
    lines_FK{11} = [data_FK.P0, data_FK.P1];   
    lines_FK{12} = [data_FK.P1, data_FK.P2];   
    lines_FK{13} = [data_FK.P2, data_FK.P3];   
    lines_FK{14} = [data_FK.P3, data_FK.P0];   
    lines_FK{15} = [data_FK.P3, data_FK.P4];   
    lines_FK{16} = [data_FK.P4, data_FK.P5];   
    lines_FK{17} = [data_FK.P5, data_FK.P_pip]; 
    lines_FK{18} = [data_FK.P_pip, data_FK.P3]; 
    
    % DIP mechanism lines
    lines_FK{19} = [data_FK.P_pip, data_FK.P6]; 
    lines_FK{20} = [data_FK.P_mcp, data_FK.P6]; 
    lines_FK{21} = [data_FK.P_pip, data_FK.P_dip]; 
    lines_FK{22} = [data_FK.P7, data_FK.P_dip]; 
    lines_FK{23} = [data_FK.P7, data_FK.P6];    
    
    % Create figure for visualization
    fig = figure('Visible', 'off');
    set(gcf, 'unit', 'normalized', 'position', [0.2, 0.0, 0.70, 0.48]);
    
    % Create subplots for different viewing angles
    for k = 1:2
        subplot(1, 2, k);
        
        % Plot origin point
        plot3(O(1), O(2), O(3), 'o', 'MarkerSize', 8, 'MarkerFaceColor', 'k');
        
        % Set axis limits and properties
        axis([-50 50 -50 50 -10 90]);
        grid on;
        view(90*(k-1), 0);  
        xlabel('X (mm)');
        ylabel('Y (mm)');
        zlabel('Z (mm)');
        hold on;
        
        % Plot finger structure lines
        for i = 1:size(lines_FK, 2)
            plot3(lines_FK{i}(1, :), lines_FK{i}(2, :), lines_FK{i}(3, :), ...
                  'LineWidth', 2);
        end
        
        % Add point labels
        text(data_FK.A1(1), data_FK.A1(2), data_FK.A1(3), 'A1', 'FontSize', 10);
        text(data_FK.A2(1), data_FK.A2(2), data_FK.A2(3), 'A2', 'FontSize', 10);
        text(data_FK.B1(1), data_FK.B1(2), data_FK.B1(3), 'B1', 'FontSize', 10);
        text(data_FK.B2(1), data_FK.B2(2), data_FK.B2(3), 'B2', 'FontSize', 10);
        text(data_FK.C1(1), data_FK.C1(2), data_FK.C1(3), 'C1', 'FontSize', 10);
        text(data_FK.C2(1), data_FK.C2(2), data_FK.C2(3), 'C2', 'FontSize', 10);
        text(data_FK.P1(1), data_FK.P1(2), data_FK.P1(3), 'P1', 'FontSize', 10);
        text(data_FK.P0(1), data_FK.P0(2), data_FK.P0(3), 'P0', 'FontSize', 10);
        text(data_FK.P2(1), data_FK.P2(2), data_FK.P2(3), 'P2', 'FontSize', 10);
        text(data_FK.P3(1), data_FK.P3(2), data_FK.P3(3), 'P3', 'FontSize', 10);
        text(data_FK.P4(1), data_FK.P4(2), data_FK.P4(3), 'P4', 'FontSize', 10);
        text(data_FK.P5(1), data_FK.P5(2), data_FK.P5(3), 'P5', 'FontSize', 10);
        text(data_FK.P6(1), data_FK.P6(2), data_FK.P6(3), 'P6', 'FontSize', 10);
        text(data_FK.P7(1), data_FK.P7(2), data_FK.P7(3), 'P7', 'FontSize', 10);
        text(data_FK.P_mcp(1), data_FK.P_mcp(2), data_FK.P_mcp(3), 'P_{mcp}', 'FontSize', 10);
        text(data_FK.P_pip(1), data_FK.P_pip(2), data_FK.P_pip(3), 'P_{pip}', 'FontSize', 10);
        text(data_FK.P_dip(1), data_FK.P_dip(2), data_FK.P_dip(3), 'P_{dip}', 'FontSize', 10);
    end
    
    % Capture frame for animation
    frame = getframe(fig);
    im{j} = frame2im(frame);
    disp("Frame " + num2str(j) + " completed");
    close(fig);
end

%% Generate GIF Animation
filename = "d3FK.gif";  

% Create GIF file
for idx = 1:nImages
    [A, map] = rgb2ind(im{idx}, 256);
    if idx == 1
        imwrite(A, map, filename, 'gif', 'LoopCount', Inf, 'DelayTime', 0.1);
    else
        imwrite(A, map, filename, 'gif', 'WriteMode', 'append', 'DelayTime', 0.1);
    end
end

disp('Forward kinematics animation saved as: ' + filename);