/** 
 * \file path.c
 * \brief path source file
 * \details File contains functions definitions for PATH regulators
 *
 * \author Juraj Golej
 * \version 1.0
 */

#include "path.h"

struct PATH_app {
	struct AXIS_app axisX;
	struct AXIS_app axisY;
	path_move_end callback;

	struct AXIS_app* axis2;
	volatile int32_t axis1_diff;
	volatile int32_t axis2_diff;
};

struct PATH_app path;

void _PATH_moveToCBFUNC(struct PWM_module* const module)
{
	struct AXIS_app* axis = (struct AXIS_app*)((void*)module - (void*)sizeof(struct TIMER_module));

	if(!PIO_pin_level(axis->module.stepPin)) {
		PIO_setpin_low(path.axis2->module.stepPin);
	}
	else {
		if(((int32_t)axis->position.incSteps * path.axis2_diff) / path.axis1_diff != (((int32_t)axis->position.incSteps + 1) * path.axis2_diff) / path.axis1_diff) {
			path.axis2->position.absolute += sign(path.axis2_diff);
			PIO_setpin_high(path.axis2->module.stepPin);
		}

		axis->position.incSteps--;
		axis->position.absolute += sign(path.axis1_diff);

		if(axis->position.rampSteps && (axis->position.incSteps == axis->position.rampSteps)) {
			axis->frequency.required = axis->frequency.end;
			TIMER_start(&axis->timer, TIMER_MODE_REPEAT);
		}

		if(axis->position.incSteps == 0) {
			PWM_stop(&axis->pwm);
			TIMER_stop(&axis->timer);
			axis->mode = AXIS_MODE_NONE;
			if(axis->frequency.required == 0) {
				axis->frequency.actual = 0;
			}
			path.axis2->frequency.actual = (int32_t)(axis->frequency.actual * path.axis2_diff) / path.axis1_diff;
			if(path.callback) {
				path.callback();
			}
		}
	}
}

void _PATH_moveFreeCBFUNC(struct PWM_module* const module)
{
	struct AXIS_app* axis = (struct AXIS_app*)((void*)module - (void*)sizeof(struct TIMER_module));

	if(!PIO_pin_level(axis->module.stepPin)) {
		axis->position.absolute += sign(axis->frequency.actual);
	}
}

enum retCode PATH_init()
{
	/* Init PWM and sample TIMER */
	Try(PWM_init(&path.axisX.pwm, PATH_CFG_AXISX_PWM, PATH_CFG_AXISX_PWMCH, PATH_CFG_AXISX_PWMCLK), EInvalidParameter);
	Try(TIMER_init(&path.axisX.timer, PATH_CFG_AXISX_TIMER, PATH_CFG_AXISX_TIMERCH, PATH_CFG_AXISX_TIMERCLK, PATH_CFG_AXISX_TIMERTICK), EInvalidParameter);
	Try(PWM_init(&path.axisY.pwm, PATH_CFG_AXISY_PWM, PATH_CFG_AXISY_PWMCH, PATH_CFG_AXISY_PWMCLK), EInvalidParameter);
	Try(TIMER_init(&path.axisY.timer, PATH_CFG_AXISY_TIMER, PATH_CFG_AXISY_TIMERCH, PATH_CFG_AXISY_TIMERCLK, PATH_CFG_AXISY_TIMERTICK), EInvalidParameter);
	PWM_setAlignment(&path.axisX.pwm, PWM_ALIG_CENTER);
	PWM_setAlignment(&path.axisY.pwm, PWM_ALIG_CENTER);
	PWM_setInterruptEvent(&path.axisX.pwm, true);
	PWM_setInterruptEvent(&path.axisY.pwm, true);
	TIMER_registerCallback(&path.axisX.timer, AXIS_freqReg);
	TIMER_registerCallback(&path.axisY.timer, AXIS_freqReg);

	/* Set variables */
	path.axisX.mode = AXIS_MODE_NONE;
	path.axisX.module.stepPin = PATH_CFG_AXISX_PWMPIN;
	path.axisX.module.dirPin = PATH_CFG_AXISX_DIRPIN;
	path.axisX.module.inverse = PATH_CFG_AXISX_INVERSE;
	path.axisX.conv.microstep = PATH_CFG_AXISX_MICROSTEP;
	path.axisX.conv.rev = PATH_CFG_AXISX_ONEREV;
	path.axisX.conv.pitch = PATH_CFG_AXISX_PITCH;
	AXIS_calculateConvConst(&path.axisX);
	path.axisX.frequency.acceleration = (float)PATH_CFG_AXISX_ACCEL * path.axisX.conv.constant;
	path.axisX.frequency.constant = ((float)path.axisX.frequency.acceleration * (float)PATH_CFG_AXISX_TIMERTICK) / 1000000;
	path.axisX.maxPos = PATH_CFG_AXISX_MAXPOS;
	path.axisX.maxSpeed = PATH_CFG_AXISX_MAXSPEED;

	path.axisY.mode = AXIS_MODE_NONE;
	path.axisY.module.stepPin = PATH_CFG_AXISY_PWMPIN;
	path.axisY.module.dirPin = PATH_CFG_AXISY_DIRPIN;
	path.axisY.module.inverse = PATH_CFG_AXISY_INVERSE;
	path.axisY.conv.microstep = PATH_CFG_AXISY_MICROSTEP;
	path.axisY.conv.rev = PATH_CFG_AXISY_ONEREV;
	path.axisY.conv.pitch = PATH_CFG_AXISY_PITCH;
	AXIS_calculateConvConst(&path.axisY);
	path.axisY.frequency.acceleration = (float)PATH_CFG_AXISY_ACCEL * path.axisY.conv.constant;
	path.axisY.frequency.constant = ((float)path.axisY.frequency.acceleration * (float)PATH_CFG_AXISY_TIMERTICK) / 1000000;
	path.axisY.maxPos = PATH_CFG_AXISX_MAXPOS;
	path.axisY.maxSpeed = PATH_CFG_AXISY_MAXSPEED;

	path.callback = NULL;

	return ENone;
}

enum retCode PATH_moveFree(const enum PATH_AXIS direction, const int16_t speed)
{
	/* Get pointer on axis */
	struct AXIS_app* axis = PATH_getAxis(direction);

	/* Check, if speed is lower than maximum */
	Try(abs(speed) > axis->maxSpeed, EInvalidParameter);

	/* Check, if mode is FREE or NONE */
	Try(path.axisX.mode == AXIS_MODE_MOVETO || path.axisY.mode == AXIS_MODE_MOVETO, EDenied);

	/* Calculate frequency */
	axis->frequency.required = (float)speed * axis->conv.constant;

	/* Set move free position regulator */
	PWM_registerCallback(&axis->pwm, _PATH_moveFreeCBFUNC);
	AXIS_connect(axis);

	/* Start timer */
	if(!TIMER_isBusy(&axis->timer)) {
		AXIS_start(axis, AXIS_MODE_MOVEFREE);
	}
	
	return ENone;
}

enum retCode PATH_moveToAbs(const float posX, const float posY, const uint16_t speed, const uint16_t endSpeed)
{
	/* Check, if mode is NONE */
	Try(path.axisX.mode != AXIS_MODE_NONE || path.axisY.mode != AXIS_MODE_NONE, EDenied);

	/* Check, if required position is in range of machine */
	Try(posX < 0 || posX > path.axisX.maxPos || posY < 0 || posY > path.axisY.maxPos, EInvalidParameter);

	/* Calculate differences */
	int32_t dx = (int32_t)(posX * path.axisX.conv.constant) - path.axisX.position.absolute;
	int32_t dy = (int32_t)(posY * path.axisY.conv.constant) - path.axisY.position.absolute;

	/* Check, if differences are higher than zero */
	if(!(dx || dy)) {
		if(path.callback) {
			path.callback();
		}
		return ENone;
	}

	/* Set primary and secondary axis */
	struct AXIS_app* axis;
	if(abs(dx) >= abs(dy)) {
		axis = &path.axisX;
		path.axis2 = &path.axisY;
		path.axis1_diff = dx;
		path.axis2_diff = dy;
	} 
	else {
		axis = &path.axisY;
		path.axis2 = &path.axisX;
		path.axis1_diff = dy;
		path.axis2_diff = dx;
	}

	/* Check, if speed is lower than maximum */
	Try(speed > axis->maxSpeed || endSpeed > axis->maxSpeed, EInvalidParameter);

	/* Calculate frequency */
	axis->frequency.required = (int32_t)((float)speed * axis->conv.constant) * sign(path.axis1_diff);

	/* Calculate end frequency */
	axis->frequency.end = (float)endSpeed * axis->conv.constant;
	if(axis->frequency.end > abs(axis->frequency.required)) {
		axis->frequency.end = axis->frequency.required;
	}

	/* Calculate steps */
	axis->position.incSteps = abs(path.axis1_diff);

	/* Calculate ramp (deceleration) steps */
	int32_t ramp1 = ((axis->frequency.required * axis->frequency.required - axis->frequency.actual * axis->frequency.actual) / axis->frequency.acceleration) >> 1;
	int32_t ramp2 = ((axis->frequency.required * axis->frequency.required - axis->frequency.end * axis->frequency.end) / axis->frequency.acceleration) >> 1;
	int32_t middleSteps = (int32_t)axis->position.incSteps - ramp1 - ramp2;
	if(ramp2 && (middleSteps < 0)) {
		ramp2 = ((int32_t)axis->position.incSteps >> 1) + (((axis->frequency.actual * axis->frequency.actual - axis->frequency.end * axis->frequency.end) / axis->frequency.acceleration) >> 2);
		if(ramp2 < 0) {
			ramp2 = 0;
		}
	}
	
	axis->position.rampSteps = ramp2;

	/* Disconnect secondary axis and set direction */
	AXIS_disconnect(path.axis2);
	if((path.axis2_diff > 0) ^ path.axis2->module.inverse) {
		PIO_setpin_low(path.axis2->module.dirPin);
	}
	else {
		PIO_setpin_high(path.axis2->module.dirPin);
	}

	/* Connect primary axis and start timer */
	PWM_registerCallback(&axis->pwm, _PATH_moveToCBFUNC);
	AXIS_connect(axis);
	AXIS_start(axis, AXIS_MODE_MOVETO);

	return ENone;
}

enum retCode PATH_moveToInc(const float posX, const float posY, const uint16_t speed, const uint16_t endSpeed)
{
	return PATH_moveToAbs(AXIS_getAbsPos(PATH_getAxis(PATH_AXIS_X)) + posX, AXIS_getAbsPos(PATH_getAxis(PATH_AXIS_Y)) + posY, speed, endSpeed);
}

struct AXIS_app* PATH_getAxis(const enum PATH_AXIS axis)
{
	if(axis == PATH_AXIS_X) {
		return &path.axisX;
	}
	else {
		return &path.axisY;
	}
}

void PATH_registerCallback(path_move_end callback)
{
	path.callback = callback;
}
