#include "PID.h"
#include "util.h"

PID createPID(PIDConfig config) {
	PID pid = (PID){
		.p = 0.0,
		.i = 0.0,
		.d = 0.0,
		.kP = config.kP,
		.kI = config.kI,
		.kD = config.kD,
		.angleMode = config.angleMode,
		.doClamp = config.doClamp,
		.min = config.min,
		.max = config.max,
		.error = 0.0,
		.pastError = 0.0,
		.output = 0.0
	};

	return pid;
}

float runPID(float setpoint, float processValue, PID * pid) {
	// Get error.
	if (pid->angleMode)
		pid->error = angleDis(setpoint, processValue);
	else
		pid->error = setpoint - processValue;

	// Set p, i and d.
	pid->p = pid->error * pid->kP;
	pid->i += pid->error * pid->kI;
	pid->d = (pid->error - pid->pastError) * pid->kD;

	// Update error.
	pid->pastError = pid->error;

	// Set output.
	pid->output = pid->p + pid->i + pid->d;

	// Clamp.
	if (pid->doClamp)
		pid->output = Clamp(pid->output, pid->min, pid->max);

	return pid->output;
}

void resetPID(PID * pid) {
	pid->p = 0.0;
	pid->i = 0.0;
	pid->d = 0.0;
	pid->error = 0.0;
	pid->pastError = 0.0;
	pid->output = 0.0;
}

float angleDis(float a, float b) {
	float dir = b - a;

	if (fabsf(dir) > (PI/2))
		dir = -(signum(dir) * PI) + dir;

	return dir;
}