#ifndef UTILS_HH
#define UTILS_HH

#include <cmath>      // for M_PI
#include <cassert>
#include <string>			// for string
#include <iostream>		// for cout
#include <iomanip>		// for setf
#include <sstream>		// for ostringstream
#include <fstream>		// for ofstream and ifstream
#include <vector>			// for std::vector
#include <unistd.h> 	// for usleep
#include <climits>		// for INT_MAX
#include <chrono>			// for chrono
#include <random> // for random numbers
#include <fstream> // for ifstream
#include <GL/glew.h>
#include <GLFW/glfw3.h>
//#include <glm/glm.hpp>
//#include <glm/gtc/matrix_transform.hpp>
using std::cout; using std::cerr; using std::endl; using std::ostream; using std::setw;
using std::string; using std::ostringstream; using std::ofstream; using std::ifstream; using std::istringstream;
using glm::vec2; using glm::vec3; using glm::vec4; using std::ifstream;
using glm::mat3; using glm::mat4;

const bool DEBUGGING = true;


//-------- max (stdvec): return largest element in vector
template<class T>
T max (std::vector<T> &a) {
	return *max_element(a.begin(), a.end()); 
}

string readLine (ifstream &f) {
	string s;
	getline (f, s);
	return s;
}

#include <algorithm> 
#include <cctype>
#include <locale>
// trim from start (in place)
static inline void ltrim(std::string &s) {
    s.erase(s.begin(), std::find_if(s.begin(), s.end(), [](unsigned char ch) {
        return !std::isspace(ch);
    }));
}

// trim from end (in place)
static inline void rtrim(std::string &s) {
    s.erase(std::find_if(s.rbegin(), s.rend(), [](unsigned char ch) {
        return !std::isspace(ch);
    }).base(), s.end());
}

// trim from both ends (in place)
static inline void trim(std::string &s) {
    ltrim(s);
    rtrim(s);
}

// trim from start (copying)
static inline std::string ltrim_copy(std::string s) {
    ltrim(s);
    return s;
}

// trim from end (copying)
static inline std::string rtrim_copy(std::string s) {
    rtrim(s);
    return s;
}

// trim from both ends (copying)
static inline std::string trim_copy(std::string s) {
    trim(s);
    return s;
}



template<class T>
ostream& operator<< (ostream& out, std::vector<T> &vec) {
	for (auto x:vec) cout << x << "\n";
	return out;
}



//=======================================================================
//  UTILITIES
//=======================================================================
ostream& operator<< (ostream& out, vec3 a) {
	for (int i=0; i<3; ++i) {out << setw(15) << a[i]; } out << endl; return out;
}
ostream& operator<< (ostream& out, mat3 a) {
	for (int i=0; i<3; ++i) { for (int j=0; j<3; ++j) { out << setw(15) << a[j][i]; } // Note order!!!!
	out << endl; } out << endl; return out;
}
const char* stringFromFile (const char* fileName) {
	std::ifstream ifs (fileName);
	if (!ifs.is_open()) {cerr << "Failed to open " << fileName << "!\n"; exit(1);}
	string* content = new string ( (std::istreambuf_iterator<char>(ifs) ),
																 (std::istreambuf_iterator<char>()    ) ); // Yeah, ugly
	return content->c_str();
}
vec3 normalize (vec3 v) {
	GLfloat k = 1/sqrt(v.x*v.x + v.y*v.y + v.z*v.z);
	return vec3 (k*v);
}	
mat3 rotateToAlign (vec3 cc, vec3 bb) { // Find R such that R.z==c and R.y==b (if possible)
	vec3 c = normalize(cc); // "Up" vector (the new z axis)
	vec3 b = normalize(bb); // "North" vector (a vector that is kinda along the new y axis)
	vec3 a = cross(b, c);   // "East" vector (the new x axis)
	if (dot(a,a) < 1e-8) {b = vec3 (1, 0, 0); a = cross(b, c);} // Attempt 2
	if (dot(a,a) < 1e-8) {b = vec3 (0, 1, 0); a = cross(b, c);} // Attempt 3 -- must succeed!
	a = normalize (a);
	b = cross(c, a);
	return mat3 (a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z);
} // Note that (a.x, a.y, a.z) is the first COLUMN of this matrix!
//=======================================================================
//  RANDOM NUMBERS (WRAPPER AROUND std::mt19937_64)
//=======================================================================
unsigned        seed = std::chrono::system_clock::now().time_since_epoch().count(); // 12345
std::mt19937_64 rng (seed);   // std::default_random_engine
float frand()         {return std::uniform_real_distribution<float> (0., 1.) (rng); }
double drand()      {return std::uniform_real_distribution<double> (0., 1.) (rng); }
int irand(int n)    {return std::uniform_int_distribution<int> (0, n-1) (rng);}
float frandNormal()   {return std::normal_distribution<float> (0., 1.) (rng); }
float sqr (float x) {return x*x;}

//########################################
// 		ARRAY CLASSES
//########################################
template<typename T>
class array2 {
	int xmax,ymax;
	T *data;
public:
	array2 (int xmax=0, int ymax=0) : xmax(xmax),ymax(ymax) {this->data = new T[size()];}
	int size0 () {return xmax;}
	int size1 () {return ymax;}
	int size () {return xmax*ymax;}
	void resize (int xmax, int ymax) {
		delete[] data;
		this->xmax = xmax;
		this->ymax = ymax;
		this->data = new T[size()];
	}
	T& operator() (const int i)              {return data[i];}
	T& operator() (const int x, const int y) {return data[x+xmax*y];}
	~array2 ()                               {delete[] data; }
};

template<typename T>
ostream& operator<< (ostream& out, array2<T> &a) {
	for (int x=0; x<a.size0(); ++x) {
		for (int y=0; y<a.size1(); ++y) {  // note order
			out << setw(15) << a(x,y); 
		}
		out << endl; 
	} 
	out << endl; 
	return out;
}





template <typename T>
void readArray (string filename, std::vector<T> &an, int nmax) {
	std::ifstream ifs (filename);
	if (ifs.fail()) {cerr << "readArray: failed to read " << filename << endl; abort();}
	an.resize (nmax);
	for (int n=0; n<nmax; ++n) ifs >> an[n];
	if (ifs.fail()) {cerr << "readArray: failed to read " << filename << endl; abort();}
	ifs.close ();
}
template <typename T>
void readArray (string filename, array2<T> &axy, int xmax, int ymax) {
	std::ifstream ifs (filename);
	if (ifs.fail()) {cerr << "readArray: failed to read " << filename << endl; abort();}
	axy.resize (xmax, ymax);
	for (int x=0; x<xmax; ++x) for (int y=0; y<ymax; ++y) ifs >> axy(x,y);
	if (ifs.fail()) {cerr << "readArray: failed to read " << filename << endl; abort();}
	ifs.close ();
}


//########################################
// 		PARAMETER PARSING
//########################################
// string readPar (string sInFileName, string sPar) {
// 	ifstream f (sInFileName);
// 	if (f.fail()) {cerr << "File " << sInFileName << " not found!\n"; abort();}
//   while (!f.eof()) {
// 		string s1,s2;
// 		f >> s1 >> s2;
// 		if (s1==sPar) {
// 			f.close ();
// 			return s2;
// 		}
// 	}
//   f.close ();
// 	return "";
// }
template<class T>
void readPar (string sInFileName, string sPar, T& var) {
	ifstream f (sInFileName);
	if (f.fail()) {cerr << "File " << sInFileName << " not found!\n"; abort();}
  while (!f.eof()) {
		string s = readLine(f);	
    ltrim (s);                  // remove whitespace from left
		
		auto p = s.find("#"); 			// find first "#"
		if (p!=string::npos) s = s.substr (0, p-1); // trim any comments starting at the #
		if (s=="") continue;        // empty string
		auto p1 = s.find("=");  		// split at the = sign
		if (p1==string::npos) {cerr << "Invalid parameter line!\n"; abort();}
		string s1,s2;  // we are expecting "PARNAME = PARVALUE"
		s1 = s.substr(0, p1); trim(s1);
		s2 = s.substr(p1+1, string::npos); trim(s2);
		//cerr << "KEY|" << s1 << "|    VALUE|" << s2 << "|" <<  endl;
		if (s1==sPar) {
			istringstream(s2) >> var; 
			cerr << "Setting " << s1 << " = " << var << endl;
			f.close ();
			return;		
		}
	}
	cerr << "Warning: parameter " << sPar << " not found, using default. " << endl;
	f.close ();
}




//=======================================================================
//  STOPWATCH (WRAPPER AROUND std::chrono)
//=======================================================================
class stopwatch {
public:
	std::chrono::time_point<std::chrono::high_resolution_clock> timeStarted;
	double timeElapsed;
	void tic() {timeStarted = std::chrono::high_resolution_clock::now();}
	void toc() {timeElapsed = std::chrono::duration<double> 
		(std::chrono::high_resolution_clock::now() - timeStarted) . count();}
} sw1,sw2;

//=======================================================================
//  RECORDER (WRITES TO history.dat)
//=======================================================================
class Recorder {
	std::ofstream ofsHistory;
public:
	Recorder()  { ofsHistory.open ("history.dat"); }
	~Recorder() { ofsHistory.close ();             }
	template <typename T>
	void add (std::vector<T> x) {
		for (auto item : x) { //=x.begin(),j=x.end(); i!=j; ++i) {
			ofsHistory << std::setw(16) << item;
		}
		ofsHistory << std::endl;
	}
};
//=======================================================================
//  REBLOCKER (PERFORMS ERROR ESTIMATION FROM MARKOV CHAIN DATA)
//=======================================================================
class TReblocker {
private:
	typedef long long int64;
	static const bool debugging = false;                // whether to print diagnostics
	float *aa;	float &a (int l, int i) {return aa[l*imax+i];}
	float *bb; float &b (int l, int i) {return bb[l*imax+i];}
	float *yy; float &y (int i) {return yy[i];}
	// The on-the-fly binary reblocking algorithm (Loh 2006) maintains a virtual binary heap.
	// l is the level of each node (l=0 being the lowest level).  Nodes can be left or right nodes.
	// a(l) is the value stored in the most recent left node at each level.
	// b(l) is the sum of squares of all nodes at the current level.
	// y is workspace.	
public:
	int64 nmax;     // number of samples
	int lmax;		    // maximum number of levels of the heap
	int imax;       // number of data components in each sample
	
	TReblocker () {
		aa=bb=yy=NULL; nmax=lmax=imax=0;
	}
	
	void init (int lmax, int imax) {
		this->lmax = lmax;
		this->imax = imax;
		this->nmax = 0;
		aa = new float[lmax*imax];
		bb = new float[lmax*imax];
		yy = new float[imax];
		cout << "Initialized reblocker with " << lmax << " " << imax << endl;
		//======== SET TO ZERO, ESPECIALLY b	!
		for (int l=0; l<lmax; ++l) for (int i=0; i<imax; ++i) a(l,i) = 0.;
		for (int l=0; l<lmax; ++l) for (int i=0; i<imax; ++i) b(l,i) = 0.;
		for (int i=0; i<imax; ++i) y(i) = 0.;
	}
	
	float sqr (float x)	{return x*x;}                 // utility
	
	void add (float *x, int imax) {
		if (nmax==0) init (48, imax);
		int64 nwork = nmax;
		for (int i=0; i<imax; i++) y(i) = x[i];
		for (int l=0; l<lmax; l++) {
			for (int i=0; i<imax; i++) b(l,i) += sqr( y(i) );
			// right node
			if (nwork&1) {	for (int i=0; i<imax; i++) y(i) += a(l, i);		nwork >>= 1;	}
			// left node
			else				 {	for (int i=0; i<imax; i++) a(l, i) = y(i);		break;				}
		}
		nmax++;
	}
	
	void report (float*& xavg, float*& xerr, float*& xtau) {
		int lUsed,lOptimal;
		int64 S,M;
		float sb[imax];					                           // sum of blockspins
		float embm[lmax][imax];	                       // error in mean of block mean
		float eembm[lmax][imax];	                      // error in error in mean of block mean
		if (xavg!=NULL) delete[] xavg;		xavg = new float[imax];
		if (xerr!=NULL) delete[] xerr;		xerr = new float[imax];
		if (xtau!=NULL) delete[] xtau;		xtau = new float[imax];
		//----- Sanity check
		if (nmax<=1) {cerr << "TReblocker::report(): nmax<=1, cannot estimate variance! \n"; abort();}
		//----- Find level of largest heap
		int64 nwork = nmax;
		for (lUsed=0; nwork; lUsed++) nwork >>= 1;
		
		//----- Accumulate sum from largest heap to smallest
		for (int i=0; i<imax; i++) sb[i] = 0.0;	        // running sum of blockspins
		
		for (int l=lUsed; l>=0; l--) {
			//----- Determine S (blocksize) and M (number of blocks)
			S = (1L << l);		M = nmax / S;               // integer division
			//----- Accumulate weight of all left nodes
			if (nmax & (1L << l)) {	for (int i=0; i<imax; i++) sb[i] += a(l, i);	}
			//----- Find variance of block mean and variance of variance of mean of block mean
			if (M >= 2) {
				for (int i=0; i<imax; i++) {
					float vbm = (b(l, i) - sqr(sb[i]) / M) / (sqr(S)*(M-1.0));
					float vmbm = vbm / M;
					embm[l][i] = sqrt(vmbm);
					eembm[l][i] = embm[l][i] / sqrt(2.0*(M-1));
				}
			}
		}
		
		//----- Choose optimal block size
		for (int i=0; i<imax; i++) {
			int l;											                           // Careful!
			for (l=0; l<lUsed-2; l++) {                 // Careful!
				float fractChange = (embm[l+1][i] - embm[l][i]) / eembm[l][i];
				if (fractChange < 0.25) break;
			}
			lOptimal = l;
			xavg[i] = sb[i]/nmax;
			xerr[i] = embm[lOptimal][i];
			float ztemp = sqr( embm[0][i] / embm[lOptimal][i]);
			if (ztemp >= 1.) ztemp = 1.;
			xtau[i] = 1/(2.*atanh (  ztemp ));          // weird formula; may result in tau=0!
			
			if (lOptimal == lUsed-2) {cerr << "TReblocker::report(): Warning: tau>simulation time!\n";}
			
			if (debugging) {
				// You may wish to dump the guts (a,b,y) also
				cout << "TReblocker::report():" << endl;
				cout << "nmax                 = " << nmax << endl;
				cout << "Levels used          = " << lUsed << endl;
				cout << "Statistic index    i = " << i << endl;
				cout << "Optimal block level  = " << lOptimal << endl;
				cout << "Mean                 = " << xavg[i] << endl;
				cout << "Error                = " << xerr[i] << endl;
				cout << "Autocorrelation time = " << xtau[i] << endl;
				if (i==0) { 
					std::ofstream ofs ("reblockreport.dat");
					for (int l=0; l<lUsed-1; l++) {
						ofs << setw(8) << l << setw(24) << embm[l][i] << setw(24) << eembm[l][i] << endl;
					}
				}
			}
			
		}
	}
};


#endif
