/** 
 * \file tool.c
 * \brief tool source file
 * \details File contains functions definitions for Axis Z stepper motor controller
 *
 * \author Juraj Golej
 * \version 1.0
 */

#include "tool.h"

struct TOOL_app {
	struct AXIS_app axisZ;
	tool_move_end callback;
};

struct TOOL_app tool;

void _TOOL_moveToCBFUNC(struct PWM_module* const module)
{
	/* Increment/decrement absolute value */
	tool.axisZ.position.absolute += sign(tool.axisZ.frequency.actual);

	/* Decrement required steps */
	--tool.axisZ.position.incSteps;

	/* If is time to slow down */
	if(tool.axisZ.position.incSteps == tool.axisZ.position.rampSteps) {
		tool.axisZ.frequency.required = 0;
		TIMER_start(&tool.axisZ.timer, TIMER_MODE_REPEAT);
	}

	/* Stop PWM before the last impulse (the impulse is still coming) */
	if(tool.axisZ.position.incSteps == 1) {
		PWM_stop(&tool.axisZ.pwm);
	}
	else if(tool.axisZ.position.incSteps == 0) {
		tool.axisZ.mode = AXIS_MODE_NONE;
		tool.axisZ.frequency.actual = 0;
		if(tool.callback) {
			tool.callback();
		}
	}
}

void _TOOL_moveFreeCBFUNC(struct PWM_module* const module)
{
	if(!PIO_pin_level(tool.axisZ.module.stepPin)) {
		tool.axisZ.position.absolute += sign(tool.axisZ.frequency.actual);
	}
}

enum retCode TOOL_init()
{
	/* Init PWM and sample TIMER */
	Try(PWM_init(&tool.axisZ.pwm, TOOL_CFG_PWM, TOOL_CFG_PWMCH, TOOL_CFG_PWMCLK), EInvalidParameter);
	Try(TIMER_init(&tool.axisZ.timer, TOOL_CFG_TIMER, TOOL_CFG_TIMERCH, TOOL_CFG_TIMERCLK, TOOL_CFG_TIMERTICK), EInvalidParameter);
	PWM_setAlignment(&tool.axisZ.pwm, PWM_ALIG_CENTER);
	TIMER_registerCallback(&tool.axisZ.timer, AXIS_freqReg);

	/* Set variables */
	tool.axisZ.mode = AXIS_MODE_NONE;
	tool.axisZ.module.stepPin = TOOL_CFG_PWMPIN;
	tool.axisZ.module.dirPin = TOOL_CFG_DIRPIN;
	tool.axisZ.module.inverse = TOOL_CFG_INVERSE;
	tool.axisZ.conv.microstep = TOOL_CFG_MICROSTEP;
	tool.axisZ.conv.rev = TOOL_CFG_ONEREV;
	tool.axisZ.conv.pitch = TOOL_CFG_PITCH;
	AXIS_calculateConvConst(&tool.axisZ);
	tool.axisZ.frequency.acceleration = (float)TOOL_CFG_ACCEL * tool.axisZ.conv.constant;
	tool.axisZ.frequency.constant = ((float)tool.axisZ.frequency.acceleration * (float)TOOL_CFG_TIMERTICK) / 1000000;
	tool.axisZ.maxPos = TOOL_CFG_MAXPOS;
	tool.axisZ.maxSpeed = TOOL_CFG_MAXSPEED;

	return ENone;
}

enum retCode TOOL_moveFree(const int16_t speed)
{
	/* Check, if speed is lower than maximum */
	Try(abs(speed) > tool.axisZ.maxSpeed, EInvalidParameter);

	/* Check, if mode is FREE or NONE */
	Try(tool.axisZ.mode == AXIS_MODE_MOVETO, EDenied);

	/* Calculate frequency */
	tool.axisZ.frequency.required = (float)speed * tool.axisZ.conv.constant;

	/* Set move free position regulator */
	PWM_registerCallback(&tool.axisZ.pwm, _TOOL_moveFreeCBFUNC);

	/* Start timer */
	if(!TIMER_isBusy(&tool.axisZ.timer)) {
		AXIS_start(&tool.axisZ, AXIS_MODE_MOVEFREE);
	}
	
	return ENone;
}

enum retCode TOOL_moveToAbs(const float pos, const uint16_t speed)
{
	/* Check, if mode is NONE */
	Try(tool.axisZ.mode != AXIS_MODE_NONE, EDenied);

	/* Check, if required position is in range of machine */
	Try(pos < 0 || pos > tool.axisZ.maxPos, EInvalidParameter);

	/* Check, if the speed is lower than maximum */
	Try(speed > tool.axisZ.maxSpeed, EInvalidParameter);

	/* Calculate difference */
	int32_t dif = (int32_t)(pos * tool.axisZ.conv.constant) - tool.axisZ.position.absolute;

	/* Check, if difference is higher than zero */
	if(!dif) {
		if(tool.callback) {
			tool.callback();
		}
		return ENone;
	}

	/* Calculate frequency */
	tool.axisZ.frequency.required = (int32_t)((float)speed * tool.axisZ.conv.constant) * sign(dif);

	/* Calculate steps */
	tool.axisZ.position.incSteps = abs(dif);

	/* Calculate ramp (deceleration) steps */
	tool.axisZ.position.rampSteps = ((tool.axisZ.frequency.required * tool.axisZ.frequency.required) / tool.axisZ.frequency.acceleration) >> 1;
	if(tool.axisZ.position.rampSteps > (tool.axisZ.position.incSteps >> 1)) {
		tool.axisZ.position.rampSteps = (tool.axisZ.position.incSteps >> 1);
	}

	/* Start timer */
	PWM_registerCallback(&tool.axisZ.pwm, _TOOL_moveToCBFUNC);
	AXIS_start(&tool.axisZ, AXIS_MODE_MOVETO);

	return ENone;
}

enum retCode TOOL_moveToInc(const float pos, const uint16_t speed)
{
	return TOOL_moveToAbs(AXIS_getAbsPos(&tool.axisZ) + pos, speed);
}

struct AXIS_app* TOOL_getAxis()
{
	return &tool.axisZ;
}

void TOOL_registerCallback(tool_move_end callback)
{
	tool.callback = callback;
}
