function tcp = scale_legs(tcp, leg_scaling_factor, selIdx, zero_point, rotation_angle)
% scales the legs in the xz direction, along the leg axis.
%
% :param tcp:                      Current target control points
% :param leg_scaling_factor:       Scaling factor for the legs
% :param scaling_options:          Settings for the scaling
% :param zero_point:               Position relative which scaling is done.
% :param rotation_angle:           Angle of leg axis relative to the XZ plane
%
% :return tcp:                     Updated tcp positions
%
% Created by : ABR august@lightness.eu
% Date : 2024-06-13

vectors_from_zero_point = tcp(selIdx,:) - zero_point;

rigid_2D_rot_matrix = [sind(rotation_angle+90) -cosd(rotation_angle+90) ;
                       cosd(rotation_angle+90)  sind(rotation_angle+90)];

for bLoop = 1:size(vectors_from_zero_point,1)
  tcp_rot_temp =  rigid_2D_rot_matrix * vectors_from_zero_point(bLoop,[1 3])';
  tcp_rotated(bLoop,:) = [tcp_rot_temp(1) vectors_from_zero_point(bLoop,2) tcp_rot_temp(2)];
end

tcp_rotated(:,1) = tcp_rotated(:,1) * leg_scaling_factor;

for bLoop = 1:size(vectors_from_zero_point,1)
  tcp_rot_temp =  rigid_2D_rot_matrix' * tcp_rotated(bLoop,[1 3])';
  tcp_rotated2(bLoop,:) = [tcp_rot_temp(1) vectors_from_zero_point(bLoop,2) tcp_rot_temp(2)];
end
tcp_rotated2 = tcp_rotated2 + zero_point;

tcp(selIdx,:) = tcp_rotated2;

