#include "train.h"
#include <stdlib.h>
#include <avr/interrupt.h>

Train::Train(const uint8_t address, uint8_t functionmask, uint8_t quirks):
Item(address), _functionmask(functionmask), _quirks(quirks)
{
}

uint16_t Train::packetAddSpeed()
{
	uint16_t packet = 0;
	if(_speed > 14)
		_speed = 14;
	for(uint8_t i = 0; i < 4; i++)
	{
		packet |= ((bool)((_speed+1) & (1 << i))) << (i*2+2);
	}
	return packet;
}

uint16_t Train::packetAddDirection()
{
	uint16_t packet = 0;

	uint8_t data = 0;
	if(!_direction)
	{
		if(_speed > 6)
			data = 0b0100;
		else if(_speed > 0)
			data = 0b0101;
	}
	else
	{
		if(_speed > 0)
			data = 0b1011;
		else if (_speed > 6)
			data = 0b1010;
	}

	if(_speed == 0)
		data = !_direction ?  0b0101 : 0b1011;

	for(uint8_t i = 0; i < 4; i++)
	{
		uint8_t bit = (data & (1 << (3-i))) >> (3-i);
		packet |= bit << ((i*2)+3);
	}
	return packet;
}

uint16_t Train::packetAddFunction(const uint8_t function)
{
	uint16_t packet = 0;
	bool enabled = _function & (1 << function);
	if(function == 0)
	{
		packet |= enabled ? 0b00000011 : 0b00000000;
	}
	else
	{
		if(enabled)
			packet |=  0b1000000000;
		for(uint8_t i = 0; i < 4; ++i )
		{
			if(function > i)
			packet = packet |  (1 << (7-i*2));
			else
			packet = packet & ~(1 << (7-i*2));
		}
	}
	return packet;
}

void Train::setSpeed(int8_t speed)
{
	bool direction = _direction;

	if(_quirks & (1 << 1))
		speed = 0-speed;

	if(speed < 0)
		_direction = true;
	else if(speed > 0)
		_direction = false;
	
	_speed = abs(speed);
	
	if((_quirks & 1) && direction != _direction)
	{
		cli();
		for(uint8_t i = 0; i < 2; ++i)
		{
			sendRaw(0x28c);
			_delay_ms(4);
		}
		if(!_direction)
		{
			for(uint8_t i = 0; i < 2; ++i)
			{
				sendRaw(0x224);
				_delay_ms(4);
			}
		}
		sei();
	}
	else if(!directSendBlock)
	{
		sendData();
	}
}

int8_t Train::getSpeed()
{
	int8_t speed = _direction ? 0-_speed : _speed; 
	return _quirks & (1 << 1) ? 0-speed : speed;
}

void Train::stop()
{
	_speed = 0;
	_function = 0;
	if(!directSendBlock)
		sendData();
}


uint16_t Train::assembleSpeedPacket()
{
	uint16_t packet = packetAddSpeed() | packetAddFunction(0) | packetAddDirection();
	return packet;
}

void Train::sendData(bool single)
{
	sendRaw(assembleSpeedPacket(), single);
	if(_functionmask)
	{
		uint8_t functionToResend = (_function & 0xF0) >> 4;
		++functionToResend;
		if (functionToResend == 4)
			functionToResend = 1;
		if(_functionmask & (1 << functionToResend))
		{
			_delay_ms(2);
			sendRaw(packetAddSpeed() | packetAddFunction(functionToResend) | packetAddFunction(0), single);
		}
		_function &= ~0xF0;
		_function |= functionToResend << 4;
	}
}

uint16_t Train::getLastPacket()
{
	return assembleSpeedPacket();
}

void Train::reverse()
{
	_direction = !_direction;
	if(!directSendBlock)
		sendData();
}


uint8_t Train::getFunctions()
{
	return _function & 0x0F;
}
	
uint8_t Train::getFunctionMask()
{
	return _functionmask;
}

void Train::sendFunction(const uint8_t function, bool enable)
{
	if(function > 3)
		return;
	_function = (_function & ~(1 << function)) | (enable << function);
	if(!directSendBlock)
		sendData();
}

uint8_t Train::getQuirks()
{
	return _quirks;
}

void Train::setQuirks(uint8_t quirks)
{
	_quirks = quirks;
}