/* code by: iperetta@ieee.org */
#ifndef MYMATRIX_H_
#define MYMATRIX_H_

#include <iostream>
#include <fstream>
#include <cmath>
#include <vector>
#include <stdexcept>
#include <limits>
#include <lapacke.h>
#include "myfun.h"

using namespace std;

#ifndef MyEPS_
#define MyEPS_
static double EPS = numeric_limits<double>::epsilon();
#endif
// =====================================================================
template<typename T>
struct brace {
	T first, second;
};

template<typename T> brace<T> make_brace(const T &m1, const T &m2) {
	brace<T> ans;
	ans.first = m1;
	ans.second = m2;
	return ans;
}

template<typename T>
struct triplet {
	T first, second, third;
};

template<typename T> triplet<T> make_triplet(const T &m1, const T &m2,
		const T &m3) {
	triplet<T> ans;
	ans.first = m1;
	ans.second = m2;
	ans.third = m3;
	return ans;
}
// =====================================================================
class Matrix;
Matrix column_vector(const vector<double> &vec);
Matrix row_vector(const vector<double> &vec);
Matrix identityMatrix(const size_t &N);
Matrix diagonalMatrix(const vector<double> &vec);

class Matrix { // Matrix A_mXn
	friend Matrix operator+(const double &lhs, const Matrix &rhs);
	friend Matrix operator-(const double &lhs, const Matrix &rhs);
	friend Matrix operator*(const double &lhs, const Matrix &rhs);
	friend Matrix operator/(const double &lhs, const Matrix &rhs);
	friend ostream& operator<<(ostream &os, const Matrix& M);
  protected:
	vector<double> elements;
	void badConvergence(const string &str) const;
	void invalidArgument(const int &errid, const string &str) const;
	void checkDim(const size_t &i_, const string &str = "") const;
	void checkDim(const size_t &i_, const size_t &j_,
		const string &str = "") const;
	void checkSize(const size_t &i_, const size_t &j_,
		const string &str = "") const;
	void ensureSquare(const string &str = "") const;
	void ensureSameSize(const Matrix &B, const string &str = "") const;
	void ensureNonSingularity(const string &str = "") const;
	void ensureVector(const string &str) const;
  public:
	size_t m; // number of rows of the matrix
	size_t n; // number of columns of the matrix
	Matrix(); //constructor
    Matrix(const size_t &m_, const size_t &n_, const double &cns = 0.);
    Matrix(const size_t &m_, const size_t &n_, const double *elem,
		const size_t &sz_elem);
	Matrix(const size_t &m_, const size_t &n_,
		const vector<double> &elem);
    void swap(Matrix &);
    ~Matrix() {};//destructor
    void print() const;
    string str() const;
    double& at(const size_t &i_);
    const double& at(const size_t &i_) const;
    double& at(const size_t &i_, const size_t &j_);
    const double& at(const size_t &i_, const size_t &j_) const;
    vector<double> as_vector() const;
    vector<double> row(const size_t &r_) const;
    void assign2row(const size_t &r_, const double &value);
    void assign2row(const size_t &r_, const vector<double> &values);
    void assign2row(const size_t &r_, const double *values,
		const size_t &sz);
    vector<double> column(const size_t &c_) const;
    void assign2column(const size_t &c_, const double &value);
    void assign2column(const size_t &c_, const vector<double> &values);
    void assign2column(const size_t &c_, const double *values,
		const size_t &sz);
    vector<double> diagonal() const;
    void assign2diagonal(const double &value);
    void assign2diagonal(const vector<double> &values);
    void assign2diagonal(const double *values, const size_t &sz);
	void swap_rows(const size_t &r1, const size_t &r2);
	void swap_columns(const size_t &c1, const size_t &c2);

    void self_concat_row(const double &cte);
    void self_concat_row(const vector<double> &values);
	void self_concat_row(const Matrix &B);
	void self_concat_column(const double &cte);
	void self_concat_column(const vector<double> &values);
	void self_concat_column(const Matrix &B);
	void self_delete_row(const size_t &r_);
	void self_delete_column(const size_t &c_);

	Matrix concat_row(const double &cte) const;
    Matrix concat_row(const vector<double> &values) const;
	Matrix concat_row(const Matrix &B) const;
	Matrix concat_column(const double &cte) const;
	Matrix concat_column(const vector<double> &values) const;
	Matrix concat_column(const Matrix &B) const;
	Matrix delete_row(const size_t &r_) const;
	Matrix delete_column(const size_t &c_) const;

	size_t rank() const;
	double determinant() const;
	double trace() const;
	double norm() const;
	double infinity_norm() const;
	double frobenius_norm() const;
	double condition_number() const;
	double cofactor(const size_t &r_, const size_t &c_) const;
	double highest_eigenvalue() const;
    Matrix transpose() const;
    Matrix adjoint() const;
    Matrix inverse() const;
    brace<Matrix> eigen() const;
    triplet<Matrix> lu_p_factorization() const;
    brace<Matrix> qr_factorization() const;
    brace<Matrix> lq_factorization() const;
    triplet<Matrix> svd() const;
    Matrix pseudoinverse() const;

    Matrix operator+(const double &value) const;
    void operator+=(const double &value);
    Matrix operator-(const double &value) const;
    void operator-=(const double &value);
    Matrix operator*(const double &value) const;
    void operator*=(const double &value);
    Matrix operator/(const double &value) const;
    void operator/=(const double &value);
    Matrix operator+(const Matrix &value) const;
    void operator+=(const Matrix &value);
    Matrix operator-(const Matrix &value) const;
    void operator-=(const Matrix &value);
    Matrix operator*(const Matrix &value) const;
    void operator*=(const Matrix &value);
    bool operator==(const Matrix &B) const;
    bool operator!=(const Matrix &B) const;

    double min_all(const bool &absolute) const;
    double max_all(const bool &absolute) const;

    bool isFullRank() const;
    bool isOrthogonal() const;
    bool isDiagonal() const;
    bool isPositiveDefinite() const;
    bool isSingular() const;
    bool isSymmetric() const;
    bool isSkewSymmetric() const;
    bool isValid() const;

    void adjust_elements(const double &tol);

    void saveAs(const string &namefile) const;
};

Matrix::Matrix() : m(0), n(0) {
}

Matrix::Matrix(const size_t &m_, const size_t &n_, const double &cns) :
		m(m_), n(n_) {
	this->elements.assign(m*n,cns);
}

Matrix::Matrix(const size_t &m_, const size_t &n_, const double *elem,
		const size_t &sz_elem) : m(m_), n(n_) {
	this->checkSize(this->m*this->n, sz_elem,"(CONSTRUCTOR)");
	this->elements.assign(elem, elem+sz_elem);
}

Matrix::Matrix(const size_t &m_, const size_t &n_, const vector<double>
		&elem) : m(m_), n(n_) {
	this->checkSize(this->m*this->n, elem.size(),"(CONSTRUCTOR)");
	this->elements = elem;
}

void Matrix::swap(Matrix &other) {
    // need to swap all data members
    std::swap(this->elements, other.elements);
    std::swap(this->m, other.m);
    std::swap(this->n, other.n);
}
// =====================================================================
void Matrix::badConvergence(const string &str) const {
	string MSG = (str.empty() ? "" : "in ") + str +
		(str.empty() ? "" : ", ") +
		"the algorithm failed to converge.";
	throw std::runtime_error(MSG.c_str());
}

void Matrix::invalidArgument(const int &errid, const string &str)
		const {
	string MSG = (str.empty() ? "" : "in ") + str +
		(str.empty() ? "" : ", ") +
		"invalid argument";
	cerr << "[ " << errid << " ] ";
	throw std::invalid_argument(MSG.c_str());
}

void Matrix::checkDim(const size_t &i_, const string &str) const {
	string MSG = (str.empty() ? "" : "in ") + str +
		(str.empty() ? "" : ", ") +
		"index exceeds matrix dimension";
	if(i_ >= this->elements.size())
		throw std::out_of_range(MSG.c_str());
}

void Matrix::checkDim(const size_t &i_, const size_t &j_,
		const string &str) const {
	string MSG = (str.empty() ? "" : "in ") + str +
		(str.empty() ? "" : ", ") +
		"index exceeds matrix dimension";
	if(i_ >= this->m || j_ >= this->n)
		throw std::out_of_range(MSG.c_str());
}

void Matrix::checkSize(const size_t &i_, const size_t &j_,
		const string &str) 	const {
	string MSG = (str.empty() ? "" : "in ") + str +
		(str.empty() ? "" : ", ") +
		"sizes of matrices don't match";
	if(i_ != j_)
		throw std::invalid_argument(MSG.c_str());
}

void Matrix::ensureSquare(const string &str) const {
	string MSG = (str.empty() ? "" : "in ") + str +
		(str.empty() ? "" : ", ") +
		"exclusive to square matrices";
	if(this->m != this->n)
		throw std::invalid_argument(MSG.c_str());
}

void Matrix::ensureSameSize(const Matrix &B, const string &str) const {
	string MSG = (str.empty() ? "" : "in ") + str +
		(str.empty() ? "" : ", ") +
		"matrices must be of same size";
	if(this->m != B.m || this->n != B.n)
		throw std::invalid_argument(MSG.c_str());
}

void Matrix::ensureNonSingularity(const string &str) const {
	string MSG = (str.empty() ? "" : "in ") + str +
		(str.empty() ? "" : ", ") +
		"matrix is singular";
	if(this->isSingular())
		throw std::invalid_argument(MSG.c_str());
}

void Matrix::ensureVector(const string &str) const {
	string MSG = (str.empty() ? "" : "in ") + str +
		(str.empty() ? "" : ", ") +
		"matrices must be row or column vectors";
	if(this->m != 1 && this->n != 1)
		throw std::invalid_argument(MSG.c_str());
}
// =====================================================================
void Matrix::print() const {
	cout << endl << ">> Matrix_" << this->m << "X" << this->n
		 << " =" << endl;
	for(size_t i = 0; i < this->m; i++) {
		for(size_t j = 0; j < this->n; j++)
			cout << this->elements.at(i*this->n + j) << '\t';
		cout << endl;
	}
	cout << endl;
}

string Matrix::str() const {
	string ST = "";
	for(size_t i = 0; i < this->m; i++) {
		for(size_t j = 0; j < this->n; j++)
			ST += my::num2str<double>(this->elements.at(i*this->n + j))
				+ ((j == this->n-1) ? '\0' : '\t');
		ST += '\n';
	}
	return ST;
}

ostream& operator<<(ostream &os, const Matrix& M) {
	return os << M.str();
}

double& Matrix::at(const size_t &i_) {
	this->checkDim(i_,"AT");
	return this->elements.at(i_);
}

const double& Matrix::at(const size_t &i_) const {
	this->checkDim(i_,"AT");
	return this->elements.at(i_);
}

double& Matrix::at(const size_t &i_, const size_t &j_) {
	this->checkDim(i_,j_,"AT");
	return this->elements.at(i_*(this->n)+j_);
}

const double& Matrix::at(const size_t &i_, const size_t &j_) const {
	this->checkDim(i_,j_,"AT");
	return this->elements.at(i_*(this->n)+j_);
}

vector<double> Matrix::as_vector() const {
	return this->elements;
}

vector<double> Matrix::row(const size_t &r_) const {
	this->checkDim(r_,0,"ROW");
	vector<double> row_(this->elements.begin()+(r_*this->n),
		this->elements.begin()+(r_*this->n+this->n));
	return row_;
}

void Matrix::assign2row(const size_t &r_, const double &value) {
	this->checkDim(r_,0,"ASSIGN2ROW");
	for(size_t i = 0; i < this->n; i++)
		this->elements.at(r_*this->n+i) = value;
}

void Matrix::assign2row(const size_t &r_,
		const vector<double> &values) {
	this->checkDim(r_,0,"ASSIGN2ROW");
	this->checkSize(this->n,values.size(),"ASSIGN2ROW");
	for(size_t i = 0; i < this->n; i++)
		this->elements.at(r_*this->n+i) = values.at(i);
}

void Matrix::assign2row(const size_t &r_, const double *values,
		const size_t &sz) {
	vector<double> vec(values,values+sz);
	this->assign2row(r_,vec);
}

vector<double> Matrix::column(const size_t &c_) const {
	this->checkDim(0,c_,"COLUMN");
	vector<double> column_(this->m,0.);
	for(size_t i = 0; i < this->m; i++)
		column_.at(i) = this->elements.at(i*this->n+c_);
	return column_;
}

void Matrix::assign2column(const size_t &c_, const double &value) {
	this->checkDim(0,c_,"ASSIGN2COLUMN");
	for(size_t i = 0; i < this->m; i++)
		this->elements.at(i*this->n+c_) = value;
}

void Matrix::assign2column(const size_t &c_,
		const vector<double> &values) {
	this->checkDim(0,c_,"ASSIGN2COLUMN");
	this->checkSize(this->m,values.size(),"ASSIGN2COLUMN");
	for(size_t i = 0; i < this->m; i++)
		this->elements.at(i*this->n+c_) = values.at(i);
}

void Matrix::assign2column(const size_t &c_, const double *values,
		const size_t &sz) {
	vector<double> vec(values,values+sz);
	this->assign2column(c_,vec);
}

vector<double> Matrix::diagonal() const {
	size_t dim = min(this->m,this->n);
	vector<double> diag_(dim,0.);
	for(size_t i = 0; i < dim; i++)
		diag_.at(i) = this->elements.at(i*this->n+i);
	return diag_;
}

void Matrix::assign2diagonal(const double &value) {
	size_t dim = min(this->m,this->n);
	for(size_t i = 0; i < dim; i++)
		this->elements.at(i*this->n+i) = value;
}

void Matrix::assign2diagonal(const vector<double> &values) {
	size_t dim = min(this->m,this->n);
	this->checkSize(dim,values.size(),"ASSIGN2DIAGONAL");
	for(size_t i = 0; i < dim; i++)
		this->elements.at(i*this->n+i) = values.at(i);
}

void Matrix::assign2diagonal(const double *values, const size_t &sz) {
	vector<double> vec(values,values+sz);
	this->assign2diagonal(vec);
}

void Matrix::swap_rows(const size_t &r1, const size_t &r2) {
	this->checkDim(r1,0,"SWAP_ROW");
	this->checkDim(r2,0,"SWAP_ROW");
	double aux;
	for(size_t j = 0; j < this->n; j++) {
		aux = this->elements.at(r1*this->n+j);
		this->elements.at(r1*this->n+j) =
			this->elements.at(r2*this->n+j);
		this->elements.at(r2*this->n+j) = aux;
	}
}

void Matrix::swap_columns(const size_t &c1, const size_t &c2) {
	this->checkDim(0,c1,"SWAP_COLUMN");
	this->checkDim(0,c2,"SWAP_COLUMN");
	double aux;
	for(size_t i = 0; i < this->m; i++) {
		aux = this->elements.at(i*this->n+c1);
		this->elements.at(i*this->n+c1) =
			this->elements.at(i*this->n+c2);
		this->elements.at(i*this->n+c2) = aux;
	}
}

void Matrix::self_concat_row(const double &cte) {
	Matrix M(this->concat_row(cte));
	this->swap(M);
}

void Matrix::self_concat_row(const vector<double> &values) {
	Matrix M(this->concat_row(values));
	this->swap(M);
}

void Matrix::self_concat_row(const Matrix &B) {
	Matrix M(this->concat_row(B));
	this->swap(M);
}

void Matrix::self_concat_column(const double &cte) {
	Matrix M(this->concat_column(cte));
	this->swap(M);
}

void Matrix::self_concat_column(const vector<double> &values) {
	Matrix M(this->concat_column(values));
	this->swap(M);
}

void Matrix::self_concat_column(const Matrix &B) {
	Matrix M(this->concat_column(B));
	this->swap(M);
}

void Matrix::self_delete_row(const size_t &r_) {
	Matrix M(this->delete_row(r_));
	this->swap(M);
}

void Matrix::self_delete_column(const size_t &c_) {
	Matrix M(this->delete_column(c_));
	this->swap(M);
}

Matrix Matrix::concat_row(const double &cte) const {
	vector<double> values(this->n,cte);
	return concat_row(values);
}

Matrix Matrix::concat_row(const vector<double> &values) const {
	this->checkSize(this->n,values.size(),"CONCAT_ROW");
	Matrix M(this->m+1,this->n);
	for(size_t i = 0; i < this->m; i++)
		for(size_t j = 0; j < M.n; j++)
			M.elements.at(i*M.n+j) = this->elements.at(i*this->n+j);
	for(size_t j = 0; j < M.n; j++)
		M.elements.at(this->m*M.n+j) = values.at(j);
	return M;
}

Matrix Matrix::concat_row(const Matrix &B) const {
	this->checkSize(this->n,B.n,"CONCAT_ROW");
	Matrix M(this->m+B.m,this->n);
	for(size_t i = 0; i < this->m; i++)
		for(size_t j = 0; j < M.n; j++)
			M.elements.at(i*M.n+j) = this->elements.at(i*this->n+j);
	for(size_t i = 0; i < B.m; i++)
		for(size_t j = 0; j < M.n; j++)
			M.elements.at((this->m+i)*M.n+j) =
				B.elements.at(i*B.n+j);
	return M;
}

Matrix Matrix::concat_column(const double &cte) const {
	vector<double> values(this->m,cte);
	return concat_column(values);
}

Matrix Matrix::concat_column(const vector<double> &values) const {
	this->checkSize(this->m,values.size(),"CONCAT_COLUMN");
	Matrix M(this->m,this->n+1);
	for(size_t i = 0; i < M.m; i++)
		for(size_t j = 0; j < this->n; j++)
			M.elements.at(i*M.n+j) = this->elements.at(i*this->n+j);
	for(size_t i = 0; i < M.m; i++)
		M.elements.at(i*M.n+this->n) = values.at(i);
	return M;
}

Matrix Matrix::concat_column(const Matrix &B) const {
	this->checkSize(this->m,B.m,"CONCAT_COLUMN");
	Matrix M(this->m,this->n+B.n);
	for(size_t i = 0; i < M.m; i++)
		for(size_t j = 0; j < this->n; j++)
			M.elements.at(i*M.n+j) = this->elements.at(i*this->n+j);
	for(size_t i = 0; i < M.m; i++)
		for(size_t j = 0; j < B.n; j++)
			M.elements.at(i*M.n+(this->n+j)) = B.elements.at(i*B.n+j);
	return M;
}

Matrix Matrix::delete_row(const size_t &r_) const {
	this->checkDim(r_,0,"DELETE_ROW");
	Matrix M(this->m-1,this->n);
	for(size_t i = 0; i < this->m; i++)
		for(size_t j = 0; j < this->n; j++)
			if(i != r_)
				M.elements.at(((i<r_)?i:i-1)*M.n+j) =
					this->elements.at(i*this->n+j);
	return M;
}

Matrix Matrix::delete_column(const size_t &c_) const {
	this->checkDim(0,c_,"DELETE_COLUMN");
	Matrix M(this->m,this->n-1);
	for(size_t i = 0; i < this->m; i++)
		for(size_t j = 0; j < this->n; j++)
			if(j != c_)
				M.elements.at(i*M.n+((j<c_)?j:j-1)) =
					this->elements.at(i*this->n+j);
	return M;
}
// ===================================
size_t Matrix::rank() const {
	size_t rk = 0;
	this->checkSize((this->m)*(this->n),this->elements.size(),"RANK");
	lapack_int m = this->m, n = this->n;
	lapack_int lda = n, ldu = m, ldvt = n, info;
	double *superb = new double[min(m,n)-1];
	double *a = new double[m*n],
		   *s = new double[min(m,n)],
		   *u = new double[m*m],
		   *vt = new double[n*n];
	for(size_t i = 0; i < this->elements.size(); i ++)
		a[i] = this->elements.at(i);
	info = LAPACKE_dgesvd( LAPACK_ROW_MAJOR, 'N', 'N', m, n, a, lda,
		s, u, ldu, vt, ldvt, superb );
    if(info < 0) 	// Check for invalid (NaN)
		this->invalidArgument((int)info,"RANK");
	if(info > 0) 	// Check for convergence
		this->badConvergence("RANK");
	for(int i = 0; i < min(m,n); i++)
		rk += (my::isLikelyZero(s[i],max(this->m,this->n))) ? 0 : 1;
	delete [] superb;
	delete [] a;
	delete [] s;
	delete [] u;
	delete [] vt;
	return rk;
}

double Matrix::determinant() const {
	this->ensureSquare("DETERMINANT");
	double det;
	switch(this->m) {
		case 1:
			det = this->elements.at(0);
			break;
		case 2:
			det = this->elements.at(0)*this->elements.at(3) -
				this->elements.at(1)*this->elements.at(2);
			break;
		case 3:
			det = this->elements.at(0)*this->elements.at(4)*
					this->elements.at(8)
				+ this->elements.at(1)*this->elements.at(5)*
					this->elements.at(6)
				+ this->elements.at(2)*this->elements.at(3)*
					this->elements.at(7)
				- this->elements.at(2)*this->elements.at(4)*
					this->elements.at(6)
				- this->elements.at(0)*this->elements.at(5)*
					this->elements.at(7)
				- this->elements.at(1)*this->elements.at(3)*
					this->elements.at(8);
			break;
		default:
			det = 0;
			for(size_t j = 0; j < this->n; j++)
				det += this->cofactor(0,j)*this->elements.at(j); // i=0
	}
	return det;
}

double Matrix::trace() const {
	this->ensureSquare("TRACE");
	vector<double> diag(this->diagonal());
	double total(0.);
	for(vector<double>::iterator it = diag.begin();
		it < diag.end(); it++) total += *it;
	return total;
}

double Matrix::norm() const {
	// for square matrices, this is the same as the "spectral norm",
	// the "induced matrix 2-norm" or the "Euclidean norm"
	if(this->m != 1 && this->n != 1) {
		this->checkSize((this->m)*(this->n),this->elements.size(),
			"NORM");
		lapack_int m = this->m, n = this->n;
		lapack_int lda = n, ldu = m, ldvt = n, info;
		double *superb = new double[min(m,n)-1];
		double *a = new double[m*n],
			   *s = new double[min(m,n)],
			   *u = new double[m*m],
			   *vt = new double[n*n];
		for(size_t i = 0; i < this->elements.size(); i ++)
			a[i] = this->elements.at(i);
		info = LAPACKE_dgesvd( LAPACK_ROW_MAJOR, 'N', 'N', m, n, a, lda,
			s, u, ldu, vt, ldvt, superb );
		if(info < 0) 	// Check for invalid (NaN)
			this->invalidArgument((int)info,"NORM");
		if(info > 0) 	// Check for convergence
			this->badConvergence("NORM");
		double hSingVal = s[0];
		for(int i = 1; i < min(m,n); i++)
			if(s[i] > hSingVal)
				hSingVal = s[i];
		delete [] superb;
		delete [] a;
		delete [] s;
		delete [] u;
		delete [] vt;
		return hSingVal;
	} else
		return sqrt(((this->m == 1) ? (*this)*(this->transpose()) :
			(this->transpose())*(*this)).elements.at(0));
}

double Matrix::infinity_norm() const {
	// Maximum absolute row sum norm
	double infnorm = 0., sum_;
	for(size_t i = 0; i < this->m; i++) {
		sum_ = 0.;
		for(size_t j = 0; j < this->n; j++)
			sum_ += abs(this->elements.at(i*this->n+j));
		if(sum_ > infnorm)
			infnorm = sum_;
	}
	return infnorm;
}

double Matrix::frobenius_norm() const {
	this->checkSize((this->m)*(this->n),this->elements.size(),
		"FRBNORM");
	double sum = 0.;
	for(size_t i = 0; i < this->m; i++)
		for(size_t j = 0; j < this->n; j++)
			sum += pow(this->elements.at(i*this->n+j),2.);
	return sqrt(sum);
}

double Matrix::condition_number() const
{
	triplet<Matrix> SVD = this->svd();
	vector<double> singular_values = SVD.second.diagonal();
	return my::max(singular_values)/my::min(singular_values);
}
double Matrix::cofactor(const size_t &r_, const size_t &c_) const {
	this->ensureSquare("COFACTOR");
	Matrix CF(*this);
	CF.self_delete_row(r_);
	CF.self_delete_column(c_);
	return (((r_+c_)%2)?-1:1)*CF.determinant();
}

Matrix Matrix::transpose() const {
	Matrix T(this->n,this->m);
	for(size_t i = 0; i < this->m; i++)
		for(size_t j = 0; j < this->n; j++)
			T.elements.at(j*this->m+i) = this->elements.at(i*this->n+j);
	return T;
}

Matrix Matrix::adjoint() const {
	this->ensureSquare("ADJOINT");
	Matrix A(this->m,this->n);
	for(size_t i = 0; i < this->m; i++)
		for(size_t j = 0; j < this->n; j++)
			A.elements.at(i*this->n+j) = cofactor(i,j);
	return A.transpose();
}

Matrix Matrix::inverse() const {
	this->ensureSquare("INVERSE");
	this->ensureNonSingularity("INVERSE");
	return this->adjoint()/this->determinant();
}

brace<Matrix> Matrix::eigen() const {
	this->checkSize((this->n)*(this->n),this->elements.size(),"EIGEN");
	this->ensureSquare("EIGEN");
	lapack_int n = this->n, info;
	double *a = new double[n*n],
		   *b = new double[n*n],
		   *alphar = new double[n],
		   *alphai = new double[n],
		   *beta = new double[n],
		   *vl = new double,
		   *vr = new double[n*n];
	for(size_t i = 0; i < this->elements.size(); i ++) {
		a[i] = this->elements.at(i);
		b[i] = (i%(n+1) == 0) ? 1. : 0.;
	}
	info = LAPACKE_dggev( LAPACK_ROW_MAJOR, 'N', 'V', n, a, n, b, n,
		alphar, alphai, beta, vl, 1, vr, n );
	if(info < 0) 	// Check for invalid (NaN)
		this->invalidArgument((int)info,"EIGEN");
	if(info > 0) 	// Check for convergence
		this->badConvergence("EIGEN");
	Matrix EVAL(n,n),
		   EVEC(n,n,vr,n*n);
	vector<double> lambda(n,0.);
	for(int i = 0; i < n; i++) {
		lambda.at(i) = (abs(alphai[i]) <= EPS) ? ((abs(beta[i]) <= EPS)
			? NAN : alphar[i]/beta[i] ) : -NAN;
	}
	EVAL.assign2diagonal(lambda);
	delete [] a;
	delete [] b;
	delete [] alphar;
	delete [] alphai;
	delete [] beta;
	delete vl;
	delete [] vr;
	return make_brace<Matrix>(EVAL,EVEC);
}

triplet<Matrix> Matrix::lu_p_factorization() const {
	lapack_int m = this->m, n = this->n, info;
	double *a = new double[m*n];
	lapack_int *ipiv = new lapack_int[max(1,min(m,n))];
	for(size_t i = 0; i < this->elements.size(); i ++) {
		a[i] = this->elements.at(i);
	}
	info = LAPACKE_dgetrf( LAPACK_ROW_MAJOR, m, n, a, n, ipiv );
	if(info < 0) 	// Check for invalid (NaN)
		this->invalidArgument((int)info,"LU_P_FACTORIZATION");
	if(info > 0) 	// Check for convergence
		cerr << ">> Warning! Matrix U is singular." << endl;
	Matrix L(m,min(m,n)),
		   U(min(m,n),n),
		   P(m,m);
	vector<double> ones(min(m,n),1.);
	L.assign2diagonal(ones);
	for(int i = 0; i < m; i++)
		for(int j = 0; j < n; j++)
			if(j >= i)
				U.at(i,j) = a[i*n+j];
			else
				L.at(i,j) = a[i*n+j];
	vector<size_t> seq;
	size_t aux;
	for(int i = 0; i < m; i++)
		seq.push_back(i);
	for(int i = 0; i < min(m,n); i++) {
		aux = seq.at(i);
		seq.at(i) = seq.at(ipiv[i]-1);
		seq.at(ipiv[i]-1) = aux;
	}
	for(int i = 0; i < m; i++)
		P.at(i,seq.at(i)) = 1;
	delete [] a;
	delete [] ipiv;
	return make_triplet<Matrix>(L,U,P);
}

brace<Matrix> Matrix::qr_factorization() const {
	lapack_int m = this->m, n = this->n, info;
	double *a = new double[m*n];
	double *tau = new double[max(1,min(m,n))];
	for(size_t i = 0; i < this->elements.size(); i ++) {
		a[i] = this->elements.at(i);
	}
    info = LAPACKE_dgeqrf( LAPACK_ROW_MAJOR, m, n, a, n, tau );
	if(info < 0) 	// Check for invalid (NaN)
		this->invalidArgument((int)info,"QR_FACTORIZATION");
	/* The matrix Q is represented as a product of elementary reflectors
	*
	*     Q = H(1) H(2) . . . H(k), where k = min(m,n).
	*
	*  Each H(i) has the form
	*
	*     H(i) = I - tau * v * v'
	*
	*  where tau is a real scalar, and v is a real vector with
	*  v(1:i-1) = 0 and v(i) = 1; v(i+1:m) is stored on exit in
	*  A(i+1:m,i), and tau in TAU(i).*/
	Matrix Q(m,min(m,n)),
		   R(min(m,n),n),
		   H(identityMatrix(m)),
		   v(m,1);
	vector<double> ones(min(m,n),1.);
	Q.assign2diagonal(ones);
	for(int i = 0; i < m; i++)
		for(int j = 0; j < n; j++)
			if(j >= i)
				R.at(i,j) = a[i*n+j];
			else
				Q.at(i,j) = a[i*n+j];
	for(int i = 0; i < min(m,n); i++) {
		v = column_vector(Q.column(i));
		H *= identityMatrix(m)-(v*v.transpose())*tau[i];
	}
	Q = H;
	if(m > n)
		for(int i = n; i < m; i++)
			Q.self_delete_column(Q.n-1);
	delete [] a;
	delete [] tau;
	return make_brace<Matrix>(Q,R);
}

brace<Matrix> Matrix::lq_factorization() const {
	lapack_int m = this->m, n = this->n, info;
	double *a = new double[m*n];
	double *tau = new double[max(1,min(m,n))];
	for(size_t i = 0; i < this->elements.size(); i ++) {
		a[i] = this->elements.at(i);
	}
    info = LAPACKE_dgelqf( LAPACK_ROW_MAJOR, m, n, a, n, tau );
	if(info < 0) 	// Check for invalid (NaN)
		this->invalidArgument((int)info,"LQ_FACTORIZATION");
	Matrix L(m,min(m,n)),
		   Q(min(m,n),n),
		   H(identityMatrix(n)),
		   v(n,1);
	vector<double> ones(min(m,n),1.);
	Q.assign2diagonal(ones);
	for(int i = 0; i < m; i++)
		for(int j = 0; j < n; j++)
			if(j <= i)
				L.at(i,j) = a[i*n+j];
			else
				Q.at(i,j) = a[i*n+j];
	for(int i = 0; i < min(m,n); i++) {
		v = row_vector(Q.row(i));
		H *= identityMatrix(n)-(v.transpose()*v)*tau[i];
	}
	Q = H.transpose();
	if(m < n)
		for(int i = m; i < n; i++)
			Q.self_delete_row(Q.m-1);
	delete [] a;
	delete [] tau;
	return make_brace<Matrix>(L,Q);
}

triplet<Matrix> Matrix::svd() const {
	this->checkSize((this->m)*(this->n),this->elements.size(),"SVD");
	lapack_int m = this->m, n = this->n;
	lapack_int lda = n, ldu = m, ldvt = n, info;
	double *superb = new double[min(m,n)-1];
	double *a = new double[m*n],
		   *s = new double[min(m,n)],
		   *u = new double[m*m],
		   *vt = new double[n*n];
	for(size_t i = 0; i < this->elements.size(); i ++)
		a[i] = this->elements.at(i);
	info = LAPACKE_dgesvd( LAPACK_ROW_MAJOR, 'A', 'A', m, n, a, lda,
		s, u, ldu, vt, ldvt, superb );
    if(info < 0) 	// Check for invalid (NaN)
		this->invalidArgument((int)info,"SVD");
	if(info > 0) 	// Check for convergence
		this->badConvergence("SVD");
	// Adjust zeros
	for(size_t i = 0; i < min(this->m,this->n); i++)
		if(my::isLikelyZero(s[i],max(this->m,this->n)))
			s[i] = 0.;
	for(int i = 0; i < m*m; i++)
		if(my::isLikelyZero(u[i],max(this->m,this->n)))
			u[i] = 0.;
	for(int i = 0; i < n*n; i++)
		if(my::isLikelyZero(vt[i],max(this->m,this->n)))
			vt[i] = 0.;
	// Create matrices
	Matrix S(m,n),
		   U(m,m,u,m*m),
		   VT(n,n,vt,n*n);
	S.assign2diagonal(s,min(m,n));
	delete [] superb;
	delete [] a;
	delete [] s;
	delete [] u;
	delete [] vt;
	// Return matrices U, Sigma and V
	// To retrieve original matrix: U*Sigma*V.transpose()
	return make_triplet<Matrix>(U,S,VT.transpose());
}

Matrix Matrix::pseudoinverse() const {
	triplet<Matrix> SVD = this->svd();
	vector<double> sv = SVD.second.diagonal();
	double maxsv = 0;
	for(size_t i = 0; i < sv.size(); i++)
		if(maxsv < sv.at(i))
			maxsv = sv.at(i);
	for(size_t i = 0; i < sv.size(); i++) {
		if(sv.at(i) <= this->m*this->n*EPS)
			sv.at(i) = 0.;
		else
			sv.at(i) = 1./sv.at(i);
	}
	SVD.second.assign2diagonal(sv);
	return (SVD.first*SVD.second*SVD.third.transpose()).transpose();
}
// =========
double Matrix::min_all(const bool &absolute) const
{
	double minelem = (absolute)? abs(this->elements.front()) :
		this->elements.front();
	for(size_t i = 1; i < this->elements.size(); i++)
		if((absolute)? abs(this->elements.at(i)) < minelem :
			this->elements.at(i) < minelem)
				minelem = (absolute)? abs(this->elements.at(i)) :
					this->elements.at(i);
	return minelem;
}
double Matrix::max_all(const bool &absolute) const
{
	double maxelem = (absolute)? abs(this->elements.front()) :
		this->elements.front();
	for(size_t i = 1; i < this->elements.size(); i++)
		if((absolute)? abs(this->elements.at(i)) > maxelem :
			this->elements.at(i) > maxelem)
				maxelem = (absolute)? abs(this->elements.at(i)) :
					this->elements.at(i);
	return maxelem;
}
// =========
Matrix Matrix::operator+(const double &value) const {
	Matrix A(*this);
	A += value;
	return A;
}

void Matrix::operator+=(const double &value) {
	for(size_t i = 0; i < this->m; i++)
		for(size_t j = 0; j < this->n; j++)
			this->elements.at(i*this->n+j) += value;
}

Matrix Matrix::operator-(const double &value) const {
	Matrix A(*this);
	A -= value;
	return A;
}

void Matrix::operator-=(const double &value) {
	for(size_t i = 0; i < this->m; i++)
		for(size_t j = 0; j < this->n; j++)
			this->elements.at(i*this->n+j) -= value;
}

Matrix Matrix::operator*(const double &value) const {
	Matrix A(*this);
	A *= value;
	return A;
}

void Matrix::operator*=(const double &value) {
	for(size_t i = 0; i < this->m; i++)
		for(size_t j = 0; j < this->n; j++)
			this->elements.at(i*this->n+j) *= value;
}

Matrix Matrix::operator/(const double &value) const {
	Matrix A(*this);
	A /= value;
	return A;
}

void Matrix::operator/=(const double &value) {
	for(size_t i = 0; i < this->m; i++)
		for(size_t j = 0; j < this->n; j++)
			this->elements.at(i*this->n+j) /= value;
}

Matrix Matrix::operator+(const Matrix &B) const {
	Matrix A(*this);
	A += B;
	return A;
}

void Matrix::operator+=(const Matrix &B) {
	this->ensureSameSize(B,"OPERATOR+");
	for(size_t i = 0; i < this->m; i++)
		for(size_t j = 0; j < this->n; j++)
			this->elements.at(i*this->n+j) += B.elements.
				at(i*this->n+j);
}

Matrix Matrix::operator-(const Matrix &B) const {
	Matrix A(*this);
	A -= B;
	return A;
}

void Matrix::operator-=(const Matrix &B) {
	this->ensureSameSize(B,"OPERATOR-");
	for(size_t i = 0; i < this->m; i++)
		for(size_t j = 0; j < this->n; j++)
			this->elements.at(i*this->n+j) -= B.elements.
				at(i*this->n+j);
}

Matrix Matrix::operator*(const Matrix &B) const {
	this->checkSize(this->n,B.m,"OPERATOR*");
	Matrix R(this->m,B.n);
	for(size_t i = 0; i < this->m; i++)
		for(size_t j = 0; j < this->n; j++)
			for(size_t k = 0; k < B.n; k++) {
				R.elements.at(i*B.n+k) += this->elements.
					at(i*this->n+j)*B.elements.at(j*B.n+k);
				if(abs(R.elements.at(i*B.n+k)) <= this->m*EPS)
					R.elements.at(i*B.n+k) = 0.;
			}
	return R;
}

void Matrix::operator*=(const Matrix &B) {
	Matrix A = (*this)*B;
	this->swap(A);
}

bool Matrix::operator==(const Matrix &B) const {
	if(this->m != B.m || this->n != B.n)
		return false;
	else {
		bool chk = true;
		for(size_t i = 0; i < this->m; i++) {
			for(size_t j = 0; j < this->n; j++) {
				if(abs(this->elements.at(i*this->n+j)-
						B.elements.at(i*B.n+j)) > 0.5*EPS) {
					chk = false;
					break;
				}
			}
			if(!chk) break;
		}
		return chk;
	}
}

bool Matrix::operator!=(const Matrix &B) const {
	return !((*this) == B);
}
// ======
Matrix operator+(const double &lhs, const Matrix &rhs) {
	return rhs+lhs;
}
Matrix operator-(const double &lhs, const Matrix &rhs) {
	return rhs*(-1.)+lhs;
}
Matrix operator*(const double &lhs, const Matrix &rhs) {
	return rhs*lhs;
}
Matrix operator/(const double &lhs, const Matrix &rhs) {
	// This is not an inverse! It's an element-wise division
	Matrix result(rhs);
	for(size_t i = 0; i < result.m; i++)
		for(size_t j = 0; j < result.n; j++)
			result.at(i,j) = lhs/result.at(i,j);
	return result;
}
// ======
bool Matrix::isFullRank() const {
	return this->rank() == min(this->m,this->n);
}

bool Matrix::isOrthogonal() const {
	Matrix O = (*this)*(this->transpose());
	return (O == identityMatrix(O.m));
}

bool Matrix::isDiagonal() const {
	Matrix D = diagonalMatrix(this->diagonal());
	if(this->m != this->n) {
		if(this->m > this->n) {
			vector<double> zeros(this->n,0.);
			for(size_t i = this->n; i < this->m; i++)
				D.self_concat_row(zeros);
		} else {
			vector<double> zeros(this->m,0.);
			for(size_t i = this->m; i < this->n; i++)
				D.self_concat_column(zeros);
		}
	}
	return (*this == D);
}

bool Matrix::isPositiveDefinite() const {
	//real symmetric matrix
	if(!(this->isSymmetric()))
		return false;
	else {
		// if all its eigenvalues are positive.
		cout << "not implemented yet!" << endl;
		return false;
	}
}

bool Matrix::isSingular() const {
	return (my::isLikelyZero(this->determinant()));
}

bool Matrix::isSymmetric() const {
	return (*this == this->transpose());
}

bool Matrix::isSkewSymmetric() const {
	return (*this == this->transpose()*(-1.));
}

bool Matrix::isValid() const {
	bool chk = true;
	for(vector<double>::const_iterator it = this->elements.begin();
		it != this->elements.end(); it++)
	{
		if(std::isnan(*it) || my::isinf(*it))
		{
			chk = false;
			break;
		}
	}
	return chk;
}

void Matrix::adjust_elements(const double &tol)
{
	for(size_t i = 0; i < this->elements.size(); i++)
		if(my::isLikelyZero(this->elements.at(i),this->m*this->n) ||
				abs(this->elements.at(i)) < tol)
			this->elements.at(i) = 0.;
}

void Matrix::saveAs(const string &namefile) const
{
	ofstream matrixfile;
	matrixfile.open(namefile);
	if (matrixfile.is_open())
	{
		for(size_t i = 0; i < this->m; i++)
		{
			for(size_t j = 0; j < this->n; j++)
				matrixfile << this->elements.at(i*(this->n)+j)
						   << ((j+1 == this->n) ? "" : "\t");
			matrixfile << endl;
		}
	}
	else
	{
		cerr << "Unable to open " << namefile << " file!" << endl;
	}
	matrixfile.close();
}
// =====================================================================
Matrix column_vector(const vector<double> &vec) {
	Matrix A(vec.size(),1,vec);
	return A;
}

Matrix column_vector(const size_t &qty, const double &val) {
	vector<double> vec(qty,val);
	Matrix A(qty,1,vec);
	return A;
}

Matrix row_vector(const vector<double> &vec) {
	Matrix A(1,vec.size(),vec);
	return A;
}

Matrix row_vector(const size_t &qty, const double &val) {
	vector<double> vec(qty,val);
	Matrix A(1,qty,vec);
	return A;
}

Matrix subMatrix(const Matrix &M, const size_t &fstr,
		const size_t &fstc, const size_t &lstr, const size_t &lstc) {
	cout << "row: " << fstr << " " << lstr << endl;
	cout << "col: " << fstc << " " << lstc << endl;
	cout << "M: " << M.m << " " << M.n << endl;
	if(fstr > lstr || lstr >= M.m || fstc > lstc || lstc >= M.n) {
		string MSG = "in SUBMATRIX, index exceeds matrix dimension";
		throw std::out_of_range(MSG.c_str());
	}
	Matrix A(lstr-fstr+1,lstc-fstc+1);
	cout << A.m << " " << A.n << endl;
	for(size_t i = 0; i < A.m; i++)
		for(size_t j = 0; j < A.n; j++)
			A.at(i,j) = M.at(i+fstr,j+fstc);
	return A;
}

Matrix identityMatrix(const size_t &N) {
	Matrix A(N,N);
	A.assign2diagonal(1.);
	return A;
}

Matrix diagonalMatrix(const vector<double> &vec) {
	Matrix A(vec.size(),vec.size());
	A.assign2diagonal(vec);
	return A;
}

Matrix linear_AASTtoRowVector_VAR(const AAST &tree, const size_t &n_var)
{
	Matrix A, b(n_var, 1);
	vector<double> aux(n_var, 0.);
	double cnst = tree.eval_with_variables(aux);
	aux.at(0) = 1.;
	A = row_vector(aux);
	b.at(0) = tree.eval_with_variables(aux) - cnst;
	for(size_t i = 1; i < n_var; i++)
	{
		aux.assign(n_var, 0.);
		aux.at(i) = 1.;
		A.self_concat_row(row_vector(aux));
		b.at(i) = tree.eval_with_variables(aux) - cnst;
	}
	return ((A.pseudoinverse()*b).transpose()).concat_column(cnst);
}

Matrix linear_AASTtoRowVector_UNK(const AAST &tree, const size_t &n_unk)
{
	Matrix A, b(n_unk, 1);
	vector<double> aux(n_unk, 0.);
	double cnst = tree.eval_with_unknowns(aux);
	aux.at(0) = 1.;
	A = row_vector(aux);
	b.at(0) = tree.eval_with_unknowns(aux) - cnst;
	for(size_t i = 1; i < n_unk; i++)
	{
		aux.assign(n_unk, 0.);
		aux.at(i) = 1.;
		A.self_concat_row(row_vector(aux));
		b.at(i) = tree.eval_with_unknowns(aux) - cnst;
	}
	return ((A.pseudoinverse()*b).transpose()).concat_column(cnst);
}
// =====================================================================
#endif


static char help[] =
"Simple ODE system to test implicit and IMEX methods when *both* RHSFunction()\n"
"and IFunction() are supplied.  Jacobians are supplied as well.  The problem\n"
"has form\n"
"   F(t,u,dudt) = G(t,u)\n"
"where  u(t) = (x(t),y(t))  and dF/d(dudt) is invertible, so the problem is a\n"
"2D ODE IVP.  Functions F (IFunction) and G (RHSFunction) are actually linear,\n"
"but the ProblemType is set to TS_NONLINEAR type.  By default the system is\n"
"   x' + y' +  x + 2y =  x + 8y\n"
"        y' + 3x + 4y = 4x + 4y\n"
"The implicit methods like BEULER, THETA, CN, BDF should be able to handle\n"
"this system.  Apparently ROSW can also handle it.  However, ARKIMEX and EIMEX\n"
"require dF/d(dudt) to be the identity.  (But consider option\n"
"-ts_arkimex_fully_implicit.)  If option -identity_in_F is given then an\n"
"equivalent system is formed,\n"
"   x'      +  x + 2y = 8y\n"
"        y' + 3x + 4y = 4x + 4y\n"
"Both of these systems are trivial transformations of the system\n"
"x'=6y-x, y'=x.  With the initial conditions chosen below, x(0)=1 and y(0)=3,\n"
"the exact solution to the above systems is x(t) = -3 e^{-3t} + 4 e^{2t},\n"
"y(t) = e^{-3t} + 2 e^{2t}, and we compute the final numerical error\n"
"accordingly.  Default type is BDF.  See the manual pages for the various\n"
"methods (e.g. TSROSW, TSARKIMEX, TSEIMEX, ...) for further information.\n\n";

#include <petsc.h>

typedef struct {
  PetscBool  identity_in_F;
} Ctx;

extern PetscErrorCode FormIFunction(TS,PetscReal,Vec,Vec,Vec F,void*);
extern PetscErrorCode FormIJacobian(TS,PetscReal,Vec,Vec,PetscReal,Mat,Mat,void*);
extern PetscErrorCode FormRHSFunction(TS,PetscReal,Vec,Vec,void*);
extern PetscErrorCode FormRHSJacobian(TS,PetscReal,Vec,Mat,Mat,void*);

int main(int argc,char **argv)
{
  PetscErrorCode ierr;
  TS             ts;
  Vec            u, uexact;
  Mat            JF,JG;
  Ctx            user;
  PetscReal      tf,xf,yf,errnorm;
  PetscInt       steps;

  PetscInitialize(&argc,&argv,(char*)0,help);

  user.identity_in_F = PETSC_FALSE;
  ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"",
           "Simple F(t,u,u')=G(t,u) ODE system.","TS"); CHKERRQ(ierr);
  ierr = PetscOptionsBool("-identity_in_F","set up system so the dF/d(dudt) = I",
           "ex54.c",user.identity_in_F,&(user.identity_in_F),NULL);CHKERRQ(ierr);
  ierr = PetscOptionsEnd(); CHKERRQ(ierr);

  ierr = VecCreate(PETSC_COMM_WORLD,&u); CHKERRQ(ierr);
  ierr = VecSetSizes(u,PETSC_DECIDE,2); CHKERRQ(ierr);
  ierr = VecSetFromOptions(u); CHKERRQ(ierr);

  ierr = MatCreate(PETSC_COMM_WORLD,&JF); CHKERRQ(ierr);
  ierr = MatSetSizes(JF,PETSC_DECIDE,PETSC_DECIDE,2,2); CHKERRQ(ierr);
  ierr = MatSetFromOptions(JF); CHKERRQ(ierr);
  ierr = MatSetUp(JF); CHKERRQ(ierr);
  ierr = MatDuplicate(JF,MAT_DO_NOT_COPY_VALUES,&JG); CHKERRQ(ierr);

  ierr = TSCreate(PETSC_COMM_WORLD,&ts); CHKERRQ(ierr);
  ierr = TSSetApplicationContext(ts,&user); CHKERRQ(ierr);

  ierr = TSSetIFunction(ts,NULL,FormIFunction,&user); CHKERRQ(ierr);
  ierr = TSSetIJacobian(ts,JF,JF,FormIJacobian,&user); CHKERRQ(ierr);

  ierr = TSSetRHSFunction(ts,NULL,FormRHSFunction,&user); CHKERRQ(ierr);
  ierr = TSSetRHSJacobian(ts,JG,JG,FormRHSJacobian,&user); CHKERRQ(ierr);

  ierr = TSSetProblemType(ts,TS_NONLINEAR); CHKERRQ(ierr);
  // helps with ARKIMEX if you also set -ts_arkimex_fully_implicit:
  // ierr = TSSetEquationType(ts,TS_EQ_IMPLICIT); CHKERRQ(ierr);
  ierr = TSSetType(ts,TSBDF); CHKERRQ(ierr);
  ierr = TSSetTime(ts,0.0); CHKERRQ(ierr);
  ierr = TSSetMaxTime(ts,1.0); CHKERRQ(ierr);
  ierr = TSSetTimeStep(ts,0.01); CHKERRQ(ierr);
  ierr = TSSetExactFinalTime(ts,TS_EXACTFINALTIME_MATCHSTEP); CHKERRQ(ierr);
  ierr = TSSetFromOptions(ts);CHKERRQ(ierr);

  ierr = VecSetValue(u,0,1.0,INSERT_VALUES); CHKERRQ(ierr);
  ierr = VecSetValue(u,1,3.0,INSERT_VALUES); CHKERRQ(ierr);
  ierr = VecAssemblyBegin(u); CHKERRQ(ierr);
  ierr = VecAssemblyEnd(u); CHKERRQ(ierr);
  ierr = TSSolve(ts,u); CHKERRQ(ierr);

  ierr = TSGetStepNumber(ts,&steps); CHKERRQ(ierr);
  ierr = TSGetTime(ts,&tf); CHKERRQ(ierr);
  xf = -3.0 * PetscExpReal(-3.0*tf) + 4.0 * PetscExpReal(2.0*tf);
  yf = PetscExpReal(-3.0*tf) + 2.0 * PetscExpReal(2.0*tf);
  //ierr = PetscPrintf(PETSC_COMM_WORLD,
  //         "xf = %.6f, yf = %.6f\n",xf,yf); CHKERRQ(ierr);
  ierr = VecDuplicate(u,&uexact); CHKERRQ(ierr);
  ierr = VecSetValue(uexact,0,xf,INSERT_VALUES); CHKERRQ(ierr);
  ierr = VecSetValue(uexact,1,yf,INSERT_VALUES); CHKERRQ(ierr);
  ierr = VecAssemblyBegin(uexact); CHKERRQ(ierr);
  ierr = VecAssemblyEnd(uexact); CHKERRQ(ierr);
  ierr = VecAXPY(u,-1.0,uexact); CHKERRQ(ierr); // u <- u + (-1.0) uexact
  ierr = VecNorm(u,NORM_2,&errnorm); CHKERRQ(ierr);
  ierr = PetscPrintf(PETSC_COMM_WORLD,
           "error norm at tf = %.6f from %d steps:  |u-u_exact| =  %.5e\n",
           tf,steps,errnorm); CHKERRQ(ierr);

  VecDestroy(&u);  VecDestroy(&uexact);
  MatDestroy(&JF);  MatDestroy(&JG);
  TSDestroy(&ts);
  return PetscFinalize();
}

PetscErrorCode FormIFunction(TS ts, PetscReal t, Vec u, Vec dudt,
                             Vec F, void *user) {
    PetscErrorCode  ierr;
    const PetscReal *au, *adudt;
    PetscReal       *aF;
    PetscBool       flag = ((Ctx*)user)->identity_in_F;

    ierr = VecGetArrayRead(u,&au); CHKERRQ(ierr);
    ierr = VecGetArrayRead(dudt,&adudt); CHKERRQ(ierr);
    ierr = VecGetArray(F,&aF);
    if (flag) {
        aF[0] = adudt[0] + au[0] + 2.0 * au[1];
        aF[1] = adudt[1] + 3.0 * au[0] + 4.0 * au[1];
    } else {
        aF[0] = adudt[0] + adudt[1] + au[0] + 2.0 * au[1];
        aF[1] = adudt[1] + 3.0 * au[0] + 4.0 * au[1];
    }
    ierr = VecRestoreArrayRead(u,&au); CHKERRQ(ierr);
    ierr = VecRestoreArrayRead(dudt,&adudt); CHKERRQ(ierr);
    ierr = VecRestoreArray(F,&aF); CHKERRQ(ierr);
    return 0;
}

// computes  J = dF/du + a dF/d(dudt)
PetscErrorCode FormIJacobian(TS ts, PetscReal t, Vec u, Vec dudt,
                             PetscReal a, Mat J, Mat P, void *user) {
    PetscErrorCode ierr;
    PetscInt       row[2] = {0, 1},  col[2] = {0, 1};
    PetscReal      v[4];
    PetscBool      flag = ((Ctx*)user)->identity_in_F;

    if (flag) {
        v[0] = a + 1.0;    v[1] = 2.0;
        v[2] = 3.0;        v[3] = a + 4.0;
    } else {
        v[0] = a + 1.0;    v[1] = a + 2.0;
        v[2] = 3.0;        v[3] = a + 4.0;
    }
    ierr = MatSetValues(P,2,row,2,col,v,INSERT_VALUES); CHKERRQ(ierr);
    ierr = MatAssemblyBegin(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    if (J != P) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}

PetscErrorCode FormRHSFunction(TS ts, PetscReal t, Vec u,
                               Vec G, void *user) {
    PetscErrorCode  ierr;
    const PetscReal *au;
    PetscReal       *aG;
    PetscBool       flag = ((Ctx*)user)->identity_in_F;

    ierr = VecGetArrayRead(u,&au); CHKERRQ(ierr);
    ierr = VecGetArray(G,&aG); CHKERRQ(ierr);
    if (flag) {
        aG[0] = 8.0 * au[1];
        aG[1] = 4.0 * au[0] + 4.0 * au[1];
    } else {
        aG[0] = au[0] + 8.0 * au[1];
        aG[1] = 4.0 * au[0] + 4.0 * au[1];
    }
    ierr = VecRestoreArrayRead(u,&au); CHKERRQ(ierr);
    ierr = VecRestoreArray(G,&aG); CHKERRQ(ierr);
    return 0;
}

PetscErrorCode FormRHSJacobian(TS ts, PetscReal t, Vec u, Mat J, Mat P,
                               void *user) {
    PetscErrorCode ierr;
    PetscInt       row[2] = {0, 1},  col[2] = {0, 1};
    PetscReal      v[4];
    PetscBool      flag = ((Ctx*)user)->identity_in_F;

    if (flag) {
        v[0] = 0.0;    v[1] = 8.0;
        v[2] = 4.0;    v[3] = 4.0;
    } else {
        v[0] = 1.0;    v[1] = 8.0;
        v[2] = 4.0;    v[3] = 4.0;
    }
    ierr = MatSetValues(P,2,row,2,col,v,INSERT_VALUES); CHKERRQ(ierr);
    ierr = MatAssemblyBegin(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    if (J != P) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}


#ifndef __MATH_VECTOROPS_INCLUDED
#define __MATH_VECTOROPS_INCLUDED

#include <cmath>
#include <limits>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
<<<<<<< vectorops.h
#include <gsl/gsl_cblas.h>
#include "math/specialfunc.h"
=======
#include <gsl/gsl_blas.h>
#include "specialfunc.h"
>>>>>>> 1.27

#ifndef M_PI
#define M_PI        3.14159265358979323846
#endif

#ifndef isnan
# define isnan(x) ((x) != (x))
#endif

/*
 * take the exponent of a vector
 *
 * If the exponent is infinite, then we replace the value with a
 * suitably large max_val
 */
void vexp(const gsl_vector* v,
          gsl_vector* exp_v,
          double max_val = std::numeric_limits<double>::infinity()) {
  assert(exp_v->size >= v->size);
  for (unsigned int ii = 0; ii < v->size; ++ii) {
    double val = exp(gsl_vector_get(v, ii));
    if (val == std::numeric_limits<double>::infinity() || val > max_val) {
      val = max_val;
    }
    gsl_vector_set(exp_v, ii, val);
  }
}

/* take the exponent of a matrix */
void mexp(const gsl_matrix* m, gsl_matrix* exp_m) {
  for (unsigned int ii = 0; ii < m->size1; ++ii) {
    for (unsigned int jj = 0; jj < m->size2; ++jj) {
      double val = exp(gsl_matrix_get(m, ii, jj));
      assert(!isnan(val));
      gsl_matrix_set(exp_m, ii, jj, val);
    }
  }
}

/* like vexp except that it also computes sum x log x */
double vexp_entropy(const gsl_vector* v, gsl_vector* exp_v) {
  double entropy = 0.0;
  for (unsigned int ii = 0; ii < v->size; ++ii) {
    double logval = gsl_vector_get(v, ii);
    double val = exp(logval);
    assert(!isnan(val));
    gsl_vector_set(exp_v, ii, val);
    if (val != 0) {
      entropy -= val * logval;
    }
  }
  return entropy;
}

double ventropy(const gsl_vector* v) {
  double entropy = 0.0;
  for (unsigned int ii = 0; ii < v->size; ++ii) {
    double val = gsl_vector_get(v, ii);
    if (val != 0) {
      entropy -= val * log(val);
    }
  }
  return entropy;
}

double lgamma(double x) {
    double x0,x2,xp,gl,gl0;
    int n=0,k=0;
    static double a[] = {
        8.333333333333333e-02,
       -2.777777777777778e-03,
        7.936507936507937e-04,
       -5.952380952380952e-04,
        8.417508417508418e-04,
       -1.917526917526918e-03,
        6.410256410256410e-03,
       -2.955065359477124e-02,
        1.796443723688307e-01,
       -1.39243221690590};

    x0 = x;
    if (x <= 0.0) return 1e308;
    else if ((x == 1.0) || (x == 2.0)) return 0.0;
    else if (x <= 7.0) {
        n = (int)(7-x);
        x0 = x+n;
    }
    x2 = 1.0/(x0*x0);
    xp = 2.0*M_PI;
    gl0 = a[9];
    for (k=8;k>=0;k--) {
        gl0 = gl0*x2 + a[k];
    }
    gl = gl0/x0+0.5*log(xp)+(x0-0.5)*log(x0)-x0;
    if (x <= 7.0) {
        for (k=1;k<=n;k++) {
            gl -= log(x0-1.0);
            x0 -= 1.0;
        }
    }
    return gl;
}

void mlog(const gsl_matrix* m, gsl_matrix* log_m) {
  for (unsigned int ii = 0; ii < m->size1; ++ii) {
    for (unsigned int jj = 0; jj < m->size2; ++jj) {
      gsl_matrix_set(log_m, ii, jj, log(gsl_matrix_get(m, ii, jj)));
    }
  }
}

void vlog(const gsl_vector* v, gsl_vector* log_v) {
  for (unsigned int ii = 0; ii < v->size; ++ii)
    gsl_vector_set(log_v, ii, log(gsl_vector_get(v, ii)));
}

void vlogit(const gsl_vector* v, gsl_vector* log_v) {
  for (unsigned int ii = 0; ii < v->size; ++ii) {
    double p = gsl_vector_get(v, ii);
    assert(p >= 0.0);
    assert(p <= 1.0);
    gsl_vector_set(log_v, ii, log(p / (1 - p)));
  }
}

void vsigmoid(const gsl_vector* v, gsl_vector* sig_v) {
  for (unsigned int ii = 0; ii < v->size; ++ii) {
    double p = gsl_vector_get(v, ii);
    gsl_vector_set(sig_v, ii, 1. / (1. + exp(-p)));
  }
}

double vlog_entropy(const gsl_vector* v, gsl_vector* log_v) {
  double entropy = 0;
  for (unsigned int ii = 0; ii < v->size; ++ii) {
    double val = gsl_vector_get(v, ii);
    entropy -= val * log(val);
    gsl_vector_set(log_v, ii, log(val));
  }
  return entropy;
}

double entropy(const gsl_vector* v) {
  double entropy = 0;
  for (unsigned int ii = 0; ii < v->size; ++ii) {
    double val = gsl_vector_get(v, ii);
    entropy -= val * log(val);
  }
  return entropy;
}

void vdigamma(const gsl_vector* v, gsl_vector* digamma_v) {
  for (unsigned int ii = 0; ii < v->size; ++ii)
    // gsl_sf_psi throws an error when its argument is 0, whereas digamma returns inf.
    //    gsl_vector_set(digamma_v, ii, gsl_sf_psi(gsl_vector_get(v, ii)));
    gsl_vector_set(digamma_v, ii, digamma(gsl_vector_get(v, ii)));
}

void vlgamma(const gsl_vector* v, gsl_vector* lgamma_v) {
  for (unsigned int ii = 0; ii < v->size; ++ii)
    gsl_vector_set(lgamma_v, ii, lgamma(gsl_vector_get(v, ii)));
}

double gsl_blas_dsum(const gsl_vector* v) {
  double sum = 0;
  for (unsigned int ii = 0; ii < v->size; ++ii) {
    sum += gsl_vector_get(v, ii);
  }
  return sum;
}

double gsl_blas_dsum(const gsl_matrix* v) {
  double sum = 0;
  for (unsigned int ii = 0; ii < v->size1; ++ii) {
    for (unsigned int jj = 0; jj < v->size2; ++jj) {
      sum += gsl_matrix_get(v, ii, jj);
    }
  }
  return sum;
}

double gsl_matrix_rowsum(const gsl_matrix* m, const int row) {
  double sum = 0;
  for(unsigned int i=0; i < m->size2; i++) {
    sum += gsl_matrix_get(m, row, i);
  }
  return sum;
}

double dot_product(const gsl_vector* a, const gsl_vector* b) {
  assert(a->size == b->size);
  double val = 0;
  for(unsigned i=0; i<a->size; i++) {
    val += gsl_vector_get(a, i) * gsl_vector_get(b, i);
  }
  return val;
}

void uniform(gsl_vector* v) {
	gsl_vector_set_all(v, 1.0 / (double)v->size);
}

double normalize(gsl_vector* v) {
  double sum = gsl_blas_dsum(v);
  gsl_blas_dscal(1 / sum, v);
  return sum;
}

/*
  This function takes as input a multinomial parameter vector and
  computes the "total" variance, i.e., the sum of the diagonal of the
  covariance matrix.

  If the multinomial parameter is unnormalized, then the variance of
  the normalized multinomial vector will be computed and then
  multiplied by the scale of the vector.
 */
double MultinomialTotalVariance(const gsl_vector* v) {
  double scale = gsl_blas_dsum(v);
  double variance = 0.0;
  for (size_t ii = 0; ii < v->size; ++ii) {
    double val = gsl_vector_get(v, ii) / scale;
    variance += val * (1. - val);
  }
  return variance * scale;
}

/*
  Computes covariance using the renormalization above and adds it to
  an existing matrix.
*/
void MultinomialCovariance(double alpha,
			   const gsl_vector* v,
			   gsl_matrix* m) {
  double scale = gsl_blas_dsum(v);
  gsl_blas_dger(-alpha / scale, v, v, m);
  gsl_vector_view diag = gsl_matrix_diagonal(m);
  gsl_blas_daxpy(alpha, v, &diag.vector);
}

double MatrixProductSum(const gsl_matrix* m1,
			const gsl_matrix* m2) {
  double val = 0;
  assert(m1->size1 == m2->size1);
  assert(m1->size2 == m2->size2);
  for (size_t ii = 0; ii < m1->size1; ++ii) {
    for (size_t jj = 0; jj < m2->size2; ++jj) {
      val += gsl_matrix_get(m1, ii, jj) *
	gsl_matrix_get(m2, ii, jj);
    }
  }
  return val;
}

double MatrixProductProductSum(const gsl_matrix* m1,
			       const gsl_matrix* m2,
			       const gsl_matrix* m3) {
  double val = 0;
  assert(m1->size1 == m2->size1);
  assert(m1->size2 == m2->size2);
  assert(m1->size1 == m3->size1);
  assert(m1->size2 == m3->size2);
  for (size_t ii = 0; ii < m1->size1; ++ii) {
    for (size_t jj = 0; jj < m2->size2; ++jj) {
      for (size_t kk = 0; kk < m3->size2; ++kk) {
	val += gsl_matrix_get(m1, ii, jj) *
	  gsl_matrix_get(m2, ii, jj) *
	  gsl_matrix_get(m3, ii, jj);
      }
    }
  }
  return val;
}

double SumLGamma(const gsl_vector* v) {
  double s = 0.0;
  for (size_t ii = 0; ii < v->size; ++ii) {
    s += lgamma(gsl_vector_get(v, ii));
  }
  return s;
}

void mtx_fprintf(const char* filename, const gsl_matrix * m)
{
    FILE* fileptr;
    fileptr = fopen(filename, "w");
    gsl_matrix_fprintf(fileptr, m, "%20.17e");
    fclose(fileptr);
}

void mtx_fscanf(const char* filename, gsl_matrix * m)
{
    FILE* fileptr;
    fileptr = fopen(filename, "r");
    gsl_matrix_fscanf(fileptr, m);
    fclose(fileptr);
}

double mtx_accum(const int i,
                 const int j,
                 const double contribution,
                 gsl_matrix* m) {

  double new_val = gsl_matrix_get(m, i, j) + contribution;
  gsl_matrix_set(m, i, j, new_val);
  return new_val;
}

void vct_fscanf(const char* filename, gsl_vector* v)
{
    FILE* fileptr;
    fileptr = fopen(filename, "r");
    gsl_vector_fscanf(fileptr, v);
    fclose(fileptr);
}

void vct_fprintf(const char* filename, gsl_vector* v)
{
    FILE* fileptr;
    fileptr = fopen(filename, "w");
    gsl_vector_fprintf(fileptr, v, "%20.17e");
    fclose(fileptr);
}

#endif


// mathematical routines

#include "../include/bjutils.h"

double integrate_qag(double (*func)(double, void*),void *par,double low,double up,double *error)
{
  const int lim=2000;
  const int key=5;
  const double eabs=0.0;
  const double erel=1.e-4;
  double res,err;

  gsl_integration_workspace *work=gsl_integration_workspace_alloc(lim);

  gsl_function F;
  F.function=func;
  F.params=par;

  gsl_integration_qag(&F,low,up,eabs,erel,lim,key,work,&res,&err);

  gsl_integration_workspace_free(work);
  if (error!=NULL) *error=err;
  return(res);
}

double findroot(double (*func)(double, void*),void *par,double low,double hig,double tol,double *error)
{
  #include <gsl/gsl_errno.h>
  int status,iter=0;
  double root,rnext;
  const int max_iter=1000;    // maximum no. of iterations

  gsl_function F;
  F.function=func;
  F.params=par;

  const gsl_root_fsolver_type *T=gsl_root_fsolver_brent;
  gsl_root_fsolver *solv=gsl_root_fsolver_alloc(T);
  gsl_root_fsolver_set(solv,&F,low,hig);

  do {
    iter++;
    status=gsl_root_fsolver_iterate(solv);
    root=gsl_root_fsolver_root(solv);
    low=gsl_root_fsolver_x_lower(solv);
    hig=gsl_root_fsolver_x_upper(solv);
    status=gsl_root_test_interval(low,hig,0.0,tol);
    if (status==GSL_SUCCESS) printf ("Root finder converged after %i iterations\n",iter);
  } while (status==GSL_CONTINUE && iter<max_iter);

  gsl_root_fsolver_iterate(solv);
  rnext=gsl_root_fsolver_root(solv);
  *error=fabs(root-rnext);
  gsl_root_fsolver_free(solv);
  return(root);
}

// index search via bisection (log N process)
// array, dimension of array, value to be found, returned: index of array lower than x
void bisect_interpol(double *a,int dim,double x,int *erg)
{
  int l=0,u=dim-1,m;

  if ((x<a[l]) || (x>a[u])) {
    printf("Error: index out of range: %g not in %g-%g\n",x,a[l],a[u]);
    exit(-1);
  }
  while (u-l>1) {
    m=(u+l)/2;
    if (x>a[m]) l=m;
    else u=m;
  }
  *erg=l;
}

// 1D linear interpolation + extrapolation
double interpol_linear_extra_1D(double *ax,int NX,double *z,double x)
{
  if (NX==1) return(z[0]);  // assume z constant if only one data point
  if (x<=ax[0]) return((z[1]-z[0])/(ax[1]-ax[0])*(x-ax[0])+z[0]);
  else if (x>=ax[NX-1]) return((z[NX-1]-z[NX-2])/(ax[NX-1]-ax[NX-2])*(x-ax[NX-1])+z[NX-1]);
  else {
    int ix;
    bisect_interpol(ax,NX,x,&ix);
    double t=(x-ax[ix])/(ax[ix+1]-ax[ix]);
    return((1-t)*z[ix]+t*z[ix+1]);
  }
}

// 2D linear interpolation + extrapolation
double interpol_linear_extra_2D(double *ax,int NX,double *ay,int NY,double **z,double x,double y)
{
  if ((x<=ax[0])&&(y>ay[0])&&(y<ay[NY-1])) {
    double int1=interpol_linear_extra_1D(ay,NY,z[0],y);
    double int2=interpol_linear_extra_1D(ay,NY,z[1],y);
    return(int1+(int2-int1)/(ax[1]-ax[0])*(x-ax[0]));
  }
  else if ((x>=ax[NX-1])&&(y>ay[0])&&(y<ay[NY-1])) {
    double int1=interpol_linear_extra_1D(ay,NY,z[NX-2],y);
    double int2=interpol_linear_extra_1D(ay,NY,z[NX-1],y);
    return(int1+(int2-int1)/(ax[NX-1]-ax[NX-2])*(x-ax[NX-2]));
  }
  else if ((y<=ay[0])&&(x>ax[0])&&(x<ax[NX-1])) {
    int i;
    double tmp1[NX],tmp2[NX];
    for(i=0;i<NX;i++) {
      tmp1[i]=z[i][0];
      tmp2[i]=z[i][1];
    }
    double int1=interpol_linear_extra_1D(ax,NX,tmp1,x);
    double int2=interpol_linear_extra_1D(ax,NX,tmp2,x);
    return(int1+(int2-int1)/(ay[1]-ay[0])*(y-ay[0]));
  }
  else if ((y>=ay[NY-1])&&(x>ax[0])&&(x<ax[NX-1])) {
    int i;
    double tmp1[NX],tmp2[NX];
    for(i=0;i<NX;i++) {
      tmp1[i]=z[i][NY-2];
      tmp2[i]=z[i][NY-1];
    }
    double int1=interpol_linear_extra_1D(ax,NX,tmp1,x);
    double int2=interpol_linear_extra_1D(ax,NX,tmp2,x);
    return(int1+(int2-int1)/(ay[NY-1]-ay[NY-2])*(y-ay[NY-2]));
  }
  else {  // produces error via 'bisect' if both dimensions outside range
    int ix,iy;
    bisect_interpol(ax,NX,x,&ix);
    bisect_interpol(ay,NY,y,&iy);
    double t=(x-ax[ix])/(ax[ix+1]-ax[ix]);
    double u=(y-ay[iy])/(ay[iy+1]-ay[iy]);
    return((1-t)*(1-u)*z[ix][iy]+t*(1-u)*z[ix+1][iy]+(1-t)*u*z[ix][iy+1]+t*u*z[ix+1][iy+1]);
  }
}

// 3D linear interpolation + extrapolation
double interpol_linear_extra_3D(double *ax,int NX,double *ay,int NY,double *az,int NZ,double ***f,double x,double y,double z)
{
  if ((x<=ax[0])&&(y>ay[0])&&(y<ay[NY-1])&&(z>az[0])&&(z<az[NZ-1])) {   // x out of range, low
    double int1=interpol_linear_extra_2D(ay,NY,az,NZ,f[0],y,z);
    double int2=interpol_linear_extra_2D(ay,NY,az,NZ,f[1],y,z);
    return(int1+(int2-int1)/(ax[1]-ax[0])*(x-ax[0]));
  }
  else if ((x>=ax[NX-1])&&(y>ay[0])&&(y<ay[NY-1])&&(z>az[0])&&(z<az[NZ-1])) {   // x out of range, high
    double int1=interpol_linear_extra_2D(ay,NY,az,NZ,f[NX-2],y,z);
    double int2=interpol_linear_extra_2D(ay,NY,az,NZ,f[NX-1],y,z);
    return(int1+(int2-int1)/(ax[NX-1]-ax[NX-2])*(x-ax[NX-2]));
  }
  else if ((y<=ay[0])&&(x>ax[0])&&(x<ax[NX-1])&&(z>az[0])&&(z<az[NZ-1])) {   // y out of range, low
    int i,j;
    double **tmp1=malloc(NX*sizeof(double *));
    double **tmp2=malloc(NX*sizeof(double *));
    for(i=0;i<NX;i++) {
      tmp1[i]=malloc(NZ*sizeof(double));
      tmp2[i]=malloc(NZ*sizeof(double));
    }
    for(i=0;i<NX;i++) {
      for(j=0;j<NZ;j++) {
	tmp1[i][j]=f[i][0][j];
	tmp2[i][j]=f[i][1][j];
      }
    }
    double int1=interpol_linear_extra_2D(ax,NX,az,NZ,tmp1,x,z);
    double int2=interpol_linear_extra_2D(ax,NX,az,NZ,tmp2,x,z);
    for(i=0;i<NX;i++) {
      free(tmp1[i]);
      free(tmp2[i]);
    }
    free(tmp1);
    free(tmp2);
    return(int1+(int2-int1)/(ay[1]-ay[0])*(y-ay[0]));
  }
  else if ((y>=ay[NY-1])&&(x>ax[0])&&(x<ax[NX-1])&&(z>az[0])&&(z<az[NZ-1])) {   // y out of range, high
    int i,j;
    double **tmp1=malloc(NX*sizeof(double *));
    double **tmp2=malloc(NX*sizeof(double *));
    for(i=0;i<NX;i++) {
      tmp1[i]=malloc(NZ*sizeof(double));
      tmp2[i]=malloc(NZ*sizeof(double));
    }
    for(i=0;i<NX;i++) {
      for(j=0;j<NZ;j++) {
	tmp1[i][j]=f[i][NY-2][j];
	tmp2[i][j]=f[i][NY-1][j];
      }
    }
    double int1=interpol_linear_extra_2D(ax,NX,az,NZ,tmp1,x,z);
    double int2=interpol_linear_extra_2D(ax,NX,az,NZ,tmp2,x,z);
    for(i=0;i<NX;i++) {
      free(tmp1[i]);
      free(tmp2[i]);
    }
    free(tmp1);
    free(tmp2);
    return(int1+(int2-int1)/(ay[NY-1]-ay[NY-2])*(y-ay[NY-2]));
  }
  else if ((z<=az[0])&&(x>ax[0])&&(x<ax[NX-1])&&(y>ay[0])&&(y<ay[NY-1])) {   // z out of range, low
    int i,j;
    double **tmp1=malloc(NX*sizeof(double *));
    double **tmp2=malloc(NX*sizeof(double *));
    for(i=0;i<NX;i++) {
      tmp1[i]=malloc(NY*sizeof(double));
      tmp2[i]=malloc(NY*sizeof(double));
    }
    for(i=0;i<NX;i++) {
      for(j=0;j<NY;j++) {
	tmp1[i][j]=f[i][j][0];
	tmp2[i][j]=f[i][j][1];
      }
    }
    double int1=interpol_linear_extra_2D(ax,NX,ay,NY,tmp1,x,y);
    double int2=interpol_linear_extra_2D(ax,NX,ay,NY,tmp2,x,y);
    for(i=0;i<NX;i++) {
      free(tmp1[i]);
      free(tmp2[i]);
    }
    free(tmp1);
    free(tmp2);
    return(int1+(int2-int1)/(az[1]-az[0])*(z-az[0]));
  }
  else if ((z>=az[NZ-1])&&(x>ax[0])&&(x<ax[NX-1])&&(y>ay[0])&&(y<ay[NY-1])) {   // z out of range, high
    int i,j;
    double **tmp1=malloc(NX*sizeof(double *));
    double **tmp2=malloc(NX*sizeof(double *));
    for(i=0;i<NX;i++) {
      tmp1[i]=malloc(NY*sizeof(double));
      tmp2[i]=malloc(NY*sizeof(double));
    }
    for(i=0;i<NX;i++) {
      for(j=0;j<NY;j++) {
	tmp1[i][j]=f[i][j][NZ-2];
	tmp2[i][j]=f[i][j][NZ-1];
      }
    }
    double int1=interpol_linear_extra_2D(ax,NX,ay,NY,tmp1,x,y);
    double int2=interpol_linear_extra_2D(ax,NX,ay,NY,tmp2,x,y);
    for(i=0;i<NX;i++) {
      free(tmp1[i]);
      free(tmp2[i]);
    }
    free(tmp1);
    free(tmp2);
    return(int1+(int2-int1)/(az[NZ-1]-az[NZ-2])*(z-az[NZ-2]));
  }
  else {  // produces error via 'bisect' if more than 1 dimension outside range
    int ix,iy,iz;
    double f1a,f1b,f2a,f2b;

    bisect_interpol(ax,NX,x,&ix);
    bisect_interpol(ay,NY,y,&iy);
    bisect_interpol(az,NZ,z,&iz);

    double q1=(x-ax[ix])/(ax[ix+1]-ax[ix]);
    double q2=(y-ay[iy])/(ay[iy+1]-ay[iy]);
    double q3=(z-az[iz])/(az[iz+1]-az[iz]);
    double p1=1-q1;
    double p2=1-q2;

    f1a=p1*f[ix][iy][iz]+q1*f[ix+1][iy][iz];
    f1b=p1*f[ix][iy+1][iz]+q1*f[ix+1][iy+1][iz];
    f2a=p2*f1a+q2*f1b;

    f1a=p1*f[ix][iy][iz+1]+q1*f[ix+1][iy][iz+1];
    f1b=p1*f[ix][iy+1][iz+1]+q1*f[ix+1][iy+1][iz+1];
    f2b=p2*f1a+q2*f1b;

    return((1-q3)*f2a+q3*f2b);
  }
}

// 4D linear interpolation + extrapolation
double interpol_linear_extra_4D(double *ax,int NX,double *ay,int NY,double *az,int NZ,double *aw,int NW,double ****f,double x,double y,double z,double w)
{
  int ix;
  double f1,f2,t;

  if (((x<ax[0])||(x>ax[NX-1]))&&((y<ay[0])||(y>ay[NY-1])||(z<az[0])||(z>az[NZ-1])||(w<aw[0])||(w>aw[NW-1]))) {
    printf("Error in routine 'interpol_linear_extra_4D': attempt extrapolation in more than one dimension.\n");
    exit(-1);
  }

  if (x<=ax[0]) {
    f1=interpol_linear_extra_3D(ay,NY,az,NZ,aw,NW,f[0],y,z,w);
    f2=interpol_linear_extra_3D(ay,NY,az,NZ,aw,NW,f[1],y,z,w);
    return((f2-f1)/(ax[1]-ax[0])*(x-ax[0])+f1);
  }
  else if (x>=ax[NX-1]) {
    f1=interpol_linear_extra_3D(ay,NY,az,NZ,aw,NW,f[NX-2],y,z,w);
    f2=interpol_linear_extra_3D(ay,NY,az,NZ,aw,NW,f[NX-1],y,z,w);
    return((f2-f1)/(ax[NX-1]-ax[NX-2])*(x-ax[NX-1])+f2);
  }
  else {
    bisect_interpol(ax,NX,x,&ix);

    f1=interpol_linear_extra_3D(ay,NY,az,NZ,aw,NW,f[ix],y,z,w);
    f2=interpol_linear_extra_3D(ay,NY,az,NZ,aw,NW,f[ix+1],y,z,w);

    t=(x-ax[ix])/(ax[ix+1]-ax[ix]);
    return((1-t)*f1+t*f2);
  }
}

void mattimesmat_gen(gsl_matrix *a,gsl_matrix *b,gsl_matrix *erg,int dim1,int dim2,int dim3)
{
  int i,j,k;
  double sum;
  for (i=0;i<dim1;i++) {
    for (j=0;j<dim3;j++) {
      sum=0.0;
      for (k=0;k<dim2;k++) {
        sum+=gsl_matrix_get(a,i,k)*gsl_matrix_get(b,k,j);
      }
      gsl_matrix_set(erg,i,j,sum);
    }
  }
}

void mattimesmat(gsl_matrix *a,gsl_matrix *b,gsl_matrix *erg,int N)
{
  int i,j,k;
  double sum;
  for (i=0;i<N;i++) {
    for (j=0;j<N;j++) {
      sum=0.0;
      for (k=0;k<N;k++) {
        sum+=gsl_matrix_get(a,i,k)*gsl_matrix_get(b,k,j);
      }
      gsl_matrix_set(erg,i,j,sum);
    }
  }
}

void mattimesvec_gen(gsl_matrix *a,gsl_vector *b,gsl_vector *erg,int dim1,int dim2)
{
  int i,j;
  double sum;
  for (i=0;i<dim1;i++) {
    sum=0.0;
    for (j=0;j<dim2;j++) {
      sum+=gsl_matrix_get(a,i,j)*gsl_vector_get(b,j);
    }
    gsl_vector_set(erg,i,sum);
  }
}

void mattimesvec(gsl_matrix *a,gsl_vector *b,gsl_vector *erg,int N)
{
  int i,j;
  double sum;
  for (i=0;i<N;i++) {
    sum=0.0;
    for (j=0;j<N;j++) {
      sum+=gsl_matrix_get(a,i,j)*gsl_vector_get(b,j);
    }
    gsl_vector_set(erg,i,sum);
  }
}

int matrixinvers(gsl_matrix *m,gsl_matrix *inv,int DIM)
{
  int i,j,k,flag=0;
  double elem,sum;
  const int DOCHECK=1;

  gsl_matrix *v=gsl_matrix_alloc(DIM,DIM);
  gsl_matrix *id=gsl_matrix_alloc(DIM,DIM);
  gsl_matrix *u=gsl_matrix_alloc(DIM,DIM);
  gsl_vector *work=gsl_vector_alloc(DIM);
  gsl_vector *s=gsl_vector_alloc(DIM);

  gsl_matrix_memcpy(u,m);
  gsl_linalg_SV_decomp(u,v,s,work);

  for (i=0;i<DIM;i++) {
    elem=gsl_vector_get(s,i);
    if (fabs(elem)==0) {
      printf("Error: zero singular value!\n");
      exit(-1);
    }
    if (fabs(elem)<1.e-35) {
      printf("Possible singular value: %g\n",elem);
      flag=1;
    }
    //printf("eigenvalue: %g\n",elem);
  }

  for (i=0;i<DIM;i++) {
    for (j=0;j<DIM;j++) {
      sum=0.0;
      for (k=0;k<DIM;k++) {
	sum+=gsl_matrix_get(v,i,k)/gsl_vector_get(s,k)*gsl_matrix_get(u,j,k);
      }
      gsl_matrix_set(inv,i,j,sum);
    }
  }

  if (DOCHECK) {
    mattimesmat(m,inv,v,DIM);           //v reused
    gsl_matrix_set_identity(id);
    gsl_matrix_sub(v,id);                //result in v
    for (i=0;i<DIM;i++) {
      for (j=0;j<DIM;j++) {
	elem=gsl_matrix_get(v,i,j);
	if (fabs(elem)>1.e-2) {
	  printf("Problematic matrix inversion: %g\n",elem);
	  flag=2;
	  break;
	}
      }
    }
  }

  gsl_matrix_free(u);
  gsl_matrix_free(v);
  gsl_matrix_free(id);
  gsl_vector_free(work);
  gsl_vector_free(s);
  return(flag);
}

gsl_matrix *pseudoinvers(gsl_matrix *m,double thresh,double *eigen)
{
  int i,j,k;
  double sum;

  const int dim=(int)m->size1;
  if (m->size1!=m->size2) {
    printf("Error in 'pseudoinvers': non-square matrix - not implemented yet.\n");
    exit(-1);
  }

  gsl_matrix *res=gsl_matrix_alloc(dim,dim);
  gsl_matrix *v=gsl_matrix_alloc(dim,dim);
  gsl_matrix *cov=gsl_matrix_alloc(dim,dim);
  gsl_vector *work=gsl_vector_alloc(dim);
  gsl_vector *s=gsl_vector_alloc(dim);
  gsl_vector *invs=gsl_vector_alloc(dim);

  gsl_matrix_memcpy(cov,m);
  gsl_linalg_SV_decomp(cov,v,s,work);

  for (i=0;i<dim;i++) {
    eigen[i]=gsl_vector_get(s,i);
    if (eigen[i]<=thresh) gsl_vector_set(invs,i,0.0);
    else gsl_vector_set(invs,i,1./eigen[i]);
  }

  for (i=0;i<dim;i++) {
    for (j=0;j<dim;j++) {
      sum=0.0;
      for (k=0;k<dim;k++) {
	sum+=gsl_matrix_get(v,i,k)*gsl_vector_get(invs,k)*gsl_matrix_get(cov,j,k);
      }
      gsl_matrix_set(res,i,j,sum);
    }
  }

  gsl_matrix_free(cov);
  gsl_matrix_free(v);
  gsl_vector_free(work);
  gsl_vector_free(s);
  gsl_vector_free(invs);
  return(res);
}

int covinvers(gsl_matrix *m,gsl_matrix *inv,int DIM)
{
  #include<gsl/gsl_errno.h>
  gsl_set_error_handler_off();
  const int DOCHECK=1;

  int i,flag=0;
  gsl_matrix *check=gsl_matrix_alloc(DIM,DIM);
  gsl_vector *b=gsl_vector_alloc(DIM);

  gsl_matrix_memcpy(check,m);

  flag=gsl_linalg_cholesky_decomp(check);

  if (!flag) {
    for (i=0;i<DIM;i++) {
      gsl_vector_set_basis(b,i);
      gsl_linalg_cholesky_svx(check,b);
      gsl_matrix_set_col(inv,i,b);
    }

    if (DOCHECK) {
      gsl_matrix *id=gsl_matrix_alloc(DIM,DIM);
      gsl_matrix_set_identity(id);
      int j;
      double elem;

      mattimesmat(m,inv,check,DIM);           //check reused
      gsl_matrix_sub(check,id);               //result in check
      for (i=0;i<DIM;i++) {
	for (j=0;j<DIM;j++) {
	  elem=gsl_matrix_get(check,i,j);
	  if (fabs(elem)>1.e-6) {
	    printf("Problematic matrix inversion: %g\n",elem);
	    flag=2;
	    break;
	  }
	}
      }
      gsl_matrix_free(id);
    }
  }

  gsl_matrix_free(check);
  gsl_vector_free(b);
  return(flag);
}

double matrix_trace(gsl_matrix *a,int N)
{
  int i;
  double sum=0.0;
  for (i=0;i<N;i++) {
    sum+=gsl_matrix_get(a,i,i);
  }
  return(sum);
}

double matrix_det(gsl_matrix *a,int N)
{
  int sign;
  gsl_permutation *perm=gsl_permutation_alloc(N);
  gsl_matrix *LU=gsl_matrix_alloc(N,N);
  gsl_matrix_memcpy(LU,a);
  gsl_linalg_LU_decomp(LU,perm,&sign);
  gsl_permutation_free(perm);
  return(gsl_linalg_LU_det(LU,sign));
}

void matrix_transpose(gsl_matrix *a,gsl_matrix *t,int dim1,int dim2)
{
  int i,j;
  for (i=0;i<dim1;i++) {
    for (j=0;j<dim2;j++) {
      gsl_matrix_set(t,j,i,gsl_matrix_get(a,i,j));
    }
  }
  return;
}

void matrix_print(gsl_matrix *a,char *file)
{
  int i,j;
  FILE *dat;
  if ((dat=fopen(file,"w"))==NULL) {
    printf("Error in routine 'matrix_print': could not open file %s\n",file);
    exit(-1);
  }
  for (i=0;i<a->size1;i++) {
    for (j=0;j<a->size2;j++) {
      fprintf(dat,"%15.10g\t",gsl_matrix_get(a,i,j));
    }
    fprintf(dat,"\n");
  }
  fclose(dat);
  return;
}

void vector_print(gsl_vector *v,char *file)
{
  int i;
  FILE *dat;
  if ((dat=fopen(file,"w"))==NULL) {
    printf("Error in routine 'vector_print': could not open file %s\n",file);
    exit(-1);
  }
  for (i=0;i<v->size;i++) {
      fprintf(dat,"%15.10g\n",gsl_vector_get(v,i));
  }
  fclose(dat);
  return;
}

void solve_lse(gsl_matrix *m,gsl_vector *in,gsl_vector *res)
{
  if (m->size1!=res->size) {
    printf("Error in routine 'solve_lse': matrix row dimension does not match dimension of solution vector: %zu - %zu\n",m->size1,res->size);
    exit(-1);
  }
  if (m->size2!=in->size) {
    printf("Error in routine 'solve_lse': matrix column dimension does not match dimension of input vector: %zu - %zu\n",m->size2,in->size);
    exit(-1);
  }

  gsl_matrix *v=gsl_matrix_alloc(m->size2,m->size2);
  gsl_matrix *u=gsl_matrix_alloc(m->size1,m->size2);
  gsl_vector *work=gsl_vector_alloc(m->size2);
  gsl_vector *s=gsl_vector_alloc(m->size2);

  gsl_matrix_memcpy(u,m);
  gsl_linalg_SV_decomp(u,v,s,work);
  gsl_linalg_SV_solve(u,v,s,in,res);

  gsl_matrix_free(u);
  gsl_matrix_free(v);
  gsl_vector_free(work);
  gsl_vector_free(s);
  return;
}

// inverse of symmetric matrix via Cholesky decomposition
int inverse_symm(gsl_matrix *in,gsl_matrix *out)
{
  #include<gsl/gsl_errno.h>
  gsl_set_error_handler_off();

  int i,flag=0;
  const int dim=(int)in->size1;
  gsl_matrix *check=gsl_matrix_alloc(dim,dim);
  gsl_vector *b=gsl_vector_alloc(dim);
  gsl_matrix_memcpy(check,in);  // do not overwrite 'in'

  flag=gsl_linalg_cholesky_decomp(check);

  if (!flag) {
    for (i=0;i<dim;i++) {
      gsl_vector_set_basis(b,i);
      gsl_linalg_cholesky_svx(check,b);
      gsl_matrix_set_col(out,i,b);
    }
  }
  gsl_matrix_free(check);
  gsl_vector_free(b);
  return(flag);
}

int fisher_ellipse(gsl_matrix *fisher,int index1,int index2,double *semi_major,double *semi_minor,double *angle)
{
  int flag;

  // setup
  if (index1==index2) return(2);  // can only make ellipse for two different indices
  const int dim=fisher->size1;
  gsl_matrix *inv=gsl_matrix_alloc(dim,dim);
  gsl_matrix *sub=gsl_matrix_alloc(2,2);
  gsl_matrix *v=gsl_matrix_alloc(2,2);
  gsl_vector *work=gsl_vector_alloc(2);
  gsl_vector *s=gsl_vector_alloc(2);

  // invert Fisher matrix
  flag=inverse_symm(fisher,inv);

  // get ellipse plotting parameters
  gsl_matrix_set(sub,0,0,gsl_matrix_get(inv,index1,index1));
  gsl_matrix_set(sub,0,1,gsl_matrix_get(inv,index1,index2));
  gsl_matrix_set(sub,1,0,gsl_matrix_get(inv,index2,index1));
  gsl_matrix_set(sub,1,1,gsl_matrix_get(inv,index2,index2));

  gsl_linalg_SV_decomp(sub,v,s,work);
  *semi_major=sqrt(gsl_vector_get(s,0));
  *semi_minor=sqrt(gsl_vector_get(s,1));
  *angle=atan(gsl_matrix_get(v,1,0)/gsl_matrix_get(v,0,0));

  // clean up
  gsl_vector_free(work);
  gsl_matrix_free(v);
  gsl_matrix_free(inv);
  gsl_matrix_free(sub);
  gsl_vector_free(s);
  return(flag);
}

// provides KDE in 1D with Gaussians, using Silverman's rule if width<=0 on call
void kde_1D(double *dp,int nd,int nstep,double step_min,double step_max,double h,double *steps,double *func)
{
  int i,j;
  double width=1.;
  const double gauss_norm=1./sqrt(2.*PI);

  // set binning
  for (i=0;i<nstep;i++) {
    steps[i]=step_min+i*(step_max-step_min)/(1.*nstep-1.);
  }

  // get width of Gaussian
  if (h<=0.0) {  // apply Silverman's rule
    width=1.06*gsl_stats_sd(dp,1,nd)*pow(nd,-0.2);
  }
  else width=h;

  // perform KDE
  for (i=0;i<nstep;i++) {
    func[i]=0.0;
    for (j=0;j<nd;j++) {
      func[i]+=exp(-0.5*(steps[i]-dp[j])*(steps[i]-dp[j])/(width*width))/width;
    }
    func[i]*=gauss_norm;  // normalised to nd if integrated
  }

  return;
}


#ifndef SCALING_H_SAL4XICS
#define SCALING_H_SAL4XICS

#include <algorithm>
#include <gsl/gsl>
#include <type_traits>
#include <utility>

namespace sens_loc::math {

/// This class is a small wrapper to clarify the mathematical notation
/// of ranges. It does NOT distinguish between \f$[min, max)\f$,
/// \f$[min, max]\f$, \f$(min, max)\f$.
/// \tparam Real underlying type of the range, integer type for example
/// \warning this struct does not ensure that \p min is always smaller \p max.
template <typename Real>
struct numeric_range {
    static_assert(std::is_arithmetic_v<Real>);

    Real min;  ///< lower bound of the range
    Real max;  ///< uppoer bound of the range
};

/// This function scales \p value from the range \p source_range to the
/// new range \p target_range.
/// \note \p value is clamped to \p source_range
/// \pre the ranges are well formed (\p min is smaller \p max)
/// \returns the scaled value that is clamped to \p target_range.
template <typename Real>
inline constexpr Real scale(const numeric_range<Real>& source_range,
                            const numeric_range<Real>& target_range,
                            Real                       value) noexcept {
    static_assert(std::is_arithmetic_v<Real>);

    Expects(source_range.min < source_range.max);
    Expects(target_range.min < target_range.max);

    const Real clamped = std::clamp(value, source_range.min, source_range.max);

    Ensures(clamped <= source_range.max);
    Ensures(clamped >= source_range.min);

    const Real scaled =
        ((target_range.max - target_range.min) * (value - source_range.min)) /
            (source_range.max - source_range.min) +
        target_range.min;

    const Real target_clamped =
        std::clamp(scaled, target_range.min, target_range.max);

    Ensures(target_clamped <= target_range.max);
    Ensures(target_clamped >= target_range.min);

    return target_clamped;
}
}  // namespace sens_loc::math

#endif /* end of include guard: SCALING_H_SAL4XICS */


#include <stdio.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv2.h>

// y' = exp(x)
// https://www.wolframalpha.com/input/?i=y%27+%3D+2y+%2B+x%2C+y%280%29+%3D+1%2C+y%281%29
// https://www.wolframalpha.com/input/?i=1%2F4+*+%285+*+e%5E2+-+3%29
int odefunc (double x, const double y[], double f[], void *params)
{
    // f[0] = x+2*y[0];
	// pow(): 멱급수(a, b) = a^b (a의 b승)
	// math.h에 있으며 pow를 활용하기 위해서는 -lm 옵션이 추가로 필요합니다.
	// GSL 라이브러리를 활용하기 위해선 -lgsl 옵션이 필요합니다.
	f[0] = pow(M_E, x);

    return GSL_SUCCESS;
}

int * jac;

int main(void)
{
    int dim = 1;
	// 미방을 풀기 위한 데이터 타입
    gsl_odeiv2_system sys = {odefunc, NULL, dim, NULL};

	// 드라이버는 미방을 쉽게 풀 수 있도록 시간에 따른 변화, 제어, 스텝에 대한 래퍼
	// gsl_odeiv2_step_rkf45는 룬게쿠타 4-5차를 의미함
	// 다음 인자는 미분방정식의 시작지점
	// 절대적인 에러 바운드와 상대적인 에러 바운드
    gsl_odeiv2_driver * d = gsl_odeiv2_driver_alloc_y_new (&sys, gsl_odeiv2_step_rkf45, 1e-6, 1e-6, 0.0);
    int i;
    double x0 = 0.0,  xf = 10.0; /* start and end of integration interval */
    double x = x0;
    double y[1] = { 1 };  /* initial value */
	double tmp = 0.0;

    for (i = 0; tmp <= xf; tmp += 0.1)
    {
		// for문을 포함하여 아래 한 줄을 통해 상황에 따라
		// 간소화된 형태로 결과를 출력할 수도 있고
		// 시간이 좀 더 오래걸리더라도 더 정확한 데이터를 산출할 수도 있음
        double xi = x0 + tmp * (xf-x0) / xf;
        int status = gsl_odeiv2_driver_apply (d, &x, xi, y);

        if (status != GSL_SUCCESS)
        {
            printf ("error, return value=%d\n", status);
            break;
        }

        printf ("%.8e %.8e\n", x, y[0]);
    }

    gsl_odeiv2_driver_free (d);
    return 0;
}


#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_permutation.h>
#include "matrixUtils.h"
#include "../mcmc/mvnormMCMC/mvrandist.h"

/*A collection of matrix functions*/
/*Alexander G. Shanku*/
/*Wed 12/05/2012 12:44:41 EST*/

//// Makes a matrix filled with random U(0,1) elements ////
gsl_matrix fill_rand_matrix(gsl_matrix *X, gsl_rng *r_num){
	int i,j;
	/*int step = 0;*/
	int n = X->size1;
	int m = X->size2;
	for (i = 0; i < m; ++i){
		for (j = 0; j < n; ++j){
			gsl_matrix_set(X, i, j, gsl_rng_uniform(r_num));
		}
	}
	return(*X);
}

//// Makes a matrix filled with sequential elements, starting with 0 ////
gsl_matrix fill_matrix(gsl_matrix *X){
	int i,j;
	int n = X->size1;
	int m = X->size2;
	for (i = 0; i < m; ++i){
		for (j = 0; j < n; ++j){
			gsl_matrix_set(X, i, j, i+(j*m));
		}
	}
	return(*X);
}

//// Do LU decomp, get determinant and keep input matrix untouched ////
double matrix_determ(gsl_matrix *X){
	int s;
	int n = X->size1;
	int m = X->size2;
	gsl_matrix *a_copy = gsl_matrix_alloc(m, n);
	gsl_matrix_memcpy(a_copy, X );
	gsl_permutation *P = gsl_permutation_alloc(n);
	gsl_linalg_LU_decomp(a_copy, P, &s);
	double my_det = gsl_linalg_LU_det (a_copy, s);
	return(my_det);
}

//// Get trace of given matrix ////
double matrix_trace(gsl_matrix *X){
	int i, m;
	m = X->size1;
	double trace = 0.0;
	for(i=0;i<m;i++){
		trace += gsl_matrix_get(X,i,i);
	}
	return(trace);
}

//// Multivariate Gamma function ////
double mv_gamma(double a, double d){
	double val = 1.0;
	int i;
		for(i = 1; i <= d; i++){
		 val *= gsl_sf_gamma(a - (0.5 * (i - 1)));
		}
	val *=  pow(M_PI, (d * (d - 1) / 4.0));
	return(val);
}

//// Get Inverse Wishart PDF ////
double iwishpdf(gsl_matrix *X, gsl_matrix *Scale, gsl_matrix *inv, double dof){
	// From http://en.wikipedia.org/wiki/Inverse-Wishart_distribution
	double X_det, scale_det, denom, pdf, trace, numerator;
	int m = X->size1;
	int n = X->size2;

	// Allocate matrix for inv(X)
	gsl_matrix *for_mult = gsl_matrix_alloc(m, n);

	// Get determinant of X and Scale matrix
	X_det = matrix_determ(X);
	scale_det = matrix_determ(Scale);

	// Invert X
	inv_matrix(X,inv);

	// Multiple Scale * inv(X)
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Scale, inv, 1.0, for_mult);

	// Get trace of above.
	trace = matrix_trace(for_mult);

	numerator =  pow(scale_det, dof / 2.0) * pow(X_det, (-dof-m-1)/ 2.0) * exp(-0.5 * trace);
	denom = pow(2,dof * m / 2) * mv_gamma(dof/2, m);

	pdf = (numerator/denom);

	return(pdf);
}

//// Invert a matrix and keep input matrix untouched ////
gsl_matrix inv_matrix(gsl_matrix *X, gsl_matrix *inv){
	int s;
	int n = X->size1;
	int m = X->size2;
	gsl_matrix *a_copy = gsl_matrix_alloc(m, n);
	gsl_matrix_memcpy( a_copy, X );
	gsl_permutation *P = gsl_permutation_alloc(n);
	gsl_linalg_LU_decomp(a_copy, P, &s);
	gsl_linalg_LU_invert(a_copy, P, inv);
	return(*inv);
}

//// Print a matrix to stdout ////
void print_matrix(gsl_matrix *X){
	int i, j;
	int n = X->size1;
	int m = X->size2;
	printf("\n");
	for(i=0; i<n; i++){
		for(j=0; j<m; j++){
			printf("%05.2f ", gsl_matrix_get(X, i, j));
		}
		printf("\n");
	}
}

///////// Added Sat 01/19/2013 15:24:41 EST /////////

gsl_vector array_to_gsl_vec(gsl_vector *dest, double src[3]){
	int i;
	int n = dest->size;
		for (i = 0; i < n; ++i){

				gsl_vector_set(dest, i, src[i]);
			}
	return(*dest);
}

///// Transform user supplied matrix into a gsl matrix ////
gsl_matrix mdarray_to_gsl_mat(gsl_matrix *dest, double src[3][3]){
	int i, j;
	int n = dest->size1;
	int m = dest->size2;
		for (i = 0; i < m; ++i){
			for (j = 0; j < n; ++j){
				gsl_matrix_set(dest, i, j, src[i][j]);
			}
		}
	return(*dest);
}

//// Print a vector to stdout ////
void print_vector(gsl_vector *src){
	int i;
	int n = src->size;
	printf("\n");
	for (i = 0; i < n; ++i){
		printf("%05.4f ", gsl_vector_get(src,i));
	}
	printf("\n");
}

//// Do cholesky decomp to matrix ////
gsl_matrix get_chol(gsl_matrix *src, gsl_matrix *dest){
	gsl_matrix_memcpy(dest, src);
	gsl_linalg_cholesky_decomp(dest);
	return(*dest);
}

//// Add gaussian noise to chol decomp matrix ////
gsl_matrix mat_noise(gsl_matrix *src, gsl_rng *r_num){
	int i,j;
	int n = src->size1;
	int m = src->size2;
	double tmp;

	for(i = 0; i < n; i++){
		for(j = 0; j < m; j++){
			if(i >= j){
				tmp = gsl_matrix_get(src,i,j) + gsl_ran_gaussian(r_num,0.05);
				gsl_matrix_set(src, i, j, tmp);
			}

		}
	}
	return(*src);
}

//// Transform cholesky decomp matrix into vector ////
void fill_chol_vec(gsl_matrix *src, gsl_vector *dest){
	int i, j, count = 0;

	for(i = 0; i < src->size1; i++){
		for(j=0;j<src->size2;j++){
			if(i<=j){
				 gsl_vector_set(dest,count++, gsl_matrix_get(src,i,j));
			}

		}
	}
}

//// Add gaussian noise to each element in vector ////
gsl_vector vec_noise(gsl_rng *r_num, gsl_vector *src, gsl_vector *dest){
	int i;
	int n = src->size;
	double tmp;

	for(i=0;i<n;i++){
			tmp = gsl_vector_get(src,i) + gsl_ran_gaussian(r_num, 0.05);
			gsl_vector_set(dest,i,tmp);
		}
		return(*dest);
}

//// Multiply cholesky decomp matrix by its transpose ////
gsl_matrix chol_mult(gsl_matrix *src, gsl_matrix *dest){
	gsl_blas_dgemm(CblasNoTrans, CblasTrans,1.0, src, src,1.0, dest);
	return(*dest);
}

//// Zeros out everything but lower tri-- in place ////
//// From adk's adkGSL.c ////
gsl_matrix gsl_matrix_lower_tri(gsl_matrix *src){
	int i, j;
	for(i = 0; i < src->size1; i++){
		for(j = 0; j < src->size2; j++){
			if(i<j)	gsl_matrix_set(src,i,j,0.0);
		}
	}
	return(*src);
}

// Random Wishart matrix.  Taken from Andy Kern's mvnorm_gsl.c
/*void rwishartGelman(gsl_matrix *Scale, double dof){
*//* Wishart distribution random number generator */
/*
*	n	 gives the dimension of the random matrix
*	dof	 degrees of freedom
*	scale	 scale matrix of dimension n x n
*	result	 output variable with a single random matrix Wishart distribution generation
*/
/*	int i;
	gsl_vector gsl_vector_alloc(ndim)

	gsl_vector_set_all(mean,0.0);
	gsl_matrix_set_all(output,0.0);
	for(i = 0; i < dof; i++){
		gsl_vector_set_all(xm,0);
		rmvnorm_prealloc(r, n, mean, scale, work, xm);
		gsl_vector_outer_product(xm,xm,work2);
		gsl_matrix_add(output,work2);
	}
}
*/

/*void rwishartGelman(const gsl_rng *r, const int n, const int dof, const gsl_matrix *scale,
	gsl_matrix *work,gsl_matrix *work2, gsl_vector *mean,gsl_vector *xm, gsl_matrix *output)

rwishartGelman(r, ndim, nsnps+ndim, p2->winv,
			p2->work,p2->work2, p2->means, p2->xm, p2->covMat);

*/


/*
** Kendall's coefficient of concordance - Kendall's W
**
** G.Lohmann, July 2010
*/
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sort_double.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_cdf.h>

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

extern void gsl_sort_vector_index (gsl_permutation *,gsl_vector *);

#define SQR(x) ((x) * (x))
#define ABS(x) ((x) > 0 ? (x) : -(x))

double kendall(size_t **xrank,double *r,int n,int m,int otype)
{
  int i,j;
  double S,W,R,chi_square=0;
  double nx=0,mx=0;

  nx = (double)n;
  mx = (double)m;

  for (i=0; i<n; i++) {
    r[i] = 0;
    for (j=0; j<m; j++) r[i] += xrank[j][i];
  }

  R = 0;
  for (i=0; i<n; i++) R += r[i];
  R /= nx;

  S = 0;
  for (i=0; i<n; i++) S += SQR(r[i] - R);

  W = 12.0*S/(mx*mx*(nx*nx-1)*nx);
  if (otype == 0) return W;

  /* by central limit theorem, chi_square is approx normal for large n */
  /* see en.wikipedia.org/wiki/Chi-square_distribution */
  chi_square = (nx-1)*mx*W;
  return (chi_square - nx)/(sqrt(2.0*nx));
}

double VKendall_W(gsl_matrix *data,int nvoxels,int otype)
{
  int i,j,n,m;
  double W0;
  static double *r=NULL;
  static gsl_vector *vec = NULL;
  static size_t **xrank=NULL;
  static gsl_permutation *perm=NULL,*rank=NULL;

  m = data->size1;
  n = nvoxels;

  if (vec == NULL) {
    r    = (double *) calloc(data->size2,sizeof(double));
    vec  = gsl_vector_calloc(data->size2);
    perm = gsl_permutation_alloc(data->size2);
    rank = gsl_permutation_alloc(data->size2);

    xrank = (size_t **) calloc(m,sizeof(size_t *));
    for (i=0; i<m; i++) xrank[i] = (size_t *) calloc(data->size2,sizeof(size_t));
  }

  gsl_vector_set_all (vec,9999);
  for (i=0; i<m; i++) {
    for (j=0; j<n; j++) {
      gsl_vector_set(vec,j,gsl_matrix_get(data,i,j));
    }
    gsl_sort_vector_index (perm, vec);
    gsl_permutation_inverse (rank, perm);
    for (j=0; j<n; j++) xrank[i][j] = rank->data[j];
  }

  /* Kendall's W of measured data */
  W0 = kendall(xrank,r,n,m,otype);
  return W0;
}


#pragma once

#include <cmath>

#include <gsl/gsl_math.h>

bool to_fraction(double num, long& numerator, long& denominator){
    double sign = num < 0 ? -1 : 1;
    double g = std::abs(num);
    long a = 0, b = 1, c = 1, d = 0;
    long s;
    int iter = 0;
    numerator = 0, denominator = 1;
    while (iter++ < 1e6){
        s = (long) std::floor(g);
        numerator = a + s*c;
        denominator = b + s*d;
        a = c;
        b = d;
        c = numerator;
        d = denominator;
        g = 1.0/(g-s);
        if(gsl_fcmp(sign*numerator, num*denominator, 1e-13) == 0){
            numerator *= sign;
            return true;
        }
    }
    return false;
}


/*solver/integration.c
*
* Function for numerical integration
* using quadrature and Monte Carlo
* methods.
*
* Author: Benjamin Vatter.
* email: benjaminvatterj@gmail.com
* date: 15 August 2015
*/

#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include "integration.h"
#include "dbg.h"
#include <gsl/gsl_math.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_rng.h>

typedef struct {
	int dims;
	int depth;
	double *lb;
	double *ub;
	double *real_x;
	double epsabs;
	double epsrel;
	size_t limit;
	gsl_integration_workspace * workspace;
	double * abserr;
	nquad_function *f;
} nquad_params;

void gsl_error (const char * reason, const char * file, int line, int gsl_errno)
{
  gsl_stream_printf ("NQUAD ERROR", file, line, reason);

  if (gsl_errno != GSL_EROUND) {
	  fflush (stderr);
	  fflush (stdout);
	  abort ();
  } else {
  	log_warn("NQUAD ERROR: rounding error detected. This result might be unreliable.");
  	fflush (stderr);
  	return;
  }
}

/*
SUbroutine for recursive multidimensional integration
*/
double nquad_f(double x, void * params)
{
	nquad_params *in_pars = (nquad_params *) params;
	double result;
	double abserr;
	int step = in_pars->depth;
	// Add value
	if (step>0) {
	 	in_pars->real_x[step-1] = x;
	}
	if (in_pars->dims == in_pars->depth){
		result = in_pars->f->function(in_pars->real_x, in_pars->dims, in_pars->f->params);
		return result;
	} else {
		// Consistency
		assert((step+1) <= in_pars->dims);
		// increase depth
		in_pars->depth += 1;
		gsl_integration_workspace *w = gsl_integration_workspace_alloc(in_pars->limit);
		gsl_function F = {.function = &nquad_f, .params = in_pars};
		gsl_integration_qags(&F, in_pars->lb[step], in_pars->ub[step], in_pars->epsabs, in_pars->epsrel, in_pars->limit, w,&result, &abserr);
		//gsl_integration_qag(&F, in_pars->lb[step], in_pars->ub[step], in_pars->epsabs, in_pars->epsrel, in_pars->limit, 2, w,&result, &abserr);
		in_pars->abserr[step] = abserr;
		gsl_integration_workspace_free(w);
		in_pars->depth -= 1;
		return result;
	}
}

/*
An extension of gsl_integration_qags
to n dimensions. The first parameter is
the number or dimensions. the signature of
f must be
f(double *x, void * params)

Parameters:
n - dimensions
f - function and parameters
lb - lower bounds to integrals
ub - upper bounds to integrals
epsabs - desired absolute error in each integral (only in nested-quadrature)
epsrel - desired relative error in each integral (only in nested-quadrature)
limit - max number of iterations
result - on out contains result
abserr - on out contains estimated error
*/
void nquad(int n, nquad_function *f,  double *lb, double *ub,
           double epsabs, double epsrel, size_t limit, double * result, double *abserr)
{
	assert(n>=1);
	nquad_params *new_params = malloc(sizeof(nquad_params));
	new_params->dims = n;
	new_params->depth = 0;
	new_params->lb = lb;
	new_params->ub = ub;
	new_params->epsabs = epsabs;
	new_params->epsrel = epsrel;
	new_params->limit = limit;
	new_params->abserr = malloc(sizeof(double) * n);
	new_params->f = f;
	new_params->real_x = malloc(sizeof(double) * n);

	// Set error handle
	gsl_error_handler_t *old_handler = gsl_set_error_handler(&gsl_error);

	// integrate
	result[0] = nquad_f(0.0, new_params);
	abserr[0] = 1.0;
	int i;
	double err = 0.0;
	for (i = 0; i<n; i++){
		err = (err < new_params->abserr[i])? new_params->abserr[i]: err;
	}
	*abserr = err;

	// release real x
	free(new_params->real_x);
	free(new_params->abserr);
	free(new_params);
	// recover error handle
	gsl_set_error_handler(old_handler);
}

/* Monte Carlo integration routine
* It has the same signature as the nquad function
* but it doesn't really use the epsrel value. It is
* done so to allow interchanging those two functions
* during runtime.
*
* This is based on the Monte Carlo plain routine found
* in GSL, written by Michael Booth
*/
void mcint(int n, nquad_function *f, double *lb, double *ub, double epsabs,
           double epsrel, size_t limit, double *result, double *abserr)
{
	double vol=1.0, m = 0, q = 0;
	double *x = malloc(sizeof(double)*n);
	size_t i, j;
	const gsl_rng_type *T;
	gsl_rng *r;
	gsl_rng_env_setup ();
	T = gsl_rng_default;
	r = gsl_rng_alloc (T);

	/* Sanity check */
	for(i=0; i<n; i++) {
		if(lb[i] >= ub[i]) {
			GSL_ERROR("Lower bound is higher than upper bound. Null feasible region.", GSL_EINVAL);
		}
		if(ub[i] - lb[i] > GSL_DBL_MAX) {
			GSL_ERROR("Range of integration is too large, please rescale",
                     GSL_EINVAL);
		}
		vol *= ub[i] - lb[i];
	}

	j=0;

	do
	{
		for (i=0; i < n; i++) {
			x[i] = lb[i] + gsl_rng_uniform_pos(r) * (ub[i] - lb[i]);
		}
		double fval = (f->function)(x, n, f->params);
		double d = fval - m;
		m += d / (n + 1.0);
		q += d * d * (n/(n +1.0));

		if (j < 2){ *abserr = GSL_POSINF;}
		else {
			*abserr = vol * sqrt(q / (limit * (limit - 1.0)));
		}
		j++;
	} while((*abserr > epsabs) && j < limit);

	*result = vol * m;
	if (*abserr > epsabs){
		log_warn("Monte Carlo integration max evaluation %d was insufficient"
		"to achieve the desired absolute error of %e", (int)limit, epsabs);
	}

	free(x);
	gsl_rng_free (r);
}


#include <petsc.h>

int main(int argc, char **argv) {

    PetscErrorCode ierr;
    PetscMPIInt    rank;
    PetscInt       i;
    PetscReal      localval, globalsum;
    PetscReal      x = 1; // default value

    ierr = PetscInitialize(&argc, &argv, NULL,
                           "Compute e in parallel with PETSc.\n\n");
    if (ierr) return ierr;

    ierr = MPI_Comm_rank(PETSC_COMM_WORLD, &rank); CHKERRQ(ierr);

    ierr = PetscOptionsBegin(PETSC_COMM_WORLD, NULL,
                             "Maclaurin Series for exp(x)",
                             NULL); CHKERRQ(ierr);
      ierr = PetscOptionsReal("-x", "input to exp(x) function",
                              NULL, x, &x, NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);

    // compute  x^n / n!  where n = (rank of process) + 1
    localval = 1.;
    for (i = 1; i < rank+1; i++) localval *= x/i;

    // sum the contributions over all processes
    ierr = MPI_Allreduce(&localval, &globalsum, 1, MPIU_REAL, MPIU_SUM,
                         PETSC_COMM_WORLD); CHKERRQ(ierr);

    // output estimate of e and report on work from each process
    ierr = PetscPrintf(PETSC_COMM_WORLD,
                       "exp(x) is about %17.15f\n",
                       globalsum); CHKERRQ(ierr);

    ierr = PetscPrintf(PETSC_COMM_SELF,
                       "rank %d did %d flops\n",
                       rank, (rank > 0) ? rank-1 : 0); CHKERRQ(ierr);

    return PetscFinalize();
}


#include <err.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>

#define X   y[0]
#define V_X y[1]
#define Y   y[2]
#define V_Y y[3]

const double g = 9.81;        /* m/s^2 */
const double v_0 = 700.0;     /* m/s */
const double B2_m = 4.0e-5;   /* 1/m */
const double y_d = 1.0e4;     /* m */

int func(double t, const double y[], double f[], void *params);

int main(int argc, char *argv[]) {
    gsl_odeiv2_system sys = {func, NULL, 4, NULL};
    gsl_odeiv2_driver *driver = gsl_odeiv2_driver_alloc_y_new(
            &sys, gsl_odeiv2_step_rkf45, 1.0e-6, 1.0e-6, 0.0
    );
    int i = 1;
    double t = 0.0;
    double alpha = argc > 1 ? strtod(argv[1], NULL) : 1.0;
    double y[4];
    X = Y = 0.0;
    V_X = v_0*cos(alpha);
    V_Y = v_0*sin(alpha);
    while (Y >= 0.0) {
        double t_i = i/50.0;
        int status = gsl_odeiv2_driver_apply(driver, &t, t_i, y);
        if (status != GSL_SUCCESS) {
            warnx("apply status = %d", status);
            break;
        }
        printf("%.12le %.12le %.12le %.12le %.12le\n", t, X, Y, V_X, V_Y);
        i++;
    }
    gsl_odeiv2_driver_free(driver);
    return EXIT_SUCCESS;
}

#define DX_DT f[0]
#define DVX_DT f[1]
#define DY_DT f[2]
#define DVY_DT f[3]

int func(double t, const double y[], double f[], void *params) {
    double v = sqrt(V_X*V_X + V_Y*V_Y);
    double corr = exp(-Y/y_d);
    DX_DT  = V_X;
    DVX_DT = -B2_m*corr*v*V_X;
    DY_DT  = V_Y;
    DVY_DT = -g - B2_m*corr*v*V_Y;
    return GSL_SUCCESS;
}


/* roots/falsepos.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Reid Priedhorsky, Brian Gough
 *
 * 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 2 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.
 */

/* falsepos.c -- falsepos root finding algorithm

   The false position algorithm uses bracketing by linear interpolation.

   If a linear interpolation step would decrease the size of the
   bracket by less than a bisection step would then the algorithm
   takes a bisection step instead.

   The last linear interpolation estimate of the root is used. If a
   bisection step causes it to fall outside the brackets then it is
   replaced by the bisection estimate (x_upper + x_lower)/2.

*/

#include <config.h>

#include <stddef.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <float.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_roots.h>

#include "roots.h"

typedef struct
  {
    double f_lower, f_upper;
  }
falsepos_state_t;

static int falsepos_init (void * vstate, gsl_function * f, double * root, double x_lower, double x_upper);
static int falsepos_iterate (void * vstate, gsl_function * f, double * root, double * x_lower, double * x_upper);

static int
falsepos_init (void * vstate, gsl_function * f, double * root, double x_lower, double x_upper)
{
  falsepos_state_t * state = (falsepos_state_t *) vstate;

  double f_lower, f_upper ;

  *root = 0.5 * (x_lower + x_upper);

  SAFE_FUNC_CALL (f, x_lower, &f_lower);
  SAFE_FUNC_CALL (f, x_upper, &f_upper);

  state->f_lower = f_lower;
  state->f_upper = f_upper;

  if ((f_lower < 0.0 && f_upper < 0.0) || (f_lower > 0.0 && f_upper > 0.0))
    {
      GSL_ERROR ("endpoints do not straddle y=0", GSL_EINVAL);
    }

  return GSL_SUCCESS;

}

static int
falsepos_iterate (void * vstate, gsl_function * f, double * root, double * x_lower, double * x_upper)
{
  falsepos_state_t * state = (falsepos_state_t *) vstate;

  double x_linear, f_linear;
  double x_bisect, f_bisect;

  double x_left = *x_lower ;
  double x_right = *x_upper ;

  double f_lower = state->f_lower;
  double f_upper = state->f_upper;

  double w ;

  if (f_lower == 0.0)
    {
      *root = x_left ;
      *x_upper = x_left;
      return GSL_SUCCESS;
    }

  if (f_upper == 0.0)
    {
      *root = x_right ;
      *x_lower = x_right;
      return GSL_SUCCESS;
    }

  /* Draw a line between f(*lower_bound) and f(*upper_bound) and
     note where it crosses the X axis; that's where we will
     split the interval. */

  x_linear = x_right - (f_upper * (x_left - x_right) / (f_lower - f_upper));

  SAFE_FUNC_CALL (f, x_linear, &f_linear);

  if (f_linear == 0.0)
    {
      *root = x_linear;
      *x_lower = x_linear;
      *x_upper = x_linear;
      return GSL_SUCCESS;
    }

  /* Discard the half of the interval which doesn't contain the root. */

  if ((f_lower > 0.0 && f_linear < 0.0) || (f_lower < 0.0 && f_linear > 0.0))
    {
      *root = x_linear ;
      *x_upper = x_linear;
      state->f_upper = f_linear;
      w = x_linear - x_left ;
    }
  else
    {
      *root = x_linear ;
      *x_lower = x_linear;
      state->f_lower = f_linear;
      w = x_right - x_linear;
    }

  if (w < 0.5 * (x_right - x_left))
    {
      return GSL_SUCCESS ;
    }

  x_bisect = 0.5 * (x_left + x_right);

  SAFE_FUNC_CALL (f, x_bisect, &f_bisect);

  if ((f_lower > 0.0 && f_bisect < 0.0) || (f_lower < 0.0 && f_bisect > 0.0))
    {
      *x_upper = x_bisect;
      state->f_upper = f_bisect;
      if (*root > x_bisect)
        *root = 0.5 * (x_left + x_bisect) ;
    }
  else
    {
      *x_lower = x_bisect;
      state->f_lower = f_bisect;
      if (*root < x_bisect)
        *root = 0.5 * (x_bisect + x_right) ;
    }

  return GSL_SUCCESS;
}

static const gsl_root_fsolver_type falsepos_type =
{"falsepos",                            /* name */
 sizeof (falsepos_state_t),
 &falsepos_init,
 &falsepos_iterate};

const gsl_root_fsolver_type  * gsl_root_fsolver_falsepos = &falsepos_type;