#define PROG_NAME "test_interp"
#define PROG_DESC "test of dyadic grids: interpolation"
#define PROG_VERS "1.0"

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

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

#define PROG_HELP \
  "  " PROG_NAME " \\\n" \
  "    " tin_options_HELP " \\\n" \
  "    " tin_plot_options_HELP " \\\n" \
  "    " argparser_help_info_HELP " \\\n" \
  "    < {INFILE}"

#define PROG_INFO \
  "NAME\n" \
  "  " PROG_NAME " - " PROG_DESC "\n" \
  "\n" \
  "SYNOPSIS\n" \
  PROG_HELP "\n" \
  "\n" \
  "DESCRIPTION\n" \
  "  This program takes a set of sampling points {p[0..n-1]}," \
  " the values of a function {f} at those points, and the" \
  " corresponding spatial derivatives of {f} up to order {CONT};" \
  " and computes a dyadic spline {h} with continuity {CONT} such" \
  " that {h(p[i]) = f(p[i])} for all {i}.\n" \
  "\n" \
  "  Each sampling point {p[i]} must be a vertex of the {d}-dimensional" \
  " dyadic multigrid, at a specific rank {r[i]}.  The spline {h} is" \
  " defined over the smallest finite dyadic grid {G} where every data" \
  " point {p[i]} is a complete vertex in layer {r[i]} of {G}.\n" \
  "\n" \
  "  The interpolant has the form\n" \
  "\n" \
  "    {h = SUM { u[j]*B[j] : j = 0..m-1 }}       (1)\n" \
  "\n" \
  "  where {B[0..m-1]} consists of all the maximal (or minimal)" \
  " polynomial tents of continuity order {CONT} over {G} that" \
  " are centered on the vertices {p[0..n-1]} and have" \
  " ranks {r[0..n-1]}. The coefficients {u[j]} are found by" \
  " the ordered interpolation algorithm (see C.Cardoso's thesis).\n" \
  "\n" \
  "  The function {f} and the interpolating spline {h} are" \
  " actually vector-valued, with {FDIM} components per domain point.\n" \
  "\n" \
  "INPUT FILE FORMAT\n" \
  "  The input file must contain one entry for each sampling point.  Each" \
  " entry of the input file must start on a new line, and may" \
  " continue for as many lineas as needed.  It must contain\n" \
  "\n" \
  "    * an integer, the rank {r[i]} of sampling point {p[i]};\n" \
  "    * {DDIM} numbers, the root-relative coors of {p[i]};\n" \
  "    * {FDIM*ND} numbers, the function and derivative values at {p[i]}.\n" \
  "\n" \
  "  The number {ND} is the count of all distinct partial derivatives" \
  " up to order {CONT} of a function with {DDIM} arguments, including" \
  " the derivative of order 0 (the function itself).  These data must" \
  " be sorted by increasing derivation order, and then by the axes of" \
  " derivation.  For example, if {DDIM} is 3 and {CONT} is 2, then the" \
  " number of derivatives {ND} is 10, and they must be presented in the" \
  " order {f,D0f,D1f,D2f,D00f,D01f,D02f,D11f,D12f,D22f}, where {D02f} (say)" \
  " means the second derivative of {f} along the domain" \
  " axes 0 and 2.\n" \
  "\n" \
  "  Each function or derivative value actually consists of {FDIM} consecutive" \
  " numbers.\n" \
  "\n" \
  "OPTIONS\n" \
  tin_options_HELP_INFO "\n" \
  "\n" \
  "PLOT OPTIONS\n" \
  tin_plot_options_HELP_INFO "\n" \
  "\n" \
  "DOCUMENTATION OPTIONS\n" \
  argparser_help_info_HELP_INFO "\n" \
  "\n" \
  "SEE ALSO\n" \
  "  test_approx(1).\n" \
  "\n" \
  "AUTHOR\n" \
  "  Started 2005-06-05 or earlier by Jorge Stolfi, IC-UNICAMP.\n" \
  "\n" \
  "MODIFICATION HISTORY\n" \
  "  2007-10-14 updated for new dyadic grid libs, by J. Stolfi.\n" \
  "\n" \
  "WARRANTY\n" \
  argparser_help_info_NO_WARRANTY "\n" \
  "\n" \
  "RIGHTS\n" \
  "  " test_approx_COPYRIGHT ".\n" \
  "\n" \
  argparser_help_info_STANDARD_RIGHTS

/* Datapoint interpolation with 2D dyadic splines. */

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

#include <affirm.h>
#include <jsmath.h>
#include <intsort.h>
#include <vec.h>
#include <r2.h>
#include <fget.h>
#include <pswr.h>
#include <pswr_color.h>
#include <pswr_iso.h>

#include <udg_grid.h>
#include <udg_pulse.h>
#include <udg_spline.h>
#include <mdg_grid.h>
#include <dg_tree.h>
#include <dg_locus.h>
#include <dg_tent.h>
#include <dg_tent_basis.h>
#include <bz_basic.h>
#include <dg_plot.h>
#include <dg_plot_2D_func.h>
#include <dg_makegrid.h>
#include <dg_test_tools.h>

typedef struct tin_options_t
  { mdg_dim_t dDim;             /* Dimension of domain space. */
    dg_cont_t continuity;      /* Continuity order of splines. */
    bz_patch_rdim_t fDim;      /* Dimension of range space. */
    bool_t superior;           /* TRUE uses a wide basis, FALSE (default) narrow. */
    /* Output options: */
    bool_t plot;               /* TRUE to generate function and error plots. */
    struct tin_plot_options_t *po; /* Plotting format options. */
    char *outName;             /* Prefix for output file names. */
  } tin_options_t;

#define tin_options_HELP \
  "    -dDim {DDIM} \\\n" \
  "    -fDim {FDIM} \\\n" \
  "    -maxRank {MAXRANK} \\\n" \
  "    -continuity {CONT} \\\n" \
  "    [ -superior | -inferior ] \\\n" \
  "    -outName NAME \\\n" \
  "    [ -plot ]" \

#define tin_options_INFO \
  "  -dDim {DDIM}\n" \
  "    Specifies the domain's dimension as {DDIM}.\n" \
  "\n" \
  "  -fDim {FDIM}\n" \
  "    Specifies the range's dimension as {FDIM}.\n" \
  "\n" \
  "  -maxRank {MAXRANK}\n" \
  "    Specifies the maximum rank of sampling vertices and tents.\n" \
  "\n" \
  "  -continuity {CONT}\n" \
  "    Specifies the continuity {CONT} of the interpolating splines.\n" \
  "\n" \
  "  -superior\n" \
  "  -inferior\n" \
  "    These two mutually exclusive flags specify the preferred" \
  " support size of the basis elements: widest possible or" \
  " narrowest possible, respectively.  if neither is specified," \
  " assumes \"-inferior\".\n" \
  "\n" \
  "  -outName {OUT_NAME}\n" \
  "    Specifies the common prefix {OUT_NAME} of all output file names.\n" \
  "\n" \
  "  -plot\n" \
  "    If this option is present, the program will generate a plot of" \
  " the interpolated function." 

typedef struct tin_plot_options_t 
  { mdg_rank_t plotDp; /* Maximum plot depth. */
    bool_t isolines;  /* TRUE to plot the isolines. */
  } tin_plot_options_t;
  
#define tin_plot_options_HELP \
  "    -plotDp {PLOTDP} \\\n" \
  "    [ -isolines ]" \

#define tin_plot_options_HELP_INFO \
  "  -plotDp {PLOTDP}\n" \
  "    This mandatory argument specifies the maximum recursion" \
  " depth when plotting.\n" \
  "\n" \
  "  -isolines\n" \
  "    If present, this optional switch causes the isolines" \
  " to be drawn."


int main(int argc, char **argv);
  
tin_options_t *tin_get_options(int argn, char **argc);

void tin_read_data
  ( FILE *rd,
    mdg_dim_t dDim,
    dg_locus_vec_t *L, 
    int nd,
    bz_patch_rdim_t fDim, 
    double_vec_t *f,  
    dg_tree_t G
  );
  /* Reads from {rd} a list of sampling points {p[0..np-1]}
    and {nf = nd*fDim} data values per point {f.e[0..np*nf-1]}}.

    Allocates the vector {*L} and stores into {L.e[i]} the vertex
    locus corresponding to the sampling point {p[i]}, for {i} in
    {0..np-1}. Also expands the tree {G} as needed so that each locus
    {L.e[i]} is a complete vertex.
    
    Allocates the vector {f} and stores into {f.e[k+fDim*(r+nd*i)]}
    the value of derivative number {r} of layer {k} of the
    function at the sampling point {p[i]}, for each
    {k} in {0..fDim-1}, {r} in {0..nd-1}, and {i} in {0..np-1}. */

bool_t tin_read_sample(FILE *rd, mdg_rank_t *r, mdg_dim_t dDim, double p[], int nf, double fp[]);
  /* Tries to read one entry of the input file, corresponding to one
    sampling point. The entry consists of a rank, which will be stored
    in {*r}; {dDim} point coordinates, which will be stored into
    {p[0..dDim-1]}, and {nf} data values, which will be stored into
    {f[0..nf-1]}.
    
    The procedure skips white space as needed. If it runs into the
    end-of-file before the first number, it just returns FALSE without
    changing {p} or {fp}. If it fails for any other reason, including
    invalid data format or premature end-of-file, it prints a message
    and fails. Otherwise it returns TRUE. */

dg_locus_t tin_point_to_locus(mdg_dim_t dDim, double p[], mdg_rank_t r);
  /* Returns the vertex locus {L} with rank {r} that is closest to the
    point whose root-relative coordinates are {p[0..dDim-1]}. Each
    coordinate is implicitly reduced to the range {[0 _ 1)} according
    to the toroidal topology. */

void tin_make_locus_complete(mdg_dim_t dDim, dg_locus_t L, dg_tree_t G);
  /* Makes sure that the {dDim}-dimensional vertex locus {L} is
    complete in the tree {G}; namely, that all {2^dDim} cells of its
    level that have {L} as a corner are present in {G}. */

void tin_sort_loci
  ( dg_locus_vec_t *L, 
    int nd, 
    bz_patch_rdim_t fDim, 
    double_vec_t *f
  );
  /* Reorders the loci {L.e[0..L.ne-1]} in order of increasing depth.
    Also reorders the corresponding array of data values {f.e[0..f.ne-1]},
    assumed to have {nd*fDim} consecutive elements for each locus in {L}. */

dg_tent_vec_t tin_basis_from_loci
  ( mdg_dim_t dDim, 
    udg_pulse_family_t fam[],
    dg_locus_vec_t *L
  );
  /* Builds a basis of tents for a {dDim}-dimensional dyadic grid,
    whose elements belong to family {fam[0..dDim-1]} and are centered
    at the loci {L.e[0..L.ne-1]}. */

void tin_interpolate
  ( mdg_dim_t dDim, 
    udg_pulse_family_t fam[],  
    dg_tent_vec_t *B,
    int nd, 
    bz_patch_rdim_t fDim, 
    double f[],  
    double u[] 
  );
  /* Takes a basis {B.e[0..nb-1]} of tents belonging to tent family
    {fam[0..dDim-1]}, where {nb = B.ne}; and a list {f[0..f.ne-1]} of
    values and derivatives of a {fDim}-valued function at the centers
    of those tents. Computes the coefficients {u[0..nb*fDim-1]} of a
    spline {h} with domain dimension {dDim} that interpolates those
    values and derivatives at the central loci of those tents.
    
    Let {nd} be the number of values and derivatives per point and
    layer that are required for interpolation of order {fam[j].c}
    along each axis {j}. Assumes that the family {fam} is of the
    Hermite kind; that is, there are {nd} mother tents in the family,
    all vertex-centerd, which comprise a cardinal basis for 
    the {nd} derivatives at the center.
    
    More precisely, let {np = nb/nd} be the number of sampling points.
    The procedure assumes that {B} is sorted by rank, and that
    {B.e[r+i*nd]} is the tent of family that is cardinal for the
    spatial derivative number {r} at {p[i]}, the sampling point number
    {i}. It also assumes that {f.e[k+fDim*(r+nd*i)]} is the given
    value of derivative number {r} of layer {k} of the function at
    {p[i]}; and it will put in {u[k+fDim*(r+nd*i)]} the coefficient of
    tent {B.e[r+nd*i]} in layer {k} of the function, for all {k} in
    {0..fDim-1}, {r} in {0..nd-1}, and {i} in {0..np-1}. */

void tin_build_spline
  ( mdg_dim_t dDim, 
    udg_pulse_family_t fam[],  
    dg_tent_vec_t *B,
    bz_patch_rdim_t fDim,
    double u[], 
    dg_tree_t G
  );
  /* Given a basis {B} with {nb = B.ne} tents belonging to family
    {fam[0..dDim-1]} and defined over the dyadic grid {G}, and a list
    of coefficients {u[0..nb*fDim-1]}, computes the spline {h} whose
    layer number {k} is {SUM{ u[k+fDim*j]*B.e[j] : j in 0..nb-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}. */

void tin_plot_spline
  ( mdg_dim_t dDim, 
    bz_patch_rdim_t fDim,
    dg_tree_t G,
    char *name,
    tin_plot_options_t *po
  );
  /* Generates a plot of the {dDim}-dimensional spline stored in {G}.
    Assumes that the spline has {fDim} values at each domain point. 
    The plot file will be named "{name}.eps". */

int tin_derivatives_count(mdg_dim_t dDim, dg_cont_t c);
  /* The number of derivatives of a function {f} with {dDim}
    arguments, up to order {c} (including the derivative of order 0,
    that is, the function itself). */

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

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

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

/* Max dimension of grid's domain: */
#define MAX_DDIM (4)  

/* Max dimension of function's range space: */
#define MAX_FDIM (4)  

/* TYPES */

int main(int argc, char **argv)
  { 
    tin_options_t *o = tin_get_options(argc, argv);
    int dDim = o->dDim;
    bz_patch_rdim_t fDim = o->fDim;
    int c = o->continuity;
    
    /* The grid's tree: */
    dg_tree_node_t *G = dg_test_make_trivial_tree();

    /* Number of function values and derivatives per range layer and sampling point: */
    int nd = tin_derivatives_count(dDim, c);

    /* Number of function values and derivatives per sampling point: */
    int nf = nd*fDim;
      
    /* Read the sampling vertices {L}, their function values {f}, and expand the tree {G}: */
    dg_locus_vec_t L; /* List of loci (vertices + depths) for interpolation: */
    double_vec_t f; /* The corresponding function values: */
    tin_read_data(stdin, dDim, &L, nd, fDim, &f, G);
    assert(f.ne == nf*L.ne);
    
    /* Sort the loci by increasing depth: */
    tin_sort_loci(&L, nd, fDim, &f);

    /* Build a Hermite tent family of the right continuity and degree: */
    int g = 2*c + 1;
    udg_pulse_family_t fam[dDim];
    int i;
    for (i = 0; i < dDim; i++) { fam[i] = udg_pulse_family(udg_PK_H, c, g); }
    assert(nd == dg_tent_mother_count(dDim, fam));
    
    /* Build a tent basis with one set of tents at each sampling vertex: */
    dg_tent_vec_t B = tin_basis_from_loci(dDim, fam, &L);
    assert(B.ne == nd*L.ne);
    assert(f.ne == fDim*B.ne);
    
    /* Coefficient vector: */
    double u[fDim*B.ne];
    /*  {u[i*fDim + r]} is the coefficient of tent {B[i]} for component {r} of the function. */

    /* Interpolate the function values at the given points: */
    tin_interpolate(dDim, fam, &B, nd, fDim, f.e, u);
    
    tin_build_spline(dDim, fam, &B, fDim, u, G);
    
    tin_plot_spline(dDim, fDim, G, o->outName, o->po);
    
    return 0;
  }
  
void tin_read_data
  ( FILE *rd,
    mdg_dim_t dDim,
    dg_locus_vec_t *L, 
    int nd,
    bz_patch_rdim_t fDim, 
    double_vec_t *f,  
    dg_tree_t G
  )   
  {
    int nf = nd*fDim; /* Number of data values per sampling point. */
    int np = 0;       /* Number of data points read so far. */
    (*L) = dg_locus_vec_new(0);
    (*f) = double_vec_new(0);
    while (TRUE) 
      { /* Read a sampling point {p} and its function values {fp[0..nf-1]}: */
        mdg_rank_t rp;
        double p[dDim];
        double fp[nf];
        bool_t ok = tin_read_sample(rd, &rp, dDim, p, nf, fp);
        fget_eol(rd);
        if (! ok) break;
        /* Convert {p} to a locus {Lp} of rank {rp}: */
        dg_locus_t Lp = tin_point_to_locus(dDim, p, rp);
        /* Make sure {L.e[np]} is a complete vertex in {G}: */
        tin_make_locus_complete(dDim, Lp, G);
        /* Store {Lp,fp} into {L,f}: */
        dg_locus_vec_expand(L, np);
        L->e[np] = Lp;
        double_vec_expand(f, (np + 1)*nf - 1);
        int k;
        for (k = 0; k < nf; k++) { f->e[np*nf + k] = fp[k]; }
      }

    dg_locus_vec_trim(L, np);
    double_vec_trim(f, np*nf);
  }

void tin_sort_loci
  ( dg_locus_vec_t *L, 
    int nd, 
    bz_patch_rdim_t fDim, 
    double_vec_t *f
  )
  {
    int np = L->ne;   /* Number of loci. */
    int nf = nd*fDim; /* Number of data values per locus. */
    
    /* Allocates the perm vector {ord} and its inverse {dro}: */
    int ord[np], i; 
    
    /* Set {ord[0..np-1]} to indices of elems of {L.e}, in rank order: */
    for (i = 0; i < np; i++) { ord[i] = i; }
    auto sign_t cmp_locus_rank(int x, int y); /* Compares ranks of {L.e[x]} and {L.e[y]}. */
    isrt_heapsort(ord, np, cmp_locus_rank, +1);
    
    /* Compute the inverse per {dro} of {ord}: */
    int dro[np];
    for (i = 0; i < np; i++) { dro[ord[i]] = i; }
    
    /* Swaps the elements in the order given by {ord}: */
    for (i = 0; i < np; i++)
      { int j = ord[i];
        assert(dro[j] == i);
        /* Swap locus {i} with locus {j}: */
        { dg_locus_t Lt = L->e[i]; L->e[i] = L->e[j]; L->e[j] = Lt; }
        /* Swap function values of locus {i} with locus {j}: */
        { int inf = i*nf, jnf = j*nf, k; 
          for (k = 0; k < nf; k++) 
            { double ft = f->e[inf+k]; f->e[inf+k] = f->e[jnf+k]; f->e[jnf+k] = ft; }
        }
        /* Remember that elem {L.e[i]} is now at {L.e[j]}: */
        dro[j] = dro[i]; ord[dro[i]] = j;
        ord[i] = i; dro[i] = i;
      }
      
    return;
        
    sign_t cmp_locus_rank(int x, int y)
      { mdg_rank_t rx = dg_locus_rank(L->e[x]); 
        mdg_rank_t ry = dg_locus_rank(L->e[y]);
        return mdg_rank_cmp(&rx, &ry);
      }
  }
  
void tin_interpolate
  ( mdg_dim_t dDim, 
    udg_pulse_family_t fam[],  
    dg_tent_vec_t *B,
    int nd, 
    bz_patch_rdim_t fDim, 
    double f[],  
    double u[] 
  )
  {
    /* Interpolation algorithm: */
    int i;
    for (i = 0; i < B.ne; i++)
      { 
        /* Get the locus (vertex) {Li}: */
        dg_tent_t Bi = B->e[i];
        
        /* Get the root-relative bbox {B} of its superior cell: */
        interval_t B[dDim];
        dg_cell_box_root_relative(dDim, Li.cell, B);
        
        /* Extract the lower corner {x} (root-relative vertex coords): */
        double x[dDim];
        for (r=0; r < dDim; r++) { x[i] = LO(B[i]); }
        
        /* Get the function values {fi} specified for it: */
        double *fi = &(f[i*nf]);
        
        /* Add values and derivatives {s[...]} of all tents for all prev loci: */
        double s[nf];
        for (kr = 0; kr < nf; kr++) { s[kr] = 0; }
        for (j = 0; j < i; j++)
          { 
            for (tix = 0; tix < nd; tix++)
              { 
                dg_tent_t t = B.e[j*nd + tix];
                assert(t.tix == tix);
                assert(t.cell == Li.cell);
                /* Coefficients for this tent: */
                double *ujt = &(u[j*nf + tix*fDim]);
                /* Get value and all derivs of {t} at {x}: */
                double v[tDim];
                dg_tent_eval_diff_root_relative(dDim, DG_TK_H, c, g, t, x, v);
                /* Accumulate on {s}: */
                for (k = 0; k < tDim, k++)
                  { 
                    for (r = 0; r < fDim; r++)
                      { 
                        s[k*fDim + r] += ujt[r] * v[k*fDim + r];
                      }
                  }
              }
        
          }
          
        /* Assume that the tents are cardinal with respect to derivative: */
        /* Subtract {s} from specified values {fi}, result is coeffs {u}: */
        for (kr = 0; kr < nf; kr++) { ui[kr] = fi[kr] - s[kr]; }
      }
    
  }
  
void tin_build_spline
  ( mdg_dim_t dDim, 
    bz_patch_rdim_t fDim,
    udg_pulse_family_t fam[],  
    dg_tent_vec_t *B,
    double u[], 
    dg_tree_t G
  )
  {
    affirm(FALSE, "not implemented");
  }

void tin_plot_spline
  ( mdg_dim_t dDim, 
    bz_patch_rdim_t fDim,
    dg_tree_t G,
    char *name,
    tin_plot_options_t *po
  )
  {
    /* Plot the result: */
    char *plot_name = o->outName;
    PSStream *ps = dg_plot_init(plot_name, TRUE, "a4", "Plot test", bbox);
    dg_plot_2D_func(ps, &b, 0, NULL, G, o->plotDp, o->isolines);
    dg_plot_finish(ps);
  }

tin_options_t *tin_get_options(int argn, char **argc)
  {
    argparser_t *pp = argparser_new(stderr, argc, argv);
    
    argparser_set_help(pp, PROG_NAME " version " PROG_VERS ", usage:\n" PROG_HELP);
    argparser_set_info(pp, PROG_INFO);
    argparser_process_help_info_options(pp);
     
    tin_options_t *o = notnull(malloc(sizeof(tin_options_t)), "no mem");

    argparser_get_keyword(pp, "-dDim");                               
    o->dDim = argparser_get_next_int(pp, 1, dg_MAX_DIM);  
                                                 
    argparser_get_keyword(pp, "-fDim");                               
    o->fDim = argparser_get_next_int(pp, 1, bz_MAX_DIM);  
                                                 
    argparser_get_keyword(pp, "-continuity");                               
    o->consinuity = argparser_get_next_int(pp, 1, dg_MAX_DEGREE);  
    
    if (argparser_keyword_present(pp, "-superior"))
      { o->superior = TRUE; }
    else if (argparser_keyword_present(pp, "-inferior"))
      { o->superior = FALSE; }
    else 
      { o->superior = TRUE; }
      
    o->plot = argparser_keyword_present(pp, "-plot");
    
    o->po = tin_get_plot_options(pp);
                                                 
    argparser_get_keyword(pp, "-outName");                               
    o->outName = argparser_get_next(pp);  

    /* Check for spurious args: */
    argparser_finish(pp);

    return o;
  }

tin_plot_options_t *tin_get_plot_options(argparser_t *pp)
  {
    tin_options_t *po = notnull(malloc(sizeof(tin_plot_options_t)), "no mem");

    argparser_get_keyword(pp, "-plotDp");                               
    po->plotDp = argparser_get_next_int(pp, 1, dg_MAX_RANK);  
                                                 
    po->isolines = argparser_keyword_present(pp, "-isolines");                               
    po-> = argparser_get_next_int(pp, 1, bz_MAX_DIM);  

    return po;
  }