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

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

pickup_cancel::pickup_cancel(float fst_f, float eps_u, int nharm) :
	act_f(fst_f), act_phi(0.0f),
	max_f(fst_f+eps_u),min_f(fst_f-eps_u), decay_cst(fst_f/50.0f), error_filter(0.0f),
	size(nharm), cosine_coeffs(nharm, 0.0f), sine_coeffs(nharm, 0.0f)
{}

bool pickup_cancel::is_in_sync() {
	return cosine_coeffs[0]*cosine_coeffs[0]
		     <= 0.5f * sine_coeffs[0] * sine_coeffs[0];
}

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

	float cosphi = std::cos(act_phi);
	float sinphi = std::sin(act_phi);

	// Always between -1 and 1
	error_filter += decay_cst * (s * cosphi - error_filter);	

	// Sensitivity setting to tune?
	float scaled_err = 10.0f * error_filter;
	scaled_err = std::clamp(scaled_err, -1.0f, 1.0f);

	act_f = (min_f + max_f)/2 + (max_f - min_f)/2 * scaled_err;

	float corr = 0.0f;

	float cos = 1.0f;
	float sin = 0.0f;
	for(int i = 0; i < size; ++i) {
		sin = std::sin((i+1)*act_phi);
		cos = std::cos((i+1)*act_phi);

		corr += cos*cosine_coeffs[i] + sin*sine_coeffs[i];

		cosine_coeffs[i] += (i+1) * decay_cst * (2 * s * cos - cosine_coeffs[i]);
		sine_coeffs[i] += (i+1) * decay_cst * (2 * s * sin - sine_coeffs[i]);		
	}

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