#include "adrc.h"

void ladrc_init(ladrc_t*  adrc, float ts, float wo, float wc, float b0) {
	adrc->z1 = adrc->z2 = adrc->out = 0;
	adrc->ts = ts;
	adrc->b0 = b0;
	adrc->wcv = wc;
	adrc->p1 = 2 * wo;
	adrc->p2 = wo * wo;
}

float ladrc_run(ladrc_t *adrc, float in, float feedback) {
	float y = feedback;
	float u = adrc->out;
	//do eso
	float error = y - adrc->z1;
	
	adrc->z2 = adrc->z2 + adrc->ts * (error * adrc->p2);
	adrc->z1 = adrc->z1 + adrc->ts * (error * adrc->p1 + adrc->z2 + u * adrc->b0);

	//do error compestion
	float ref = in;
	float out = ((ref - adrc->z1) * adrc->wcv - adrc->z2) / adrc->b0;
	adrc->out = fclamp(out, adrc->out_min, adrc->out_max);

	return adrc->out;
}

void ladrc_reset(ladrc_t* adrc, float feedback, float out_init) {
	adrc->z1 = feedback;
	adrc->z2 = -out_init * adrc->b0;
	adrc->out = out_init;
}

void ladrc_copy(ladrc_t *dst, ladrc_t *src) {
	dst->z1 = src->z1;
	dst->z2 = src->z2;
	dst->out = src->out;
}