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

#include "treetrie.h"
#include "fasthash.h"

typedef uint32_t tt_hash_t;

/* /\* Customized special-purpose fasthash variation *\/ */
/* #define mix(h) ({					\ */
/* 			(h) ^= (h) >> 23;		\ */
/* 			(h) *= 0x2127599bf4325c37ULL;	\ */
/* 			(h) ^= (h) >> 47; }) */
/* static inline uint64_t fasthash_4_ints(uint32_t v1, uint32_t v2, uint32_t v3, uint32_t v4) { */
/*   const uint64_t    m = 0x880355f21e6d1965ULL; */
/*   uint64_t h = (16 * m); */
/*   uint64_t v; */
/*   v = (((uint64_t) v2) << 32) | v1; */
/*   h ^= mix(v); */
/*   h *= m; */
/*   v = (((uint64_t) v4) << 32) | v3; */
/*   h ^= mix(v); */
/*   h *= m; */
/*   return mix(h); */
/* } */

static inline tt_hash_t hash(uint32_t tag,
			     uint32_t index,
			     tt_node_idx_t a,
			     tt_node_idx_t b)
{
  /* uint64_t x = fasthash_4_ints(tag, index, a, b); */
  /* return x - (x >> 32); */

  /* uint32_t keyblock[4] = { tag, */
  /* 			   index, */
  /* 			   a, */
  /* 			   b }; */
  /* assert(sizeof(keyblock) == 4 * sizeof(uint32_t)); */
  /* return (tt_hash_t) fasthash32(keyblock, sizeof(keyblock), 0); */

  /* uint64_t x = tag; */
  /* x = (x << 8) ^ index; */
  /* x = (x << 8) ^ a; */
  /* x = (x << 8) ^ b; */
  /* return x - (x >> 32); */

  uint64_t x = tag;
  x ^= index * 11;
  x ^= a * 11;
  x ^= b * 11;
  return x - (x >> 32);
}

static inline tt_hash_t tt_hash_node(tt_arena_t *a, tt_node_idx_t i) {
  return hash(a->headers[i].inuse.tag,
	      a->headers[i].inuse.index,
	      a->nodes[i].a,
	      a->nodes[i].b);
}

static void chain_init(tt_arena_t *a, tt_free_chain_t *chain) {
  chain->head = chain->tail = TT_ERROR;
}

static void chain_append(tt_arena_t *a, tt_free_chain_t *chain, tt_node_idx_t i) {
  a->headers[i].next_free = TT_ERROR;
  if (chain->tail == TT_ERROR) {
    chain->head = i;
  } else {
    a->headers[chain->tail].next_free = i;
  }
  chain->tail = i;
}

/* Does not modify chain2. */
static void chain_splice(tt_arena_t *a, tt_free_chain_t *chain1, tt_free_chain_t *chain2) {
  if (chain2->head == TT_ERROR) {
    /* do nothing */
  } else if (chain1->head == TT_ERROR) {
    *chain1 = *chain2;
  } else {
    a->headers[chain1->tail].next_free = chain2->head;
    chain1->tail = chain2->tail;
  }
}

static tt_node_idx_t chain_pop(tt_arena_t *a, tt_free_chain_t *chain) {
  tt_node_idx_t i = chain->head;
  if (i != TT_ERROR) {
    chain->head = a->headers[i].next_free;
    if (chain->tail == i) {
      chain->tail = chain->head;
    }
  }
  return i;
}

int tt_arena_init(tt_arena_t *a) {
  a->max_probe = 0;
  a->table_length = 16411; /* 16384; */
  a->table = calloc(a->table_length, sizeof(a->table[0]));
  a->headers = calloc(a->table_length, sizeof(a->headers[0]));
  a->nodes = calloc(a->table_length, sizeof(a->nodes[0]));
  a->free_count = 0;
  chain_init(a, &a->free_chain);

  if (a->table == NULL || a->headers == NULL || a->nodes == NULL) {
    free(a->table);
    free(a->headers);
    free(a->nodes);
    errno = ENOMEM;
    return -1;
  }

  {
    int i;
    for (i = TT_FIRST_VALID_NODE_IDX; i < a->table_length; i++) {
      chain_append(a, &a->free_chain, i);
      a->free_count++;
    }
  }

  return 0;
}

static void register_node(tt_arena_t *a, tt_node_idx_t node, tt_hash_t initial_hash) {
  tt_hash_t h = initial_hash;
  int i = 0;
  while (1) {
    unsigned int index = (h + i) % a->table_length;
    tt_node_idx_t candidate = a->table[index];

    /* printf("checking robinhood at h %d i %d index %d candidate %d\n", h, i, index, candidate); */

    if (i > a->max_probe) {
      a->max_probe = i;
    }

    if (candidate < TT_FIRST_VALID_NODE_IDX) {
      /* This slot in the table is free. */
      /* printf("slot free!\n"); */
      a->table[index] = node;
      break;
    }

    /* printf("slot not free.\n"); */
    {
      tt_hash_t candidate_h = tt_hash_node(a, candidate);
      int distance = index - (candidate_h % a->table_length);
      if (distance < 0) distance += a->table_length;

      if (distance < i) {
	a->table[index] = node;
	h = candidate_h;
	i = distance + 1;
	node = candidate;
      } else {
	/* keep scanning. */
	i++;
      }
    }
  }
}

static int tt_grow(tt_arena_t *a) {
  tt_node_idx_t *old_table = a->table;
  unsigned int old_table_length = a->table_length;
  unsigned int new_table_length = old_table_length << 1;

  /* printf("PREGROW\n"); */
  /* tt_dump_arena(a); */

  {
    tt_node_idx_t *new_table = calloc(new_table_length, sizeof(a->table[0]));
    tt_header_t *new_headers = realloc(a->headers, new_table_length * sizeof(a->headers[0]));
    tt_node_t *new_nodes = realloc(a->nodes, new_table_length * sizeof(a->nodes[0]));

    if (new_table == NULL || new_headers == NULL || new_nodes == NULL) {
      free(new_table);
      free(new_headers);
      free(new_nodes);
      errno = ENOMEM;
      return -1;
    }

    memset(new_headers + old_table_length, 0,
	   (new_table_length - old_table_length) * sizeof(a->headers[0]));
    memset(new_nodes + old_table_length, 0,
	   (new_table_length - old_table_length) * sizeof(a->nodes[0]));

    a->max_probe = 0;
    a->table_length = new_table_length;
    a->table = new_table;
    a->headers = new_headers;
    a->nodes = new_nodes;

    {
      int i;
      for (i = old_table_length; i < new_table_length; i++) {
	chain_append(a, &a->free_chain, i);
	a->free_count++;
      }
    }
  }

  /* printf("//////////////////////////////////////// GROW starting (length %d)\n", a->table_length); */

  {
    int i;
    for (i = 0; i < old_table_length; i++) {
      tt_node_idx_t n = old_table[i];
      if (n >= TT_FIRST_VALID_NODE_IDX) {
	register_node(a, n, tt_hash_node(a, n));
      }
    }
  }

  /* printf("//////////////////////////////////////// GROW finished (length %d)\n", a->table_length); */

  /* printf("POSTGROW\n"); */
  /* tt_dump_arena(a); */

  free(old_table);
  return 0;
}

void tt_arena_done(tt_arena_t *a) {
  free(a->table);
  free(a->headers);
  free(a->nodes);
  memset(a, 0, sizeof(*a));
}

static size_t arena_size(tt_arena_t *a) {
  return sizeof(*a) +
    (a->table_length * sizeof(a->table[0])) +
    (a->table_length * sizeof(a->headers[0])) +
    (a->table_length * sizeof(a->nodes[0]));
}

void tt_dump_arena_summary(tt_arena_t *a) {
  printf("size in bytes: %lu\n", arena_size(a));
  printf("max_probe: %u\n", a->max_probe);
  printf("table_length: %u\n", a->table_length);
  printf("free_count: %u\n", a->free_count);
}

void tt_dump_arena(tt_arena_t *a) {
  tt_dump_arena_summary(a);

  printf("free_chain:");
  {
    tt_node_idx_t fp = a->free_chain.head;
    while (fp != TT_ERROR) {
      printf(" %d", fp);
      fp = a->headers[fp].next_free;
    }
  }
  printf("\n");

  {
    int i;
    for (i = 0; i < a->table_length; i++) {
      tt_node_idx_t n = a->table[i];
      if (n < TT_FIRST_VALID_NODE_IDX) {
	/* Skip. */
      } else if (n >= a->table_length) {
	printf("%12u -> %12u ?!?!?!\n", i, n);
      } else {
	tt_hash_t h = tt_hash_node(a, n);
	int distance = i - (h % a->table_length);
	if (distance < 0) distance += a->table_length;
	printf("%12u -> %12u: dist %d ref %d ",
	       i,
	       n,
	       distance,
	       a->headers[n].inuse.refcount);
	switch (a->headers[n].inuse.tag) {
	  case TT_TAG_TAIL:
	    printf("tail %u\n", a->nodes[n].a);
	    break;
	  case TT_TAG_BRANCH:
	    printf("branch %u %u\n", a->nodes[n].a, a->nodes[n].b);
	    break;
	  case TT_TAG_LEAF:
	    printf("leaf %u %u\n", a->nodes[n].a, a->nodes[n].b);
	    break;
	  case TT_TAG_NODE:
	    printf("node index %d, %u %u\n",
		   a->headers[n].inuse.index,
		   a->nodes[n].a,
		   a->nodes[n].b);
	    break;
	}
      }
    }
  }
}

void tt_arena_flush1(tt_arena_t *a, tt_free_chain_t *c) {
  tt_node_idx_t i = a->free_chain.head;
  chain_splice(a, c, &a->free_chain);
  chain_init(a, &a->free_chain);
  while (i >= TT_FIRST_VALID_NODE_IDX) {
    tt_drop(a, a->nodes[i].a);
    tt_drop(a, a->nodes[i].b);
    a->nodes[i].a = TT_ERROR;
    a->nodes[i].b = TT_ERROR;
    i = a->headers[i].next_free;
  }
}

void tt_arena_flush(tt_arena_t *a) {
  tt_free_chain_t c;
  c.head = c.tail = TT_ERROR;
  while (a->free_chain.head != TT_ERROR) {
    tt_arena_flush1(a, &c);
  }
  a->free_chain = c;
}

static void recycle_node(tt_arena_t *a, tt_node_idx_t ni) {
  tt_hash_t h;
  int i;

  /* printf("++++++++++++++++++++++++++++++++++++++++ recycling %d\n", ni); */

  assert(ni >= TT_FIRST_VALID_NODE_IDX);
  h = tt_hash_node(a, ni);

  if (a->headers[ni].inuse.tag == TT_TAG_LEAF) {
    a->nodes[ni].b = TT_ERROR;
  }
  chain_append(a, &a->free_chain, ni);
  a->free_count++;

  for (i = 0; i < a->max_probe+1; i++) {
    unsigned int index = (h + i) % a->table_length;
    tt_node_idx_t candidate = a->table[index];

    /* printf("hunting i=%d index=%d ni=%d candidate=%d\n", i, index, ni, candidate); */
    assert(candidate >= TT_FIRST_VALID_NODE_IDX); /* Internal error if node not in table */

    if (candidate == ni) {
      /* We found it. Now swap in elements. */
      while (1) {
	unsigned int nextindex = (index + 1) % a->table_length;
	tt_node_idx_t next_n = a->table[nextindex];
	tt_hash_t next_h;
	int distance;

	a->table[index] = TT_ERROR;

	if (next_n < TT_FIRST_VALID_NODE_IDX) {
	  break;
	}

	next_h = tt_hash_node(a, next_n);
	distance = nextindex - (next_h % a->table_length);
	if (distance < 0) distance += a->table_length;

	if (distance == 0) {
	  break;
	}

	a->table[index] = next_n;
	index = nextindex;
      }
      break;
    }
  }
}

tt_node_idx_t tt_arena_cons(tt_arena_t *a,
			    uint32_t tag,
			    uint32_t nindex,
			    tt_node_idx_t na,
			    tt_node_idx_t nb)
{
  tt_hash_t h = hash(tag, nindex, na, nb);
  int i;

  for (i = 0; i < a->max_probe+1; i++) {
    unsigned int index = (h + i) % a->table_length;
    tt_node_idx_t candidate = a->table[index];

    /* printf("cons at %d candidate %d\n", i, candidate); */
    /* TODO: perhaps also bail early if we detect that the hash code changes */
    if (candidate < TT_FIRST_VALID_NODE_IDX) {
      /* printf("cons empty cell\n"); */
      break;
    }

    /* printf("tag %d %d\n", a->headers[candidate].inuse.tag, tag); */
    /* printf("index %d %d\n", a->headers[candidate].inuse.index, nindex); */
    /* printf("a %d %d\n", a->nodes[candidate].a, na); */
    /* printf("b %d %d\n", a->nodes[candidate].b, nb); */

    if (a->headers[candidate].inuse.tag == tag &&
	a->headers[candidate].inuse.index == nindex &&
	a->nodes[candidate].a == na &&
	a->nodes[candidate].b == nb) {
      /* printf("cons located correct candidate\n"); */
      return candidate;
    }
  }

  /* Not found */
  /* printf("cons needs to alloc\n"); */

  if (a->free_count < (a->table_length >> 2)) {
    if (tt_grow(a) != 0) {
      return TT_ERROR;
    }
  }

  {
    tt_node_idx_t node = chain_pop(a, &a->free_chain);

    tt_grab(a, na);
    if (tag != TT_TAG_LEAF) tt_grab(a, nb);
    tt_drop(a, a->nodes[node].a);
    tt_drop(a, a->nodes[node].b);
    a->free_count--;

    a->headers[node].inuse.refcount = 0;
    a->headers[node].inuse.tag = tag;
    a->headers[node].inuse.index = nindex;
    a->nodes[node].a = na;
    a->nodes[node].b = nb;

    register_node(a, node, h);
    return node;
  }
}

tt_node_idx_t tt_grab(tt_arena_t *a, tt_node_idx_t i) {
  if (i >= TT_FIRST_VALID_NODE_IDX && a->headers[i].inuse.refcount < TT_REFCOUNT_LIMIT) {
    a->headers[i].inuse.refcount++;
  }
  return i;
}

void tt_drop(tt_arena_t *a, tt_node_idx_t i) {
  if (i >= TT_FIRST_VALID_NODE_IDX && a->headers[i].inuse.refcount < TT_REFCOUNT_LIMIT) {
    /* printf("++++++++++++++++++++++++++++++ dropping %d\n", i); */
    if (--(a->headers[i].inuse.refcount) == 0) {
      recycle_node(a, i);
    }
  }
}