function [q1, q2, q3, data] = ILDA_FK(d1, d2, d3, data)
%ILDA_FK Forward Kinematics for ILDA Dexterous Hand Finger
%
% This function calculates the joint angles (q1, q2, q3) from actuator 
% displacements (d1, d2, d3) for the ILDA finger model.
%
% Inputs:
%   d1, d2, d3 - Actuator displacements (mm)
%   data       - Structure containing finger geometric parameters
%
% Outputs:
%   q1, q2, q3 - Joint angles (radians)
%   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 base frame points
A1 = [ax; ay; 0];      % Axis1 base point
A2 = [-ax; ay; 0];     % Axis2 base point
C1 = A1 + [0; 0; d1];  % d1 actuator end point
C2 = A2 + [0; 0; d2];  % d2 actuator end point
P_mcp = [0; 0; p];     % MCP center point

% Calculate MCP joint angles
h1 = by;               
N = [0; ay; (d1+d2)/2]; 
h2 = (l1+l2)/2;        
h3 = sqrt(ay^2 + (p-(d1+d2)/2)^2); 

% Calculate phi angles for MCP flexion
phi1 = acos((h1^2 + h3^2 - h2^2)/(2*h1*h3));
phi2 = asin(ay/h3);
q1 = phi1 + phi2 - pi/2;  % MCP flexion angle

% Calculate MCP abduction/adduction
q2 = asin((d2-d1)/(2*bx)); % MCP abduction angle

% Calculate MCP mechanism points
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;
b1p = [bx by 0]';
b2p = [-bx by 0]';

B1 = P_mcp+R02*b1p;
B2 = P_mcp+R02*b2p;
M = (B1 + B2)/2; 

%% PIP Joint Kinematics (Proximal Interphalangeal)
% Define PIP base points
P0 = [0; cy; 0];       
P1 = [0; 0; d3];       
P3 = M;                

% Calculate PIP mechanism parameters
P13 = P3 - P1;         
P03 = P3 - P0;         
h4 = norm(P13, 2);     
l5 = norm(P03, 2);     

% Calculate PIP joint angles based on actuator position
if d3 == 0
    phi3 = 0;
    phi4 = acos((l3^2 + h4^2 - l4^2)/(2*l3*h4));
    phi5 = pi/2 - phi4 - asin(P03(3)/l5);
else
    phi3 = acos((d3^2 + h4^2 - l5^2)/(2*d3*h4));
    phi4 = acos((l3^2 + h4^2 - l4^2)/(2*l3*h4));
    phi5 = pi - phi3 - phi4;
end

% Calculate PIP mechanism points
P2 = [0; l3*sin(phi5); l3*cos(phi5)+d3];
P3p = [0; by; 0];      
P23 = P3 - P2;         

% 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;       

% Calculate PIP mechanism angles
P2p = P3p - R02' * P23;
alpha = asin(P2p(3)/l4);
theta1 = pi - alpha - beta1;

% Solve PIP mechanism using geometric constraints
H = 2*r3*(r1*sin(theta1) - r4*sin(theta4));
J = 2*r3*(r1*cos(theta1) - r4*cos(theta4));
K = r2^2 - r4^2 - r1^2 - r3^2 + 2*r4*r1*cos(theta1 - theta4);
theta3 = asin(K/sqrt(H^2 + J^2)) - atan(J/H) + pi;

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

%% DIP Joint Kinematics (Distal Interphalangeal)
% Calculate DIP mechanism parameters
gamma1 = theta3 - beta2 + beta3 - beta4;
delta1 = pi - beta2 - beta4;

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

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

% Calculate DIP joint angle
q3 = atan((u1*sin(gamma1) + u2*sin(gamma2))/(u1*cos(gamma1) + u2*cos(gamma2) + u3));

% Calculate DIP mechanism points
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;          
data.N = N;            

end