/* See SOFunction.h */
/* Last edited on 2003-07-03 19:44:45 by ra014852 */

#include <SOGrid.h>
#include <SOFunction.h>
#include <SOTentFunction.h>
#include <SOProcFunction.h>
#include <SOWaveFunction.h>
#include <SOErrorFunction.h>
#include <SOLinCombFunction.h>

#include <filefmt.h>
#include <nget.h>
#include <fget.h>
#include <math.h> 
#include <stdlib.h> 
#include <string.h> 
#include <rn.h>
#include <rmxn.h> 
#include <js.h> 

#define OnBox SOIntegral_OnBox

#define FourPi 12.56637061435917

#define T SOFunction

void SOFunction_WriteAsSubclass(SOFunction *f, FILE *wr);
  /* Casts {f} to its widest proper effective subclass, and 
    calls the corresponding {write} method (which must shadow,
    but must not override, the {write} method of {f}. */
    
void debugmat(int m, int n, char *name, double *A);
  /* Prints the {m} by {n} matrix {A}, linearized by rows, on {stderr}. */

void SOFunction_RawTreeDot
  ( SOIntegral_Func integrand, 
    SOGrid_Node *C, 
    SOGrid_Index index,
    SOGrid_Index rank,
    SOGrid_Dim pDim, 
    SOGrid_Dim fDim, 
    double *sum,
    double *corr   
  );
  /* Finds recursivelly integral domains for each (SOGrid_Tree) leaf for 
     calculating precise dot products. */

/* IMPLEMENTATIONS */

void debugmat(int m, int n, char *name, double *A)
  { if (n != 5) { return; }
    fprintf(stderr, "\n%s:\n", name);
    rmxn_gen_print(stderr, m, n, A, "%16.9e", "", "", "\n", "  ", " ", "\n");
  }

SOFunction *SOFunction_Cast(OBJ *f)
  { SOFunction *ff = (SOFunction *)f;
    if ((f != NULL) && isprefix(SOFunction_TypeId, ff->type))
      { return (SOFunction *)f; }
    else
      { return NULL; }
  }


#define SOFunction_FileFormat "2003-01-16"

void SOFunction_WriteMth(T *f, FILE *wr)
  { char *t = f->type;
    assert(isprefix(SOFunction_TypeId, t), "type mismatch");
    filefmt_write_header(wr, "SOFunction", SOFunction_FileFormat);
    fprintf(wr, "type = %s\n", t);
    fprintf(wr, "domain_dim = %d\n", f->d->pDim);
    fprintf(wr, "range_dim = %d\n", f->d->fDim);
    SOFunction_WriteAsSubclass(f, wr);
    filefmt_write_footer(wr, "SOFunction");
  }
  
void SOFunction_WriteAsSubclass(SOFunction *f, FILE *wr)
  {
    { SOTentFunction *fp = SOTentFunction_Cast(f);
      if (fp != NULL) { fp->m->write(fp, wr); return; } 
    }
    { SOProcFunction *fp = SOProcFunction_Cast(f);
      if (fp != NULL) { fp->m->write(fp, wr); return; } 
    }
    { SOWaveFunction *fp = SOWaveFunction_Cast(f);
      if (fp != NULL) { fp->m->write(fp, wr); return; } 
    }
    { SOLinCombFunction *fp = SOLinCombFunction_Cast(f);
      if (fp != NULL) { fp->m->write(fp, wr); return; } 
    }
    { SOErrorFunction *fp = SOErrorFunction_Cast(f);
      if (fp != NULL) { fp->m->write(fp, wr); return; } 
    }
    /* 
    { SOSpline *fp = SOSpline_Cast(f);
      if (fp != NULL) { fp->m->write(fp, wr); return; } 
    }
    { SOBezFunction *fp = SOBezFunction_Cast(f);
      if (fp != NULL) { fp->m->write(fp, wr); return; } 
    }
    */
    fprintf(stderr, "unknown SOFunction subclass \"%s\"\n", f->type);
    assert(FALSE, "aborted");
  }

SOFunction *SOFunction_Read(FILE *rd)
  { char *t;
    SOFunction *f;
    int pDim, fDim;
    filefmt_read_header(rd, "SOFunction", SOFunction_FileFormat);

    t = nget_string(rd, "type"); fget_eol(rd);
    assert(isprefix(SOFunction_TypeId, t), "incorrect SOFunction type");
    pDim = nget_int(rd, "domain_dim"); fget_eol(rd);
    assert((pDim >= 1) && (pDim <= MAX_PDIM), "invalid domain dimension");
    fDim = nget_int(rd, "range_dim"); fget_eol(rd);
    assert((pDim >= 1) && (pDim <= MAX_FDIM), "invalid range dimension");
    /* Find the concrete subclass and call the corresponding {Read} function: */
    if (isprefix(SOTentFunction_TypeId, t))
      { f = (SOFunction *)SOTentFunction_Read(rd, pDim); }
    else if (isprefix(SOProcFunction_TypeId, t))
      { f = (SOFunction *)SOProcFunction_Read(rd, pDim, fDim); } 
    else if (isprefix(SOWaveFunction_TypeId, t))
      { f = (SOFunction *)SOWaveFunction_Read(rd, pDim); } 
    else if (isprefix(SOErrorFunction_TypeId, t))
      { f = (SOFunction *)SOErrorFunction_Read(rd, pDim); } 
    else if (isprefix(SOLinCombFunction_TypeId, t))
      { f = (SOFunction *)SOLinCombFunction_Read(rd, pDim);}
    else
      { fprintf(stderr, "unknown SOFunction subclass \"%s\"\n", t);
        assert(FALSE, "aborted");
      }

    fget_skip_formatting_chars(rd);

    filefmt_read_footer(rd, "SOFunction");
    free(t);
    return f;
  }

#define SOFunction_Basis_FileFormat "2003-01-16"
  
void SOFunction_WriteBasis(FILE *wr, Basis F)
  { int i;
    filefmt_write_header(wr, "SOFunction_Basis", SOFunction_Basis_FileFormat);
    fprintf(wr, "basis_dim = %d\n", F.nel);
    for (i = 0; i < F.nel; i++)
      { SOFunction *f = F.el[i]; 
        fprintf(wr, "elem_index = %d\n", i);
        f->m->write(f, wr); fputc('\n', wr);
      }
    filefmt_write_footer(wr, "SOFunction_Basis");
    fflush(wr);
  }

Basis SOFunction_ReadBasis(FILE *rd)
  { int dim;
    Basis F;
    int i;
    filefmt_read_header(rd, "SOFunction_Basis", SOFunction_Basis_FileFormat);
    dim = nget_int(rd, "basis_dim"); fget_eol(rd);
    assert(dim >= 0, "bad dimension");
    F = SOFunctionRef_vec_new(dim);
    fget_skip_formatting_chars(rd);
    for (i = 0; i < dim; i++) 
      { int ii = nget_int(rd, "elem_index"); 
        assert(ii == i, "wrong element index");
        fget_eol(rd);
        F.el[i] = SOFunction_Read(rd); fget_skip_formatting_chars(rd);
      }
    filefmt_read_footer(rd, "SOFunction_Basis");
    return F;
  }

static char **nameCache = NULL;
static Basis *basCache = NULL;
static nat szCache = 0; /* Allocated size of {nameCache}, {basCache}. */
static nat nCached = 0; /* Occupied entries in {nameCache}, {basCache}. */

Basis SOFunction_ReadBasisCached(char *fileName)
  { int i;
    Basis bas;
    /* Look up in cache */
    for (i = 0; i < nCached; i++)
      { if (strcmp(fileName, nameCache[i]) == 0) { return basCache[i]; }
      }
    /* Not found, read from file: */
    bas = SOFunction_ReadBasis(open_read(fileName));
    /* Ensure there is space in cache: */
    if (nCached >= szCache)
      { /* Expand cache: */
        nat sztmp = 2*szCache + 10;
        char **tmpc = (char **)notnull(malloc(sztmp*sizeof(char*)), "no mem for tmpc");         
        Basis *tmpb = (Basis *)notnull(malloc(sztmp*sizeof(Basis)), "no mem for tmpb");
        for (i = 0; i < nCached; i++) 
          { tmpc[i] = nameCache[i]; tmpb[i] = basCache[i]; }
        if (nameCache != NULL) { free(nameCache); }
        if (basCache != NULL) { free(basCache); }
        nameCache = tmpc;
        basCache = tmpb;
        szCache = sztmp;
      }
    /* Store in cache: */
    nameCache[nCached] = fileName;
    basCache[nCached] = bas;
    nCached++;
    return bas;
  }

/* INTEGRALS */

FuncMap IdentityMap (SOGrid_Dim uDim, SOGrid_Dim pDim)
  { FuncMap NoMap;
    NoMap.map = (FuncMapProc *)NULL;
    NoMap.uDim = uDim;
    NoMap.pDim = pDim;
    NoMap.vDim = uDim; /* Same as {uDim}. */
    return NoMap;
  }

void SOFunction_Integral
  ( SOFunction *f,
    FuncMap *FMap, 
    SOFunction *w,
    SOGrid_Tree *tree,
    bool verbose,
    double *rv
  )
  {
    static double fv[MAX_FDIM];

    SOFunction_RawIntegral(f, FMap, w, tree, verbose, fv);

  }

void SOFunction_RawIntegral
  ( SOFunction *f,
    FuncMap *FMap, 
    SOFunction *w,
    SOGrid_Tree *tree,
    bool verbose,
    double *rv
  )
  { SOGrid_Dim pDim = f->d->pDim; /* Dimension of domain. */
    SOGrid_Dim iDim = FMap->vDim; /* Dimension of integrand. */
    static double corr[MAX_FDIM]; /* Correction term for Kahan summation. */
    
    auto void Func(double *p, double *iv);
      /* Computes the integrand {I(p) = w(p)*FMap(f(p),p)}; 
        returns the result in {iv}. */

    void Func(double *p, double *iv) 
      { 
        static double fv[MAX_FDIM];
        double wv;
        /* Compute {iv = FMap(f(p),p)}: */
        if (FMap->map == NULL) 
          { f->m->eval(f, p, iv); }
        else
          { f->m->eval(f, p, fv);
            FMap->map(fv, p, iv);
          }
        /* Apply weight {w(p)}: */
        if(w != NULL) 
          { w->m->eval(w, p, &wv);
            rn_scale(iDim, wv, iv, iv);
          }
      }

    assert(f != NULL, "null function"); 
    assert(FMap != NULL, "null FuncMap"); 
    assert(w->d->fDim == 1, "weight must be a scalar field");
    assert(w->d->pDim == f->d->pDim, "weight/function domain mismatch");
    assert(f->d->fDim == FMap->uDim, "function/FuncMap range mismatch"); 
    rn_zero(iDim, rv);
    rn_zero(iDim, corr);
    SOIntegral_OnRootCell(Func, pDim, iDim, tree, rv, corr);
  }

/* INNER PRODUCTS */

void SOFunction_Dot
  ( SOFunction *f,
    FuncMap *FMap, 
    SOFunction *g, 
    FuncMap *GMap,
    SOFunction *w,
    SOGrid_Tree *tree,
    double *iv,
    bool verbose
  )
  {
    if (isprefix(SOTentFunction_TypeId, f->type) && isprefix(SOTentFunction_TypeId, g->type))
      { /* Two piecewise functions with same grid ({tree}, if given): */
	//        printf("## DEBUG> tenda x tenda\n\n");
        SOTentFunction *ftf = SOTentFunction_Cast(f);
        SOTentFunction *gtf = SOTentFunction_Cast(g);
        *iv = SOTentFunction_DotBoth(ftf, gtf);
        return;
      }
    else
    if (isprefix(SOTentFunction_TypeId, f->type))
      { /* A piecewise function (on {tree}, if given) versus a general function: */
	//        printf("## DEBUG> func x tenda\n\n");
        SOTentFunction *ftf = SOTentFunction_Cast(f);
        SOTentFunction_DotSingle(ftf, g, GMap, iv);
        return;
      }
    else
    if (isprefix(SOTentFunction_TypeId, g->type))
      { /* A piecewise function (on {tree}, if given) versus a general function: */
	//        printf("## DEBUG> tenda x func\n\n");
        SOTentFunction *gtf = SOTentFunction_Cast(g);
        SOTentFunction_DotSingle(gtf, f, FMap, iv);
        return;
      }
    else
      { /* Use a general integrator: */
	//        printf("## DEBUG> func x func\n\n");
        SOFunction_RawDot(f, FMap, g, GMap, w, NULL, iv);
        return;
      }
  }


void SOFunction_GradDot
  ( SOFunction *f,
    SOFunction *g, 
    SOFunction *w,
    SOGrid_Tree *tree,
    double *iv,
    bool verbose
  )
  { 
    if (isprefix(SOTentFunction_TypeId, f->type) && isprefix(SOTentFunction_TypeId, g->type))
      { /* Two piecewise functions with same grid ({tree}, if given): */
        SOTentFunction *ftf = SOTentFunction_Cast(f);
        SOTentFunction *gtf = SOTentFunction_Cast(g);
        *iv = SOTentFunction_GradDotBoth(ftf, gtf);
        return;
      }
    else
    if (isprefix(SOTentFunction_TypeId, f->type))
      { /* A piecewise function (on {tree}, if given) versus a general function: */
        SOTentFunction *ftf = SOTentFunction_Cast(f);
        SOTentFunction_GradDotSingle(ftf, g, iv);
        return;
      }
    else
    if (isprefix(SOTentFunction_TypeId, g->type))
      { /* A piecewise function (on {tree}, if given) versus a general function: */
        SOTentFunction *gtf = SOTentFunction_Cast(g);
        SOTentFunction_GradDotSingle(gtf, f, iv);
        return;
      }
    else
      SOFunction_RawGradDot(f, g, tree, iv);
  }


void SOFunction_Grad1Dot
  ( SOFunction *f,
    SOFunction *g, 
    SOFunction *w,
    SOGrid_Tree *tree,
    double *iv,
    bool verbose,
    int gradaxis
  )
  { 
    if (isprefix(SOTentFunction_TypeId, f->type) && isprefix(SOTentFunction_TypeId, g->type))
      { /* Two piecewise functions with same grid ({tree}, if given): */
        SOTentFunction *ftf = SOTentFunction_Cast(f);
        SOTentFunction *gtf = SOTentFunction_Cast(g);
        *iv = SOTentFunction_Grad1DotBoth(ftf, gtf, gradaxis);
        return;
      }
/*     else */
/*     if (isprefix(SOTentFunction_TypeId, f->type)) */
/*       { /\* A piecewise function (on {tree}, if given) versus a general function: *\/ */
/*         SOTentFunction *ftf = SOTentFunction_Cast(f); */
/*         SOTentFunction_Grad1DotSingle(ftf, g, iv, gradaxis); */
/*         return; */
/*       } */
/*     else */
/*     if (isprefix(SOTentFunction_TypeId, g->type)) */
/*       { /\* A piecewise function (on {tree}, if given) versus a general function: *\/ */
/*         SOTentFunction *gtf = SOTentFunction_Cast(g); */
/*         SOTentFunction_Grad1DotSingle(gtf, f, iv, gradaxis); */
/*         return; */
/*       } */
/*     else */
/*       SOFunction_RawGrad1Dot(f, g, tree, iv, gradaxis); */
  }


void SOFunction_RawDot
  ( SOFunction *f,
    FuncMap *FMap, 
    SOFunction *g, 
    FuncMap *GMap,
    SOFunction *w,
    SOGrid_Tree *tree, 
    double *sum
  )
  { SOGrid_Dim pDim = f->d->pDim; /* Dim of domain. */
    SOGrid_Dim mDim = FMap->vDim; /* Dim of {FMap(f(p),p)} and {GMap(g(p),p)}. */
    double fv[f->d->fDim], mfv[FMap->vDim];
    double gv[g->d->fDim], mgv[GMap->vDim];
    double corr[mDim];
    int i, j, l;

    assert(f != NULL, "null function f"); 
    assert(FMap != NULL, "null FuncMap FMap");
    assert(FMap->uDim == f->d->fDim, "function/FuncMap range mismatch");
    assert(FMap->pDim == f->d->pDim, "function/FuncMap domain mismatch");
    
    assert(g != NULL, "null function g"); 
    assert(GMap != NULL, "null FuncMap GMap");
    assert(GMap->uDim == g->d->fDim, "function/FuncMap range mismatch"); 
    assert(GMap->pDim == f->d->pDim, "function/FuncMap domain mismatch");

    for(i = 0; i < mDim; i++){ sum[i] = 0.0; corr[i] = 0.0; }
    
    auto void integrand(double *p, double *iv);

    void integrand(double *p, double *iv) 
      { int k;

        if (FMap->map == NULL) 
          { f->m->eval(f, p, mfv); }
        else
          { f->m->eval(f, p, fv);
            FMap->map(fv, p, mfv);
          }
        
        if (GMap->map == NULL) 
          { g->m->eval(g, p, mgv); }
        else
          { g->m->eval(g, p, gv);
            GMap->map(gv, p, mgv);
          }
       
        for(k = 0; k < mDim; k++) 
          iv[k] = (double) mfv[k] * mgv[k];           

        if(w != NULL) 
          { double wv;
            w->m->eval(w, p, &wv); 
            for(k = 0; k < mDim; k++) iv[k] *= wv;
          }
      }

    if( tree != NULL ) SOFunction_RawTreeDot(integrand, tree->root, 1, 0, pDim, mDim, sum, corr);
    else
      { 
        /* Uses 1.0 as grid upper limit for every direction. */ 
        int sidetimes = (int)ceil(1 / SOIntegral_MaxSize);
        int tottimes = 1.0, inc[pDim];
        double min[pDim], max[pDim], sideinc = SOIntegral_MaxSize;

        inc[0] = 1; for(i = 1; i < pDim; i++) inc[i] = inc[i - 1] * sidetimes;          

        /* Uses 0.0 as grid lower limit for every direction. */ 
        min[pDim - 1] = 0.0 - sideinc; max[pDim - 1] = 0.0;

        for(i = 0; i < pDim; i++) tottimes *= sidetimes;

        for(i = 0; i < tottimes; i++) 
          { for(j = 1; j < pDim; j++)
              if(i % inc[j] == 0)
                { for(l = 0; l < j; l++){min[l] = 0.0; max[l] = sideinc;}
                  min[j] += sideinc; max[j] += sideinc;
                } 
              else
                { min[0] += sideinc; max[0] += sideinc; }

            SOIntegral_Gauss_Limits(integrand, pDim, mDim, sum, corr, min, max, 0);
          }

      }

  }

void SOFunction_RawTreeDot
  ( SOIntegral_Func integrand, 
    SOGrid_Node *C, 
    SOGrid_Index index,
    SOGrid_Index rank,
    SOGrid_Dim pDim, 
    SOGrid_Dim fDim, 
    double *sum,
    double *corr   
  )
  {
    int i;
    if(C->c[0] && C->c[1])
      { 
        SOFunction_RawTreeDot(integrand, C->c[0], 2*index, rank+1, pDim, fDim, sum, corr);
        SOFunction_RawTreeDot(integrand, C->c[1], (2*index)+1, rank+1, pDim, fDim, sum, corr);
      }
    else
      { double min[pDim], max[pDim];  
        SOGrid_cell_coords(index, rank, pDim, min, max); /* Find limits for the leaf cell. */
        for(i = 0; i < pDim; i++)                        /* Canonical geometry. */
          { min[i] = min[i] / pow(2.0, (double)i/pDim); 
            max[i] = max[i] / pow(2.0, (double)i/pDim); 
          }

        SOIntegral_Gauss_Limits(integrand, pDim, fDim, sum, corr, min, max, 0);
      }
  }

void SOFunction_RawGradDot
  ( SOFunction *f, 
    SOFunction *g, 
    SOGrid_Tree *tree, 
    double *sum
  )
  { SOGrid_Dim pDim, fDim;          /* Dimension of domain and range of (f) and (g) */
    double corr[f->d->fDim];    /* Low-order bits of {*sum}. */
    int i, j, l;

    pDim = f->d->pDim;
    fDim = f->d->fDim;
    for(i = 0; i < fDim; i++){ sum[i] = 0.0; corr[i] = 0.0; }

    assert(pDim == g->d->pDim, "Functions domain dimension must be equal");
    assert(fDim == g->d->fDim, "Functions range dimension must be equal");

    auto void integrand(double *x, double *dx);

    void integrand(double *x, double *dx)
    { double dg[pDim*fDim], df[pDim*fDim];
      int i, j;

      /* Evaluate the function {f} gradient {df} at {x}: */
      f->m->grad((SOFunction *)f, x, df);

      /* Evaluate the function {g} gradient {dg} at {x}: */
      g->m->grad((SOFunction *)g, x, dg);

      for(i = 0; i < fDim; i++)
        {  
          dx[i] = 0;
 
          for(j = 0; j < pDim; j++) 
            { dx[i] += dg[(i*pDim)+j]*df[(i*pDim)+j]; }
        }
    }

    if( tree != NULL ) SOFunction_RawTreeDot(integrand, tree->root, 1, 0, pDim, fDim, sum, corr);
    else
      { int sidetimes = (int)ceil(1 / SOIntegral_MaxSize);
        int tottimes = 1.0, inc[pDim];
        double min[pDim], max[pDim], sideinc = SOIntegral_MaxSize;

        inc[0] = 1; for(i = 1; i < pDim; i++) inc[i] = inc[i - 1] * sidetimes;          
        min[pDim - 1] = 0.0 - sideinc; max[pDim - 1] = 0.0;

        for(i = 0; i < pDim; i++) tottimes *= sidetimes;

        for(i = 0; i < tottimes; i++) 
          { for(j = 1; j < pDim; j++)
              if(i % inc[j] == 0)
                { for(l = 0; l < j; l++){min[l] = 0.0; max[l] = sideinc;}
                  min[j] += sideinc; max[j] += sideinc;
                } 
              else
                { min[0] += sideinc; max[0] += sideinc; }

            SOIntegral_Gauss_Limits(integrand, pDim, fDim, sum, corr, min, max, 0);
          }

      }

  }


/* Arrays of {SOFunction*}: */

SOFunctionRef_vec SOFunctionRef_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(SOFunctionRef));
    SOFunctionRef_vec r;
    r.nel = v.nel; r.el = (SOFunctionRef*)v.el;
    return r;
  }