%% инициализация

clc;
clear all;

f_begin = 47000; % начальная частота 47 кГц
f_end = 53000; % конечная частота 53 кГц
freq_noice = 50000;
noise_amp = 1.0; % амплитуда шума
freq_noice_amp = 0.5; % амплитуда шума на частоте freq_noice
f_dt = 26430; % частота дискретизации Гц (26434, 19734)
tch_begin = 0; % начальное время моделирования
tch1 = 0; % начальное время модели
tch2 = 9.72e-3; % конечное время модели (9.72, 13.0)
tch_end = 3*tch2; % конечное время моделирования
tch_dt = 1/f_dt; % время одного дискрета
%Nt = tch2*f_dt;

%% моделирование

% время моделирования от tch_begin до tch_end шаг tch_dt
%tch = linspace(tch_begin,tch_end,768);
% время существования моделей
tch_lf = tch1:tch_dt:(tch2-tch_dt);
tch_all = tch_begin:tch_dt:(tch_end-tch_dt);
% количество выборок в модели
tm = length(tch_lf);
tm_all = length(tch_all);
fprintf('Количество дискретов за время существ. моделей tch2 мсек %d\n', tm);

% Формирование ЛЧМ сигнала на участке 39 мсек
y_t1 = zeros(1,tm);
y_t2 = zeros(1,tm);
y_lf_t = chirp(tch_lf,f_begin,tch2,f_end, 'li', pi);
% шум
%noise = sin(2*pi*freq_noice*tch_lf) + noise_amp*randn(size(y_lf_t));
%y_lf = y_lf_t + noise;
% сигнал + шум
%y = cat(2,y_t1,y_lf,y_t2);
%y_t = cat(2,y_t1,y_lf,y_t2);
y_t = cat(2,y_t1,y_lf_t,y_t2);
% количество элементов матрицы y
%tst = length(y);
% fprintf('Количество дискретов за время моделирования 39 мсек %d\n', tst);
%noise = freq_noice_amp*sin(2*pi*freq_noice*tch_all) + noise_amp*randn(size(y_t));
noise = noise_amp*randn(size(y_t));
% шум по всей дистанции
%noise = freq_noice_amp*sin(2*pi*freq_noice*tch_all) + noise_amp*randn(1, tm_all);
% сигнал + шум
y = y_t + noise;
% длина матрицы сигнала
tst = length(y);

% косинусная модель
x_cos = chirp(tch_lf,f_begin,tch2,f_end);
% синусная модель
x_sin = imag(hilbert(x_cos));
% сигнал в противофазе
%x_sin = x_cos.*x_cos;


% модель cos в логике
x_log_cos = dec2bin( reshape( x_cos > 0, [], 1 ));
% модель sin в логике
x_log_sin = dec2bin( reshape( x_sin > 0, [], 1 ));
% сигнал в логике
y_log = dec2bin( reshape( y > 0, [], 1 ));
fprintf('Модель cos %s', x_log_cos);
fprintf('\n');
fprintf('Модель sin %s', x_log_sin);
fprintf('\n');
fprintf('Сигнал+шум %s', y_log);
fprintf('\n');
fprintf('\n');

% x_log_col = zeros(length(x_log_cos),2);
% for i = 1:length(x_log_cos)
% x_log_col(1, = x_log_cos(i,1);
% x_log_col(2, = x_log_sin(i,1);
% fprintf(' %s', x_log_col);
% fprintf('\n');
% end

% Following is a sample MIF:
% DEPTH = 32; % Memory depth and width are required %
% % DEPTH is the number of addresses %
% WIDTH = 14; % WIDTH is the number of bits of data per word %
% % DEPTH and WIDTH should be entered as decimal numbers %
% ADDRESS_RADIX = HEX; % Address and value radixes are required %
% DATA_RADIX = HEX; % Enter BIN, DEC, HEX, OCT, or UNS; unless %
% % otherwise specified, radixes = HEX %
% -- Specify values for addresses, which can be single address or range
% CONTENT
% BEGIN
% [0..F]: 3FFF; % Range--Every address from 0 to F = 3FFF %
% 6 : F; % Single address--Address 6 = F %
% 8 : F E 5; % Range starting from specific address %
% -- % Addr[8] = F, Addr[9] = E, Addr[A] = 5 %
% END

% запись в .mif
fid = fopen('modcossin2.mif','W');
count = 0;

fprintf(fid,'WIDTH = 2;\n');
fprintf(fid,'DEPTH = %d;\n', length(x_log_cos));
fprintf(fid,'ADDRESS_RADIX = UNS;\n');
fprintf(fid,'DATA_RADIX = BIN;\n');
fprintf(fid,'CONTENT BEGIN\n\n');
% for i=1:256
% fprintf(fid,'%d : ' ,count);
% count = count + 1;
% fprintf(fid,'%s%s ;\n', x_log_cos(i,,x_log_sin(i,);
% end
fprintf(fid,'END;\n');
fclose(fid);

%%
% бинарный формат моделей и сигнала
x_bit_cos = reshape( x_cos > 0, [], 1 );
x_bit_sin = reshape( x_sin > 0, [], 1 );
y_bit = reshape( y > 0, [], 1 );

% распечатка HEX кода моделей если кол. дискр. моделей без остатка /4
if (mod(tm,(4*round(tm/4))) == 0)
rsh_cos = reshape( x_cos > 0, [], tm/4 );
rsh_sin = reshape( x_sin > 0, [], tm/4 );
% модели в hex
x_hex_cos = dec2hex( bi2de(rsh_cos', 'left-msb'));
x_hex_sin = dec2hex( bi2de(rsh_sin', 'left-msb'));
% формат строки
formatSpec_x_cos_hex = 'Модель cos hex %s';
formatSpec_x_sin_hex = 'Модель sin hex %s';
% значения y_hex в hex виде
fprintf(formatSpec_x_cos_hex, x_hex_cos);
fprintf('\n');
fprintf(formatSpec_x_sin_hex, x_hex_sin);
fprintf('\n');
fprintf('\n');

% открытие файла на запись косинусной модели
fid = fopen('x_hex_cos.txt', 'wb');
% проверка корректности открытия
if fid == -1
error('File is not opened');
end

% запись в файл
fwrite(fid, x_hex_cos);
% закрытие файла
fclose(fid);

% открытие файла на запись синусной модели
fid = fopen('x_hex_sin.txt', 'wb');
% проверка корректности открытия
if fid == -1
error('File is not opened');
end

% запись в файл
fwrite(fid, x_hex_sin);
% закрытие файла
fclose(fid);

else
fprintf('Невозможно создать полный HEX код моделей\n')
fprintf('Количество дисретов моделей не делится без остатка на 4\n');
fprintf('Необходимо изменить частоту дискретизации \n\n');

end

z_cos = zeros(3*tm);
z_sin = zeros(3*tm);
z = zeros(3*tm);
% Коррелятор знаковый
for tm_mod = 1:(2*tm)

% побитовая XOR в матрицу xy_cos
xy_cos = bitxor(y_bit(tm_mod:(tm_mod+(tm-1))), x_bit_cos);
% побитовая XOR в матрицу xy_sin
xy_sin = bitxor(y_bit(tm_mod:(tm_mod+(tm-1))), x_bit_sin);
% проход по 256 выборкам
xy_cos_sum = 0;
xy_sin_sum = 0;
% квадратурная свертка
for i = 1:tm
% счетчик совпадений по модели cos
if (xy_cos(i) == 0)
xy_cos_sum = xy_cos_sum + 1;
else
xy_cos_sum = xy_cos_sum - 1;
end
% счетчик совпадений по модели sin
if (xy_sin(i) == 0)
xy_sin_sum = xy_sin_sum + 1;
else
xy_sin_sum = xy_sin_sum - 1;
end
end
% сумма
z(tm_mod) = xy_cos_sum + xy_sin_sum;
% подсчет совпадений по образцу "128-"
% z(tm_mod) = (tm/2 - length(find(xy_cos))) + (tm/2 - length(find(xy_sin)));

end

% максимальное значение в свертке
fprintf('Максимум в свертке %d\n', max(z(:,1)));
fprintf('\n');


%% графика

figure
tr = 1:tst;
plot(tr/f_dt, y), grid;
xlabel('Время мсек');
title('ЛЧМ сигнал');
zoom xon;

figure
plot(tch_lf, x_cos), grid;
xlabel('Время (в сек)');
title('ЛЧМ модель cos');
zoom xon;

figure
plot(tch_lf, x_sin), grid;
xlabel('Время (в сек)');
title('ЛЧМ модель sin');
zoom xon;

figure
tl = 1:(3*tm);
plot(tl, z), grid;
xlabel('Дискреты');
title('Знаковый коррелятор');
zoom xon;

% figure
% plot(tl, abs(fft(y))), grid;
% xlabel('Дискреты');
% title('Спектр сигнала');
% zoom xon;

% spectrogram(x_cos,256,250,256,0.01,'yaxis')

        if (s_q_data xor s_q_sin) = '0' then
            s_div_sin <= s_div_sin + 1;
        else
            s_div_sin <= s_div_sin - 1;
        end if;

        if (s_q_data xor s_q_cos) = '0'  then
            s_div_cos <= s_div_cos + 1;
        else
            s_div_cos <= s_div_cos - 1;
        end if;

s_div_sum <= conv_std_logic_vector((conv_integer(signed(s_div_sin)) + conv_integer(signed(s_div_cos))), 10);