%% ILDA Inverse Kinematics Visualization and Animation
% This script demonstrates the inverse 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_IK = struct();

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

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

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

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

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

%% Animation Parameters
% Define joint angle range for animation
x = 0:3:45;               
nImages = length(x);      
im = cell(size(x));       

%% Generate Animation Frames
for j = 1:nImages
    % Calculate inverse kinematics for current joint angles
    % Fixed MCP angles: q1=40°, q2=0°, variable q3
    [d1, d2, d3, data_IK] = ILDA_IK(40/180*pi, 0/180*pi, x(j)/180*pi, data_IK);
    
    % Define visualization lines for finger structure
    O = [0; 0; 0];        
    
    % Base frame lines
    lines_IK{1} = [O, data_IK.A1];        
    lines_IK{2} = [O, data_IK.A2];        
    lines_IK{3} = [O, data_IK.P_mcp];     
    
    % MCP mechanism lines
    lines_IK{4} = [data_IK.P_mcp, data_IK.B1]; 
    lines_IK{5} = [data_IK.P_mcp, data_IK.B2]; 
    lines_IK{6} = [data_IK.B1, data_IK.B2];    
    
    % Actuator lines
    lines_IK{7} = [data_IK.A1, data_IK.C1];    
    lines_IK{8} = [data_IK.A2, data_IK.C2];    
    lines_IK{9} = [data_IK.C1, data_IK.B1];    
    lines_IK{10} = [data_IK.C2, data_IK.B2];   
    
    % PIP mechanism lines
    lines_IK{11} = [data_IK.P0, data_IK.P1];   
    lines_IK{12} = [data_IK.P1, data_IK.P2];   
    lines_IK{13} = [data_IK.P2, data_IK.P3];   
    lines_IK{14} = [data_IK.P3, data_IK.P0];   
    lines_IK{15} = [data_IK.P3, data_IK.P4];   
    lines_IK{16} = [data_IK.P4, data_IK.P5];   
    lines_IK{17} = [data_IK.P5, data_IK.P_pip]; 
    lines_IK{18} = [data_IK.P_pip, data_IK.P3]; 
    
    % DIP mechanism lines
    lines_IK{19} = [data_IK.P_pip, data_IK.P6]; 
    lines_IK{20} = [data_IK.P_mcp, data_IK.P6]; 
    lines_IK{21} = [data_IK.P_pip, data_IK.P_dip]; 
    lines_IK{22} = [data_IK.P7, data_IK.P_dip]; 
    lines_IK{23} = [data_IK.P7, data_IK.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_IK, 2)
            plot3(lines_IK{i}(1, :), lines_IK{i}(2, :), lines_IK{i}(3, :), ...
                  'LineWidth', 2);
        end
        
        % Add point labels
        text(data_IK.A1(1), data_IK.A1(2), data_IK.A1(3), 'A1', 'FontSize', 10);
        text(data_IK.A2(1), data_IK.A2(2), data_IK.A2(3), 'A2', 'FontSize', 10);
        text(data_IK.B1(1), data_IK.B1(2), data_IK.B1(3), 'B1', 'FontSize', 10);
        text(data_IK.B2(1), data_IK.B2(2), data_IK.B2(3), 'B2', 'FontSize', 10);
        text(data_IK.C1(1), data_IK.C1(2), data_IK.C1(3), 'C1', 'FontSize', 10);
        text(data_IK.C2(1), data_IK.C2(2), data_IK.C2(3), 'C2', 'FontSize', 10);
        text(data_IK.P1(1), data_IK.P1(2), data_IK.P1(3), 'P1', 'FontSize', 10);
        text(data_IK.P0(1), data_IK.P0(2), data_IK.P0(3), 'P0', 'FontSize', 10);
        text(data_IK.P2(1), data_IK.P2(2), data_IK.P2(3), 'P2', 'FontSize', 10);
        text(data_IK.P3(1), data_IK.P3(2), data_IK.P3(3), 'P3', 'FontSize', 10);
        text(data_IK.P4(1), data_IK.P4(2), data_IK.P4(3), 'P4', 'FontSize', 10);
        text(data_IK.P5(1), data_IK.P5(2), data_IK.P5(3), 'P5', 'FontSize', 10);
        text(data_IK.P6(1), data_IK.P6(2), data_IK.P6(3), 'P6', 'FontSize', 10);
        text(data_IK.P7(1), data_IK.P7(2), data_IK.P7(3), 'P7', 'FontSize', 10);
        text(data_IK.P_mcp(1), data_IK.P_mcp(2), data_IK.P_mcp(3), 'P_{mcp}', 'FontSize', 10);
        text(data_IK.P_pip(1), data_IK.P_pip(2), data_IK.P_pip(3), 'P_{pip}', 'FontSize', 10);
        text(data_IK.P_dip(1), data_IK.P_dip(2), data_IK.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 = "q3IK.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('Inverse kinematics animation saved as: ' + filename);