/*
        Version 1.0

Correlation service; initial version will accept text input (optionally gzipped)
where comments and any integer (ie containing no ".") in initial columns are
skipped:

[# comments]
[int int int ] fl.oat fl.oat ...

([] means optional.)

Each row is a node, each column a TR, separated by whitespace. This is the
.1D surface fmri data format created by NIH AFNI/SUMA's 3dVol2Surf, which
interpolates voxel rows data onto each node (vertex) in a MGH FreeSurfer
surface. But the rows could be any rows data; no spatial information is
used (nor is it supplied by any recognized input arguments).

Normalizes each row by default. Saves correlationss as platform single-precision
floats with .1D.gz output an option. Uses Cblas API so link with the Cblas
implementation library of choice for your platform (Atlas, Gotoblas, MKL,
vecLib, cuBLAS, OpenCL, etc). Tested with GotoBlas2 on 64-bit Linux and vecLib
on 64-bit Mac OS X

Most CBLAS implementations are multicore, so the parallelism (within-machine)
should come from that.

Ben Singer <bdsinger@princeton.edu> April 2013, revised October 2014

*/

#define _GNU_SOURCE
#include <libgen.h>
#include <assert.h>
#include <string.h>
#include <limits.h>
#include <stdlib.h>

#ifdef __APPLE__
#include <malloc/malloc.h>
#include <Accelerate/Accelerate.h>
#else
#include <cblas.h>
#endif

#include "pnicorr.h"

// ******* Memory limits
#define BYTES_PER_GB (1000000000LL)
#define BYTES_PER_MB (1000000LL)
#define MAX_FILENAME 256
#define CBLAS_ALPHA (1.0F)
#define CBLAS_BETA (0.0F)
#define PNICORR_ALIGNMENT 64
enum { PNICORR_CORRELATIONS_TAG = 1, PNICORR_NUMTAGS };

#define PNICORR_PROC_BODY_PARAMS                                               \
  (const float *restrict full_matrix, float *restrict submatrix,               \
   float *restrict correlations, const int *restrict offset_for_task,          \
   const int *restrict rows_for_task, const int nt, const int num_rows,        \
   const int num_columns, const char *restrict outfile,                        \
   const pnicorr_iotype_t outtype, const int my_task_id)
#define PNICORR_PROC_BODY_ARGS                                                 \
  (full_matrix, submatrix, correlations, offset_for_task, rows_for_task, nt,   \
   num_rows, num_columns, outfile, outtype, my_task_id)
#define PNICORR_PARAMSET_PARAMS                                                \
  (const int my_task_id, const int *num_rows, const int *num_columns,          \
   long long *ntasks, int *num_workers, float **restrict full_matrix)
#define PNICORR_PARAMSET_ARGS                                                  \
  (my_task_id, &num_rows, &num_columns, &ntasks, &num_workers, &full_matrix)
#define PNICORR_TASK_PROCESSING_PARAMS                                         \
  (const float *restrict full_matrix, float *restrict submatrix,               \
   float *restrict correlations, const int offset_for_task,                    \
   const int rows_for_task, const int num_rows, const int num_columns)
#define PNICORR_TASK_PROCESSING_ARGS(id)                                       \
  (full_matrix, submatrix, correlations, offset_for_task[id],                  \
   rows_for_task[id], num_rows, num_columns)

#define COR_TASK_PROCESSING(id)                                                \
  pnicorr_task_processing PNICORR_TASK_PROCESSING_ARGS(id)

#ifdef HAVE_MPI
/* use MPI */
#include <mpi.h>

void pnicorr_mpi_init(int *restrict my_task_id, int *restrict num_workers);
void pnicorr_mpi_finalize(void);
void pnicorr_mpi_paramset PNICORR_PARAMSET_PARAMS;
void pnicorr_mpi_proc_body PNICORR_PROC_BODY_PARAMS;

#define COR_MPI_INIT(my_task_id, num_workers)                                  \
  pnicorr_mpi_init(&my_task_id, &num_workers)

#define COR_MPI_FINALIZE pnicorr_mpi_finalize()

#define COR_MPI_PARAMSET pnicorr_mpi_paramset PNICORR_PARAMSET_ARGS

#define COR_TASK_PROC_BODY pnicorr_mpi_proc_body PNICORR_PROC_BODY_ARGS

#else
/* no MPI */

#define NOP (void)0
#define COR_MPI_INIT(my_task_id, num_workers) NOP
#define COR_MPI_FINALIZE NOP
#define COR_MPI_PARAMSET NOP
#define COR_TASK_PROC_BODY pnicorr_proc_body PNICORR_PROC_BODY_ARGS

#endif

// ******* Helper functions

// ****************** normalize_fvec ***************
static void normalize_fvec(float *restrict full_matrix, const int num_rows,
                           const int N) {
  const int incX = 1;
  float *dptr = full_matrix;
  for (int i = 0; i < num_rows; ++i) {
    float norm2 = cblas_snrm2(N, dptr, incX);
    const float alpha = 1.0 / norm2;
    cblas_sscal(N, alpha, dptr, incX);
    dptr += N;
  }
}

void pnicorr_task_processing PNICORR_TASK_PROCESSING_PARAMS;

void pnicorr_proc_body PNICORR_PROC_BODY_PARAMS;

void pnicorr_savematrix_for_task(const int i, const int *restrict rows_for_task,
                                 const int num_rows, const char *outfile,
                                 const pnicorr_iotype_t outtype,
                                 const float *restrict correlations);

// ****************** MAIN ***************
int main(const int argc, const char *argv[]) {
  float *full_matrix = NULL;
  float *correlations = NULL;
  char outfile[MAX_FILENAME];
  int num_rows = 0, num_columns = 0;
  int my_task_id = 0;
  long long ntasks = 1;
  int num_workers = 1;
  pnicorr_iotype_t outtype = pnicorr_iotype_numiotypes;

  COR_MPI_INIT(my_task_id, num_workers);

  struct opts2struct_t *ops2s = opts2struct_create();

  if (0 == my_task_id) {

    // ***** parse input args ******
    const char *ext;
    const char *comp;
    const int pre_args = 2;
    char *basec = strdup(argv[0]);
    char *bname = basename(basec);

    if (argc < pre_args) {
      fprintf(stderr, "\nusage: %s file.1D.dset[.gz] -[no]norm -mem=MB "
                      "-iotype=1D|1Dgz|mat\n",
              bname);
      exit(EXIT_FAILURE);
    }
    const char *filename = argv[1];

    opts2struct_parseopts(ops2s, argc, argv);

    int normalize = ops2s->i[norm];
    long long maxmem = ops2s->i[mem];

    if (!strncmp(ops2s->iotype, "1D", 2)) {
      outtype = pnicorr_iotype_1D;
      ext = "1D.dset";
      comp = "";
    } else if (!strncmp(ops2s->iotype, "1Dgz", 4)) {
      outtype = pnicorr_iotype_1Dgz;
      ext = "1D.dset";
      comp = ".gz";
    } else if (!strncmp(ops2s->iotype, "mat", 3)) {
      outtype = pnicorr_iotype_mat;
      ext = "mat";
      comp = "";
    } else {
      ext = "";
      comp = "";
      ERRIF(1, "unrecognized iotype %s", ops2s->iotype);
    }

    //  **********  get full_matrix from file ************
    full_matrix = pnicorr_load_1D(filename, &num_rows, &num_columns);

    // get outfile name
    pnicorr_genoutfile(bname, num_rows, ext, comp, outfile);
    free(basec);

    //  **********  Normalize -- unless asked not to via "-nonorm" ************
    if (normalize) {
      LOG("normalizing each row/rows independently ...\n");
      TIC;
      normalize_fvec(full_matrix, num_rows, num_columns);
      TOC("normalize");
    }

    // ************  do the matrix multiplication ***************
    // (results in correlation after normalization)
    long long corbytes =
        (long long)num_rows * (long long)num_rows * (long long)sizeof(float);
    // must split up the task s.t. each task uses MAX_MEM_PER_TASK bytes
    ntasks = (long long)((float)corbytes /
                             (float)((long long)maxmem * BYTES_PER_MB) +
                         0.5);
    LOG("%lld task(s), since %d^2 floats is %.3f MB and the max MB per "
        "task is %lld MB\n",
        ntasks, num_rows, (float)corbytes / (float)BYTES_PER_MB, maxmem);

    if (ntasks <= 1) {
      // get time info immediately
      setbuf(stdout, NULL);

      // *********   all in one go; enough memory to do so *********
      LOG("number of bytes needed for correlations is small enough for a "
          "single "
          "task\n");
      LOG("allocating memory...\n");
      int err =
          posix_memalign((void *)&correlations, PNICORR_ALIGNMENT, corbytes);
      ERRIF(err, "posix_memalign");
      LOG("calling cblas_ssyrk to perform AA' ...\n");

      TIC;
      cblas_ssyrk(CblasRowMajor, CblasUpper, CblasNoTrans, num_rows,
                  num_columns, CBLAS_ALPHA, full_matrix, num_columns,
                  CBLAS_BETA, correlations, num_rows);
      TOC("calculation");

      // *** save ***
      LOG("saving %d x %d task correlations to %s\n", num_rows, num_rows,
          outfile);
      TIC;
      pnicorr_savematrix(correlations, num_rows, num_rows, "w", outtype,
                         outfile);
      TOC("saving");
      LOG("done\n");
    } // ntasks > 1
  }   // my_task_id == 0

  COR_MPI_PARAMSET;

  if (ntasks > 1) {
    float *submatrix = NULL;

    // ****************  need to do it in blocks and append correlations
    // *********
    int nt = (int)ntasks;
    int num_rows_per_task = num_rows / nt;
    if (num_rows_per_task < 1) {
      num_rows_per_task = 1;
      nt = num_rows;
    }

    int *rows_for_task = calloc(num_workers, sizeof(int));
    int *offset_for_task = calloc(num_workers, sizeof(int));
    // have task 0 (master in case of MPI) do the work for leftover rows
    // when total doesn't evenly divide ntasks
    rows_for_task[0] = num_rows - (num_rows_per_task * (nt - 1));
    offset_for_task[0] = 0;
    int maxrows_for_tasks = (rows_for_task[0] > num_rows_per_task)
                                ? rows_for_task[0]
                                : num_rows_per_task;
    for (int i = 1; i < nt; ++i) {
      offset_for_task[i] = offset_for_task[i - 1] + rows_for_task[i - 1];
      rows_for_task[i] = num_rows_per_task;
    }
    int err = posix_memalign((void *)&submatrix, PNICORR_ALIGNMENT,
                             maxrows_for_tasks * num_columns * sizeof(float));
    ERRIF(err, "posix_memalign submatrix");
    err = posix_memalign((void *)&correlations, PNICORR_ALIGNMENT,
                         maxrows_for_tasks * num_rows * sizeof(float));
    ERRIF(err, "posix_memalign correlations");

    COR_TASK_PROC_BODY;

    free(rows_for_task);
    free(offset_for_task);
    if (submatrix) {
      free(submatrix);
      submatrix = NULL;
    }
  }

  if (full_matrix) {
    free(full_matrix);
    full_matrix = NULL;
  }
  if (correlations) {
    free(correlations);
    correlations = NULL;
  }

  COR_MPI_FINALIZE;

  return EXIT_SUCCESS;
}

void pnicorr_task_processing PNICORR_TASK_PROCESSING_PARAMS {

  LOG("submatrix %p full_matrix %p\n", submatrix, full_matrix);

  memcpy(submatrix, full_matrix + offset_for_task,
         rows_for_task * num_columns * sizeof(float));
  memset(correlations, 0, num_rows * rows_for_task * sizeof(float));

  TIC;
  LOG("calling cblas_sgemm using %d num_rows (rows) at a time\n",
      rows_for_task);
  cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans, rows_for_task, num_rows,
              num_columns, CBLAS_ALPHA, submatrix, num_columns, full_matrix,
              num_columns, CBLAS_BETA, correlations, num_rows);
  TOC("calculating");
}

void pnicorr_savematrix_for_task(const int i, const int *restrict rows_for_task,
                                 const int num_rows, const char *outfile,
                                 const pnicorr_iotype_t outtype,
                                 const float *restrict correlations) {
  // ******** save/append *********
  LOG("task %d: %s %d x %d task correlations to %s\n", i,
      (i == 0) ? "writing" : "appending", rows_for_task[i], num_rows, outfile);
  TIC;
  pnicorr_savematrix(correlations, rows_for_task[i], num_rows,
                     (i == 0) ? "w" : "a", outtype, outfile);
  TOC("saving");
}

#ifdef HAVE_MPI

void pnicorr_mpi_init(int *my_task_id, int *num_workers) {
  int initialized;
  MPI_Initialized(&initialized);
  if (0 == initialized) {
    MPI_Init(NULL, NULL);
  }
  MPI_Comm_rank(MPI_COMM_WORLD, my_task_id);
  MPI_Comm_size(MPI_COMM_WORLD, num_workers);
}

void pnicorr_mpi_finalize(void) {
  int initialized;
  MPI_Initialized(&initialized);
  if (initialized) {
    int finalized;
    MPI_Finalized(&finalized);
    if (0 == finalized) {
      MPI_Finalize();
    }
  }
}

void pnicorr_mpi_paramset PNICORR_PARAMSET_PARAMS {

  MPI_Bcast((void *)num_rows, 1, MPI_INT, 0, MPI_COMM_WORLD);
  MPI_Bcast((void *)num_columns, 1, MPI_INT, 0, MPI_COMM_WORLD);
  MPI_Bcast((void *)ntasks, 1, MPI_LONG_LONG, 0, MPI_COMM_WORLD);
  MPI_Bcast((void *)num_workers, 1, MPI_INT, 0, MPI_COMM_WORLD);

  long long fullmatrixbytes = *num_rows * *num_columns * sizeof(float);
  if (my_task_id > 0) {
    int err =
        posix_memalign((void *)full_matrix, PNICORR_ALIGNMENT, fullmatrixbytes);
    ERRIF(err, "posix_memalign");
  } else {
    ALOGIF(*num_workers != *ntasks, "setting ntasks equal to num_workers: (%d "
                                    "workers, %d tasks -> %d tasks)\n",
           *num_workers, (int)*ntasks, *num_workers);
  }
  *ntasks = *num_workers;

  MPI_Bcast((void *)(*full_matrix), *num_rows * *num_columns, MPI_FLOAT, 0,
            MPI_COMM_WORLD);

  MPI_Barrier(MPI_COMM_WORLD);
}

void pnicorr_mpi_proc_body PNICORR_PROC_BODY_PARAMS {
  COR_TASK_PROCESSING(my_task_id);

  if (0 == my_task_id) {
    pnicorr_savematrix_for_task(0, rows_for_task, num_rows, outfile, outtype,
                                correlations);
    for (int i = 1; i < nt; ++i) {
      MPI_Recv(correlations, num_rows * rows_for_task[i], MPI_FLOAT, i,
               PNICORR_CORRELATIONS_TAG, MPI_COMM_WORLD, MPI_STATUS_IGNORE);
      pnicorr_savematrix_for_task(i, rows_for_task, num_rows, outfile, outtype,
                                  correlations);
    }
  } else {
    LOG("[%d] sending %d x %d correlations\n", my_task_id,
        rows_for_task[my_task_id], num_rows);
    MPI_Send(correlations, num_rows * rows_for_task[my_task_id], MPI_FLOAT, 0,
             PNICORR_CORRELATIONS_TAG, MPI_COMM_WORLD);
  }
}

#else

void pnicorr_proc_body PNICORR_PROC_BODY_PARAMS {
  int num_rows_processed = 0;
  for (int i = 0; i < nt; ++i) {

    COR_TASK_PROCESSING(i);

    pnicorr_savematrix_for_task(i, rows_for_task, num_rows, outfile, outtype,
                                correlations);
    num_rows_processed += rows_for_task[i];

    ALOG("%d%% complete\n",
         (int)((float)num_rows_processed / (float)num_rows * 100.0f));
  }
}

#endif


/*
 * Copyright (c) 2018-2021 Aleksas Mazeliauskas, Stefan Floerchinger,
 *                    Eduardo Grossi, and Derek Teaney
 * All rights reserved.
 *
 * FastReso is distributed under MIT license;
 * see the LICENSE file that should be present in the root
 * of the source distribution, or alternately available at:
 * https://github.com/amazeliauskas/FastReso/
 */
#ifndef FASTFO_TParticle_h
#define FASTFO_TParticle_h

#include <string>
#include <vector>
#include <math.h>

#include <vector>
#include <string>
#include <iostream>
#include <grid_params.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_spline.h>
//! Enum class for deciding on Bose-Einstein, Fermi-Dirac or Botlzmann particle dsitribution
enum class EParticleType : int { kBoson = 1, kFermion = -1, kBoltzman = 0 };
enum class EFjIndex : int {
  kFeq1 = 0,
  kFeq2 = 1,
  kFshear1 = 2,
  kFshear2 = 3,
  kFshear3 = 4,
  kFbulk1  = 5,
  kFbulk2  = 6,
  kFtemp1  = 7,
  kFtemp2  = 8,
  kFvel1  = 9,
  kFvel2  = 10,
  kFvel3  = 11 };

//! Particle class with basic properties and universal decay spectra components
class TParticle {
  protected:
    //! Particle properties
    std::string fParticleName;  // particle name
    std::string fDescription;  // particle name
    std::string fDescriptionHeader;  // particle name
    std::string fHeader = "1:pbar [GeV]\t2:m [GeV]";
    std::vector<std::string> fComponentNames = {"Feq 1", "Feq 2","Fshear 1","Fshear 2","Fshear 3","Fbulk 1","Fbulk 2","Ftemp 1","Ftemp 2","Fvel 1","Fvel 2","Fvel 3"};
    //std::string fHeader = "1:pbar [GeV]\t2:m [GeV]\t3:feq 1\t4:feq 2\t5:fshear 1\t6:fshear 2\t7:fshear 3\t8:fbulk 1\t9:fbulk 2\t10:ftemperature 1\t11:ftemperature 2\t12:fvelocity 1\t13:fvelocity 2\t14:fvelocity 3";

        // particle name
    double fMass;   // [GeV] resonance mass
    double fGamma;  // [GeV] resonance width (currently not used)
    double fIsospin; // isospin J |J,m>
    int fPDGCode;   // number code according to PDG
    int fNu;        // degeneracy of a particle
    double fQB;     // baryon charge
    double fQS;     // strange charge
    double fQC;     // charme charge
    EParticleType fParticleType; // particle statistic (bose-einstein, fermi-dirac, boltzmann)

    double fTfo;
    double fNtotal; //integrated particle number
    double fNtotal_buffer;

    //! Grid and interpolator for decay spectra components
    const gsl_interp_type   *fSpline_type = gsl_interp_cspline;
    double fPbar_arr[grid_params::fNpbar]; // array of fluid restframe momentum
    double fFj_arr[grid_params::fNf][grid_params::fNpbar]; // array of scalar functions Fj
    double fFj_arr_buffer[grid_params::fNf][grid_params::fNpbar]; // buffer array of scalar functions Fj
    gsl_spline *fSpline_Fj[grid_params::fNf] ;
    gsl_interp_accel *fPbar_acc;
    //! Bool variables to control data read in/out-put
    bool fIsLocked = false; //if true, prevent modifying fFj_arr data
    bool fIsModified[grid_params::fNf]; //if true, require initilization of interpolator
    //! After any update of the grid, don't forget to reinitialized the interpolator!
    //! initiliazes the interpolator
    void init_grid(std::string comps="Feq Fshear Fbulk Ftemp Fvel");
    void init(int j);
  public:

    std::vector <EFjIndex> fComponents;
    //! Contructor with particle properties
    ~TParticle() ;
    //! return parameters about the particle
    std::string getName() {return fParticleName;};
    double getM() {return fMass;};
    double getGamma() {return fGamma;};
    double getQB() {return fQB;};
    double getQS() {return fQS;};
    double getQC() {return fQC;};
    double getIsospin() {return fIsospin;};
    double getN() {return fNtotal;};
    int getNu() {return fNu;};
    int getPDG() {return fPDGCode;};
    EParticleType getType() {return fParticleType;};

    //! increment integrated particle number
    void addN(double n) {
            if (fIsLocked){
        std::cerr << "\033[1mTParticle.h\033[0m : \033[1;31merror\033[0m : attemped to modify locked "<< fParticleName <<" data" << std::endl;
        exit(EXIT_FAILURE);}
      else { fNtotal+=n; fNtotal_buffer+=n; }};
    //! set initial thermal temperature
    void setTfo(double Tfo) {fTfo=Tfo;};
    //! set lock on data modification
    void lock() {fIsLocked=true;};
    bool hasdecayed() {return fIsLocked;};
    //! clean the buffer
    void clean_buffer() {
      for(int i = 0; i <grid_params::fNpbar; i++){
        for (int jj=0; jj<grid_params::fNf; jj++){
          fFj_arr_buffer[jj][i]=0.0;
        }
        fNtotal_buffer=0.0; }
    };

   //! return the energy and momentum of grid point i
   double getEbar(int i) {return sqrt(fMass*fMass+getPbar(i)*getPbar(i));};
   double getPbar(int i) {return fPbar_arr[i];};
   //! Return the number of momentum grid points
    int getNpbar() {return grid_params::fNpbar;};
    //! Return of the initialization/freeze-out temperature
    double getTfo() {return fTfo;};

    //! increment Fj array at cite i by F
    void addFj(int j, int i, double F) {
      if (fIsLocked){
        std::cerr << "\033[1mTParticle.h\033[0m : \033[1;31merror\033[0m : attemped to modify locked "<< fParticleName <<" data" << std::endl;
        exit(EXIT_FAILURE);}
      else {
        fFj_arr[j][i]+=F;
        fFj_arr_buffer[j][i]+=F;
        fIsModified[j]=true;
      }
    };

    //! Returns the universal scalar functions Fj at Ebar
    double get_Fj(int j, double Ebar);

    //Routines to print the data to screen or file
    std::string getDescriptionHeader(){ return fDescriptionHeader;};
    std::string getDescription(){return fDescription;};
    void print();
    void print(std::string tag);
    void print_buffer();
    void print_buffer(std::string tag);
};

#endif


#ifndef GENERIC_API_NAMESPACE
	#error "Wrong usage of this header"
#endif

#ifndef GENERIC_API_ROUTINES_NAMESPACE
	#error "Wrong usage of this header"
#endif

#define ROUTINES_NAMESPACE namespace GENERIC_API_ROUTINES_NAMESPACE
#define BLAS_NAMESPACE namespace GENERIC_API_NAMESPACE

#ifndef GENERIC_API_DEFINE

namespace cl
{
	namespace routines
	{
		ROUTINES_NAMESPACE
		{
			template<MathDomain md>
			static void Add(MemoryBuffer&, const MemoryBuffer&, const MemoryBuffer&, const double)
			{
				throw NotImplementedException();
			}

			template<MathDomain md>
			static void AddEqual(MemoryBuffer&, const MemoryBuffer&, const double)
			{
				throw NotImplementedException();
			}

			template<MathDomain md>
			static void AddEqualMatrix(MemoryTile&, const MemoryTile&, const MatrixOperation, const MatrixOperation, const double, const double)
			{
				throw NotImplementedException();
			}

			template<MathDomain md>
			static void Scale(MemoryBuffer&, const double)
			{
				throw NotImplementedException();
			}

			template<MathDomain md>
			static void ScaleColumns(MemoryTile&, const MemoryBuffer&)
			{
				throw NotImplementedException();
			}

			template<MathDomain md>
			static void ElementwiseProduct(MemoryBuffer&, const MemoryBuffer&, const MemoryBuffer&, const double)
			{
				throw NotImplementedException();
			}

			template<MathDomain md>
			static void SubMultiply(MemoryTile&, const MemoryTile&, const MemoryTile&, const unsigned, const unsigned, const unsigned, const MatrixOperation, const MatrixOperation, const double, const double)
			{
				throw NotImplementedException();
			}

			template<MathDomain md>
			static void BatchedMultiply(MemoryCube&, const MemoryCube&, const MemoryCube&, const unsigned, const unsigned, const MatrixOperation, const MatrixOperation, const double, const double)
			{
				throw NotImplementedException();
			}

			template<MathDomain md>
			static void Dot(MemoryBuffer&, const MemoryTile&, const MemoryBuffer&, const MatrixOperation, const double, const double)
			{
				throw NotImplementedException();
			}

			template<MathDomain md>
			static void KroneckerProduct(MemoryTile&, const MemoryBuffer&, const MemoryBuffer&, const double)
			{
				throw NotImplementedException();
			}

			template<MathDomain md>
			static void Solve(const MemoryTile&, MemoryTile&, const MatrixOperation, const LinearSystemSolverType)
			{
				throw NotImplementedException();
			}

			template<MathDomain md>
			static void ArgAbsMin(int&, const MemoryBuffer&)
			{
				throw NotImplementedException();
			}

			template<MathDomain md>
			static void ColumnWiseArgAbsMin(MemoryBuffer&, const MemoryTile&)
			{
				throw NotImplementedException();
			}

			template<MathDomain md>
			static void ArgAbsMax(int&, const MemoryBuffer&)
			{
				throw NotImplementedException();
			}

			template<MathDomain md>
			static void ColumnWiseArgAbsMax(MemoryBuffer&, const MemoryTile&)
			{
				throw NotImplementedException();
			}

			// norm = ||x||_2
			template<MathDomain md>
			static void EuclideanNorm(double&, const MemoryBuffer&)
			{
				throw NotImplementedException();
			}
		}
	}	 // namespace routines
}	 // namespace cl

#else

	#include <cmath>
	#include <complex>

BLAS_NAMESPACE
{
	#include <cblas.h>
	#include <lapacke.h>
}

namespace cl
{
	namespace routines
	{
		ROUTINES_NAMESPACE
		{
			static constexpr GENERIC_API_NAMESPACE::CBLAS_ORDER columnMajorLayout = { GENERIC_API_NAMESPACE::CBLAS_ORDER::CblasColMajor };
			static constexpr std::array<GENERIC_API_NAMESPACE::CBLAS_TRANSPOSE, 2> operationsEnum = { GENERIC_API_NAMESPACE::CBLAS_TRANSPOSE::CblasNoTrans, GENERIC_API_NAMESPACE::CBLAS_TRANSPOSE::CblasTrans };

			template<MathDomain md>
			static void Add(MemoryBuffer & z, const MemoryBuffer& x, const MemoryBuffer& y, const double alpha);

			template<>
			inline void Add<MathDomain::Float>(MemoryBuffer & z, const MemoryBuffer& x, const MemoryBuffer& y, const double alpha)
			{
				Copy<MathDomain::Float>(z, y);
				GENERIC_API_NAMESPACE::cblas_saxpy(static_cast<int>(z.size), static_cast<float>(alpha), reinterpret_cast<float*>(x.pointer), 1, reinterpret_cast<float*>(z.pointer), 1);
			}
			template<>
			inline void Add<MathDomain::Double>(MemoryBuffer & z, const MemoryBuffer& x, const MemoryBuffer& y, const double alpha)
			{
				Copy<MathDomain::Double>(z, y);
				GENERIC_API_NAMESPACE::cblas_daxpy(static_cast<int>(z.size), alpha, reinterpret_cast<double*>(x.pointer), 1, reinterpret_cast<double*>(z.pointer), 1);
			}

			template<MathDomain md>
			static void AddEqual(MemoryBuffer & z, const MemoryBuffer& x, const double alpha);

			template<>
			inline void AddEqual<MathDomain::Float>(MemoryBuffer & z, const MemoryBuffer& x, const double alpha)
			{
				GENERIC_API_NAMESPACE::cblas_saxpy(static_cast<int>(z.size), static_cast<float>(alpha), reinterpret_cast<float*>(x.pointer), 1, reinterpret_cast<float*>(z.pointer), 1);
			}
			template<>
			inline void AddEqual<MathDomain::Double>(MemoryBuffer & z, const MemoryBuffer& x, const double alpha)
			{
				GENERIC_API_NAMESPACE::cblas_daxpy(static_cast<int>(z.size), alpha, reinterpret_cast<double*>(x.pointer), 1, reinterpret_cast<double*>(z.pointer), 1);
			}

			template<MathDomain md>
			static void AddEqualMatrix(MemoryTile & A, const MemoryTile& B, const MatrixOperation aOperation, const MatrixOperation bOperation, const double alpha, const double beta);

			template<>
			inline void AddEqualMatrix<MathDomain::Float>(MemoryTile & A, const MemoryTile& B, const MatrixOperation aOperation, const MatrixOperation bOperation, const double alpha, const double beta)
			{
				if (aOperation != MatrixOperation::None)
					throw NotImplementedException();
				if (bOperation != MatrixOperation::None)
					throw NotImplementedException();

				GENERIC_API_NAMESPACE::cblas_sgeadd(columnMajorLayout, static_cast<int>(A.nRows), static_cast<int>(A.nCols), static_cast<float>(beta), reinterpret_cast<float*>(B.pointer), static_cast<int>(B.leadingDimension), static_cast<float>(alpha), reinterpret_cast<float*>(A.pointer), static_cast<int>(A.leadingDimension));
			}
			template<>
			inline void AddEqualMatrix<MathDomain::Double>(MemoryTile & A, const MemoryTile& B, const MatrixOperation aOperation, const MatrixOperation bOperation, const double alpha, const double beta)
			{
				if (aOperation != MatrixOperation::None)
					throw NotImplementedException();
				if (bOperation != MatrixOperation::None)
					throw NotImplementedException();

				GENERIC_API_NAMESPACE::cblas_dgeadd(columnMajorLayout, static_cast<int>(A.nRows), static_cast<int>(A.nCols), beta, reinterpret_cast<double*>(B.pointer), static_cast<int>(B.leadingDimension), alpha, reinterpret_cast<double*>(A.pointer), static_cast<int>(A.leadingDimension));
			}

			template<MathDomain md>
			static void Scale(MemoryBuffer & z, const double alpha);

			template<>
			inline void Scale<MathDomain::Float>(MemoryBuffer & z, const double alpha)
			{
				GENERIC_API_NAMESPACE::cblas_sscal(static_cast<int>(z.size), static_cast<float>(alpha), reinterpret_cast<float*>(z.pointer), 1);
			}
			template<>
			inline void Scale<MathDomain::Double>(MemoryBuffer & z, const double alpha)
			{
				GENERIC_API_NAMESPACE::cblas_dscal(static_cast<int>(z.size), alpha, reinterpret_cast<double*>(z.pointer), 1);
			}

			template<MathDomain md>
			static void ScaleColumns(MemoryTile & z, const MemoryBuffer& alpha);

			template<>
			inline void ScaleColumns<MathDomain::Float>(MemoryTile & z, const MemoryBuffer& alpha)
			{
				for (size_t i = 0; i < z.nCols; ++i)
					GENERIC_API_NAMESPACE::cblas_sscal(static_cast<int>(z.nRows), *reinterpret_cast<float*>(alpha.pointer + i * alpha.ElementarySize()), reinterpret_cast<float*>(z.pointer + i * z.nRows * z.ElementarySize()), 1);
			}
			template<>
			inline void ScaleColumns<MathDomain::Double>(MemoryTile & z, const MemoryBuffer& alpha)
			{
				for (size_t i = 0; i < z.nCols; ++i)
					GENERIC_API_NAMESPACE::cblas_dscal(static_cast<int>(z.nRows), *reinterpret_cast<double*>(alpha.pointer + i * alpha.ElementarySize()), reinterpret_cast<double*>(z.pointer + i * z.nRows * z.ElementarySize()), 1);
			}

			template<MathDomain md>
			static void SubMultiply(MemoryTile & A, const MemoryTile& B, const MemoryTile& C, const unsigned nRowsB, const unsigned nColsB, const unsigned nColsC, const MatrixOperation bOperation, const MatrixOperation cOperation, const double alpha, const double beta);

			template<>
			inline void SubMultiply<MathDomain::Float>(MemoryTile & A, const MemoryTile& B, const MemoryTile& C, const unsigned nRowsB, const unsigned nColsB, const unsigned nColsC, const MatrixOperation bOperation, const MatrixOperation cOperation, const double alpha, const double beta)
			{
				GENERIC_API_NAMESPACE::cblas_sgemm(columnMajorLayout, operationsEnum[static_cast<unsigned>(bOperation)], operationsEnum[static_cast<unsigned>(cOperation)], static_cast<int>(nRowsB), static_cast<int>(nColsC), static_cast<int>(nColsB), static_cast<float>(alpha), reinterpret_cast<float*>(B.pointer), static_cast<int>(B.leadingDimension), reinterpret_cast<float*>(C.pointer), static_cast<int>(C.leadingDimension), static_cast<float>(beta), reinterpret_cast<float*>(A.pointer), static_cast<int>(A.leadingDimension));
			}

			template<>
			inline void SubMultiply<MathDomain::Double>(MemoryTile & A, const MemoryTile& B, const MemoryTile& C, const unsigned nRowsB, const unsigned nColsB, const unsigned nColsC, const MatrixOperation bOperation, const MatrixOperation cOperation, const double alpha, const double beta)
			{
				GENERIC_API_NAMESPACE::cblas_dgemm(columnMajorLayout, operationsEnum[static_cast<unsigned>(bOperation)], operationsEnum[static_cast<unsigned>(cOperation)], static_cast<int>(nRowsB), static_cast<int>(nColsC), static_cast<int>(nColsB), alpha, reinterpret_cast<double*>(B.pointer), static_cast<int>(B.leadingDimension), reinterpret_cast<double*>(C.pointer), static_cast<int>(C.leadingDimension), beta, reinterpret_cast<double*>(A.pointer), static_cast<int>(A.leadingDimension));
			}

			template<MathDomain md>
			static void Dot(MemoryBuffer & y, const MemoryTile& A, const MemoryBuffer& x, const MatrixOperation aOperation, const double alpha = 1.0, const double beta = 0.0);

			template<>
			inline void Dot<MathDomain::Float>(MemoryBuffer & y, const MemoryTile& A, const MemoryBuffer& x, const MatrixOperation aOperation, const double alpha, const double beta)
			{
				GENERIC_API_NAMESPACE::cblas_sgemv(columnMajorLayout, operationsEnum[static_cast<unsigned>(aOperation)], static_cast<int>(A.nRows), static_cast<int>(A.nCols), static_cast<float>(alpha), reinterpret_cast<float*>(A.pointer), static_cast<int>(A.leadingDimension), reinterpret_cast<float*>(x.pointer), 1, static_cast<float>(beta), reinterpret_cast<float*>(y.pointer), 1);
			}
			template<>
			inline void Dot<MathDomain::Double>(MemoryBuffer & y, const MemoryTile& A, const MemoryBuffer& x, const MatrixOperation aOperation, const double alpha, const double beta)
			{
				GENERIC_API_NAMESPACE::cblas_dgemv(columnMajorLayout, operationsEnum[static_cast<unsigned>(aOperation)], static_cast<int>(A.nRows), static_cast<int>(A.nCols), alpha, reinterpret_cast<double*>(A.pointer), static_cast<int>(A.leadingDimension), reinterpret_cast<double*>(x.pointer), 1, beta, reinterpret_cast<double*>(y.pointer), 1);
			}

			template<MathDomain md>
			static void KroneckerProduct(MemoryTile & A, const MemoryBuffer& x, const MemoryBuffer& y, const double alpha);

			template<>
			inline void KroneckerProduct<MathDomain::Float>(MemoryTile & A, const MemoryBuffer& x, const MemoryBuffer& y, const double alpha)
			{
				GENERIC_API_NAMESPACE::cblas_sger(columnMajorLayout, static_cast<int>(x.size), static_cast<int>(y.size), static_cast<float>(alpha), reinterpret_cast<float*>(x.pointer), 1, reinterpret_cast<float*>(y.pointer), 1, reinterpret_cast<float*>(A.pointer), static_cast<int>(A.nRows));
			}
			template<>
			inline void KroneckerProduct<MathDomain::Double>(MemoryTile & A, const MemoryBuffer& x, const MemoryBuffer& y, const double alpha)
			{
				GENERIC_API_NAMESPACE::cblas_dger(columnMajorLayout, static_cast<int>(x.size), static_cast<int>(y.size), alpha, reinterpret_cast<double*>(x.pointer), 1, reinterpret_cast<double*>(y.pointer), 1, reinterpret_cast<double*>(A.pointer), static_cast<int>(A.nRows));
			}

			template<MathDomain md>
			static void Solve(const MemoryTile& A, MemoryTile& B, const MatrixOperation aOperation, const LinearSystemSolverType solver);

			template<>
			inline void Solve<MathDomain::Float>(const MemoryTile& A, MemoryTile& B, const MatrixOperation aOperation, const LinearSystemSolverType solver)
			{
				// Need to copy A, as it will be overwritten by its factorization
				MemoryTile aCopy(A);
				Alloc(aCopy);
				Copy<MathDomain::Float>(aCopy, A);

				const auto nra = static_cast<int>(A.nRows);
				const auto lda = static_cast<int>(A.leadingDimension);
				const auto ncb = static_cast<int>(B.nCols);
				const auto ldb = static_cast<int>(B.leadingDimension);

				// Initializes auxliary value for solver
				int info = 0;

				switch (solver)
				{
					case LinearSystemSolverType::Lu:
					{
						// allocate memory for pivoting
						MemoryBuffer pivot(0, A.nRows, A.memorySpace, MathDomain::Int);
						Alloc(pivot);

						// Factorize A (and overwrite it with L)
						info = GENERIC_API_NAMESPACE::LAPACKE_sgetrf(static_cast<int>(columnMajorLayout), nra, nra, reinterpret_cast<float*>(aCopy.pointer), lda, reinterpret_cast<int*>(pivot.pointer));
						if (info != 0)
							throw OpenBlasException(__func__);

						// Solve factorized system
						info = GENERIC_API_NAMESPACE::LAPACKE_sgetrs(static_cast<int>(columnMajorLayout), openBlasOperation[static_cast<unsigned>(aOperation)], nra, ncb, reinterpret_cast<float*>(aCopy.pointer), lda, reinterpret_cast<int*>(pivot.pointer), reinterpret_cast<float*>(B.pointer), ldb);
						if (info != 0)
							throw OpenBlasException(__func__);

						// free memory
						Free(pivot);

						break;
					}
					case LinearSystemSolverType::Qr:
					{
						// allocate memory for tau
						MemoryBuffer tau(0, A.nRows, A.memorySpace, MathDomain::Float);
						Alloc(tau);

						// A = Q * R
						/* int matrix_layout, lapack_int m, lapack_int n,
								   float* a, lapack_int lda, float* tau */
						info = GENERIC_API_NAMESPACE::LAPACKE_sgeqrf(static_cast<int>(columnMajorLayout), nra, nra, reinterpret_cast<float*>(aCopy.pointer), lda, reinterpret_cast<float*>(tau.pointer));
						if (info != 0)
							throw OpenBlasException(__func__);

						// B = Q^T * B
						/*
						 *  int matrix_layout, char side, char trans,
								   lapack_int m, lapack_int n, lapack_int k,
								   const float* a, lapack_int lda, const float* tau,
								   float* c, lapack_int ldc */
						info = GENERIC_API_NAMESPACE::LAPACKE_sormqr(static_cast<int>(columnMajorLayout), 'L', 'T', nra, nra, ncb, reinterpret_cast<float*>(aCopy.pointer), lda, reinterpret_cast<float*>(tau.pointer), reinterpret_cast<float*>(B.pointer), ldb);
						if (info != 0)
							throw OpenBlasException(__func__);

						// Solve (x = R \ (Q^T * B))
						GENERIC_API_NAMESPACE::cblas_strsm(columnMajorLayout, GENERIC_API_NAMESPACE::CBLAS_SIDE::CblasLeft, GENERIC_API_NAMESPACE::CBLAS_UPLO::CblasUpper, operationsEnum[static_cast<unsigned>(aOperation)], GENERIC_API_NAMESPACE::CBLAS_DIAG::CblasNonUnit, nra, nra, 1.0, reinterpret_cast<float*>(aCopy.pointer), lda, reinterpret_cast<float*>(B.pointer), ldb);

						// free memory
						Free(tau);

						break;
					}
					default:
						throw NotImplementedException();
				}

				Free(aCopy);
			}

			template<>
			inline void Solve<MathDomain::Double>(const MemoryTile& A, MemoryTile& B, const MatrixOperation aOperation, const LinearSystemSolverType solver)
			{
				// Need to copy A, as it will be overwritten by its factorization
				MemoryTile aCopy(A);
				Alloc(aCopy);
				Copy<MathDomain::Double>(aCopy, A);

				const auto nra = static_cast<int>(A.nRows);
				const auto lda = static_cast<int>(A.leadingDimension);
				const auto ncb = static_cast<int>(B.nCols);
				const auto ldb = static_cast<int>(B.leadingDimension);

				// Initializes auxliary value for solver
				int info = 0;

				switch (solver)
				{
					case LinearSystemSolverType::Lu:
					{
						// allocate memory for pivoting
						MemoryBuffer pivot(0, A.nRows, A.memorySpace, MathDomain::Int);
						Alloc(pivot);

						// Factorize A (and overwrite it with L)
						info = GENERIC_API_NAMESPACE::LAPACKE_dgetrf(static_cast<int>(columnMajorLayout), nra, nra, reinterpret_cast<double*>(aCopy.pointer), lda, reinterpret_cast<int*>(pivot.pointer));
						if (info != 0)
							throw OpenBlasException(__func__);

						// Solve factorized system
						info = GENERIC_API_NAMESPACE::LAPACKE_dgetrs(static_cast<int>(columnMajorLayout), openBlasOperation[static_cast<unsigned>(aOperation)], nra, ncb, reinterpret_cast<double*>(aCopy.pointer), lda, reinterpret_cast<int*>(pivot.pointer), reinterpret_cast<double*>(B.pointer), ldb);
						if (info != 0)
							throw OpenBlasException(__func__);

						// free memory
						Free(pivot);

						break;
					}
					case LinearSystemSolverType::Qr:
					{
						// allocate memory for tau
						MemoryBuffer tau(0, A.nRows, A.memorySpace, MathDomain::Double);
						Alloc(tau);

						// A = Q * R
						/* int matrix_layout, lapack_int m, lapack_int n,
								   double* a, lapack_int lda, double* tau */
						info = GENERIC_API_NAMESPACE::LAPACKE_dgeqrf(static_cast<int>(columnMajorLayout), nra, nra, reinterpret_cast<double*>(aCopy.pointer), lda, reinterpret_cast<double*>(tau.pointer));
						if (info != 0)
							throw OpenBlasException(__func__);

						// B = Q^T * B
						/*
						 *  int matrix_layout, char side, char trans,
								   lapack_int m, lapack_int n, lapack_int k,
								   const double* a, lapack_int lda, const double* tau,
								   double* c, lapack_int ldc */
						info = GENERIC_API_NAMESPACE::LAPACKE_dormqr(static_cast<int>(columnMajorLayout), 'L', 'T', nra, nra, ncb, reinterpret_cast<double*>(aCopy.pointer), lda, reinterpret_cast<double*>(tau.pointer), reinterpret_cast<double*>(B.pointer), ldb);
						if (info != 0)
							throw OpenBlasException(__func__);

						// Solve (x = R \ (Q^T * B))
						cblas_dtrsm(columnMajorLayout, GENERIC_API_NAMESPACE::CBLAS_SIDE::CblasLeft, GENERIC_API_NAMESPACE::CBLAS_UPLO::CblasUpper, operationsEnum[static_cast<unsigned>(aOperation)], GENERIC_API_NAMESPACE::CBLAS_DIAG::CblasNonUnit, nra, nra, 1.0, reinterpret_cast<double*>(aCopy.pointer), lda, reinterpret_cast<double*>(B.pointer), ldb);

						// free memory
						Free(tau);

						break;
					}
					default:
						throw NotImplementedException();
				}

				Free(aCopy);
			}

			template<MathDomain md>
			static void ArgAbsMin(int& argMin, const MemoryBuffer& x);

			template<>
			inline void ArgAbsMin<MathDomain::Float>(int&, const MemoryBuffer&)
			{
				// TODO: apparently there's isamax but not isamin??
				// argMin = static_cast<int>(cblas_isamin(static_cast<int>(x.size), reinterpret_cast<float*>(x.pointer), 1));
			}
			template<>
			inline void ArgAbsMin<MathDomain::Double>(int&, const MemoryBuffer&)
			{
				// TODO: apparently there's isamax but not isamin??
				// argMin = static_cast<int>(cblas_idamin(static_cast<int>(x.size), reinterpret_cast<double*>(x.pointer), 1));
			}

			template<MathDomain md>
			static void ColumnWiseArgAbsMin(MemoryBuffer & argMin, const MemoryTile& A);

			template<>
			inline void ColumnWiseArgAbsMin<MathDomain::Float>(MemoryBuffer&, const MemoryTile&)
			{
				// TODO
				//		auto* argMinPtr = reinterpret_cast<int*>(argMin.pointer);
				//		// 1 added for compatibility
				//		for (size_t j = 0; j < A.nCols; ++j)
				//			argMinPtr[j] = 1 + static_cast<int>(cblas_isamin(static_cast<int>(A.nRows), reinterpret_cast<float*>(A.pointer + j * A.nRows * A.ElementarySize()), 1));
			}
			template<>
			inline void ColumnWiseArgAbsMin<MathDomain::Double>(MemoryBuffer&, const MemoryTile&)
			{
				// TODO
				//		// 1 added for compatibility
				//		auto* argMinPtr = reinterpret_cast<int*>(argMin.pointer);
				//		for (size_t j = 0; j < A.nCols; ++j)
				//			argMinPtr[j] = 1 + static_cast<int>(cblas_idamin(static_cast<int>(A.nRows), reinterpret_cast<double*>(A.pointer + j * A.nRows * A.ElementarySize()), 1));
			}

			template<MathDomain md>
			static void ArgAbsMax(int& argMax, const MemoryBuffer& x);

			template<>
			inline void ArgAbsMax<MathDomain::Float>(int& argMax, const MemoryBuffer& x)
			{
				argMax = static_cast<int>(GENERIC_API_NAMESPACE::cblas_isamax(static_cast<int>(x.size), reinterpret_cast<float*>(x.pointer), 1));
			}
			template<>
			inline void ArgAbsMax<MathDomain::Double>(int& argMax, const MemoryBuffer& x)
			{
				argMax = static_cast<int>(GENERIC_API_NAMESPACE::cblas_idamax(static_cast<int>(x.size), reinterpret_cast<double*>(x.pointer), 1));
			}

			template<MathDomain md>
			static void ColumnWiseArgAbsMax(MemoryBuffer & argMax, const MemoryTile& A);

			template<>
			inline void ColumnWiseArgAbsMax<MathDomain::Float>(MemoryBuffer & argMax, const MemoryTile& A)
			{
				// 1 added for compatibility
				auto* argMaxPtr = reinterpret_cast<int*>(argMax.pointer);
				for (size_t j = 0; j < A.nCols; ++j)
					argMaxPtr[j] = 1 + static_cast<int>(GENERIC_API_NAMESPACE::cblas_isamax(static_cast<int>(A.nRows), reinterpret_cast<float*>(A.pointer + j * A.nRows * A.ElementarySize()), 1));
			}
			template<>
			inline void ColumnWiseArgAbsMax<MathDomain::Double>(MemoryBuffer & argMax, const MemoryTile& A)
			{
				// 1 added for compatibility
				auto* argMaxPtr = reinterpret_cast<int*>(argMax.pointer);
				for (size_t j = 0; j < A.nCols; ++j)
					argMaxPtr[j] = 1 + static_cast<int>(GENERIC_API_NAMESPACE::cblas_idamax(static_cast<int>(A.nRows), reinterpret_cast<double*>(A.pointer + j * A.nRows * A.ElementarySize()), 1));
			}

			// norm = ||x||_2
			template<MathDomain md>
			static void EuclideanNorm(double& norm, const MemoryBuffer& z);

			template<>
			inline void EuclideanNorm<MathDomain::Float>(double& norm, const MemoryBuffer& z)
			{
				norm = static_cast<double>(GENERIC_API_NAMESPACE::cblas_snrm2(static_cast<int>(z.size), reinterpret_cast<float*>(z.pointer), 1));
			}
			template<>
			inline void EuclideanNorm<MathDomain::Double>(double& norm, const MemoryBuffer& z)
			{
				norm = GENERIC_API_NAMESPACE::cblas_dnrm2(static_cast<int>(z.size), reinterpret_cast<double*>(z.pointer), 1);
			}
		}
	}	 // namespace routines
}	 // namespace cl

#endif

#undef GENERIC_API_DEFINE
#undef GENERIC_API_NAMESPACE
#undef GENERIC_API_ROUTINES_NAMESPACE
#undef ROUTINES_NAMESPACE
#undef BLAS_NAMESPACE


#include <stdio.h>
#include <string.h>
#include <gsl/gsl_statistics_double.h>
#include "tz_error.h"
#include "tz_constant.h"
#include "tz_image_lib.h"
#include "tz_objdetect.h"
#include "tz_imatrix.h"
#include "tz_stack_math.h"
#include "tz_stack_bwdist.h"
#include "tz_pixel_array.h"
#include "tz_stack_bwmorph.h"
#include "tz_stack_draw.h"
#include "tz_dmatrix.h"

INIT_EXCEPTION_MAIN(e)

int main(int argc, char* argv[])
{
  char neuron_name[100];
  if (argc == 1) {
    strcpy(neuron_name, "fly_neuron");
  } else {
    strcpy(neuron_name, argv[1]);
  }

  char file_path[100];

  sprintf(file_path, "../data/%s/objseed.tif", neuron_name);
  Stack *seedimg = Read_Stack(file_path);
  Object_3d_List *objs = Stack_Find_Object_N(seedimg, NULL, 1, 0, 26);

  Stack *stack2 = NULL;
  Object_3d *obj = NULL;
  stack2 = NULL;

  Object_3d_List *objs2 = NULL;

  sprintf(file_path, "../data/%s/bundle.tif", neuron_name);
  stack2 = Read_Stack(file_path);

  while (objs != NULL) {
    obj = Stack_Grow_Object(stack2, 1, objs->data->voxels[0]);
    if (obj != NULL) {
      if (obj->size > 1000) {
	if (objs2 == NULL) {
	  objs2 = Object_3d_List_New();
	  objs2->data = obj;
	} else {
	  Object_3d_List_Add(&objs2, obj);
	  Print_Object_3d_Info(obj);
	}
      } else {
	Kill_Object_3d(obj);
      }
    }
    objs = objs->next;
  }

  Print_Object_3d_List_Compact(objs2);

  Stack *bundle = Make_Stack(GREY, stack2->width, stack2->height, stack2->depth);
  Zero_Stack(bundle);

  Stack_Draw_Objects_Bw(bundle, objs2, 1);

  sprintf(file_path, "../data/%s/grow_bundle.tif", neuron_name);
  Write_Stack(file_path, bundle);

  return 0;
}


/* interpolation/test2d.c
 *
 * Copyright 2012 David Zaslavsky
 *
 * 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_interp2d.h>
#include <gsl/gsl_spline2d.h>

/* tests a single evaluator function from the low-level interface */
static int
test_single_low_level(
  double (*evaluator)(const gsl_interp2d *, const double[], const double[],
                      const double[], const double, const double,
                      gsl_interp_accel *, gsl_interp_accel *),
  int (*evaluator_e)(const gsl_interp2d *, const double[], const double[],
                     const double[], const double, const double,
                     gsl_interp_accel *, gsl_interp_accel *, double *),
  const gsl_interp2d * interp, const double xarr[], const double yarr[],
  const double zarr[], const double x, const double y,
  gsl_interp_accel * xa, gsl_interp_accel * ya,
  const double expected_results[], size_t i)
{
  if (expected_results != NULL)
    {
      int status;
      double result = evaluator(interp, xarr, yarr, zarr, x, y, xa, ya);
      gsl_test_rel(result, expected_results[i], 1e-10,
                   "low level %s %d", gsl_interp2d_name(interp), i);

      status = evaluator_e(interp, xarr, yarr, zarr, x, y, xa, ya, &result);
      if (status == GSL_SUCCESS)
        {
          gsl_test_rel(result, expected_results[i], 1e-10,
                       "low level _e %s %d", gsl_interp2d_name(interp), i);
        }
    }

  return 0;
}

/* tests a single evaluator function from the high-level interface */
static int
test_single_high_level(
    double (*evaluator)(const gsl_spline2d *, const double, const double,
                        gsl_interp_accel *, gsl_interp_accel *),
    int (*evaluator_e)(const gsl_spline2d *, const double, const double,
                       gsl_interp_accel *, gsl_interp_accel *, double *),
    const gsl_spline2d * interp, const double x, const double y,
    gsl_interp_accel * xa, gsl_interp_accel * ya,
    const double expected_results[], size_t i)
{
  if (expected_results != NULL)
    {
      int status;
      double result = evaluator(interp, x, y, xa, ya);

      gsl_test_rel(result, expected_results[i], 1e-10,
                   "high level %s %d", gsl_spline2d_name(interp), i);

      status = evaluator_e(interp, x, y, xa, ya, &result);
      if (status == GSL_SUCCESS)
        {
          gsl_test_rel(result, expected_results[i], 1e-10,
                       "high level _e %s %d", gsl_spline2d_name(interp), i);
        }
    }

  return 0;
}

/*
 * Tests that a given interpolation type reproduces the data points
 * it is given, and then tests that it correctly reproduces additional
 * values.
 */
static int
test_interp2d(
const double xarr[], const double yarr[], const double zarr[], /* data */
              size_t xsize, size_t ysize,                /* array sizes */
              const double xval[], const double yval[],  /* test points */
              const double zval[],                       /* expected results */
              const double zxval[], const double zyval[],
              const double zxxval[], const double zyyval[], const double zxyval[],
              size_t test_size,                          /* number of test points */
              const gsl_interp2d_type * T)
{
  gsl_interp_accel *xa = gsl_interp_accel_alloc();
  gsl_interp_accel *ya = gsl_interp_accel_alloc();
  int status = 0;
  size_t xi, yi, zi, i;
  gsl_interp2d * interp = gsl_interp2d_alloc(T, xsize, ysize);
  gsl_spline2d * interp_s = gsl_spline2d_alloc(T, xsize, ysize);
  unsigned int min_size = gsl_interp2d_type_min_size(T);

  gsl_test_int(min_size, T->min_size, "gsl_interp2d_type_min_size on %s", gsl_interp2d_name(interp));

  gsl_interp2d_init(interp, xarr, yarr, zarr, xsize, ysize);
  gsl_spline2d_init(interp_s, xarr, yarr, zarr, xsize, ysize);

  /* First check that the interpolation reproduces the given points */
  for (xi = 0; xi < xsize; xi++)
    {
      double x = xarr[xi];
      for (yi = 0; yi < ysize; yi++)
        {
          double y = yarr[yi];

          zi = gsl_interp2d_idx(interp, xi, yi);
          test_single_low_level(&gsl_interp2d_eval, &gsl_interp2d_eval_e,
                                interp, xarr, yarr, zarr, x, y,
                                xa, ya, zarr, zi);
          test_single_low_level(&gsl_interp2d_eval_extrap,
                                &gsl_interp2d_eval_extrap_e, interp,
                                xarr, yarr, zarr, x, y, xa, ya, zarr, zi);
          test_single_high_level(&gsl_spline2d_eval, &gsl_spline2d_eval_e,
                                 interp_s, x, y, xa, ya, zarr, zi);
          test_single_high_level(&gsl_spline2d_eval_extrap, &gsl_spline2d_eval_extrap_e,
                                 interp_s, x, y, xa, ya, zarr, zi);
        }
    }

  /* Then check additional points provided */
  for (i = 0; i < test_size; i++)
    {
      double x = xval[i];
      double y = yval[i];

      test_single_low_level(&gsl_interp2d_eval,         &gsl_interp2d_eval_e,          interp, xarr, yarr, zarr, x, y, xa, ya, zval, i);
      test_single_low_level(&gsl_interp2d_eval_deriv_x, &gsl_interp2d_eval_deriv_x_e,  interp, xarr, yarr, zarr, x, y, xa, ya, zxval, i);
      test_single_low_level(&gsl_interp2d_eval_deriv_y, &gsl_interp2d_eval_deriv_y_e,  interp, xarr, yarr, zarr, x, y, xa, ya, zyval, i);
      test_single_low_level(&gsl_interp2d_eval_deriv_xx,&gsl_interp2d_eval_deriv_xx_e, interp, xarr, yarr, zarr, x, y, xa, ya, zxxval, i);
      test_single_low_level(&gsl_interp2d_eval_deriv_yy,&gsl_interp2d_eval_deriv_yy_e, interp, xarr, yarr, zarr, x, y, xa, ya, zyyval, i);
      test_single_low_level(&gsl_interp2d_eval_deriv_xy,&gsl_interp2d_eval_deriv_xy_e, interp, xarr, yarr, zarr, x, y, xa, ya, zxyval, i);

      test_single_high_level(&gsl_spline2d_eval,         &gsl_spline2d_eval_e,          interp_s, x, y, xa, ya, zval, i);
      test_single_high_level(&gsl_spline2d_eval_extrap,  &gsl_spline2d_eval_extrap_e,          interp_s, x, y, xa, ya, zval, i);
      test_single_high_level(&gsl_spline2d_eval_deriv_x, &gsl_spline2d_eval_deriv_x_e,  interp_s, x, y, xa, ya, zxval, i);
      test_single_high_level(&gsl_spline2d_eval_deriv_y, &gsl_spline2d_eval_deriv_y_e,  interp_s, x, y, xa, ya, zyval, i);
      test_single_high_level(&gsl_spline2d_eval_deriv_xx,&gsl_spline2d_eval_deriv_xx_e, interp_s, x, y, xa, ya, zxxval, i);
      test_single_high_level(&gsl_spline2d_eval_deriv_yy,&gsl_spline2d_eval_deriv_yy_e, interp_s, x, y, xa, ya, zyyval, i);
      test_single_high_level(&gsl_spline2d_eval_deriv_xy,&gsl_spline2d_eval_deriv_xy_e, interp_s, x, y, xa, ya, zxyval, i);

      test_single_low_level(&gsl_interp2d_eval_extrap, &gsl_interp2d_eval_extrap_e, interp, xarr, yarr, zarr, x, y, xa, ya, zval, i);
    }

  gsl_interp_accel_free(xa);
  gsl_interp_accel_free(ya);
  gsl_interp2d_free(interp);
  gsl_spline2d_free(interp_s);

  return status;
}

/*
 * Tests bilinear interpolation using a symmetric function, f(x,y)==f(y,x),
 * and diagonal interpolation points (x,y) where x==y. If these tests don't
 * pass, something is seriously broken.
 */
static int
test_bilinear_symmetric()
{
  int status;
  double xarr[] = {0.0, 1.0, 2.0, 3.0};
  double yarr[] = {0.0, 1.0, 2.0, 3.0};
  double zarr[] = {1.0, 1.1, 1.2, 1.3,
                   1.1, 1.2, 1.3, 1.4,
                   1.2, 1.3, 1.4, 1.5,
                   1.3, 1.4, 1.5, 1.6};
  double xval[] = {0.0, 0.5, 1.0, 1.5, 2.5, 3.0};
  double yval[] = {0.0, 0.5, 1.0, 1.5, 2.5, 3.0};
  double zval[] = {1.0, 1.1, 1.2, 1.3, 1.5, 1.6};
  size_t xsize = sizeof(xarr) / sizeof(xarr[0]);
  size_t ysize = sizeof(yarr) / sizeof(yarr[0]);
  size_t test_size = sizeof(xval) / sizeof(xval[0]);

  status = test_interp2d(xarr, yarr, zarr, xsize, ysize, xval, yval, zval,
                         NULL, NULL, NULL, NULL, NULL, test_size,
                         gsl_interp2d_bilinear);
  gsl_test(status, "bilinear interpolation with symmetric values");

  return status;
}

/*
 * Tests bilinear interpolation with an asymmetric function, f(x,y)!=f(y,x),
 * and off-diagonal interpolation points (x,y) where x and y may or may not
 * be equal.
 */
static int
test_bilinear_asymmetric_z()
{
  int status;
  double xarr[] = {0.0, 1.0, 2.0, 3.0};
  double yarr[] = {0.0, 1.0, 2.0, 3.0};
  double zarr[] = {1.0, 1.1, 1.2, 1.4,
                   1.3, 1.4, 1.5, 1.7,
                   1.5, 1.6, 1.7, 1.9,
                   1.6, 1.9, 2.2, 2.3};
  double xval[] = { 0.0, 0.5, 1.0, 1.5,  2.5, 3.0,
                    1.3954, 1.6476, 0.824957,
                    2.41108,  2.98619, 1.36485 };
  double yval[] = {0.0, 0.5, 1.0, 1.5,  2.5, 3.0,
                   0.265371, 2.13849, 1.62114,
                   1.22198, 0.724681, 0.0596087 };

  /* results computed using Mathematica 9.0.1.0 */
  double zval[] = {1.0, 1.2, 1.4, 1.55, 2.025, 2.3,
                   1.2191513, 1.7242442248, 1.5067237,
                   1.626612, 1.6146423, 1.15436761};
  size_t xsize = sizeof(xarr) / sizeof(xarr[0]);
  size_t ysize = sizeof(yarr) / sizeof(yarr[0]);
  size_t test_size = sizeof(xval) / sizeof(xval[0]);

  status = test_interp2d(xarr, yarr, zarr, xsize, ysize, xval, yval, zval,
                         NULL, NULL, NULL, NULL, NULL, test_size,
                         gsl_interp2d_bilinear);
  gsl_test(status, "bilinear interpolation with asymmetric z values");

  return status;
}

static int
test_bicubic()
{
  int status;
  double xarr[] = {0.0, 1.0, 2.0, 3.0};
  double yarr[] = {0.0, 1.0, 2.0, 3.0};
  double zarr[] = {1.0, 1.1, 1.2, 1.3,
                   1.1, 1.2, 1.3, 1.4,
                   1.2, 1.3, 1.4, 1.5,
                   1.3, 1.4, 1.5, 1.6};
  double xval[] = {1.0, 1.5, 2.0};
  double yval[] = {1.0, 1.5, 2.0};
  double zval[] = {1.2, 1.3, 1.4};
  size_t xsize = sizeof(xarr) / sizeof(xarr[0]);
  size_t ysize = sizeof(yarr) / sizeof(yarr[0]);
  size_t test_size = sizeof(xval) / sizeof(xval[0]);

  status = test_interp2d(xarr, yarr, zarr, xsize, ysize, xval, yval, zval,
                         NULL, NULL, NULL, NULL, NULL, test_size,
                         gsl_interp2d_bicubic);
  gsl_test(status, "bicubic interpolation on linear function");

  return status;
}

static int
test_bicubic_nonlinear()
{
  int status;
  double xarr[] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0};
  double yarr[] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0};
  /* least common multiple of x and y */
  double zarr[] = { 1,  2,  3,  4,  5,  6,  7,  8,
                    2,  2,  6,  4, 10,  6, 14,  8,
                    3,  6,  3, 12, 15,  6, 21, 24,
                    4,  4, 12,  4, 20, 12, 28,  8,
                    5, 10, 15, 20,  5, 30, 35, 40,
                    6,  6,  6, 12, 30,  6, 42, 24,
                    7, 14, 21, 28, 35, 42,  7, 56,
                    8,  8, 24,  8, 40, 24, 56,  8};
  double xval[] = {1.4, 2.3, 4.7, 3.3, 7.5, 6.6, 5.1};
  double yval[] = {1.0, 1.8, 1.9, 2.5, 2.7, 4.1, 3.3};

  /* results computed using GSL 1D cubic interpolation twice */
  double zval[] = { 1.4, 3.11183531264736, 8.27114315792559, 5.03218982537718,
                    22.13230634702637, 23.63206834997871, 17.28553080971182 };
  size_t xsize = sizeof(xarr) / sizeof(xarr[0]);
  size_t ysize = sizeof(yarr) / sizeof(yarr[0]);
  size_t test_size = sizeof(xval) / sizeof(xval[0]);

  status = test_interp2d(xarr, yarr, zarr, xsize, ysize, xval, yval, zval,
                         NULL, NULL, NULL, NULL, NULL, test_size,
                         gsl_interp2d_bicubic);
  gsl_test(status, "bicubic interpolation on nonlinear symmetric function");

  return status;
}

/* This function contributed by Andrew W. Steiner <awsteiner@gmail.com> */
static int
test_bicubic_nonlinear_nonsq()
{
  int status;
  double xarr[] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0};
  double yarr[] = {1.0, 4.0, 6.0, 8.0, 10.0, 12.0, 14.0, 16.0};
  double zarr[] = { 1,  2,  3,  4,  5,  6,  7,  8, 9, 10,
                    2,  2,  6,  4, 10,  6, 14,  8, 11, 12,
                    3,  6,  3, 12, 15,  6, 21, 24, 13, 14,
                    4,  4, 12,  4, 20, 12, 28,  8, 15, 16,
                    5, 10, 15, 20,  5, 30, 35, 40, 17, 18,
                    6,  6,  6, 12, 30,  6, 42, 24, 19, 20,
                    7, 14, 21, 28, 35, 42,  7, 56, 21, 22,
                    8,  8, 24,  8, 40, 24, 56,  8, 23, 24};
  double xval[] = {1.4, 2.3, 9.7, 3.3, 9.5, 6.6, 5.1};
  double yval[] = {1.0, 1.8, 1.9, 2.5, 2.7, 4.1, 3.3};

  /* results computed using GSL 1D cubic interpolation twice */
  double zval[] = { 1.4, 2.46782030941187003, 10.7717721621846465,
                    4.80725067958096375, 11.6747032398627297,
                    11.2619968682970111, 9.00168877916872567};
  size_t xsize = sizeof(xarr) / sizeof(xarr[0]);
  size_t ysize = sizeof(yarr) / sizeof(yarr[0]);
  size_t test_size = sizeof(xval) / sizeof(xval[0]);

  status = test_interp2d(xarr, yarr, zarr, xsize, ysize, xval, yval, zval,
                         NULL, NULL, NULL, NULL, NULL, test_size,
                         gsl_interp2d_bicubic);
  gsl_test(status, "bicubic interpolation on nonlinear symmetric function");

  return status;
}

/* runs all the tests */
int
test_interp2d_main(void)
{
  int status = 0;

  status += test_bilinear_symmetric();
  status += test_bilinear_asymmetric_z();
  status += test_bicubic();
  status += test_bicubic_nonlinear();
  status += test_bicubic_nonlinear_nonsq();

  return status;
}


#ifndef __Rand_H__
#define __Rand_H__

#define RAND Rand::Instance()

#include <gsl/gsl_rng.h>                                    // GSL random number generator
#include <gsl/gsl_randist.h>

/*
 * Rand Singleton - interface to GSL rng (random number generator)
 *
 * Singletons and global variables are sometimes frowned-upon, but doing it this
 * way means the objects don't need to be passed around to all and sundry.
 * I think convenience and clarity sometimes trump dogma.
 */

class Rand {

private:

    Rand() { m_Rng = NULL; };
    Rand(Rand const&) = delete;
    Rand& operator = (Rand const&) = delete;

    static Rand*      m_Instance;

    gsl_rng*          m_Rng;                                                                           // GSL random number generator

    unsigned long int m_Seed;

public:

    static Rand*  Instance();

    void          Initialise();
    void          Free();

    unsigned long int CurrentSeed()                    { return m_Seed; }
    unsigned long int DefaultSeed()                    { return gsl_rng_default_seed; }
    unsigned long int Seed(const unsigned long p_Seed) { gsl_rng_set(m_Rng, p_Seed); m_Seed = p_Seed; return p_Seed; }

    double        Random();
    double        Random(const double p_Lower, const double p_Upper);
    int           RandomInt(const int p_Lower, const int p_Upper);
    int           RandomInt(const int p_Upper) { return p_Upper < 0 ? 0 : RandomInt(0, p_Upper); }
    double        RandomGaussian(const double p_Sigma);
};

#endif // __Rand_H__


/*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
**                                                                                **
** This file forms part of the Underworld geophysics modelling application.         **
**                                                                                **
** For full license and copyright information, please refer to the LICENSE.md file  **
** located at the project root, or contact the authors.                             **
**                                                                                **
**~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*/
#if 0
/*

Performs y = ( G^T G )^{-1} ( G^T K G ) ( G^T G )^{-1} x

*/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include <petsc.h>
#include <petscmat.h>
#include <petscvec.h>
#include <petscksp.h>
#include <petscpc.h>

#include "common-driver-utils.h"

#include <petscversion.h>
#if ( (PETSC_VERSION_MAJOR >= 3) && (PETSC_VERSION_MINOR >=3) )
  #if (PETSC_VERSION_MINOR >=6)
     #include "petsc/private/pcimpl.h"
     #include "petsc/private/kspimpl.h"
  #else
     #include "petsc-private/pcimpl.h"
     #include "petsc-private/kspimpl.h"
  #endif
#else
  #include "private/pcimpl.h"
  #include "private/kspimpl.h"
#endif

#include "pc_ScaledGtKG.h"

#include <StGermain/StGermain.h>
#include <StgDomain/StgDomain.h>

#define PCTYPE_SCGtKG "scgtkg"

/* private data */

typedef struct {
	Mat         B, Bt, F, C;   /* Bt \in [M x N], F \in [M x M] */
	Vec         X1,X2, Y1,Y2;  /* the scaling vectors */
	Vec         s,t,X;         /* s \in [M], t \in [N], X \in [M] */
	KSP         ksp_BBt;       /* The user MUST provide this operator */
	PetscTruth  BBt_has_cnst_nullspace;
	PetscTruth  monitor_activated;
} _PC_SC_GtKG;

typedef _PC_SC_GtKG* PC_SC_GtKG;

/* private prototypes */
PetscErrorCode BSSCR_PCApply_ScGtKG( PC pc, Vec x, Vec y );
PetscErrorCode BSSCR_PCApplyTranspose_ScGtKG( PC pc, Vec x, Vec y );
PetscErrorCode BSSCR_PCSetUp_GtKG( PC pc );

PetscErrorCode BSSCR_BSSCR_pc_error_ScGtKG( PC pc, const char func_name[] )
{
	const PCType type;
	PCGetType( pc, &type );

	if( strcmp(type,PCTYPE_SCGtKG)!=0 ) {
		printf("Error(%s): PC type (%s) should be scgtkg. \n",func_name, type );
		PetscFinalize();
		exit(0);
	}
	PetscFunctionReturn(0);
}

void BSSCR_BSSCR_get_number_nonzeros_AIJ_ScGtKG( Mat A, PetscInt *nnz )
{
	MatInfo info;

	MatGetInfo( A, MAT_GLOBAL_SUM, &info );
	*nnz = info.nz_used;

}

PetscErrorCode BSSCR_PCScGtKGBBtContainsConstantNullSpace( PC pc, PetscTruth *has_cnst_nullsp )
{
	PC_SC_GtKG    ctx = (PC_SC_GtKG)pc->data;
	PetscInt N;
	PetscScalar sum;
	PetscReal nrm;
	Vec l,r;
	Mat BBt,A;

	Stg_KSPGetOperators( ctx->ksp_BBt, &BBt, PETSC_NULL, PETSC_NULL );
	A = BBt;

	MatGetVecs( A, &r, &l ); // l = A r

	VecGetSize(r,&N);
	sum  = 1.0/N;
	VecSet(r,sum);

	/* scale */
	VecPointwiseMult( r, r, ctx->Y2 );
	/* {l} = [A] {r} */
	MatMult( A,r, l );
	/* scale */
	VecPointwiseMult( l, l, ctx->X2 );

	VecNorm(l,NORM_2,&nrm);
	if (nrm < 1.e-7) {
		*has_cnst_nullsp = PETSC_TRUE;
	}
	else {
		*has_cnst_nullsp = PETSC_FALSE;
	}

	Stg_VecDestroy(&l);
	Stg_VecDestroy(&r);

	PetscFunctionReturn(0);
}

/*
I should not modify setup called!!
This is handled via petsc.
*/
PetscErrorCode BSSCR_PCSetUp_ScGtKG( PC pc )
{
	PC_SC_GtKG    ctx = (PC_SC_GtKG)pc->data;

	if( ctx->F == PETSC_NULL ) {   Stg_SETERRQ( PETSC_ERR_SUP, "gtkg: F not set" );   }
	if( ctx->Bt == PETSC_NULL ) {  Stg_SETERRQ( PETSC_ERR_SUP, "gtkg: Bt not set" );  }

	if(ctx->ksp_BBt==PETSC_NULL) {
		BSSCR_PCScGtKGUseStandardBBtOperator( pc ) ;
	}

	BSSCR_PCScGtKGBBtContainsConstantNullSpace( pc, &ctx->BBt_has_cnst_nullspace );
	if( ctx->BBt_has_cnst_nullspace == PETSC_TRUE ) {
		PetscPrintf( PETSC_COMM_WORLD, "\t* Detected prescence of constant nullspace in BBt-C\n" );
	}

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCDestroy_ScGtKG( PC pc )
{
	PC_SC_GtKG ctx = (PC_SC_GtKG)pc->data;

	if( ctx == PETSC_NULL ) {	PetscFunctionReturn(0); }

	if( ctx->ksp_BBt != PETSC_NULL ) {	Stg_KSPDestroy(&ctx->ksp_BBt );		}

	if( ctx->s != PETSC_NULL ) {		Stg_VecDestroy(&ctx->s );		}
	if( ctx->X != PETSC_NULL ) {		Stg_VecDestroy(&ctx->X );		}
	if( ctx->t != PETSC_NULL ) {		Stg_VecDestroy(&ctx->t );		}

	if( ctx->X1 != PETSC_NULL ) {		Stg_VecDestroy(&ctx->X1 );		}
	if( ctx->X2 != PETSC_NULL ) {		Stg_VecDestroy(&ctx->X2 );		}
	if( ctx->Y1 != PETSC_NULL ) {		Stg_VecDestroy(&ctx->Y1 );		}
	if( ctx->Y2 != PETSC_NULL ) {		Stg_VecDestroy(&ctx->Y2 );		}

	PetscFree( ctx );

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCView_ScGtKG( PC pc, PetscViewer viewer )
{
	PC_SC_GtKG ctx = (PC_SC_GtKG)pc->data;

	PetscViewerASCIIPushTab(viewer); //1

	PetscViewerASCIIPrintf( viewer, "gtkg-ksp \n" );
	PetscViewerASCIIPrintf(viewer,"---------------------------------\n");
	PetscViewerASCIIPushTab(viewer);
		KSPView( ctx->ksp_BBt, viewer );
	PetscViewerASCIIPopTab(viewer);
	PetscViewerASCIIPrintf(viewer,"---------------------------------\n");

	PetscViewerASCIIPopTab(viewer); //1

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCRBSSCR_Lp_monitor_ScGtKG( KSP ksp, PetscInt index )
{
	PetscInt max_it;
	PetscReal rnorm;
	KSPConvergedReason reason;

	KSPGetIterationNumber( ksp, &max_it );
	KSPGetResidualNorm( ksp, &rnorm );
	KSPGetConvergedReason( ksp, &reason );
	if (reason >= 0) {
		PetscPrintf(((PetscObject)ksp)->comm,"\t<Lp(%d)>: Linear solve converged. its.=%.4d ; |r|=%5.5e ; Reason=%s\n",
				index, max_it, rnorm, KSPConvergedReasons[reason] );
	} else {
		PetscPrintf(((PetscObject)ksp)->comm,"\t<Lp(%d)>: Linear solve did not converge. its.=%.4d ; |r|=%5.5e ; Residual reduction=%2.2e ; Reason=%s\n",
				index, max_it, rnorm, (ksp->rnorm0/rnorm), KSPConvergedReasons[reason]);
	}

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCScBFBTSubKSPMonitor( KSP ksp, PetscInt index, PetscLogDouble time )
{
	PetscInt max_it;
	PetscReal rnorm;
	KSPConvergedReason reason;

	KSPGetIterationNumber( ksp, &max_it );
	KSPGetResidualNorm( ksp, &rnorm );
	KSPGetConvergedReason( ksp, &reason );
	PetscPrintf(((PetscObject)ksp)->comm,"    PCScBFBTSubKSP (%d): %D Residual norm; r0 %12.12e, r %12.12e: Reason %s: Time %5.5e \n",
			index, max_it, ksp->rnorm0, rnorm, KSPConvergedReasons[reason], time );

	PetscFunctionReturn(0);
}

/*
Performs y <- S*^{-1} x
  S*^{-1} = ( B' Bt' )^{-1} B' F' Bt' ( B' Bt' )^{-1}
where
  F'  = X1 F Y1
  Bt' = X1 Bt Y2
  B'  = X2 B Y1

Thus, S*^{-1} = [ X2 B Y1 X1 Bt Y2 ]^{-1} . [ X2 B Y1 . X1 F Y1 . X1 Bt Y2 ] . [ X2 B Y1 X1 Bt Y2 ]^{-1}
              = Y2^{-1} ksp_BBt X2^{-1} . [ B' F' Bt' ] . Y2^{-1} ksp_BBt X2^{-1}
              = Y2^{-1} ksp_BBt . [ B Y1 . X1 F Y1 . X1 Bt ] . ksp_BBt X2^{-1}
*/
PetscErrorCode BSSCR_PCApply_ScGtKG( PC pc, Vec x, Vec y )
{
	PC_SC_GtKG ctx = (PC_SC_GtKG)pc->data;

	KSP ksp;
	Mat F, Bt;
	Vec s,t,X;
	PetscLogDouble t0,t1;

	ksp = ctx->ksp_BBt;
	F = ctx->F;
	Bt = ctx->Bt;
	s = ctx->s;
	t = ctx->t;
	X = ctx->X;

	/* Apply scaled Poisson operator */
	/* scale x */
	/* ========================================================
	     NOTE:
	       I THINK TO OMIT THESE AS WE WANT TO UNSCALE THE
	        PRECONDITIONER AS S IN THIS CASE IS NOT SCALED
	======================================================== */
//	VecPointwiseDivide( x, x, ctx->X2 ); /* x <- x/X2 */  /* NEED TO BE SURE */

	if( ctx->BBt_has_cnst_nullspace == PETSC_TRUE ) {
	  BSSCR_VecRemoveConstNullspace( x, PETSC_NULL );
	}
	PetscGetTime(&t0);
	KSPSolve( ksp, x, t ); /* t <- GtG_inv x */
	PetscGetTime(&t1);
	if (ctx->monitor_activated) {
		BSSCR_PCScBFBTSubKSPMonitor(ksp,1,(t1-t0));
	}

	/* Apply Bt */
	MatMult( Bt, t, s ); /* s <- G t */
	VecPointwiseMult( s, s, ctx->X1 ); /* s <- s * X1 */

	/* Apply F */
	VecPointwiseMult( s, s, ctx->Y1 ); /* s <- s * Y1 */
	MatMult( F, s, X ); /* X <- K s */
	VecPointwiseMult( X, X, ctx->X1 ); /* X <- X * X1 */

	/* Apply B */
	VecPointwiseMult( X, X, ctx->Y1 ); /* X <- X * Y1 */
	MatMultTranspose( Bt, X, t ); /* t <- Gt X */

	if( ctx->BBt_has_cnst_nullspace == PETSC_TRUE ) {
		BSSCR_VecRemoveConstNullspace( t, PETSC_NULL );
	}
	PetscGetTime(&t0);
	KSPSolve( ksp, t, y ); /* y <- GtG_inv t */
	PetscGetTime(&t1);
	if (ctx->monitor_activated) {
		BSSCR_PCScBFBTSubKSPMonitor(ksp,2,(t1-t0));
	}

	VecPointwiseMult( y, y, ctx->Y2 ); /* y <- y/Y2 */

	/* undo modification made to x on entry */
//	VecPointwiseMult( x, x, ctx->X2 ); /* x <- x/X2 */ /* NEED TO BE SURE */

	PetscFunctionReturn(0);
}

/*
Performs y <- S*^{-1} x
  S*^{-1} = ( B' Bt' )^{-1} B' F' Bt' - C' ( B' Bt' )^{-1}
where
  F'  = X1 F Y1
  Bt' = X1 Bt Y2
  B'  = X2 B Y1
  C'  = X2 C Y2

Thus, S*^{-1} = [ X2 B Y1 X1 Bt Y2 ]^{-1} . [ X2 B Y1 . X1 F Y1 . X1 Bt Y2 - X2 C Y2 ] . [ X2 B Y1 X1 Bt Y2 ]^{-1}
              = Y2^{-1} ksp_BBt X2^{-1} . [ B' F' Bt' - C' ] . Y2^{-1} ksp_BBt X2^{-1}
              = Y2^{-1} ksp_BBt . [ B Y1 . X1 F Y1 . X1 Bt - C ] . ksp_BBt X2^{-1}
*/
PetscErrorCode BSSCR_BSSCR_PCApply_ScGtKG_C( PC pc, Vec x, Vec y )
{
	PC_SC_GtKG ctx = (PC_SC_GtKG)pc->data;

	KSP ksp;
	Mat F, Bt,C;
	Vec s,t,X;
	PetscLogDouble t0,t1;

	ksp = ctx->ksp_BBt;
	F = ctx->F;
	Bt = ctx->Bt;
	C = ctx->C;
	s = ctx->s;
	t = ctx->t;
	X = ctx->X;

	/* Apply scaled Poisson operator */
	/* scale x */
	/* ========================================================
	     NOTE:
	       I THINK TO OMIT THESE AS WE WANT TO UNSCALE THE
	        PRECONDITIONER AS S IN THIS CASE IS NOT SCALED
	======================================================== */
//	VecPointwiseDivide( x, x, ctx->X2 ); /* x <- x/X2 */  /* NEED TO BE SURE */

	if( ctx->BBt_has_cnst_nullspace == PETSC_TRUE ) {
		BSSCR_VecRemoveConstNullspace( x, PETSC_NULL );
	}
	PetscGetTime(&t0);
	KSPSolve( ksp, x, t ); /* t <- GtG_inv x */
	PetscGetTime(&t1);
	if (ctx->monitor_activated) {
		BSSCR_PCScBFBTSubKSPMonitor(ksp,1,(t1-t0));
	}

	/* Apply Bt */
	MatMult( Bt, t, s ); /* s <- G t */
	VecPointwiseMult( s, s, ctx->X1 ); /* s <- s * X1 */

	/* Apply F */
	VecPointwiseMult( s, s, ctx->Y1 ); /* s <- s * Y1 */
	MatMult( F, s, X ); /* X <- K s */
	VecPointwiseMult( X, X, ctx->X1 ); /* X <- X * X1 */

	/* Apply B */
	VecPointwiseMult( X, X, ctx->Y1 ); /* X <- X * Y1 */
	MatMultTranspose( Bt, X, s ); /* s <- Gt X */

	/* s <- s - C t */
	VecScale( s, -1.0 );
	MatMultAdd( C, t, s, s );
	VecScale( s, -1.0 );

	if( ctx->BBt_has_cnst_nullspace == PETSC_TRUE ) {
		BSSCR_VecRemoveConstNullspace( s, PETSC_NULL );
	}
	PetscGetTime(&t0);
	KSPSolve( ksp, s, y ); /* y <- GtG_inv s */
	PetscGetTime(&t1);
	if (ctx->monitor_activated) {
		BSSCR_PCScBFBTSubKSPMonitor(ksp,2,(t1-t0));
	}

	VecPointwiseMult( y, y, ctx->Y2 ); /* y <- y/Y2 */

	/* undo modification made to x on entry */
//	VecPointwiseMult( x, x, ctx->X2 ); /* x <- x/X2 */ /* NEED TO BE SURE */

	PetscFunctionReturn(0);
}

/* Need to check this one if correct */
/*
S^{-1} = ( G^T G )^{-1} G^T K G ( G^T G )^{-1}
       = A C A
S^{-T} = A^T (A C)^T
       = A^T C^T A^T, but A = G^T G which is symmetric
       = A C^T A
       = A G^T ( G^T K )^T A
       = A G^T K^T G A

*/
PetscErrorCode BSSCR_PCApplyTranspose_ScGtKG( PC pc, Vec x, Vec y )
{
	PC_SC_GtKG ctx = (PC_SC_GtKG)pc->data;

	KSP ksp;
	Mat F, Bt;
	Vec s,t,X;
	PetscLogDouble t0,t1;

	ksp = ctx->ksp_BBt;
	F = ctx->F;
	Bt = ctx->Bt;
	s = ctx->s;
	t = ctx->t;
	X = ctx->X;

	/* Apply scaled Poisson operator */
	/* scale x */
	/* ========================================================
	     NOTE:
	       I THINK TO OMIT THESE AS WE WANT TO UNSCALE THE
	        PRECONDITIONER AS S IN THIS CASE IS NOT SCALED
	======================================================== */
//	VecPointwiseDivide( x, x, ctx->X2 ); /* x <- x/X2 */  /* NEED TO BE SURE */

	if( ctx->BBt_has_cnst_nullspace == PETSC_TRUE ) {
		BSSCR_VecRemoveConstNullspace( x, PETSC_NULL );
	}
	PetscGetTime(&t0);
	KSPSolveTranspose( ksp, x, t ); /* t <- GtG_inv x */
	PetscGetTime(&t1);
	if (ctx->monitor_activated) {
		BSSCR_PCScBFBTSubKSPMonitor(ksp,1,(t1-t0));
	}

	/* Apply Bt */
	MatMult( Bt, t, s ); /* s <- G t */
	VecPointwiseMult( s, s, ctx->X1 ); /* s <- s * X1 */

	/* Apply F */
	VecPointwiseMult( s, s, ctx->Y1 ); /* s <- s * Y1 */
	MatMultTranspose( F, s, X ); /* X <- K s */
	VecPointwiseMult( X, X, ctx->X1 ); /* X <- X * X1 */

	/* Apply B */
	VecPointwiseMult( X, X, ctx->Y1 ); /* X <- X * Y1 */
	MatMultTranspose( Bt, X, t ); /* t <- Gt X */

	if( ctx->BBt_has_cnst_nullspace == PETSC_TRUE ) {
		BSSCR_VecRemoveConstNullspace( t, PETSC_NULL );
	}
	PetscGetTime(&t0);
	KSPSolveTranspose( ksp, t, y ); /* y <- GtG_inv t */
	PetscGetTime(&t1);
	if (ctx->monitor_activated) {
		BSSCR_PCScBFBTSubKSPMonitor(ksp,2,(t1-t0));
	}

	VecPointwiseMult( y, y, ctx->Y2 ); /* y <- y/Y2 */

	/* undo modification made to x on entry */
//	VecPointwiseMult( x, x, ctx->X2 ); /* x <- x/X2 */ /* NEED TO BE SURE */

	PetscFunctionReturn(0);
}

/*
Performs y <- S^{-1} x
S^{-1} = ( G^T Di G )^{-1} G^T Di K Di G ( G^T Di G )^{-1}
where Di = diag(M)^{-1}
*/

/*
Only the options related to GtKG should be set here.
*/
PetscErrorCode BSSCR_PCSetFromOptions_ScGtKG( PC pc )
{
	PC_SC_GtKG ctx = (PC_SC_GtKG)pc->data;
	PetscTruth ivalue, flg;

	if(ctx->ksp_BBt!=PETSC_NULL) {
		PetscOptionsGetTruth( PETSC_NULL, "-pc_gtkg_monitor", &ivalue, &flg );
		BSSCR_PCScGtKGSetSubKSPMonitor( pc, ivalue );

	}

	PetscFunctionReturn(0);
}

/* ---- Exposed functions ---- */

PetscErrorCode BSSCR_PCCreate_ScGtKG( PC pc )
{
	PC_SC_GtKG      pc_data;
	PetscErrorCode  ierr;

	/* create memory for ctx */
	ierr = Stg_PetscNew( _PC_SC_GtKG,&pc_data);CHKERRQ(ierr);

	/* init ctx */
	pc_data->F   = PETSC_NULL;
	pc_data->Bt  = PETSC_NULL;
	pc_data->B   = PETSC_NULL;
	pc_data->BBt_has_cnst_nullspace = PETSC_FALSE;
	pc_data->ksp_BBt   = PETSC_NULL;
	pc_data->monitor_activated = PETSC_FALSE;

	pc_data->X1 = PETSC_NULL;
	pc_data->X2 = PETSC_NULL;
	pc_data->Y1 = PETSC_NULL;
	pc_data->Y2 = PETSC_NULL;

	pc_data->s = PETSC_NULL;
	pc_data->t = PETSC_NULL;
	pc_data->X = PETSC_NULL;

	/* set ctx onto pc */
	pc->data  = (void*)pc_data;

	ierr = PetscLogObjectMemory(pc,sizeof(_PC_SC_GtKG));CHKERRQ(ierr);

	/* define operations */
	pc->ops->setup   = BSSCR_PCSetUp_ScGtKG;
	pc->ops->view    = BSSCR_PCView_ScGtKG;
	pc->ops->destroy = BSSCR_PCDestroy_ScGtKG;
	pc->ops->setfromoptions = BSSCR_PCSetFromOptions_ScGtKG;

	pc->ops->apply          = BSSCR_PCApply_ScGtKG;
	pc->ops->applytranspose = BSSCR_PCApplyTranspose_ScGtKG;

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCScGtKGGetScalings( PC pc, Vec *X1, Vec *X2, Vec *Y1, Vec *Y2 )
{
	PC_SC_GtKG ctx = (PC_SC_GtKG)pc->data;

	BSSCR_BSSCR_pc_error_ScGtKG( pc, __func__ );

	if( X1 != PETSC_NULL ) {  *X1 = ctx->X1;  }
	if( X2 != PETSC_NULL ) {  *X2 = ctx->X2;  }

	if( Y1 != PETSC_NULL ) {  *Y1 = ctx->Y1;  }
	if( Y2 != PETSC_NULL ) {  *Y2 = ctx->Y2;  }

	PetscFunctionReturn(0);
}

/*
F & Bt must different to PETSC_NULL
B may be PETSC_NULL
C can be PETSC_NULL
*/
PetscErrorCode BSSCR_PCScGtKGSetOperators( PC pc, Mat F, Mat Bt, Mat B, Mat C )
{
	PC_SC_GtKG ctx = (PC_SC_GtKG)pc->data;
	BSSCR_BSSCR_pc_error_ScGtKG( pc, __func__ );

	ctx->F  = F;
	ctx->Bt = Bt;
	ctx->B  = B;
	ctx->C  = C;

	if( C != PETSC_NULL ) {
		pc->ops->apply          = BSSCR_BSSCR_PCApply_ScGtKG_C;
		pc->ops->applytranspose = PETSC_NULL;
	}

	/* Create vectors */
	if( ctx->s == PETSC_NULL ) {  MatGetVecs( ctx->F, &ctx->s, PETSC_NULL );  }
	if( ctx->X == PETSC_NULL ) {  MatGetVecs( ctx->F, PETSC_NULL, &ctx->X );  }
	if( ctx->t == PETSC_NULL ) {  MatGetVecs( ctx->Bt, &ctx->t, PETSC_NULL ); }

	if( ctx->F == PETSC_NULL ) {   Stg_SETERRQ( PETSC_ERR_SUP, "gtkg: F not set" );   }
	if( ctx->Bt == PETSC_NULL ) {  Stg_SETERRQ( PETSC_ERR_SUP, "gtkg: Bt not set" );  }

	if( ctx->X1 == PETSC_NULL ) {  MatGetVecs( ctx->F, &ctx->X1, PETSC_NULL );  }
	if( ctx->Y1 == PETSC_NULL ) {  MatGetVecs( ctx->F, &ctx->Y1, PETSC_NULL );  }
	if( ctx->X2 == PETSC_NULL ) {  MatGetVecs( ctx->Bt, &ctx->X2, PETSC_NULL ); }
	if( ctx->Y2 == PETSC_NULL ) {  MatGetVecs( ctx->Bt, &ctx->Y2, PETSC_NULL ); }

	VecSet( ctx->X1, 1.0 );
	VecSet( ctx->Y1, 1.0 );
	VecSet( ctx->X2, 1.0 );
	VecSet( ctx->Y2, 1.0 );

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCScGtKGAttachNullSpace( PC pc )
{
	PC_SC_GtKG ctx = (PC_SC_GtKG)pc->data;
	MatNullSpace nsp;

	BSSCR_BSSCR_pc_error_ScGtKG( pc, __func__ );

	/* Attach a null space */
	MatNullSpaceCreate( PETSC_COMM_WORLD, PETSC_TRUE, PETSC_NULL, PETSC_NULL, &nsp );
#if ( (PETSC_VERSION_MAJOR >= 3) && (PETSC_VERSION_MINOR <6) )
	KSPSetNullSpace( ctx->ksp_BBt, nsp );
#else
    Mat A;
    KSPGetOperators(ctx->ksp_BBt,&A,NULL);//Note: DOES NOT increase the reference counts of the matrix, so you should NOT destroy them.
    MatSetNullSpace( A, nsp);
#endif
	/*
	NOTE: This does NOT destroy the memory for nsp, it just decrements the nsp->refct, so that
	the next time MatNullSpaceDestroy() is called, the memory will be released. The next time this
	is called will be by KSPDestroy();
	*/
	MatNullSpaceDestroy( nsp );

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCScGtKGGetKSP( PC pc, KSP *ksp )
{
	PC_SC_GtKG ctx = (PC_SC_GtKG)pc->data;

	BSSCR_BSSCR_pc_error_ScGtKG( pc, __func__ );

	if( ksp != PETSC_NULL ) {
		(*ksp) = ctx->ksp_BBt;
	}

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCScGtKGSetKSP( PC pc, KSP ksp )
{
	PC_SC_GtKG ctx = (PC_SC_GtKG)pc->data;

	BSSCR_BSSCR_pc_error_ScGtKG( pc, __func__ );

	if( ctx->ksp_BBt != PETSC_NULL ) {
		Stg_KSPDestroy(&ctx->ksp_BBt);
	}

	PetscObjectReference( (PetscObject)ksp );
	ctx->ksp_BBt = ksp;

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCScGtKGSetSubKSPMonitor( PC pc, PetscTruth flg )
{
	PC_SC_GtKG ctx = (PC_SC_GtKG)pc->data;

	BSSCR_BSSCR_pc_error_ScGtKG( pc, __func__ );

	ctx->monitor_activated = flg;

	PetscFunctionReturn(0);
}

PetscErrorCode BSSCR_PCScGtKGUseStandardScaling( PC pc )
{
	PC_SC_GtKG ctx = (PC_SC_GtKG)pc->data;
	Mat K,G,D,C;
	Vec rG;
	PetscScalar rg2, rg, ra;
	PetscInt N;
	Vec rA, rC;
	Vec L1,L2, R1,R2;

	BSSCR_BSSCR_pc_error_ScGtKG( pc, __func__ );

	L1 = ctx->X1;
	L2 = ctx->X2;

	R1 = ctx->Y1;
	R2 = ctx->Y2;

	rA = L1;
	rC = L2;

	K = ctx->F;
	G = ctx->Bt;
	D = ctx->B;
	C = ctx->C;

	VecDuplicate( rA, &rG );

	/* Get magnitude of K */
	MatGetRowMax( K, rA, PETSC_NULL );

	VecSqrt( rA );
	VecReciprocal( rA );

	VecDot( rA,rA, &ra );
	VecGetSize( rA, &N );
	ra = PetscSqrtScalar( ra/N );

	/* Get magnitude of G */
	MatGetRowMax( G, rG, PETSC_NULL );

	VecDot( rG, rG, &rg2 );
	VecGetSize( rG, &N );
	rg = PetscSqrtScalar(rg2/N);
	//      printf("rg = %f \n", rg );

	VecSet( rC, 1.0/(rg*ra) );

	Stg_VecDestroy(&rG );

	VecCopy( L1, R1 );
	VecCopy( L2, R2 );

	PetscFunctionReturn(0);
}

/*
Builds
  B Y1 X1 Bt
and creates a ksp when C=0, otherwise it builds
  B Y1 X1 Bt - C
*/
PetscErrorCode BSSCR_PCScGtKGUseStandardBBtOperator( PC pc )
{
	PC_SC_GtKG ctx = (PC_SC_GtKG)pc->data;
	PetscReal  fill;
	Mat        diag_mat,C;
	Vec        diag;
	PetscInt   M,N, m,n;
	MPI_Comm   comm;
	PetscInt   nnz_I, nnz_G;
	MatType    mtype;
	const char *prefix;
	Mat BBt;
	KSP ksp;
	PetscTruth ivalue, flg, has_cnst_nullsp;

	BSSCR_BSSCR_pc_error_ScGtKG( pc, __func__ );

	/* Assemble BBt */
	MatGetSize( ctx->Bt, &M, &N );
	MatGetLocalSize( ctx->Bt, &m, &n );

	MatGetVecs( ctx->Bt, PETSC_NULL, &diag );

	/* Define diagonal matrix Y1 X1 */
	VecPointwiseMult( diag, ctx->Y1, ctx->X1 );

	PetscObjectGetComm( (PetscObject)ctx->F, &comm );
	MatCreate( comm, &diag_mat );
	MatSetSizes( diag_mat, m,m , M, M );
#if (((PETSC_VERSION_MAJOR==3) && (PETSC_VERSION_MINOR>=3)) || (PETSC_VERSION_MAJOR>3) )
        MatSetUp(diag_mat);
#endif
	MatGetType( ctx->Bt, &mtype );
	MatSetType( diag_mat, mtype );

	MatDiagonalSet( diag_mat, diag, INSERT_VALUES );

	/* Build operator B Y1 X1 Bt */
	BSSCR_BSSCR_get_number_nonzeros_AIJ_ScGtKG( diag_mat, &nnz_I );
	BSSCR_BSSCR_get_number_nonzeros_AIJ_ScGtKG( ctx->Bt, &nnz_G );
	/*
	Not sure the best way to estimate the fill factor.
	BBt is a laplacian on the pressure space.
	This might tell us something useful...
	*/
	fill = (PetscReal)(nnz_G)/(PetscReal)( nnz_I );
	MatPtAP( diag_mat, ctx->Bt, MAT_INITIAL_MATRIX, fill, &BBt );

	Stg_MatDestroy(&diag_mat );
	Stg_VecDestroy(&diag );

	C = ctx->C;
	if( C !=PETSC_NULL ) {
		MatAXPY( BBt, -1.0, C, DIFFERENT_NONZERO_PATTERN );
	}

	/* Build the solver */
	KSPCreate( ((PetscObject)pc)->comm, &ksp );

	Stg_KSPSetOperators( ksp, BBt, BBt, SAME_NONZERO_PATTERN );

	PCGetOptionsPrefix( pc,&prefix );
	KSPSetOptionsPrefix( ksp, prefix );
	KSPAppendOptionsPrefix( ksp, "pc_gtkg_" ); /* -pc_GtKG_ksp_type <type>, -ksp_GtKG_pc_type <type> */

	BSSCR_PCScGtKGSetKSP( pc, ksp );

	BSSCR_MatContainsConstNullSpace( BBt, NULL, &has_cnst_nullsp );
	if( has_cnst_nullsp == PETSC_TRUE ) {
		BSSCR_PCScGtKGAttachNullSpace( pc );
	}

	PetscOptionsGetTruth( PETSC_NULL, "-pc_gtkg_monitor", &ivalue, &flg );
	BSSCR_PCScGtKGSetSubKSPMonitor( pc, ivalue );

	Stg_KSPDestroy(&ksp);
	Stg_MatDestroy(&BBt);

	PetscFunctionReturn(0);
}

#endif


#include <stdio.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

void mcmc
(
 int &mcID,
 int &seed,
 int &imh,
 int &vmh,
 int &neighborPast,
 int &n_neighbor,
 int &ptAccpt,
 double &pllh,
 double *jump,
 struct FXP &fxp,
 struct PAR &par_a,
 struct PAR &par_c,
 struct RND &rnd,
 struct VAR &var,
 struct DATA &data,
 struct DIAG &diag,
 struct HIST &hist,
 struct INCV &incv,
 struct POST &post,
 struct PRED &pred,
 gsl_rng *rng,
 FILE *outp
);

void logQuasiposterior
(
 //=======================================================
 double &llh,
 struct FXP &fxp,
 struct PAR &par,
 struct RND &rnd,
 struct VAR &var,
 struct DATA &data,
 struct DIAG &diag,
 struct INCV &incv,
 struct PRED &pred
//=======================================================
);

void logQuasiposterior
(
 //=======================================================
 double &llh,
 double *expDeltaIn,
 struct PAR &par,
 struct VAR &var
//=======================================================
);

void rollbackTest
(
 int &imh,
 int &vmh,
 int &n_neighbor,
 int &neighborPast,
 double &pllh,
 double &pllhbackup,
 struct FXP &fxp,
 struct PAR &par,
 struct VAR &var,
 struct DATA &data,
 struct DIAG &diag,
 struct HIST &hist,
 struct INCV &incv,
 struct PRED &pred
);