## 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_eval2 (perr, ferr, slave_oversample)

Nc = 10;
Nspc = 96;

ka = 0.9;
klead = 0.2;
klag = 0.01/slave_oversample;

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

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

# Create slave
pc = 1.0/slave_oversample + Nspc/2 + perr;
dpc = 1.0/slave_oversample + ferr;

sample_count = slave_oversample;

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

function perr = phase_det(lo, x, Nspc)
	perr = mod((lo - x), Nspc) - Nspc/2;
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