atan2_fixpt(im,re) %#eml

function a = atan2_fixpt(im,re) %#eml

Trad = numerictype(1,16,13);
Tcar = numerictype(1,16,14);
Frad = fimath('SumWordLength', 16, 'SumFractionLength', 13, 'SumMode', 'Specify');
Fcar = fimath('SumWordLength', 16, 'SumFractionLength', 14, 'SumMode', 'Specify');

im = fi(im,'FiMath', Fcar);
re = fi(re,'FiMath', Fcar);
%im.FiMath = Fcar;
%re.FiMath = Fcar;

assert(isequal(numerictype(im), Tcar));
assert(isequal(numerictype(re), Tcar));

pidiv2 = fi(pi/2,'NumericType', Trad, 'FiMath', Frad);
pidiv1 = fi(pi,'NumericType', Trad, 'FiMath', Frad);

% Pre-allocate output
a = fi(zeros(size(im)),'NumericType', Trad, 'FiMath', Frad);

for idx = 1:length(a)

    %% Four quadrants
    if abs(re(idx)) > abs(im(idx)), % abs( mod (angle, pi) < pi/4
        a(idx) = cordic_atan_fixpt(abs(im(idx)), abs(re(idx)));
    else
        a(idx) = pidiv2 - cordic_atan_fixpt(abs(re(idx)), abs(im(idx)));
    end

    if re(idx) < 0, % second and third quadrant
        if im(idx) < 0,
            a(idx) = -pidiv1 + a(idx);
        else
            a(idx) = pidiv1 - a(idx);
        end
    else
        if im(idx) < 0,
            a(idx) = -a(idx);
        end
    end
end

%% Fixed-Point datatypes
%
% The input should accomodate the unit circle, making necessary a datatyp
% of 16 bits with a 14 bit fraction length. The range of this fixed-point
% data type is [-2 2]. This is usefull, because the gain of the CORDIC is
% max 1.1644. So overflow are not possible in the rotation of the input
% vector.
%
% With regards to the output: the temporary result of the cordic algorithm 
% is always between +pi/4 and -pi/4 which is within the range of 16 bit
% fixed-point number with a 15 bit fraction length. However, the output of
% the algorithm is always between -pi and pi, so a fraction length of 13
% bits makes more sense, as more precision is lost in the output.

% Now with regards to the number of rotations/iterations: the trick is to
% find trade-off between fixed-point precision and algorithmic precision.
% The algorithmic precision is approximately atan(2.^-N). The fixed-point
% precision is 2^-13. The trade-off is around N = 12.

      

function a = cordic_atan_fixpt(im,re)

N = 12; %Number of iterations

Trad = numerictype(1,16,13);
Tcar = numerictype(1,16,14);
Frad = fimath('SumWordLength', 16, 'SumFractionLength', 13, 'SumMode', 'Specify');
Fcar = fimath('SumWordLength', 16, 'SumFractionLength', 14, 'SumMode', 'Specify');

% Knowing that the angles we are looking for is between 0 and 45 degrees we
% can step the first step of the classical CORDIC algorithm (i = 1)

angles = fi(atan(2.^-(1:N)),'NumericType', Trad, 'FiMath', Frad);
reg = fi(0, 'NumericType',Tcar, 'FiMath', Fcar); % Temporary register

a = fi(0,'NumericType', Trad, 'FiMath', Frad);

for i = 1:N,
    
    if bitget(im,16), % negative angle
        a = a - angles(i);
        
        reg = re - bitshift(im,-i);
        im = bitshift(re,-i) + im;
        re = reg;
        
    else
        a = a + angles(i);
        
        reg = re + bitshift(im,-i);
        im = -bitshift(re,-i) + im;
        re = reg;
        
    end
    
end