## 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} =} master_slave_eval (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn

## Author: Jens <jens@orion>
## Created: 2020-08-04

function retval = master_slave_eval (input1, input2)
N_cycles = 10

N_phases_per_cycle = 4
dN = 24

N = N_cycles*N_phases_per_cycle

k_jitter = 2;
# Create Master
xp = zeros(1, N_cycles*N_phases_per_cycle*dN);
xv = zeros(1, N_cycles*N_phases_per_cycle*dN);

phase = 0;
v = 0;
for n=0:N-1,
	pos = max(1, n*dN + 1 + fix(0.5 + 2*k_jitter*randn()/sqrt(12)));
	xp(pos) = phase;
	xv(pos) = v + 1;
	phase = mod(phase + 1, N_phases_per_cycle);
	v = mod(v + 1, N_phases_per_cycle);

endfor

# Slave
k_upd = 0.5;

phi = 0;	
dphi = 0;
last_n = 0;
last_xp = 0;
param_changed = 1;

_phi = [];
_dphi = [];
_err_phi = [];
_err_dphi = [];
err_phi = 0
err_dphi = 0
for n=1:length(xp)
	yp(n) = phi;
	phi = mod(phi + dphi, N_phases_per_cycle);
	
	if xv(n) > 0
		if last_n > 0
			doRecalc = param_changed;
			param_changed = 0;
			
			den = (n - last_n);
			dp = mod(xp(n) - last_xp, N_phases_per_cycle);
			xdphi = dp/den;

			err_phi = phase_error_detector(phi, xp(n));
			err_dphi = frequency_error_detector(dphi, xdphi);
			dphi = dphi - k_upd*err_dphi;

			if doRecalc
				phi = xp(n);
				dphi = xdphi;
			end
			
			if xv(n) == 2
	#			phi = xp(n);
			endif
		endif
		last_xp = xp(n);
		last_n = n;
	end
	
	_err_phi = [_err_phi err_phi];
	_err_dphi = [_err_dphi err_dphi];
	_phi = [_phi phi];
	_dphi = [_dphi dphi];

endfor

function err = phase_error_detector(lo, ext)
	err = lo - ext;
endfunction

function err = frequency_error_detector(lo, ext)
	err = lo - ext;
endfunction

close all 
subplot(3, 1, 1)
plot (1:length(xp), xp, 1:length(yp), yp); legend('xp','yp'); grid;

subplot(3, 1, 2)
plot (1:length(_err_phi), _err_phi); legend('err_{phi}'); grid;

subplot(3, 1, 3)
plot (1:length(_err_dphi), _err_dphi); legend('err_{dphi}'); grid;

endfunction