function ofdm_rx(filename, foff, flip_spec, SNR, plot_mode)
%
% [sym_out, H, W] = ofdm_rx(filename, foff, flip_spec, SNR, plot_mode)
% Example: ofdm_rx('drm-15435-if.wav', -174.65, 0, 400, 1)
% Example: ofdm_rx('xv.wav', 0, 0, 400, 1)
%       or ofdm_rx('yv.wav', 0, 0, 400, 1)

% --------------------------------------------------------------
close all;

% --------------------------------------------------------------
% fixed stuff
% --------------------------------------------------------------
MIN_ABS_H = 1e-10;
KI_track = 0.2;			% Integral gain CFO tracking
KI_acq = 0.2;			% Integral gain CFO acquisition

drm_mode = 'B';
drm_bw = '10k';

% Mode 'B'
fac_cell_list = ...
{
    [],
    [],
    [13, 25, 43, 55, 67],
    [15, 27, 45, 57, 69],
    [17, 29, 47, 59, 71],
    [19, 31, 49, 61, 73],
    [9, 21, 33, 51, 63, 75],
    [11, 23, 35, 53, 65, 77],
    [13, 25, 37, 55, 67, 79],
    [15, 27, 39, 57, 69, 81],
    [17, 29, 41, 59, 71, 83],
    [19, 31, 43, 61, 73],
    [21, 33, 45, 63, 75],
    [23, 35, 47, 65, 77],
    []
};

% --------------------------------------------------------------
% Precalculation
% --------------------------------------------------------------
[ofdm_drm_params, ofdm_spec_occ] = drm_params(drm_mode, drm_bw);

[carrier_pilot, phi_pilot, mag_pilot] = getRefFreq(drm_mode);
[carrier_time, phi_time, mag_time] = getRefTime(drm_mode);

N = ofdm_drm_params.nu;
Ng = ofdm_drm_params.ng;
symbols_per_frame = ofdm_drm_params.nspf;
symbols_to_delay = ofdm_drm_params.y;
frames_per_window = 2*ofdm_drm_params.y;
carriers = ofdm_spec_occ.kmin:ofdm_spec_occ.kmax;
carrier_indexes = c2i(N, carriers);

% Read wav file
[wav_size wav_fs] = wavread(filename, 'size');
[wav_x, wav_fs, nbits] = wavread(filename, 0);
wav_numCh = wav_size(2);
wav_size = wav_size(1);

filter_h = FIRCalcLowpass(0.25, 15);
filter_z = [];

% --------------------------------------------------------------
% Determine data mode
% --------------------------------------------------------------
isModePassband = 0;
if (wav_numCh == 1)
    fprintf('Mode: Passband\n');
	isModePassband = 1;
	fs = fix(wav_fs/4);

elseif (wav_numCh == 2)
    fprintf('Mode: Baseband\n');
	fs = wav_fs;
else
	error('Cannot read file');
end

% --------------------------------------------------------------
% Timing coarse stuff
timingCoarse = struct('Z', 0, 'started', 0, 'sync_counter', 20, 'sync', 0, 'cp_off', 0, 'cp_off_', [], 'Nd', 0);

% CFO coarse integer stuff
cfoCoarseInteger = struct('Z', 0, 'started', 0, 'sync_counter', 20, 'sync', 0, 'f_err', 0);

% CFO coarse fractional stuff
cfoCoarseFractional = struct('Z', 0, 'started', 0, 'sync_counter', 20, 'sync', 0, 'f_err', 0, 'f_err_', [], 'df', 0, 'df_', []);

% CFO Fine stuff
cfoFine = struct('Z', 0, 'started', 0, 'sync_counter', 20, 'sync', 0, 'f_err', 0, 'f_err_', [], 'df', 0, 'df_', [], 'Xlast', [], 'bin_track', 1);

% Frame timing stuff
frameTiming = struct('Z', 0, 'started', 0, 'sync_counter', 20, 'sync', 0, 'kc_max', -1000, 'n', 0, 'symbol_start', 0, 'sample_start', 0);

% Equalization stuff
equalizer = struct('Z', 0, 'started', 0, 'sync_counter', 20, 'sync', 0, 'sync_symbol_num', 0, 'f_err', 1000, 'f_err_', [], 's', 0, 'm', 0, 'symbol_count', 1, 'eps_abs_h', MIN_ABS_H);

fileChunkSize = 16*N;
fileCunkCounter = 0;
phi_mix = 0;
phi_foff = 0;
phi_track = 0;
df_track = 0;
df_track_ = [];
sfo_ = [];
max_H_plot = 0;

fileRemain = wav_size;

buf_recv = buffer();
buf_Z = buffer();
sample_offset = 0;
sample_offset_integer = 0;
sample_offset_fractional = 0;
% --------------------------------------------------------------
% Main loop
% --------------------------------------------------------------
while((fileRemain > fileChunkSize) | (buf_recv.len() > 2*N))	% ToDo: don't leave loop, until file has been consumed.
	
	if (fileRemain > fileChunkSize)
		n0 = fileCunkCounter*fileChunkSize+1;
		n1 = (fileCunkCounter+1)*fileChunkSize;
		[wav_x] = wavread(filename, [n0 n1]);

		if (~isModePassband)
			wav_x = wav_x(:, 1) + j*wav_x(:, 2);
			x = wav_x;	% ToDo: Remove DC
			x = x.*exp(-j*(2*pi*(-foff/fs)*(0:length(x)-1)' + phi_foff)); 
			phi_foff = mod(phi_foff + 2*pi*(-foff/fs)*length(x), 2*pi);
		end

		if (isModePassband)
			x = wav_x;	% ToDo: Remove DC
			x = x .*exp(-j*(2*pi*(0.25-foff/wav_fs)*(0:length(x)-1)' + phi_mix)); 
			phi_mix = mod(phi_mix + 2*pi*(0.25-foff/wav_fs)*length(x), 2*pi);
			[x, filter_z] = filter(filter_h, 1, x, filter_z);
			x = x(1:4:length(x));
		end

		if flip_spec
			x = conj(x);
		end

		% add noise
		x = awgn(x, SNR);

		% Write to buffer
		buf_recv.write(x);
	end
	
	% --------------------------------------------------------------
	% Timing coarse
	if (~timingCoarse.sync)
		if (~timingCoarse.started)
			timingCoarse.started = 1;
			fprintf('Coarse timing acquisition');
		end

		x = buf_recv.read(N+Ng);
		if ~isempty(x)
			[timingCoarse.cp_off, timingCoarse.Nd, valid, timingCoarse.Z] = timing_coarse(x, N, Ng, Ng, timingCoarse.Z);
			if (valid)
				timingCoarse.cp_off_ = [timingCoarse.cp_off_ timingCoarse.cp_off];
				if (timingCoarse.sync_counter > 0)
					timingCoarse.sync_counter = timingCoarse.sync_counter - 1;
				else
					timingCoarse.sync = 1;
				end
			end;
			if (timingCoarse.sync)
				timingCoarse.cp_off = round(median(timingCoarse.cp_off_));

				% Reset buffer to Cp offset
				buf_recv.setReadIndex(timingCoarse.cp_off);

				fprintf(' (cp_off=%d, Nd=%d)\n', timingCoarse.cp_off, timingCoarse.Nd);
			end
		end
	end
	
	% Integer CFO coarse
	if (timingCoarse.sync) & (~cfoCoarseInteger.sync)
		if (~cfoCoarseInteger.started)
			cfoCoarseInteger.started = 1;
			fprintf('Coarse integer CFO acquisition');
		end

		x = buf_recv.read(N+Ng);
		if (~isempty(x))
			[d_bin, valid, cfoCoarseInteger.Z] = cfo_coarse_int(x(Ng+1:N+Ng), N, carrier_pilot, 11, cfoCoarseInteger.Z);
			if valid
				cfoCoarseInteger.f_err = -d_bin(1)/N*fs;
				if (cfoCoarseInteger.sync_counter > 0)
					cfoCoarseInteger.sync_counter = cfoCoarseInteger.sync_counter - 1;
				else
					cfoCoarseInteger.sync = 1;
					% Reset buffer to Cp offset
					buf_recv.setReadIndex(timingCoarse.cp_off);
					fprintf(' (f_err_i=%f)\n', cfoCoarseInteger.f_err);
				end
			end
		end
	end
	
	% Fractional CFO coarse
	if (cfoCoarseInteger.sync) & (~cfoCoarseFractional.sync)
		if (~cfoCoarseFractional.started)
			cfoCoarseFractional.started = 1;
			fprintf('Coarse fractional CFO acquisition');
		end
		x = buf_recv.read(N+Ng);
		if (~isempty(x))
			[f_err, cfoCoarseFractional.Z] = cfo_coarse_fract(x, N, Ng, Ng, timingCoarse.Nd, cfoCoarseFractional.Z);
			cfoCoarseFractional.f_err_ = [cfoCoarseFractional.f_err_ f_err*fs];

			if (cfoCoarseFractional.sync_counter > 0)
				cfoCoarseFractional.sync_counter = cfoCoarseFractional.sync_counter - 1;
			else
				cfoCoarseFractional.sync = 1;
				cfoCoarseFractional.f_err = mean(cfoCoarseFractional.f_err_);
				cfoCoarseFractional.df = cfoCoarseInteger.f_err + cfoCoarseFractional.f_err;
				cfoCoarseFractional.df_ = cfoCoarseInteger.f_err + cfoCoarseFractional.f_err_;

				% Reset buffer to Cp offset
				buf_recv.setReadIndex(timingCoarse.cp_off);
				fprintf(' (f_err_f=%f, df_est=%f)\n', cfoCoarseFractional.f_err, cfoCoarseFractional.df);
				
			end
		end
	end

	% Create symbols
	if (cfoCoarseFractional.sync)
		if (~cfoFine.started)
			cfoFine.started = 1;
			df_track = cfoCoarseFractional.df;
			fprintf('Creating Symbols\n');
			fprintf('Fine fractional CFO acquisition\n');
			delta_theta = 0;
			leadLag_f_z = 0;
			leadLag_i_z = 0;
		end
		kn = fs/(2*pi*(1+Ng/N))/N;
		sym_pilot = exp(j*2*pi*phi_pilot'/1024);
		bin_pilot = c2i(N, carrier_pilot);

		x = buf_recv.readAt(N+Ng, -sample_offset_integer);
		if (~isempty(x))

			if equalizer.sync
				df_track = df_track + KI_track*equalizer.f_err;
			else
				df_track = df_track + KI_acq*cfoFine.f_err;
			end
			df_track_ = [df_track_ df_track];
			
			x = x.*exp(j*(2*pi*df_track/fs*(0:N+Ng-1)' + phi_track));
			phi_track = mod(phi_track + 2*pi*df_track/fs*(N+Ng), 2*pi);
			Fc = exp(j*(2*pi*(0:N-1)/(N)*(sample_offset_fractional)));
			X = fft(x(Ng+1:N+Ng)) .* [Fc(N/2:-1:1) Fc(N:-1:N/2+1)].';
			X = X .* exp(j*2*pi*(0:N-1)/(N)*(sample_offset_integer)).';
			buf_Z.write(X);
			% CFO Fine acquisition
			if ~equalizer.sync
				if (~isempty(cfoFine.Xlast))
					Z = cfoFine.Xlast .* conj(X);
					f_err = kn*angle(Z(bin_pilot));
					cfoFine.f_err = mean(f_err(cfoFine.bin_track));
					cfoFine.f_err_ = [cfoFine.f_err_ cfoFine.f_err];
				end
				cfoFine.Xlast = X;
			end
		end

		% Frame timing acquisition
		bins = c2i(N, carrier_time);
		skip_syms = ofdm_drm_params.nspf*4;
		if (~frameTiming.sync) && (buf_Z.len() >= (N*(1+ofdm_drm_params.nspf+skip_syms)))
			if (~frameTiming.started)
				frameTiming.started = 1;
				fprintf('Frame timing acquisition');
			end

			if (frameTiming.n < ofdm_drm_params.nspf)
				frameTiming.Z = 0;
				Z1 = buf_Z.peekAt(N, N*skip_syms);
				Z2 = buf_Z.peekAt(N, N*(ofdm_drm_params.nspf+skip_syms));
				kc = abs(Z1(bins)' * Z2(bins));
				if valid
					if kc > frameTiming.kc_max
						frameTiming.symbol_start = frameTiming.n;
						frameTiming.kc_max = kc;
					end
				end
				frameTiming.n = frameTiming.n + 1;
				buf_Z.read(N);
			else
				frameTiming.sync = 1;
				frameTiming.sample_start = timingCoarse.cp_off + (N+Ng)*frameTiming.symbol_start;
				% Reset Buffer to frame start
				buf_Z = buffer();
				buf_recv.setReadIndex(frameTiming.sample_start);
				fprintf(' (symbol_start=%d, sample_start=%d)\n', frameTiming.symbol_start, frameTiming.sample_start);
			end
		end

		if (frameTiming.sync)
			
			if (~equalizer.started)
				equalizer.started = 1;
				H_gain_cell = cell (frames_per_window, 1);
				H_raw = zeros(length(carriers),1);
				sample_offset_fractional_ = [];
				sample_offset_integer_ = [];
				
				fprintf('Channel estimation and symbol reception\n');
				[W_syms, W_pilots] = calcWiener(drm_mode, drm_bw);
			end

			Z = transpose(buf_Z.read(N));
			if ~isempty(Z)
				equalizer.n = rem(equalizer.m + symbols_to_delay-1, ofdm_drm_params.y);

				[carrier_gain, phi_gain, mag_gain] = getRefGain(ofdm_drm_params, ofdm_spec_occ, equalizer.s);
				carrier_gain_bins = c2i(N, carrier_gain);
				
				H_ = Z(carrier_gain_bins)./(mag_gain.*exp(j*2*pi*phi_gain/1024));
				H_raw(carrier_gain_bins) = H_;
				H_gain_cell{equalizer.m+1} = H_;
				str = sprintf('Symbol %d, s=%d, m=%d, n=%d', equalizer.symbol_count, equalizer.s, equalizer.m, equalizer.n);

				if equalizer.symbol_count >= frames_per_window

					ii = 1+mod(equalizer.m+symbols_to_delay-1+(0:frames_per_window-1), frames_per_window);

					H_gain = [H_gain_cell{ii}];
					H_syms = H_gain*W_syms{equalizer.n+1};
					H_pilots = (H_gain)*W_pilots{equalizer.n+1};

					% Get the correctly delayed symbols
					Z = buf_Z.peekAt(N, -N*(symbols_to_delay+1));
	
					sym = Z(carrier_indexes).';

					% Get the correctly delayed raw channel estimate
					Hr = H_gain_cell{mod(equalizer.m-1, frames_per_window)+1};

					% get frequency error
					% correlate wiener filtered channel with raw estimate
					equalizer.f_err = fs/(2*pi*N)*angle(H_pilots * Hr' + equalizer.eps_abs_h);
					equalizer.f_err_ = [equalizer.f_err_ equalizer.f_err];

					if (abs(equalizer.f_err) < 0.1) & (equalizer.sync == 0)
						if equalizer.sync_counter > 0
							equalizer.sync_counter = equalizer.sync_counter - 1;
						else
							equalizer.sync = 1;
							equalizer.sync_symbol_num = equalizer.symbol_count;
							fprintf('Enter fine tracking mode at symbol %d\n', equalizer.sync_symbol_num);
						end
					end
					
					% check for too small H
					rms_H = sqrt( abs( H_syms*H_syms' )/length(H_syms) );
					equalizer.eps_abs_h = max( rms_H/20, MIN_ABS_H );
					H_too_small_index = find( abs(H_syms)<equalizer.eps_abs_h );
					H_syms( H_too_small_index ) = equalizer.eps_abs_h;

					% SFO
					% CTO per carrier
					c_sfo = H_syms.' .* conj(H_raw(carrier_indexes)) + equalizer.eps_abs_h;
					sum_sfoL = sum(c_sfo(1:103));
					sum_sfoH = sum(c_sfo(105:207));
					sfoL = angle(sum_sfoL);
					sfoH = angle(sum_sfoH);
					sfo_ = [sfo_ 2*fs/(2*pi*N)*(sfoH - sfoL)];

					% STO
					H = [H_syms zeros(1, N-length(H_syms))].';
					Tg = Ng;
					Tu = N;
					Tgh = floor(Ng/2);
					Tgs = floor(Ng);
					max_delta_theta = Tgh;
					h_absq = abs( ifft( transpose(H)) ).^2;	%note thate h_absq is mirrored in time direction because we use fft instead of ifft
					h_filter_sto = sin([1:Tgs]/(Tgs+1)*pi).^(0.9);
					[dummy, delta_theta] = max(filter(h_filter_sto , 1, [h_absq,h_absq] ) );	%find a window with maximum energy inside
					delta_theta =  ( rem( N + Tgh + 1 - delta_theta + N*1.5, N ) - N/2 )*Tu/N;
%					delta_theta = sign(delta_theta);
					
					[sample_offset_fractional, leadLag_f_z] = leadLag(delta_theta, 0.01, 0.000, leadLag_f_z);
					[sample_offset_integer, leadLag_i_z] = leadLag(delta_theta-sample_offset_integer, 0.01, 0.00, leadLag_i_z);
					
					sw_f = 0;
					sw_i = 1;
					sample_offset_fractional = sw_f *(sample_offset_fractional - sw_i*floor(sample_offset_fractional));
					sample_offset_fractional_ = [sample_offset_fractional_ sample_offset_fractional];
					sample_offset_integer = sw_i*floor(sample_offset_integer);
					sample_offset_integer_ = [sample_offset_integer_ sample_offset_integer];
					
					% equalize symbol
					sym_eq = sym ./ H_syms;
					if strcmp(plot_mode, 'h_pilots')
						h = ifft(H_syms);
						subplot(3, 1, 1)
						plot(0:length(h)-1, real(h), 'b-'); grid; 
						subplot(3, 1, 2)
						plot(0:length(h_absq)-1, real(h_absq), 'b-'); grid; 
						hold on;
						for b=c2i(N, carrier_pilot)
							x = [-delta_theta -delta_theta];
							y = [0 2];
							plot(x,y, 'r-');
						end
						hold off;
						subplot(3, 1, 3)
						plot(0:length(h_filter_sto)-1, real(h_filter_sto), 'b-'); grid; 
						title(str);
						pause(0.01);
					end
					if strcmp(plot_mode, 'H_pilots')
						subplot(3, 1, 1)
						plot(1:length(Hr), abs(Hr), 'b-', 1:length(H_pilots), abs(H_pilots), 'r-'); grid; 
						subplot(3, 1, 2)
						plot(1:length(Hr), angle(Hr), 'b-', 1:length(H_pilots), angle(H_pilots), 'r-'); grid; 
						subplot(3, 1, 3)
						plot(1:length(c_sfo), angle(c_sfo), 'b-'); grid; 
						title(str);
						pause(0.01);
					end
					if strcmpi(plot_mode, 'h_syms')
						max_H_plot = ceil(max(max_H_plot, max(abs(H_raw(2:2:N)))));
						H_raw(1:2:N) = H_raw(2:2:N);
						subplot(3, 1, 1)
						plot(carriers, abs(H_raw(carrier_indexes)), 'b-', carriers, abs(H_syms), 'r-'); grid; title('abs(H_{syms})'); axis([ofdm_spec_occ.kmin ofdm_spec_occ.kmax 0 max_H_plot]); title(str);
						subplot(3, 1, 2)
						plot(carriers, angle(H_raw(carrier_indexes)), 'b-', carriers, angle(H_syms), 'r-'); grid; title('phi(H_{syms})'); axis([ofdm_spec_occ.kmin ofdm_spec_occ.kmax -4 4]); title(str);
						subplot(3, 1, 3)
						plot(carriers, abs(sym_eq), 'b-'); grid; title('abs(Sym_{Eq})'); axis([ofdm_spec_occ.kmin ofdm_spec_occ.kmax 0 2]); title(str);
						hold on;
						for b=c2i(N, carrier_pilot)
							x = [b b];
							y = [0 2];
							plot(x-1,y, 'r-');
						end
						hold off;

						pause(0.01);
					end
					if strcmpi(plot_mode, 'fac') & (equalizer.sync)
						fac_c = fac_cell_list{mod(equalizer.s-symbols_to_delay, symbols_per_frame)+1};
						if (~isempty(fac_c))
							fac_i = fac_c - ofdm_spec_occ.kmin + 1;
							hold on;
							plot(sym_eq(fac_i), '+', 'MarkerSize', 4);
							pause(0.01);
							grid; title('FAC'); axis([-1 1 -1 1]);
							hold off;
						end
					end
				end
				equalizer.symbol_count = equalizer.symbol_count + 1;
				equalizer.m = mod(equalizer.m + 1, frames_per_window);
				equalizer.s = mod(equalizer.s + 1, symbols_per_frame);
			end
		end
	end
	fileRemain = fileRemain - fileChunkSize;
	fileCunkCounter = fileCunkCounter + 1;
end

% --------------------------------------------------------------
% Output
% --------------------------------------------------------------
figure;
plot(0:length(timingCoarse.cp_off_)-1, timingCoarse.cp_off_, 0:length(timingCoarse.cp_off_)-1, timingCoarse.cp_off*ones(size(timingCoarse.cp_off_)), 'r-'); grid; title('cp_{off}')

figure;
subplot(4, 1, 1)
plot(0:length(cfoCoarseFractional.df_)-1, cfoCoarseFractional.df_); grid; title('df_{coarse} (Acquisition)')

subplot(4, 1, 2)
plot(0:length(cfoFine.f_err_.')-1, cfoFine.f_err_.'); grid; title('Error(df_{fine}) (Acquisition)')

subplot(4, 1, 3)
plot(0:length(equalizer.f_err_)-1, equalizer.f_err_); grid; title('Error(df_{Eq}) (Track)')

subplot(4, 1, 4)
plot(0:length(df_track_)-1, df_track_); grid; title('df_{track}')
hold on;

figure;
subplot(3, 1, 1)
plot(0:length(sfo_)-1, sfo_, '-'); grid; title('SFO');
subplot(3, 1, 2)
plot(0:length(sample_offset_fractional_)-1, sample_offset_fractional_, '-'); grid; title('STO_F');
subplot(3, 1, 3)
plot(0:length(sample_offset_integer_)-1, sample_offset_integer_, '-'); grid; title('STO_I');

% --------------------------------------------------------------
% Write wave files
% --------------------------------------------------------------
fprintf('Writing wav files ');

buf_recv.setReadIndex(0);
x = buf_recv.read(buf_recv.len());

fprintf('.');
xout = [real(x) imag(x)];
wavwrite(xout,fs,nbits,'rx_x.wav');

% Modulate on carrier
fprintf('.');
xup = upsample(x, 4);
hlp = FIRCalcLowpass(0.25, 129);

fprintf('.');
xup = filter(hlp, 1, xup);

fprintf('.');
y = exp(j*2*pi.*(0:length(xup)-1)'*(0.25+0.25*df_track/fs)) .* xup;
wavwrite(0.8*y,fs*4,nbits,'rx_y.wav');

fprintf('.');
tt = equalizer.f_err_ ./ max(abs(equalizer.f_err_));
wavwrite(0.8*tt,fs/8,nbits,'rx_cfo_err.wav');
fprintf(' done\n');


% --------------------------------------------------------------
function Y = spec_order(N, X)
    Y = [X(N/2+1:N)' X(1:N/2)']';

function [y, zf] = leadLag(x, kI, kP, zi)
	if isempty(zi) | isnumeric(zi)
		zi = struct('state', 0);
	end
	
	zi.state = zi.state + x;
	y = kI*zi.state + x*kP;
	
	zf = zi;