#include "pickup_cancel.h"
#include <numbers>
#include <algorithm>
#include <cstdio>

using namespace std::complex_literals;

static constexpr float sinc(float x) {
	if(__builtin_expect(x * x <= 0x1p-24, 0)) {
		return 1.0f;
	} else {
		return std::sin(x)/x;
	}
}

static constexpr float cosc_help(float u) {
	if(__builtin_expect((u - 0.5)*(u - 0.5) <= 0x1p-24, 0)) {
		return 1.0f;
	} else {
		return std::cos(std::numbers::pi_v<float> * u)/(1.0f - 4*u*u);
	}
}

/*
	We have a cutoff at 480 * f0 and final 0 at (2048 - 480)*f0
	beta = 480 / (2048 - 480)
	Also, 1/T = "symbol rate" = (2048 - 480)*f0 and
	1/dt = "sampling rate" = 2048*f0

	Therefore t/T = (2048 - 480) / 2048 * n.

	Use a lookup table?
*/
static constexpr float rc_filter(float spdist) {

	float beta = 1.0 - 480.0f/(2048.0f - 480.0f);
	float x = spdist * (2048.0 - 480.0) / 2048;
	float u = beta*x;

	return sinc(std::numbers::pi_v<float> * x) * cosc_help(u);
}

static constexpr float twopi = 2.0f*std::numbers::pi_v<float>;
static constexpr float pi = 2.0f*std::numbers::pi_v<float>;

pickup_cancel::pickup_cancel(float fst_f, float eps_u) :
	act_f(fst_f), act_phi(0.0f),
	max_f(fst_f+eps_u),min_f(fst_f-eps_u),
	decay_cst(fst_f/200.0f), error_filter(0.0f),
	lockin_filter(1024), filter_index(0), nom_f(fst_f), lock_phi(0.0f)
{
}

bool pickup_cancel::is_in_sync() {
	return sin_power * sin_power >= 2.0f * cos_power * cos_power;
}

float pickup_cancel::do_sample(float s) {
	lock_phi += nom_f;
	if(lock_phi >= twopi) {
		lock_phi -= twopi;
	}

	std::complex<float> ex = std::exp(std::complex<float>(1.0i) * lock_phi);
	std::complex<float> c = s * ex;

	filter_sum += c - lockin_filter[filter_index & 1023u];
	lockin_filter[filter_index & 1023u] = c;
	filter_index++;

	/* Output of the filter is like a * (e^{in Df} - 1)/(e^{iDf} - 1)
		 so the output phase is (n - 1)/2 * Df. We have nDf <~ 2/48 = 1/24,
		 we should be okay.
	*/

	float pll_sample = std::real(filter_sum * ex)/1024;

	// Sensitivity of Ks = 2.0/pi u/rad
	float pd =  std::clamp(10000.0f * pll_sample * std::cos(act_phi), -1.0f, 1.0f);
	
	// See below
	error_filter = (pd + (pd - pd_delayed) * 10.0f * pi/(max_f - min_f)
		+ error_filter * 1.0/decay_cst) * decay_cst/(decay_cst + 1.0f);
	
	// Kosc = (Delta f) * 0.5 (rad/sample) / u
	act_f = std::clamp(nom_f + 0.5f*(max_f - min_f) * error_filter, min_f, max_f);

	pd_delayed = pd;
	// Integration gain is 1 rad / (rad / sample)

	act_phi += act_f;
	if(act_phi >= twopi) {
		act_phi -= twopi;
	}
	/* Loop gain is 1/jw * Kosc * Ks * H_filter
		 we thus want that at w = Kosc*Ks = Df/pi,
		 H_filter has phase close to 0°.

		 H_filter = (1 + jw tau_1)/(1 + jwtau_2),
		 with tau_2 ~= 1000/nom_f.
		 we want tau_1 somewhat above pi/Df, say .5 * pi/Df

		 The difference equation is thus
		 y + (1000/nom_f) * (y - y_d)/te = pd + (pd - pd_l) * (.5 * pi/Df)/te
		 (here te = 1 samp).
	*/

	/* If error filter is at -1.0f, we expect to lock at pi phase difference,
		 if we are at 0 we expect +-pi/2, and if we are at 1.0f we expect to be in
		 phase.

		 Therefore, we can try to compute
			E[s(t) cos(act_phi + pi/2 * (act_f - nom_f)/(max_f - min_f))]
		 and same for sin.
	*/
	sin_power +=
		.1f/48000 * (pll_sample * std::sin(act_phi + pi * (act_f - nom_f)/(max_f - min_f)) - sin_power);
	cos_power +=
		.1f/48000 * (pll_sample * std::cos(act_phi + pi * (act_f - nom_f)/(max_f - min_f)) - cos_power);

	float sampphase = act_phi/twopi * 2048; // in [0, 2048]
	float corr = 0.0f;

	// Do reconstruction using raised cosine filter
	int head = (int)sampphase - 16;
	float dist = head - sampphase;
	float coeffs[32];

	unsigned int iter = (unsigned)head;
	for(int i = 0; i < 32; ++i) {
		coeffs[i] = rc_filter(dist);
		corr += pickup_timedomain[iter&2047u] * coeffs[i];
		iter++;

		dist += 1.0f;
	}	

	/* Apprentissage */
	float err = s - corr;
	iter = head;
	for(int i = 0; i < 32; ++i) {
		pickup_timedomain[iter&2047u] += mu * err * coeffs[i];
		iter++;
	}

	if(!this->is_in_sync()) {return 0.0f;} else {return corr;}
}