% Phase Estimator via Cordic Algorithm
% LuminousLogic.com


% This script demonstrates how the CORDIC algorithm may be used to estimate
% the phase of a complex number with only bit-shifts and adds.

% Note that let bit-shifts in this script are realized by multiplying by
% 2^-n since Matlab's bit-shift operator rounds to integer (undesired for
% this application)

% Initialize workspace
clear all; 
close all;


% Select number of CORDIC iterations
num_iter = 7;


% Generate CORDIC table based on # of iterations
%    This would be pre-computed and stored in ROM
for L=0:num_iter-1
    cordic_ph_tbl(L+1) = atan(2^-L) * 180/pi;
end


% Choose either a random or user-selected phase for simulation
if 0 % Random
    ph = rand*2*pi;
else % User-selected
    ph = 35.11*pi/180;
end


% Generate a complex # with that phase
I    = cos(ph);
Q    = sin(ph);
orig = I + 1i*Q; % save a copy since we'll modify I & Q below


% First rotate to Quadrant I or IV if not there already
if I<0
    if Q>0 % Quadrant II, move to Quadrant I
        Ip      = +Q;
        Qp      = -I;
        ang_sum = -90;
    else    % Quadrant III, move to Quadrant IV
        Ip      = -Q;
        Qp      = +I;
        ang_sum = +90;
    end
else
    Ip      = I;
    Qp      = Q;
    ang_sum = 0;
end
I = Ip;
Q = Qp;


% Now perform CORDIC iterations
for iter=1:num_iter
 
    L = iter-1;
    if Q>0
        Ip = I + (2^-L)*Q; % This multiply by 2^X would be implemented as a bit shift
        Qp = Q - (2^-L)*I; % This multiply by 2^X would be implemented as a bit shift
        ang_sum = ang_sum - cordic_ph_tbl(iter);
    else
        Ip = I - (2^-L)*Q; % This multiply by 2^X would be implemented as a bit shift
        Qp = Q + (2^-L)*I; % This multiply by 2^X would be implemented as a bit shift
        ang_sum = ang_sum + cordic_ph_tbl(iter);
    end
    I = Ip;
    Q = Qp;

    fprintf(1,'Iteration %d. Actual = %3.2f, so far = %3.2f\n', L, ph*180/pi, mod(-ang_sum,360))
end

fprintf(1,'Actual angle    = %3.2f degrees\n',ph*180/pi);
fprintf(1,'Estimated angle = %3.2f degrees\n', mod(-ang_sum,360));