function Result_struct = calculate_key_measurements_control_points(control_points, key_points)
% Calculates key measurements on the control points for comparison with the measured values
% and the same key measurements calculated using the nodes.
%
% :param nMat:  Matrix with control points.
%
% :param key_points: Struct with the nodes corresponding to the key locations on
%                    the body.
%
% :return Result_struct: Struct with the key points and the key measurements.
%                        This can be used for reporting or further processing.
%
% Created by : ABR august@lightness.eu
% Date : 2024-06-11
C1 = control_points(  key_points.C1(1), :);       % Top of head
C3 = control_points(  key_points.C3(1), :);       % Groin
C4 = control_points(  key_points.C4(1), :);       % Heel, left
C5 = control_points(  key_points.C5(1), :);       % Right hand shoulder
C6 = control_points(  key_points.C6(1), :);       % Left hand shoulder
C15 = control_points( key_points.C15(1), :);      % Knee joint center
C16 = control_points( key_points.C16(1), :);      % Hip joint center

C7  = control_points( key_points.C7(1), :);
C11 = control_points( key_points.C11(1), :);
C12 = control_points( key_points.C12(1), :);
C13 = control_points( key_points.C13(1), :);
C14 = control_points( key_points.C14(1), :);


% Shoulder-to-groin length
shoulder_to_groin = C5(3) - C3(3);

% Shoulder width
shoulder_width = C6(2) - C5(2);

% Total length
knee_to_foot_XZ = sqrt( (C15(1)-C4(1))^2 + (C15(3)-C4(3))^2 );
hip_to_knee_XZ = sqrt( (C16(1)-C15(1))^2 + (C16(3)-C15(3))^2 );

total_length = (C1(3) - C3(3)) - (C16(3) - C3(3)) + hip_to_knee_XZ + knee_to_foot_XZ;

% Head circumference
circumferenceMatrix = [C13 ; C12 ; C11 ; C14 ; C7];

head_circumference = 2.0 * sum ( sqrt( diff(circumferenceMatrix(:,1)).^2 + diff(circumferenceMatrix(:,2)).^2 ) );

% Calculate the angle between the z-axis and the foot-knee vector
dx_foot_to_knee = C15(1)-C4(1);
dz_foot_to_knee = C15(3)-C4(3);
angleZX_foot_to_knee = atand(dx_foot_to_knee / dz_foot_to_knee);

dx_hip_to_knee = C16(1)-C15(1);
dz_hip_to_knee = C16(3)-C15(3);
angleZX_hip_to_knee = atand(dx_hip_to_knee / dz_hip_to_knee);


Result_struct.C1 = C1;
Result_struct.C3 = C3;
Result_struct.C4 = C4;
Result_struct.C5 = C5;
Result_struct.C6 = C6;
Result_struct.C7 = C7;
Result_struct.C11 = C11;
Result_struct.C12 = C12;
Result_struct.C13 = C13;
Result_struct.C14 = C14;
Result_struct.C15 = C15;
Result_struct.C16 = C16;


Result_struct.shoulder_to_groin = shoulder_to_groin;
Result_struct.shoulder_width = shoulder_width;
Result_struct.total_length = total_length;
Result_struct.head_circumference = head_circumference;
Result_struct.angleZX_foot_to_knee = angleZX_foot_to_knee;
Result_struct.angleZX_hip_to_knee = angleZX_hip_to_knee;

