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

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


function error_nass = convertErrorBasis(pos, setpoint, ty, ry, rz)
% convertErrorBasis -
%
% Syntax: convertErrorBasis(p_error, ty, ry, rz)
%
% Inputs:
%    - p_error - Position error of the sample w.r.t. the granite [m, rad]
%    - ty - Measured translation of the Ty stage [m]
%    - ry - Measured rotation of the Ry stage [rad]
%    - rz - Measured rotation of the Rz stage [rad]
%
% Outputs:
%    - P_nass - Position error of the sample w.r.t. the NASS base [m]
%    - R_nass - Rotation error of the sample w.r.t. the NASS base [rad]
%
% Example:
%

%% If line vector => column vector
if size(pos, 2) == 6
    pos = pos';
end

if size(setpoint, 2) == 6
    setpoint = setpoint';
end

%% Position of the sample in the frame fixed to the Granite
P_granite = [pos(1:3); 1]; % Position [m]
R_granite = [setpoint(1:3); 1]; % Reference [m]

%% Transformation matrices of the stages
% T-y
TMty = [1 0 0 0  ;
        0 1 0 ty ;
        0 0 1 0  ;
        0 0 0 1 ];

% R-y
TMry = [ cos(ry) 0 sin(ry) 0 ;
        0       1 0       0 ;
        -sin(ry) 0 cos(ry) 0 ;
        0        0 0       1 ];

% R-z
TMrz = [cos(rz) -sin(rz) 0 0 ;
        sin(rz)  cos(rz) 0 0 ;
        0        0       1 0 ;
        0        0       0 1 ];

%% Compute Point coordinates in the new reference fixed to the NASS base
% P_nass = TMrz*TMry*TMty*P_granite;
P_nass = TMrz\TMry\TMty\P_granite;
R_nass = TMrz\TMry\TMty\R_granite;

dx = R_nass(1)-P_nass(1);
dy = R_nass(2)-P_nass(2);
dz = R_nass(3)-P_nass(3);

%% Compute new basis vectors linked to the NASS base
% ux_nass = TMrz*TMry*TMty*[1; 0; 0; 0];
% ux_nass = ux_nass(1:3);
% uy_nass = TMrz*TMry*TMty*[0; 1; 0; 0];
% uy_nass = uy_nass(1:3);
% uz_nass = TMrz*TMry*TMty*[0; 0; 1; 0];
% uz_nass = uz_nass(1:3);

ux_nass = TMrz\TMry\TMty\[1; 0; 0; 0];
ux_nass = ux_nass(1:3);
uy_nass = TMrz\TMry\TMty\[0; 1; 0; 0];
uy_nass = uy_nass(1:3);
uz_nass = TMrz\TMry\TMty\[0; 0; 1; 0];
uz_nass = uz_nass(1:3);

%% Rotations error w.r.t. granite Frame
% Rotations error w.r.t. granite Frame
rx_nass = pos(4);
ry_nass = pos(5);
rz_nass = pos(6);

% Rotation matrices of the Sample w.r.t. the Granite
Mrx_error = [1 0              0 ;
            0 cos(-rx_nass) -sin(-rx_nass) ;
            0 sin(-rx_nass)  cos(-rx_nass)];

Mry_error = [ cos(-ry_nass) 0 sin(-ry_nass) ;
              0             1 0 ;
            -sin(-ry_nass) 0 cos(-ry_nass)];

Mrz_error = [cos(-rz_nass) -sin(-rz_nass) 0 ;
            sin(-rz_nass)  cos(-rz_nass) 0 ;
            0              0             1];

% Rotation matrix of the Sample w.r.t. the Granite
Mr_error = Mrz_error*Mry_error*Mrx_error;

%% Use matrix to solve
R = Mr_error/[ux_nass, uy_nass, uz_nass]; % Rotation matrix from NASS base to Sample

[thetax, thetay, thetaz] = RM2angle(R);

error_nass = [dx; dy; dz; 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));
        % thetaybis = pi-thetay;
        thetax = atan2(R(3, 2)/cos(thetay), R(3, 3)/cos(thetay));
        % thetaxbis = atan2(R(3, 2)/cos(thetaybis), R(3, 3)/cos(thetaybis));
        thetaz = atan2(R(2, 1)/cos(thetay), R(1, 1)/cos(thetay));
        % thetazbis = atan2(R(2, 1)/cos(thetaybis), R(1, 1)/cos(thetaybis));
    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