#include "train.h"

static volatile unsigned char *_port = &PORTD;

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

Train::Train()
{
    writePin(_port, _pinHigh, false);
    writePin(_port, _pinLow, true);
    _address = 0;
}

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

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

void Train::setOutput(const uint8_t state)
{
    if(state == HIGH)
    {
        writePin(_port, _pinLow, true);
        _delay_us(5);
        writePin(_port, _pinHigh, true);
    }
    else if (state == LOW)
    {
        writePin(_port, _pinHigh, false);
        _delay_us(5);
        writePin(_port, _pinLow, false);
    }
    else
    {
        writePin(_port, _pinHigh, false);
        writePin(_port, _pinLow, true);
    }
}

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::sendData(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));
            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 uint8_t function)
{
    sendData((function << 1) | 1);
}