#include "train.h"
#include <stdlib.h>

static volatile unsigned char *_port = &PORTD;

Train::Train(const uint8_t address, uint8_t functionmask):  _address(address), _functionmask(functionmask)
{
}

Train::Train()
{
    _address = 0;
}

void Train::setAddress(const uint8_t address)
{
    _address = address;
}

uint8_t Train::getAddress()
{
    return _address;
}

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

void Train::off()
{
    writePin(_port, _pinHighA, false);
    writePin(_port, _pinHighB, false);
    writePin(_port, _pinLowA,  true);
    writePin(_port, _pinLowB,  true);
}

void Train::setOutput(const uint8_t state)
{
    if(state == HIGH)
    {
        off();
        _delay_us(3);
        writePin(_port, _pinHighA, true);
        writePin(_port, _pinLowB,  false);
    }
    else if (state == LOW)
    {
        off();
        _delay_us(3);
        writePin(_port, _pinHighB, true);
        writePin(_port, _pinLowA,  false);
    }
    else
    {
        off();
    }
}

void Train::sendBit(const bool bit)
{
    if(bit)
    {
        setOutput(HIGH);
        _delay_us(170);
        setOutput(LOW);
        _delay_us(25);
    }
    else
    {
        setOutput(HIGH);
        _delay_us(20);
        setOutput(LOW);
        _delay_us(175);
    }
}

void Train::sendAddress(uint8_t address)
{
    for(uint8_t i = 0; i < 8; i++) 
    {
        sendBit(address & (1 << i));
    }
}


void Train::sendRawAddr(const uint16_t address, const uint16_t data)
{
    for(uint8_t j = 0; j < SEND_COUNT; j++)
    {
        sendAddress(address);
        for(uint8_t i = 0; i < 10; i++)
        {
            sendBit(data & (1 << i));
        }
        _delay_ms(1);
    }
}

void Train::sendRaw(const uint16_t data)
{
    sendRawAddr(_address, data);
}

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(uint8_t speed)
{
    _speed = speed;
    sendData();
}

uint8_t Train::getSpeed()
{
    return _speed;
}

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

void Train::sendData()
{
    sendRaw(assemblePacket());
	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));
		}
		_function &= ~0xF0;
		_function |= functionToResend << 4;
	}
}

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

void Train::reverse()
{
    _direction = !_direction;
    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);
    sendData();
}