/* Reflow Oven Controller
 *
 * Copyright (C) 2020  Mario Hüttel <mario.huettel@gmx.net>
 *
 * This file is part of the Reflow Oven Controller Project.
 *
 * The reflow oven controller is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * The Reflow Oven Control Firmware 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 the reflow oven controller project.
 * If not, see <http://www.gnu.org/licenses/>.
 */

/** @addtogroup pid-controller
 *  @{
 */

#include <reflow-controller/pid-controller.h>
#include <string.h>

void pid_init(struct pid_controller *pid, float k_deriv, float k_int, float k_p, float output_sat_min,
	      float output_sat_max, float integral_max, float kd_tau, float sample_period)
{
	if (!pid)
		return;

	pid->sample_period = sample_period;
	pid->k_p = k_p;
	pid->k_int = k_int;
	pid->k_deriv = k_deriv;
	pid->k_int_t = pid->k_int * pid->sample_period * 0.5f;
	pid->k_deriv_t = pid->k_deriv * 2.0f / (pid->sample_period + 2.0f * kd_tau);
	pid->k_inv_deriv_t = (2.0f * kd_tau - sample_period) / (2.0f * kd_tau + sample_period);
	pid->output_sat_max = output_sat_max;
	pid->output_sat_min = output_sat_min;
	pid->integral_max = integral_max;

	pid_zero(pid);
}

int pid_copy(struct pid_controller *dest, const struct pid_controller *src)
{
	if (!dest || !src)
		return -1;

	memcpy(dest, src, sizeof(struct pid_controller));

	return 0;
}

void pid_zero(struct pid_controller *pid)
{
	pid->control_output = 0.0f;
	pid->last_in = 0.0f;
	pid->integral = 0.0f;
}

static void calculate_integral(struct pid_controller *pid, float deviation)
{
	pid->integral = pid->integral + pid->k_int_t * (deviation + pid->last_in);

	/* Saturate integral term to specified maximum */
	if (pid->integral > pid->integral_max)
		pid->integral = pid->integral_max;
	else if (pid->integral < 0.0f)
		pid->integral = 0.0f;
}

float pid_sample(struct pid_controller *pid, float deviation)
{
	float output;

	if (!pid)
		return 0.0f;

	output = deviation * pid->k_p;

	/* PID runaway compensation */
	if (!(deviation > 0.0f && pid->control_output > pid->output_sat_max - 0.5f && pid->integral > 0) &&
	    !(deviation < 0.0f && pid->control_output < pid->output_sat_min + 0.5f && pid->integral < 0)) {
		calculate_integral(pid, deviation);
	}

	/* Calculate derivative part */
	pid->derivate = pid->k_deriv_t * (deviation - pid->last_in) +
			pid->k_inv_deriv_t * pid->derivate;

	output += pid->derivate;
	output += pid->integral;

	/* Saturate output */
	if (output < pid->output_sat_min)
		output = pid->output_sat_min;
	else if (output > pid->output_sat_max)
		output = pid->output_sat_max;

	pid->control_output = output;
	pid->last_in = deviation;

	return output;
}

float pid_get_control_output(const struct pid_controller *pid)
{
	if (!pid)
		return 0.0f;

	return pid->control_output;
}

/** @} */