#include <avr/interrupt.h>
#include <stddef.h>

#include "dcc.h"
#include "debounce.h"
#include "millis.h"
#include "pins.h"

volatile bool g_request_transmit = false;
volatile bool g_transmitting_packet = false;
unsigned char g_packet[DCC_PACKET_LENGTH];
size_t g_packet_length;
unsigned char g_transmit_bit;
bool g_next_bit = false;
// Allow mirroring of the DCC signal.
bool g_phase = false;
unsigned char g_transmit_repeat = 0;

void DCC_Setup()
{
    HERMES_DDR_SIGNAL = (unsigned char)(
            HERMES_DDR_SIGNAL |
            ((1 << HERMES_DD_BLOCK) | (1 << HERMES_DD_COMMON))
            );
    HERMES_PORT_SIGNAL = (unsigned char)(
            HERMES_PORT_SIGNAL &
            ~((1 << HERMES_BIT_BLOCK) | (1 << HERMES_BIT_COMMON))
            );
}

void DCC_SignalUpdateIsr()
{
    TCNT2 = 0;
    // The NMRA specifies that each half of a zero bit should be at least 100us
    // and that each half of a one bit should be between 55 and 61us (nominally
    // 58us).
    static const unsigned char ZERO_TIME = 112;
    static const unsigned char ONE_TIME = 57;

    HERMES_PORT_SIGNAL = (unsigned char)(
            HERMES_PORT_SIGNAL ^
                ((1 << HERMES_BIT_COMMON) | (1 << HERMES_BIT_BLOCK))
            );

    OCR2A = g_next_bit ? ONE_TIME : ZERO_TIME;

    if(HERMES_PORT_SIGNAL & (g_phase ? (1 << HERMES_BIT_COMMON) : (1 << HERMES_BIT_BLOCK)))
    {
        // Common signal is high.

        // Second half of bit.  Set g_next_bit.

        if(g_transmitting_packet && g_transmit_bit > g_packet_length)
        {
            // Finish this packet.
            g_next_bit = false;
            --g_transmit_repeat;
            if(g_transmit_repeat == 0)
                // Stop transmitting data.
                g_transmitting_packet = false;
            else
                // Retransmit the packet.
                g_transmit_bit = 0;
        }
        else if(g_transmitting_packet)
        {
            // Continue this packet.
            const size_t byte_index = g_transmit_bit / 8;
            const size_t bit_index = g_transmit_bit % 8;
            g_next_bit = (g_packet[byte_index] >> (7 - bit_index)) & 0b00000001;
            ++g_transmit_bit;
        }
        else if(g_request_transmit)
        {
            // Hack: mirror the DCC signal for alternate packets.
            // Some locomotives won't respond to packets if they are railed in
            // the wrong direction.  Mirroring the packet solves this, but each
            // packet must be broadcast twice.
            g_phase = !g_phase;
            // Start transmitting a packet.
            g_transmit_bit = 0;
            g_next_bit = true;
            g_transmitting_packet = true;
            g_request_transmit = false;
        }
    }
}

void DCC_Start()
{
    cli();

    HERMES_DDR_SIGNAL |= (unsigned char)(1 << HERMES_DD_COMMON);
    HERMES_DDR_SIGNAL |= (unsigned char)(1 << HERMES_DD_BLOCK);
    HERMES_PORT_SIGNAL |= (unsigned char)(1 << HERMES_BIT_COMMON);
    HERMES_PORT_SIGNAL &= (unsigned char)~(1 << HERMES_BIT_BLOCK);

    TCNT2 = 0;
    // Set OC2A on compare match.
    TCCR2A = (unsigned char)((1 << COM2A1) | (1 << COM2A0));
    // Set the prescaler to /8.
    TCCR2B = (unsigned char)(1 << CS21);
    // Clear the timer 2 compare interrupts.
    TIFR2 = (unsigned char)(TIFR2 & ~(1 << OCF2A));
    TIFR2 = (unsigned char)(TIFR2 & ~(1 << OCF2B));
    // Enable timer 2 compare A interrupt (switching DCC signal).
    TIMSK2 = (unsigned char)(TIMSK2 | (1 << OCIE2A));

    /*PORTB = (unsigned char)(PORTB | (1 << PB1));*/

    sei();
}

void DCC_Stop()
{
    DCC_AwaitCompletion();
    cli();
    TCNT2 = 0;
    TIFR2 = (unsigned char)(TIFR2 & ~((1 << OCF2A) | (1 << OCF2B)));
    // Cancel timer 2 compare interrupts.
    TIMSK2 = (unsigned char)(TIMSK2 & ~((1 << OCIE2A) | (1 << OCIE2B)));
    HERMES_PORT_SIGNAL = (unsigned char)(
            HERMES_PORT_SIGNAL &
                ~((1 << HERMES_BIT_COMMON) | (1 << HERMES_BIT_BLOCK))
            );
    // Make PB1 and PB2 high-impedence.
    /*HERMES_DDR_SIGNAL = (unsigned char)(*/
            /*HERMES_DDR_SIGNAL &*/
                /*~((1 << HERMES_DD_COMMON) | (1 << HERMES_DD_BLOCK))*/
            /*);*/
    sei();
}

void DCC_Transmit(const packet p)
{
    DCC_TransmitRepeat(p, 2);
}

void DCC_TransmitRepeat(const packet p, unsigned char repeat)
{
    // Wait for transmission of a previous packet to end.
    while(g_transmitting_packet);
    g_transmit_repeat = repeat;
    // Clear the outgoing packet buffer.
    for(int i = 0; i < DCC_PACKET_LENGTH; ++i)
        g_packet[i] = 0;
    // Set each bit in the outgoing buffer, up to the packet length.
    Packet_RawData(g_packet, &p);
    g_packet_length = Packet_RawLength(&p);
    // Signal to the interrupt routine that a new packet is ready.
    g_request_transmit = true;
    // Wait for the interrupt routine to accept the packet, setting
    // g_transmitting_packet.  Otherwise, this routine could be called,
    // attempting to transmit a packet while one is waiting.
    while(g_request_transmit);
}

void DCC_AwaitCompletion()
{
    while(g_transmitting_packet);
}

void DCC_TransmitPagePresetSequence()
{
    for(unsigned char i = 0; i < 5; ++i)
        DCC_Transmit(Packet_CreateReset());
    for(unsigned char i = 0; i < 15; ++i)
        DCC_Transmit(Packet_CreatePagePreset());
}

void DCC_AutoCvWrite(cv_type cv, unsigned char value)
{
    if(cv > 0 && cv < 5)
        DCC_RegisterCvWrite((unsigned char)cv, value);
    if(cv == 29)
        DCC_RegisterCvWrite(5, value);

    if(cv >= 5)
        DCC_PagedCvWrite(cv, value);
}

void DCC_DirectCvWrite(cv_type address, unsigned char data)
{
    // Move address to the 0-1023 address space.
    address -= 1;
    DCC_Start();
    // Idle packets, decoder power on time.
    // NMRA specs require 20 packets.
    DCC_TransmitRepeat(Packet_CreateIdle(), 100);
    // 3 or more reset packets, required by NMRA specs.
    DCC_TransmitRepeat(Packet_CreateReset(), 15);
    // 5 or more writes to a single CV, followed by 6 or more identical write
    // or reset packets, required by NMRA specs.
    DCC_TransmitRepeat(
        Packet_CreateServiceMode3(
            (unsigned char)(0b01111100 | ((address >> 8) & 0b00000011)),
            (unsigned char)(address & 0b11111111),
            data
            ),
        15
        );
    DCC_TransmitRepeat(Packet_CreateReset(), 15);
    // Allow the decoder plenty of time to interpret the instruction.
    DCC_TransmitRepeat(Packet_CreateIdle(), 100);
    DCC_Stop();
}

void DCC_PagedCvWrite(cv_type address, unsigned char value)
{
    address -= 1;
    DCC_Start();
    DCC_TransmitRepeat(Packet_CreateIdle(), 100);
    unsigned char page = (unsigned char)((address / 4) + 1);
    unsigned char reg = (unsigned char)(address % 4);
    DCC_TransmitRepeat(Packet_CreateReset(), 5);
    DCC_TransmitRepeat(Packet_CreateServiceMode2(0b01111101, page), 15);
    DCC_TransmitRepeat(Packet_CreateReset(), 15);
    DCC_TransmitRepeat(Packet_CreateServiceMode2(0b01111000 | reg, value), 15);
    DCC_AwaitCompletion();
    // Allow the decoder plenty of time to interpret the instruction.
    DCC_TransmitRepeat(Packet_CreateIdle(), 100);
    DCC_Stop();
}

void DCC_RegisterCvWrite(unsigned char reg, unsigned char value)
{
    reg = (unsigned char)(reg - 1);
    DCC_Start();
    DCC_TransmitRepeat(Packet_CreateIdle(), 100);
    DCC_TransmitPagePresetSequence();
    DCC_TransmitRepeat(Packet_CreateReset(), 5);
    DCC_TransmitRepeat(
        Packet_CreateServiceMode2(
            (unsigned char)(0b01111000 | (reg & 0b00000111)),
            value
            ),
        15
        );
    DCC_AwaitCompletion();
    // Allow the decoder plenty of time to interpret the instruction.
    DCC_TransmitRepeat(Packet_CreateIdle(), 100);
    DCC_Stop();
}

void DCC_HardReset()
{
    /*
     * The NMRA specs state that the decoder should reset when a value of 8 is
     * written to CV #8.  This is equivalent to the instruction 0b01111111,
     * 0b00001000.
     *
     * On older decoders, setting another CV will reset the decoder.
     */
    DCC_RegisterCvWrite(8, 8);
}