/* See {dg_test_tools.h} */
/* Last edited on 2014-05-16 00:10:38 by stolfilocal */

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <math.h>
#include <assert.h>

#include <bool.h>
#include <vec.h>
#include <jsmath.h>
#include <intheap.h>

#include <dg_grid.h>
#include <dg_tree.h>

#include <dg_test_tools.h>

/* ADAPTIVE TREE GENERATION */

#define dg_test_TOL_EPS (0.001)

double dg_test_dist_canonical_torus(dg_dim_t d, double x[], double y[])
  { /* Get the sizes of the canonical cell: */
    const double *sz = dg_cell_root_canonical_sizes(d);
    int i;
    double sum2 = 0;
    for (i = 0; i < d; i++)
      { double di = fabs(x[i] - y[i]);
        double szi = sz[i];
        while (di > szi) { di -= szi; }
        if (di > szi/2) { di = szi - di; }
        sum2 += di*di;
      }
    return sqrt(sum2);
  }

double dg_test_dist_to_sphere(dg_dim_t d, double x[], double u, double v, double r);
  /* Distance from {x[0..d-1]} to a hypersphere with radius {r}. The
    center coordinates {y[0..d-1]} of the sphere are obtained by
    interleaving {u} and {v}, and scaling each coordinate by the
    corresponding size of the canonical cell. */
  
#define dg_test_SING_1_u (0.80)
#define dg_test_SING_1_v (0.77)
#define dg_test_SING_1_r (0.00)

#define dg_test_SING_2_u (0.23)
#define dg_test_SING_2_v (0.18)
#define dg_test_SING_2_r (0.00)

#define dg_test_SING_3_u (0.37)
#define dg_test_SING_3_v (0.31)
#define dg_test_SING_3_r (0.10)

double dg_test_local_tol(dg_dim_t d, double x[], double eps)
  { /* Compute the distances from the singular features : */
    double d1 = dg_test_dist_to_sphere(d, x, dg_test_SING_1_u, dg_test_SING_1_v, dg_test_SING_1_r);
    double d2 = dg_test_dist_to_sphere(d, x, dg_test_SING_2_u, dg_test_SING_2_v, dg_test_SING_2_r);
    double d3 = dg_test_dist_to_sphere(d, x, dg_test_SING_3_u, dg_test_SING_3_v, dg_test_SING_3_r);
    /* Take the soft minimum (harmonic mean): */
    double maxr = 0.25 * 3.0/(1.0/d1 + 1.0/d2 + 1.0/d3);
    /* Blur it so that the minimum is {eps}: */
    return hypot(maxr, eps);
  }

double dg_test_dist_to_sphere(dg_dim_t d, double x[], double u, double v, double r)
  { /* Get the sizes of the canonical cell: */
    const double *sz = dg_cell_root_canonical_sizes(d);
    /* Map {u,v} to singularity center {y[0..d-1]} in the canonical cell: */
    double y[d];
    int i;
    for (i = 0; i < d; i++) { y[i] = (i%2 == 0 ? u : v)*sz[i]; }
    /* Compute the toroidal distance from {x} to {y}: */
    double dxy = dg_test_dist_canonical_torus(d, x, y);
    /* Subtract {r} and take the absolute value (plus a fudge to keep it nonzero): */
    return fabs(dxy - r) + 1.0e-200;
  }

dg_tree_t dg_test_make_trivial_tree(void)
  { return dg_tree_node_new(NULL, 1); }

dg_tree_t dg_test_make_adaptive_grid
  ( dg_dim_t d, 
    dg_rank_t minRank, 
    dg_rank_t maxRank, 
    int64_t numCells
  )
  {    
    bool_t debug = TRUE;
    
    /* Round {numCells} up to odd: */
    numCells |= 1;
    
    /* Number of cells in grid: */
    int64_t minCells = ipow(2, minRank + 1) - 1;
    int64_t maxCells = ipow(2, maxRank + 1) - 1;
    int64_t totCells = numCells;
    if (totCells < minCells) { totCells = minCells; }
    if (totCells > maxCells) { totCells = maxCells; }
    
    
    /* Approximate radius of lowest-level cell: */
    double tol_eps = 0.25*pow(0.5, ((double)maxRank)/d);

    /* Cell list and heap */
    int nCells = 0;
    dg_tree_t node[totCells]; /* Cells are {node[0..nCells-1]}. */
    
    /* Heap of indices into {node}, sorted by badness: */
    int nLeaves = 0;
    int ix[totCells]; /* {node[ix[0]]} is the baddest leaf cell. */

    /* Start with a trivial tree: */
    dg_tree_t G = dg_test_make_trivial_tree();
    node[0] = G; nCells = 1;
    ix[0] = 0; nLeaves = 1;
    
    auto double badness(int k);
      /* A badness function of node {node[k]}. */
    
    double badness(int k)
      { dg_tree_node_id_t nix = node[k]->id;
        /* Badness depends on ratio between cell radius and a local tolerance function: */
        interval_t bx[d];
        dg_cell_box_canonical(d, nix, bx);
        double rad = box_radius(d, bx);
        double ctr[d];
        box_center(d, bx, ctr);
        double tol = dg_test_local_tol(d, ctr, tol_eps);
        return rad/tol;
      }
      
    auto int cmp_badness(int k0, int k1);
    /* Compares cells {node[k0]} with {node[k1]} for badness.
      Returns -1 if the first is worse than the second. */ 
    
    int cmp_badness(int k0, int k1)
      { if (k0 == k1) { /* Shouldn't happen, but... */ return 0; }
        if (debug)
          { fprintf(stderr, "    comparing nodes (node[%d] : node[%d])", k0, k1);
            fprintf(stderr, " = (cell %lu : cell %lu)\n", node[k0]->id, node[k1]->id);
          }
        double bad0 = badness(k0);
        double bad1 = badness(k1);
        if (bad0 > bad1)
          { return -1; }
        else if (bad0 < bad1)
          { return +1; }
        else
          { return 0; }
      }
    
    /* Break leaves until we have {totCells} cells: */
    while (nCells < totCells)
      { 
        /* Get leaf cell with largest badness: */
        int ixw = ihp_heap_pop(ix, &nLeaves, cmp_badness, +1);
        dg_tree_t w = node[ixw];
        if (debug) { fprintf(stderr, "splitting node[%d] = cell %lu\n", ixw, w->id); }
        
        /* Split it and add children to cell list: */
        dg_tree_node_split(w);
        int ich;
        for (ich = 0; ich < 2; ich++)
          { dg_tree_t ch = w->ch[ich];
            int ixch = nCells; nCells++;
            node[ixch] = ch;
            if (debug) { fprintf(stderr, "  adding child %d node[%d] = cell %lu\n", ich, ixch, ch->id); }
            ihp_heap_insert(ix, &nLeaves, ixch, cmp_badness, +1);
          }
      }
    assert(nCells == numCells);
    return node[0];
  }