% Параметры.
OUTPUT_WIDTH = 9;
PHASE_WIDTH = 18;
Fd = 50e6;
Fc = 3.12345e6;
ACC_WIDTH = 32;
X0 = 128;
N = 32768;

iterations = OUTPUT_WIDTH;
phase_incr = int32(((2.0 ^ 32) * Fc) / Fd);

% Таблица арктангенсов.
atan_table = zeros(1, iterations, 'int32');
for i = 1:iterations
    atan_table(i) = (atan(2 ^ -(i - 1))  / pi) * (2 ^ (PHASE_WIDTH - 1));
end

% Заполнение массива фаз.
phase = zeros(1, N, 'int32');
phase_acc = int32(0);
for i = 1:N
    p = int64(phase_acc) + int64(phase_incr);
    if (p > intmax('int32'))
        p = int64(intmin('int32')) + (p - int64(intmax('int32')));
    end
    phase_acc = p;
    phase(i) = bitshift(phase_acc, -32 + PHASE_WIDTH);
end

% CORDIC вариант с делением.
CORDIC1_X = int32([]);
for p = phase
    x = zeros(1, iterations + 1, 'int32');
    y = zeros(1, iterations + 1, 'int32');
    z = zeros(1, iterations + 1, 'int32');
    quadrant = bitand(bitshift(p, -(PHASE_WIDTH - 2)), 3);
    if (quadrant == 0 || quadrant == 3)
        x(1) = X0;
        z(1) = p;
    end
    if (quadrant == 1)
        y(1) = X0;
        z(1) = bitand(p, (2 ^ (PHASE_WIDTH - 2)) - 1);
    end
    if (quadrant == 2)
        y(1) = -X0;
        z(1) = bitor(p, bitshift(3, (PHASE_WIDTH - 2)));
    end
    for i = 1:iterations
        if (z(i) > 0)
            x(i + 1) = x(i) - fix(double(y(i)) / (2.0 ^ (i - 1)));
            y(i + 1) = y(i) + fix(double(x(i)) / (2.0 ^ (i - 1)));
            z(i + 1) = z(i) - atan_table(i);
        else
            x(i + 1) = x(i) + fix(double(y(i)) / (2.0 ^ (i - 1)));
            y(i + 1) = y(i) - fix(double(x(i)) / (2.0 ^ (i - 1)));
            z(i + 1) = z(i) + atan_table(i);
        end
    end
    CORDIC1_X = cat(1, CORDIC1_X, x(iterations + 1));
    y(iterations + 1);
end

% CORDIC вариант со сдвигом.
CORDIC2_X = int32([]);
for p = phase
    x = zeros(1, iterations + 1, 'int32');
    y = zeros(1, iterations + 1, 'int32');
    z = zeros(1, iterations + 1, 'int32');
    quadrant = bitand(bitshift(p, -(PHASE_WIDTH - 2)), 3);
    if (quadrant == 0 || quadrant == 3)
        x(1) = X0;
        z(1) = p;
    end
    if (quadrant == 1)
        y(1) = X0;
        z(1) = bitand(p, (2 ^ (PHASE_WIDTH - 2)) - 1);
    end
    if (quadrant == 2)
        y(1) = -X0;
        z(1) = bitor(p, bitshift(3, (PHASE_WIDTH - 2)));
    end
    for i = 1:iterations
        if (z(i) > 0)
            x(i + 1) = x(i) - bitshift(y(i), -(i - 1));
            y(i + 1) = y(i) + bitshift(x(i), -(i - 1));
            z(i + 1) = z(i) - atan_table(i);
        else
            x(i + 1) = x(i) + bitshift(y(i), -(i - 1));
            y(i + 1) = y(i) - bitshift(x(i), -(i - 1));
            z(i + 1) = z(i) + atan_table(i);
        end
    end
    CORDIC2_X = cat(1, CORDIC2_X, x(iterations + 1));
    y(iterations + 1);
end

F = Fd * (-N/2:N/2-1) / N;

CORDIC1_X = (double(CORDIC1_X));
CORDIC1_F = fftshift(fft(CORDIC1_X));
CORDIC1_P = CORDIC1_F .* conj(CORDIC1_F) / (N * N);

CORDIC2_X = (double(CORDIC2_X));
CORDIC2_F = fftshift(fft(CORDIC2_X));
CORDIC2_P = CORDIC2_F .* conj(CORDIC2_F) / (N * N);

figure;
subplot(2,1,1);
plot(F, 10 * log10(CORDIC1_P), 'k');
title('Целочисленное деление')
grid on

subplot(2,1,2);
plot(F, 10 * log10(CORDIC2_P), 'b');
title('Логический сдвиг')
grid on

xlabel('Частота, Гц');
ylabel('Мощность, дБ');