/* See dgtree.h */
/* Last edited on 2004-08-17 23:08:40 by stolfi */

#include <js.h>
#include <dgbasic.h>
#include <stdlib.h>

/* INTERVALS */

dg_Interval dg_interval_split(dg_Interval v, dg_BinaryDir dir)
  { 
    if (dir == BLO)
      { return (dg_Interval){{ v.end[0], (v.end[0] + v.end[1])/2 }}; }
    else
      { return (dg_Interval){{ (v.end[0] + v.end[1])/2, v.end[1] }}; }
  }

dg_Interval dg_interval_join(dg_Interval *u, dg_Interval *v)
  { double ulo = u->end[0], uhi = u->end[1];
    double vlo = v->end[0], vhi = v->end[1];
    if (ulo > uhi) 
      { return *v; }
    else if (vlo > vhi)
      { return *u; }
    else
      { dg_Interval w;
        w.end[0] = (ulo < vlo ? ulo : vlo);
        w.end[1] = (uhi > vhi ? uhi : vhi);
        return w;
      }
  }

dg_Interval dg_interval_meet(dg_Interval *u, dg_Interval *v)
  { double ulo = u->end[0], uhi = u->end[1];
    double vlo = v->end[0], vhi = v->end[1];
    if (ulo > uhi)
      { return *u; }
    if (vlo > vhi)
      { return *v; }
    else
      { dg_Interval w;
        w.end[0] = (ulo > vlo ? ulo : vlo);
        w.end[1] = (uhi < vhi ? uhi : vhi);
        return w;
      }
  }

/* BOXES */

void dg_box_lo_corner(dg_Dim d, dg_Interval B[], double p[])
  { int i;
    for (i = 0; i < d; i++) { p[i] = B[i].end[BLO]; }
  }

void dg_box_hi_corner(dg_Dim d, dg_Interval B[], double p[])
  { int i;
    for (i = 0; i < d; i++) { p[i] = B[i].end[BHI]; }
  }

void dg_box_corner(dg_Dim d, dg_Interval B[], dg_BinaryDir dir[], double p[])
  { int i;
    for (i = 0; i < d; i++) { p[i] = B[i].end[dir[i]]; }
  }

double dg_box_size(dg_Dim d, dg_Interval B[])
  { int i;
    double sz = 0;
    for (i = 0; i < d; i++) 
      { double szi = B[i].end[BHI]-B[i].end[BLO]; if (szi > sz) { sz = szi; } }
    return sz;
  }

void dg_box_join(dg_Dim d, dg_Interval A[], dg_Interval B[], dg_Interval C[])
  { int i;
    for (i = 0; i < d; i++) { C[i] = dg_interval_join(&(A[i]), &(B[i])); }
  }

void dg_box_meet(dg_Dim d, dg_Interval A[], dg_Interval B[], dg_Interval C[])
  { int i;
    for (i = 0; i < d; i++) { C[i] = dg_interval_meet(&(A[i]), &(B[i])); } 
  }

void dg_point_box_map(dg_Dim d, double z[], dg_Interval B[], double x[])
  { int i, j;
    for (i = 0, j = 0; i < d; i++)
      { double lo = B[i].end[0];
        double hi = B[i].end[1];
        if (lo < hi)
          { double wd = hi - lo;
            x[i] = lo + wd*z[j];
            j++;
          }
        else
          { affirm(lo == hi, "empty box"); 
            x[i] = lo;
          }
      }
  }

void dg_point_box_unmap(dg_Dim d, double x[], dg_Interval B[], double z[])
  { int i, j;
    for (i = 0, j = 0; i < d; i++)
      { double lo = B[i].end[0];
        double hi = B[i].end[1];
        if (lo < hi)
          { double wd = hi - lo;
            z[j] = (x[i] - lo)/wd;
            j++;
          }
        else
          { affirm(lo == hi, "empty box"); }
      }
  }

void dg_box_box_map(dg_Dim d, dg_Interval Z[], dg_Interval B[], dg_Interval X[])
  { int i, j;
    for (i = 0, j = 0; i < d; i++)
      { double lo = B[i].end[0];
        double hi = B[i].end[1];
        if (lo < hi)
          { double wd = hi - lo;
            X[i].end[0] = lo + wd*Z[j].end[0];
            X[i].end[1] = lo + wd*Z[j].end[1];
            j++;
          }
        else
          { affirm(lo == hi, "empty box"); 
            X[i].end[0] = X[i].end[1] = lo;
          }
      }
  }

void dg_box_box_unmap(dg_Dim d, dg_Interval X[], dg_Interval B[], dg_Interval Z[])
  { int i, j;
    for (i = 0, j = 0; i < d; i++)
      { double lo = B[i].end[0];
        double hi = B[i].end[1];
        if (lo < hi)
          { double wd = hi - lo;
            Z[j].end[0] = (X[i].end[0] - lo)/wd;
            Z[j].end[1] = (X[i].end[1] - lo)/wd;
            j++;
          }
        else
          { affirm(lo == hi, "empty box"); }
      }
  }

/* BOX ORIENTATION */

int dg_axes_count(dg_Axes A)
  { int n = 0;
    while (A != 0) { if (A & 1) { n++; }  A >>= 1; }
    return n;
  }
    
dg_Axes dg_axes_complement(dg_Dim d, dg_Axes A)
  { dg_Axes all = (1<<d) - 1;
    return all & (~A);
  }
    
dg_Axis dg_axis(dg_Axes A, dg_AxisIndex j)
  { dg_Axis ax = 0;
    while (A != 0) 
      { if ((A & 1) == 1) { if (j == 0) { return ax; } j--; } 
        A >>= 1; ax++;
      }
    return DG_NO_AXIS;
  }

dg_AxisIndex dg_find_axis(dg_Axes A, dg_Axis ax)
  { dg_AxisIndex j = 0;
    while (ax > 0) { if ((A & 1) == 1) { j++; } A >>= 1; ax--; }
    return ((A & 1) == 1 ? j : DG_AXIS_NOT_FOUND);
  }

/* RELATIVE FACE INDICES */

dg_Dim dg_face_dimension(dg_Dim m, dg_FaceIndex fi)
  { while (fi != 0)
      { dg_SignedDir dirj = ((fi + 1) % 3) - 1; 
        if (dirj != SMD) { m--; }
        fi /= 3;
      }
    return m;
  }

dg_SignedDir dg_face_position(dg_FaceIndex fi, dg_AxisIndex j)
  { while (j > 0) { j--; fi /= 3; }
    return ((fi + 1) % 3) - 1; 
  }

void dg_face_signature(dg_Dim m, dg_FaceIndex fi, dg_SignedDir dir[])
  { dg_AxisIndex j;
    for (j = 0; j < m; j++)
      { dir[j] = ((fi + 1) % 3) - 1; 
        fi /= 3;
      }
  }
 
void dg_face_rel_box(dg_Dim m, dg_FaceIndex fi, dg_Interval box[])
  { dg_AxisIndex j;
    for (j = 0; j < m; j++)
      { dg_SignedDir dirj = ((fi + 1) % 3) - 1; 
        double lo = 0.0, hi = 1.0;
        if (dirj == SLO)
          { hi = 0.0; }
        else if (dirj == SHI)
          { lo = 1.0; }
        box[j] = (dg_Interval){{lo,hi}};
        fi /= 3;
      }
  }

void dg_face_print(FILE *wr, dg_Dim m, dg_FaceIndex fi)
  { dg_AxisIndex j;
    for (j = 0; j < m; j++)
      { int dj = (fi % 3); fprintf(stderr, "%c", ("0+-")[dj]);  fi /= 3; }
  }
  
/* THE DYADIC MULTIGRID */

dg_Axis dg_split_axis(dg_Dim d, dg_Rank r)
  { return r % d; }
  
#define DG_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. */

void dg_grid_size(dg_Dim d, dg_Rank r, dg_GridSize sz[])
  { int i;
    for (i = 0; i < d; i++) { sz[i] = (1LL << DG_GRID_POS_BITS(d,r,i)); }
  }

dg_GridPos dg_max_grid_pos(dg_Dim d, dg_Rank r, dg_Axis i)
  { return (1LL << DG_GRID_POS_BITS(d,r,i))-1; }

/* CELL INDICES */
  
dg_Rank dg_cell_rank(dg_CellIndex k)
  { int r = 0;
    while (k > (1<<16)) { k >>= 16; r+= 16; }
    while (k > (1<<4)) { k >>= 4; r+= 4; }
    while (k > 1) { k >>= 1; r+= 1; }
    return r;
  }

void dg_cell_position(dg_Dim d, dg_CellIndex k, dg_GridPos pos[])
  { dg_CellIndex t = ((1LL << dg_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;
      }
  }
  
dg_CellIndex dg_cell_shift(dg_Dim d, dg_CellIndex k, dg_Axis ax, dg_GridPos dp)
  { dg_Rank rank = dg_cell_rank(k);
    /* Limit for bit values that can be added to {k}: */
    dg_CellIndex tlim = (1LL << rank);
    /* Add {dp} to {k}, starting at bit {ax} with stride {d}: */
    dg_CellIndex t = 1 << ((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;
  }
  
dg_CellIndex dg_cell_translate(dg_Dim d, dg_CellIndex k, dg_GridPos dp[])
  { dg_Rank rank = dg_cell_rank(k);
    /* Limit for bit values that can be added to {k}: */
    dg_CellIndex tlim = (1LL << rank);
    int ax;
    for (ax = 0; ax < d; ax++) 
      { dg_GridPos dpax = dp[ax]; 
        /* Add {dpax} to {k}, starting at bit {ax} with stride {d}: */
        nat64 t = 1 << ((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;
  }

dg_CellIndex dg_cell_index_add(dg_Dim d, dg_CellIndex ka, dg_CellIndex kb)
  { dg_Rank ra = dg_cell_rank(ka);
    dg_Rank rb = dg_cell_rank(kb);
    affirm(ra == rb, "unequal ranks");
    /* Limit for bit values that can be added to {ka}: */
    dg_CellIndex tlim = (1LL << ra);
    /* Adds the interleaved cell positions. */
    /* Variable {carry} contains the {d} carry bits, rotated. */
    nat64 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;
  }

/* LOCUS IN A MULTIGRID */

dg_Dim dg_locus_dimension(dg_Dim d, dg_Locus E)
  { return d - dg_axes_count(E.norm); }

dg_Axis dg_locus_normal_axis(dg_Dim d, dg_Locus E, dg_AxisIndex j)
  { return dg_axis(E.norm,j); }

dg_Axis dg_locus_spanning_axis(dg_Dim d, dg_Locus E, dg_AxisIndex j)
  { return dg_axis(dg_axes_complement(d,E.norm),j); }

dg_Locus dg_locus_minimize_rank(dg_Dim d, dg_Locus E)
  { dg_Rank r = dg_cell_rank(E.cell);
    dg_Axes NrmE = E.norm;
    while
      ( dg_axis_belongs((r +d - 1) % d, NrmE) && 
        ((E.cell & 1LL) == 0)
      )
      { E.cell >>= 1; r--; }
    return E;
  }

/* FACES AND SUB-FACES OF LOCI */

dg_Locus dg_locus_facet(dg_Dim d, dg_Locus E, dg_Axis ax, dg_SignedDir dir)
  {
    dg_Axes da = 1 << ax;
    affirm(ax < d, "bad axis");
    if ((E.norm & da) != 0)
      { /* Locus {E} is already normal to axis {ax}: */
        return E;
      }
    else if (dir == SLO)
      { /* Just add {ax} to the normal set: */
        return dg_locus(E.norm | da, E.cell); 
      } 
    else if (dir == SHI)
      { /* Must increment the cell index along axis {ax}: */
        return dg_locus(E.norm | da, dg_cell_shift(d,E.cell,ax,1)); 
      }
    else
      { /* Asked {dir == SMD}, means {E} itself: */
        return E; 
      } 
  }

dg_Locus dg_locus_face(dg_Dim d, dg_Locus E, dg_FaceIndex fi)
  { int ax = 0;
    dg_Axes ES = dg_axes_complement(d, E.norm); 
    while (ES != 0)
      { if (ES & 1)
          { dg_SignedDir dir = ((fi + 1) % 3) - 1; 
            if (dir != SMD) { E = dg_locus_facet(d, E, ax, dir); }
            fi /= 3;
          }
        ax++; ES >>= 1;
      }
    return E;
  }

dg_Locus dg_locus_extend(dg_Dim d, dg_Locus E, dg_Axis ax, dg_SignedDir dir)
  {
    dg_Axes da = 1 << ax;
    affirm(ax < d, "bad axis");
    if ((E.norm & da) == 0)
      { /* Locus {E} is already parallel to axis {ax}: */
        return E;
      }
    else if (dir == SHI)
      { /* Just remove {ax} from the normal set: */
        return dg_locus(E.norm & (~da), E.cell); 
      } 
    else if (dir == SLO)
      { /* Must decrement the cell index along axis {ax}: */
        return dg_locus(E.norm & (~da), dg_cell_shift(d, E.cell, ax, -1)); 
      }
    else
      { /* Asked {dir == SMD}, means {E} itself: */
        return E; 
      } 
  }

dg_Locus dg_locus_face_from_signature(dg_Dim d, dg_Locus E, dg_SignedDir dir[])
  { dg_AxisIndex j = 0; dg_Axis ax = 0;
    dg_Axes ES = dg_axes_complement(d,E.norm); 
    while (ES != 0)
      { if (ES & 1) 
          { dg_SignedDir dirj = dir[j];
            if (dirj != SMD) { E = dg_locus_facet(d, E, ax, dirj); }
            j++;
          }
        ax++; ES >>= 1;
      }
    return E;
  }
  
/* Sizes of canonical cells for various dimensions: */
static double dg_root_size[10] = 
  { 1.0,                 
    1.0, 0.7071067811865475,
    1.0, 0.7937005259840997, 0.6299605249474366,
    1.0, 0.8408964152537145, 0.7071067811865475, 0.5946035575013605 
  };

void dg_cell_box_root_relative(dg_Dim d, dg_CellIndex k, dg_Interval B[])
  { dg_GridPos pos[DG_MAX_GRID_DIM];
    dg_cell_position(d, k, pos);
    
    dg_GridSize sz[DG_MAX_GRID_DIM];
    dg_grid_size(d, dg_cell_rank(k), sz);
    
    int i;
    for (i = 0; i < d; i++)
      { double scale = 1.0/((double)(sz[i]));
        B[i].end[0] = scale*((double)pos[i]);
        B[i].end[1] = scale*((double)(pos[i] + 1));
      }
  }

void dg_cell_box_canonical(dg_Dim d, dg_CellIndex k, dg_Interval B[])
  { dg_cell_box_root_relative(d, k, B); 
    int i, j;
    for (i = 0, j = d*(d-1)/2; i < d; i++, j++)
      { double sz = dg_root_size[j];
        B[i].end[0] *= sz;
        B[i].end[1] *= sz;
      }
  }
  
void dg_locus_box_root_relative(dg_Dim d, dg_Locus E, dg_Interval B[])
  {
    int i;
    dg_cell_box_root_relative(d, E.cell, B);
    for (i = 0; i < d; i++)
      { if (dg_axis_belongs(i, E.norm)) 
          { B[i].end[1] = B[i].end[0]; }
      }
  }

bool dg_cell_intersects_star(dg_Dim d, dg_CellIndex k, dg_Locus E)
  {
    dg_Rank Er = dg_cell_rank(E.cell);
    dg_GridPos Ep[DG_MAX_GRID_DIM];
    dg_cell_position(d, E.cell, Ep);
    
    /* If {k} is deeper than {E}, get its ancestor of same rank: */
    dg_Rank kr = dg_cell_rank(k);
    if (kr > Er) { k = k >> (kr - Er); kr = Er; }
    /* Get posns of min and max descendants of {k} on level {Er}: */
    dg_GridPos kp_lo[DG_MAX_GRID_DIM];
    dg_cell_position(d, k << (Er - kr), kp_lo);
    dg_GridPos kp_hi[DG_MAX_GRID_DIM];
    dg_cell_position(d, ((k + 1) << (Er - kr)) - 1, kp_hi);

    /* Get grid sizes on level {Er}: */
    dg_GridSize sz[DG_MAX_GRID_DIM];
    dg_grid_size(d, Er, sz);
    
    int i;
    for (i = 0; i < d; i++)
      { dg_GridPos Epi = Ep[i];
        if ((Epi < kp_lo[i]) || (Epi > kp_hi[i]))
          { /* Try adjacent cell of {K(E)}, if any: */
            if (dg_axis_belongs(i, E.norm))
              { dg_GridSize m = sz[i];
                Epi = (Epi + m - 1) % m;
                if ((Epi < kp_lo[i]) || (Epi > kp_hi[i]))
                  { return FALSE; }
              }
            else
              { return FALSE; }
          }
      }
    return TRUE;
  }

void dg_locus_star(dg_Dim d, dg_Locus E, dg_CellIndex k[])
  {
    dg_GridPos pos[DG_MAX_GRID_DIM];
    dg_cell_position(d, E.cell, pos);
    int i, ix;
    k[0] = E.cell; ix = 1;
    for (i = 0; i < d; i++)
      { if (dg_axis_belongs(i, E.norm))
          { /* Copy all cells already found, shift the rest down: */
            int ix0;
            for (ix0 = 0; ix0 < ix; ix0++)
              { k[ix + ix0] = k[ix0];
                k[ix0] = dg_cell_shift(d, k[ix0], i, -1);
              }
            ix = 2*ix;
          }
      }
  }

dg_WingIndex dg_cell_index_in_star(dg_Dim d, dg_CellIndex k, dg_Locus E)
  {
    dg_Rank k_rank = dg_cell_rank(k);
    dg_Rank E_rank = dg_cell_rank(E.cell);
    if (k_rank != E_rank) { return DG_NOT_WING; }

    dg_GridPos k_pos[DG_MAX_GRID_DIM];
    dg_cell_position(d, k, k_pos);
    
    dg_GridPos E_pos[DG_MAX_GRID_DIM];
    dg_cell_position(d, E.cell, E_pos);
 
    int i, j;
    dg_WingIndex ix = 0;
    for (i = 0, j = 0; i < d; i++)
      { if (dg_axis_belongs(i, E.norm))
          { if (k_pos[i] != E_pos[i])
              { dg_GridPos m = dg_max_grid_pos(d, E_rank, i);
                dg_GridPos dp = (E_pos[i] + m - k_pos[i]) % m;
                if (dp != 1) { return DG_NOT_WING; }
                ix += (1 << j);
              }
            j++;
          }
        else
          { if (k_pos[i] != E_pos[i]) 
              { return DG_NOT_WING; }
          }
      }
    return ix;
  }

void dg_star_box_root_relative(dg_Dim d, dg_Locus E, dg_Interval B[])
  {
    int i;
    dg_cell_box_root_relative(d, E.cell, B);
    for (i = 0; i < d; i++)
      { if (dg_axis_belongs(i, E.norm))
          { B[i].end[0] = 2*B[i].end[0] - B[i].end[1]; }
      }
  }

/* PRINTOUT */

void dg_cell_print(FILE *wr, dg_Dim d, dg_CellIndex k)
  { 
    int i;
    dg_GridPos kp[DG_MAX_GRID_DIM];
    dg_Rank kr = dg_cell_rank(k);
    dg_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, ")");
  }
  
void dg_locus_print(FILE *wr, dg_Dim d, dg_Locus E)
  { 
    int i;
    dg_GridPos Ep[DG_MAX_GRID_DIM];
    dg_cell_position(d, E.cell, Ep);
    dg_cell_print(wr, d, E.cell);
    fprintf(wr, ":");
    for (i = 0; i < d; i++) 
      { fprintf(wr, "%d", (dg_axis_belongs(i, E.norm) ? 1 : 0)); }
  }

/* VECTOR ALLOCATION */

dg_CellIndex_vec_t dg_CellIndex_vec_new(nat nel)
  { /* This is not a macro only because gcc does not allow cast of struct: */
    vec_t v = vec_new(nel, sizeof(dg_CellIndex));
    dg_CellIndex_vec_t r;
    r.nel = v.nel; r.el = (dg_CellIndex *)v.el;
    return r;
  }

dg_Locus_vec_t dg_Locus_vec_new(nat nel)
  { /* This is not a macro only because gcc does not allow cast of struct: */
    vec_t v = vec_new(nel, sizeof(dg_Locus));
    dg_Locus_vec_t r;
    r.nel = v.nel; r.el = (dg_Locus *)v.el;
    return r;
  }