function [q, error, flag] = fqa(VaN,VmN)
% a is 3x1, m is 3x1

% Xiaoping Yun,  May 7, 2008
% input a = 3-dim acceleration, m=3-dim local magnetic measurement

%Mref = [0.4943  0.0  0.8693];

epsilon = 0.10;   % accuracy control constant
alpha = 30*pi/180;   % offset angle

% x is 3x2, first col = magnetometer, second col = accelerometer.

a_bar = VaN;
m_bar = VmN;
% a_bar = VaN/norm(VaN);            % make sure that it is normalized
% m_b = VmN/norm(VmN);

sin_th = a_bar(1);
cos_th = sqrt(1-sin_th^2);

% singularity avoidance algorithm
if (cos_th <= epsilon)
    singular_flag = 1;
    q_offset =cos(alpha/2)*[1 0 0 0]' + sin(alpha/2)*[0 0 1 0]';
    
    a_bar_q = [0; a_bar];
    m_bar_q = [0; m_bar];
    a_q_offset = rotate_v_by_q(a_bar_q,q_offset);
    m_q_offset = rotate_v_by_q(m_bar_q,q_offset);
    
    a_bar = a_q_offset(2:4);
    m_bar = m_q_offset(2:4);

else
   % do not do anything other than setting the flag. 
   singular_flag = 0;
end 

%% elevation quaternion
sin_th = a_bar(1);%h(1);
%cos_th = sqrt(1-sin_th^2);
cos_th = sqrt(a_bar(2)^2 + a_bar(3)^2);  %J.C. 1/30/2009

% computing half-angle values
cos_half_th=sqrt((1+cos_th)/2);             
if (cos_th<=-1)      % this "if" is needed since sign(0) = 0.
    sin_half_th = 1;
else
    sin_half_th=sign(sin_th)*sqrt((1-cos_th)/2);
end

qe = cos_half_th*[1;0;0;0] + sin_half_th*[0;0;1;0];

%% Roll Quaternion
b = [a_bar(2) a_bar(3)];
c = b/norm(b);
sin_phi = -c(1);
cos_phi = -c(2);

cos_half_phi=sqrt((1+cos_phi)/2);
if (cos_phi<=-1)
    sin_half_phi = 1;
else
    sin_half_phi=sign(sin_phi)*sqrt((1-cos_phi)/2);
end

qr = cos_half_phi*[1;0;0;0] + sin_half_phi*[0;1;0;0];

%% Azimuth Quaternion
m_bar_q = [0; m_bar];
 
q_er = q_mult2(qe,qr);
q_er_inv =[q_er(1); -q_er(2); -q_er(3); -q_er(4)];
m_e = q_mult2(q_er,q_mult2(m_bar_q, q_er_inv));

M = [m_e(2),m_e(3)];
M = M/norm(M);
N = [1; 0];
tmp =  [ M(1) M(2);
        -M(2) M(1)]*N;
cos_psi = tmp(1);
sin_psi = tmp(2);

cos_half_psi=sqrt((1+cos_psi)/2);
if (cos_psi<=-1)                    %%%% IMPORTANT %%%  if it is written as cos_psi==-1, it does not work.
                                    %%%% cos_psi is potentially less than -1.   
    sin_half_psi = 1;
else
     sin_half_psi=sign(sin_psi)*sqrt((1-cos_psi)/2);
end

qa = cos_half_psi*[1;0;0;0]+ sin_half_psi*[0;0;0;1]; 

q_tmp1 = q_mult2(qe,qr);
q_tmp = q_mult2(qa,q_tmp1);  

if (singular_flag == 1)
    q = q_mult2(q_tmp, q_offset);
else
    q = q_tmp;
end

error = cos_th;
flag = singular_flag;

end