Huffman coding

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <errno.h>
#include <stdbool.h>


struct HNode
{
    struct HNode*   prev;
    struct HNode*   next;
    struct HNode*   left;
    struct HNode*   right;
    struct HNode*   parent;
    double          freq;
    uint8_t         length;
    uint8_t         symbol;
    uint32_t        code;
};


static void list_insert(struct HNode* after, struct HNode* node)
{
    node->next = after->next;
    node->prev = after;
    after->next->prev = node;
    after->next = node;
}


static void list_remove(struct HNode* node)
{
    if (node->prev != NULL)
    {
        node->prev->next = node->next;
    }
    if (node->next != NULL)
    {
        node->next->prev = node->prev;
    }
    node->prev = NULL;
    node->next = NULL;
}


static bool list_empty(const struct HNode* head)
{
    return head->next == head;
}


static struct HNode* list_peek(const struct HNode* head)
{
    if (!list_empty(head))
    {
        return head->next;
    }

    return NULL;
}


static void tree_insert(struct HNode* parent, struct HNode* x, struct HNode* y)
{
    list_remove(x);
    list_remove(y);

    x->parent = parent;
    y->parent = parent;

    parent->parent = NULL;
    parent->left = x;
    parent->right = y;
    parent->freq = x->freq + y->freq;
}


static void tree_clear(struct HNode** node)
{
    struct HNode* root = *node;

    if (root != NULL)
    {
        tree_clear(&root->left);
        tree_clear(&root->right);
        free(root);
    }

    *node = NULL;
}


static void list_clear(struct HNode* head)
{
    while (!list_empty(head))
    {
        struct HNode* node = list_peek(head);
        list_remove(node);
        tree_clear(&node);
    }
}


static struct HNode* find_position(struct HNode* head, double freq)
{
    struct HNode* prev = head;
    struct HNode* curr = head->next;

    while (curr != head)
    {
        if (curr->freq >= freq)
        {
            return prev;
        }

        prev = curr;
        curr = curr->next;
    }

    return curr;
}


static void count_frequency(double* frequency, const void* data, size_t length)
{
    bool even = true;
    const unsigned char* ptr = data;
    const unsigned char* end = ptr + length;

    for (; ptr != end; ptr++)
    {
        ++frequency[*ptr];
        even = !!even;
    }

    if (!even)
    {
        for (int i = 0; i < 256; ++i)
        {
            if (frequency[i] == 0)
            {
                frequency[i] += 1;
                break;
            }
        }
    }
}


static void node_clear(struct HNode* node)
{
    node->next = node;
    node->prev = node;
    node->left = NULL;
    node->right = NULL;
    node->parent = NULL;
    node->symbol = 0;
    node->freq = 0;
    node->length = 0;
    node->code = 0;
}


static struct HNode* node_create()
{
    struct HNode* node = malloc(sizeof(struct HNode));
    if (node == NULL)
    {
        return NULL;
    }

    node_clear(node);
    return node;
}


static void tree_build_codes(struct HNode* node, uint8_t length, uint32_t code)
{
    if (node->left == NULL)
    {
        node->length = length;
        node->code = code;
    }
    else
    {
        tree_build_codes(node->left, length + 1, code);
        tree_build_codes(node->right, length + 1, code | (1 << length));
    }
}


static int frequency_tree(struct HNode** root, struct HNode** table, double* frequency)
{
    struct HNode unassigned;
    node_clear(&unassigned);

    *root = NULL;

    for (int i = 0; i < 256; ++i)
    {
        table[i] = NULL;
        if (frequency[i] > 0)
        {
            struct HNode* leaf = node_create();
            if (leaf == NULL)
            {
                list_clear(&unassigned);
                return errno;
            }
            leaf->symbol = i;
            leaf->freq = frequency[i];
            table[i] = leaf;

            struct HNode* after = find_position(&unassigned, leaf->freq);
            list_insert(after, leaf);
        }
    }

    while (unassigned.next->next != &unassigned)
    {
        struct HNode* parent = node_create();
        if (parent == NULL)
        {
            list_clear(&unassigned);
            return errno;
        }

        struct HNode* x = list_peek(&unassigned);
        list_remove(x);

        struct HNode* y = list_peek(&unassigned);
        list_remove(y);

        tree_insert(parent, x, y);

        struct HNode* after = find_position(&unassigned, parent->freq);
        list_insert(after, parent);
    }

    struct HNode* node = list_peek(&unassigned);
    list_remove(node);

    tree_build_codes(node, 0, 0);

    *root = node;
    return 0;
}


static void print_symbol(const struct HNode* node)
{
    int i;
    fprintf(stderr, "%02x %3u: ", node->symbol, node->length);
    for (i = 0; i < node->length; ++i)
    {
        char bit = !!(node->code & (1 << i)) + '0';
        fprintf(stderr, "%c", bit);
    }

    for (; i < 8; ++i)
    {
        fprintf(stderr, " ");
    }

    //fprintf(stderr, " %8.0f\n", node->freq);
    fprintf(stderr, "\n");
}


static void print_table(struct HNode** table)
{
    for (int i = 0; i < 256; ++i)
    {
        if (table[i] != NULL)
        {
            print_symbol(table[i]);
        }
    }
}


static size_t encode(struct HNode** table, const void* src, size_t len, void* dst)
{
    const unsigned char* input = src;
    const unsigned char* end = src + len;
    unsigned char* output = dst;

    int bit = 0;
    unsigned char byte = 0;

    while (input != end)
    {
        const struct HNode* ptr = table[*input++];

        for (int i = 0; i < ptr->length; ++i)
        {
            byte <<= 1;
            byte |= !!(ptr->code & (1 << i));

            if (++bit == 8)
            {
                *output++ = byte;
                byte = 0;
                bit = 0;
            }
        }
    }

    if (bit != 0)
    {
        byte <<= (8 - bit);
        *output = byte;
    }

    return (output - ((unsigned char*) dst)) * 8 + bit;
}


static size_t decode(const struct HNode* root, const void* src, size_t len, void* dst)
{
    const unsigned char* input = src;
    unsigned char* output = dst;

    int bitmask = 0x80;
    const struct HNode* ptr = root;

    for (size_t i = 0; i < len; ++i)
    {
        bool right = !!(*input & bitmask);
        bitmask >>= 1;

        if (bitmask == 0)
        {
            bitmask = 0x80;
            ++input;
        }

        if (right)
        {
            ptr = ptr->right;
        }
        else
        {
            ptr = ptr->left;
        }

        if (ptr->left == NULL)
        {
            *output++ = ptr->symbol;
            ptr = root;
        }
    }

    return (output - ((unsigned char*) dst)) * 8;
}


int main(int argc, char** argv)
{
    const char* string = "XXXX og XXXXX gjør ting de ikke kan, som f.eks. å implementere Huffman-koding";
    size_t n = strlen(string) + 1;

    double freqs[256];
    for (int i = 0; i < 256; ++i)
    {
        freqs[i] = 0;
    }

    count_frequency(freqs, string, n);

    struct HNode* root;
    struct HNode* table[256];
    frequency_tree(&root, table, freqs);

    print_table(table);
    fprintf(stderr, "\n");

    char encoded[n];
    size_t len = encode(table, string, n, encoded);

    char decoded[n];
    len = decode(root, encoded, len, decoded);

    tree_clear(&root);

    fprintf(stderr, "%s\n", string);
    fprintf(stderr, "%s\n", decoded);

    return 0;
}