/** 
 * \file axis.c
 * \brief axis source file
 * \details File contains functions definitions for one axis
 *
 * \author Juraj Golej
 * \version 1.0
 */

#include "axis.h"

void AXIS_freqReg(struct TIMER_module* const module)
{
	struct AXIS_app* axis = (struct AXIS_app*)module;

	/* Calculate slope direction */
	int32_t slopeDirect = sign(axis->frequency.required - axis->frequency.actual);

	/* If slope direction is 1 or -1, make calculation. Else stop timer (calculations are not required) */
	if(slopeDirect) {
		if(abs(axis->frequency.required - axis->frequency.actual) <= axis->frequency.constant) {
			if(axis->frequency.required == 0 && axis->mode == AXIS_MODE_MOVEFREE) {
				AXIS_stop(axis);
				return;
			}
			/* If frequency is near required, set this frequency to actual frequency */
			axis->frequency.actual = axis->frequency.required;
		}
		else {
			/* Calculate new frequency */
			axis->frequency.actual += slopeDirect * axis->frequency.constant;
		}

		/* Calculate period register */
		uint32_t periodRegister = 0xFFFFul;
		if(axis->frequency.actual != 0) {
			periodRegister = PWM_getClkFreq(&axis->pwm) / ((abs(axis->frequency.actual) << 1));		// << 1 for center alignment
			if(periodRegister > 0xFFFFul) {
				periodRegister = 0xFFFFul;
			}
		}

		/* Control direction pin */
		if((axis->frequency.actual > 0) ^ axis->module.inverse) {
			PIO_setpin_low(axis->module.dirPin);
		}
		else {
			PIO_setpin_high(axis->module.dirPin);
		}

		/* Set parameters and start PWM */
		PWM_setParameters(&axis->pwm, (uint16_t)periodRegister, (uint16_t)periodRegister >> 1);
		if(!PWM_isEnabled(&axis->pwm)) {
			PWM_start(&axis->pwm);
		}
	}
	else {
		TIMER_stop(&axis->timer);
	}
}