% computeSetpoint
%   :PROPERTIES:
%   :header-args:matlab+: :tangle ../src/computeSetpoint.m
%   :header-args:matlab+: :comments org :mkdirp yes
%   :header-args:matlab+: :eval no :results none
%   :END:
%   <<sec:computeSetpoint>>

% This Matlab function is accessible [[file:../src/computeSetpoint.m][here]].


function setpoint = computeSetpoint(ty, ry, rz)
%%
setpoint = zeros(6, 1);

%% Ty
Ty = [1 0 0 0  ;
      0 1 0 ty ;
      0 0 1 0  ;
      0 0 0 1 ];

% Tyinv = [1 0 0  0  ;
%          0 1 0 -ty ;
%          0 0 1  0  ;
%          0 0 0  1 ];

%% Ry
Ry = [ cos(ry) 0 sin(ry) 0 ;
      0       1 0       0 ;
      -sin(ry) 0 cos(ry) 0 ;
      0        0 0       1 ];

% TMry = Ty*Ry*Tyinv;

%% Rz
Rz = [cos(rz) -sin(rz) 0 0 ;
      sin(rz)  cos(rz) 0 0 ;
      0        0       1 0 ;
      0        0       0 1 ];

% TMrz = Ty*TMry*Rz*TMry'*Tyinv;

%% All stages
% TM = TMrz*TMry*Ty;

TM = Ty*Ry*Rz;

[thetax, thetay, thetaz] = RM2angle(TM(1:3, 1:3));

setpoint(1:3) = TM(1:3, 4);
setpoint(4:6) = [thetax, thetay, thetaz];

%% Custom Functions
function [thetax, thetay, thetaz] = RM2angle(R)
    if abs(abs(R(3, 1)) - 1) > 1e-6 % R31 != 1 and R31 != -1
        thetay = -asin(R(3, 1));
        thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
        thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
    else
        thetaz = 0;
        if abs(R(3, 1)+1) < 1e-6 % R31 = -1
            thetay = pi/2;
            thetax = thetaz + atan2(R(1, 2), R(1, 3));
        else
            thetay = -pi/2;
            thetax = -thetaz + atan2(-R(1, 2), -R(1, 3));
        end
    end
end
end