/* See udg_pack.h */
/* Last edited on 2014-05-15 21:12:00 by stolfilocal */

#define _GNU_SOURCE
#include <affirm.h>
#include <bool.h>
#include <ix.h>
#include <stdlib.h>
#include <assert.h>
#include <vec.h>

#include <udg_grid.h>
  
#include <udg_pack.h>

/* CELL PACKS */

void udg_pack_cells(udg_cell_id_t idb, udg_grid_size_t psz, udg_cell_id_t id[])
  { udg_pack_slot_index_t ix = 0;
    for (ix = 0; ix < psz; ix++)
      { id[ix] = udg_cell_shift(idb, ix); }
  }

udg_pack_slot_index_t udg_pack_find_cell
  ( udg_cell_id_t id, 
    udg_cell_id_t idb,
    udg_grid_size_t psz
  )
  {
    /* Get ranks of {id} and {idb}: */
    udg_rank_t r = udg_cell_rank(id);
    udg_rank_t rb = udg_cell_rank(idb);
    if (r != rb) { return udg_NOT_SLOT; }

    /* Get position of {id} and {idb} in that layer: */
    udg_grid_pos_t p = udg_cell_position(id);
    udg_grid_pos_t pb = udg_cell_position(idb);
 
    /* Get position of {id} relative to {idb}, beware of wrap-around: */
    udg_grid_size_t gsz = udg_grid_size(rb);
    udg_grid_pos_t e = (p - pb + gsz) % gsz;
    if (e >= psz) { return udg_NOT_SLOT; }
    return (udg_pack_slot_index_t)e;
  }

bool_t udg_pack_intersects_cell
  ( udg_cell_id_t id, 
    udg_cell_id_t idb,
    udg_grid_size_t psz
  )
  {
    /* Get rank of {idb}, its position in that layer, and size of layer: */
    udg_rank_t rb = udg_cell_rank(idb);
    udg_grid_pos_t pb = udg_cell_position(idb);
    udg_grid_size_t gszb = udg_grid_size(rb);
    
    /* Get rank of {id}: */
    udg_rank_t r = udg_cell_rank(id);
    
    /* If {id} is deeper than {idb}, get its ancestor of same rank: */
    if (r > rb) { id = id >> (r - rb); r = rb; }
    
    /* Get positions of min and max descendants of {id} on level {Er}: */
    udg_grid_pos_t p_lo = udg_cell_position(id << (rb - r));
    udg_grid_pos_t p_hi = udg_cell_position(((id + 1) << (rb - r)) - 1);
    assert(p_lo <= p_hi); /* A single cell can't wrap around. */

    /* Get rel position range {pp_lo..pp_hi} of pack in level {rb}: */ 
    udg_grid_pos_t pp_lo = pb;
    udg_grid_pos_t pp_hi = (pp_lo + psz - 1) % gszb;
    
    /* Make sure {pp_hi >= pp_lo}: */
    if (pp_hi < pp_lo) { pp_hi += gszb; }
    
    /* Check whether the ranges intersect (with or without wrap-around): */
    if (p_lo < pp_lo)
      { if ((p_hi < pp_lo) && (pp_hi < p_lo + gszb)) { return FALSE; } }
    else if (pp_lo < p_lo)
      { if (p_lo > pp_hi) { return FALSE; }}
    
    return TRUE;
  }

void udg_pack_interval_root_relative
  ( udg_cell_id_t idb,
    udg_grid_size_t psz, 
    interval_t *B
  )
  { udg_cell_interval_root_relative(idb, B);
    double cwd = HI((*B)) - LO((*B));
    double pwd = ((double)(psz-1))*cwd;
    HI((*B)) = HI((*B)) + pwd;
  }