#include "packet.h"

#define DCC_REGISTER_PAGE 0b101
#define DCC_REGISTER_D0 0b000
#define DCC_REGISTER_D1 0b001
#define DCC_REGISTER_D2 0b010
#define DCC_REGISTER_D3 0b011

packet Packet_CreateEmpty()
{
    packet p = { .length = 0, .service_mode = false };
    return p;
}

packet Packet_CreateReset()
{
    return Packet_CreateServiceMode2(0, 0);
}

packet Packet_CreatePagePreset()
{
    return Packet_CreateServiceMode2(0b01111101, 0b00000001);
}

packet Packet_CreateIdle()
{
    return Packet_CreateServiceMode2(0b11111111, 0b00000000);
}

packet Packet_CreateSpeed128(
        cfg_address_type address,
        bool forward,
        unsigned char speed
        )
{
    if(speed > 0)
        ++speed;
    return Packet_Create3(
            (unsigned char)address,
            0b00111111,
            (unsigned char)((forward ? 0b10000000 : 0) | (speed & 0b01111111))
            );
}

packet Packet_CreateSpeed28(
        cfg_address_type address,
        bool forward,
        unsigned char speed
        )
{
    if(speed > 0)
        speed = (unsigned char)(speed + 3);
    unsigned char data = 0b01000000;
    if(forward)
        data |= 0b00100000;
    if(speed & 0b00000001)
        data |= 0b00010000;
    data = (unsigned char)(data | ((speed >> 1) & 0x0f));
    return Packet_Create2((unsigned char)address, data);
}

packet Packet_CreatePomAccWrite(
        cfg_address_type target_address,
        cfg_address_type cv_no,
        unsigned char cv_data
        )
{
    // Move the CV no. to the 0-1023 address space.
    --cv_no;
    return Packet_Create5(
            // First byte: 10AAAAAA.
            (unsigned char)((target_address & 0b00111111) | 0b10000000),
            // Second byte: 1AAACDDD.  When CDDD == 0000, the CV written refers
            // to the entire decoder.  The three upper address bytes in one's
            // complement.
            (unsigned char)(
                ((~(target_address & 0b111000000) >> 2) & 0b01110000) |
                0b10000000
                ),
            // Third byte: 1110CCVV.  When CC == 11, this is a 'write byte'
            // packet.  VV is the two highest bits of the CV number.
            (unsigned char)(0b11101100 | ((cv_no >> 8) & 0b00000011)),
            // Fourth byte: eight lower bits of the CV number.
            (unsigned char)(cv_no & 0b11111111),
            // Fifth byte: CV data.
            cv_data
            );
}

packet Packet_CreatePomLocoWrite(
        cfg_address_type target_address,
        cfg_address_type cv_no,
        unsigned char cv_data
        )
{
    // Move the CV no. to the 0-1023 address space.
    --cv_no;
    return Packet_Create4(
            (unsigned char)(target_address),
            (unsigned char)(0b11101100 | ((cv_no >> 8) & 0b00000011)),
            (unsigned char)(cv_no & 0b11111111),
            cv_data
            );
}

packet Packet_CreateAccessoryBasic(
        cfg_address_type address,
        unsigned char output,
        bool state
        )
{
    // Address MSB bytes (second byte):
    // "By convention these bits (bits 4-6 of
    // the second data byte) are in ones complement."
    return Packet_Create2(
            (unsigned char)((address & 0b00111111) | 0b10000000),
            (unsigned char)(
                ((~(address & 0b111000000) >> 2) & 0b01110000) |
                (state ? 0b00001000 : 0) | 0b10000000 |
                (output & 0b111)
                )
            );
}

packet Packet_Create2(unsigned char byte1, unsigned char byte2)
{
    packet out;
    out.length = 3;
    out.service_mode = false;
    out.data[0] = byte1;
    out.data[1] = byte2;
    out.data[2] = byte1 ^ byte2;
    return out;
}

packet Packet_Create3(unsigned char byte1, unsigned char byte2, unsigned char byte3)
{
    packet out;
    out.length = 4;
    out.service_mode = false;
    out.data[0] = byte1;
    out.data[1] = byte2;
    out.data[2] = byte3;
    out.data[3] = byte1 ^ byte2 ^ byte3;
    return out;
}

packet Packet_Create4(
        unsigned char byte1,
        unsigned char byte2,
        unsigned char byte3,
        unsigned char byte4
        )
{
    packet out;
    out.length = 5;
    out.service_mode = false;
    out.data[0] = byte1;
    out.data[1] = byte2;
    out.data[2] = byte3;
    out.data[3] = byte4;
    out.data[4] = byte1 ^ byte2 ^ byte3 ^ byte4;
    return out;
}

packet Packet_Create5(
        unsigned char byte1,
        unsigned char byte2,
        unsigned char byte3,
        unsigned char byte4,
        unsigned char byte5
        )
{
    packet out;
    out.length = 6;
    out.service_mode = false;
    out.data[0] = byte1;
    out.data[1] = byte2;
    out.data[2] = byte3;
    out.data[3] = byte4;
    out.data[4] = byte5;
    out.data[5] = byte1 ^ byte2 ^ byte3 ^ byte4 ^ byte5;
    return out;
}

packet Packet_CreateServiceMode2(unsigned char byte1, unsigned char byte2)
{
    packet out;
    out.length = 3;
    out.service_mode = true;
    out.data[0] = byte1;
    out.data[1] = byte2;
    out.data[2] = byte1 ^ byte2;
    return out;
}

packet Packet_CreateServiceMode3(unsigned char byte1, unsigned char byte2, unsigned char byte3)
{
    packet out;
    out.length = 4;
    out.service_mode = true;
    out.data[0] = byte1;
    out.data[1] = byte2;
    out.data[2] = byte3;
    out.data[3] = byte1 ^ byte2 ^ byte3;
    return out;
}

bool Packet_AppendByte(packet *p, unsigned char b)
{
    if(p->length >= PACKET_MAX_LENGTH)
        return false;
    p->data[p->length] = b;
    p->length++;
    return true;
}

bool Packet_EccValid(const packet *p)
{
    unsigned char ecc = 0;
    for(int i = 0; i < p->length; ++i)
        ecc ^= p->data[i];
    return (ecc == 0);
}

unsigned char Packet_RawLength(const packet *p)
{
    return (unsigned char)(
        (
            p->service_mode ?
            PACKET_PROGRAMMING_PREAMBLE_LENGTH : PACKET_NORMAL_PREAMBLE_LENGTH
        ) + p->length * 9 + 1
        );
}

void Packet_RawData(unsigned char *out, const packet *p)
{
    const unsigned char len = Packet_RawLength(p);
    const unsigned char pre =
            p->service_mode ?
            PACKET_PROGRAMMING_PREAMBLE_LENGTH : PACKET_NORMAL_PREAMBLE_LENGTH;
    // Each output bit.
    for(unsigned char i = 0; i < len; ++i)
    {
        // Position to write to in 'out'.
        const unsigned char out_byte = i / 8;
        const unsigned char out_shift = 7 - (i % 8);

        if(i < pre)
        {
            // Preamble bit.
            out[out_byte] = (unsigned char)(out[out_byte] | (1 << out_shift));
        }
        else
        {
            // i >= pre

            // Byte and bit to take from p->data.
            const unsigned char in_byte = (unsigned char)((i - pre) / 9);
            const unsigned char in_shift = (unsigned char)(8 - ((i - pre) % 9));

            if(in_byte < p->length)
            {
                if(in_shift == 8)
                {
                    // Byte start bit.
                    out[out_byte] =
                        (unsigned char)(out[out_byte] & ~(1 << out_shift));
                }
                else
                {
                    // Data bit.
                    if(p->data[in_byte] & (1 << in_shift))
                        out[out_byte] =
                            (unsigned char)(out[out_byte] | (1 << out_shift));
                    else
                        out[out_byte] =
                            (unsigned char)(out[out_byte] & ~(1 << out_shift));
                }
            }
            else
            {
                // Packet end bit.
                out[out_byte] =
                    (unsigned char)(out[out_byte] | (1 << out_shift));
            }
        }
    }
}

bool Packet_Matches(const packet *a, const packet *b, const packet *mask)
{
    if(a->length != b->length || a->length != mask->length)
        return false;
    // Do not check the checksum byte.
    for(int i = 0; i < a->length - 1; ++i)
        if((a->data[i] & mask->data[i]) != (b->data[i] & mask->data[i]))
            return false;
    return true;
}

bool Packet_IsReset(const packet *p)
{
    return p->length == 3 && p->data[0] == 0 && p->data[1] == 0;
}

bool Packet_IsServiceMode(const packet *p)
{
    return (p->length >= 1 && p->data[0] >= 112 && p->data[0] <= 127);
}

bool Packet_Is28Step(const packet *p)
{
    packet cmp = Packet_Create2(0, 0b01000000);
    packet mask = Packet_Create2(0b10000000, 0b11000000);
    return Packet_Matches(p, &cmp, &mask);
}

cfg_address_type Packet_28StepAddress(const packet *p)
{
    return p->data[0] & 0b01111111;
}

unsigned char Packet_28StepSpeed(const packet *p)
{
    unsigned char s =
        (unsigned char)(
                ((p->data[1] & 0b00001111) << 1) | ((p->data[1] & 0b00010000) >> 4)
                );
    if(s >= 1)
        s = (unsigned char)(s - 1);
    return s;
}

bool Packet_28StepFwd(const packet *p)
{
    return p->data[1] & 0b00100000;
}

bool Packet_Is128Step(const packet *p)
{
    packet cmp = Packet_Create3(0, 0b00111111, 0);
    packet mask = Packet_Create3(0b10000000, 0b11111111, 0);
    return Packet_Matches(p, &cmp, &mask);
}

unsigned char Packet_128StepSpeed(const packet *p)
{
    unsigned char s = p->data[2] & 0b01111111;
    if(s >= 1)
        s = (unsigned char)(s - 1);
    return s;
}

bool Packet_128StepFwd(const packet *p)
{
    return p->data[2] & 0b10000000;
}

cfg_address_type Packet_AdvAddress(const packet *p)
{
    if(p->length < 1)
        return 0;
    return p->data[0] & 0b01111111;
}

bool Packet_IsDirectProgramming(const packet *p)
{
    packet cmp = Packet_Create3(0b01111100, 0, 0);
    packet mask = Packet_Create3(0b01111100, 0, 0);
    return Packet_Matches(p, &cmp, &mask);
}

/*bool Packet_IsRegisterProgramming(const packet *p)*/
/*{*/
    /*packet cmp = Packet_Create2(0b01110000, 0);*/
    /*packet mask = Packet_Create2(0b11110000, 0);*/
    /*return Packet_Matches(p, &cmp, &mask);*/
/*}*/

unsigned short Packet_DirectCv(const packet *p)
{
    return (unsigned short)((((p->data[0] & 0b11) << 8) | p->data[1]) + 1);
}

unsigned char Packet_DirectValue(const packet *p)
{
    return p->data[2];
}

bool Packet_IsRegisterProgramming(const packet *p)
{
    packet cmp = Packet_Create2(0b01111000, 0);
    packet mask = Packet_Create2(0b11111000, 0);
    return Packet_Matches(p, &cmp, &mask);
}

unsigned char Packet_Register(const packet *p)
{
    return p->data[0] & 0b00000111;
}

unsigned char Packet_RegisterValue(const packet *p)
{
    return p->data[1];
}

bool Packet_IsLocoPomWrite(const packet *p)
{
    packet cmp = Packet_Create4(0, 0b11101100, 0, 0);
    packet mask = Packet_Create4(0, 0b11111100, 0, 0);
    return Packet_Matches(p, &cmp, &mask);
}

bool Packet_IsPomAccelerationWrite(const packet *p)
{
    packet cmp = Packet_Create3(0, 0b11110010, 0);
    packet mask = Packet_Create3(0, 0xff, 0);
    return Packet_Matches(p, &cmp, &mask);
}

bool Packet_IsPomDecelerationWrite(const packet *p)
{
    packet cmp = Packet_Create3(0, 0b11110011, 0);
    packet mask = Packet_Create3(0, 0xff, 0);
    return Packet_Matches(p, &cmp, &mask);
}

unsigned short Packet_LocoPomCv(const packet *p)
{
    return (unsigned short)((((p->data[1] & 0b11) << 8) | p->data[2]) + 1);
}

unsigned char Packet_LocoPomValue(const packet *p)
{
    return p->data[3];
}

bool Packet_IsBasicAccessory(const packet *p)
{
    // Optimisation: return early if the packet is of the wrong length.
    if(p->length != 3)
        return false;
    packet cmp = Packet_Create2(0b10000000, 0b10000000);
    packet mask = Packet_Create2(0b11000000, 0b10000000);
    return Packet_Matches(p, &cmp, &mask);
}

cfg_address_type Packet_BasicAccessoryAddress(const packet *p)
{
    if(p->length != 3)
        return 0;
    // Bits 6, 5, 4 of byte 1 in one's complement and bits 5-0 of byte 0.
    return (cfg_address_type)(
            (((p->data[1] & 0b01110000) << 2) ^ 0b111000000) |
            (p->data[0] & 0b00111111)
            );
}

bool Packet_BasicAccessoryState(const packet *p)
{
    if(p->length != 3)
        return false;
    return (p->data[1] & 0b00001000);
}

unsigned char Packet_BasicAccessoryOutput(const packet *p)
{
    if(p->length != 3)
        return false;
    return (p->data[1] & 0b00000111);
}

bool Packet_IsFunction(const packet *p)
{
    {
        packet cmp = Packet_Create2(0, 0b10000000);
        packet mask = Packet_Create2(0, 0b11100000);
        if(Packet_Matches(p, &cmp, &mask))
            return true;
    }
    {
        packet cmp = Packet_Create2(0, 0b10110000);
        packet mask = Packet_Create2(0, 0b11110000);
        if(Packet_Matches(p, &cmp, &mask))
            return true;
    }
    {
        packet cmp = Packet_Create2(0, 0b10100000);
        packet mask = Packet_Create2(0, 0b11110000);
        if(Packet_Matches(p, &cmp, &mask))
            return true;
    }
    {
        packet cmp = Packet_Create3(0, 0b11011110, 0);
        packet mask = Packet_Create3(0, 0b11111111, 0);
        if(Packet_Matches(p, &cmp, &mask))
            return true;
    }
    {
        packet cmp = Packet_Create3(0, 0b11011111, 0);
        packet mask = Packet_Create3(0, 0b11111111, 0);
        if(Packet_Matches(p, &cmp, &mask))
            return true;
    }
    return false;
}

packet Packet_CreateFunction0_4(cfg_address_type address, unsigned char data)
{
    return Packet_Create2(
            (unsigned char)address,
            (unsigned char)(
                0b10000000 |
                ((data >> 1) & 0b00001111) |
                ((data << 4) & 0b00010000))
            );
}

packet Packet_CreateFunction5_8(cfg_address_type address, unsigned char data)
{
    return Packet_Create2(
            (unsigned char)address,
            (unsigned char)(0b10110000 | (data & 0b00001111))
            );
}

packet Packet_CreateFunction9_12(cfg_address_type address, unsigned char data)
{
    return Packet_Create2(
            (unsigned char)address,
            (unsigned char)(0b10100000 | (data & 0b00001111))
            );
}

packet Packet_CreateFunction13_20(cfg_address_type address, unsigned char data)
{
    return Packet_Create3((unsigned char)address, 0b11011110, data);
}

packet Packet_CreateFunction21_28(cfg_address_type address, unsigned char data)
{
    return Packet_Create3((unsigned char)address, 0b11011111, data);
}

bool Packet_IsAccPomWrite(const packet *p)
{
    // Optimisation: return early if the packet is of the wrong length.
    /*if(p->length != 6)*/
        /*return false;*/
    packet cmp = Packet_Create5(
            // Lower address bits.
            0b10000000,
            // 1AAACDDD - upper bits of address, CDDD == 0000 to access a CV
            // for the decoder rather than an output.
            0b10000000,
            // Write byte instruction.
            0b11101100,
            // Lower eight bits of CV number.
            0,
            // CV data.
            0
            );
    packet mask = Packet_Create5(0b11000000, /* accept packets addressed to any input */ 0b10000000, 0b11111100, 0, 0);
    return Packet_Matches(p, &cmp, &mask);
}

cfg_address_type Packet_AccPomAddress(const packet *p)
{
    // Bits 6, 5, 4 of byte 1 in one's complement and bits 5-0 of byte 0.
    return (cfg_address_type)(
            (((p->data[1] & 0b01110000) << 2) ^ 0b111000000) |
            (p->data[0] & 0b111111)
            );
}

cfg_address_type Packet_AccPomCv(const packet *p)
{
    return (cfg_address_type)((((p->data[2] & 0b11) << 8) | p->data[3]) + 1);
}

unsigned char Packet_AccPomValue(const packet *p)
{
    return p->data[4];
}