function [d1, d2, d3, data] = ILDA_IK(q1, q2, q3, data)
%ILDA_IK Inverse Kinematics for ILDA Dexterous Hand Finger
%
% This function calculates the actuator displacements (d1, d2, d3) from 
% joint angles (q1, q2, q3) for the ILDA finger model.
%
% Inputs:
%   q1, q2, q3 - Joint angles (radians)
%   data       - Structure containing finger geometric parameters
%
% Outputs:
%   d1, d2, d3 - Actuator displacements (mm)
%   data       - Updated structure with calculated positions
%
% Author: GenAbyss
% Date: 2025.8.11
% Reference: Integrated linkage-driven dexterous anthropomorphic robotic hand

%% Extract Geometric Parameters
% Please refer to the following parameters for specific meanings: https://mp.weixin.qq.com/s/CysLC0w8-a0Tx0XXKnmZPQ
ax = data.ax;          
ay = data.ay;          
bx = data.bx;          
by = data.by;          
p = data.p;           

l1 = data.l1;          
l2 = data.l2;          
l3 = data.l3;         
l4 = data.l4;          
cy = data.cy;         

r1 = data.r1;          
r2 = data.r2;          
r3 = data.r3;          
r4 = data.r4;          
r5 = data.r5;  

u1 = data.u1;          
u2 = data.u2;          
u3 = data.u3;          
u4 = data.u4;          

beta1 = data.beta1;    
beta2 = data.beta2;    
beta3 = data.beta3;    
beta4 = data.beta4;    
theta4 = data.theta4;  

%% MCP Joint Kinematics (Metacarpophalangeal)
% Calculate rotation matrices
R01 = [1 0 0; 0 cos(q1) -sin(q1); 0 sin(q1) cos(q1)]; 
R12 = [cos(q2) 0 sin(q2); 0 1 0; -sin(q2) 0 cos(q2)]; 
R02 = R12 * R01;       % Combined MCP rotation

% Define base frame points
A1 = [ax; ay; 0];      % Axis1 base point
A2 = [-ax; ay; 0];     % Axis2 base point
b1p = [bx; by; 0];     % B1 point at u-w Coordinate System
b2p = [-bx; by; 0];    % B2 point at u-w Coordinate System
P_mcp = [0; 0; p];     % MCP center point

% Calculate MCP mechanism points in global frame
B1 = P_mcp + R02 * b1p; 
B2 = P_mcp + R02 * b2p; 

% Calculate actuator displacements using geometric constraints
k1 = B1 - A1;          
k2 = B2 - A2;         

% Solve for actuator displacements using quadratic equations
t1 = sqrt(k1(3)^2 + l1^2 - sum(k1.*k1)); 
t2 = sqrt(k2(3)^2 + l2^2 - sum(k2.*k2)); 

d1 = k1(3) - t1;       % d1 actuator displacement
d2 = k2(3) - t2;       % d2 actuator displacement

% Calculate actuator end points
C1 = A1 + [0; 0; d1];  % d1 actuator end point
C2 = A2 + [0; 0; d2];  % d2 actuator end point

%% DIP Joint Kinematics (Distal Interphalangeal)
% Solve DIP mechanism using geometric constraints
E = 2*u2*u4*sin(q3);
F = 2*u2*u4*cos(q3) - 2*u2*u3;
G = u4^2 + u2^2 + u3^2 - u1^2 - 2*u3*u4*cos(q3);

% Calculate DIP mechanism angles
gamma2 = asin(G/sqrt(E^2 + F^2)) - atan(F/E);
if gamma2 > 0
    gamma2 = gamma2 - pi;
end

gamma1 = asin((-u2*sin(gamma2) + u4*sin(q3))/u1);
theta3 = gamma1 + beta2 - beta3 + beta4;

%% PIP Joint Kinematics (Proximal Interphalangeal)
% Solve PIP mechanism using geometric constraints
A = 2*r1*(r3*sin(theta3) - r4*sin(theta4));
B = 2*r1*(r3*cos(theta3) - r4*cos(theta4));
D = r2^2 - r1^2 - r3^2 - r4^2 + 2*r3*r4*cos(theta3 - theta4);

% Calculate PIP mechanism angles
theta1 = asin(D/sqrt(A^2 + B^2)) - atan(B/A) + pi;
alpha = pi - theta1 - beta1;

% Calculate PIP mechanism points
P23p = [0; l4*cos(alpha); -l4*sin(alpha)]; 
P23 = R02 * P23p;      
P3p = [0; by; 0];      
P3 = R02 * P3p + P_mcp; 
P0 = [0; cy; 0];       

% Calculate d3 actuator displacement
P03 = P3 - P0;         
P12 = P03 - P23;      
t3 = sqrt(l3^2 - P12(1)^2 - P12(2)^2);
d3 = P12(3) - t3;  % d3 actuator displacement   

% Calculate PIP mechanism points
P1 = [0; 0; d3];       
P2 = P3 - P23;         

% Calculate additional PIP mechanism points
P4p = P3p + [0; r1*cos(theta1); r1*sin(theta1)];
P_pipp = P3p + [0; r4*cos(theta4); r4*sin(theta4)];
P_pip = R02 * P_pipp + P_mcp;
P5p = [0; P_pipp(2)+r3*cos(pi-theta3); P_pipp(3)-r3*sin(pi-theta3)];
P4 = R02 * P4p + P_mcp;
P5 = R02 * P5p + P_mcp;
P6p = [0; r5*cos(beta2); r5*sin(beta2)];
P6 = R02 * P6p + P_mcp;

% Calculate DIP mechanism points
delta1 = pi - beta2 - beta4;
P7p = P6p + [0; u4*cos(q3-delta1); u4*sin(q3-delta1)];
P7 = R02 * P7p + P_mcp;
sigma = beta3 - (pi - theta3);
P_dipp = P_pipp + [0; u1*cos(sigma); u1*sin(sigma)];
P_dip = R02 * P_dipp + P_mcp;

%% Store Results
% Update data structure with calculated positions
data.P0 = P0;          
data.P1 = P1;         
data.P2 = P2;          
data.P3 = P3;          
data.P4 = P4;          
data.P5 = P5;          
data.P6 = P6;          
data.P7 = P7;         
data.P_mcp = P_mcp;    
data.P_pip = P_pip;    
data.P_dip = P_dip;    
data.A1 = A1;          
data.A2 = A2;          
data.B1 = B1;
data.B2 = B2;
data.C1 = C1;
data.C2 = C2;

end
