%% Тест ФАПЧ

clear all;
close all;

%% Исходные установки

%bit_stream = (rand(1, 20) > 0.5);
bit_stream = [0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1];
N = length(bit_stream);

% Частота семплирования 288 кГц (8 выборок на период)
fs = 288000;
% Несущая частота относительно частоты выборок 36 кГц
f = 0.125;
% Частота доплера
fdop = 0;
% время выборки
Ts = 1/fs;
fprintf('Время выборки Ts %d\n', Ts);
% Амплитуда
A = 1.0;
% Символьная частота Гц
fsim = 3600;
% Количество выборок на период несущей
ns = 1/f;
% Количество выборок необх. для одного символа
% при символьной скорости fsim
tsymbol = fs*f*ns/fsim;
fprintf('Кол. пер. несущ. в одном симв. tsymbol %d\n', fs*f/fsim);
% несущая частота Гц
fref = f*fs;
fprintf('Несущая %d\n', fref);
ttotal = N*tsymbol;

%% Входной сигнал

t = 0;
for i = 1:length(bit_stream)
% phase = pi * bit_stream(i);
phase = 0;
for j = 1:tsymbol
% входной сигнал
BPSK_signal(t+1) = cos(2*pi*(f+fdop)*t + phase);
t = t + 1;
end
end

%% Расчет коэффициентов петлевого фильтра

% натуральная частота Гц
fn = 500;
% коэффициент усиления NCO (по факту минимальный шаг соотв. разр. сетке)
Knco= 1/4096;
% коэфф. усил. фазового детектора 1/cycles
KP = 2;
% демпинг фактор
zeta = 1.0;
% круговая натуральная частота rad/s fn = fn/(2*pi)
wn = 2*pi*fn;
% пропорциональный коэффициент петлевого фильтра
KL= 2*zeta*wn*Ts/(KP*Knco);
% интегральный коэффициент петлевого фильтра
KI= wn^2*Ts^2/(KP*Knco);
% распечатка
fprintf('Пропорц. коэфф KL %d\n', KL);
fprintf('Интегр. коэфф KI %d\n', KI);

% Расчет коэффициентов передаточной функции с замкнутого контура u / ref_phase
% CL(z) = (b0 + b1z^-1)/(a2Z^-2 + a1z^-1 + 1)
b0= KP*KL*Knco;
b1= KP*Knco*(KI - KL);
a1= KP*KL*Knco - 2;
a2= 1 + KP*Knco*(KI - KL);
% коэффициенты петлевого фильтра (numerator denominator)
b = [b0 b1];
a = [1 a1 a2];
fprintf('b %d\n', ;
fprintf('a %d\n', a);

%% Собственно ФАПЧ

% начальная частота NCO Гц
fnco = fref;
fprintf('fnco Гц %d\n', fnco);
% индексы времени модели
n = 0:ttotal;
% циклы начальной фазы опорного сигнала
init_phase = 0.7;
% циклы фазы опорного сигнала
ref_phase = fref*n*Ts + init_phase;
% циклы фазы опорн. сигн. по mod 1
ref_phase = mod(ref_phase,1);
fprintf('ref_phase %d\n', ref_phase);

u(1) = 0;
ur(1) = 0;
int(1)= 0;
% начальная фазовая ошибка
phase_error(1) = -init_phase;
% начальная значение на входе NCO
vtune(1) = -init_phase*KL;
fprintf('vtune(1) %d\n', vtune(1));

%per = 0.05;

for step = 2:ttotal

% NCO

x = fnco*Ts + u(step-1) + vtune(step-1)*Knco; % циклы NCO фазы
% x = fnco*Ts + u(step-1); % циклы NCO фазы
u(step) = mod(x,1); % циклы NCO фазы по mod 1
s = cos(2*pi*u(step-1)); % NCO sin выход
y(step)= round(2^15*s)/2^15; % квантованный выход синуса NCO

% Fref входной сигнал

% xr = fref*Ts + ur(step-1); % циклы fref
% ur(step) = mod(xr,1); % циклы fref фазы по mod 1
% sr = sin(2*pi*ur(n-1)); % sin выход fref
sr = BPSK_signal(step-1); % sin выход fref
yr(step)= round(2^15*sr)/2^15; % квантованный выход fref

% выход умножителя опорного и NCO
Detect(step) = yr(step)*y(step);

% фазовый детектор

per= ref_phase(step-1) - u(step-1); % ошибка фазы
per= 2*(mod(per+1/2,1) - 1/2); % обертка, если пересечение фаз +/- 1/2 цикла
phase_error(step) = per;

% Петлевой фильтр
int(step) = KI*per + int(step-1); % интегратор
vtune(step) = int(step) + KL*per; % выход петлевого фильтра

end

%% графика

% фазовая ошибка с фазового детектора
figure
plot(phase_error, 'b-', 'LineWidth', 2),grid
axis([0 3 -1 1])
xlabel('t (ms)'),ylabel('phase_error'),title('фазовая ошибка с фаз. детектора')

figure;
plot(Detect, 'r-', 'LineWidth', 2);
hold on;
grid on;
title('Выход умножителя входного и NCO сигналов')

% выход VCO
figure
plot(vtune, 'r-', 'LineWidth', 2),grid
%axis([0 30 -1 5])
title('выходной сигнал петлевого фильтра')

% sin NCO и fref
figure;
plot(yr, 'b-', 'LineWidth', 2);
hold on;
grid on;
plot(y, 'r-', 'LineWidth', 2);
hold on
hold off;
title('Входной и опорный сигналы')

% Частотная характеристика замкнутого контура
figure
u = 0:.1:.9;
f= 10* 10 .^u; % log-scale frequencies
f = [f 10*f 100*f 1000*f];
z = exp(j*2*pi*f/fs); % complex frequency z
CL= (b0 + b1*z.^-1)./(1 + a1*z.^-1 + a2*z.^-2); % closed-loop response
CL_dB= 20*log10(abs(CL));
semilogx(f,CL_dB),grid
xlabel('Hz'),ylabel('CL(z) dB'),title('Частотная характеристика замкнутого контура')