/*
 Authors
 Martin Schlather, schlather@math.uni-mannheim.de

 Simulation of a random field by Cholesky or SVD decomposition

 Copyright (C) 2001 -- 2014 Martin Schlather, 

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 3
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
*/

#include <math.h>
#include <stdio.h>  
#include <stdlib.h>
 
#include "RF.h"
#include "randomshape.h"
#include <R_ext/Lapack.h>
//#include <R_ext/Linpack.h>

bool debug=false;


#define DIRECT_METHOD (COMMON_GAUSS + 1)
#define DIRECT_SVDTOL (COMMON_GAUSS + 2)
#define DIRECT_MAXVAR (COMMON_GAUSS + 3)

int check_directGauss(cov_model *cov) {
#define nsel 4
  cov_model *next=cov->sub[0];
  location_type *loc = Loc(cov);
  int j, err ; // taken[MAX DIM],
  direct_param *gp  = &(GLOBAL.direct); //
  
  ROLE_ASSERT(ROLE_GAUSS);

  if ((err = check_common_gauss(cov)) != NOERROR) return err;

  kdefault(cov, DIRECT_METHOD, (int) gp->inversionmethod);
  kdefault(cov, DIRECT_SVDTOL, gp->svdtolerance);
  kdefault(cov, DIRECT_MAXVAR, gp->maxvariables);
  if ((err = checkkappas(cov)) != NOERROR) return err;
  if ((cov->tsdim != cov->xdimprev || cov->tsdim != cov->xdimown) &&
      (!loc->distances || cov->xdimprev!=1)) {
    return ERRORDIM;
  }

  Types type = PosDefType;

  for (j=0; j<=1; j++) {
    if ((err = CHECK(next, cov->tsdim,  cov->xdimprev, 
		     type, KERNEL, SYMMETRIC,
		     SUBMODEL_DEP, ROLE_COV)) == NOERROR) break;
    type = NegDefType;
  }
  if (err != NOERROR) return err;
  
  if (next->pref[Direct] == PREF_NONE) return ERRORPREFNONE;

  setbackward(cov, next);
  return NOERROR;
}




void range_direct(cov_model *cov, range_type *range) {
  range_common_gauss(cov, range);

  range->min[DIRECT_METHOD] = Cholesky;
  range->max[DIRECT_METHOD] = NoFurtherInversionMethod;
  range->pmin[DIRECT_METHOD] = Cholesky;
  range->pmax[DIRECT_METHOD] = NoFurtherInversionMethod;
  range->openmin[DIRECT_METHOD] = false;
  range->openmax[DIRECT_METHOD] = true; 

  range->min[DIRECT_SVDTOL] = 0;
  range->max[DIRECT_SVDTOL] = 1;
  range->pmin[DIRECT_SVDTOL] = 1e-17;
  range->pmax[DIRECT_SVDTOL] = 1e-8;
  range->openmin[DIRECT_SVDTOL] = false;
  range->openmax[DIRECT_SVDTOL] = true; 

  range->min[DIRECT_MAXVAR] = 0;
  range->max[DIRECT_MAXVAR] = 10000;
  range->pmin[DIRECT_MAXVAR] = 500;
  range->pmax[DIRECT_MAXVAR] = 5000;
  range->openmin[DIRECT_MAXVAR] = false;
  range->openmax[DIRECT_MAXVAR] = false; 
}


int init_directGauss(cov_model *cov, storage VARIABLE_IS_NOT_USED *S) {
  cov_model *next = cov->sub[0];
  double //*xx,
    svdtol = P0(DIRECT_SVDTOL), 
    *G=NULL, 
    *Cov=NULL, 
    *U=NULL, 
    *VT=NULL, 
    *work=NULL,
    *D=NULL, 
    *SICH=NULL;
  int err,
    maxvariab = P0INT(DIRECT_MAXVAR),
    *iwork=NULL;
  int dim=cov->tsdim;
  direct_storage* s=NULL;
  InversionMethod
    method = (InversionMethod) P0INT(DIRECT_METHOD);
  location_type *loc = Loc(cov);
  bool storing = GLOBAL.warn.stored_init; //
  // nonstat_covfct cf;
  long 
    vdim = cov->vdim,
    locpts = loc->totalpoints,
//    loctot = locpts *dim,
    vdimtot = vdim * locpts,
    //     vdimSqtot = vdim * vdimtot,
    // vdimtotSq = vdimtot * locpts,
    vdimSqtotSq = vdimtot * vdimtot;

  ROLE_ASSERT_GAUSS;

  cov->method = Direct;

  if ((err = alloc_cov(cov, dim, vdim, vdim)) != NOERROR) return err;

  if (cov->Sdirect != NULL) DIRECT_DELETE(&cov->Sdirect);
  if (vdimtot > maxvariab) {
    sprintf(ERRORSTRING_OK, 
	    "number of columns less than or equal to %d", maxvariab);
    sprintf(ERRORSTRING_WRONG,"%ld", vdimtot);
    err=ERRORCOVFAILED;
    goto ErrorHandling;
  }
   
  if (cov->Sdirect != NULL) free(cov->Sdirect);
  cov->Sdirect = NULL;
  if ((Cov =(double *) MALLOC(sizeof(double) * vdimSqtotSq))==NULL ||
      (U =(double *) MALLOC(sizeof(double) * vdimSqtotSq))==NULL ||
   //for SVD/Chol intermediate results AND  memory space for do_directGauss:
      (G = (double *)  CALLOC(vdimtot + 1, sizeof(double)))==NULL ||
      (cov->Sdirect = (direct_storage*) MALLOC(sizeof(direct_storage)))==NULL) {
    err=ERRORMEMORYALLOCATION;  
    goto ErrorHandling;
  }  
  s = cov->Sdirect;
  DIRECT_NULL(s);
  s->U = s->G = NULL;


  /* ********************* */
  /* matrix creation part  */
  /* ********************* */

  CovarianceMatrix(next, Cov);
  assert(R_FINITE(Cov[0]));
 
  if (false) {
    int i,j;
    PRINTF("\n");
    for (i=0; i<locpts * vdim; i++) {
       for (j=0; j<locpts * vdim; j++) {
	 PRINTF("%+2.3f ", Cov[i  + locpts * vdim * j]);
       }
       PRINTF("\n");
    }
    assert(false);
  }
   
  
  if (!isPosDef(next)) {
    if (isNegDef(next)) {
      int i, j, v;
      double min,
	*C = Cov;
      min = RF_INF;
      for (i=0; i< vdimSqtotSq; i++) if (Cov[i] < min) min=Cov[i];
      //       print("Cov %f\n", min);
      // Die Werte der Diagonalbloecke werden erh\"oht:
      for (C=Cov, v=0; v<vdim; v++, C += locpts) { 
	for (i=0; i<locpts; i++, C+=vdimtot) {
	  for (j=0; j<locpts; C[j++] -= min);
	}
      }
    } else {
      //   APMI(next);
      err = ERRORNOVARIOGRAM; 
      goto ErrorHandling;
    }
  }

  if (false) {
    int i,j;
    PRINTF("\n");
    for (i=0; i<locpts * vdim; i++) {
       for (j=0; j<locpts * vdim; j++) {
	 PRINTF("%+2.2f ", Cov[i  + locpts * vdim * j]);
       }
       PRINTF("\n");
    }
    // assert(false);
  }
   
   
  /* ********************** */
  /*  square root of matrix */
  /* ********************** */
  int row, Err,k;
  switch (method) {
  case Cholesky : 
    // only works for strictly positive def. matrices
    if (PL>=PL_STRUCTURE) { LPRINT("method to the root=Cholesky\n"); }
    row=vdimtot;
    // dchdc destroys the input matrix; upper half of U contains result!
    MEMCOPY(U, Cov, sizeof(double) * vdimSqtotSq);
    if (debug) {
      err = ERRORDECOMPOSITION; goto ErrorHandling;
    }   
    F77_CALL(dpotrf)("Upper", &row, U, &row, &Err);  

    if (false)  {
      double *sq  = (double *) MALLOC(sizeof(double) * vdimtot * vdimtot);
      AtA(U, vdimtot, vdimtot, sq);
      
      int i,j;
      PRINTF("AtA \n");
      for (i=0; i<locpts * vdim; i++) {
	for (j=0; j<locpts * vdim; j++) {
	  PRINTF("%+2.2f ", Cov[i  + locpts * vdim * j]);
	}
	PRINTF("\n");
      }
      //  assert(false);
    }
 
    
    // F77_NAME(dchdc)(U, &row, &row, G, NULL, &choljob, &err);
    if (Err!=NOERROR) {
      if (PL>=PL_SUBIMPORTANT) { 
	LPRINT("Error code F77_CALL(dpotrf) = %d\n", Err); }
      err=ERRORDECOMPOSITION;
    } else break;
    // try next method : 
    // most common error: singular matrix 
       
    if (svdtol <= 0.0) break;
    
   case SVD :  // works for any positive semi-definite matrix
     double sum;
     method = SVD; // necessary if the value of method has been Cholesky.
     //               originally
     if (vdimtot>maxvariab * 0.8) {
       sprintf(ERRORSTRING_OK, 
	       "number of points less than 0.8 * RFparameters()$direct.maxvariables (%d) for SVD",
	       maxvariab);
       sprintf(ERRORSTRING_WRONG,"%ld", vdimtot);
       err=ERRORCOVFAILED; goto ErrorHandling;
     }
     if (PL>=PL_STRUCTURE) { LPRINT("method to the root=SVD\n"); }
     if ((VT =(double *) MALLOC(sizeof(double) * vdimSqtotSq))==NULL ||
	 (D =(double *) MALLOC(sizeof(double) * vdimtot))==NULL ||
	 (iwork = (int *) MALLOC(sizeof(int) * 8 * vdimtot))==NULL ||
	 (SICH =(double *) MALLOC(sizeof(double) * vdimSqtotSq))==NULL) {
       err=ERRORMEMORYALLOCATION;
       goto ErrorHandling;
     }
     MEMCOPY(SICH, Cov, sizeof(double) * vdimSqtotSq);
     row=vdimtot;
     // dsvdc destroys the input matrix !!!!!!!!!!!!!!!!!!!!
     
     // DGESDD (or DGESVD)
     // dgesdd destroys the input matrix Cov;
     // F77_NAME(dsvdc)(Cov, &row, &row, &row, D, e, U, &row, V, &row, G,
     //		&jobint /* 11 */, &err);
     double optim_lwork;
     int lwork;
     lwork = -1;
     F77_CALL(dgesdd)("A", &row, &row, SICH, &row, D, U, &row, VT, &row, 
		      &optim_lwork, &lwork, iwork, &Err);
     if ((err=Err) != NOERROR) {
       err=ERRORDECOMPOSITION;
       goto ErrorHandling;
     }
     lwork = (int) optim_lwork;
     if ((work = (double *) MALLOC(sizeof(double) * lwork))==NULL)
       goto ErrorHandling;
     F77_CALL(dgesdd)("A",  &row,  &row, SICH, &row, D, U, &row, VT, &row, 
		      work, &lwork, iwork, &Err);
     
     err = Err;
     if (err==NOERROR && ISNA(D[0])) err=9999;
     if (err!=NOERROR) {
       if (PL>PL_ERRORS) { 
	 LPRINT("Error code F77_CALL(dgesdd) = %d\n", err); 
	   }
       err=ERRORDECOMPOSITION;
       goto ErrorHandling;
     }

     int i,j;
	 /* calculate SQRT of covariance matrix */
     for (k=0,j=0;j<vdimtot;j++) {
       double dummy;
       //printf("diag=%d %f\n", j, D[j]);
       dummy = sqrt(D[j]);
       for (i=0;i<vdimtot;i++) {
	     U[k++] *= dummy;
       }
     }
     
     /* check SVD */
     if (svdtol >=0) {
       for (i=0; i<vdimtot; i++) {
	 for (k=i; k<vdimtot; k++) {
	   sum = 0.0;
	   for (j=0; j<vdimSqtotSq; j+=vdimtot) sum += U[i+j] * U[k+j];
	   
	   if (fabs(Cov[i * vdimtot + k] - sum) > svdtol) {
	     if (PL > PL_ERRORS) {
	       LPRINT("difference %e at (%d,%d) between the value (%e) of the covariance matrix and the square of its root (%e).\n", 
		      Cov[i * vdimtot +k] - sum, i, k, Cov[i* vdimtot +k ], 
		      sum);
	     }
	     GERR("required precision not attained: probably invalid model.\nSee also parameter 'svdtolerance'.");
	     goto ErrorHandling;
	   }
	 }
       }
     }
     break;
   default : assert(false);
   } // switch

 
  err = FieldReturn(cov);

  ErrorHandling: // and NOERROR...

   if (s != NULL) s->method = method;
   if (!storing && err!=NOERROR) {
     if (U!=NULL) free(U);
     if (G!=NULL) free(G); 
     U = G = NULL;
   } else {
    if (s != NULL) {
      s->U=U;
      s->G=G;
    }
  }
  if (SICH!=NULL) free(SICH);
  SICH = NULL;
  if (Cov!=NULL) free(Cov);
  if (D!=NULL) free(D);
  if (work!=NULL) free(work);
  if (iwork!=NULL) free(iwork);
  if (VT!=NULL) free(VT);

  return err;
}

void do_directGauss(cov_model *cov, storage VARIABLE_IS_NOT_USED *S) {  
  location_type *loc = Loc(cov);
  direct_storage *s = cov->Sdirect;
  long i, j, k,
    locpts = loc->totalpoints,
    vdim = cov->vdim,
    vdimtot = locpts * vdim;
  double dummy,
    *G = NULL,
    *U = NULL,
    *res = cov->rf;  
  int m, n;
  bool loggauss = (bool) P0INT(LOG_GAUSS),
    vdim_close_together = GLOBAL.general.vdim_close_together;

  // APMI(cov);

//  print("vdim %d %d\n", meth->cov->vdim, loc->totalpoints);

  U = s->U;// S^{1/2}
  G = s->G;// only the memory space is of interest (stored to avoid 
  //          allocation errors here)
  for (i=0; i<vdimtot; i++) {
    G[i] = GAUSS_RANDOM(1.0);
    //printf("%d %f\n", i, G[i]);
  }
  
  switch (s->method) {
  case Cholesky :
    if (vdim_close_together) {
      for (k=0, i=0; i<vdimtot; i++, k+=vdimtot) {
	double *Uk = U + k; 
	dummy =0.0;
	for (j=0; j<=i; j++){
	  dummy += G[j] * Uk[j];
	}
	res[i] = (res_type) dummy; 
      }
    } else {
      //printf("vdim %d vdimtot %d\n", vdim, vdimtot);
      for (k=i=m=0; m<vdim; m++) {
	for (n=m; n<vdimtot; n+=vdim, i++, k+=vdimtot) {
	  double *Uk = U + k;
	  dummy = 0.0;
	  for (j=0; j<=i; j++){
	    dummy += G[j] * Uk[j];
	  }
	  res[n] = (res_type) dummy; 
	  //	printf("%d %f\n", n, res[n]);
	}
      }
    }
    //{int i; for(i=0;i<18;i++) printf("%d:%f \n", i,res[i]); printf("\n");APMI(cov)}
    break;
  case SVD :
    if (vdim_close_together) {
      for (i=0; i<vdimtot; i++){
	dummy = 0.0;
      for (j=0, k=i; j<vdimtot; j++, k+=vdimtot){
	dummy += U[k] * G[j];
      }
      res[i] = (res_type) dummy; 
      }
    } else {
      for (i=m=0; m<vdim; m++) {
	for (n=m; n<vdimtot; n+=vdim, i++) {
	  dummy = 0.0;
	  for (j=0, k=i; j<vdimtot; j++, k+=vdimtot){
	    dummy += U[k] * G[j];
	  }
	  res[n] = (res_type) dummy; 
	}
      }
    } 
    break;
  default : assert(false);
  }

  if (loggauss) {
    for (i=0; i<vdimtot; i++) res[i] = exp(res[i]);
  }

}