/* Reflow Oven Controller
*
* Copyright (C) 2021  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 temp-profile
 * @{
 */

#include <reflow-controller/temp-profile/temp-profile-executer.h>
#include <reflow-controller/systick.h>
#include <helper-macros/helper-macros.h>
#include <reflow-controller/oven-driver.h>
#include <reflow-controller/temp-converter.h>
#include <reflow-controller/adc-meas.h>
#include <reflow-controller/digio.h>

#include <reflow-controller/safety/safety-controller.h>

static struct tpe_exec_state IN_SECTION(.ccm.data) current_state = {
		.status = TPE_OFF,
		.start_timestamp = 0,
		};
static bool IN_SECTION(.ccm.bss) pid_should_run;
struct pid_controller IN_SECTION(.ccm.bss) pid;
bool IN_SECTION(.ccm.bss) cmd_continue;

static SlList *command_list = NULL;

static void tpe_abort(void)
{
	temp_profile_executer_stop();
	current_state.status = TPE_ABORT;
}

enum pl_ret_val temp_profile_executer_start(const char *filename)
{
	uint32_t parsed_count = 0;
	enum pl_ret_val res;

	current_state.setpoint = 0.0f;
	current_state.start_timestamp = 0ULL;
	current_state.setpoint = 0.0f;
	current_state.step = 0;
	current_state.profile_steps = 0;

	oven_pid_stop();
	pid_should_run = false;
	current_state.status = TPE_OFF;
	current_state.profile_steps = 0;
	cmd_continue = false;

	/* This should never happen... But who knows */
	if (command_list) {
		temp_profile_free_command_list(&command_list);
	}

	res = temp_profile_parse_from_file(filename, &command_list, MAX_PROFILE_LENGTH, &parsed_count);
	if (res == PL_RET_SUCCESS) {
		current_state.profile_steps = parsed_count;
		current_state.status = TPE_RUNNING;
		current_state.start_timestamp = systick_get_global_tick();
	} else {
		if (command_list)
			temp_profile_free_command_list(&command_list);
	}

	return res;
}

static bool cmd_wait_temp(struct pl_command *cmd, bool cmd_continue)
{
	(void)cmd_continue;
	float resistance;
	int res;
	float temp;

	res = adc_pt1000_get_current_resistance(&resistance);
	if (res < 0) {
		tpe_abort();
		return false;
	}

	(void)temp_converter_convert_resistance_to_temp(resistance, &temp);

	if (ABS(cmd->params[0] - temp) < 3.0f)
		return true;

	return false;
}

static bool cmd_wait_time(struct pl_command *cmd, bool cmd_continue)
{
	static uint64_t temp_tick = 0UL;

	if (cmd_continue) {
		if (systick_ticks_have_passed(temp_tick,
					      (uint64_t)(cmd->params[0] * 1000.0f)))
			return true;
	} else {
		temp_tick = systick_get_global_tick();
	}

	return false;
}

static void reactivate_pid_if_suspended(void)
{
	if (oven_pid_get_status() == OVEN_PID_DEACTIVATED)
		oven_pid_init(&pid);

	pid_should_run = true;
}

static bool cmd_set_temp(struct pl_command *cmd)
{
	reactivate_pid_if_suspended();
	oven_pid_set_target_temperature(cmd->params[0]);
	current_state.setpoint = cmd->params[0];
	return true;
}

static bool cmd_ramp(struct pl_command *cmd, bool cmd_continue)
{
	static uint64_t IN_SECTION(.ccm.bss) start_timestamp;
	static float IN_SECTION(.ccm.bss) start_temp;
	static float IN_SECTION(.ccm.bss) slope;
	float secs_passed;
	bool ret = false;

	if (!cmd_continue) {
		/* Init of command */
		start_temp = current_state.setpoint;
		slope = (cmd->params[0] - start_temp) / cmd->params[1];
		reactivate_pid_if_suspended();
		oven_pid_set_target_temperature(start_temp);
		start_timestamp = systick_get_global_tick();
	} else {
		secs_passed = ((float)(systick_get_global_tick() - start_timestamp)) / 1000.0f;
		if ((current_state.setpoint <= cmd->params[0] && start_temp < cmd->params[0]) ||
				(current_state.setpoint >= cmd->params[0] && start_temp > cmd->params[0])) {
			current_state.setpoint = start_temp + secs_passed * slope;
		} else {
			current_state.setpoint = cmd->params[0];
			ret = true;
		}
		oven_pid_set_target_temperature(current_state.setpoint);
	}

	return ret;
}

/**
 * @brief Try to acknowledge all set flags.
 */
static void cmd_ack_flags(void)
{
	uint32_t flag_cnt;
	uint32_t i;
	enum safety_flag flag_enum;
	bool status;

	flag_cnt = safety_controller_get_flag_count();
	for (i = 0; i < flag_cnt; i++) {
		safety_controller_get_flag_by_index(i, &status, &flag_enum);
		if (status)
			(void)safety_controller_ack_flag(flag_enum);
	}

}

static void cmd_digio_conf(uint8_t digio_num, uint8_t mode)
{
	uint8_t pin_mode;
	uint8_t alt_func = 0;

	if (mode == 0 || mode == 1) {
		pin_mode = mode;
	} else if (mode >= 0x80 && mode <= 0x87) {
		/* Alternate function */
		pin_mode = 2;
		alt_func = mode - 0x80;
	} else {
		return;
	}

	digio_setup_pin(digio_num, pin_mode, alt_func);
}

bool cmd_digio_wait(uint8_t digio_num, uint8_t digio_state)
{
	bool advance = false;
	int res;

	res = digio_get(digio_num);
	if (res < 0 || (uint8_t)res == digio_state)
		advance = true;

	return advance;
}

int temp_profile_executer_handle(void)
{
	struct pl_command *current_cmd;
	static uint64_t last_tick = 0UL;
	bool advance;

	uint32_t next_step;


	/* Return if no profile is currently executed */
	if (current_state.status != TPE_RUNNING)
		return -1;

	/* Abort profile execution if oven PID is aborted. This is most likely due to some error flags */
	if (oven_pid_get_status() == OVEN_PID_ABORTED && pid_should_run) {
		tpe_abort();
		oven_pid_stop();
		return -1;
	}

	/* Execute Temp Profile every 100 ms. If not yet due, return */
	if (!systick_ticks_have_passed(last_tick, 100))
		return 0;

	current_cmd = (struct pl_command *)sl_list_nth(command_list, current_state.step)->data;
	next_step = current_state.step;

	switch (current_cmd->cmd) {
	case PL_WAIT_FOR_TIME:
		advance = cmd_wait_time(current_cmd, cmd_continue);
		break;
	case PL_WAIT_FOR_TEMP:
		advance = cmd_wait_temp(current_cmd, cmd_continue);
		break;
	case PL_SET_TEMP:
		advance = cmd_set_temp(current_cmd);
		break;
	case PL_LOUDSPEAKER_SET:
		loudspeaker_set((uint16_t)current_cmd->params[0]);
		advance = true;
		break;
	case PL_OFF:
		oven_pid_stop();
		pid_should_run = false;
		advance = true;
		break;
	case PL_PID_CONF:
		pid_init(&pid, current_cmd->params[0], /* Kd */
				current_cmd->params[1], /* Ki */
				current_cmd->params[2], /* Kp */
				0.0f, 0.0f,
				current_cmd->params[3], /* Int max */
				current_cmd->params[4], /* Kd tau */
				current_cmd->params[5]); /* Period */
		oven_pid_init(&pid);
		advance = true;
		pid_should_run = true;
		break;
	case PL_SET_RAMP:
		advance = cmd_ramp(current_cmd, cmd_continue);
		break;
	case PL_CLEAR_FLAGS:
		cmd_ack_flags();
		advance = true;
		break;
	case PL_DIGIO_CONF:
		advance = true;
		cmd_digio_conf((uint8_t)current_cmd->params[0], (uint8_t)current_cmd->params[1]);
		break;
	case PL_DIGIO_SET:
		digio_set((uint8_t)current_cmd->params[0], current_cmd->params[1] ? 1u : 0u);
		advance = true;
		break;
	case PL_DIGIO_WAIT:
		advance = cmd_digio_wait((uint8_t)current_cmd->params[0], current_cmd->params[1] ? 1u : 0u);
		break;
	default:
		tpe_abort();
		advance = true;
		break;
	}

	if (advance)
		next_step++;

	if (next_step != current_state.step) {
		current_state.step = next_step;
		if (next_step >= current_state.profile_steps) {
			(void)temp_profile_executer_stop();
		} else {
			cmd_continue = false;
		}
	} else {
		cmd_continue = true;
	}
	last_tick = systick_get_global_tick();

	return 0;
}

const struct tpe_exec_state *temp_profile_executer_status(void)
{
	return &current_state;
}

int temp_profile_executer_stop(void)
{
	if (current_state.status == TPE_RUNNING) {
		current_state.status = TPE_OFF;
		oven_pid_stop();
	}

	/* Free the command list */
	if (command_list)
		temp_profile_free_command_list(&command_list);

	/* Reset loudspeaker and reset default state of DIGIO channels */
	loudspeaker_set(0);
	digio_set_default_values();

	return 0;
}

/** @} */