#define PROG_NAME "test_approx"
#define PROG_DESC "test of dyadic grids: least squares approximation"
#define PROG_VERS "1.0"

/* Last edited on 2021-07-17 23:53:15 by jstolfi */

#define test_approx_COPYRIGHT "Copyright © 2007 by the State University of Campinas (UNICAMP)."

#define PROG_HELP \
  "  " PROG_NAME " \\\n" \
  "    -funcName {NAME} \\\n" \
  "    -minRank {MIN_RANK} \\\n" \
  "    -maxRank {MAX_RANK} \\\n" \
  "    [ -superior | -inferior ] \\\n" \
  "    [ -cholesky | \\\n" \
  "      -gaussSeidel [-omega {OMEGA}] [-tol {TOL}] [-maxIter {MAX_ITER}]\\\n" \
  "    ] \\\n" \
  "    [ -gaussOrder {GAUSS_ORDER} ] \\\n" \
  "    -outName NAME \\\n" \
  "    " argparser_help_info_HELP " \\\n" \
  "    < {INFILE} \\\n" \
  "    > {OUTFILE}"

#define PROG_INFO \
  "NAME\n" \
  "  " PROG_NAME " - " PROG_DESC "\n" \
  "\n" \
  "SYNOPSIS\n" \
  PROG_HELP "\n" \
  "\n" \
  "DESCRIPTION\n" \
  "  This program computes a least squares approximation {g}," \
  " for a function {f}, of the form\n" \
  "\n" \
  "   {g = SUM { u[i]*B[i] : i = 0..m-1 }}       (1)\n" \
  "\n" \
  "  where {B[0..m-1]} is a basis of dyadic splines.  The" \
  " coefficients {u[i]} are found by solving the linear system\n" \
  "\n" \
  "   {G u == b}    (2)\n" \
  "\n" \
  "  where {G} is the rigidity matrix, {G[i,j] == <B[i] | B[j]>}," \
  " and the vector {b} is given by {b[i] == <f | B[i]>}.\n" \
  "\n" \
  "OPTIONS\n" \
  "  -blabber {AMOUNT}\n" \
  "    Blabbers for that {AMOUNT}. May also bla" \
  " bla bla bla bla bla bla bla bla bla bla bla bla" \
  " bla bla bla bla bla bla bla.\n" \
  "\n" \
  "DOCUMENTATION OPTIONS\n" \
  argparser_help_info_HELP_INFO "\n" \
  "\n" \
  "SEE ALSO\n" \
  "  ls(1).\n" \
  "\n" \
  "AUTHOR\n" \
  "  Started 2005-06-05 by Jorge Stolfi, IC-UNICAMP.\n" \
  "\n" \
  "MODIFICATION HISTORY\n" \
  "  ?? .\n" \
  "\n" \
  "WARRANTY\n" \
  argparser_help_info_NO_WARRANTY "\n" \
  "\n" \
  "RIGHTS\n" \
  "  " test_approx_COPYRIGHT ".\n" \
  "\n" \
  argparser_help_info_STANDARD_RIGHTS

#define _GNU_SOURCE
#include <stdio.h>
#include <assert.h>
#include <stdlib.h>
#include <math.h>

#include <vec.h>
#include <bool.h>
#include <argparser.h>
#include <intheap.h>
#include <jsmath.h>
#include <affirm.h>
#include <rn.h>
#include <dspmat.h>
#include <pswr.h>
#include <pswr_color.h>
#include <pswr_iso.h>

#include <udg_grid.h>
#include <udg_spline.h>
#include <udg_pulse.h>

#include <mdg_grid.h>
#include <dg_tree.h>
#include <dg_tent.h>
#include <dg_tent_basis.h>
#include <dg_plot.h>
#include <dg_makegrid.h>
#include <dg_test_tools.h>

#include <bz_basic.h>

/* Dimension of grid's domain: */
#define DDIM (2)  

/* Dimension of function's range space: */
#define FDIM (1)  

/* Dimension of grid's range space {(X,Y,F(X,Y))}: */
#define MAX_RDIM (5)

/* Number of faces in a cell of dimension {DDIM}, namely {3^DDIM}: */
#define NF (9)

/* Flatness tolerance (mm): */
#define TOL_MM (0.5)

/* TRUE to debug {make_shape} instead of {dg_enum_faces}: */
#define DEBUG_SHAPE FALSE

/* TYPES */

typedef int tap_plot_options_t; /* For now */

typedef struct tap_options_t
  { char *funcName;          /* Name of function to approximate. */
    char *outName;           /* Prefix for output file names. */
    /* Parameters of the grid to be used: */
    int minRank;             /* Minimum rank for grid generation. */  
    int maxRank;             /* Maximum rank for grid generation. */  
    int numCells;            /* Desired number of cells in grid. */
    bool_t superior;         /* TRUE uses the superior basis, FALSE (default) inferior. */       
    uint32_t gaussOrder;     /* Gaussian quadrature order for dot products. */
    /* Method used to solve the linear system: */
    bool_t cholesky;         /* TRUE uses direct method based on Cholesky factzn. */
    bool_t gaussSeidel;      /* TRUE uses Gauss-Seidel iteration. */
    bool_t conjGrad;         /* TRUE uses the conjugate gradient method. */
    /* Parameters for Gauss-Seidel iteration: */
    double omega;            /* Relaxation factor. */
    uint32_t maxIter;        /* Maximum iterations. */
    double relTol;           /* Stopping criterion: relative change. */
    double absTol;           /* Stopping criterion: absolute change. */
    /* Plotting options: */
    bool_t plot;             /* TRUE to generate function and error plots. */
    tap_plot_options_t plt;  /* Plot format and style options. */
  } tap_options_t;

/* INTERNAL PROTOTYPES */

int main(int argc, char **argv);
  
tap_options_t *tap_get_options(int argn, char **argc);

typedef void tap_func_t(double *x, mdg_dim_t nx, double *f, mdg_dim_t nf);
  /* Type of a procedure that evaluates some function from {R^nx} to {R^nf}. 
    The fucntion argument is {x[0..nx-1]}. The procedure should store the 
    result in {f[0..nf-1]}. */

double_vec_t tap_compute_approx
  ( tap_func_t func,
    udg_pulse_family_t fam[],
    dg_tent_vec_t B, 
    dg_tree_t G
  );
  /* Computes a spline over {G} that is a least-square approximation to {func}.
    The Bezier coefficients of the approximation are stored in the leaves of {G}. */


void tap_build_spline
  ( dg_tent_vec_t B,
    double_vec_t coef, 
    dg_tree_t G
  );
  /* Given a basis {B[0..n-1]} of spline elements defined over the 
    dyadic grid {G}, and a list of coefficients {coef[0..n-1]},
    computes the spline {f = SUM{ coef[i]*B[i] : i in 0..n-1 }}.
    Each patch of the spline is stored into a newly allocated 
    {bz_patch_t} record, whose address is stored into the {bz} field
    of the corresponding node of {G}. */

PSStream *tap_init_plot
  ( char *name, 
    bool_t epsformat,
    char *paperSize,
    interval_t bbox[]
  );
  /* Opens a Postscript plotfile with given name, plus the 
    appropriate extension (".eps" or ".ps"), and initializes
    it for plotting.  The scales are set so that the 
    rectangle {bbox[0] × bbox[1]} fits snugly in the available 
    plotting area, with equal scales. */

dg_locus_proc_t tap_plot2D;
  /* Plots a leaf item of a finte multigrid. */
     
bz_patch_t *tap_do_extract_face(bz_patch_t *b, box_face_index_t f);
/* Extracts face {f} of the Bezier patch {b} and stores it into a newly
  allocated record. */

void tap_finish_plot
  ( PSStream *ps,  
    bool_t epsformat
  );
  /* Finalizes and closes the Postscript plotfile {f}. */

void tap_slow_plot(PSStream *ps, bz_patch_t *b);
void tap_slow_plot_vertex(PSStream *ps, bz_patch_t *b, int steps);
void tap_slow_plot_edge(PSStream *ps, bz_patch_t *b, int steps);
void tap_slow_plot_face(PSStream *ps, bz_patch_t *b, int steps, double R, double G, double B);
  /* Slow and dumb plotting routines, for debugging. */

double tap_tol(double x, double y);
  /* Maximum box radius allowed near {p}. */ 

double tap_inside(double x, double y, double rad);
  /* TRUE if a box of radius {rad} centered at {p} intersects the 
    desired domain. */ 

double tap_dsoft(double dx, double dy, double eps);
  /* Length of {(dx,dy)}, cooked to be at least {eps}. */

void tap_test_plot(char *name, bz_degree_t g, bz_patch_rdim_t n, udg_rank_t treeDp, udg_rank_t plotDp);
  /* Tests of plotting routines, with a root patch of degree {g},
    a random tree of maximum depth {treeDp}, and maximum plotting split 
    depth {plotDp}. */

/* IMPLEMENTATIONS */

int main(int argc, char **argv)
  { 
    tap_options_t *o = tap_get_options(argc, argv);
    
    bz_patch_t *b = NULL;  /* Shape of root cell. */
    
    auto void func (double *x, mdg_dim_t nx, double *f, mdg_dim_t nf);
      /* Evaluates the test function to be approximated at the point
        {x[0..nx-1]}. Returns the result in {f[0..nf-1]}. */
      
    void func (double *x, mdg_dim_t nx, double *f, mdg_dim_t nf)
      { double r2 = rn_norm_sqr(nx, x);
        double tt = 5.0 * 2*M_PI * r2/(1.0 + r2);
        double mm = 1.0/(1.0 + r2);
        if (nf > 0) { f[0] = mm*sin(tt); }
        if (nf > 1) { f[1] = mm*cos(tt); }
        if (nf > 3) { f[1] = mm; }
        int i;
        for (i = 3; i < nf; i++) { f[i] = f[i - 3]; } 
      }

    dg_tree_t G = tap_make_grid(DDIM, o->minRank, o->maxRank, o->numCells, b);
    
    /* Build tent basis. */
    udg_pulse_family_t fam[DDIM];
    int i;
    for (i = 0; i < DDIM; i++) { fam[i] = udg_pulse_family(udg_PK_B, 0, 1); }
    dg_tent_vec_t B = dg_tent_basis_get(DDIM, fam, G, o->superior);
    
    /* Write basis? */
    /* tap_write_basis(B, o->outName); */
    
    /* Compute a least-squares approximation: */
    double_vec_t sol = tap_compute_approx(&func, fam, B, G); 
    
    /* Output final solution: */
    double fMax[FDIM], eMax[FDIM];
    
    /* Compute the approximating spline and save it in the leaf cells of the grid: */
    tap_build_spline(B, sol, G);
    
    /* The basis approximation: */
    auto void appr (double *x, mdg_dim_t nx, double *f, mdg_dim_t nf);
      /* Evaluates the spline stored in the leaves of {G} at the 
        point {x[0..nx-1]}.  Returns the result in {f[0..nf-1]}. */
      
    /* The basis approximation: */
    auto void aerr (double *x, mdg_dim_t nx, double *f, mdg_dim_t nf);
      /* Evaluates the difference between {fuc} and {appr} at the 
        point {x[0..nx-1]}.  Returns the result in {f[0..nf-1]}. */
      
    void appr (double *x, mdg_dim_t nx, double *f, mdg_dim_t nf)
      { dg_tree_node_t *nd = tap_find_leaf_cell(G, x);
        bz_patch_t *bz = (bz_patch_t *)nd->data;
        bz_patch_eval(bz, x, f);
      }

    void aerr (double *x, mdg_dim_t nx, double *f, mdg_dim_t nf)
      { double ff[nf], fa[nf];
        appr(x, nx, ff, nf);
        func(x, nx, fa, nf);
        int i;
        for (i = 0; i < nf; i++) { f[i] = fa[i] - ff[i]; }
      }

    tap_print_max_error_values(func, appr, fMax, eMax);
    
    /* 2D Plotting */
    if((o->plot)&&(DDIM == 2)&&(FDIM == 1))
      {
        tap_plot_function(func);
        tap_plot_function(appr);
        tap_plot_function(aerr);
      } 

    return 0;
  } 

double_vec_t tap_compute_approx
  ( tap_func_t func,
    udg_pulse_family_t fam[],
    dg_tent_vec_t B, 
    dg_tree_t G
  )
  {
    interval_t rootBox[DDIM]; /* absolute bounds of root cell */

    /* Compute the matrix {H[i,j] = <B[i]|B[j]> (lower half only). */
    int i, j;
    dspmat_t H = dspmat_new(B.ne,B.ne,B.ne*15); /* Initial guess. */
    dspmat_pos_t pH = 0;
    for (i = 0; i < B.ne; i++)
      { dg_tent_t ti = B.e[i];
        for (j = 0; j <= i; j++)
          { dg_tent_t tj = B.e[j];
            double Hij = dg_tent_tent_dot(DDIM, fam, ti, tj);
            pH = dspmat_add_element(&H, pH, i, j, Hij);
          }
      }
 
    /* Compute the right-hand-side vector(s): */
    double_vec_t b = double_vec_new(FDIM * B.ne);
    for (i = 0; i < B.ne; i++);
    { dg_tent_t ti = B.e[i];
      double bi = dg_tent_func_dot(DDIM, fam, ti, func);
    }

    /* Allocate auxiliary vectors: */ 
    double_vec_t u = double_vec_new(FDIM * B.ne);
    double_vec_t uj = double_vec_new(B.ne); // Column {j} of {u}.
    double_vec_t bj = double_vec_new(B.ne); // Column {j} of {b}.
    double_vec_t y = double_vec_new(B.ne);
    
    switch(sys_method)
      { 
        case tap_sys_CHOLESKY:
          { dspmat_t C = smt_cholesky(H, 0.0);        
            tap_Cholesky_solve(C, FDIM, b, u, bj, uj, y, TRUE);
            break;
          }
        case tap_sys_GAUSSSEIDEL:
          { 
            tap_guess_solution(v);
            tap_insert_upper_half(H);
            tap_Gauss_Seidel_solve
              ( H, b, 
                o->omega, o->maxIter, o->absTol, o->relTol,
                v, u, bj, uj, TRUE
              );
            break;
          }
        default:
          { affirm(FALSE , "unspecified/invalid solution method"); }
      }
      
    return u;
  }

void tap_plot_function(void)
  {
    /* Builds error function: */
    Basis error_B = SOFunctionRef_vec_new(2);
    double_vec_t errc = double_vec_new(2); errc.e[0] = 1; errc.e[1] = -1;
    error_B.e[0] = (SOFunction *)f; error_B.e[1] = (SOFunction *)g;
    SOFunction *e = 
      (SOFunction *)SOLinCombFunction_Make(DDIM, FDIM,"Errorf.B", error_B, errc);

    double fPlotMin = 0, fPlotMax = 0;
    double gPlotMin = 0, gPlotMax = 0;
    double ePlotMin = 0, ePlotMax = 0;

    /* Plots the true solution {f}, the approximation {g}, and the error {e}: */
    SOApprox_PlotSingleFunction(f, &NoMap, tree, txtcat(o->outName, "-sol"),  &(o->plt), &fPlotMin, &fPlotMax);
    SOApprox_PlotSingleFunction(g, &NoMap, tree, txtcat(o->outName, "-apr"),  &(o->plt), &gPlotMin, &gPlotMax);

    /* Cortes 1D. */
    double initP[2] = {0.0, 0.5}, finP[2] = {1.0, 0.5}, D1min, D1max;
    SO1DPlot_SingleFunction
    (TRUE, NULL, 300, 200, g, &NoMap, DDIM, initP, finP, -0.5, 12.0, 8, 0.10, "corteX", &D1min, &D1max);
    initP[0] = 0.25; initP[1] = 0.0; finP[0] = 0.25; finP[1] = SQRTHALF;
    SO1DPlot_SingleFunction
    (TRUE, NULL, 300, 200, g, &NoMap, DDIM, initP, finP, -0.5, 12.0, 8, 0.10, "corteY", &D1min, &D1max);

    o->plt.fRange = o->plt.fRange / 10.0;
    o->plt.fStep = o->plt.fStep / 10.0;
    SOApprox_PlotSingleFunction(e, &NoMap, tree, txtcat(o->outName, "-err"),  &(o->plt), &ePlotMin, &ePlotMax);

    fprintf(stderr, "observed SOLUTION function extrema:");
    fprintf(stderr, " min = %16.12f max = %16.12f\n", fPlotMin, fPlotMax);
    fprintf(stderr, "\n");

    fprintf(stderr, "observed APPROXIMATION function extrema:");
    fprintf(stderr, " min = %16.12f max = %16.12f\n", gPlotMin, gPlotMax);
    fprintf(stderr, "\n");

    fprintf(stderr, "observed ERROR function extrema:");
    fprintf(stderr, " min = %16.12f max = %16.12f\n", ePlotMin, ePlotMax);
    fprintf(stderr, "\n");

    fprintf(stderr, "Basis with %d tents; Grid with %d leaf cells.\n", B.ne, leaf_cells);

    double distance = 0.0;
    FILE *rd2 = open_read("../SOFindTentBasis/regular10.tree", TRUE);
    SOGrid_Tree *tree10 = SOGrid_Tree_read(rd2);
    SOFunction_Dot(e, &NoMap, e, &NoMap, (SOFunction *)NULL, tree10, &distance, TRUE);
    distance = sqrt(distance);
    fprintf(stderr, "observed DISTANCE ( |f-g| = sqrt(<f-g, f-g>) ): %.16g \n", distance);

  }

#define PPUSAGE SOParams_SetUsage

tap_options_t *tap_get_options(int argn, char **argc)
  {
    tap_options_t *o = (tap_options_t *)notnull(malloc(sizeof(tap_options_t)), "no mem");
    SOParams_T *pp = SOParams_NewT(stderr, argn, argc);

    /* Plotting options instructions. */
    PPUSAGE(pp, "  [ -plot ] \\\n");
    PPUSAGE(pp, SOPlotParams_FunctionHelp " \n");

    SOParams_GetKeyword(pp, "-funcName");                               
    o->funcName = SOParams_GetNext(pp);  
       
    SOParams_KeywordPresent(pp, "-matName");
    o->matName = SOParams_GetNext(pp);
       
    SOParams_GetKeyword(pp, "-outName");                               
    o->outName = SOParams_GetNext(pp);  
                                                 
    o->cholesky = SOParams_KeywordPresent(pp, "-cholesky");
    o->gaussSeidel = SOParams_KeywordPresent(pp, "-gaussSeidel");
    o->conjGrad = SOParams_KeywordPresent(pp, "-conjGrad");
    if (! (o->cholesky || o->gaussSeidel || o->conjGrad))
      { SOParams_Error(pp, "must specify a linear solution method"); }
    else if ((o->cholesky + o->gaussSeidel + o->conjGrad) > 1)
      { SOParams_Error(pp, "please specify only one linear solution method"); }

    SOParams_KeywordPresent(pp, "-minRank");
    o->minRank = SOParams_GetNextInt(pp, 0, INT_MAX);

    SOParams_KeywordPresent(pp, "-maxRank");
    o->maxRank = SOParams_GetNextInt(pp, 0, INT_MAX);

    SOParams_KeywordPresent(pp, "-diff_limit");
    o->diff_limit = SOParams_GetNextDouble(pp, 0, DBL_MAX);

    o->wide = SOParams_KeywordPresent(pp, "-wide");

    if (o->gaussSeidel)
      { 
        if (SOParams_KeywordPresent(pp, "-omega"))
          { o->omega = SOParams_GetNextDouble(pp, 0.0, DBL_MAX); }
        else
          { o->omega = 1.0; }

        if (SOParams_KeywordPresent(pp, "-maxIter"))
          { o->maxIter = SOParams_GetNextInt(pp, 0, INT_MAX); }
        else
          { o->maxIter = 20; }

        if (SOParams_KeywordPresent(pp, "-absTol"))
          { o->absTol = SOParams_GetNextDouble(pp, 0.0, DBL_MAX); }
        else
          { o->absTol = 0.0; }

        if (SOParams_KeywordPresent(pp, "-relTol"))
          { o->relTol = SOParams_GetNextDouble(pp, 0.0, DBL_MAX); }
        else
          { o->relTol = 0.0; }
      }

    SOParams_GetKeyword(pp, "-gaussOrder");
    o->gaussOrder = SOParams_GetNextInt(pp, 1, INT_MAX);

    /* Plotting options: */

    o->plot = SOParams_KeywordPresent(pp, "-plot");
    o->plt = SOPlotParams_FunctionParse(pp);

    SOParams_Finish(pp);
    return o;
  }



static PSStream *ps;
static udg_rank_t maxPlotDepth;

void tap_plot2D(
    mdg_dim_t d,
    dg_locus_t E,
    udg_rank_t r, 
    bz_patch_t *b,
    dg_node_star_t *na
  )
  { int m = b->m; /* Dimension of item. */
    int n = b->n; /* Dimension of range (coords plus func values). */
    fprintf(stderr, "+ tap_plot2D(E = <%d:%lld> r = %d)\n", E.norm, E.cell, r);
    fprintf(stderr, "item shape\n");
    bz_print(stderr, b, "%6.2f");
    affirm(m <= 2, "bad domain dimension");
    double fMax = 1.0;
    double fStep = 0.2;
    double fSync = fStep/2;
    int kMin = pswr_inf_isoline(fSync, fStep, -fMax);
    int kMax = pswr_sup_isoline(fSync, fStep, +fMax);
    int N;
    double *R, *G, *B;
    pswr_make_color_table(
      fSync, fStep, kMin, kMax,
      0.000, 0.500, 1.000,
      1.000, 1.000, 1.000,
      1.000, 0.167, 0.000,
      &N, &R, &G, &B);
    switch(m)
    { case 0: 
        { double *c = &(b->c[0]);
          dg_plot_2D_vertex(ps, c, n);
        }
        break;
      case 1: 
        affirm(b->g[0] == 1, "bad patch degree");
        { double *c0 = &(b->c[0]);
          double *c1 = &(b->c[n]);
          dg_plot_2D_edge(ps, c0, c1, n); 
        }
        break;
      case 2: 
        affirm((b->g[0] == 1) && (b->g[1] == 1), "bad patch degree");
        { double *c00 = &(b->c[0]);
          double *c01 = &(b->c[n]);
          double *c10 = &(b->c[2*n]);
          double *c11 = &(b->c[3*n]);
          int subDp = maxPlotDepth - r + 1;
          if (subDp < 0) { subDp = 0; }
          if (subDp > 3) { subDp = 3; }
          dg_plot_2D_face
            ( ps, c00, c01, c10, c11, n, subDp, fSync, fStep, kMin, kMax, R,G,B );
        }
        break;
      default: affirm(FALSE, "bad item dimension"); 
    }
    fprintf(stderr, "- tap_plot2D\n");
  }

void tap_test_plot(char *name, bz_degree_t g, bz_patch_rdim_t n, udg_rank_t treeDp, udg_rank_t plotDp)
  {
    bool_t epsformat = FALSE;
    bz_patch_t b = bz_make_test_patch(DDIM, n, g);
    dg_tree_t *grid_root = tap_make_grid(DDIM, treeDp, &b);
    dg_locus_t E0 = dg_locus(0, DG_ROOT_CELL);
    dg_node_star_t na = dg_node_ref_vec_new(1);
    interval_t bbox[MAX_RDIM];

    affirm(b.m == 2, "wrong dimension");
    fprintf(stderr, "grid cells = %d\n", grid_root->nodes);
    fprintf(stderr, "root shape =\n");
    bz_print(stderr, &b, "%6.2f");
    /* bz_patch_t_dev_print(stderr, sh->dev, b.m, "%.2f"); */
    /* fprintf(stderr, "\n"); */
    maxPlotDepth = plotDp;

    bz_compute_bbox(&b, bbox);
    ps = tap_init_plot(name, epsformat, "a3", bbox);
    
    if (DEBUG_SHAPE)
      { tap_slow_plot(ps, &b); }
    else
      { na.e[0] = grid_root;
        dg_enum_locus_leaves(DDIM, E0, 0, &b, &na, plotDp, tap_plot2D);
      }
    tap_finish_plot(ps, epsformat);
  }

PSStream *tap_init_plot
  ( char *name, 
    bool_t epsformat,
    char *paperSize,
    interval_t bbox[]
  )
  { double hpt, vpt;
    if (epsformat)
      { hpt = 5.0*72.0; vpt = hpt * 4.0/3.0; }
    else
      { pswr_get_paper_dimensions(paperSize, FALSE, &hpt, &vpt); }
    PSStream *ps = pswr_new_stream(name, NULL, epsformat, "doc", paperSize, FALSE, hpt+6, vpt+8);
    pswr_set_canvas_layout(ps, 3.0, 4.0, TRUE, 0.0, 0.0, (epsformat ? 0 : 1), 1,1);
    double dx = 0.1*(bbox[0].end[BHI] - bbox[0].end[BLO])/2;
    double dy = 0.1*(bbox[1].end[BHI] - bbox[1].end[BLO])/2;
    pswr_new_picture
      ( ps, 
        bbox[0].end[BLO]-dx, bbox[0].end[BHI]+dx, 
        bbox[1].end[BLO]-dy, bbox[1].end[BHI]+dy
      );
    pswr_set_pen(ps, 0,0,0, 0.15, 0,0);
    if (! epsformat)
      { pswr_add_caption(ps, "Bezier-distorted grid", 0.0); }
    pswr_set_pen(ps, 1,0,0, 0.25, 0,0);
    return ps;
  }

void tap_finish_plot
  ( PSStream *ps,  
    bool_t epsformat
  )
  { pswr_close_stream(ps); }

#define x_well_1 (2.5)
#define y_well_1 (4.5)

#define x_well_2 (4.0)
#define y_well_2 (5.0)

#define y_fault (2.5)
#define r_fault (0.5)
#define w_fault (1.5)

double tap_tol(double x, double y)
  { double d1 = tap_dsoft(x - x_well_1, y - y_well_1, 0.01);
    // double d1 = 100000.00;
    double d2 = tap_dsoft(x - x_well_2, y - y_well_2, 0.01);
    // double d2 = 100000.00;
    double yfx = y_fault + r_fault*sin(w_fault*x);
    double d3 = tap_dsoft(0.0, y - yfx, 0.01);
    // double d3 = 100000.00;
    double maxr = 0.25 * 3.0/(1.0/d1 + 1.0/d2 + 1.0/d3);
    return maxr;
  }

double tap_inside(double x, double y, double rad)
  { double dctr = hypot(x - 0.5, y - 0.5);
    return dctr - rad < 0.4; 
  }

double tap_dsoft(double dx, double dy, double eps)
  { return sqrt(dx*dx + dy*dy + eps*eps); }

dg_tree_t *tap_make_grid(mdg_dim_t d, udg_rank_t treeDp, bz_patch_t *b)
  {
    interval_t bbox[MAX_RDIM];
    double ctr[MAX_RDIM];
    int n = b->n;

    auto bool_t omit(dg_cell_index_t k, udg_rank_t r, bz_patch_t *b, dg_tree_t *p);
      /* Tells {dg_make_grid} when to discard a cell. Basically, when it
        does not intersect a specified circle. */ 

    auto bool_t split(dg_cell_index_t k, udg_rank_t r, bz_patch_t *b, dg_tree_t *p);
      /* Tells {dg_make_grid} when to split a cell. Basically, when the variation in
        the coordinates and function values within the cell is larger
        than some tolerance, which depends on the cell's center. */ 

    bool_t omit(dg_cell_index_t k, udg_rank_t r, bz_patch_t *b, dg_tree_t *p)
      { // int i; double r2 = 0.0;
        // if (r >= treeDp) return FALSE;
        // bz_compute_bbox(b, bbox);
        // for (i = 0; i < d; i++) 
        //   { interval_t bi = bbox[i];
        //     double ri = (bi.hi - bi.lo)/2;
        //     ctr[i] = (bi.lo + bi.hi)/2;
        //     r2 += ri*ri;
        //   }
        // return ! tap_inside(ctr[0], ctr[1], sqrt(r2));
        fprintf(stderr, "  omit k = %lld\n", k);
        return FALSE;
      }

    bool_t split(dg_cell_index_t k, udg_rank_t r, bz_patch_t *b, dg_tree_t *p)
      { int i; double r2 = 0.0;
        double rad, radMax;
        if (r >= treeDp) return FALSE;
        bz_compute_bbox(b, bbox);
        for (i = 0; i < n; i++) 
          { interval_t bi = bbox[i];
            double ri = (bi.end[BHI] - bi.end[BLO])/2; /* Half-width of interval. */
            ctr[i] = (bi.end[BLO] + bi.end[BHI])/2;
            r2 += ri*ri;
          }
        rad = sqrt(r2);
        radMax = tap_tol(ctr[0], ctr[1]);
        fprintf(stderr, "  split k = %lld  rad = %7.4f radMax = %7.4f\n", k, rad, radMax);
        return rad >= radMax;
      }

    return dg_make_grid(d, DG_ROOT_CELL, 0, b, NULL, &omit, &split);
  }

void tap_slow_plot(PSStream *ps, bz_patch_t *b)
  { 
    int BSTEPS = 10;
    double R = 1.0, G = 0.9, B = 0.6;
    int f;
    fprintf(stderr, "+tap_slow_plot dim = %d\n", b->m);
    /* Plot faces */
    for (f = 0; f < NF; f++)
      { if (box_face_dimension(f, b->m) == 2)
        { bz_patch_t *t = tap_do_extract_face(b, f);
          tap_slow_plot_face(ps, t, BSTEPS, R, G, B);
          free(t->c); free(t);
        }
      }
    /* Plot edges */
    pswr_set_pen(ps, 0.0, 0.0, 0.0,   0.15, 0.0, 0.0);
    for (f = 0; f < NF; f++)
      { if (box_face_dimension(f, b->m) == 1)
        { bz_patch_t *t = tap_do_extract_face(b, f);
          tap_slow_plot_edge(ps, t, BSTEPS);
          free(t->c); free(t);
        }
      }
    /* Plot vertices */
    pswr_set_pen(ps, 0.0, 0.0, 0.0,   0.15, 0.0, 0.0);
    for (f = 0; f < NF; f++)
      if (box_face_dimension(f, b->m) == 0) 
        { bz_patch_t *t = tap_do_extract_face(b, f);
          tap_slow_plot_vertex(ps, t, BSTEPS);
          free(t->c); free(t);
        }
    fprintf(stderr, "-tap_slow_plot\n");
  }

void tap_slow_plot_vertex(PSStream *ps, bz_patch_t *b, int steps)
  {
    fprintf(stderr, "+tap_slow_plot_vertex dim = %d\n", b->m);
    affirm(b->m == 0, "invalid vertex patch");
    pswr_set_fill_color(ps, 0.0,0.0,0.0);
    pswr_dot(ps, b->c[0], b->c[1], 0.25, TRUE, FALSE);
    fprintf(stderr, "-tap_slow_plot_vertex\n");
  }

void tap_slow_plot_edge(PSStream *ps, bz_patch_t *b, int steps)
  {
    fprintf(stderr, "+tap_slow_plot_edge dim = %d\n", b->m);
    affirm(b->m == 1, "invalid edge patch");
    double p[2], q[2];
    int i;
    fprintf(stderr, "shape =\n");
    bz_print(stderr, b, "%6.2f");
    for (i = 0; i <= steps; i++)
      { double ui = ((double)i)/((double)steps);
        bz_eval(b, &ui, p);
        if (i > 0) { pswr_segment(ps, q[0], q[1], p[0], p[1]); }
        q[0] = p[0]; q[1] = p[1];
      }
    fprintf(stderr, "-tap_slow_plot_edge\n");
  }

void tap_slow_plot_face(PSStream *ps, bz_patch_t *b, int steps, double R, double G, double B)
  {
    int i, j;
    fprintf(stderr, "+tap_slow_plot_face dim = %d\n", b->m);
    affirm(b->m == 2, "invalid face patch");
    fprintf(stderr, "shape =\n");
    bz_print(stderr, b, "%6.2f");
    double u[2], v[2], p[MAX_RDIM], q[MAX_RDIM], r[MAX_RDIM], s[MAX_RDIM];
    for (i = 1; i <= steps; i++)
      { for (j = 0; j <= steps; j++)
          { u[0] = ((double)i)/((double)steps);
            u[1] = ((double)j)/((double)steps);
            bz_eval(b, u, p);
            v[0] = ((double)i-1)/((double)steps);
            v[1] = ((double)j)/((double)steps);
            bz_eval(b, v, r);
            if (j > 0) 
              { double xm = (p[0]+q[0]+r[0]+s[0])/4.0;
                double ym = (p[1]+q[1]+r[1]+s[1])/4.0;
                pswr_set_fill_color(ps, R,G,B);
                pswr_triangle(ps, p[0], p[1], q[0], q[1], xm, ym, TRUE, FALSE);
                pswr_triangle(ps, p[0], p[1], r[0], r[1], xm, ym, TRUE, FALSE);
                pswr_triangle(ps, s[0], s[1], q[0], q[1], xm, ym, TRUE, FALSE);
                pswr_triangle(ps, s[0], s[1], r[0], r[1], xm, ym, TRUE, FALSE);
              }
            q[0] = p[0]; q[1] = p[1];
            s[0] = r[0]; s[1] = r[1];
          }
      }
    fprintf(stderr, "-tap_slow_plot_face\n");
  }

bz_patch_t *tap_do_extract_face(bz_patch_t *b, box_face_index_t f)
  {
    mdg_dim_t bm = b->m, tm = 0;
    box_signed_dir_t loc[DDIM];
    box_face_signature(f, bm, loc);
    bz_patch_t *t = (bz_patch_t *)notnull(malloc(sizeof(bz_patch_t)), "no mem");
    bz_degree_t tg[DDIM];
    int i;
    /* Extract degree sequence: */
    for(i = 0; i < bm; i++)
      { if (loc[i] == SMD) { tg[tm] = b->g[i]; tm++; } }
    (*t) = bz_patch_t_new(tm, b->n, tg);
    bz_get_face(b, loc, t);
    return t;
  }

void tap_shatter_node
  ( SOGrid_Dim DDIM,
    SOGrid_Node *p,
    SOGrid_Rank r,
    double *lo, 
    double *hi,
    double *maxhi,
    int minRank,
    int maxRank,
    double diff_limit,
    SOFunction *f,
    int *leaf_cells
  );

void tap_shatter_node
  ( SOGrid_Dim DDIM,
    SOGrid_Node *p,
    SOGrid_Rank r,
    double *lo, 
    double *hi,
    double *maxhi,
    int minRank,
    int maxRank,
    double diff_limit,
    SOFunction *f,
    int *leaf_cells
  )
  {
    bool_t split;//, border = FALSE;
    int i, j;
    double mid_val, maxdiff=0, diff, lo_child[DDIM], hi_child[DDIM]; //, abs_mid_val;
    double pnt_mid[DDIM], pnt_lo[DDIM], pnt_hi[DDIM], pnt_lo_val, pnt_hi_val;

    for(i = 0; i < DDIM; i++) pnt_mid[i] = (double) (hi[i] + lo[i]) / 2.0;
    f->m->eval(f, pnt_mid, &mid_val); 

    for(j = 0; j < DDIM; j++)
      {
        for(i = 0; i < DDIM; i++)
          if(i != j){ pnt_lo[i] =  pnt_mid[i];  pnt_hi[i] =  pnt_mid[i]; }
 
        pnt_lo[j] =  lo[j];  f->m->eval(f, pnt_lo, &pnt_lo_val);
        pnt_hi[j] =  hi[j];  f->m->eval(f, pnt_hi, &pnt_hi_val);

	//        printf(" ###### DEBUG-> pnt_lo[0]: %f, pnt_lo[1]: %f \n\n", pnt_lo[0], pnt_lo[1]);
	//        printf(" ###### DEBUG-> pnt_hi[0]: %f, pnt_hi[1]: %f \n\n", pnt_hi[0], pnt_hi[1]);

        diff = fabs(mid_val - (pnt_lo_val + pnt_hi_val)/2.0);
        if(diff > maxdiff) { maxdiff = diff; }

	//        if(lo[j] == 0.0 || hi[j] == maxhi[j]){ border = TRUE; abs_mid_val = fabs(mid_val); }
      }

    //    printf(" ###### DEBUG maxdiff: %.16g  diff_limit: %.16g \n\n", maxdiff, diff_limit);

    split = (maxdiff > diff_limit);// || (border && abs_mid_val > 2.0 * diff_limit));

    if (r < minRank){split = TRUE;}
    if (r >= maxRank){split = FALSE;}

    if (split)
      { 
        *leaf_cells = *leaf_cells + 1;
        if( p->c[LO] == NULL && p->c[HI] == NULL )SOGrid_Node_split(p);

        /* Compute low and high coordinates of low child. */
        for (i = 0; i < DDIM; i++)
          { lo_child[i] = lo[i]; 
            hi_child[i] = (i == (r%DDIM) ? (lo[i]+hi[i])/2 : hi[i]);
          }  
        tap_shatter_node(DDIM, p->c[LO], r+1, lo_child, hi_child, maxhi, minRank, maxRank, diff_limit, f, leaf_cells);
        
        /* Compute low and high coordinates of high child. */
        for (i = 0; i < DDIM; i++)
          { lo_child[i] = (i == (r%DDIM) ? (lo[i]+hi[i])/2 : lo[i]); 
            hi_child[i] = hi[i];
          }
        tap_shatter_node(DDIM, p->c[HI], r+1, lo_child, hi_child, maxhi, minRank, maxRank, diff_limit, f, leaf_cells);
      }
 
  }


/* ====================================================================== */

void SetGenDotMatrices
  ( MatEntry_vec *evaldot, 
    dspmat_t Meval, 
    unsigned int offset,
    unsigned int maxindices
  );

void SetDotMatrices
  ( unsigned int dim, 
    unsigned int offset,
    Basis B, 
    MatEntry_vec *evaldot, 
    MatEntry_vec *Hev_ents
  );


void SetGenDotMatrices
  ( MatEntry_vec *evaldot, 
    dspmat_t Meval, 
    unsigned int offset,
    unsigned int maxindices
  )
  {
    unsigned int i, index, indexi;

    for(i = 0; i < maxindices; i++)
      {
        evaldot[i] = MatEntry_vec_new(Meval.cols);
        evaldot[i].ne = 0;
      }

    i = 0;
    while(i < Meval.ents.ne)
      {
        index = Meval.ents.e[i].row;
        while(Meval.ents.e[i].row == index)
	  {
            indexi = Meval.ents.e[i].row - offset;
 
            evaldot[indexi].e[evaldot[indexi].ne].row = Meval.ents.e[i].row;
            evaldot[indexi].e[evaldot[indexi].ne].col = Meval.ents.e[i].col;
            evaldot[indexi].e[evaldot[indexi].ne].va = Meval.ents.e[i].va;
            evaldot[indexi].ne++;
            i++;
	  }
      }
  }


void SetDotMatrices
  ( unsigned int dim, 
    unsigned int offset,
    Basis B, 
    MatEntry_vec *evaldot, 
    MatEntry_vec *Hev_ents
  )
  {
    int i, j, k, hi_ind, low_ind;
    SOTentFunction *itf, *jtf;

    Hev_ents->nel = 0;

    for(i = 0; i < dim; i++)
      for(j = 0; j < dim; j++)
        {
          itf = (SOTentFunction *)B.e[i];
          jtf = (SOTentFunction *)B.e[j];
          
          if(itf->d->index > jtf->d->index){ hi_ind = itf->d->index; low_ind = jtf->d->index; }
          else { hi_ind = jtf->d->index; low_ind = itf->d->index; }
   
          k = 0;
          while(k < evaldot[hi_ind - offset].ne)
            {
              if(evaldot[hi_ind - offset].e[k].col == low_ind) 
                { 
                  Hev_ents->el[Hev_ents->nel].row = i;
                  Hev_ents->el[Hev_ents->nel].col = j;
                  Hev_ents->el[Hev_ents->nel].va = evaldot[hi_ind - offset].e[k].va;
                  Hev_ents->nel++;
		}
              k++;
	    }

	}

  }