clear all

% Reference
Y = sqrt(4*pi).*getRSH(1, [0 -atan(0.05/1.5)*180/pi]); 
Y(2:3)= Y(2:3)./sqrt(3);
%Y = Y*randn(1,10000);
Cref=Y*Y';
Ia = Cref(2:4,1); 
azi_ref = atan2(Ia(1), Ia(3));
elev_ref = atan2(Ia(2), sqrt(Ia(3).^2 + Ia(1).^2));
u = unitSph2cart([azi_ref elev_ref]);

% Measured (ACN/SN3D)
[x, fs] = audioread('6dof_frontal_source_noEcho_100percent_absorbers_enabled.wav');
x_foa = [x(:,1), x(:,2:4)]; % ./sqrt(3)
Cx = x_foa'*x_foa;
Ia = Cx(2:4,1); 
azi = atan2(Ia(1), Ia(3));
elev = atan2(Ia(2), sqrt(Ia(3).^2 + Ia(1).^2));
v = unitSph2cart([azi elev]);

% Compensation rotation matrix
% (https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d)
a = cross(u,v); 
s = norm(a);
c = u*v';
a_skew = [0 -a(3) a(2); a(3) 0 -a(1); -a(2) a(1) 0;];
R_zyx_euler = eye(3) +  a_skew + a_skew*a_skew' * (1/(1+c));
[U,S,V] = svd(R_zyx_euler);
R_zyx_euler = U*V.';
 
% Apply compensation
R_ACN_o4 = getSHrotMtx(R_zyx_euler,4,'real');
x_rot = x_foa * R_ACN_o4(1:4,1:4);
Cx = x_rot'*x_rot;
Ia = Cx(2:4,1); 
azi_comp = atan2(Ia(1), Ia(3));
elev_comp = atan2(Ia(2), sqrt(Ia(3).^2 + Ia(1).^2));
v_comp = unitSph2cart([azi_comp elev_comp]);

% Check
u-v_comp
assert(sum(abs(ans))<0.001)

save('eigenmike_rotation_compensation', 'R_zyx_euler', 'R_ACN_o4');





