/* See {rdo_approx_basis.h}. */
#define rdo_approx_basis_C_COPYRIGHT "Copyright © 2008 Danillo Pereira and J. Stolfi, UNICAMP"
/* Last edited on 2023-02-12 07:48:30 by stolfi */ 

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

#include <frgb.h>
#include <argparser.h>
#include <affirm.h>
#include <dspmat.h>
#include <fget.h>
#include <fget_geo.h>
#include <nget.h>
#include <vec.h>

#include <rdo_geometry.h>
#include <rdo_solid.h>
#include <rdo_camera.h>
#include <rdo_approx_basis.h>

#define debug_approx_basis (FALSE)
  /* Define as TRUE to get a printout of the basis elements. */

vec_typeimpl(approx_basis_t, approx_basis, basis_elem_t);

approx_basis_t rdo_approx_basis_create
  ( dist_type_t tpDist, 
    basis_elem_type_t tpElem, 
    site_t sb[], 
    double rb[],
    int n
  )
  { 
    approx_basis_t bas = approx_basis_new(n);
    int i;
    for (i = 0; i < n; i++)
      { basis_elem_t *eP = &(bas.e[i]); 
        eP->tpDist = tpDist;
        eP->tpElem = tpElem;
        eP->position = sb[i].position;
        eP->normal = sb[i].normal;
        eP->radius = rb[i];
        if (debug_approx_basis)
          { fprintf(stderr, "bas[%5d] = ", i);
            rdo_approx_basis_elem_write(stderr, eP);
            fprintf(stderr, "\n");
          }
      }
    return bas;
  }

dspmat_t rdo_approx_basis_build_matrix(site_vec_t *sts, approx_basis_t *bas, int avgCover)
  {
    int rows = sts->ne;
    int cols = bas->ne;
    
    if (avgCover < 1) { avgCover = 1; }
    if (avgCover > rows) { avgCover = rows; }
    dspmat_t M = dspmat_new(rows, cols, avgCover*cols);

    int nv = cols;
    double *v = NOTNULL(malloc(nv * sizeof(double)));

    int i;
    int posM = 0; /* Number of elements stored into {M}. */
    for (i = 0; i < rows; i++)
      { /* Compute row {i} of the matrix: */
        rdo_approx_basis_eval_all_elements(bas, &(sts->e[i]), v);
        posM = dspmat_add_row(&M, posM, i, v, nv);
      }
    dspmat_trim(&M, posM);
    
    free(v);
    return M;
  }
  
void rdo_approx_basis_eval_all_elements(approx_basis_t *bas, site_t *st, double v[])
  {
    int i;
    bool_t normYES = FALSE; /* Is unit-sum normalization required? */
    bool_t normNOT = FALSE; /* Is unit-sum normalization forbidden? */
    for (i = 0; i < bas->ne; i++)
      { basis_elem_t *eP = &(bas->e[i]); 
        v[i] = rdo_approx_basis_elem_raw_value(eP, st);
        bool_t norm = rdo_basis_approx_element_needs_normalization(eP->tpElem);
        normYES |= norm;
        normNOT |= (! norm);
      }
    demand(normYES != normNOT, "ambiguous base normalization class");
    if (normYES) { rdo_approx_basis_normalize_to_unit_sum(v, bas->ne); }
  }
  
bool_t rdo_basis_approx_element_needs_normalization(basis_elem_type_t tpElem)
  {
    return 
      ( (tpElem == basis_elem_type_Shepard) ||
        (tpElem == basis_elem_type_RadialShepard)
      );
  }

void rdo_approx_basis_normalize_to_unit_sum(double v[], int n)
  {
    double sum_v = TINY_SUM;
    int ninf = 0; /* Count of {+INF} weights. */
    int i;
    for (i = 0; i < n; i++)
      { if (isnan(v[i]))
          { sum_v = NAN; }
        else if (isfinite(v[i]))
          { sum_v += v[i]; }
        else
          { demand(v[i] == +INF, "invalid infinite value");
            sum_v = +INF; ninf++; 
          }
      }
    demand(ninf <= 1, "two or more infinite-valued elements at the same site");
    if (isnan(sum_v))
      { for (i = 0; i < n; i++) { v[i] = NAN; } }
    if (ninf > 0)
      { for (i = 0; i < n; i++) { v[i] = (v[i] == +INF ? 1.0 : 0.0); } }
    else
      { for (i = 0; i < n; i++) { v[i] /= sum_v; } }
  }
  
double rdo_approx_basis_elem_raw_value(basis_elem_t *e, site_t *st)
  {
    switch(e->tpElem)
      {
        case basis_elem_type_Ball:
          return rdo_approx_basis_eval_element_Ball(e, st);

        case basis_elem_type_Radial:
          return rdo_approx_basis_eval_element_Radial(e, st);
          
        case basis_elem_type_Shepard:
          return rdo_approx_basis_eval_element_Shepard(e, st);

        case basis_elem_type_RadialShepard:
          return rdo_approx_basis_eval_element_RadialShepard(e, st);

        default:
          assert(FALSE);
      }
  }

#define SUPPORT_RADIUS_TOL 1.0e-6
  /* Adjustment factor used to make sure that 
    every basis element is really zero on the boundary of its nominal
    support, in spite of any roundoff errors that may
    affect the distance from the nominal center. Namely,
    the element's value is zero if the distance is greater than
    or equal to {1-SUPPORT_RADIUS_TOL} times the nominal
    support radius. */

double rdo_approx_basis_eval_element_Ball(basis_elem_t *e, site_t *st)
  {
    demand(e->tpElem == basis_elem_type_Ball, "invalid element type");
    site_t est = (site_t){ .position = e->position, .normal = e->normal, .isd = -1};
    double dist = rdo_site_distance(&(est), st, e->tpDist);
    return (dist*(1 + SUPPORT_RADIUS_TOL) >= e->radius ? 0.0 : 1.0);
  }

double rdo_approx_basis_eval_element_Radial(basis_elem_t *e, site_t *st)
  {
    demand(e->tpElem == basis_elem_type_Radial, "invalid element type");
    site_t est = (site_t){ .position = e->position, .normal = e->normal, .isd = -1};
    double dist = rdo_site_distance(&(est), st, e->tpDist);
    if (dist*(1 + SUPPORT_RADIUS_TOL) >= e->radius)
      { return 0.0; }
    else 
      { /* The mother function is a Gaussian bell with {sigma = e->radius/3}: */
        double z = dist/(e->radius/3.0);
        return exp(-z*z/2);
      }
  }

double rdo_approx_basis_eval_element_Shepard(basis_elem_t *e, site_t *st)
  {
    demand(e->tpElem == basis_elem_type_Shepard, "invalid element type");
    site_t est = (site_t){ .position = e->position, .normal = e->normal, .isd = -1};
    double dist = rdo_site_distance(&(est), st, e->tpDist);
    if (dist*(1 + SUPPORT_RADIUS_TOL) >= e->radius)
      { return 0.0; }
    else if (dist == 0.0)
      { return +INF; }
    else
      { /* The mother function is {1/dist^2} times the Gaussian bell with {sigma = e->radius/3}: */
        double z = dist/(e->radius/3.0);
        return exp(-z*z/2)/(dist*dist);
      }
  }

double rdo_approx_basis_eval_element_RadialShepard(basis_elem_t *e, site_t *st)
  {
    demand(e->tpElem == basis_elem_type_RadialShepard, "invalid element type");
    site_t est = (site_t){ .position = e->position, .normal = e->normal, .isd = -1};
    double dist = rdo_site_distance(&(est), st, e->tpDist);
    if (dist*(1 + SUPPORT_RADIUS_TOL) >= e->radius)
      { return 0.0; }
    else if (dist == 0.0)
      { return 1.0; }
    else
      { /* The mother function is a Gaussian bell with {sigma = e->radius/3}: */
        double z = dist/(e->radius/3.0);
        return exp(-z*z/2);
      }
  }

#define MAX_NUM_NEIGHBORS 100
  /* Max value of the "-numNeighbors" option that the user may specify. */

approx_basis_options_t *rdo_approx_basis_options_parse(argparser_t *pp)
  {
    approx_basis_options_t *opBas = NOTNULL(malloc(sizeof(approx_basis_options_t)));
    /* ---------------------------------------------------------------------- */
    if (argparser_keyword_present(pp, "-numNeighbors"))
      { opBas->numNeighbors = (int)argparser_get_next_int(pp, 1, MAX_NUM_NEIGHBORS); }
    else
      { opBas->numNeighbors = 10; }
    /* ---------------------------------------------------------------------- */
    return opBas;
  }

double rdo_approx_basis_pick_elem_radius(dist_type_t tpDist, site_vec_t *sts, int ind, int m) 
  {
    int max_clos = m+1; /* Number of closest sites to find. */
    
    int n_clos = 0; /* Number of closest sites actually found ({max_clos} or less). */
    
    /* The {n_clos} sites closest to {st[ind]}, excluding itself:  */
    int ind_clos[max_clos];    /* Indices of those sites. */
    double dst_clos[max_clos]; /* Their distances to {st[ind]} in increasing order. */

    site_t *sr = &(sts->e[ind]);
    
    demand(sr->isd >= 0, "site is not on the scene's surface");
    
    /* Look first for sites on the same solid object: */
    rdo_site_find_closest_sites(sr, sts, max_clos, tpDist, TRUE, ind_clos, dst_clos, &n_clos);

    if (n_clos == 0)
      { /* No other sites on the same solid object; try again with all sites: */
        rdo_site_find_closest_sites(sr, sts, max_clos, tpDist, FALSE, ind_clos, dst_clos, &n_clos);
      }
    
    if (n_clos == 0)
      { /* No clue about {sr}'s nominal radius: */
        return NAN;
      }
    else 
      { double R = dst_clos[n_clos-1];
        /* Assume that sites are uniformly distributed on a mostly flat surface: */
        double corr = (n_clos == max_clos ? 1.0 : sqrt(((double)max_clos)/((double)n_clos)));
        return corr*R;
      }
  }

void rdo_approx_basis_elem_write(FILE *wr, basis_elem_t *be)
  {
    char *strElem = "!!!BUG!!!";
    switch(be->tpElem)
      {
        case basis_elem_type_Ball:          strElem = "Ball"; break;
        case basis_elem_type_Radial:        strElem = "Radial"; break;
        case basis_elem_type_Shepard:       strElem = "Shepard"; break;
        case basis_elem_type_RadialShepard: strElem = "RadialShepard"; break;
        case basis_elem_type_None:          strElem = "None"; break;
      }
    fprintf(wr, "%s", strElem);
    
    char *strDist = "!!!BUG!!!";
    switch(be->tpDist)
      {
        case dist_type_Euclidean:     strDist = "Euclidean"; break;
        case dist_type_Directional:   strDist = "Directional"; break;
      }
    fprintf(wr, " %s", strDist);
    
    r3_gen_print(wr, &(be->position), "%9.3f", " ( ", " ", " )");
    r3_gen_print(wr, &(be->normal), "%9.3f", " ( ", " ", " )");
    fprintf(wr, " %9.3f", be->radius);
  }                          

basis_elem_t rdo_approx_basis_elem_read(FILE *rd)
  {
    basis_elem_t be;
    
    char *strElem = fget_string(rd);
    if (strcmp(strElem, "Ball") == 0)
      { be.tpElem = basis_elem_type_Ball; }
    else if (strcmp(strElem, "Radial") == 0)
      { be.tpElem = basis_elem_type_Radial; }
    else if (strcmp(strElem, "Shepard") == 0)
      { be.tpElem = basis_elem_type_Shepard; }
    else if (strcmp(strElem, "RadialShepard") == 0)
      { be.tpElem = basis_elem_type_RadialShepard;  }
    else if (strcmp(strElem, "None") == 0)
      { be.tpElem = basis_elem_type_None; }
    else
      { demand(FALSE, "Unrecognized element type");
        be.tpElem = basis_elem_type_None;
      }
    
    char *strDist = fget_string(rd);
    if (strcmp(strDist, "Euclidean") == 0)
      { be.tpDist = dist_type_Euclidean; }
    else if (strcmp(strDist, "Directional") == 0)
      { be.tpDist = dist_type_Directional; }
    else
      { demand(FALSE, "Unrecognized element type");
        be.tpDist = dist_type_Euclidean;
      }
      
    fget_skip_spaces_and_match(rd, "(");
    be.position = fget_r3(rd);
    fget_skip_spaces_and_match(rd, ")");

    fget_skip_spaces_and_match(rd, "(");
    be.normal = fget_r3(rd);
    fget_skip_spaces_and_match(rd, ")");

    be.radius = fget_double(rd);

    return be;
  }                        

#define approx_basis_FILE_VERSION "2008-07-14"
  /* Identifies the write-out format for a list of basis_elemes. */
  
void rdo_approx_basis_write(FILE *wr, approx_basis_t *bas)
  {
    /* Writes the header line: */
    filefmt_write_header(wr, "approx_basis_t", approx_basis_FILE_VERSION);
    /* Here we should write comments provided by the client. */

    /* Writes the number of basis_elemes: */
    fprintf(wr, "num_elems = %d\n", bas->ne);
    
    /* Writes the basis_elemes: */
    int i;
    for(i = 0; i < bas->ne; i++)
      { rdo_approx_basis_elem_write(wr, &(bas->e[i]));
        fprintf(wr, "\n");    
      }
    
    /* Writes the footer line, and synchronizes the file: */
    filefmt_write_footer(wr, "approx_basis_t");
    fflush(wr);
  }

approx_basis_t rdo_approx_basis_read(FILE *rd)
  {
    /* Reads the header line, and comments (if any): */
    filefmt_read_header(rd, "approx_basis_t", approx_basis_FILE_VERSION);
    char *cmt = filefmt_read_comment(rd, '#');
    free(cmt); /* For now. */

    /* Reads the number of basis_elemes in the list: */
    int n_bas = nget_int32(rd, "num_elems"); fget_eol(rd);
    
    /* Reads the basis_elemes: */
    approx_basis_t bas = approx_basis_new(n_bas);
    int i;
    for(i = 0; i < n_bas; i++)
      { bas.e[i] = rdo_approx_basis_elem_read(rd);
        fget_eol(rd);    
      }
    
    /* Reads the footer line: */
    filefmt_read_footer(rd, "approx_basis_t");

    return bas;
  }