/* See udg_grid.h */
/* Last edited on 2009-08-21 16:01:45 by stolfilocal */

#define _GNU_SOURCE
#include "affirm.h"
#include "bool.h"
#include "indexing.h"
#include <stdlib.h>
#include <assert.h>
  
#include "udg_grid.h"
#include "mdg_grid.h"

box_axis_set_t dg_axis_set_complement(mdg_dim_t d, mdg_axis_set_t A)
  {  return set32_difference(set32_range(0,d-1), A); }

/* THE DYADIC MULTIGRID */

udg_grid_size_t udg_grid_size(udg_rank_t r)
  { return (1LL << r);  }

udg_grid_pos_t udg_max_grid_pos(udg_rank_t r)
  { return (1LL << r) - 1; }

sign_t udg_rank_cmp(udg_rank_t *x, udg_rank_t *y)
  { if ((*x) < (*y))
      { return -1; }
    else if ((*x) > (*y))
      { return +1; }
    else
      { return 0; }
  }
  
/* CELL INDICES */
  
udg_rank_t udg_cell_rank(udg_cell_index_t k)
  { int r = 0;
    while (k > (1LL << 16)) { k >>= 16; r+= 16; }
    while (k > (1LL << 4)) { k >>= 4; r+= 4; }
    while (k > 1) { k >>= 1; r+= 1; }
    return r;
  }

udg_grid_pos_t udg_cell_position(udg_cell_index_t k)
  { udg_cell_index_t t = ((1LL << udg_cell_rank(k)) >> 1);
    return k - t;
  }
  
udg_cell_index_t udg_cell_from_position(udg_rank_t r, udg_grid_pos_t pos)
  { /* First cell of layer {r}: */
    udg_cell_index_t k0 = (1LL << r);
    /* Number of cells {sz} in layer {r}: */
    udg_cell_index_t sz = (1LL << r);
    /* Displacement {t} from {k0} is {pos % sz}: */
    uint64_t t = pos & (sz - 1);
    /* Compute index: */
    return k0 + t;
  }

udg_cell_index_t udg_cell_shift(udg_cell_index_t k, udg_grid_pos_t dp)
  { udg_rank_t r = udg_cell_rank(k);
    /* First cell of layer {r}: */
    udg_cell_index_t k0 = (1LL << r);
    /* Number of cells {sz} in layer {r}: */
    udg_cell_index_t sz = (1LL << r);
    /* Displacement {t} from {k0} is {(k - k0 + dp) % sz}: */
    uint64_t t = (k - k0 + dp) & (sz - 1);
    /* Compute index: */
    return k0 + t;
  }

void udg_cell_interval_root_relative(udg_cell_index_t k, interval_t *B)
  { 
    udg_grid_pos_t pos = udg_cell_position(k);
    udg_grid_size_t gsz = udg_grid_size(udg_cell_rank(k));

    double scale = 1.0/((double)gsz);
    LO(B) = scale*((double)pos);
    HI(B) = scale*((double)(pos + 1));
  }

/* CELL PACKS */

void udg_pack_cells(udg_cell_index_t b, udg_grid_size_t psz, udg_cell_index_t k[])
  { 
    udg_rank_t br = udg_cell_rank(b);
    udg_grid_pos_t bp = udg_cell_position(b);
    udg_grid_pos_t gsz = udg_max_grid_pos(br);
    demand(psz > 0, "bad pack size");
    
    udg_pack_slot_index_t ix;
    for (ix = 0; ix < psz; ix++)
      { udg_grid_pos_t kp = (bp + ix) % gsz;
        k[ix] = udg_cell_from_position(br, kp);
      }
  }

udg_pack_slot_index_t udg_pack_find_cell
  ( udg_cell_index_t k, 
    udg_cell_index_t b,
    udg_grid_size_t psz[]
  )
  {
    demand(psz > 0, "bad pack size");
    udg_rank_t kr = udg_cell_rank(k);
    udg_rank_t br = udg_cell_rank(b);
    if (kr != br) { return udg_NOT_SLOT; }

    udg_grid_pos_t kp = udg_cell_position(k);
    udg_grid_pos_t bp = udg_cell_position(b);
 
    udg_grid_pos_t gsz = udg_max_grid_pos(br);
    /* Get relative position of {k} in pack: */
    udg_pack_slot_index_t ix = (kp - bp + gsz) % gsz;
    if (ix >= psz) { return udg_NOT_SLOT; }
    return ix;
  }

bool_t udg_pack_intersects_cell
  ( udg_cell_index_t k, 
    udg_cell_index_t b,
    udg_grid_size_t psz[]
  )
  {
    udg_rank_t br = udg_cell_rank(b);
    udg_grid_pos_t bp = udg_cell_position(b);
    
    /* If {k} is deeper than {b}, get its ancestor of same rank: */
    udg_rank_t kr = udg_cell_rank(k);
    if (kr > br) { k = k >> (kr - br); kr = br; }
    /* Get posns of min and max descendants of {k} on level {br}: */
    udg_grid_pos_t kp_lo = udg_cell_position(d, k << (br - kr), kp_lo);
    udg_grid_pos_t kp_hi[udg_MAX_GRID_DIM];
    udg_cell_position(d, ((k + 1) << (br - kr)) - 1, kp_hi);

    /* Get grid sizes on level {br}: */
    udg_grid_size_t bgsz[udg_MAX_GRID_DIM];
    udg_grid_size(d, br, bgsz);
    
    int i;
    for (i = 0; i < d; i++)
      { demand(psz[i] > 0, "bad pack size");
        udg_grid_size_t bgszi = bgsz[i];
        /* Get rel position range {plop..phip} of pack in level {br}: */ 
        udg_grid_pos_t plop = bp[i];
        udg_grid_pos_t phip = (plop + psz[i] - 1) % bgszi;
        /* Make sure {phip >= plop}: */
        if (phip < plop) { phip += bgszi; }
        /* Get rel position range {klop..khip} of {k} in the same level: */
        udg_grid_pos_t klop = kp_lo[i];
        udg_grid_pos_t khip = kp_hi[i];
        assert(klop <= khip); /* A single cell can't wrap around. */
        /* Check whether the ranges intersect (with or without wrap-around): */
        if (klop < plop)
          { if ((khip < plop) && (phip < klop + bgszi)) { return FALSE; } }
        else if (plop < klop)
          { if (klop > phip) { return FALSE; }}
      }
    return TRUE;
  }

void udg_pack_box_root_relative
  ( udg_dim_t d, 
    udg_cell_index_t b,
    udg_grid_size_t psz[], 
    interval_t B[]
  )
  { udg_cell_box_root_relative(d, b, B);
    int i;
    for (i = 0; i < d; i++)
      { demand(psz[i] > 0, "bad pack size");
        HI(B[i]) = HI(B[i]) + (psz[i]-1)*(HI(B[i]) - LO(B[i]));
      }
  }

/* PRINTOUT */

void udg_cell_print(FILE *wr, udg_dim_t d, udg_cell_index_t k)
  { 
    int i;
    udg_grid_pos_t kp[udg_MAX_GRID_DIM];
    udg_rank_t kr = udg_cell_rank(k);
    udg_cell_position(d, k, kp);
    fprintf(wr, "%d:(", (int)kr);
    for (i = 0; i < d; i++) 
      { fprintf(wr, "%s%d", (i == 0 ? "" : ","), (int)kp[i]); }
    fprintf(wr, ")");
  }
  
/* MANIPULATION OF CELL INDEX VECTORS */

vec_typeimpl(udg_cell_index_vec_t,udg_cell_index_vec,udg_cell_index_t);