## Copyright (C) 2020 Jens
## 
## This program is free software: you can redistribute it and/or modify it
## under the terms of the GNU General Public License as published by
## the Free Software Foundation, either version 3 of the License, or
## (at your option) any later version.
## 
## This program is distributed in the hope that it will be useful, but
## WITHOUT ANY WARRANTY; without even the implied warranty of
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
## GNU General Public License for more details.
## 
## You should have received a copy of the GNU General Public License
## along with this program.  If not, see
## <https://www.gnu.org/licenses/>.

## -*- texinfo -*- 
## @deftypefn {} {@var{retval} =} mc_pd_eval (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn

## Author: Jens <jens@orion>
## Created: 2020-08-07
#
# pll_burst_eval(0.5, 0.0, 10, 0)
#
# Usage:
# x = siggen(2000, 48000, 8, 0.1);
# pll_burst_eval(48000,0, x)
#

function retval = pll_burst_eval(varargin)

params_default = struct("fs", 48000, "f", 0, "ref", [], "klead", 0.004, "klag", 0.00002, 'domega_max', 2000);
params = parse_myparams(params_default, varargin);

# Create variable from params
fs = params.fs;
f = params.f;
ref = params.ref;
klead = params.klead;
klag = params.klag;

domega_max = params.domega_max;
domega_lo_max = +domega_max/fs;
domega_lo_min = -domega_max/fs;

N = length(ref);
omega = f/fs;

# Start processing
lag = 0;
phi_lo = 0;
omega_err = 0;
agc_state = 0;

for n=1:N,
	# Derotator LO
	lo = exp(j*2*pi*phi_lo);

	# AGC
	[y, agc_state] = agc(agc_state, ref(n));
	
	# Pahse detector
	[perr, pd] = phase_det(lo, y);

	# Process loop filter output kv
	lag = lag + klag*perr;
	kv = (lag + klead*perr);
	
	# Calculate delta omega for LO
	domega_lo = min(domega_lo_max, max(domega_lo_min, kv));
	omega_lo = omega + domega_lo;
	omega_err = omega_lo - omega;
	
	# advance phase accumulator for LO
	phi_lo = phi_lo + omega_lo;

	_perr(n) = perr;
	_omega_err(n) = omega_err;
	_lo(n) = lo;
	_ref_derote(n) = pd;
	_agc_state(n) = agc_state;
end

f_err = fs*omega_err

function [perr, pd] = phase_det(lo, ref)
	pd = ref*conj(lo);
	perr = imag(pd);
endfunction

function [y, state_out] = agc(state_in, x)
	alpha = 0.1;
	refi = 1.0;
	eps = 1e-3;
	state_out = (1-alpha)*state_in + alpha*abs(x);
	
	c = refi/(eps + state_out);
	y = x*c;
endfunction
	
close all;
subplot(3, 1, 1)
plot(1:N, imag(_lo), 1:N, imag(ref), 1:N, _agc_state); legend('Lo', 'ref', 'agc'); grid

subplot(3, 1, 2)
plot(1:N, real(_ref_derote), 1:N, imag(_ref_derote)); legend('Re_{Derot}', 'Im_{Derot}'); grid

subplot(3, 1, 3)
plot(1:N, _perr, 1:N, _omega_err); legend('p_{err}', 'f_{err}'); grid

endfunction