#include "decode.h"
#include "bit_stuff.h"

uint8_t decode_combine_nibbles(uint8_t high, uint8_t low){
    uint8_t byte;
    uint8_t high_nibble;
    uint8_t low_nibble;
    decode_byte(high,high_nibble);
    decode_byte(low,low_nibble);
    combine_nibles_to_byte(high_nibble, low_nibble, byte);
    return byte;
}

void decode_byte(uint8_t in, uint8_t* nibble){
    uint8_t p0 = (in >> 0) & 0x01;
    uint8_t p1 = (in >> 1) & 0x01;
    uint8_t p2 = (in >> 2) & 0x01;
    uint8_t d0 = (in >> 3) & 0x01;
    uint8_t d1 = (in >> 4) & 0x01;
    uint8_t d2 = (in >> 5) & 0x01;
    uint8_t d3 = (in >> 6) & 0x01;
    uint8_t MSB = (in >> 7) & 0x01;
    
    // Check parity bits
    uint8_t error = 0;
    if ((p0 ^ d0 ^ d1 ^ d2) != 0) error |= 0x01;  // Error in p0 group
    if ((p1 ^ d0 ^ d1 ^ d3) != 0) error |= 0x02;  // Error in p1 group
    if ((p2 ^ d0 ^ d2 ^ d3) != 0) error |= 0x04;  // Error in p2 group

    if (MSB == 0 && error != 0) {
        switch(error) {
            case 0x07: d0 ^= 1; break; // Error in d0 0x7 = 0111
            case 0x03: d1 ^= 1; break; // Error in d1 0x3 = 0011
            case 0x05: d2 ^= 1; break; // Error in d2 0x5 = 0101
            case 0x06: d3 ^= 1; break; // Error in d3 0x6 = 0110

            case 0x01: p0 ^= 1; break; // Error in p0 0x1 = 0001
            case 0x02: p1 ^= 1; break; // Error in p1 0x2 = 0010
            case 0x04: p2 ^= 1; break; // Error in p2 0x4 = 0100
        }
    } else if (MSB == 1) MSB ^= 1; // If MSB is 1, flip it

    // create nibble from data bits
    *nibble = (d3 << 3) | (d2 << 2) | (d1 << 1) | d0;
}