// included from dsearch-sg.c

#define STORE_SIZE (fd_variables_count * sizeof(*split_stores))

#ifdef STORE_IN_POOL
static __thread _fd_store main_store; // to receive work and return solutions
static __thread _fd_store alt_store;  // the other side of the split
#endif

static _fd_store *split_stores_;
#define split_stores split_stores_[agent]

#ifndef INDEX_IN_POOL
static __thread fd_int *split_variable;
#else
static int **split_variable_;
static __thread int *split_variable;
#endif

#ifdef GROWABLE_POOL
// number of stores in the pool
#define POOL_DEFAULT_SIZE 32
static __thread int pool_size;
#endif

#define POOL_INDEXES 2
static int **split_indexes;
static __thread int *local_split_indexes;
#define base_split_index local_split_indexes[0]
#define next_split_index local_split_indexes[1]
static int EMPTY_POOL[] = { 0, 0 };

#ifndef STORE_IN_POOL
#define STEAL_THRESHOLD 3 // an agent with less stores won't be stolen from
#define INDEX_SAFE 3	  // lock-free access with these many stores in the pool
#else
#define STEAL_THRESHOLD (3 + 1)
#define INDEX_SAFE (3 + 1)
#endif

static pthread_mutex_t *stores_mutexes;
static pthread_mutex_t thieves_mutex;
static int most_work;	// agent which is likely to have the most work,
			// and from which the others will try to steal

#ifdef RANDOM_VICTIM
#define MAX_ATTEMPTS 5
static pthread_spinlock_t starving_lock;
static volatile int agents_starving = 0;
#endif

#ifdef STATS_POOL
static __thread unsigned long pool_entries = 0;
static __thread unsigned long pool_max_entries = 0;
static __thread unsigned long pool_gets = 0;
#endif

#ifdef STATS_STEALS
#define MAX_AGENTS 1024		// XXX: copied from agents-splitgo*.c
static unsigned long sts_attempts[MAX_AGENTS + 1], sts_done[MAX_AGENTS + 1];
#ifdef RANDOM_VICTIM
static unsigned long sts_checks[MAX_AGENTS + 1], sts_slept[MAX_AGENTS + 1];
#endif
#endif

void _fd_init_store_depository(int agents)
{
  int i;

#ifdef NEW_ENTRANCE
  agents_searching = agents_failed = 0;
#endif

#ifdef RANDOM_VICTIM
  agents_starving = 0;
#endif

  nagents = agents;

//  most_work = agents - 1; // the last agent receives the remaining search space
  most_work = -1;

  if (split_stores_)
    {
      // depository already initialised, just try to avoid race
      // conditions
      for (i = 0; i < agents; ++i)
	memset(split_indexes[i], 0, POOL_INDEXES * sizeof(**split_indexes));

      return;
    }

  // XXX: check for NULL's
  split_stores_ = calloc(agents, sizeof(*split_stores_));
  split_indexes = calloc(agents, sizeof(*split_indexes));

  // these will be properly initialised later, by the agent;
  // for now, just make sure that if another agent tries to access it
  // before that happens, it will find a sensible value there
  for (i = 0; i < agents; ++i)
    split_indexes[i] = EMPTY_POOL;

#ifdef INDEX_IN_POOL
  split_variable_ = calloc(agents, sizeof(*split_variable_));
#endif

  stores_mutexes = calloc(agents, sizeof(*stores_mutexes));
  for (i = 0; i < agents; ++i)
    pthread_mutex_init(&stores_mutexes[i], NULL);

  pthread_mutex_init(&thieves_mutex, NULL);

#ifdef NEW_ENTRANCE
  pthread_spin_init(&entrance_lock, 0);
#endif

#ifdef RANDOM_VICTIM
  pthread_spin_init(&starving_lock, 0);
#endif

#ifdef STATS_STEALS
  memset(sts_attempts, 0, sizeof(sts_attempts));
  memset(sts_done, 0, sizeof(sts_done));
#ifdef RANDOM_VICTIM
  memset(sts_checks, 0, sizeof(sts_done));
  memset(sts_slept, 0, sizeof(sts_done));
#endif
#endif
}

static void _fd_init_local_depository()
{
  if (split_stores)
    {
#ifdef STORE_IN_POOL
      // copy the new store to the pool
      store = split_stores;
      memcpy(store, main_store, STORE_SIZE);
#ifdef INDEX_IN_POOL
      split_variable[next_split_index] = -1;
#else
      split_variable[next_split_index] = NULL;
#endif
      ++next_split_index;
#endif /* STORE_IN_POOL */

      return;
    }

#ifndef GROWABLE_POOL
#ifndef STORE_IN_POOL
  _fd_debug("[%d.%d] stores size %d + %d\n", tid, agent,
	    fd_variables_count * STORE_SIZE,
	    fd_variables_count * sizeof(*split_variable));
  if (posix_memalign((void **) &split_stores, sysconf(_SC_PAGESIZE),
		     fd_variables_count * STORE_SIZE +
		     fd_variables_count * sizeof(*split_variable)))
    _fd_fatal("unable to allocate pool memory");

  // XXX: alignment problems possible
  split_variable = ((void *) split_stores) + fd_variables_count * STORE_SIZE;
#else
  _fd_debug("[%d.%d] stores size %d + %d\n", tid, agent,
	    (fd_variables_count + 1) * STORE_SIZE,
	    fd_variables_count * sizeof(*split_variable));
  if (posix_memalign((void **) &split_stores, sysconf(_SC_PAGESIZE),
		     (fd_variables_count + 1) * STORE_SIZE +
		     fd_variables_count * sizeof(*split_variable)))
    _fd_fatal("unable to allocate pool memory");

  // XXX: alignment problems possible
  split_variable = ((void *) split_stores) + (fd_variables_count + 1) * STORE_SIZE;
#endif /* STORE_IN_POOL */
#else /* GROWABLE_POOL */
#ifndef STORE_IN_POOL
  pool_size = (fd__label_vars_count < POOL_DEFAULT_SIZE) ?
    fd__label_vars_count : POOL_DEFAULT_SIZE;
#else
  pool_size = (fd__label_vars_count + 1 < POOL_DEFAULT_SIZE) ?
    fd__label_vars_count + 1 : POOL_DEFAULT_SIZE;
#endif

  _fd_debug("[%d.%d] stores size %d + %d\n", tid, agent, pool_size * STORE_SIZE,
	    fd_variables_count * sizeof(*split_variable));
	 
  if (posix_memalign((void **) &split_stores, sysconf(_SC_PAGESIZE),
		     pool_size * STORE_SIZE))
    _fd_fatal("unable to allocate pool memory");

  if (posix_memalign((void **) &split_variable, sysconf(_SC_PAGESIZE),
		     fd_variables_count * sizeof(*split_variable)))
    _fd_fatal("unable to allocate pool variable memory");
#endif /* GROWABLE_POOL */

#if defined(INDEX_IN_POOL) && !defined(split_variable)
  split_variable_[agent] = split_variable;
#endif

  local_split_indexes = calloc(POOL_INDEXES, sizeof(*local_split_indexes));
  split_indexes[agent] = local_split_indexes;

#ifdef STORE_IN_POOL
  // install the received store in the pool
  main_store = store;
  store = split_stores;
  memcpy(store, main_store, STORE_SIZE);
#ifdef INDEX_IN_POOL
  split_variable[next_split_index] = -1;
#else
  split_variable[next_split_index] = NULL;
#endif
  ++next_split_index;
#endif /* STORE_IN_POOL */
}

#ifdef GROWABLE_POOL
static void _fd_grow_pool(void)
{
  int nsize;
  _fd_store npool, opool;

  nsize = pool_size * 2;
#ifndef STORE_IN_POOL
  nsize = (fd_variables_count < nsize) ? fd_variables_count : nsize;
#else
  nsize = (fd_variables_count + 1 < nsize) ? fd_variables_count + 1 : nsize;
#endif
  _fd_debug("[%d.%d] new pool size is %d\n", tid, agent, nsize * STORE_SIZE);
	 
  if (posix_memalign((void **) &npool, sysconf(_SC_PAGESIZE),
		     nsize * STORE_SIZE))
    _fd_fatal("unable to allocate pool memory");

  memcpy(npool, split_stores, pool_size * STORE_SIZE);

  // lock the pool
  pthread_mutex_lock(&stores_mutexes[agent]);

  opool = split_stores;
  split_stores = npool;

  // store now points to freed memory
  store = split_stores + (store - opool);

  // unlock the pool
  pthread_mutex_unlock(&stores_mutexes[agent]);

  free(opool);

  pool_size = nsize;
}
#endif /* GROWABLE_POOL */

/* 
   saving a store to the pool is done in two steps:

   1. a copy of the store which is about to be split is saved (to have
      a place to put the result of splitting a domain);
   2. the remaining data is updated (after the splitting), making the
      store visible to the other agents.
*/

static void _fd_start_saving_store(_fd_store *dst)
{
#ifdef GROWABLE_POOL
  if (next_split_index == pool_size)
    _fd_grow_pool();
#endif

#ifndef STORE_IN_POOL
  *dst = &split_stores[next_split_index * fd_variables_count];
  memcpy(*dst, store, STORE_SIZE);
#else
  *dst = store;
  store = &split_stores[next_split_index * fd_variables_count];
  memcpy(store, *dst, STORE_SIZE);
#endif
}

static void _fd_end_saving_store(fd_int variable)
{
#ifndef STORE_IN_POOL
#ifndef INDEX_IN_POOL
  split_variable[next_split_index] = variable;
#else
  split_variable[next_split_index] = variable->index;
#endif
#else /* STORE_IN_POOL */
  // here, the split also affects the store which is below the
  // (about to be) current one
#ifndef INDEX_IN_POOL
  split_variable[next_split_index - 1] = variable;
#else
  split_variable[next_split_index - 1] = variable->index;
#endif
#endif /* STORE_IN_POOL */

  ++next_split_index;
}

// try to steal a store from another agent
#ifndef INDEX_IN_POOL
bool _fd_steal_store(_fd_store store_, int agent, int retries)
#else
bool _fd_steal_store(_fd_store store_, fd_int *variable, int agent, int retries)
#endif
{
#ifndef RANDOM_VICTIM
  int v;		// the victim
  int s;		// the store
  int stores;		// how many stores the victim has
  struct timespec delay = { 0, 2500000 }; // 2.5ms

#ifdef STATS_STEALS
  sts_attempts[agent + 1]++;
#endif

#if defined(STORE_IN_POOL) && defined(DECREMENT_EARLY)
  if (agent != -1)
    --next_split_index;
#endif

  // only let one agent in at a time
  pthread_mutex_lock(&thieves_mutex);

  v = most_work;

  // if work has last been stolen by the main thread or by the agent
  // itself, look for an agent to steal work from
  if (v == -1 || v == agent
      || split_indexes[v][1] - split_indexes[v][0] < STEAL_THRESHOLD)
    {
      int most_stores = 1;
      int candidate = -1;
      bool retrying = false;

      for (;;)
	{
	  int active = 0;
	  int i;

	  // find an agent likely to own the largest search space,
	  // using the pool size as a heuristic
	  // (go through all agents, in case it is the controller who
	  // is trying to take a store)
#ifdef PREFER_NEXT
	  v = (agent + 1) % nagents;
#else
	  v = 0;
#endif

	  for (i = 0; i < nagents; ++i, v = (v + 1) % nagents)
	    {
	      if (v == agent)
		continue;

	      if ((stores = split_indexes[v][1] - split_indexes[v][0]) > 0)
		{
		  ++active;

#ifdef FIRST_CANDIDATE
		  if (stores >= STEAL_THRESHOLD)
		    {
		      candidate = v;

		      break;
		    }
#else /* FIRST_CANDIDATE */
		  if (stores > most_stores)
		    {
		      most_stores = stores;
		      candidate = v;
		    }
#endif /* FIRST_CANDIDATE */
		}
	    }

	  if (candidate != -1)
	    {
	      // have a candidate to steal from
	      v = candidate;

	      if (retrying)
		_fd_debug("[%d.%d] %d tries left\n", tid, agent, retries);

	      break;
	    }

	  /* found no agent to steal from */

	  pthread_mutex_unlock(&thieves_mutex);

	  if (active == 0 || retries == 0)
	    {
	      _fd_debug("[%d.%d] no agent can supply work\n", tid, agent);

	      if (retrying)
		_fd_debug("[%d.%d] %d tries left\n", tid, agent, retries);

	      return false;
	    }

	  /* there is still at least one active agent and we are
	     willing to try again */

	  if (retrying)
	    {
	      // increase the delay
	      delay.tv_sec <<= 1;
	      delay.tv_nsec <<= 1;
	      if (delay.tv_nsec > 1000000000)
		{
		  delay.tv_sec += delay.tv_nsec / 1000000000;
		  delay.tv_nsec %= 1000000000;
		}
	    }
	  else
	    retrying = true;

	  --retries;

	  nanosleep(&delay, NULL);

	  // re-lock and try again
	  pthread_mutex_lock(&thieves_mutex);
	}
    }

  // lock access to victim's stores
  pthread_mutex_lock(&stores_mutexes[v]);
#if DEBUG_STEALING > 1
  _fd_debug("[%d.%d] locked %d stores' mutex\n", tid, agent, v);
#endif

  stores = split_indexes[v][1] - split_indexes[v][0];

#if DEBUG_STEALING > 1
  _fd_debug("[%d.%d] %d stores left at %d\n", tid, agent, stores, v);
#endif

  // don't steal the last few stores from an agent
  if (stores < STEAL_THRESHOLD)
    {
      pthread_mutex_unlock(&stores_mutexes[v]);
      pthread_mutex_unlock(&thieves_mutex);

#ifdef DEBUG_STEALING
      _fd_debug("[%d.%d] not enough stores (%d) at %d\n", tid, agent, stores, v);
#endif

      return false;
    }

  // steal the store
  s = split_indexes[v][0]++;

#ifdef STORE_IN_POOL
  // XXX: there should be no side effects on the value of `store'
  if (agent != -1)
    alt_store = 0,
    store = store_ = split_stores;
#endif

  memcpy(store_, &split_stores_[v][s * fd_variables_count], STORE_SIZE);

#ifdef INDEX_IN_POOL
#ifndef STORE_IN_POOL
  if (variable)
#else
  if (variable && split_variable_[v][s] != -1)
#endif
    *variable = _fd_variables[split_variable_[v][s]];
#endif

  // this agent will likely become the one with the largest search space
  // (if work is being stolen by the controller, most_work will be set
  // to -1 which will force the next agent who tries to steal a store
  // to look for a suitable victim)
  most_work = agent;

  if (agent != -1)
    {
      // reset the indexes into the depository
#ifndef STORE_IN_POOL
      next_split_index = 0;
#else
      next_split_index = 1;
#endif
      base_split_index = 0;
    }

#ifdef DEBUG_STEALING
#ifdef INDEX_IN_POOL
  _fd_debug("[%d.%d] stole work (%d/%d) from %d (var %d)\n", tid, agent, s,
	    stores, v, split_variable_[v][s]);
#else
  _fd_debug("[%d.%d] stole work (%d/%d) from %d\n", tid, agent, s, stores, v);
#endif
#if DEBUG_STEALING > 2
  _fd_output("[%d.%d] ", tid, agent); _fd_cprint3(store_);
#endif
#endif

  pthread_mutex_unlock(&stores_mutexes[v]);
  pthread_mutex_unlock(&thieves_mutex);

#ifdef STATS_STEALS
  sts_done[agent + 1]++;
#endif

  return true;
#else /* RANDOM_VICTIM */
  int v;		// the victim
  int s;		// the store
  int stores;		// how many stores the victim has
  struct timespec delay = { 0, 100000 }; // 100us
  int attempts;
  int as;

#ifdef STATS_STEALS
  sts_attempts[agent + 1]++;
#endif

#if defined(STORE_IN_POOL) && defined(DECREMENT_EARLY)
  if (agent != -1)
    --next_split_index;
#endif

  pthread_spin_lock(&starving_lock);
  as = agents_starving;
  if (agent != -1)
    agents_starving = ++as;
  pthread_spin_unlock(&starving_lock);

  if (as == nagents)
    {
#ifdef DEBUG_STEALING
      _fd_debug("[%d.%d] all agents starving...\n", tid, agent);
#endif

      return false;
    }

  // only let one agent in at a time
  pthread_mutex_lock(&thieves_mutex);

  // check the most likely candidate first
  v = most_work;

  // if work has last been stolen by the main thread or by the agent
  // itself, look for another agent to steal work from
  while (v == -1 || v == agent)
    v = random() % nagents;

  attempts = 0;

  for (;;)
    {
#ifdef STATS_STEALS
      sts_checks[agent + 1]++;
#endif

      if (split_indexes[v][1] - split_indexes[v][0] >= STEAL_THRESHOLD)
	{
	  // lock access to victim's stores ...
	  pthread_mutex_lock(&stores_mutexes[v]);
#if DEBUG_STEALING > 1
	  _fd_debug("[%d.%d] locked %d stores' mutex\n", tid, agent, v);
#endif

	  // ... and make sure that the victim has enough stores
	  stores = split_indexes[v][1] - split_indexes[v][0];

#if DEBUG_STEALING > 1
	  _fd_debug("[%d.%d] %d stores left at %d\n", tid, agent, stores, v);
#endif

	  // don't steal the last few stores from an agent
	  if (stores >= STEAL_THRESHOLD)
	    break;		// all is well, proceed to stealing

	  // won't steal from this agent, unlock its store
	  pthread_mutex_unlock(&stores_mutexes[v]);

#ifdef DEBUG_STEALING
	  _fd_debug("[%d.%d] not enough stores (%d) at %d\n", tid, agent,
		    stores, v);
#endif
	}

      if (++attempts == MAX_ATTEMPTS)
	{
	  // take a break
	  pthread_mutex_unlock(&thieves_mutex);

	  // the main thread won't keep trying
	  if (agent == -1)
	    {
	      _fd_debug("[%d] giving up stealing\n", tid);

	      return false;
	    }

	  pthread_spin_lock(&starving_lock);
	  as = agents_starving;
	  pthread_spin_unlock(&starving_lock);

	  if (as == nagents)
	    {
#ifdef DEBUG_STEALING
	      _fd_debug("[%d.%d] all agents starving...\n", tid, agent);
#endif

	      return false;
	    }

#ifdef STATS_STEALS
	  sts_slept[agent + 1]++;
#endif

#ifdef DEBUG_STEALING
	  _fd_debug("[%d.%d] pausing...\n", tid, agent);
#endif

	  nanosleep(&delay, NULL);

	  pthread_spin_lock(&starving_lock);
	  as = agents_starving;
	  pthread_spin_unlock(&starving_lock);

	  if (as == nagents)
	    {
#ifdef DEBUG_STEALING
	      _fd_debug("[%d.%d] all agents starving...\n", tid, agent);
#endif

	      return false;
	    }

	  // try again to find a victim
	  attempts = 0;

	  pthread_mutex_lock(&thieves_mutex);
	}

      do
	v = random() % nagents;
      while (v == agent);
    }

  if (agent != -1)
    {
      pthread_spin_lock(&starving_lock);
      agents_starving--;
      pthread_spin_unlock(&starving_lock);
    }

  // steal the store
  s = split_indexes[v][0]++;

#ifdef STORE_IN_POOL
  // XXX: there should be no side effects on the value of `store'
  if (agent != -1)
    alt_store = 0,
    store = store_ = split_stores;
#endif

  memcpy(store_, &split_stores_[v][s * fd_variables_count], STORE_SIZE);

#ifdef INDEX_IN_POOL
#ifndef STORE_IN_POOL
  if (variable)
#else
  if (variable && split_variable_[v][s] != -1)
#endif
    *variable = _fd_variables[split_variable_[v][s]];
#endif

  // this agent will likely become the one with the largest search space
  // (if work is being stolen by the controller, most_work will be set
  // to -1 which will force the next agent who tries to steal a store
  // to look for a suitable victim)
  most_work = agent;

  // everything needed has been copied out
  pthread_mutex_unlock(&stores_mutexes[v]);

  if (agent != -1)
    {
      // reset the indexes into the depository
#ifndef STORE_IN_POOL
      next_split_index = 0;
#else
      next_split_index = 1;
#endif
      base_split_index = 0;
    }

#if DEBUG_STEALING > 0
#ifdef INDEX_IN_POOL
  _fd_debug("[%d.%d] stole work (%d/%d) from %d (var %d)\n", tid, agent, s,
	    stores, v, split_variable_[v][s]);
#else
  _fd_debug("[%d.%d] stole work (%d/%d) from %d\n", tid, agent, s, stores, v);
#endif
#if DEBUG_STEALING > 2
  _fd_output("[%d.%d] ", tid, agent); _fd_cprint3(store_);
#endif
#endif

  pthread_mutex_unlock(&thieves_mutex);

#ifdef STATS_STEALS
  sts_done[agent + 1]++;
#endif

  return true;
#endif /* RANDOM_VICTIM */
}

// copy the last saved store to the agent's store
static bool _fd_restore_store(fd_int *variable)
{
  bool locked = false;

  *variable = NULL;

#ifndef STORE_IN_POOL
  if (next_split_index <= base_split_index)
#else
  if (next_split_index <= base_split_index + 1)
#endif /* STORE_IN_POOL */
#if STEAL_WORK < 1
    return false;
#else /* STEAL_WORK < 1 */
#ifndef INDEX_IN_POOL
    return _fd_steal_store(store, agent, 0);
#else
    return _fd_steal_store(store, variable, agent, 0);
#endif
#endif /* STEAL_WORK < 1 */

#if STEAL_WORK > 0
  if (next_split_index - base_split_index < INDEX_SAFE)
    {
      //_fd_debug("[%d.%d] have %d stores\n", tid, agent, next_split_index - base_split_index);
#ifndef FAST
      if (pthread_mutex_trylock(&stores_mutexes[agent]))
	_fd_debug("[%d.%d] *** found stores locked [%d,%d[\n", tid, agent,
		  base_split_index, next_split_index),
#endif
      pthread_mutex_lock(&stores_mutexes[agent]);
      //_fd_debug("[%d.%d] locked stores\n", tid, agent);

      locked = true;
    }
#endif /* STEAL_WORK > 0 */

#ifdef STATS_POOL
  if (next_split_index - base_split_index > pool_max_entries)
    pool_max_entries = next_split_index - base_split_index;
  pool_entries += next_split_index - base_split_index;
  ++pool_gets;
#endif

  --next_split_index;

#ifndef STORE_IN_POOL
  memcpy(store, &split_stores[next_split_index * fd_variables_count],
	 STORE_SIZE);
#else
  store = alt_store;
  alt_store = &split_stores[(next_split_index - 2) * fd_variables_count];
#endif

#ifndef STORE_IN_POOL
#  ifndef INDEX_IN_POOL
  *variable = split_variable[next_split_index];
#  else
  *variable = _fd_variables[split_variable[next_split_index]];
#  endif /* INDEX_IN_POOL */
#else
#  ifndef INDEX_IN_POOL
  *variable = split_variable[next_split_index - 1];
#  else
  assert(split_variable[next_split_index - 1] != -1);
  *variable = _fd_variables[split_variable[next_split_index - 1]];
#  endif /* INDEX_IN_POOL */
#endif /* STORE_IN_POOL */

#if STEAL_WORK > 0
  if (locked)
    pthread_mutex_unlock(&stores_mutexes[agent]);
#endif

  return true;
}

void _fd_statistics_pool()
{
#ifdef STATS_POOL
  fd__info("[%d.%d] backtracks %lu, pool size: mean %g, max %lu\n", tid,
	   agent, pool_gets, (double) pool_entries / (double) pool_gets,
	   pool_max_entries);
#endif
}

void _fd_statistics_steal()
{
#ifdef STATS_STEALS
  unsigned long ta = 0, td = 0;
# ifdef RANDOM_VICTIM
  unsigned long tc = 0, ts = 0;
# endif
  int i;

# ifndef RANDOM_VICTIM
  for (i = 0; i <= nagents; ++i)
    ta += sts_attempts[i], td += sts_done[i];

  fd__info("[%d] steals: tried %lu, accomplished %lu\n", tid, ta, td);
# else
  for (i = 0; i <= nagents; ++i)
    {
#  if STATS_STEALS > 1
      fd__info("[%d.%d] steals: tried %lu, checked %lu, slept %lu, accomplished %lu\n",
	       tid, i - 1, sts_attempts[i], sts_checks[i], sts_slept[i], sts_done[i]);
#  endif

      ta += sts_attempts[i]; td += sts_done[i];
      tc += sts_checks[i]; ts += sts_slept[i];
    }

  fd__info("[%d] steals: tried %lu, checked %lu, slept %lu, accomplished %lu\n",
	   tid, ta, tc, ts, td);
# endif /* RANDOM_VICTIM */
#endif
}