## 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

function retval = mc_pd_eval3 (perr, freq, slave_oversample)

Nc = 10;
Nspc = 25;

ka = 0.9;
klead = 0.8;
klag = 0.04/slave_oversample;
lag = 0;

# Create master
xm = [];
for n=1:Nspc,
	for m=1:slave_oversample,
		xm = [xm n/Nspc];
	endfor
endfor
x = repmat(xm, 1, Nc);

N = Nc*Nspc*slave_oversample;
assert (N == length(x));

# Create slave
pc = 0.5 + perr;
dfreq = freq/slave_oversample; 
dpc = 0;

sample_count = slave_oversample;
for n=1:N,
	perr = phase_det(pc, x(n));
	sample_count = sample_count - 1;
	if sample_count == 0,
		sample_count = slave_oversample;
	else
		perr = 0;
	end
	_lo(n) = mod(pc - 0.5, 1);
	dpc = dfreq - (klag*lag + klead*perr);
	pc = mod(pc + dpc, 1);
	lag = lag + perr;
	_perr(n) = perr;
	_dpc(n) = dpc;
end

function perr = phase_det(lo, x)
	perr = mod((lo - x), 1) - 0.5;
endfunction

close all;
subplot(3, 1, 1)
plot(1:N, _lo, 1:N, x); legend('Lo', 'x'); grid

subplot(3, 1, 2)
plot(1:N, _dpc); legend('f'); grid

subplot(3, 1, 3)
plot(1:N, _perr); legend('p'); grid

endfunction