/* See SOApprox.h */
/* Last edited on 2003-09-15 17:19:58 by ra014852 */

#include <SOApprox.h>

#include <SOBasic.h>
#include <SOGrid.h>
#include <SOFunction.h>
#include <SOMatrix.h>
#include <SOBasisMatrices.h>
#include <SOLinCombFunction.h>
#include <SOTentFunction.h>
#include <SOProcFunction.h>
#include <SOPlotParams.h>
#include <SO2DPlot.h>
#include <SOPlot.h>

#include <vec.h>
#include <rn.h>
#include <js.h>
#include <gauss_elim.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <values.h>

SOGrid_Tree *SOApprox_ReadTree(char *fileName)
  { if (strcmp(fileName, "") == 0)
      { return NULL; }
    else
      { FILE *rd = (strcmp(fileName, "-") != 0 ? open_read(fileName) : stdin);
        SOGrid_Tree * tree = SOGrid_Tree_read(rd);
        if (rd != stdin) { fclose(rd); }
        return tree;
      }
  } 

SOFunction *SOApprox_ReadFunction(char *fileName)
  { if (strcmp(fileName, "") == 0)
      { return NULL; }
    else
      { FILE *rd = (strcmp(fileName, "-") != 0 ? open_read(fileName) : stdin);
        SOFunction *f = SOFunction_Read(rd);
        if (rd != stdin) { fclose(rd); }
        return f;
      }
  }

Basis SOApprox_ReadBasis(char *fileName)
  { FILE *rd = (strcmp(fileName, "-") != 0 ? open_read(fileName) : stdin);
    Basis bas = SOFunction_ReadBasis(rd);
    if (rd != stdin) { fclose(rd); }
    return bas;
  }

SOMatrix SOApprox_ReadMatrix(char *fileName)
  { FILE *rd = (strcmp(fileName, "-") != 0 ? open_read(fileName) : stdin);
    SOMatrix M = SOMatrix_Read(rd);
    if (rd != stdin) { fclose(rd); }
    return M;
  }

SOListMatrix SOApprox_ReadListMatrix(char *fileName)
  { FILE *rd = (strcmp(fileName, "-") != 0 ? open_read(fileName) : stdin);
    SOListMatrix M = SOListMatrix_Read(rd);
    if (rd != stdin) { fclose(rd); }
    return M;
  }

SOMatrix SOApprox_GetBasisMatrix(char *matName, bool cholesky)
  { if (cholesky)
      { return SOApprox_ReadMatrix(txtcat(matName, "-ev-ch.mat")); }
    else
      { return SOApprox_ReadMatrix(txtcat(matName, "-ev.mat")); }
  }
  
SOMatrix SOApprox_GetTransferMatrix(char *matName)
  { return SOApprox_ReadMatrix(txtcat(matName, "-ev.mat")); } 


void SOApprox_SimoGetRightHandSide
  ( Basis bas,                /* Basis for the approximation space. */
    SOListMatrix Hev,         /* Evaluation dot product matrix. */
    double reserv_box,
    Resv_Well_vec wells,
    SOFunction *saturation,   /* Current Water saturation approximation function. */
    SOFunction *pressure,     /* Current Oil pressure approximation function. */
    FuncMap **Frelperm, 
    FuncMap **Fvisc,
    FuncMap **Fform,
    FuncMap **Fgravden,
    FuncMap *Fabperm,
    double_vec b
  )
  { 
    /* Function maps obey positions 0: Water and 1: Oil. */
    int i, j, l;
    unsigned int dim = bas.nel;
/*     SOGrid_Dim pDim = bas.el[0]->d->pDim; */
/*     double min[pDim], max[pDim]; */

/*     fprintf(stderr, "Computing the right side vector...\n"); */
/*     assert(Frelperm[0]->map != NULL && Frelperm[1]->map != NULL && */
/*            Fform[0]->map != NULL && Fform[1]->map != NULL && */
/*            Fgravden[0]->map != NULL && Fgravden[1]->map != NULL && */
/*            Fvisc[0]->map != NULL && Fvisc[1]->map != NULL && */
/*            Fabperm->map != NULL, "NULL property function."); */

    for(i = 0; i < b.nel; i++)b.el[i] = 0.0; 

    for(i = 0; i < wells.nel; i++)
      if(wells.el[i].type == 0)
        {
          for(j = 0; j < Hev.ents.nel; j++)
            if(wells.el[i].ind == Hev.ents.el[j].row)
              for(l = 0; l < Hev.length; l++)
                b.el[Hev.ents.el[j].col] += -(Hev.ents.el[j].va[l] * reserv_box) * wells.el[i].wflux / wells.el[i].vol;        
        }
      else
        {
          for(j = 0; j < Hev.ents.nel; j++)
            if(wells.el[i].ind == Hev.ents.el[j].row)
              for(l = 0; l < Hev.length; l++)
                {
                  b.el[Hev.ents.el[j].col] += -(Hev.ents.el[j].va[l] * reserv_box) * wells.el[i].wflux / wells.el[i].vol;        
                  b.el[Hev.ents.el[j].col + dim] += -(Hev.ents.el[j].va[l] * reserv_box) * wells.el[i].oflux / wells.el[i].vol;     
                }
        }

  }


void SOApprox_SimoGetLeftMatrix
  ( Basis bas,                /* Basis for the approximation space. */
    SOMatrix *H,              /* IN/OUT Ridigity matrix. */
    SOListMatrix Hev,         /* Evaluation dot product matrix. */
    SOListMatrix Hgr,         /* Gradient dot product matrix. */
    double res_side,
    SOFunction *saturation,   /* Current Water saturation approximation function. */
    SOFunction *pressure,     /* Current Oil pressure approximation function. */
    SOFunction *porosity,     /* Porosity function. */
    FuncMap *Fpor, 
    FuncMap **Frelperm, 
    FuncMap **Fvisc,
    FuncMap **Fform,
    FuncMap *Fabperm,
    int_vec *intersec
  )
  { 
    /* Function maps obey positions 0: Water and 1: Oil. */
    unsigned int dim = bas.nel;
    SOGrid_Dim pDim = bas.el[0]->d->pDim;
    double min[pDim], max[pDim], sat, press, reserv_box, gradscale = 1.0; 
    int i, j, k, l, phase, higher, max_cells = (1 << pDim);
    double wrelperm[Hgr.ents.nel][max_cells], wvisc[Hgr.ents.nel][max_cells], wform[Hgr.ents.nel][max_cells]; 
    double orelperm[Hgr.ents.nel][max_cells], ovisc[Hgr.ents.nel][max_cells], oform[Hgr.ents.nel][max_cells]; 
    double abperm[Hgr.ents.nel][max_cells], por[Hev.ents.nel][max_cells];
    double wformev[Hev.ents.nel][max_cells], oformev[Hev.ents.nel][max_cells];
    SOTentFunction *tf[2];
    SOGrid_Index cell;
 
    fprintf(stderr, "Computing the left side matrix...\n");
    assert(Frelperm[0]->map != NULL && Frelperm[1]->map != NULL &&
           Fform[0]->map != NULL && Fform[1]->map != NULL &&
           Fvisc[0]->map != NULL && Fvisc[1]->map != NULL &&
           Fabperm->map != NULL, "NULL property function.");

    reserv_box = 1.0; for(i = 0; i < pDim; i++) reserv_box *= res_side;
    if(pDim == 1) gradscale = 1.0 / res_side;
    if(pDim == 3) gradscale = res_side;

    for(i = 0; i < Hgr.ents.nel; i++)
      { 
        tf[0] = (SOTentFunction *)bas.el[Hgr.ents.el[i].row];
        tf[1] = (SOTentFunction *)bas.el[Hgr.ents.el[i].col];

        higher = 0; if(tf[1]->d->index > tf[0]->d->index) higher = 1;

        for(l = 0; l < max_cells; l++)
          {
            cell = SOTent_get_brick(tf[higher]->d->index, pDim, tf[higher]->d->rank, l);
            SOGrid_cell_coords(cell, tf[higher]->d->rank, pDim, min, max);
            for(j = 0; j < pDim; j++)                                 /* Canonical geometry. */
              { min[j] = min[j] / pow(2.0, (double)j/pDim); 
                max[j] = max[j] / pow(2.0, (double)j/pDim); 
              }

            for(j = 0; j < pDim; j++) max[j] = (max[j] + min[j]) / 2.0;
            //saturation->m->eval(saturation, max, &sat);  pressure->m->eval(pressure, max, &press);

            /* Calculates <sat> and <press> using <intersec>. */
            sat = 0.0; press = 0.0;
            int trow = Hgr.ents.el[i].row, tcol = Hgr.ents.el[i].col;
            double tval; bool newt;
            SOLinCombFunction *lcsat = SOLinCombFunction_Cast(saturation);
            SOLinCombFunction *lcpress = SOLinCombFunction_Cast(pressure);
            for(j = 0; j < intersec[trow].nel; j++)
              {
                bas.el[intersec[trow].el[j]]->m->eval(bas.el[intersec[trow].el[j]], max, &tval);
		sat += (double)tval * lcsat->d->coef.el[intersec[trow].el[j]];
		press += (double)tval * lcpress->d->coef.el[intersec[trow].el[j]];
              }
            for(j = 0; j < intersec[tcol].nel; j++)
              {
                newt = TRUE;
                for(k = 0; k < intersec[trow].nel; k++)
                  if(intersec[tcol].el[j] == intersec[trow].el[k]) newt = FALSE;
                  
                if(newt)
                  {
                    bas.el[intersec[tcol].el[j]]->m->eval(bas.el[intersec[tcol].el[j]], max, &tval);
	            sat += (double)tval * lcsat->d->coef.el[intersec[tcol].el[j]];
                    press += (double)tval * lcpress->d->coef.el[intersec[tcol].el[j]];
                  }
              }


            phase = 0;
            Frelperm[phase]->map(&sat, max, &(wrelperm[i][l]));   Fvisc[phase]->map(&press, max, &(wvisc[i][l]));
            Fform[phase]->map(&press, max, &(wform[i][l]));

            phase = 1;
            Frelperm[phase]->map(&sat, max, &(orelperm[i][l]));   Fvisc[phase]->map(&press, max, &(ovisc[i][l]));
            Fform[phase]->map(&press, max, &(oform[i][l]));

            Fabperm->map(&press, max, &(abperm[i][l])); 
          }
      }

    for(i = 0; i < Hev.ents.nel; i++)
      { 
        tf[0] = (SOTentFunction *)bas.el[Hev.ents.el[i].row];
        tf[1] = (SOTentFunction *)bas.el[Hev.ents.el[i].col];

        higher = 0; if(tf[1]->d->index > tf[0]->d->index) higher = 1;

        for(l = 0; l < max_cells; l++)
          {
            cell = SOTent_get_brick(tf[higher]->d->index, pDim, tf[higher]->d->rank, l);
            SOGrid_cell_coords(cell, tf[higher]->d->rank, pDim, min, max);
            for(j = 0; j < pDim; j++)                                 /* Canonical geometry. */
              { min[j] = min[j] / pow(2.0, (double)j/pDim); 
                max[j] = max[j] / pow(2.0, (double)j/pDim); 
              }

            for(j = 0; j < pDim; j++) max[j] = (max[j] + min[j]) / 2.0;
            //saturation->m->eval(saturation, max, &sat);  pressure->m->eval(pressure, max, &press);

            /* Calculates <sat> and <press> using <intersec>. */
            sat = 0.0; press = 0.0;
            int trow = Hev.ents.el[i].row, tcol = Hev.ents.el[i].col;
            double tval; bool newt;
            SOLinCombFunction *lcsat = SOLinCombFunction_Cast(saturation);
            SOLinCombFunction *lcpress = SOLinCombFunction_Cast(pressure);
            for(j = 0; j < intersec[trow].nel; j++)
              {
                bas.el[intersec[trow].el[j]]->m->eval(bas.el[intersec[trow].el[j]], max, &tval);
		sat += (double)tval * lcsat->d->coef.el[intersec[trow].el[j]];
		press += (double)tval * lcpress->d->coef.el[intersec[trow].el[j]];
              }
            for(j = 0; j < intersec[tcol].nel; j++)
              {
                newt = TRUE;
                for(k = 0; k < intersec[trow].nel; k++)
                  if(intersec[tcol].el[j] == intersec[trow].el[k]) newt = FALSE;
                  
                if(newt)
                  {
                    bas.el[intersec[tcol].el[j]]->m->eval(bas.el[intersec[tcol].el[j]], max, &tval);
	            sat += (double)tval * lcsat->d->coef.el[intersec[tcol].el[j]];
                    press += (double)tval * lcpress->d->coef.el[intersec[tcol].el[j]];
                  }
              }


            phase = 0;
            Fform[phase]->map(&press, max, &(wformev[i][l]));
 
            phase = 1;
            Fform[phase]->map(&press, max, &(oformev[i][l]));

            if(Fpor->map == NULL)porosity->m->eval(porosity, max, &(por[i][l]));
            else Fpor->map(&press, max, &(por[i][l]));
          }
      }

    /* Computes the upper left G water corner. */
    for(i = 0; i < Hgr.ents.nel; i++)
      {      
        H->ents.el[H->ents.nel].va = 0.0;
        for(l = 0; l < Hgr.length; l++)
          H->ents.el[H->ents.nel].va += -(Hgr.ents.el[i].va[l] * abperm[i][l] * wrelperm[i][l]) / (wvisc[i][l] * wform[i][l]);

/*         for(l = 0; l < Hgr.length; l++) */
/*           {  */
/*             printf(" # DEBUG>> row: %d, col: %d, Hgr.va[%d]: %.16g \n", */
/*                    Hgr.ents.el[i].row,  Hgr.ents.el[i].col,  l, Hgr.ents.el[i].va[l] ); */
/*             printf(" # DEBUG>> abperm: %.16g, wrelperm: %.16g, wvisc: %.16g, wform: %.16g \n\n", */
/*                    abperm[i][l], wrelperm[i][l], wvisc[i][l], wform[i][l] ); */
/* 	  } */

        H->ents.el[H->ents.nel].va *= gradscale;
        H->ents.el[H->ents.nel].row = Hgr.ents.el[i].row;
        H->ents.el[H->ents.nel].col = Hgr.ents.el[i].col;
        H->ents.nel++;
      }

    /* Computes the upper right F water corner. */
    for(i = 0; i < Hev.ents.nel; i++)
      {
        H->ents.el[H->ents.nel].va = 0.0;
        for(l = 0; l < Hev.length; l++)
          H->ents.el[H->ents.nel].va += -(Hev.ents.el[i].va[l] * reserv_box * por[i][l]) / wformev[i][l];
        H->ents.el[H->ents.nel].row = Hev.ents.el[i].row;
        H->ents.el[H->ents.nel].col = (Hev.ents.el[i].col + dim);
        H->ents.nel++;
      }

    /* Computes the lower left G oil corner. */
    for(i = 0; i < Hgr.ents.nel; i++)
      { 
        H->ents.el[H->ents.nel].va = 0.0;
        for(l = 0; l < Hgr.length; l++)
          H->ents.el[H->ents.nel].va += -(Hgr.ents.el[i].va[l] * abperm[i][l] * orelperm[i][l]) / (ovisc[i][l] * oform[i][l]);
        H->ents.el[H->ents.nel].va *= gradscale;
        H->ents.el[H->ents.nel].row = (Hgr.ents.el[i].row + dim);
        H->ents.el[H->ents.nel].col = Hgr.ents.el[i].col;
        H->ents.nel++;
      }

    /* Computes the lower right F oil corner. */
    for(i = 0; i < Hev.ents.nel; i++)
      {           
        H->ents.el[H->ents.nel].va = 0.0;
        for(l = 0; l < Hev.length; l++)
          H->ents.el[H->ents.nel].va += (Hev.ents.el[i].va[l] * reserv_box * por[i][l]) / oformev[i][l];
        H->ents.el[H->ents.nel].row = (Hev.ents.el[i].row + dim);
        H->ents.el[H->ents.nel].col = (Hev.ents.el[i].col + dim);
        H->ents.nel++;
      }

  }


void SOApprox_GetBasisIndeces
  ( Basis bas,
    SOGrid_Dim pDim, 
    int numpos, 
    int *indices, 
    double *basispos
  )
  {
    int i, j, k, closer;
    double distance, cdistance, min[pDim], max[pDim], xdis;
    SOTentFunction *t;

    for(i = 0; i < numpos; i++)
      {
        closer = 0;
        t = SOTentFunction_Cast(bas.el[0]);
        SOGrid_cell_coords(t->d->index, t->d->rank, pDim, min, max);
        for(k = 0; k < pDim; k++)                                 /* Canonical geometry. */
          { min[k] = min[k] / pow(2.0, (double)k/pDim); 
            max[k] = max[k] / pow(2.0, (double)k/pDim); 
          }

        cdistance = 0;
        for(k = 0; k < pDim; k++)
          { xdis = max[k] - basispos[i * pDim + k];
            cdistance += xdis * xdis; 
          }
        cdistance = sqrt(cdistance);

        for(j = 1; j < bas.nel; j++)
          {
            t = SOTentFunction_Cast(bas.el[j]);
            SOGrid_cell_coords(t->d->index, t->d->rank, pDim, min, max);
            for(k = 0; k < pDim; k++)                                 /* Canonical geometry. */
              { min[k] = min[k] / pow(2.0, (double)k/pDim); 
                max[k] = max[k] / pow(2.0, (double)k/pDim); 
              }

            distance = 0;
            for(k = 0; k < pDim; k++)
              { xdis = max[k] - basispos[i * pDim + k];
                distance += xdis * xdis; 
              }
            distance = sqrt(distance);
            if(distance < cdistance){cdistance = distance; closer = j;}
          } 
        indices[i] = closer;
      }

  }

SOMatrix SOApprox_GetHelmholtzMatrix(char *matName, double c, bool cholesky)
  { SOMatrix EM = SOApprox_ReadMatrix(txtcat(matName, "-ev.mat"));
    SOMatrix GM = SOApprox_ReadMatrix(txtcat(matName, "-gr.mat"));
    SOMatrix H;
    fprintf(stderr, "computing the differential operator matrix...\n");
    H = SOMatrix_Mix(1.0, GM, c, EM);
    if (cholesky)
      { SOMatrix HL;
        fprintf(stderr, "computing the Cholesky factorization...\n");
        HL = SOMatrix_Cholesky(H, 0.0); 
        free(H.ents.el);
        return HL;
      }
    else
      { return H; }
  }

void SOApprox_ComputeRightHandSide
  ( SOFunction *f,      /* Current solution. */
    FuncMap *FMap,      /* Right-hand side of differential eqn. */
    Basis bas,          /* Basis for the approximation space. */
    SOFunction *w,      /* Weight for integrals. */
    SOGrid_Tree *tree,  /* Grid to use for integration. */
    double_vec y,       /* IN/OUT: right-hand-side vector. */ 
    bool verbose        /* TRUE mumbles and prints the change in {y}. */
  )
  { nat pDim = bas.el[0]->d->pDim;
    nat fDim = bas.el[0]->d->fDim;
    FuncMap NoMap = IdentityMap(fDim, pDim);

    auto void fDot(SOFunction *h, SOFunction *bi, double *iv);

    void fDot(SOFunction *h, SOFunction *bi, double *iv)
      { SOFunction_Dot(h, FMap, bi, &NoMap, w, tree, iv, FALSE); }
    
    SOBasisMatrices_RecomputeVectorGen(f, bas, fDot, y, verbose);
  }

void SOApprox_GaussElim
  ( double *TotH,   /* (WORK) temporary storage area. */
    SOMatrix H,     /* Left side of the system. */
    double_vec b,   /* Right side of the system. */
    double_vec x,   /* (OUT) Coordinates of new solution relative to {bas}. */
    bool verbose
  )
  { 
    int i, j, rows = H.rows, cols = H.cols;

    for(i = 0; i < (rows * (cols+1)); i++) TotH[i] = 0.0;

    for(i = 0; i < H.ents.nel; i++) 
      TotH[H.ents.el[i].row * (cols+1) + H.ents.el[i].col] = H.ents.el[i].va;

    for(i = 0; i < rows; i++) TotH[i * (cols+1) + cols] = b.el[i];

    if(verbose)
      { printf("\n DBG-> ROWS: %d, COLS: %d \n\n", H.rows, H.cols);
        printf("\n\n\n DBG-> Normal: \n");
        for(i = 0; i < rows; i++)
        { for(j = 0; j < cols + 1; j++)
            if(TotH[i*(cols+1) + j] == 0.0) printf("0.00000  ");
            else
              printf("%.1e  ", TotH[i*(cols+1) + j]);
          printf("\n\n");
        }
      }

    gsel_triangularize(rows, cols+1, TotH, 1);
    if(verbose)
      { printf("\n\n\n DBG-> Triangularization: \n");
        for(i = 0; i < rows; i++)
        { for(j = 0; j < cols + 1; j++)
            if(TotH[i*(cols+1) + j] == 0.0) printf("0.00000  ");
            else
              printf("%.1e  ", TotH[i*(cols+1) + j]);
          printf("\n\n");
        }
      }

    gsel_diagonalize(rows, cols+1, TotH, 1);
    if(verbose)
      { printf("\n\n\n DBG-> Diagonalization: \n");
        for(i = 0; i < rows; i++)
        { for(j = 0; j < cols + 1; j++)
            if(TotH[i*(cols+1) + j] == 0.0) printf("0.00000  ");
            else
              printf("%.1e  ", TotH[i*(cols+1) + j]);
          printf("\n\n");
        }
      }

    gsel_normalize(rows, cols+1, TotH, 1);
    if(verbose)
      { printf("\n\n\n DBG-> Normalization: \n");
        for(i = 0; i < rows; i++)
        { for(j = 0; j < cols + 1; j++)
            if(TotH[i*(cols+1) + j] == 0.0) printf("0.00000  ");
            else
              printf("%.1e  ", TotH[i*(cols+1) + j]);
          printf("\n\n");
        }
      }

    for(i = 0; i < rows; i++) x.el[i] = TotH[i * (cols+1) + cols];

  }

void SOApprox_ListGaussElim
  ( double *TotH,       /* (WORK) temporary storage area. */
    SOListMatrix H,     /* Left side of the system. */
    double_vec b,       /* Right side of the system. */
    double_vec x,       /* (OUT) Coordinates of new solution relative to {bas}. */
    bool verbose
  )
  { 
    int i, j, l, rows = H.rows, cols = H.cols, length = H.length;

    for(i = 0; i < rows; i++)
      for(j = 0; j < cols; j++) TotH[i*(cols+1) + j] = 0.0;

    for(i = 0; i < H.ents.nel; i++) 
      for(l = 0; l < length; l++)
        TotH[H.ents.el[i].row * (cols+1) + H.ents.el[i].col] += H.ents.el[i].va[l];

    for(i = 0; i < rows; i++) TotH[i * (cols+1) + cols] = b.el[i];

    if(verbose)
      { printf("\n DBG-> ROWS: %d, COLS: %d \n\n", H.rows, H.cols);
        printf("\n\n\n DBG-> Normal: \n");
        for(i = 0; i < rows; i++)
        { for(j = 0; j < cols + 1; j++)
            if(TotH[i*(cols+1) + j] == 0.0) printf("0.00000  ");
            else
              printf("%.1e  ", TotH[i*(cols+1) + j]);
          printf("\n\n");
        }
      }

    gsel_triangularize(rows, cols+1, TotH, 1);
    if(verbose)
      { printf("\n\n\n DBG-> Triangularization: \n");
        for(i = 0; i < rows; i++)
        { for(j = 0; j < cols + 1; j++)
            if(TotH[i*(cols+1) + j] == 0.0) printf("0.00000  ");
            else
              printf("%.1e  ", TotH[i*(cols+1) + j]);
          printf("\n\n");
        }
      }

    gsel_diagonalize(rows, cols+1, TotH, 1);
    if(verbose)
      { printf("\n\n\n DBG-> Diagonalization: \n");
        for(i = 0; i < rows; i++)
        { for(j = 0; j < cols + 1; j++)
            if(TotH[i*(cols+1) + j] == 0.0) printf("0.00000  ");
            else
              printf("%.1e  ", TotH[i*(cols+1) + j]);
          printf("\n\n");
        }
      }

    gsel_normalize(rows, cols+1, TotH, 1);
    if(verbose)
      { printf("\n\n\n DBG-> Normalization: \n");
        for(i = 0; i < rows; i++)
        { for(j = 0; j < cols + 1; j++)
            if(TotH[i*(cols+1) + j] == 0.0) printf("0.00000  ");
            else
              printf("%.1e  ", TotH[i*(cols+1) + j]);
          printf("\n\n");
        }
      }

    for(i = 0; i < rows; i++) x.el[i] = TotH[i * (cols+1) + cols];

  }

void SOApprox_CholeskySolve
  ( SOMatrix L,    /* Cholesky factor of system's matrix. */          
    int n,          /* Number of columns in soln and rhs matrices. */
    double_vec y,   /* Right-hand-side of system. */ 
    double_vec x,   /* (OUT) Coordinates of new solution relative to {bas}. */
    double_vec r,   /* (WORK) temporary storage area. */
    double_vec s,   /* (WORK) temporary storage area. */
    double_vec t,   /* (WORK) temporary storage area. */
    bool verbose    /* TRUE mumbles along the way. */
  )
  { int i, j;
    if (verbose) { fprintf(stderr, "SOApprox_CholeskySolve\n"); }
    for (j = 0; j < n; j++)
      { for (i = 0; i < L.cols; i++) { r.el[i] = y.el[i*n + j]; } 
        SOMatrix_DivCol(L, r, t);
        SOMatrix_DivRow(t, L, s);
        for (i = 0; i < L.cols; i++) { x.el[i*n + j] = s.el[i]; }
      }
  }

void SOApprox_GaussSeidelIteration(
    SOMatrix A,      /* System's matrix. */          
    int n,            /* Number of columns in soln and rhs matrices. */
    double_vec y,     /* Right-hand-side of system. */ 
    double omega,     /* Overrelaxation factor. */
    double_vec xOld,  /* (IN) Current guess at the solution. */
    double_vec xNew,  /* (OUT) New solution. */
    double_vec r,     /* (WORK) temporary storage area. */
    double_vec s,     /* (WORK) temporary storage area. */
    bool verbose      /* TRUE mumbles along the way. */
  )
  { int i, j;
    if (verbose) { fprintf(stderr, "SOApprox_GaussSeidelIteration\n"); }
    for (i = 0; i < xNew.nel; i++) { xNew.el[i] = xOld.el[i]; }
    for (j = 0; j < n; j++)
      { for (i = 0; i < A.cols; i++) 
          { r.el[i] = xNew.el[i*n + j]; 
            s.el[i] = y.el[i*n + j]; 
          } 
        SOMatrix_GaussSeidel(A, s, omega, r);
        for (i = 0; i < A.cols; i++) 
          { xNew.el[i*n + j] = r.el[i]; } 
      }
  }
  
void SOApprox_GaussSeidelSolve
  ( SOMatrix A,     /* System's matrix. */          
    int n,            /* Number of columns in soln and rhs matrices. */
    double_vec y,     /* Right-hand-side of system. */ 
    double omega,     /* Relaxation factor. */
    nat maxIter,      /* Maximum number of iterations. */
    double absTol,    /* Stopping criterion: absolute change in solution. */
    double relTol,    /* Stopping criterion: relative change in solution. */
    double_vec xOld,  /* (IN) Current solution. */
    double_vec xNew,  /* (OUT) New solution. */
    double_vec r,     /* (WORK) temporary storage area. */
    double_vec s,     /* (WORK) temporary storage area. */
    bool verbose      /* TRUE mumbles along the way. */
  )
  { nat N = xOld.nel;
    int iter = 0;
    double d, m;
    double_vec x = double_vec_new(N);
    int i, j;
    if (verbose) { fprintf(stderr, "SOApprox_GaussSeidelSolve\n"); }
    for (i = 0; i < N; i++) { x.el[i] = xOld.el[i]; }
    while (iter < maxIter)
      { for (i = 0; i < N; i++) { xNew.el[i] = x.el[i]; }
        for (j = 0; j < n; j++)
          { for (i = 0; i < A.cols; i++) 
              { r.el[i] = xNew.el[i*n + j]; 
                s.el[i] = y.el[i*n + j]; 
              } 
            SOMatrix_GaussSeidel(A, s, omega, r);
            for (i = 0; i < A.cols; i++) 
              { xNew.el[i*n + j] = r.el[i]; } 
          }
        iter++;
        d = SOApprox_UpdateSolution(xNew, x);
        m = rn_norm(N, x.el);
        if ((d <= absTol) || (d <= relTol*m)) { break; }
      }
    free(x.el);
  }

void SOApprox_ConjugateGradientSolve
  ( SOMatrix A,     /* System's matrix. */          
    int n,            /* Number of columns in soln and rhs matrices. */
    double_vec y,     /* Right-hand-side of system. */ 
    double_vec xOld,  /* (IN) Current solution. */
    double_vec xNew,  /* (OUT) New solution. */
    double_vec r,     /* (WORK) temporary storage area. */
    double_vec s,     /* (WORK) temporary storage area. */
    bool verbose      /* TRUE mumbles along the way. */
  )
  { double rsq;
    int i, j;
    if (verbose) { fprintf(stderr, "SOApprox_ConjugateGradientSolve\n"); }
    for (i = 0; i < xNew.nel; i++) { xNew.el[i] = xOld.el[i]; }
    rsq = 0;
    for (j = 0; j < n; j++)
      { for (i = 0; i < A.cols; i++) 
          { r.el[i] = xNew.el[i*n + j]; 
            s.el[i] = y.el[i*n + j]; 
          } 
        rsq += SOMatrix_ConjugateGradient(A, s, r);
        for (i = 0; i < A.cols; i++) 
          { xNew.el[i*n + j] = r.el[i]; } 
      }
    if (verbose) { fprintf(stderr, "  residual norm = %16.12f\n", rsq); }
  }
  
double SOApprox_UpdateSolution
  ( double_vec xNew, /* (IN) New solution vector. */
    double_vec x     /* (I/O)Current solution vector. */
  )
  { int i;
    double d = rn_dist(x.nel, x.el, xNew.el);
    fprintf(stderr, "change = %.12g\n", d);
    for (i = 0; i < xNew.nel; i++) { x.el[i] = xNew.el[i]; }
    x = xNew;
    return d;
  }

void SOApprox_GuessSol(double_vec x)
  { int i;
    for (i = 0; i < x.nel; i++){ x.el[i] = 0.0; }
    x.el[0] = 1.0;
  }
  
SOFunction *SOApprox_BuildFunction
  ( Basis bas,      /* Basis for space {V[r]}. */
    double_vec u,     /* Coordinates of {g} relative to  {bas} */
    char *solName,    /* Name prefix for output file. */
    char *basName,    /* Name of the (Basis bas) description file */
    SOFunction *f     /* True solution (for comparisons) */
  )
  { SOLinCombFunction *g = 
      SOLinCombFunction_Make(f->d->pDim, f->d->fDim, basName, bas, u);

    FILE *wr = open_write(txtcat(solName, "-app.fun"));
    g->m->fn.write(g, wr);  /* Writes with SOFunction encapsulation */
    fclose(wr);

/*     SOApprox_PrintSomeValues((SOFunction *)g, f); */

    return (SOFunction *)g;
  }

SOFunction *SOApprox_BuildIterFunction
  ( Basis bas,        /* Basis for space {V[r]}. */
    double_vec u,     /* Coordinates of {g} relative to  {bas} */
    char *basName,    /* Name of the (Basis bas) description file */
    SOGrid_Dim pDim,  /* Domain dimension of the new function */
    SOGrid_Dim fDim   /* Range dimension of the new function */
  )
  { SOLinCombFunction *g = 
      SOLinCombFunction_Make(pDim, fDim, basName, bas, u);

    return (SOFunction *)g;
  }
  
void SOApprox_PrintSomeValues(SOFunction *g, SOFunction *f)
  { int m = f->d->pDim;
    int n = f->d->fDim;
    int i, j, k;
    double x[m], fx[n], gx[n];
    int N = 20;                // Sample poins per axis.
    int NP = ipow(N, m);       // Total number of sample points. 
    
    assert(g->d->pDim == m, "incompatible domain dimensions");
    assert(g->d->fDim == n, "incompatible range dimensions");
    
    for (i = 0; i < NP; i++)
      { // Generate coordinates of sample point number {i}:
        int ti = i;
        for (k = m-1; k >= 0; k--)
          { x[k] = ((double)(ti % N))/((double)(N-1));
            ti /= N;
          }
 	
        fprintf(stderr, "x =       (");
        for (k = 0; k < m; k++) { fprintf(stderr, " %5.3f", x[k]); }
	fprintf(stderr, " )\n");
            
        // Evaluate
    
        g->m->eval(g, x, gx);
        fprintf(stderr, " fAppr(x) =      (");
        for (j = 0; j < n; j++) { fprintf(stderr, " %16.12f", gx[j]); }
	fprintf(stderr, " )\n");
            
        f->m->eval(f, x, fx);
        fprintf(stderr, " f(x) =          (");
        for (j = 0; j < n; j++) { fprintf(stderr, " %16.12f", fx[j]); }
	fprintf(stderr, " )\n");

        fprintf(stderr, " fAppr(x)-fCorr(x) = (");
        for (j = 0; j < n; j++)
          { double ej = gx[j] - fx[j];
            fprintf(stderr, " %16.12f", ej);
          }
        fprintf(stderr, " )\n");
      }
  }
  
void SOApprox_PrintMaxErrorValues
  ( SOFunction *g,  /* Approximation. */
    SOFunction *f,  /* Target function. */
    double *gMax,     /* OUT: max abs function value. */
    double *eMax      /* OUT: max abs error VALUE. */
  )
  {  
    int m = f->d->pDim;
    int n = f->d->fDim;
    int steps = 1, divs = 100, i, j, k;
    double point[m], mult[m];
    double gv[n], fv[n];
    bool greater;

    mult[0] = 1; for(i = 1; i < m; i++) mult[i] = mult[i - 1] * divs; 
    for(i = 0; i < m; i++){steps *= divs; point[i] = 0;}
    for(j = 0; j < n; j++){gMax[j] = 0; eMax[j] = 0;}

    for(k = 0; k < steps; k++) 
      {
        for(i = 0; i < m; i++) point[i] = (double)((int)(k / mult[i]) % divs)/divs;  
    
        g->m->eval(g, point, &(gv[0]));
        f->m->eval(f, point, &(fv[0]));
    
        greater = TRUE;
        for(j = 0; j < n; j++) if(gv[j] < gMax[j]) greater = FALSE;
        if(greater) for(j = 0; j < n; j++) gMax[j] = gv[j];

        greater = TRUE;
        for(j = 0; j < n; j++) if(fabs(gv[j]-fv[j]) < eMax[j]) greater = FALSE;
        if(greater) for(j = 0; j < n; j++) eMax[j] = fabs(gv[j]-fv[j]);
      }

    for(j = 0; j < n; j++)
      {
        fprintf(stderr, "max(fAppr)[%d] = %16.12f ", j, gMax[j]);
        fprintf(stderr, "max(fabs(fAppr-fCorr))[%d] = %16.12f \n", j, eMax[j]);
      }
  }

void SOApprox_GetLimitValues
  ( SOFunction *f,    /* General function. */
    double *maxP,     /* OUT: point with maximum value. */
    double *fMax,     /* OUT: max function values. */
    double *fMin      /* OUT: min function values. */
  )
  {  
    int m = f->d->pDim;
    int n = f->d->fDim;
    int steps = 1, divs = 100, i, j, k;
    double point[m], mult[m], fv[n];
    bool greater, smaller;

    mult[0] = 1; for(i = 1; i < m; i++) mult[i] = mult[i - 1] * divs; 
    for(i = 0; i < m; i++){steps *= divs; point[i] = 0.0; maxP[i] = 0.0;}
    for(j = 0; j < n; j++){fMax[j] = 0; fMin[j] = 10e+20;}

    for(k = 0; k < steps; k++) 
      {
        for(i = 0; i < m; i++) point[i] = (double)((int)(k / mult[i]) % divs)/divs;  
    
        f->m->eval(f, point, &(fv[0]));
    
        greater = TRUE;
        for(j = 0; j < n; j++) if(fv[j] < fMax[j]) greater = FALSE;
        if(greater) 
          { for(j = 0; j < n; j++) fMax[j] = fv[j];
            for(i = 0; i < m; i++) maxP[i] = point[i];
          }

        smaller = TRUE;
        for(j = 0; j < n; j++) if(fv[j] > fMin[j]) smaller = FALSE;
        if(smaller) for(j = 0; j < n; j++) fMin[j] = fv[j];
      }
  }
  
void SOApprox_PlotSingleFunction
  ( SOFunction *f,
    FuncMap *FMap, 
    SOGrid_Tree *tree,
    char *fName, 
    PlotOptions *plt,
    double *fObsMin,    /* (IN/OUT) Min value seen during plot. */ 
    double *fObsMax     /* (IN/OUT) Max value seen during plot. */ 
  )
  { 
    double hFigSize = plt->figSize;                /* In pt. */
    double vFigSize = plt->figSize;                /* In pt. */ 
    if(plt->eps) plt->paperSize = NULL;

    int meshDepth = (int)ceil(2*log(plt->figSize/plt->meshSize)/log(2));
    double minR[2], maxR[2];     /* Rectangle to plot. */
    minR[X] = 0.0;  maxR[X] = 1.0;
    minR[Y] = 0.0;  maxR[Y] = SQRTHALF; //1.0;

    /* Perhaps here we should fix the range according to {plt->autoRange}? */
    SO2DPlot_SingleFunction
      ( plt->eps, plt->paperSize, hFigSize, vFigSize,
        f, FMap, tree, minR, maxR, -plt->fRange, +plt->fRange, plt->fStep,
        plt->isolineWidth, plt->gridWidth, meshDepth,
        (! plt->noBands), (! plt->noIsolines), (! plt->noGrid), 
        fName, fObsMin, fObsMax
      );
  }