function tcp = scale_head_z(scp, tcp, factors, scaling_options, scp_kpis, tcp_kpis, selIdx_head, file_pointers, format_options)
% scales the head height position.
%
% :param scp:                      Source control points
% :param tcp:                      Current target control points
% :param factors:                  Scaling factors
% :param scaling_options:          Settings for the scaling
% :param selIdx_head:              Indicies for the cps associated with the head
% :param scp_kpis:                 Key measurements based on scp
% :param tcp_kpis:                 Key measurements based on tcp prior to this step
% :param file_pointers:            File pointers for I/O operations
% :param format_options:           Misc formating options
%
% :return tcp:                     Updated tcp positions
%
% Created by : ABR august@lightness.eu
% Date : 2024-06-13

% Move the head in the Z direction to match the new shoulder height
mean_of_shoulders_orig = mean([scp_kpis.C6 ; scp_kpis.C5]);
mean_of_shoulders_T3 = mean([tcp_kpis.C6 ; tcp_kpis.C5]);
head_vectors_to_centre_of_body = tcp(selIdx_head,:) - mean_of_shoulders_orig;

if scaling_options.scale_head_method == 1
    % Scaling against total length
    tcp(selIdx_head, 3) = mean_of_shoulders_T3(3) + factors(1) .* head_vectors_to_centre_of_body(:,3);
elseif scaling_options.scale_head_method == 2
    % Scaling proportional to head scaling
    tcp(selIdx_head, 3) = mean_of_shoulders_T3(3) + factors(4) .* head_vectors_to_centre_of_body(:,3);
elseif scaling_options.scale_head_method == 3
    % No scaling, only translation
    tcp(selIdx_head, 3) = mean_of_shoulders_T3(3) + head_vectors_to_centre_of_body(:,3);
else
    disp(['scaling_options.scale_head_method = ' num2str(scaling_options.scale_head_method) ' is not supported.'])
    disp(stop)
end



if format_options.plot_flag
  figure()
  scatter(scp(:,1), scp(:,2))
  hold on
  axis equal
  scatter(tcp(:,1), tcp(:,2))
  scatter( 0 , 0 , 'sr', 'filled')
  xlabel('X'); ylabel('Y');
  print([file_pointers.plot_dir 'XY_scale4'],format_options.pngprintOptions{:})

  figure()
  scatter(scp(:,1), scp(:,3))
  hold on
  axis equal
  scatter(tcp(:,1), tcp(:,3))
  xlabel('X'); ylabel('Z');
  scatter( 0 , 0 , 'sr', 'filled')
  print([file_pointers.plot_dir 'XZ_scale4'],format_options.pngprintOptions{:})

  figure()
  scatter(scp(:,2), scp(:,3))
  hold on
  axis equal
  scatter(tcp(:,2), tcp(:,3))
  scatter( 0 , 0 , 'sr', 'filled')
  xlabel('Y'); ylabel('Z');
  print([file_pointers.plot_dir 'YZ_scale4'],format_options.pngprintOptions{:})
end
