#include "liblocky.h"

#include <assert.h>
#include <stdlib.h>


void blky_pathfinder_init(blky_pathfinder_t* pf) {
  assert(pf->block_num   > 0);
  assert(pf->step_branch > 0);
  assert(pf->seed        > 0);

  assert(pf->block_num >= pf->step_branch);
  assert(pf->hopping_algo == BLKY_PATHFINDER_HOPPING_ALGO_XORSHFIT);

  pf->step_first = NULL;
  pf->step_last  = NULL;

  pf->step_bytes =
    sizeof(blky_pathfinder_step_t) +
    sizeof(uint32_t)*(pf->block_num-1);

  pf->probs = calloc(sizeof(*pf->probs), pf->block_num);
  assert(pf->probs);

  pf->probs_prev = calloc(sizeof(*pf->probs_prev), pf->block_num);
  assert(pf->probs_prev);
}

void blky_pathfinder_deinit(blky_pathfinder_t* pf) {
  free(pf->probs);
  free(pf->probs_prev);
}

void blky_pathfinder_feed(blky_pathfinder_t* pf, const double* probs) {
  blky_pathfinder_step_t* step = NULL;
  if (++pf->steps > 1) {
    step = calloc(pf->step_bytes, 1);
    assert(step);
  }

  for (uint32_t bi = 0; bi < pf->block_num; ++bi) {
    const double   prob  = probs[bi];
    const uint64_t pbase = blky_numeric_xorshift64_rev(bi);
    for (uint32_t si = 0; si < pf->step_branch; ++si) {
      const uint64_t prev_seed = pbase - si*pf->block_num/pf->step_branch;
      const uint32_t prev_bi   = (uint32_t) (prev_seed % pf->block_num);
      const double   prev_prob = pf->probs_prev[prev_bi];
      const double   sum       = prev_prob + prob;
      if (pf->probs[bi] < sum) {
        pf->probs[bi] = sum;
        if (step) step->indices[prev_bi] = bi;
      }
    }
  }

  if (step) {
    if (pf->step_last) {
      pf->step_last->next = step;
      pf->step_last       = step;
    } else {
      pf->step_first = pf->step_last = step;
    }
  }

  double* temp = pf->probs;
  pf->probs      = pf->probs_prev;
  pf->probs_prev = temp;
}