/* See mdg_grid.h */
/* Last edited on 2009-03-06 03:56:21 by stolfi */

#define _GNU_SOURCE
  
#include "mdg_grid.h"

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

/* THE DYADIC MULTIGRID */

//#warning necessario
mdg_axis_t mdg_split_axis(mdg_dim_t d, mdg_rank_t r)
  { return r % d; }
  
#define mdg_GRID_POS_BITS(d,r,j)  (((r)+(d)-1-(j))/(d))
  /* Number of bits needed to represent a grid position within level {r}
    along axis {j} of a {d}-dimensional multigrid. */

//#warning necessario
void mdg_grid_size(mdg_dim_t d, mdg_rank_t r, mdg_grid_size_t gsz[])
  { int i;
    for (i = 0; i < d; i++) { gsz[i] = (1LL << mdg_GRID_POS_BITS(d,r,i)); }
  }

mdg_grid_pos_t mdg_max_grid_pos(mdg_dim_t d, mdg_rank_t r, mdg_axis_t i)
  { return (1LL << mdg_GRID_POS_BITS(d,r,i))-1; }

sign_t mdg_rank_cmp(mdg_rank_t *x, mdg_rank_t *y)
  { if ((*x) < (*y))
      { return -1; }
    else if ((*x) > (*y))
      { return +1; }
    else
      { return 0; }
  }
  
/* CELL INDICES */
  
mdg_rank_t mdg_cell_rank(mdg_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;
  }

//#warning necessario
void mdg_cell_position(mdg_dim_t d, mdg_cell_index_t k, mdg_grid_pos_t pos[])
  { mdg_cell_index_t t = ((1LL << mdg_cell_rank(k)) >> 1);
    int ax;
    for (ax = 0; ax < d; ax++) { pos[ax] = 0; }
    ax = 0;
    while (t != 0)
      { pos[ax] <<= 1; if ((k & t) != 0) { pos[ax] |= 1;  }
        ax++; if (ax >= d) { ax = 0; }
        t >>= 1;
      }
  }
  
mdg_cell_index_t mdg_cell_from_position(mdg_dim_t d, mdg_rank_t r, mdg_grid_pos_t pos[])
  { /* First cell of layer {r}: */
    mdg_cell_index_t k = (1LL << r);
    /* Limit for bit values that can be added to {k}: */
    mdg_cell_index_t tlim = (1LL << r);
    int ax;
    for (ax = 0; ax < d; ax++) 
      { mdg_grid_pos_t posax = pos[ax]; 
        /* Add {posax} to {k}, starting at bit {ax} with stride {d}: */
        uint64_t t = 1LL << ((r + d - ax) % d);
        while ((posax != 0) && (t < tlim))
          { if (posax & 1) 
              { if ((k & t) != 0) 
                  { k &= ~t; posax += 2; }
                else
                  { k |= t; }
              }
            posax >>= 1; t <<= d;
          }
      }
    return k;
  }

mdg_cell_index_t mdg_cell_shift(mdg_dim_t d, mdg_cell_index_t k, mdg_axis_t ax, mdg_grid_pos_t dp)
  { mdg_rank_t rank = mdg_cell_rank(k);
    /* Limit for bit values that can be added to {k}: */
    mdg_cell_index_t tlim = (1LL << rank);
    /* Add {dp} to {k}, starting at bit {ax} with stride {d}: */
    mdg_cell_index_t t = 1LL << ((rank + d - ax) % d);
    while ((dp != 0) && (t < tlim))
      { if (dp & 1) 
          { if ((k & t) != 0) 
              { k &= ~t; dp = dp + 2; }
            else
              { k |= t; }
          }
        dp >>= 1; t <<= d;
      }
    return k;
  }
  
mdg_cell_index_t mdg_cell_translate(mdg_dim_t d, mdg_cell_index_t k, mdg_grid_pos_t dp[])
  { mdg_rank_t rank = mdg_cell_rank(k);
    /* Limit for bit values that can be added to {k}: */
    mdg_cell_index_t tlim = (1LL << rank);
    int ax;
    for (ax = 0; ax < d; ax++) 
      { mdg_grid_pos_t dpax = dp[ax]; 
        /* Add {dpax} to {k}, starting at bit {ax} with stride {d}: */
        uint64_t t = 1LL << ((rank + d - ax) % d);
        while ((dpax != 0) && (t < tlim))
          { if (dpax & 1) 
              { if ((k & t) != 0) 
                  { k &= ~t; dpax += 2; }
                else
                  { k |= t; }
              }
            dpax >>= 1; t <<= d;
          }
      }
    return k;
  }

mdg_cell_index_t mdg_cell_index_add(mdg_dim_t d, mdg_cell_index_t ka, mdg_cell_index_t kb)
  { mdg_rank_t ra = mdg_cell_rank(ka);
    mdg_rank_t rb = mdg_cell_rank(kb);
    affirm(ra == rb, "unequal ranks");
    /* Limit for bit values that can be added to {ka}: */
    mdg_cell_index_t tlim = (1LL << ra);
    /* Adds the interleaved cell positions. */
    /* Variable {carry} contains the {d} carry bits, rotated. */
    uint64_t t = 1;
    int carry = 0, cd = (1 << d);
    /* Discard leading bit of {kb}: */
    kb -= tlim;
    while (((kb > 0) || (carry != 0)) && (t < tlim))
      { if (kb & carry & 1) 
          { carry |= cd; }
        else if ((kb | carry) & 1)
          { if ((ka & t) != 0) 
              { ka &= ~t; carry |= cd; }
            else
              { ka |= t; }
          }
        kb >>= 1; carry >>= 1; t <<= 1;
      }
    return ka;
  }

//#warning necessario
void mdg_cell_box_root_relative(mdg_dim_t d, mdg_cell_index_t k, interval_t B[])
  { mdg_grid_pos_t pos[mdg_MAX_GRID_DIM];
    mdg_cell_position(d, k, pos);
    
    mdg_grid_size_t gsz[mdg_MAX_GRID_DIM];
    mdg_grid_size(d, mdg_cell_rank(k), gsz);
    
    int i;
    for (i = 0; i < d; i++)
      { double scale = 1.0/((double)(gsz[i]));
        LO(B[i]) = scale*((double)pos[i]);
        HI(B[i]) = scale*((double)(pos[i] + 1));
      }
  }

/* Sizes of canonical cells for various dimensions: */
static const double mdg_root_sizes_1[1] = 
  { 1.0 };
  
static const double mdg_root_sizes_2[2] = 
  { 1.0, 0.7071067811865475 };

static const double mdg_root_sizes_3[3] = 
  { 1.0, 0.7937005259840997, 0.6299605249474366 };

static const double mdg_root_sizes_4[4] = 
  { 1.0, 0.8408964152537145, 0.7071067811865475, 0.5946035575013605 };

static const double *mdg_root_sizes[mdg_MAX_GRID_DIM] = 
  { mdg_root_sizes_1,
    mdg_root_sizes_2,
    mdg_root_sizes_3,
    mdg_root_sizes_4
  };
  /* The pointer {mdg_root_sizes[d]} is the address of a vector which gives the
    extent of the cannonical cell of dimension {d} along each axis. */
//#warning necessario
const double *mdg_cell_root_canonical_sizes(mdg_dim_t d)
  { if (d == 0) { return NULL; }
    demand(d <= mdg_MAX_GRID_DIM, "invalid dimension");
    return mdg_root_sizes[d-1];
  }

//#warning necessario
void mdg_cell_box_canonical(mdg_dim_t d, mdg_cell_index_t k, interval_t B[])
  { mdg_cell_box_root_relative(d, k, B); 
    int i;
    const double *sz = mdg_root_sizes[d-1];
    for (i = 0; i < d; i++)
      { double szi = sz[i];
        LO(B[i]) *= szi;
        HI(B[i]) *= szi;
      }
  }
  
/* CELL PACKS */

uint64_t mdg_pack_slot_count(mdg_dim_t d, mdg_grid_size_t psz[])
  { uint64_t n = 1;
    int i;
    for (i = 0; i < d; i++) { n *= psz[i]; }
    return n;
  }

void mdg_pack_cells
  ( mdg_dim_t d, 
    mdg_cell_index_t b,
    mdg_grid_size_t psz[], 
    mdg_cell_index_t k[]
  )
  { mdg_grid_pos_t rpos[d]; /* Relative position of cell in pack. */
    /* Indexing steps: */
    ix_step_t pst[d];
    pst[0] = 1;
    { mdg_axis_t i; for (i=1; i <d; i++) { pst[i] = pst[i-1]*psz[i-1]; } }
    ix_fill(d, rpos, 0);
    int nC = 0;
    do
      { k[nC] = mdg_cell_translate(d, b, rpos);  nC++; }
    while (! ix_next(d, rpos, psz, ix_order_F, pst, NULL, NULL, NULL, NULL, NULL));
  }

mdg_pack_slot_index_t mdg_pack_find_cell
  ( mdg_dim_t d, 
    mdg_cell_index_t k, 
    mdg_cell_index_t b,
    mdg_grid_size_t psz[]
  )
  {
    mdg_rank_t kr = mdg_cell_rank(k);
    mdg_rank_t br = mdg_cell_rank(b);
    if (kr != br) { return mdg_NOT_SLOT; }

    mdg_grid_pos_t kp[mdg_MAX_GRID_DIM];
    mdg_cell_position(d, k, kp);
    
    mdg_grid_pos_t bp[mdg_MAX_GRID_DIM];
    mdg_cell_position(d, b, bp);
 
    int i;
    int pkx = 1;
    mdg_pack_slot_index_t ix = 0;
    for (i = 0; i < d; i++)
      { demand(psz[i] > 0, "bad pack size");
        mdg_grid_pos_t gszi = mdg_max_grid_pos(d, br, i);
        /* Get relative position of {k} in pack along axis {i}: */
        mdg_grid_pos_t ei = (kp[i] - bp[i] + gszi) % gszi;
        if (ei >= psz[i]) { return mdg_NOT_SLOT; }
        ix += ei * pkx;
        pkx *= psz[i];
      }
    return pkx;
  }

bool_t mdg_pack_intersects_cell
  ( mdg_dim_t d, 
    mdg_cell_index_t k, 
    mdg_cell_index_t b,
    mdg_grid_size_t psz[]
  )
  {
    mdg_rank_t br = mdg_cell_rank(b);
    mdg_grid_pos_t bp[mdg_MAX_GRID_DIM];
    mdg_cell_position(d, b, bp);
    
    /* If {k} is deeper than {b}, get its ancestor of same rank: */
    mdg_rank_t kr = mdg_cell_rank(k);
    if (kr > br) { k = k >> (kr - br); kr = br; }
    /* Get posns of min and max descendants of {k} on level {Er}: */
    mdg_grid_pos_t kp_lo[mdg_MAX_GRID_DIM];
    mdg_cell_position(d, k << (br - kr), kp_lo);
    mdg_grid_pos_t kp_hi[mdg_MAX_GRID_DIM];
    mdg_cell_position(d, ((k + 1) << (br - kr)) - 1, kp_hi);

    /* Get grid sizes on level {br}: */
    mdg_grid_size_t bgsz[mdg_MAX_GRID_DIM];
    mdg_grid_size(d, br, bgsz);
    
    int i;
    for (i = 0; i < d; i++)
      { demand(psz[i] > 0, "bad pack size");
        mdg_grid_size_t bgszi = bgsz[i];
        /* Get rel position range {plop..phip} of pack in level {br}: */ 
        mdg_grid_pos_t plop = bp[i];
        mdg_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: */
        mdg_grid_pos_t klop = kp_lo[i];
        mdg_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 mdg_pack_box_root_relative
  ( mdg_dim_t d, 
    mdg_cell_index_t b,
    mdg_grid_size_t psz[], 
    interval_t B[]
  )
  { mdg_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 mdg_cell_print(FILE *wr, mdg_dim_t d, mdg_cell_index_t k)
  { 
    int i;
    mdg_grid_pos_t kp[mdg_MAX_GRID_DIM];
    mdg_rank_t kr = mdg_cell_rank(k);
    mdg_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(mdg_cell_index_vec_t,mdg_cell_index_vec,mdg_cell_index_t);