#include "train.h"

static volatile unsigned char *_port = &PORTD;

Train::Train(const uint8_t address, uint8_t protocol):  _address(address), _protocol(protocol)
{
}

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

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

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

void Train::stop()
{
    lastDataPacket = 0;
    resendData();
}

bool Train::isActive()
{
    return lastDataPacket & 0b0000000111111111;
}


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()
{
    for(uint8_t i = 0; i < 8; i++) 
    {
        sendBit(_address & (1 << i));
    }
}

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

void Train::setSpeed(uint8_t speed)
{
    if(speed != 0)speed = speed + 1;
    else if(speed > 15) speed = 15;
    for(uint8_t i = 0; i < 4; i++)
    {
                                 lastDataPacket = (lastDataPacket & ~(1 << (i+1)*2))     | (((uint16_t)speed & (1 << i)) << (i+1)*2-i);
        if(_protocol == M_DELTA) lastDataPacket = (lastDataPacket & ~(1 << ((i+1)*2+1))) | (((uint16_t)speed & (1 << i)) << ((i+1)*2+1-i));
    }
    resendData();
}

uint8_t Train::getSpeed()
{
    uint8_t speed = 0;
    for(uint8_t i = 0; i < 4; i++)
        speed |= ((bool)(lastDataPacket & (1 << (i+1)*2))) << i;
    return speed;
}

void Train::resendData()
{
    sendRaw(lastDataPacket);
    if(functionTimer > 0)
    {
        --functionTimer;
    }
    else if(functionTimer == 0)
    {
        functionClear();
        --functionTimer;
    }
}

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

void Train::reverse()
{
    functionClear();
    resendData();
    resendData();
    if(getSpeed() > 0)
    {
        setSpeed(0);
        resendData();
        resendData();
        _delay_ms(2000);
    }
    sendRaw(0b000001100);
    sendRaw(0b000001100);
}

void Train::functionClear()
{
	lastDataPacket = lastDataPacket & ~(0b10101010);
}

void Train::sendFunction(const uint8_t function, bool enable)
{
    if(function == 0)
    {
        lastDataPacket = (lastDataPacket & ~0b00000001) | (enable ? 0b00000001 : 0b00000000);
        //lastDataPacket = (lastDataPacket & ~0b1100000000) | (enable ? 0b1100000000 : 0);
    }
    else if(_protocol == M_DIGITAL && function <= 3)
    {
        if(enable)
            lastDataPacket |=  0b1000000000;
        else
            lastDataPacket &= ~0b1000000000;
        for(uint8_t i = 0; i < 4; ++i )
        {
            if(function > i)
              lastDataPacket = lastDataPacket |  (1 << (7-i*2));
            else
              lastDataPacket = lastDataPacket & ~(1 << (7-i*2));
        }
    }
    resendData();
    functionTimer = 10;
}

void Train::setProtocol(const uint8_t protocol)
{
   _protocol = protocol;
}

uint8_t Train::getProtocol()
{
    return _protocol;
}