## 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
#
# mc_pd_eval3(0.5, 0.0, 10, 0)
#

function retval = mc_pd_eval3 (phase, freq, slave_oversample, USE_TRIGON)

Nc = 10;
Nspc = 25;

ka = 0.9;
klead = 0.4;
klag = 0.02/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
dfreq = freq/slave_oversample; 
omega = 0;

sample_count = slave_oversample;

a = 0.5;
y(1) = a*cos(2*pi*phase);
y(2) = a*sin(2*pi*phase);

for n=1:N,
	if USE_TRIGON
		[phase, y] = process_trigon(y, omega);
	else
		phase = process_linear_phase(phase, omega);
	endif
	_lo(n) = phase;

	perr = phase_det(phase, x(n));
	sample_count = sample_count - 1;
	if sample_count == 0,
		sample_count = slave_oversample;
	else
		perr = 0;
	end
	
	omega = dfreq - (klag*lag + klead*perr);
	lag = lag + perr;
	_perr(n) = perr;
	_omega(n) = omega;
end

function y = jmod(x)
	y = x - fix(x);
	y = y + 0^(0.5 + sign(y)/2);
endfunction

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

function phase_out = process_linear_phase(phase_in, omega)
	phase_out = jmod(phase + omega);
endfunction

function [phase_out, y] = process_trigon(y, omega)
	b = 2.0 * sin(pi*omega);
	y(1) = y(1) - b*y(2);
	y(2) = y(2) + b*y(1);
	phase_out = jmod(0.5 + 0.5*atan2(y(1), -y(2))/pi);
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, _omega); legend('f'); grid

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

endfunction