#include "stepper.h"
#include "writepin.h"

void Stepper::tick()
{
	if(++_tickCounter > UINT16_MAX - _speed )
	{
		if(_targetStep != _currentStep)
		{
			step(_targetStep > _currentStep);
		}
		else if(endlessMove) step(endlessDriection);
		_tickCounter = 0;
	}
	/*else if(halfCurrent)
	{
		if(_tickCounter % 2 == 0) disableOutputs();
		else setOutputs();
	}*/
	if(_speed < _targetSpeed) _speed = _speed + acceleration;
	else if(_speed > _targetSpeed)_speed = _speed - acceleration;
}

void Stepper::moveTo(const int32_t step)
{
	_targetStep = step;
}

void Stepper::moveRelative(const int16_t dist)
{
    moveTo(_targetStep + dist);
}

void Stepper::setSpeed(const uint16_t speed)
{
    _targetSpeed = speed;
	if(_targetSpeed < speedFloor) _targetSpeed = speedFloor;
}

void Stepper::setEndlesMove(bool endless, bool direction)
{
	endlessMove = endless;
	if(direction != endlessDriection)
	{
		_speed = speedFloor;
		endlessDriection = direction;
	}
}

uint32_t Stepper::isAt()
{
    return _currentStep;
}
    
bool Stepper::isStoped()
{
    return _currentStep == _targetStep;
}

void Stepper::shutdown()
{
	_currentStep = _targetStep;
	_speed = 0;
	disableOutputs();
}

void Stepper::disableOutputs()
{
	writePin(_port, _pinA, false);
	writePin(_port, _pinB, false);
	writePin(_port, _pinC, false);
	writePin(_port, _pinD, false);
}

void Stepper::setOutputs()
{
	switch(_currentStep & 0x03)
	{
		case 0:
		writePin(_port, _pinA, true);
		writePin(_port, _pinB, false);
		writePin(_port, _pinC, false);
		writePin(_port, _pinD, true);
		break;
		case 1:
		writePin(_port, _pinA, true);
		writePin(_port, _pinB, true);
		writePin(_port, _pinC, false);
		writePin(_port, _pinD, false);
		break;
		case 2:
		writePin(_port, _pinA, false);
		writePin(_port, _pinB, true);
		writePin(_port, _pinC, true);
		writePin(_port, _pinD, false);
		break;
		case 3:
		writePin(_port, _pinA, false);
		writePin(_port, _pinB, false);
		writePin(_port, _pinC, true);
		writePin(_port, _pinD, true);
		break;
		default:
		writePin(_port, _pinA, false);
		writePin(_port, _pinB, false);
		writePin(_port, _pinC, false);
		writePin(_port, _pinD, false);
	}
}

void Stepper::setHalfCurrent( bool current)
{
	halfCurrent = current;
}

void Stepper::step(bool direction)
{
	_currentStep = _currentStep + (direction ? 1 : -1);
	if(endlessMove) _targetStep = _currentStep;
	setOutputs();
}