#include "train.h"

static volatile unsigned char *_port = &PORTD;

Train::Train(const uint8_t address):  _address(address)
{
}

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

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

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

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(5);
        writePin(_port, _pinHighA, true);
        writePin(_port, _pinLowB,  false);
    }
    else if (state == LOW)
    {
        off();
        _delay_us(5);
        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 uint8_t data)
{
    for(uint8_t j = 0; j < SEND_COUNT; j++)
    {
        sendAddress();
        for(uint8_t i = 0; i < 5; i++)
        {
            sendBit(data & (1 << i));
        }
        _delay_ms(2);
    }
}

void Train::setSpeed(uint8_t speed)
{
    if(speed > 0) ++speed;
    if(speed <= 15)
    {
        lastSpeed = speed;
        sendData((speed << 1));
    }
}

void Train::resendSpeed()
{
    uint8_t data = lastSpeed;
    sendData((data << 1));
}

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

void Train::reverse()
{
    sendData((1 << 1));
}

void Train::sendFunction(const uint16_t function)
{
    sendDataUndoubled(function);
}