/*
 * 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


/*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
**                                                                                **
** 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 "aXe_grism.h"
#include <gsl/gsl_spline.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_sort_vector.h>
#include "fringe_conf.h"
#include "fringe_model.h"
#include "fringe_utils.h"

#define MIN(x,y) (((x)<(y))?(x):(y))
#define MAX(x,y) (((x)>(y))?(x):(y))

/**
 * Function: fringe_correct_PET
 * Performs fringe corrections to PET pixel lists
 *
 * The main function to compute and perform the fringe
 * correction to a list of PET pixels. for every pixel in
 * the PET list the pixel throughput function is
 * computed, and the relevant information from
 * the fringing model is extracted. Then the fringe
 * amplitude is computed and the count value of the pixel
 * is corrected. Also the PET pixel list is simultaneously
 * parsed and corrected for the fringing.
 *
 * Parameters:
 * @param fconf    - the fringe configuration file
 * @param act_beam - the beam aperture
 * @param obj_pet  - the PET pixel
 * @param bck_pet  - the PET pixel
 */
void
fringe_correct_PET(const fringe_conf *fconf, const beam act_beam,
		   ap_pixel *obj_pet, ap_pixel *bck_pet)
{
  ap_pixel *obj_pix=NULL;
  ap_pixel *bck_pix=NULL;

  //d_point angles;
  pixel_tput *p_tput ;
  gsl_vector **tput_vectors=NULL;

  //int ii=0;
  //int jj=0;
  int index;
  int pix_index;

  double lambda_mean;
  double pixel_ampl;
  double fringe_factor;
  double fringe_tot;

  optical_property *optprops;

  // allocate the pixel throughput structure
  p_tput = alloc_pixel_tput();

  // allocate memory for the optical property structure
  optprops = alloc_optprops_list(fconf);

  // compute the mean wavelength
  lambda_mean = (gsl_vector_get(fconf->fringe_range, 0) +
    (gsl_vector_get(fconf->fringe_range, 1)
     - gsl_vector_get(fconf->fringe_range, 0))/2.0)/1.0e+04;

  // initialize some values in the optical property list
  init_optprops_list(fconf, lambda_mean, optprops);

  // initialize the PET pixel storages
  obj_pix = obj_pet;
  bck_pix = bck_pet;

  fringe_tot = 0.0;
  pix_index = 0;
  // go over the whole PET list
  while (obj_pix->p_x != -1)
    {
      // check whether the pixel has to be fringe corrected
      // at all, based on boundaries given in the configuration
      if ( obj_pix->lambda < gsl_vector_get(fconf->fringe_range, 0)
      	   || obj_pix->lambda > gsl_vector_get(fconf->fringe_range, 1)
      	   || obj_pix->dlambda > fconf->max_dispersion)
      	{

      	  // count up the object PET
      	  obj_pix++;

      	  // count up the background PET
      	  if (bck_pet)
      	    bck_pix++;

      	  // jump to the next PET pixel
      	  continue;
      	}

      // determine the filter throughput values
      //      tput_vectors = evaluate_pixel_throughput(fconf, act_beam, obj_pix);
      // for the ISR with the WFC value
      tput_vectors = get_gauss_throughput(fconf, obj_pix, 80.0);

      // fill the optical thickness of the layers
      // into the structure
      fill_optprops_thickness(fconf->opt_layers, obj_pix->p_x,
			      obj_pix->p_y, optprops);

      pixel_ampl = 0.0;
      for (index=0; index < (int)tput_vectors[0]->size; index++)
	{
	  // fill all information in the optical
	  // property list
	  fill_optprops_all(fconf->opt_layers,
			    gsl_vector_get(tput_vectors[0],index),
			    optprops);

	  // compute and add the contribution at a wavelength
	  pixel_ampl += gsl_vector_get(tput_vectors[1],index)*
	    fringe_contrib_single(optprops, fconf);
	}

      //put together the fringe correction factor
      fringe_factor = fconf->fringe_amp * pixel_ampl + 1.0;
#ifdef DEBUGFCONF
      fprintf(stderr, "Wavelength: %f, dispersion: %f, fringe factor: %f, (x,y): (%i, %i)\n",
	      obj_pix->lambda, obj_pix->dlambda, fringe_factor, obj_pix->p_x, obj_pix->p_y);
#endif

      // correct the object PET
      obj_pix->count /= fringe_factor;
      //obj_pix->count = fringe_factor;
      //fprintf(stdout, "%e ",obj_pix->count);

      fringe_tot += fringe_factor;
      pix_index++;

      // treat the background PET pixel
      if (bck_pet)
	{
	  if (bck_pix->p_x != obj_pix->p_x || bck_pix->p_y != obj_pix->p_y)
	    aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
			 "aXe_FRINGECORR: The sequence of pixels\n"
			 "in the object PET and background PET differs.\n"
			 "Something must be wrong!\n");

	  // correct also the background PET
	  bck_pix->count /= fringe_factor;

	  // count up the background PET
	  bck_pix++;
	}

      // count up the object PET
      obj_pix++;
      //      obj_pix->p_x = -1;
    }

  //  if (pix_index)
  //    fprintf(stdout,"Mean fringe factor: %f\n", fringe_tot/(double)pix_index);

  if (tput_vectors)
    {
      // release the memory in the vectors
      gsl_vector_free(tput_vectors[0]);
      gsl_vector_free(tput_vectors[1]);
      free(tput_vectors);
    }

  // free the optical property structure
  free_optprops_list(optprops);

  // free the memory
  free_pixel_tput(p_tput);
}

/**
 * Function: evaluate_pixel_throughput
 * Computes the wavelength and filter throughputs for a PET pixel.
 *
 * The function computes and returns two vector, one with wavelength
 * steps and the second with the pixel throughputs at those wavelengths.
 * This data is computed to a single PET pixel according to the settings
 * in the fringe configuration file.
 *
 * Parameters:
 * @param fconf    - the fringe configuration file
 * @param act_beam - the beam aperture
 * @param obj_pix  - the PET pixel
 *
 * Returns:
 * @return double_vector - the wavelength and filter throughput steps
 */
gsl_vector **
evaluate_pixel_throughput(const fringe_conf *fconf,
			  const beam act_beam, const ap_pixel *obj_pix)
{
  pixel_tput *p_tput ;

  gsl_vector **double_vector;
  gsl_vector *lambda_values;
  gsl_vector *through_values;

  interpolator *interp;

  double stepsize;
  double lambda_act;
  double through_act;
  double through_tot;

  double abs_min;
  double abs_max;

  int index;

  // allocate the pixel throughput structure
  p_tput = alloc_pixel_tput();

  // allocate the space for the vectors
  lambda_values  = gsl_vector_alloc(fconf->num_steps + 1);
  through_values = gsl_vector_alloc(fconf->num_steps + 1);

  // allocate space for the return vector
  double_vector  = (gsl_vector **)malloc(2*sizeof (gsl_vector *));

  compute_pixel_tput(act_beam, obj_pix, p_tput);
#ifdef DEBUGFCONF
  print_pixel_tput(p_tput);
#endif
  // convert the pixel throughput information
  // into an interplator
  interp = create_interp_tput(p_tput);

  abs_min = MAX(interp->xmin, gsl_vector_get(fconf->fringe_range, 0));
  abs_max = MIN(interp->xmax, gsl_vector_get(fconf->fringe_range, 1));

  // compute the steps in wavelength
  stepsize = (abs_max - abs_min)/(double)fconf->num_steps;

  // set the lower wavelength end
  lambda_act = abs_min;

  // loop over the wavelength
  through_tot = 0.0;
  for (index=0; index < fconf->num_steps; index++)
    {
      // set the wavelength value
      gsl_vector_set(lambda_values, index, lambda_act);

      // compute the throughput
      through_act = eval_interp(interp, lambda_act);

      // compute the total throughput
      through_tot += through_act;

      // set the thoughput value
      gsl_vector_set(through_values, index, through_act);

      lambda_act +=  stepsize;
    }

  // treat the last step separately, avoiding
  // to leave the allowed interpolation range
  // due to rounding errors
  gsl_vector_set(lambda_values, fconf->num_steps, abs_max);
  through_act = eval_interp(interp, abs_max);
  through_tot += through_act;
  gsl_vector_set(through_values, fconf->num_steps, through_act);

  for (index=0; index < (int)through_values->size; index++)
    {
      // normalize the filter throughput values
      gsl_vector_set(through_values, index,
		     gsl_vector_get(through_values, index)/through_tot);

      // convert the wavelength from AA to micron
      gsl_vector_set(lambda_values, index,
		     gsl_vector_get(lambda_values, index)*1.0e-04);
    }
#ifdef DEBUGFCONF
  through_tot = 0.0;
  for (index=0; index < through_values->size; index++)
    {
      fprintf(stderr, "Wavelength: %f, Throughput: %f\n",
	      gsl_vector_get(lambda_values, index),
	      gsl_vector_get(through_values, index));
      through_tot += gsl_vector_get(through_values, index);
    }
  fprintf(stderr, "Total throughput: %f\n", through_tot);
#endif

  // build up the output array
  double_vector[0] = lambda_values;
  double_vector[1] = through_values;

  // free the memory of the interpolator
  free_interp(interp);

  // free the memory of the pixel throughput
  free_pixel_tput(p_tput);

  // return the two gsl vectors
  return double_vector;
}

/**
 * Function: get_gauss_throughput
 *
 * Parameters:
 * @param fconf    - the fringe configuration file
 * @param act_beam - the beam aperture
 * @param obj_pix  - the PET pixel
 *
 * Returns:
 * @return double_vector - the wavelength and filter throughput steps
 */
gsl_vector **
get_gauss_throughput(const fringe_conf *fconf, const ap_pixel *obj_pix,
		     const double fwhm)
{
  pixel_tput *p_tput ;

  gsl_vector **double_vector;
  gsl_vector *lambda_values;
  gsl_vector *through_values;

  //interpolator *interp;

  double stepsize;
  double lambda_act;
  double through_act;
  double through_tot;

  double abs_min;
  double abs_max;

  double sigma;
  double arg;

  int index;

  // convert the FWHM into sigma
  sigma = fwhm / 2.35482;

  // allocate the pixel throughput structure
  p_tput = alloc_pixel_tput();

  // allocate the space for the vectors
  lambda_values  = gsl_vector_alloc(fconf->num_steps + 1);
  through_values = gsl_vector_alloc(fconf->num_steps + 1);

  // allocate space for the return vector
  double_vector  = (gsl_vector **)malloc(2*sizeof (gsl_vector *));

  // compute the minimum and maximum wavelength
  abs_min = MAX(obj_pix->lambda - 2.5 * fwhm,
		gsl_vector_get(fconf->fringe_range, 0));
  abs_max = MIN(obj_pix->lambda + 2.5 * fwhm,
		gsl_vector_get(fconf->fringe_range, 1));

  // compute the steps in wavelength
  stepsize = (abs_max - abs_min)/(double)fconf->num_steps;

  // set the lower wavelength end
  lambda_act = abs_min;

  // loop over the wavelength
  through_tot = 0.0;
  for (index=0; index < fconf->num_steps; index++)
    {
      // set the wavelength value
      gsl_vector_set(lambda_values, index, lambda_act);

      // z=(coord-g(2))/g(3)
      // sumgauss=sumgauss+DBLE(g(1)*EXP(-0.5*z**2))
      arg = (lambda_act - obj_pix->lambda) / sigma;

      // compute the throughput
      through_act = exp(-0.5*arg*arg);

      // compute the total throughput
      through_tot += through_act;

      // set the thoughput value
      gsl_vector_set(through_values, index, through_act);

      // increment the wavelength
      lambda_act += stepsize;
    }

  // treat the last step separately, avoiding
  // to leave the allowed interpolation range
  // due to rounding errors
  gsl_vector_set(lambda_values, fconf->num_steps, abs_max);
  arg = (abs_max - obj_pix->lambda) / sigma;
  through_act = exp(-0.5*arg*arg);
  through_tot += through_act;
  gsl_vector_set(through_values, fconf->num_steps, through_act);

  for (index=0; index < (int)through_values->size; index++)
    {
      // normalize the filter throughput values
      gsl_vector_set(through_values, index,
		     gsl_vector_get(through_values, index)/through_tot);

      // convert the wavelength from AA to micron
      gsl_vector_set(lambda_values, index,
		     gsl_vector_get(lambda_values, index)*1.0e-04);
    }

#ifdef DEBUGFCONF
  through_tot = 0.0;
  for (index=0; index < through_values->size; index++)
    {
      fprintf(stderr, "Wavelength: %f, Throughput: %f\n",
	      gsl_vector_get(lambda_values, index),
	      gsl_vector_get(through_values, index));
      through_tot += gsl_vector_get(through_values, index);
    }
  fprintf(stderr, "Total throughput: %f\n", through_tot);
#endif

  // build up the output array
  double_vector[0] = lambda_values;
  double_vector[1] = through_values;

  return double_vector;
}

/**
 * Function: create_interp_tput
 * The function creates and returns an interpolator
 * for a pixel throughput function. The interpolator
 * structure is allocated, data is filled in and
 * finally initialized.
 *
 * Parameters:
 * @param p_tput   - the pixel throughput structure
 *
 * Returns:
 * @return interp - the new interpolator
 */
interpolator *
create_interp_tput(const pixel_tput *p_tput)
{
  interpolator *interp;

  double *xvals;
  double *yvals;

  // allocate memory for the independent values
  xvals = (double *) malloc(4 * sizeof(double));
  if (!xvals) {
    aXe_message (aXe_M_ERROR, __FILE__, __LINE__,
		 "Memory allocation failed");
  }

  // allocate memory for the dependent values
  yvals = (double *) malloc(4 * sizeof(double));
  if (!yvals) {
    aXe_message (aXe_M_ERROR, __FILE__, __LINE__,
		 "Memory allocation failed");
  }

  // set the independent vector values
  xvals[0] = gsl_vector_get(p_tput->lambda_values, 0);
  xvals[1] = gsl_vector_get(p_tput->lambda_values, 1);
  xvals[2] = gsl_vector_get(p_tput->lambda_values, 2);
  xvals[3] = gsl_vector_get(p_tput->lambda_values, 3);

  // set the dependent vector values
  yvals[0] = 0.0;
  yvals[1] = p_tput->tp_max;
  yvals[2] = p_tput->tp_max;
  yvals[3] = 0.0;

  // create the interpolator
  interp = create_interp(4, FILTER_INTERP_TYPE, xvals, yvals);

  // return the interpolator
  return interp;
}

/**
 * Function: compute_pixel_tput
 * The function computes all relevant data to characterize
 * the pixel throughput function for an individual PET pixel.
 * The information is written into a pixel throughput structure.
 *
 * Parameters:
 * @param act_beam - the beam aperture
 * @param obj_pix  - the PET pixel
 * @param p_tput   - the pixel throughput structure
 */
void
compute_pixel_tput(const beam act_beam, const ap_pixel *obj_pix,
		   pixel_tput *p_tput)
{
  d_point angles;

  double theta_2a;
  double theta_3a;

  double theta_1b;
  double theta_2b;

  double sin_theta_2a;
  double sin_theta_3a;

  double sin_theta_1b;
  double cos_theta_1b;
  double sin_theta_2b;

  double dist1;
  double dist2;

  // compute the anngles alpha and beta
  angles = compute_tput_angles(act_beam, obj_pix);

  // the computation of p_min-p3 in the
  // design document
  theta_1b = M_PI_2 + angles.x - angles.y;
  theta_2b = angles.y;

  // compute the sin's and cos's neede
  sin_theta_1b = sin(theta_1b);
  cos_theta_1b = cos(theta_1b);
  sin_theta_2b = sin(theta_2b);

  // compute one delta
  dist2 = obj_pix->dlambda * sin_theta_1b / sin_theta_2b;

  // the computation of p_h-p0 in the
  // design document
  theta_2a = M_PI - angles.y;
  theta_3a = angles.y - angles.x - M_PI_4;

  // next line due to sin(angle) = sin(180-angle)
  sin_theta_2a = sin_theta_2b;
  sin_theta_3a = sin(theta_3a);

  // compute the second delta
  dist1 = obj_pix->dlambda * M_SQRT1_2 * sin_theta_3a / sin_theta_2a;

  // determine the maximum throughput
  p_tput->tp_max = 1.0/MAX(fabs(sin_theta_1b), fabs(cos_theta_1b));

  // put together the distances, using
  // the deltas and the dispersion
  gsl_vector_set(p_tput->lambda_values, 0, obj_pix->lambda - dist1 - dist2);
  gsl_vector_set(p_tput->lambda_values, 1, obj_pix->lambda - dist1);
  gsl_vector_set(p_tput->lambda_values, 2, obj_pix->lambda + dist1);
  gsl_vector_set(p_tput->lambda_values, 3, obj_pix->lambda + dist1 + dist2);

  // sort the distances
  gsl_sort_vector(p_tput->lambda_values);
}

/**
 * Function: compute_tput_angles
 * The function computes the angles which characterize
 * a pixel in the lambda-cross dispersion plane.
 * In detail this is the angle between the lambda-axis
 * and the x-axis, and the angle between the lambda abd
 * the cross dispersion axis.
 * The two angles are computed from the basic pixel
 * and beam data.
 *
 *
 * Parameters:
 * @param act_beam - the beam aperture
 * @param act_pet  - the PET pixel
 *
 * Returns:
 * @return angles - the relevant pixel angles
 */
d_point
compute_tput_angles(const beam act_beam, const ap_pixel *act_pet)
{
  d_point angles;

  // compute the angle 'alpha' between lambda-axis and
  // x-axis
  angles.x = -atan2(act_beam.spec_trace->deriv(act_pet->xi,
					       act_beam.spec_trace->data),1.0);
  // compute the angle beta between lambda and cross dispersion axis
  angles.y = act_beam.orient + angles.x;

  // return the two angles
  return angles;
}

/**
 * Function: alloc_pixel_tput
 * The function allocates and returns memory
 * for a pixel throughput function
 *
 * Returns:
 * @return p_tput -the new allocated pixel throughput structure
 */
pixel_tput *
alloc_pixel_tput()
{
  pixel_tput *p_tput;

  // allcoate the pointer
  p_tput = (pixel_tput *) malloc(sizeof(pixel_tput));

  // allocate the vector in the structure
  p_tput->lambda_values = gsl_vector_alloc(4);

  // return the structure
  return p_tput;
}

/**
 * Function: print_pixel_tput
 * The function prints the content of a pixel throughput
 * structure onto the screen.
 *
 * Parameters:
 * @param p_tput - the pixel throughput structure
 */
void
print_pixel_tput(pixel_tput *p_tput)
{
  fprintf(stdout, "l_min: %f, l_1: %f, l_2: %f, l_max: %f; tp_max: %f\n\n",
	  gsl_vector_get(p_tput->lambda_values,0),
	  gsl_vector_get(p_tput->lambda_values,1),
	  gsl_vector_get(p_tput->lambda_values,2),
	  gsl_vector_get(p_tput->lambda_values,3),
	  p_tput->tp_max);
}

/**
 * Function: free_pixel_tput
 * The function releases the memory allocated
 * in a pixel throughput function.
 *
 * Parameters:
 * @param p_tput - the pixel throughput structure
 */
void
free_pixel_tput(pixel_tput *p_tput)
{
  // free the gsl-vector
  gsl_vector_free(p_tput->lambda_values);

  // free everything
  free(p_tput);

  // set it to NULL
  p_tput = NULL;
}