#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>
#include <inttypes.h>

union variant_s {
    struct cell_s *p;
    intptr_t i;
};

struct cell_s {
    intptr_t n;
    struct cell_s *p;
    union variant_s e[];
};

/*
 *  root                   heap            heaptop
 *  |32|NULL|e[0] ... e[31]|      ...      |
 *                         freeptr
 */
struct memory_s {
    struct cell_s *space;
    struct cell_s *root;
    struct cell_s *heap;
    struct cell_s *heaptop;
    struct cell_s *freeptr;
};

static inline struct cell_s *
forwarding_cell (struct cell_s const *const x, intptr_t const n)
{
    return (struct cell_s *)(x->e + n);
}

struct memory_s *
memory_initialize (struct memory_s *const vm)
{
    enum { ROOTSIZE = 32, SPACESIZE = 126 };

    vm->space = malloc ((SPACESIZE + 2) * sizeof (vm->space->e[0]));
    if (vm->space == NULL) return NULL;

    vm->root = vm->space;
    vm->root->n = ROOTSIZE;
    vm->root->p = NULL;
    for (intptr_t i = 0; i < vm->root->n; i++)
        vm->root->e[i].p = NULL;

    vm->heap = forwarding_cell (vm->root, vm->root->n);
    vm->heaptop = forwarding_cell (vm->space, SPACESIZE);
    vm->freeptr = vm->heap;

    return vm;
}

int
variant_ispointer (union variant_s const x)
{
    return (x.i & 7) == 0; 
}

int
memory_isheap_pointer (struct memory_s *const vm, union variant_s const x)
{
    return variant_ispointer (x) && vm->heap <= x.p && x.p < vm->freeptr;
}

void
memory_print_variant (struct memory_s *const vm, union variant_s const x)
{
    if (! variant_ispointer (x))
        printf ("%10"PRIdPTR, x.i);
    else if (NULL == x.p)
        printf ("      NULL");
    else if (memory_isheap_pointer (vm, x))
        printf (" #%07td#", x.p - vm->space);
    else
        printf ("%10p", x.p);
}

void
memory_print (struct memory_s *const vm)
{
    struct cell_s const *p = vm->space;
    while (p < vm->freeptr) {
        printf ("#%07td={%10"PRIdPTR, p - vm->space, p->n);
        if (p->p == NULL) printf ("       NULL");
        else printf ("  #%07td#", p->p - vm->space);
        for (intptr_t i = 0; i < p->n; i++) {
            if (i % 4 == 2) printf ("          ");
            else printf (" ");
            memory_print_variant (vm, p->e[i]);
            if (i % 4 == 1 && i + 1 < p->n) printf ("\n");
        }
        printf ("}\n");
        p = forwarding_cell (p, p->n);
    }
}

void
memory_mark (struct memory_s *const vm)
{
    vm->root->p = vm->heaptop;
    struct cell_s *top = vm->root;
    while (top != vm->heaptop) {
        struct cell_s *const cell = top;
        top = cell->p;
        for (intptr_t i = 0; i < cell->n; i++)
            if (memory_isheap_pointer (vm, cell->e[i]) && cell->e[i].p->p == NULL) {
                cell->e[i].p->p = top;
                top = cell->e[i].p;
            }
    }
}

void
memory_calculate_address (struct memory_s *const vm)
{
    struct cell_s *dst = vm->heap;
    struct cell_s *src = vm->heap;
    while (src < vm->freeptr) {
        struct cell_s *forw = forwarding_cell (src, src->n);
        if (src->p != NULL) {
            src->p = dst;
            dst = forwarding_cell (dst, src->n);
        } else {
            while (forw < vm->freeptr && forw->p == NULL) {
                src->n += 2 + forw->n;
                forw = forwarding_cell (src, src->n);
            }
        }
        src = forw;
    }
}

void
memory_update_pointer (struct memory_s *const vm)
{
    struct cell_s *cell = vm->space;
    while (cell < vm->freeptr) {
        if (cell->p != NULL)
            for (intptr_t i = 0; i < cell->n; i++)
                if (memory_isheap_pointer (vm, cell->e[i]))
                    cell->e[i].p = cell->e[i].p->p;
        cell = forwarding_cell (cell, cell->n);
    }
}

void
memory_relocate (struct memory_s *const vm)
{
    struct cell_s *dst = vm->heap;
    struct cell_s *src = vm->heap;
    while (src < vm->freeptr) {
        if (src->p) {
            src->p = NULL;
            if (dst < src) {
                dst->n = src->n;
                dst->p = src->p;
                for (intptr_t i = 0; i < dst->n; i++)
                    dst->e[i] = src->e[i];
            }
            dst = forwarding_cell (dst, dst->n);
        }
        src = forwarding_cell (src, src->n);
    }
    vm->freeptr = dst;
    vm->root->p = NULL;
}

void
memory_mark_compact_lisp2 (struct memory_s *const vm)
{
    memory_mark (vm);
    memory_calculate_address (vm);
    memory_update_pointer (vm);
    memory_relocate (vm);
}

struct cell_s *
memory_alloc (struct memory_s *const vm, intptr_t const n)
{
    if (forwarding_cell (vm->freeptr, n) > vm->heaptop) {
        memory_mark_compact_lisp2 (vm);
        if (forwarding_cell (vm->freeptr, n) > vm->heaptop)
            return NULL;
    }
    struct cell_s *const cell = vm->freeptr;
    cell->n = n;
    cell->p = NULL;
    for (intptr_t i = 0; i < n; i++)
        cell->e[i].p = NULL;
    vm->freeptr = forwarding_cell (vm->freeptr, n);
    return cell;
}

int
main (int argc, char *argv[])
{
    int i, c;
    struct cell_s *p;
    struct memory_s vm_store;
    struct memory_s *vm = memory_initialize (&vm_store);
    
    c = 0;
    if ((p = memory_alloc (vm, 2)) != NULL) {
        vm->root->e[1].p = p;
        vm->root->e[1].p->e[0].p = NULL;
        vm->root->e[1].p->e[1].i = (c++ << 1) | 1;
    }
    for (i = 0; i < 8 && (p = memory_alloc (vm, 2)) != NULL; i++) {
        vm->root->e[3].p = p;
        vm->root->e[3].p->e[0] = vm->root->e[1];
        vm->root->e[3].p->e[1].i = (c++ << 1) | 1;
    }
    for (i = 0; i < 8 && (p = memory_alloc (vm, 2)) != NULL; i++) {
        vm->root->e[5].p = p;
        vm->root->e[5].p->e[0] = vm->root->e[3];
        vm->root->e[5].p->e[1].i = (c++ << 1) | 1;
    }
    for (i = 0; i < 32 && (p = memory_alloc (vm, 2)) != NULL; i++) {
        vm->root->e[6].p = p;
        vm->root->e[6].p->e[0] = vm->root->e[6];
        vm->root->e[6].p->e[1].i = (c++ << 1) | 1;
    }
    vm->root->e[1].p = vm->root->e[3].p = NULL;
    memory_print (vm);
    free (vm->space);

    return EXIT_SUCCESS;
}