#include "encode.h"
#include "parity.h"
#include "bit_stuff.h"
#include <stdbool.h>
#include <stdint.h>
#include <stddef.h>

void encode_get_nibbles(uint8_t value, uint8_t* high, uint8_t* low) {
    if (high == NULL || low == NULL) {
        return;
    }
    // Just extract the nibbles without encoding them
    extract_nibbles_from_byte(value, high, low);
}

uint8_t encode_nibble(uint8_t nibble) {
    uint8_t p0, p1, p2;
    calculate_parity_bits(nibble, &p0, &p1, &p2);
    // Format: [0][d3][d2][d1][d0][p2][p1][p0] (MSB first)
    return ((nibble & 0x0F) << 3) | (p2 << 2) | (p1 << 1) | p0;
}

void encode_value(uint8_t input, uint8_t* high, uint8_t* low) {
    if (high == NULL || low == NULL) {
        return;
    }
    
    uint8_t high_nibble, low_nibble;
    encode_get_nibbles(input, &high_nibble, &low_nibble);
    
    // Encode each nibble with parity bits
    *high = encode_nibble(high_nibble);
    *low = encode_nibble(low_nibble);
}