%% ILDA Finger Kinematics Example Usage
% This script demonstrates how to use the ILDA forward and inverse 
% kinematics functions with practical examples.
%
% Author: GenAbyss
% Date: 2025.8.11

clear; clc; close all;

%% Initialize Finger Parameters
% Create data structure with geometric parameters
data = struct();

% Please refer to the following parameters for specific meanings:[url]
data.ax = 7.0;          
data.ay = 12.0;         
data.bx = (18.15 - 2.5) / 2; 
data.by = 14.14;        
data.p = 14.48;         

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

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

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

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

%% Example 1: Forward Kinematics
fprintf('=== Forward Kinematics Example ===\n');

% Define actuator displacements (mm)
d1 = 5.0;   
d2 = 3.0;   
d3 = 8.0;   

fprintf('Input actuator displacements:\n');
fprintf('  d1 = %.2f mm (left actuator)\n', d1);
fprintf('  d2 = %.2f mm (right actuator)\n', d2);
fprintf('  d3 = %.2f mm (PIP actuator)\n', d3);

% Calculate joint angles
[q1, q2, q3, data_fk] = ILDA_FK(d1, d2, d3, data);

fprintf('\nCalculated joint angles:\n');
fprintf('  q1 = %.2f° (MCP flexion)\n', rad2deg(q1));
fprintf('  q2 = %.2f° (MCP abduction)\n', rad2deg(q2));
fprintf('  q3 = %.2f° (DIP flexion)\n', rad2deg(q3));

%% Example 2: Inverse Kinematics
fprintf('\n=== Inverse Kinematics Example ===\n');

% Define desired joint angles (degrees)
q1_desired = 30.0;  
q2_desired = 5.0;   
q3_desired = 45.0;  

fprintf('Desired joint angles:\n');
fprintf('  q1 = %.2f° (MCP flexion)\n', q1_desired);
fprintf('  q2 = %.2f° (MCP abduction)\n', q2_desired);
fprintf('  q3 = %.2f° (DIP flexion)\n', q3_desired);

% Convert to radians
q1_rad = deg2rad(q1_desired);
q2_rad = deg2rad(q2_desired);
q3_rad = deg2rad(q3_desired);

% Calculate actuator displacements
[d1_calc, d2_calc, d3_calc, data_ik] = ILDA_IK(q1_rad, q2_rad, q3_rad, data);

fprintf('\nCalculated actuator displacements:\n');
fprintf('  d1 = %.2f mm (left actuator)\n', d1_calc);
fprintf('  d2 = %.2f mm (right actuator)\n', d2_calc);
fprintf('  d3 = %.2f mm (PIP actuator)\n', d3_calc);

%% Example 3: Verification (Round-trip test)
fprintf('\n=== Round-trip Verification ===\n');

% Use the calculated actuator displacements to verify forward kinematics
[q1_verify, q2_verify, q3_verify, ~] = ILDA_FK(d1_calc, d2_calc, d3_calc, data);

fprintf('Verification results:\n');
fprintf('  Original q1: %.2f° → Calculated: %.2f° (Error: %.4f°)\n', ...
        q1_desired, rad2deg(q1_verify), abs(q1_desired - rad2deg(q1_verify)));
fprintf('  Original q2: %.2f° → Calculated: %.2f° (Error: %.4f°)\n', ...
        q2_desired, rad2deg(q2_verify), abs(q2_desired - rad2deg(q2_verify)));
fprintf('  Original q3: %.2f° → Calculated: %.2f° (Error: %.4f°)\n', ...
        q3_desired, rad2deg(q3_verify), abs(q3_desired - rad2deg(q3_verify)));

%% Example 4: Simple Visualization
fprintf('\n=== Simple Visualization ===\n');

% Create a simple 3D plot of the finger in a specific configuration
figure('Name', 'ILDA Finger Visualization', 'Position', [100, 100, 800, 600]);

% Use the forward kinematics result from Example 1
subplot(1, 2, 1);
title('Forward Kinematics Result');
hold on; grid on; axis equal;
xlabel('X (mm)'); ylabel('Y (mm)'); zlabel('Z (mm)');

% Plot key points
plot3(data_fk.P_mcp(1), data_fk.P_mcp(2), data_fk.P_mcp(3), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
plot3(data_fk.P_pip(1), data_fk.P_pip(2), data_fk.P_pip(3), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
plot3(data_fk.P_dip(1), data_fk.P_dip(2), data_fk.P_dip(3), 'bo', 'MarkerSize', 10, 'MarkerFaceColor', 'b');

% Plot finger segments
plot3([0, data_fk.P_mcp(1)], [0, data_fk.P_mcp(2)], [0, data_fk.P_mcp(3)], 'k-', 'LineWidth', 3);
plot3([data_fk.P_mcp(1), data_fk.P_pip(1)], [data_fk.P_mcp(2), data_fk.P_pip(2)], [data_fk.P_mcp(3), data_fk.P_pip(3)], 'k-', 'LineWidth', 3);
plot3([data_fk.P_pip(1), data_fk.P_dip(1)], [data_fk.P_pip(2), data_fk.P_dip(2)], [data_fk.P_pip(3), data_fk.P_dip(3)], 'k-', 'LineWidth', 3);

% Add labels
text(0, 0, 0, 'Base', 'FontSize', 12);
text(data_fk.P_mcp(1), data_fk.P_mcp(2), data_fk.P_mcp(3), 'MCP', 'FontSize', 12);
text(data_fk.P_pip(1), data_fk.P_pip(2), data_fk.P_pip(3), 'PIP', 'FontSize', 12);
text(data_fk.P_dip(1), data_fk.P_dip(2), data_fk.P_dip(3), 'DIP', 'FontSize', 12);

view(45, 30);
axis([-20 20 -20 20 0 60]);

% Inverse kinematics result
subplot(1, 2, 2);
title('Inverse Kinematics Result');
hold on; grid on; axis equal;
xlabel('X (mm)'); ylabel('Y (mm)'); zlabel('Z (mm)');

% Plot key points
plot3(data_ik.P_mcp(1), data_ik.P_mcp(2), data_ik.P_mcp(3), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
plot3(data_ik.P_pip(1), data_ik.P_pip(2), data_ik.P_pip(3), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
plot3(data_ik.P_dip(1), data_ik.P_dip(2), data_ik.P_dip(3), 'bo', 'MarkerSize', 10, 'MarkerFaceColor', 'b');

% Plot finger segments
plot3([0, data_ik.P_mcp(1)], [0, data_ik.P_mcp(2)], [0, data_ik.P_mcp(3)], 'k-', 'LineWidth', 3);
plot3([data_ik.P_mcp(1), data_ik.P_pip(1)], [data_ik.P_mcp(2), data_ik.P_pip(2)], [data_ik.P_mcp(3), data_ik.P_pip(3)], 'k-', 'LineWidth', 3);
plot3([data_ik.P_pip(1), data_ik.P_dip(1)], [data_ik.P_pip(2), data_ik.P_dip(2)], [data_ik.P_pip(3), data_ik.P_dip(3)], 'k-', 'LineWidth', 3);

% Add labels
text(0, 0, 0, 'Base', 'FontSize', 12);
text(data_ik.P_mcp(1), data_ik.P_mcp(2), data_ik.P_mcp(3), 'MCP', 'FontSize', 12);
text(data_ik.P_pip(1), data_ik.P_pip(2), data_ik.P_pip(3), 'PIP', 'FontSize', 12);
text(data_ik.P_dip(1), data_ik.P_dip(2), data_ik.P_dip(3), 'DIP', 'FontSize', 12);

view(45, 30);
axis([-20 20 -20 20 0 60]);

fprintf('Visualization completed. Check the figure window.\n');

%% Summary
fprintf('\n=== Summary ===\n');
fprintf('This example demonstrates:\n');
fprintf('  1. Forward kinematics calculation\n');
fprintf('  2. Inverse kinematics calculation\n');
fprintf('  3. Round-trip verification\n');
fprintf('  4. Simple 3D visualization\n');
fprintf('\nFor more detailed visualizations and animations, run:\n');
fprintf('  - ILDAFK_fig.m (forward kinematics animation)\n');
fprintf('  - ILDAIK_fig.m (inverse kinematics animation)\n'); 