// Original model by Naoki Hiratani (N.Hiratani@gmail.com)
// Python-friendly interactive implementation by Roman Koshkin (roman.koshkin@gmail.com)

#include <algorithm>
#include <boost/format.hpp>
#include <chrono>
#include <cmath>
#include <deque>
#include <fstream>
#include <iostream>
#include <random>
#include <set>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <unordered_map>
#include <vector>

using namespace std;

// to complile
// g++ -std=gnu++11 -Ofast -shared -fPIC -ftree-vectorize -march=native -mavx bmm_9_haga_grid.cpp -o ./bmm.dylib
// g++ -std=gnu++11 -O3 -dynamiclib -ftree-vectorize -march=native -mavx bmm_7_haga.cpp -o ./bmm.dylib
// sudo /usr/bin/g++ -std=gnu++11 -Ofast -shared -fPIC -ftree-vectorize -march=native -mavx bmm_8_haga.cpp -o ./bmm.dylib
// icc -std=gnu++11 -O3 -shared -fPIC bmm_5_haga.cpp -o ./bmm.dylib
// g++ -std=gnu++11 -Ofast -shared -fPIC -ftree-vectorize -march=native -mavx -fopenmp bmm_9_haga.cpp -o ./bmm.dylib

struct Timer {
	std::chrono::time_point<std::chrono::high_resolution_clock> start, end;
	std::chrono::duration<float> duration;

	Timer() {
		start = std::chrono::high_resolution_clock::now();
	}

	/* when the function where this object is created returns,
	this object must be destroyed, hence this destructor is called */
	~Timer() {
		end = std::chrono::high_resolution_clock::now();
		duration = end - start;
		float ms = duration.count() * 1000.0f;
		std::cout << "Elapsed (c++ timer): " << ms << " ms." << std::endl;
	}
};

// class definition
class Model {
public:
	// input struct

	typedef struct
	{
		double alpha;
		double usd;
		double JEI;

		double T;
		double h;

		// int NE;
		// int NI;

		// probability of connection
		double cEE;
		double cIE;
		double cEI;
		double cII;

		// Synaptic weights
		double JEE;
		double JEEinit;
		double JIE;
		double JII;

		//initial conditions of synaptic weights
		double JEEh;
		double sigJ;

		double Jtmax;
		double Jtmin;

		// Thresholds of update
		double hE;
		double hI;

		double IEex;
		double IIex;
		double mex;
		double sigex;

		// Average intervals of update, ms
		double tmE;
		double tmI;

		//Short-Term Depression
		double trec;
		double Jepsilon;

		// Time constants of STDP decay
		double tpp;
		double tpd;
		double twnd;

		// Coefficients of STDP
		double g;

		//homeostatic
		int itauh;

		double hsd;
		double hh;

		double Ip;
		double a;

		double xEinit;
		double xIinit;
		double tinit;

		double U;
		double taustf;
		double taustd;
		double Cp;
		double Cd;
		bool HAGA;
		bool asym;
		double stimIntensity;
	} ParamsStructType;

	// returned struct
	typedef struct
	{
		double alpha;
		double usd;
		double JEI;

		double T;
		double h;

		int NE;
		int NI;

		// probability of connection
		double cEE;
		double cIE;
		double cEI;
		double cII;

		// Synaptic weights
		double JEE;
		double JEEinit;
		double JIE;
		double JII;

		//initial conditions of synaptic weights
		double JEEh;
		double sigJ;

		double Jtmax;
		double Jtmin;

		// Thresholds of update
		double hE;
		double hI;

		double IEex;
		double IIex;
		double mex;
		double sigex;

		// Average intervals of update, ms
		double tmE;
		double tmI;

		//Short-Term Depression
		double trec;
		double Jepsilon;

		// Time constants of STDP decay
		double tpp;
		double tpd;
		double twnd;

		// Coefficients of STDP
		double g;

		//homeostatic
		int itauh;

		double hsd;
		double hh;

		double Ip;
		double a;

		double xEinit;
		double xIinit;
		double tinit;

		double Jmin;
		double Jmax;
		double Cp;
		double Cd;
		int SNE;
		int SNI;
		int NEa;
		double t;

		double U;
		double taustf;
		double taustd;
		bool HAGA;
		bool asym;
		double stimIntensity;
		int cell_id;
	} retParamsStructType;

	// ostringstream ossSTP;
	// ofstream ofsSTP;
	double accum;

	int nstim;
	int NE, NI, N;
	int SNE, SNI; //how many neurons get updated per time step
	int NEa;			// Exact number of excitatory neurons stimulated externally
	int pmax;

	vector<vector<double>> Jo;
	vector<vector<double>> Ji;

	double alpha = 50.0; // Degree of log-STDP (50.0)
	double usd = 0.1;		 // Release probability of a synapse (0.05 - 0.5)
	double JEI = 0.15;	 // 0.15 or 0.20

	double pi = 3.14159265;
	double e = 2.71828182;

	double T = 1800 * 1000.0; // simulation time, ms
	double h = 0.01;					// time step, ms ??????

	// probability of connection
	double cEE = 0.2; //
	double cIE = 0.2; //
	double cEI = 0.5; //
	double cII = 0.5; //

	// Synaptic weights
	double JEE = 0.15;		 //
	double JEEinit = 0.15; // ?????????????
	double JIE = 0.15;		 //
	double JII = 0.06;		 //
	//initial conditions of synaptic weights
	double JEEh = 0.15; // Standard synaptic weight E-E
	double sigJ = 0.3;	//

	double Jtmax = 0.25; // J_maxˆtot
	double Jtmin = 0.01; // J_minˆtot // ??? NOT IN THE PAPER

	// WEIGHT CLIPPING     // ???
	double Jmax = 5.0 * JEE;	// ???
	double Jmin = 0.01 * JEE; // ????

	// Thresholds of update
	double hE = 1.0; // Threshold of update of excitatory neurons
	double hI = 1.0; // Threshold of update of inhibotory neurons

	double IEex = 2.0;	// Amplitude of steady external input to excitatory neurons
	double IIex = 0.5;	// Amplitude of steady external input to inhibitory neurons
	double mex = 0.3;		// mean of external input
	double sigex = 0.1; // variance of external input

	// Average intervals of update, ms
	double tmE = 5.0; //t_Eud EXCITATORY
	double tmI = 2.5; //t_Iud INHIBITORY

	//Short-Term Depression
	double trec = 600.0; // recovery time constant (tau_sd, p.13 and p.12)
	//double usyn = 0.1;
	double Jepsilon = 0.001; // BEFORE UPDATING A WEIGHT, WE CHECK IF IT IS GREATER THAN
													 // Jepsilon. If smaller, we consider this connection as
													 // non-existent, and do not update the weight.

	// Time constants of STDP decay
	double tpp = 20.0;	 // tau_p
	double tpd = 40.0;	 // tau_d
	double twnd = 500.0; // STDP window lenght, ms

	// Coefficients of STDP
	double Cp = 0.1 * JEE;			// must be 0.01875 (in the paper)
	double Cd = Cp * tpp / tpd; // must be 0.0075 (in the paper)

	//homeostatic
	//double hsig = 0.001*JEE/sqrt(10.0);
	double hsig = 0.001 * JEE; // i.e. 0.00015 per time step (10 ms)
	int itauh = 100;					 // decay time of homeostatic plasticity, (100s)

	double hsd = 0.1; // release probability
	double hh = 10.0; // SOME MYSTERIOUS PARAMETER

	double Ip = 1.0; // External current applied to randomly chosen excitatory neurons
	double a = 0.20; // Fraction of neurons to which this external current is applied

	double xEinit = 0.02; // the probability that an excitatory neurons spikes at the beginning of the simulation
	double xIinit = 0.01; // the probability that an inhibitory neurons spikes at the beginning of the simulation
	double tinit = 100.0; // period of time after which STDP kicks in

	vector<double> dvec;
	vector<int> ivec;
	deque<double> ideque; // <<<< !!!!!!!!

	vector<deque<double>> dspts; // <<<< !!!!!!!!
	vector<int> x;
	set<int> spts;

	int saveflag = 0;
	double t = 0;
	int tidx = -1;
	// bool trtof = true; // ?????? some flag
	double u;
	int j;
	vector<int> smpld;
	set<int>::iterator it;
	double k1, k2, k3, k4;
	bool Iptof = true;

	vector<vector<int>> Jinidx; /* list (len=3000) of lists. Each
        list lists indices of excitatory neurons whose weights are > Jepsilon */

	// we initialize (only excitatory) neurons with values of synaptic efficiency
	vector<double> ys;

	// classes to stream data to files
	ofstream ofsr;

	double tauh; // decay time of homeostatic plasticity, in ms
	double g;

	// method declarations
	Model(int, int, int); // construction
	double dice();
	double ngn();
	void sim(int);
	void setParams(ParamsStructType);
	retParamsStructType getState();
	vector<vector<double>> calc_J(double, double);
	vector<int> rnd_sample(int, int);
	double fd(double, double);

	vector<double> F;
	vector<double> D;
	double U = 0.6; // default release probability for HAGA
	double taustf;
	double taustd;
	bool HAGA;
	bool asym;

	void STPonSpike(int);
	void STPonNoSpike();
	void updateMembranePot(int);
	void checkIfStim(int);
	void STDP(int);
	void saveSTP();
	void reinitFD();
	void saveX();
	void saveDSPTS();
	void loadX(string);
	void loadDSPTS(string);
	deque<double> SplitString(string);
	deque<int> SplitString_int(string);
	void reinit_Jinidx();
	void saveSpts();
	void loadSpts(string);

	double acc0;
	double acc1;
	double acc2;
	double c0;
	double c1;
	double c2;
	double c;
	double acc;
	vector<int> hStim;
	double stimIntensity;
	int cell_id;

	// ostringstream ossa;
	// ofstream ofsa;
	// string fstra;

	// diagnostics:
	//         ostringstream ossb;
	//         ofstream ofsb;
	//         string fstrb;

	// void dumpClusters();

	// here you just declare pointers, but you must
	// ALLOCATE space on the heap for them in the class constructor
	double *ptr_Jo;
	double *ptr_F;
	double *ptr_D;
	double *ptr_ys;

	// flexible arrays can only be declared at the end of the class !!
	//         double sm[];
	// double* sm;
private:
	// init random number generator
	std::random_device m_randomdevice;
	std::mt19937 m_mt;
};

// double Model::dice(){
// 	return rand()/(RAND_MAX + 1.0);
// }

double Model::dice() {
	std::uniform_real_distribution<double> dist(0.0, 1.0);
	return dist(m_mt);
}

double Model::ngn() {
	// sample from a normal distribution based on two uniform distributions
	// WHY IS IT SO?
	double u = Model::dice();
	double v = Model::dice();
	return sqrt(-2.0 * log(u)) * cos(2.0 * pi * v);
}

// choose the neuron ids that will be updated at the current time step
vector<int> Model::rnd_sample(int ktmp, int Ntmp) { // when ktmp << Ntmp
	vector<int> smpld;
	int xtmp;
	bool tof;
	while (smpld.size() < ktmp) {
		xtmp = (int)floor(Ntmp * Model::dice());
		tof = true;
		// make sure that the sampled id isn't the same as any of the previous ones
		for (int i = 0; i < smpld.size(); i++) {
			if (xtmp == smpld[i]) {
				tof = false;
			}
		}
		if (tof)
			smpld.push_back(xtmp);
	}
	return smpld;
}

double Model::fd(double x, double alpha) {
	return log(1.0 + alpha * x) / log(1.0 + alpha);
}

void Model::STPonSpike(int i) {
	if (HAGA == 0) {
		ys[i] -= usd * ys[i];
	} else {
		F[i] += U * (1 - F[i]); // U = 0.6
		D[i] -= D[i] * F[i];
	}

	// remove it from the set of spiking neurons
	it = spts.find(i);
	if (it != spts.end()) {
		spts.erase(it++);
	}
	// and turn it OFF
	x[i] = 0;
}

void Model::STPonNoSpike() {
	// EVERY 10 ms Becuase STD is slow, we can apply STD updates every 10th step
	if (((int)floor(t / h)) % 10 == 0) {
		for (int i = 0; i < NE; i++) {
			if (HAGA == 0) { // @@
				if (t < 2000000) {
					ys[i] += hsd * (1.0 - ys[i]) / trec;
				} else {
					// 4th order Runge-Kutta
					k1 = (1.0 - ys[i]) / trec;
					k2 = (1.0 - (ys[i] + 0.5 * hsd * k1)) / trec;
					k3 = (1.0 - (ys[i] + 0.5 * hsd * k2)) / trec;
					k4 = (1.0 - (ys[i] + hsd * k3)) / trec;
					ys[i] += hsd * (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0;
				}
			} else {
				if (t < 2000000) {
					F[i] += hsd * (U - F[i]) / taustf; // @@ don't forget about hsd!!!
					D[i] += hsd * (1.0 - D[i]) / taustd;
				} else {
					// hsd = 1.0;								// @@ don't forget about hsd!!!
					k1 = (U - F[i]) / taustf;
					k2 = (U - (F[i] + 0.5 * hsd * k1)) / taustf; // @@ don't forget about hsd!!!
					k3 = (U - (F[i] + 0.5 * hsd * k2)) / taustf;
					k4 = (U - (F[i] + hsd * k3)) / taustf;
					F[i] += hsd * (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0;

					k1 = (1.0 - D[i]) / taustd;
					k2 = (1.0 - (D[i] + 0.5 * hsd * k1)) / taustd;
					k3 = (1.0 - (D[i] + 0.5 * hsd * k2)) / taustd;
					k4 = (1.0 - (D[i] + hsd * k3)) / taustd;
					D[i] += hsd * (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0;
				}
			}
		}
	}
}

void Model::saveDSPTS() {
	ofstream ofsDSPTS;
	// ofsDSPTS.open("DSPTS_" + std::to_string(cell_id) + "_" + std::to_string(t));
	ofsDSPTS.open("DSPTS_" + std::to_string(cell_id));
	ofsDSPTS.precision(10);

	for (int i = 0; i < NE; i++) {
		ofsDSPTS << i;
		for (int sidx = 0; sidx < dspts[i].size(); sidx++) {
			ofsDSPTS << " " << dspts[i][sidx];
		}
		ofsDSPTS << endl;
	}
}

void Model::saveX() {
	ofstream ofsX;
	// ofsX.open("X_" + std::to_string(cell_id) + "_" + std::to_string(t));
	ofsX.open("X_" + std::to_string(cell_id));
	ofsX.precision(10);
	ofsX << x[0];
	for (int i = 1; i < N; i++) {
		ofsX << " " << x[i];
	}
	ofsX << endl;
}

void Model::loadDSPTS(string tt) {
	dspts.clear();
	deque<double> iideque;
	// ifstream file("DSPTS_" + std::to_string(cell_id) + "_" + tt);
	ifstream file("DSPTS_" + std::to_string(cell_id));

	if (!file.is_open()) {
		throw "DSPTS file not found.";
	}
	string line;
	while (getline(file, line)) {
		iideque = SplitString(line.c_str());
		iideque.pop_front();
		dspts.push_back(iideque);
		// cout << i << endl;
		// cout << dspts[i].back() << endl;
	}
	file.close();
	cout << "DSPTS loaded" << endl;
}

void Model::loadX(string tt) {
	x.clear();
	deque<double> iideque;
	// ifstream file("X_" + std::to_string(cell_id) + "_" + tt);
	ifstream file("X_" + std::to_string(cell_id));
	if (!file.is_open()) {
		throw "X file not found.";
	}
	string line;
	while (getline(file, line)) {
		iideque = SplitString(line.c_str());
		for (int i = 0; i < N; i++) {
			x.push_back(iideque[i]);
		}
	}
	file.close();
	cout << "X loaded" << endl;
}

deque<double> Model::SplitString(string line) {
	deque<double> iideque;
	string temp = "";
	for (int i = 0; i < line.length(); ++i) {
		if (line[i] == ' ') {
			iideque.push_back(stod(temp));
			temp = "";
		} else {
			temp.push_back(line[i]);
		}
	}
	iideque.push_back(stod(temp));
	return iideque;
}

deque<int> Model::SplitString_int(string line) {
	deque<int> iideque;
	string temp = "";
	for (int i = 0; i < line.length(); ++i) {
		if (line[i] == ' ') {
			iideque.push_back(stoi(temp));
			temp = "";
		} else {
			temp.push_back(line[i]);
		}
	}
	return iideque;
}

void Model::updateMembranePot(int i) {
	// WE update the membrane potential of the chosen excitatory neuron (eq.4, p.12)
	// -threshold of update + steady exc input * mean/var of external stim input
	u = -hE + IEex * (mex + sigex * ngn()); // pentagon, p.12
	it = spts.begin();
	//we go over all POSTsynaptic neurons that are spiking now
	while (it != spts.end()) {
		//if a postsynaptic spiking neuron happens to be excitatory,
		if (*it < NE) {
			if (HAGA == 0) {
				u += ys[*it] * Jo[i][*it];
			} else {
				u += F[*it] * D[*it] * Jo[i][*it];
			}
			//if a postsynaptic spiking neuron happens to be inhibitory,
		} else {
			u += Jo[i][*it];
		}
		++it;
	}
}

void Model::checkIfStim(int i) {
	if (hStim[i] == 1) {
		if (dice() < stimIntensity) {
			u += Ip;
		}
		// std::cout << "stim @ t=" << t << " on neuron " << i << std::endl;
	}
}

void Model::STDP(int i) {
	// if( ( (int)floor(t/h) )%1000 == 0 ){
	//     std::cout << "USING STDP" << std::endl;
	// }
	// if the POSTsynaptic neuron chosen for update exceeds the threshold, we save its ID in the set "spts"
	spts.insert(i);

	/* dspts saves the TIME of this spike to a DEQUE, such that each row id of this deque
	corresponds to the id of the spiking neuron. The row records the times at which that
	neuron emitted at spike. */
	dspts[i].push_back(t); // SHAPE: (n_postsyn x pytsyn_sp_times)
	x[i] = 1;
	// // record a spike on an EXCITATORY neuron (because STDP is only on excitatory neurons)
	if (saveflag == 1){
		ofsr << t << " " << i << endl; // record a line to file
	}

	// BEGIN STDP ******************************************************************************
	// **************** CHECK THIS IMPLEMENTATION, I DON'T LIKE IT ****************************
	// *****************************************************************************************
	/* First (LTD), we treat the chosen neuron as PREsynaptic and loop over all the POSTSYNAPTIC excitatory
	neurons that THE CHOSEN NEURON synapses on. Since we're at time t (and this is the latest time),
	the spikes recorded on those "POSTsynaptic" neurons will have an earlier timing than the spike
	recorded on the currently chosen neuron (that we treat as PREsynaptic). This indicates that
	the synaptic weight between this chosen neuron (presynaptic) and all the other neurons
	(postsynaptic) will decrease.  */

	for (int ip = 0; ip < NE; ip++) {
		if (Jo[ip][i] > Jepsilon && t > tinit) {
			// dspts is a deque of spiking times on the ith POSTSYNAPTIC neurons
			for (int sidx = 0; sidx < dspts[ip].size(); sidx++) {
				// { ?????? Eq.7 p.12 }
				/* depression happens if presynaptic spike time (t) happens AFTER
				a postsynaptic spike time (dspts[ip][sidx])
				BY THE WAY: HERE WE HAVE ASYMMETRIC STDP */
				if (HAGA == 0) {
					if (asym == 1) {
						Jo[ip][i] -= Cd * fd(Jo[ip][i] / JEE, alpha) * exp(-(t - dspts[ip][sidx]) / tpd); /// !!! <<<<<<<<<<<<<<<<<<<<< !!!!!!
					} else {
						// old (and apparently wrong) way
						// Jo[ip][i] += Cd*fd(Jo[ip][i]/JEE, alpha)*exp( -(t-dspts[ip][sidx])/tpd );
						// new (and hopefully right) way. See STDP_explained.ipynb
						Jo[ip][i] += Cp * exp((dspts[ip][sidx] - t) / tpp) - fd(Jo[ip][i] / JEE, alpha) * Cd * exp((dspts[ip][sidx] - t) / tpd);
					}
				} else {
					if (asym == 1) {
						Jo[ip][i] -= F[i] * D[i] * Cd * fd(Jo[ip][i] / JEE, alpha) * exp(-(t - dspts[ip][sidx]) / tpd);
					} else {
						// old (and apparently wrong) way
						// Jo[ip][i] += F[i] * D[i] * Cd*fd(Jo[ip][i]/JEE, alpha)*exp( -(t-dspts[ip][sidx])/tpd );
						// new (and hopefully right) way. See STDP_explained.ipynb

						// STP-dependent (new HAGA)
						Jo[ip][i] += F[i] * D[i] * (Cp * exp((dspts[ip][sidx] - t) / tpp) - fd(Jo[ip][i] / JEE, alpha) * Cd * exp((dspts[ip][sidx] - t) / tpd));
						// STP-independent (new Hiratani)
						// Jo[ip][i] += (Cp * exp((dspts[ip][sidx] - t) / tpp) - fd(Jo[ip][i] / JEE, alpha) * Cd * exp((dspts[ip][sidx] - t) / tpd));
					}
				}
			}
			// we force the weights to be no less than Jmin, THIS WAS NOT IN THE PAPER
			if (Jo[ip][i] > Jmax)
				Jo[ip][i] = Jmax;
			if (Jo[ip][i] < Jmin)
				Jo[ip][i] = Jmin;
		}
	}

	// (LTP)

	/* Jinidx is a list of lists (shape (2500 POSTsyn, n PREsyn)). E.g. if in row 15 we have number 10,
	it means that the weight between POSTsynaptic neuron 15 and presynaptc neuron 10 is greater than Jepsilon */

	/* we treat the currently chosen neuron as POSTtsynaptic, and we loop over all the presynaptic
	neurons that synapse on the current postsynaptic neuron. At time t (the latest time, and we don't
	yet know any spikes that will happen in the future) all the spikes on the presynaptic neurons with
	id j will have an earlier timing that the spike on the currently chosen neuron i (that we treat
	as postsynaptic for now). This indicates that the weights between the chosen neuron treated as post-
	synaptic for now and all the other neurons (treated as presynaptic for now) will be potentiated.  */

	for (int jidx = 0; jidx < Jinidx[i].size(); jidx++) {
		j = Jinidx[i][jidx]; // at each loop we get the id of the jth presynaptic neuron with J > Jepsilon
		if (t > tinit) {
			for (int sidx = 0; sidx < dspts[j].size(); sidx++) {
				// we loop over all the spike times on the jth PRESYNAPTIC neuron
				if (HAGA == 0) {
					if (asym == 1) {
						Jo[i][j] += g * Cp * exp(-(t - dspts[j][sidx]) / tpp);
					} else {
						Jo[i][j] += Cp * exp(-(t - dspts[j][sidx]) / tpp) - fd(Jo[i][j] / JEE, alpha) * Cd * exp(-(t - dspts[j][sidx]) / tpd);
					}
				} else {
					if (asym == 1) {
						Jo[i][j] += F[j] * D[j] * g * Cp * exp(-(t - dspts[j][sidx]) / tpp);
					} else {
						// STP-dependent (по новому определению Haga)
						Jo[i][j] += F[j] * D[j] * (Cp * exp(-(t - dspts[j][sidx]) / tpp) - fd(Jo[i][j] / JEE, alpha) * Cd * exp(-(t - dspts[j][sidx]) / tpd));
						// STP-independent (по новому определению Hiratani: не так как было у Хиратани в статье, а просто без F*D)
						// Jo[i][j] += (Cp * exp(-(t - dspts[j][sidx]) / tpp) - fd(Jo[i][j] / JEE, alpha) * Cd * exp(-(t - dspts[j][sidx]) / tpd));
					}
				}
				// as per eq. (7) p.12, it actually should be
				// Jo[i][j] += g * Cp * exp((dspts[j][sidx] - t)/tpp)
				// because dspts[j][sidx] is t_pre, and t is t_post
			}
			// we force the weights to be no more than Jmax, THIS WAS NOT IN THE PAPER
			if (Jo[i][j] > Jmax)
				Jo[i][j] = Jmax;
			if (Jo[i][j] < Jmin)
				Jo[i][j] = Jmin;
		}
	}
}

// void Model::saveSTP() {
// 	if (HAGA == false) {
// 		accum = 0;
// 	    ofsSTP << t;
// 		for (int i = 0; i < 5; i++){
// 			accum = 0;
// 			for(int j = 0; j < 500; j++){
// 				accum += ys[j+500*i];
// 			}
// 			ofsSTP << " " << accum/500;
// 		}
// 		ofsSTP << std::endl;
// 	} else {
// 		accum = 0;
// 	    ofsSTP << t;
// 		for (int i = 0; i < 5; i++){
// 			accum = 0;
// 			for(int j = 0; j < 500; j++){
// 				accum += F[j+500*i] * D[j+500*i];
// 			}
// 			ofsSTP << " " << accum/500;
// 		}
// 		ofsSTP << std::endl;
// 	}
// }

// void Model::saveSTP() {
// 	if (HAGA == false) {
// 	    ofsSTP << t;
// 		for (int i = 0; i < NE; i++){
// 			ofsSTP << " " << ys[i];
// 		}
// 		ofsSTP << std::endl;
// 	} else {
// 	    ofsSTP << t;
// 		for (int i = 0; i < NE; i++){
// 			ofsSTP << " " << F[i] * D[i];
// 		}
// 		ofsSTP << std::endl;
// 	}
// }

void Model::sim(int interval) {
	// dumpClusters();
	// std::cout << "SNE = " << SNE << ", SNI = " << SNI << std::endl;
	std::cout << "t = " << t << std::endl;
	Timer timer;

	// HAVING INITIALIZED THE NETWORK, WE go time step by time step
	while (interval > 0) {

		// // save STP states every millisecond
		// if( (int)floor(t/h) % 1000 == 0) {
		//     saveSTP();
		// }

		t += h;
		interval -= 1;

		// we decide which EXCITATORY neurons will be updated
		// they may or may not be spiking at the current step
		smpld = rnd_sample(SNE, NE);

		// to log the distribution of sampled neurons
		// ofsb << smpld[0] << " " << smpld[1] << " " << smpld[2] << " " << smpld[3] << " " << smpld[4] << " " << std::endl;

		// we cycle through those chosen neurons
		for (int iidx = 0; iidx < smpld.size(); iidx++) {
			int i = smpld[iidx];

			// STP (empty square, eq. 6 p.12)
			// if a chosen neuron is ALREADY on
			if (x[i] == 1) {
				STPonSpike(i);
			}
			// either way
			updateMembranePot(i);
			checkIfStim(i);

			// WE PERFORM AN STDP on the chosen neuron if it spikes u > 0
			if (u > 0) {
				STDP(i);
			}
		}

		// we sample INHIBITORY neurons to be updated at the current step
		smpld = rnd_sample(SNI, NI);

		for (int iidx = 0; iidx < smpld.size(); iidx++) {
			int i = NE + smpld[iidx];
			/* if this inhibitory neuron is spiking we set it to zero
			in the binary vector x and remove its index from the set
			of currently spiking neurons */

			// crazy optimization, but in fact eq.(5) p.12 (filled circle)
			if (x[i] == 1) {
				it = spts.find(i);
				if (it != spts.end()) {
					// removing a spike time from the SET of spikes on inhibitory neurons is the same as
					// subtracting them
					spts.erase(it++);
				}
				x[i] = 0;
			}

			//update the membrane potential on a chosen inhibitory neuron
			u = -hI + IIex * (mex + sigex * ngn()); // hexagon, eq.5, p.12
			it = spts.begin();
			while (it != spts.end()) {
				u += Jo[i][*it];
				++it;
			}

			// if the membrane potential on the currently chosen INHIBITORy neuron
			// is greater than the threshold, we record a spike on this neuron.
			if (u > 0) {
				spts.insert(i);
				x[i] = 1;
				// // record a spike on an INHIBITORY NEURON
				if (saveflag == 1) {
					ofsr << t << " " << i << endl;
				}
			}
		}

		STPonNoSpike();

		// EVERY 10 ms Homeostatic plasticity, weight clipping, boundary conditions, old spike removal
		if (((int)floor(t / h)) % 1000 == 0) {

			//Homeostatic Depression
			for (int i = 0; i < NE; i++) {
				for (int jidx = 0; jidx < Jinidx[i].size(); jidx++) {
					j = Jinidx[i][jidx];
					// ?????????? THAT'S NOT EXACTLY WHAT THE PAPER SAYS
					k1 = (JEEh - Jo[i][j]) / tauh;
					k2 = (JEEh - (Jo[i][j] + 0.5 * hh * k1)) / tauh;
					k3 = (JEEh - (Jo[i][j] + 0.5 * hh * k2)) / tauh;
					k4 = (JEEh - (Jo[i][j] + hh * k3)) / tauh;
					Jo[i][j] += hh * (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0 + hsig * ngn();
					// we clip the weights from below and above
					if (Jo[i][j] < Jmin)
						Jo[i][j] = Jmin; // ????? Jmin is zero, not 0.0015, as per Table 1
					if (Jo[i][j] > Jmax)
						Jo[i][j] = Jmax;
				}
			}

			//boundary condition
			for (int i = 0; i < NE; i++) {
				double Jav = 0.0;
				for (int jidx = 0; jidx < Jinidx[i].size(); jidx++) {
					// find the total weight per each postsynaptic neuron
					Jav += Jo[i][Jinidx[i][jidx]];
				}
				// find mean weight per each postsynaptic neuron
				Jav = Jav / ((double)Jinidx[i].size());
				if (Jav > Jtmax) {
					for (int jidx = 0; jidx < Jinidx[i].size(); jidx++) {
						j = Jinidx[i][jidx];
						// if the total weight exceeds Jtmax, we subtract the excess value
						Jo[i][j] -= (Jav - Jtmax);
						// but if a weight is less that Jmin, we set it to Jmin (clip from below)
						if (Jo[i][j] < Jmin) {
							Jo[i][j] = Jmin;
						}
					}

					// if the total weight is less that Jtmin
				} else if (Jav < Jtmin) {
					for (int jidx = 0; jidx < Jinidx[i].size(); jidx++) {
						j = Jinidx[i][jidx];
						/* ???????? we top up each (!!!???) weight by the difference
						between the total min and current total weight */
						Jo[i][j] += (Jtmin - Jav);
						// but if a weight is more that Jmax, we clip it to Jmax
						if (Jo[i][j] > Jmax) {
							Jo[i][j] = Jmax;
						}
					}
				}
			}

			// remove spikes older than 500 ms
			for (int i = 0; i < NE; i++) {
				for (int sidx = 0; sidx < dspts[i].size(); sidx++) {
					//if we have spike times that are occured more than 500 ms ago, we pop them from the deque
					if (t - dspts[i][0] > twnd) {
						dspts[i].pop_front();
					}
				}
			}
		}

		// EVERY 1s
		if (((int)floor(t / h)) % (1000 * 100) == 0) {

			tidx += 1; // we count the number of 1s cycles
			int s = 0;
			it = spts.begin();
			while (it != spts.end()) {
				++s;
				++it;
			}

			// exit if either no neurons are spiking or too many spiking after t > 200 ms
			if (s == 0 || (s > 1.0 * NE && t > 200.0)) {
				std::cout << "Exiting because either 0 or too many spikes at t =" << t << std::endl;
				break;
			}
		}
	}
}

// initialize the weight matrix
vector<vector<double>> Model::calc_J(double JEEinit, double JEI) {
	vector<vector<double>> J;
	int mcount = 0;
	for (int i = 0; i < NE; i++) {
		J.push_back(dvec);
		for (int j = 0; j < NE; j++) {
			J[i].push_back(0.0);
			// first E-E weights consistent with the E-E connection probability
			if (i != j && dice() < cEE) {

				// @@ !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1
				J[i][j] = JEEinit * (1.0 + sigJ * ngn());
				// if (HAGA == 1) {
				// 	J[i][j] = JEEinit * (1.0 + sigJ*ngn()); // should be JEEinit, but the matrix is initialized together with the model, but the parameters are set later
				// } else {
				// 	J[i][j] += JEEinit*(1.0 + sigJ*ngn());
				// }

				// if some weight is out of range, we clip it
				if (J[i][j] < Jmin)
					J[i][j] = Jmin;
				if (J[i][j] > Jmax)
					J[i][j] = Jmax;
			}
		}
		// then the E-I weights
		for (int j = NE; j < N; j++) {
			J[i].push_back(0.0); // here the matrix J is at first of size 2500, we extend it
			if (dice() < cEI) {
				J[i][j] -= JEI; /* becuase jth presynaptic inhibitory synapsing on
				an ith excitatory postsynaptic neuron should inhibit it. Hence the minus */
			}
		}
	}

	// then the I-E and I-I weights
	for (int i = NE; i < N; i++) {
		J.push_back(dvec);
		for (int j = 0; j < NE; j++) {
			J[i].push_back(0.0);
			if (dice() < cIE) {
				J[i][j] += JIE;
			}
		}
		for (int j = NE; j < N; j++) {
			J[i].push_back(0.0);
			if (i != j && dice() < cII) {
				J[i][j] -= JII;
			}
		}
	}

	return J;
}

void Model::reinitFD() {
	for (int i = 0; i < NE; i++) {
		F[i] = U; // @@ push back usd (0.99)
		D[i] = 1.0;
		ys[i] = 1.0 / (1.0 + usd * 0.05 * trec / tmE);
	}
}

void Model::reinit_Jinidx() {
	Jinidx.clear();
	for (int i = 0; i < NE; i++) {
		Jinidx.push_back(ivec); // shape = (3000, max3000)
		for (int i2 = 0; i2 < NE; i2++) {
			if (Jo[i][i2] > Jepsilon) {
				Jinidx[i].push_back(i2);
			}
		}
	}
}

void Model::saveSpts() {
	//   for (it = spts.begin(); it != spts.end(); it++) {
	//       cout << *it << endl;
	//   }
	// same as above (using an iterator, but more pythonic for ... in...)
	ofstream ofsSpts;
	// ofsSpts.open("SPTS_" + to_string(cell_id) + "_" + to_string(t));
	ofsSpts.open("SPTS_" + to_string(cell_id));

	for (int spt : spts) {
		ofsSpts << spt << " ";
	}
}

void Model::loadSpts(string tt) {
	spts.clear();

	deque<int> iideque;
	// ifstream file("SPTS_" + to_string(cell_id) + "_" + tt);
	ifstream file("SPTS_" + to_string(cell_id));

	if (!file.is_open()) {
		throw "X file not found.";
	}
	string line;
	while (getline(file, line)) {
		iideque = SplitString_int(line.c_str());
		for (auto spt : iideque) {
			spts.insert(spt);
		}
	}
	file.close();
	cout << "SPTS loaded" << endl;
}

// class construction _definition_. Requires no type specifiction.
Model::Model(int _NE, int _NI, int _cell_id) : m_mt(m_randomdevice()) {

	cell_id = _cell_id;
	NE = _NE;
	NI = _NI;
	N = NE + NI; //

	// initialize the weight matrix Jo
	Jo = calc_J(JEEinit, JEI);

	// initialize the STF and STD vectors
	for (int i = 0; i < NE; i++) {
		F.push_back(U); // @@ push back usd (0.99)
		D.push_back(1.0);
		ys.push_back(1.0 / (1.0 + usd * 0.05 * trec / tmE));
	}

	// allocate heap memory for the array of pointers (once per model instantiation)
	ptr_Jo = new double[N * N]; // ?
	ptr_F = new double[NE];
	ptr_D = new double[NE];
	ptr_ys = new double[NE];

	// since no suitable conversion function from "std::vector<double, std::allocator<double>>"
	// to "double *" exists, we need to copy the addresses one by one
	for (int i = 0; i < NE; i++) {
		ptr_F[i] = F[i];
		ptr_D[i] = D[i];
		ptr_ys[i] = ys[i];
	}

	for (int i = 0; i < N; i++) {
		for (int j = 0; j < N; j++) {
			ptr_Jo[j + N * i] = Jo[i][j];
		}
	}

	//how many neurons get updated per time step
	SNE = (int)floor(NE * h / tmE + 0.001);
	SNI = (int)floor(NI * h / tmI + 0.001);

	NEa = (int)floor(NE * a + 0.01); // Exact number of excitatory neurons stimulated externally
	pmax = NE / NEa;

	srand((unsigned int)time(NULL));

	// ossSpike << "stp" << ".txt";

	// spts_fname = "spike_times_" + cell_id;
	ofsr.open("/flash/FukaiU/roman/GRID_withFD/spike_times_" + std::to_string(cell_id)); // <<<<<<<<<
	ofsr.precision(10);

	// // log STP dynamics:
	// ossSTP << "stp" << ".txt";
	// string fstrSTP = ossSTP.str();
	// ofsSTP.open( fstrSTP.c_str() );
	// ofsSTP.precision(5);

	// diagnistics:
	// ossb << "distro.txt";
	// string fstrb = ossb.str();
	// ofsb.open( fstrb.c_str() );
	// ofsb.precision(10);

	// ossa << "mean_t_.txt";
	//    string fstra = ossa.str();
	//    ofsa.open( fstra.c_str() );
	// ofsa.precision(10);

	for (int i = 0; i < NE; i++) {
		Jinidx.push_back(ivec); // shape = (3000, max3000)
		for (int i2 = 0; i2 < NE; i2++) {
			if (Jo[i][i2] > Jepsilon) {
				Jinidx[i].push_back(i2);
			}
		}
	}

	// create a vector size N and fill it with zeros
	// this vector says if a neuron is spiking or not
	for (int i = 0; i < N; i++) {
		x.push_back(0);
	}

	// we remember in spts the ids of neurons that are spiking at the current step
	// and set neurons with these ids to 1
	// at the beginning of the simulation, some neurons have to spike, so we
	// initialize some neurons to 1 to make them spike
	for (int i = 0; i < N; i++) {
		// elements corresponding to excitatory neurons are filled with
		// ones with probability xEinit (0.02)
		if (i < NE && dice() < xEinit) {
			spts.insert(i);
			x[i] = 1;
		}
		// elements corresponding to inhibitory neurons are filled with
		// ones with probability xIinit (0.01)
		if (i >= NE && dice() < xIinit) {
			spts.insert(i);
			x[i] = 1;
		}
	}

	for (int i = 0; i < NE; i++) {
		dspts.push_back(ideque);
	}

	// initialize stimulus
	for (int i = 0; i < NE; i++) {
		hStim.push_back(0);
	}
}

void Model::setParams(Model::ParamsStructType params) {
	alpha = params.alpha;				// Degree of log-STDP (50.0)
	usd = params.usd;						// Release probability of a synapse (0.05 - 0.5)
	JEI = params.JEI;						// 0.15 or 0.20
	T = params.T;								// simulation time, ms (1800*1000.0)
	h = params.h;								// time step, ms ??????
	cEE = params.cEE;						// 0.2
	cIE = params.cIE;						// 0.2
	cEI = params.cEI;						// 0.5
	cII = params.cII;						// 0.5
	JEE = params.JEE;						// 0.15
	JEEinit = params.JEEinit;		// 0.18
	JIE = params.JIE;						// 0.15
	JII = params.JII;						// 0.06
	JEEh = params.JEEh;					// Standard synaptic weight E-E 0.15
	sigJ = params.sigJ;					// 0.3
	Jtmax = params.Jtmax;				// J_maxˆtot (0.25)
	Jtmin = params.Jtmin;				// J_minˆtot // ??? NOT IN THE PAPER (0.01)
	hE = params.hE;							// Threshold of update of excitatory neurons 1.0
	hI = params.hI;							// Threshold of update of inhibotory neurons 1.0
	IEex = params.IEex;					// Amplitude of steady external input to excitatory neurons 2.0
	IIex = params.IIex;					// Amplitude of steady external input to inhibitory neurons 0.5
	mex = params.mex;						// mean of external input 0.3
	sigex = params.sigex;				// variance of external input 0.1
	tmE = params.tmE;						//t_Eud EXCITATORY 5.0
	tmI = params.tmI;						//t_Iud INHIBITORY 2.5
	trec = params.trec;					// recovery time constant (tau_sd, p.13 and p.12) 600.0
	Jepsilon = params.Jepsilon; // ???????? 0.001
	tpp = params.tpp;						// tau_p 20.0
	tpd = params.tpd;						// tau_d 40.0
	Cp = params.Cp;
	Cd = params.Cd;
	twnd = params.twnd;			// STDP window lenght, ms 500.0
	g = params.g;						// ???? 1.25
	itauh = params.itauh;		// decay time of homeostatic plasticity,s (100)
	hsd = params.hsd;				// 0.1 release probability
	hh = params.hh;					// SOME MYSTERIOUS PARAMETER 10.0
	Ip = params.Ip;					// External current applied to randomly chosen excitatory neurons 1.0
	a = params.a;						// Fraction of neurons to which this external current is applied 0.20
	xEinit = params.xEinit; // prob that an exc neurons spikes at the beginning of the simulation 0.02
	xIinit = params.xIinit; // prob that an inh neurons spikes at the beginning of the simulation 0.01
	tinit = params.tinit;		// period of time after which STDP kicks in 100.0

	// recalculate values that depend on the parameters
	SNE = (int)floor(NE * h / tmE + 0.001);
	SNI = (int)floor(NI * h / tmI + 0.001);

	Jmax = 5.0 * JEE;	 // ???
	Jmin = 0.01 * JEE; // ????
	// Cp = 0.1*JEE;              // must be 0.01875 (in the paper)
	// Cd = Cp*tpp/tpd;           // must be 0.0075 (in the paper)
	hsig = 0.001 * JEE;							 // i.e. 0.00015 per time step (10 ms)
	NEa = (int)floor(NE * a + 0.01); // Exact number of excitatory neurons stimulated externally
	pmax = NE / NEa;

	tauh = itauh * 1000.0; // decay time of homeostatic plasticity, in ms

	U = params.U;
	taustf = params.taustf;
	taustd = params.taustd;
	HAGA = params.HAGA;
	asym = params.asym;
	stimIntensity = params.stimIntensity;

	reinitFD();
}

Model::retParamsStructType Model::getState() {
	Model::retParamsStructType ret_struct;
	ret_struct.alpha = alpha;
	ret_struct.usd = usd;
	ret_struct.JEI = JEI;
	ret_struct.T = T;
	ret_struct.h = h;
	ret_struct.NE = NE;
	ret_struct.NI = NI;
	ret_struct.cEE = cEE;
	ret_struct.cIE = cIE;
	ret_struct.cEI = cEI;
	ret_struct.cII = cII;
	ret_struct.JEE = JEE;
	ret_struct.JEEinit = JEEinit;
	ret_struct.JIE = JIE;
	ret_struct.JII = JII;
	ret_struct.JEEh = JEEh;
	ret_struct.sigJ = sigJ;
	ret_struct.Jtmax = Jtmax;
	ret_struct.Jtmin = Jtmin;
	ret_struct.hE = hE;
	ret_struct.hI = hI;
	ret_struct.IEex = IEex;
	ret_struct.IIex = IIex;
	ret_struct.mex = mex;
	ret_struct.sigex = sigex;
	ret_struct.tmE = tmE;
	ret_struct.tmI = tmI;
	ret_struct.trec = trec;
	ret_struct.Jepsilon = Jepsilon;
	ret_struct.tpp = tpp;
	ret_struct.tpd = tpd;
	ret_struct.twnd = twnd;
	ret_struct.g = g;
	ret_struct.itauh = itauh;
	ret_struct.hsd = hsd;
	ret_struct.hh = hh;
	ret_struct.Ip = Ip;
	ret_struct.a = a;
	ret_struct.xEinit = xEinit;
	ret_struct.xIinit = xIinit;
	ret_struct.tinit = tinit;

	ret_struct.Jmin = Jmin;
	ret_struct.Jmax = Jmax;
	ret_struct.Cp = Cp;
	ret_struct.Cd = Cd;
	ret_struct.SNE = SNE;
	ret_struct.SNI = SNI;
	ret_struct.NEa = NEa;
	ret_struct.t = t;

	ret_struct.U = U;
	ret_struct.taustf = taustf;
	ret_struct.taustd = taustd;
	ret_struct.HAGA = HAGA;
	ret_struct.asym = asym;
	ret_struct.stimIntensity = stimIntensity;

	return ret_struct;
}

extern "C" {

// we create a pointer to the object of type Net. The pointer type must be of the same type as the
// object/variable this pointer points to
Model *createModel(int _NE, int _NI, int _cell_id) {
	return new Model(_NE, _NI, _cell_id);
}

void dumpSpikeStates(Model *m) {
	m->saveDSPTS();
	m->saveX();
	m->saveSpts();
}

void loadSpikeStates(Model *m, char *fname) {
	// this func takes a c_char_p ctypes type, dereferences it into a string
	// fname comes in as a zero-terminated pointer, so c++ knows the start and stop
	cout << "loading spike states" << endl;
	m->loadDSPTS(fname);
	m->loadX(fname);
	m->loadSpts(fname);
}

void set_t(Model *m, double _t) {
	(m->t) = _t;
}

void saveSpikes(Model *m, int _saveflag) {
	(m->saveflag) = _saveflag;
}

// this func takes a pointer to the object of type Net and calls the bar() method of that object
// because this is a pointer, to access a member of the class, you use an arrow, not dot
void sim(Model *m, int steps) {
	m->sim(steps);
}

double *getWeights(Model *m) {
	const int x = m->N;
	vector<vector<double>> *arrayOfPointersVec = &(m->Jo);
	for (int i = 0; i < x; i++) {
		for (int j = 0; j < x; j++) {
			(m->ptr_Jo)[j + x * i] = (double)(*arrayOfPointersVec)[i][j];
		}
	}
	return m->ptr_Jo;
}

double *getF(Model *m) {
	const int x = m->NE;
	// because we can't return a pointer of type vector<double>*,
	// (C doesn't know this type), we have to create a new pointer
	// of type double*
	vector<double> *arrayOfPointersVec = &(m->F);
	for (int i = 0; i < x; i++) {
		(m->ptr_F)[i] = (double)(*arrayOfPointersVec)[i];
	}
	return m->ptr_F;
}

double *getD(Model *m) {
	// because we can't return a pointer of type vector<double>*,
	// (C doesn't know this type), we have to create a new pointer
	// of type double*
	const int x = m->NE;
	vector<double> *arrayOfPointersVec = &(m->D);
	for (int i = 0; i < x; i++) {
		(m->ptr_D)[i] = (double)(*arrayOfPointersVec)[i];
	}
	return m->ptr_D;
}

double *getys(Model *m) {
	// because we can't return a pointer of type vector<double>*,
	// (C doesn't know this type), we have to create a new pointer
	// of type double*
	const int x = m->NE;
	vector<double> *arrayOfPointersVec = &(m->ys);
	for (int i = 0; i < x; i++) {
		(m->ptr_ys)[i] = (double)(*arrayOfPointersVec)[i];
	}
	return m->ptr_ys;
}

void setParams(Model *m, Model::ParamsStructType params) {
	m->setParams(params);
}

void setWeights(Model *m, double *W) {
	// if a function takes a pointer the * symbol means that the address
	// gets automatically dereferenced
	const int x = m->N;
	for (int i = 0; i < x; i++) {
		for (int j = 0; j < x; j++) {
			// we take a pointer, dereference the Jo, write to [i, j]
			// dereferenced values of W[i,j] (remember, square brackets
			// dereference a pointer)
			// it seems that only pointer of type <vector<double>> can be addressed like 2d arrays
			(m->Jo)[i][j] = W[j + x * i];
		}
	}
	cout << "Weights set" << endl;
	m->reinit_Jinidx();
	cout << "Jinidx recalculated" << endl;
}

void setF(Model *m, double *F) {
	const int x = m->NE;
	for (int i = 0; i < x; i++) {
		// we take a pointer, dereference the F, write to [i, j]
		// dereferenced values of F[i] (remember, square brackets
		// dereference a pointer)
		// it seems that only pointer of type <vector<double>> can be addressed like 2d arrays
		(m->F)[i] = F[i];
	}
	cout << "F loaded" << endl;

}

void setD(Model *m, double *D) {
	const int x = m->NE;
	for (int i = 0; i < x; i++) {
		// we take a pointer, dereference the D, write to [i]
		// dereferenced values of D[i] (remember, square brackets
		// dereference a pointer)
		// it seems that only pointer of type <vector<double>> can be addressed like 2d arrays
		(m->D)[i] = D[i];
	}
	cout << "D loaded" << endl;

}

void setys(Model *m, double *ys) {
	const int x = m->NE;
	for (int i = 0; i < x; i++) {
		// we take a pointer, dereference the ys, write to [i]
		// dereferenced values of ys[i] (remember, square brackets
		// dereference a pointer)
		// it seems that only pointer of type <vector<double>> can be addressed like 2d arrays
		(m->ys)[i] = ys[i];
	}
	cout << "ys set" << endl;
}

void setStim(Model *m, int *hStim) {
	const int x = m->NE;
	for (int i = 0; i < x; i++) {
		// we take a pointer, dereference the ys, write to [i]
		// dereferenced values of ys[i] (remember, square brackets
		// dereference a pointer)
		// it seems that only pointer of type <vector<double>> can be addressed like 2d arrays
		(m->hStim)[i] = hStim[i];
	}
	cout << "stim set" << endl;
}

Model::retParamsStructType getState(Model *m) {
	return m->getState();
}
}


#include <iostream>
#include <vector>
#include <set>
#include <algorithm>
#include <iterator>
#include <fstream>
#include <sstream>
#include <list>
#include <map>
#include <ext/algorithm>
#include <numeric>
#include <sys/timeb.h>
#include <time.h>
#include <cstdlib>
#include <cstdio>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/utility.hpp>

#include "rbt_coarse.h"
#include "rbt_dense.h"
#include "rbt_pose.h"

#include "CGAL/Cartesian.h"
#include "CGAL/Segment_tree_k.h"
#include "CGAL/Range_segment_tree_traits.h"

#include "CGAL/basic.h"
#include "CGAL/Point_3.h"
#include "CGAL/Range_tree_k.h"

typedef CGAL::Cartesian<double> CTP_Representation;
typedef CGAL::Range_tree_map_traits_3<CTP_Representation, unsigned short> CTP_Traits;
typedef CGAL::Range_tree_3<CTP_Traits> CTP_Range_tree_3_type;
typedef CTP_Traits::Key CTP_Key;
typedef CTP_Traits::Pure_key CTP_Pure_key;
typedef CTP_Traits::Interval CTP_Interval;

using namespace std;

//typedef double RotationMatrix[3][3];
//typedef vector<vector<double> > RotationMatrix(3, vector<double>(3, 0));

template <class FirstType, class SecondType>
struct less_than_univ{
    bool operator()(pair<FirstType, SecondType> pm_A, pair<FirstType, SecondType> pm_B)
    {
        if (pm_A.first < pm_B.first) return true;
        else return false;
    }
};

template <class T>
T rdl_dot_3d(const vector<T>& pm_v1, const vector<T>& pm_v2)
{
    T     r;
    r  = pm_v1[0] * pm_v2[0];
    r += pm_v1[1] * pm_v2[1];
    r += pm_v1[2] * pm_v2[2];

    return r;
}

template <class T>
void rdl_rotate(vector<T> const &pm_axis, \
                vector<T> const &pm_old_point, \
                T  	            pm_angle, \
                vector<T>       &pm_new_point)
{
    pm_new_point[0] = 0.0;
    pm_new_point[1] = 0.0;
    pm_new_point[2] = 0.0;
    double costheta, sintheta;

    costheta = cos(pm_angle);
    sintheta = sin(pm_angle);

    pm_new_point[0] += (costheta + (1 - costheta) * pm_axis[0] * pm_axis[0]) * pm_old_point[0];
    pm_new_point[0] += ((1 - costheta) * pm_axis[0] * pm_axis[1] - pm_axis[2] * sintheta) * pm_old_point[1];
    pm_new_point[0] += ((1 - costheta) * pm_axis[0] * pm_axis[2] + pm_axis[1] * sintheta) * pm_old_point[2];

    pm_new_point[1] += ((1 - costheta) * pm_axis[0] * pm_axis[1] + pm_axis[2] * sintheta) * pm_old_point[0];
    pm_new_point[1] += (costheta + (1 - costheta) * pm_axis[1] * pm_axis[1]) * pm_old_point[1];
    pm_new_point[1] += ((1 - costheta) * pm_axis[1] * pm_axis[2] - pm_axis[0] * sintheta) * pm_old_point[2];

    pm_new_point[2] += ((1 - costheta) * pm_axis[0] * pm_axis[2] - pm_axis[1] * sintheta) * pm_old_point[0];
    pm_new_point[2] += ((1 - costheta) * pm_axis[1] * pm_axis[2] + pm_axis[0] * sintheta) * pm_old_point[1];
    pm_new_point[2] += (costheta + (1 - costheta) * pm_axis[2] * pm_axis[2]) * pm_old_point[2];

    return;
}

void normalize(vector<double>& v)
{
	double n;
	n = v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
	if (n < 0.5){
		cerr << "n is very small! = " << n << endl;
	}
	n = sqrt(n);
	v[0] /= n;
	v[1] /= n;
	v[2] /= n;
}

struct MyBall{
	CTP_Range_tree_3_type*      m_index;

	MyBall()
	{
		m_index = NULL;
	}

	void query_local(const vector<double>&	pm_point, double pm_r, vector<unsigned short>& pm_out)
	{
		pm_out.clear();

		vector<CTP_Key> OutputList;

		CTP_Pure_key a = CTP_Pure_key(pm_point[0] - pm_r, pm_point[1] - pm_r, pm_point[2] - pm_r);
		CTP_Pure_key b = CTP_Pure_key(pm_point[0] + pm_r, pm_point[1] + pm_r, pm_point[2] + pm_r);
		CTP_Interval win = CTP_Interval(CTP_Key(a, 0), CTP_Key(b, 0));
		m_index->window_query(win, back_inserter(OutputList));

		if (OutputList.size() == 0){
			cerr << "ERROR: OutputList.size() == 0 in query_nnb(...)! " << endl;
			cerr << "\twhen query (" << pm_point[0] << ", " << pm_point[1] << ", " << pm_point[2] << ")" << endl;
			exit(1);
		}

		vector<pair<double, unsigned short> >	tmp_buffer;
		double tmp_dist;
		double tmp_rsq = pm_r * pm_r;
		size_t i;
		for (i = 0; i < OutputList.size(); i++){
			tmp_dist  = (pm_point[0] - OutputList[i].first.x()) * (pm_point[0] - OutputList[i].first.x());
			tmp_dist += (pm_point[1] - OutputList[i].first.y()) * (pm_point[1] - OutputList[i].first.y());
			tmp_dist += (pm_point[2] - OutputList[i].first.z()) * (pm_point[2] - OutputList[i].first.z());
			if (tmp_dist <= tmp_rsq){
				pm_out.push_back(OutputList[i].second);
			}
		}
	}

	unsigned short query_nn(const vector<double>&	pm_point)
	{
		vector<CTP_Key> OutputList;

		double	tmp_local_dist = 0.09;
		CTP_Pure_key a = CTP_Pure_key(pm_point[0] - tmp_local_dist, pm_point[1] - tmp_local_dist, pm_point[2] - tmp_local_dist);
		CTP_Pure_key b = CTP_Pure_key(pm_point[0] + tmp_local_dist, pm_point[1] + tmp_local_dist, pm_point[2] + tmp_local_dist);
		CTP_Interval win = CTP_Interval(CTP_Key(a, 0), CTP_Key(b, 0));
		m_index->window_query(win, back_inserter(OutputList));

		if (OutputList.size() == 0){
			cerr << "ERROR: OutputList.size() == 0 in query_nnb(...)! " << endl;
			cerr << "\twhen query (" << pm_point[0] << ", " << pm_point[1] << ", " << pm_point[2] << ")" << endl;
			exit(1);
		}

		vector<pair<double, unsigned short> >	tmp_buffer;
		double tmp_dist;
		size_t i;
		for (i = 0; i < OutputList.size(); i++){
			tmp_dist  = (pm_point[0] - OutputList[i].first.x()) * (pm_point[0] - OutputList[i].first.x());
			tmp_dist += (pm_point[1] - OutputList[i].first.y()) * (pm_point[1] - OutputList[i].first.y());
			tmp_dist += (pm_point[2] - OutputList[i].first.z()) * (pm_point[2] - OutputList[i].first.z());
			tmp_buffer.push_back(pair<double, unsigned short>(tmp_dist, OutputList[i].second));
		}

		sort(tmp_buffer.begin(), tmp_buffer.end(), less_than_univ<double, unsigned short>());

		return tmp_buffer[0].second;
	}
};

struct MyBall_Coarse: public MyBall{

	void init()
	{
		vector<CTP_Key>             InputList;
		vector<double>              tmp_point(3, 0);
		size_t i;

		for (i = 0; i < points_default_coarse; i ++){
			InputList.push_back(CTP_Key(CTP_Pure_key(vertex_default_coarse[i][0], vertex_default_coarse[i][1], vertex_default_coarse[i][2]), i));
		}

		m_index = new CTP_Range_tree_3_type(InputList.begin(), InputList.end());
	}
};

struct MyBall_Dense: public MyBall{

	void init()
	{
		vector<CTP_Key>             InputList;
		vector<double>              tmp_point(3, 0);
		size_t i;

		for (i = 0; i < points_default_dense; i ++){
			InputList.push_back(CTP_Key(CTP_Pure_key(vertex_default_dense[i][0], vertex_default_dense[i][1], vertex_default_dense[i][2]), i));
		}

		m_index = new CTP_Range_tree_3_type(InputList.begin(), InputList.end());
	}
};

struct MyBall_Pose: public MyBall{
	vector<vector<vector<double> > >		m_arcs;

	void init()
	{
		vector<CTP_Key>             InputList;
		vector<double>              tmp_point(3, 0);
		size_t i;

		for (i = 0; i < points_default_pose; i ++){
			InputList.push_back(CTP_Key(CTP_Pure_key(vertex_default_pose[i][0], vertex_default_pose[i][1], vertex_default_pose[i][2]), i));
		}

		m_index = new CTP_Range_tree_3_type(InputList.begin(), InputList.end());
	}

	void init_pose()
	{
		size_t i, j;
		vector<double>	curr_point(3, 0);
		vector<double>	rand_point(3, 0);
		double			dist;
		double			theta;
		vector<double>	tmp_Z(3, 0);
		vector<double>	tmp_Y(3, 0);
		vector<double>	tmp_X(3, 0);

		srand(time(NULL));
		vector<vector<double> >	tmp_buff;
		vector<vector<double> >	tmp_coord;

		m_arcs.clear();
		for (i = 0; i < points_default_pose; i ++){
			curr_point[0] = vertex_default_pose[i][0];
			curr_point[1] = vertex_default_pose[i][1];
			curr_point[2] = vertex_default_pose[i][2];
			j = rand() % points_default_pose;
			rand_point[0] = vertex_default_pose[j][0];
			rand_point[1] = vertex_default_pose[j][1];
			rand_point[2] = vertex_default_pose[j][2];
			dist  = (curr_point[0] - rand_point[0]) * (curr_point[0] - rand_point[0]);
			dist += (curr_point[1] - rand_point[1]) * (curr_point[1] - rand_point[1]);
			dist += (curr_point[2] - rand_point[2]) * (curr_point[2] - rand_point[2]);
			while(dist < 1 || dist > 3){
				j = rand() % points_default_pose;
				rand_point[0] = vertex_default_pose[j][0];
				rand_point[1] = vertex_default_pose[j][1];
				rand_point[2] = vertex_default_pose[j][2];
				dist  = (curr_point[0] - rand_point[0]) * (curr_point[0] - rand_point[0]);
				dist += (curr_point[1] - rand_point[1]) * (curr_point[1] - rand_point[1]);
				dist += (curr_point[2] - rand_point[2]) * (curr_point[2] - rand_point[2]);
			}

			tmp_Y[0] = curr_point[1] * rand_point[2] - curr_point[2] * rand_point[1];
			tmp_Y[1] = curr_point[2] * rand_point[0] - curr_point[0] * rand_point[2];
			tmp_Y[2] = curr_point[0] * rand_point[1] - curr_point[1] * rand_point[0];
			normalize(tmp_Y);
			tmp_X[0] = tmp_Y[1] * curr_point[2] - tmp_Y[2] * curr_point[1];
			tmp_X[1] = tmp_Y[2] * curr_point[0] - tmp_Y[0] * curr_point[2];
			tmp_X[2] = tmp_Y[0] * curr_point[1] - tmp_Y[1] * curr_point[0];
			normalize(tmp_X);
			tmp_buff.clear();
			for (j = 0; j < 360; j ++){
				theta = 2.0*i*M_PI/360;
				rdl_rotate(curr_point, tmp_X, theta, rand_point);
				tmp_buff.push_back(rand_point);
			}
			random_shuffle(tmp_buff.begin(), tmp_buff.end());
			rand_point = tmp_buff[rand()%tmp_buff.size()];

			tmp_Y[0] = curr_point[1] * rand_point[2] - curr_point[2] * rand_point[1];
			tmp_Y[1] = curr_point[2] * rand_point[0] - curr_point[0] * rand_point[2];
			tmp_Y[2] = curr_point[0] * rand_point[1] - curr_point[1] * rand_point[0];
			normalize(tmp_Y);
			tmp_X[0] = tmp_Y[1] * curr_point[2] - tmp_Y[2] * curr_point[1];
			tmp_X[1] = tmp_Y[2] * curr_point[0] - tmp_Y[0] * curr_point[2];
			tmp_X[2] = tmp_Y[0] * curr_point[1] - tmp_Y[1] * curr_point[0];
			normalize(tmp_X);
			tmp_Z = curr_point;

			tmp_coord.clear();
			tmp_coord.push_back(tmp_X);
			tmp_coord.push_back(tmp_Y);
			tmp_coord.push_back(tmp_Z);
			m_arcs.push_back(tmp_coord);

            //cout << "X*Y = " << rdl_dot_3d(tmp_X, tmp_Y) << endl;
            //cout << "Y*Z = " << rdl_dot_3d(tmp_Y, tmp_Z) << endl;
            //cout << "Z*X = " << rdl_dot_3d(tmp_Z, tmp_X) << endl;

			tmp_coord.clear();
			tmp_X[0] = 0 - tmp_X[0];
			tmp_X[1] = 0 - tmp_X[1];
			tmp_X[2] = 0 - tmp_X[2];

			tmp_Y[0] = 0 - tmp_Y[0];
			tmp_Y[1] = 0 - tmp_Y[1];
			tmp_Y[2] = 0 - tmp_Y[2];

			tmp_coord.push_back(tmp_X);
			tmp_coord.push_back(tmp_Y);
			tmp_coord.push_back(tmp_Z);
			m_arcs.push_back(tmp_coord);

            //cout << "X*Y = " << rdl_dot_3d(tmp_X, tmp_Y) << endl;
            //cout << "Y*Z = " << rdl_dot_3d(tmp_Y, tmp_Z) << endl;
            //cout << "Z*X = " << rdl_dot_3d(tmp_Z, tmp_X) << endl << endl;
		}
	}
};

struct MyUnitBall{
	vector<vector<double> >		m_unitball_coarse;
	vector<vector<double> >		m_unitball_dense;

	MyBall_Coarse			m_coarse;
	MyBall_Dense				m_dense;
	MyBall_Pose				m_pose;
	vector<vector<unsigned short> >				m_table;		//[12560*2][1256]
	vector<vector<pair<unsigned short, unsigned short> > >	m_table_sort;		//[12560*2][1256]
	vector<vector<unsigned short> >				m_table_coarse2dense;
	vector<vector<unsigned short> >				m_table_dense2coarse;
	vector<vector<vector<double> > >			m_rmatrix;

	void ssave_matching_table(string pm_filename)
	{
		std::ofstream ofs(pm_filename.c_str());
		boost::archive::text_oarchive oa(ofs);
		oa & m_table;
	}

	void ssave_matching_table_sort(string pm_filename)
	{
		std::ofstream ofs(pm_filename.c_str());
		boost::archive::text_oarchive oa(ofs);
		oa & m_table_sort;
	}

	void ssave_matching_table_coarse2dense(string pm_filename)
	{
		std::ofstream ofs(pm_filename.c_str());
		boost::archive::text_oarchive oa(ofs);
		oa & m_table_coarse2dense;
	}

	void ssave_matching_table_dense2coarse(string pm_filename)
	{
		std::ofstream ofs(pm_filename.c_str());
		boost::archive::text_oarchive oa(ofs);
		oa & m_table_dense2coarse;
	}

	void ssave_matching_table_rmatrix(string pm_filename)
	{
		std::ofstream ofs(pm_filename.c_str());
		boost::archive::text_oarchive oa(ofs);
		oa & m_rmatrix;
	}

	void sload_matching_table(string pm_filename)
	{
		m_table.clear();
		std::ifstream ifs(pm_filename.c_str(), std::ios::binary);
		boost::archive::text_iarchive la(ifs);
		la & m_table;
	}

	void sload_matching_table_sort(string pm_filename)
	{
		m_table_sort.clear();
		std::ifstream ifs(pm_filename.c_str(), std::ios::binary);
		boost::archive::text_iarchive la(ifs);
		la & m_table_sort;
	}

	void sload_matching_table_coarse2dense(string pm_filename)
	{
		m_table_coarse2dense.clear();
		std::ifstream ifs(pm_filename.c_str(), std::ios::binary);
		boost::archive::text_iarchive la(ifs);
		la & m_table_coarse2dense;
	}

	void sload_matching_table_dense2coarse(string pm_filename)
	{
		m_table_dense2coarse.clear();
		std::ifstream ifs(pm_filename.c_str(), std::ios::binary);
		boost::archive::text_iarchive la(ifs);
		la & m_table_dense2coarse;
	}

	void sload_matching_table_rmatrix(string pm_filename)
	{
		m_rmatrix.clear();
		std::ifstream ifs(pm_filename.c_str(), std::ios::binary);
		boost::archive::text_iarchive la(ifs);
		la & m_rmatrix;
	}

	void check_table_sort()
	{
		size_t i, j;

		for (i = 0; i < m_table_sort.size(); i ++){
			for (j = 0; j < m_table_sort[i].size(); j ++){
				if (m_table[i][m_table_sort[i][j].first] != m_table_sort[i][j].second){
					cout << "ERROR: " << m_table[i][m_table_sort[i][j].first] << " " << m_table_sort[i][j].second << endl;
 				}
			}
		}
	}

	void write_bin_matching_table_sort(string pm_filename)
	{
        ofstream out_file (pm_filename.c_str(), ios::out|ios::binary);
		unsigned short* out_buffer = new unsigned short [m_table_sort.size() * m_table_sort[0].size() * 2];
		char* head_buffer = reinterpret_cast<char *>(out_buffer);
		size_t i, j;

		for (i = 0; i < m_table_sort.size(); i ++){
			for (j = 0; j < m_table_sort[i].size(); j ++){
				*(out_buffer++) = m_table_sort[i][j].first;
				*(out_buffer++) = m_table_sort[i][j].second;
			}
		}

		unsigned int len = m_table_sort.size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));
		len = m_table_sort[0].size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));

		out_file.write(head_buffer, m_table_sort.size() * m_table_sort[0].size() * 2 * sizeof(unsigned short));
		out_file.close();
	}

	void read_bin_matching_table_sort(string pm_filename)
	{
		m_table_dense2coarse.clear();
        ifstream in_file (pm_filename.c_str(), ios::in | ios::binary | ios::ate);
		ifstream::pos_type in_size = in_file.tellg();
		char* head_buffer = new char [in_size];
		in_file.seekg(0, ios::beg);
		in_file.read(head_buffer, in_size);
		in_file.close();

		unsigned int num_row = *(reinterpret_cast<unsigned int *>(head_buffer));
		unsigned int num_column = *(reinterpret_cast<unsigned int *>(head_buffer+sizeof(unsigned int)));
		m_table_sort.insert(m_table_sort.end(), num_row, vector<pair<unsigned short, unsigned short> >(num_column, pair<unsigned short, unsigned short>(0, 0)));

		size_t i, j;

		for (i = 0; i < num_row; i ++){
			for (j = 0; j < num_column; j ++){
				m_table_sort[i][j].first = (reinterpret_cast<unsigned short *>(head_buffer+2*sizeof(unsigned int)))[i*num_column*2 + j*2];
				m_table_sort[i][j].second = (reinterpret_cast<unsigned short *>(head_buffer+2*sizeof(unsigned int)))[i*num_column*2 + j*2 + 1];
			}
		}
		delete[] head_buffer;
	}

	void write_bin_matching_table_dense2coarse(string pm_filename)
	{
        ofstream out_file (pm_filename.c_str(), ios::out|ios::binary);
		unsigned short* out_buffer = new unsigned short [m_table_dense2coarse.size() * m_table_dense2coarse[0].size()];
		char* head_buffer = reinterpret_cast<char *>(out_buffer);
		size_t i, j;

		for (i = 0; i < m_table_dense2coarse.size(); i ++){
			for (j = 0; j < m_table_dense2coarse[i].size(); j ++){
				*(out_buffer) = m_table_dense2coarse[i][j];
				out_buffer ++;
			}
		}

		unsigned int len = m_table_dense2coarse.size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));
		len = m_table_dense2coarse[0].size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));

		out_file.write(head_buffer, m_table_dense2coarse.size() * m_table_dense2coarse[0].size() * sizeof(unsigned short));
		out_file.close();
	}

	void read_bin_matching_table_dense2coarse()
	{
		m_table_dense2coarse.clear();
		ifstream in_file ("matching_table_dense2coarse.bin", ios::in | ios::binary | ios::ate);
		ifstream::pos_type in_size = in_file.tellg();
		char* head_buffer = new char [in_size];
		in_file.seekg(0, ios::beg);
		in_file.read(head_buffer, in_size);
		in_file.close();

		unsigned int num_row = *(reinterpret_cast<unsigned int *>(head_buffer));
		unsigned int num_column = *(reinterpret_cast<unsigned int *>(head_buffer+sizeof(unsigned int)));
		m_table_dense2coarse.insert(m_table_dense2coarse.end(), num_row, vector<unsigned short>(num_column, 0));

		size_t i, j;

		for (i = 0; i < num_row; i ++){
			for (j = 0; j < num_column; j ++){
				m_table_dense2coarse[i][j] = (reinterpret_cast<unsigned short *>(head_buffer+2*sizeof(unsigned int)))[i*num_column + j];
			}
		}
		delete[] head_buffer;
	}

	void write_bin_matching_table_coarse2dense(string pm_filename)
	{
        ofstream out_file (pm_filename.c_str(), ios::out|ios::binary);
		unsigned short* out_buffer = new unsigned short [m_table_coarse2dense.size() * m_table_coarse2dense[0].size()];
		char* head_buffer = reinterpret_cast<char *>(out_buffer);
		size_t i, j;

		for (i = 0; i < m_table_coarse2dense.size(); i ++){
			for (j = 0; j < m_table_coarse2dense[i].size(); j ++){
				*(out_buffer) = m_table_coarse2dense[i][j];
				out_buffer ++;
			}
		}

		unsigned int len = m_table_coarse2dense.size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));
		len = m_table_coarse2dense[0].size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));

		out_file.write(head_buffer, m_table_coarse2dense.size() * m_table_coarse2dense[0].size() * sizeof(unsigned short));
		out_file.close();
	}

	void read_bin_matching_table_coarse2dense()
	{
		m_table_coarse2dense.clear();
		ifstream in_file ("matching_table_coarse2dense.bin", ios::in | ios::binary | ios::ate);
		ifstream::pos_type in_size = in_file.tellg();
		char* head_buffer = new char [in_size];
		in_file.seekg(0, ios::beg);
		in_file.read(head_buffer, in_size);
		in_file.close();

		unsigned int num_row = *(reinterpret_cast<unsigned int *>(head_buffer));
		unsigned int num_column = *(reinterpret_cast<unsigned int *>(head_buffer+sizeof(unsigned int)));
		m_table_coarse2dense.insert(m_table_coarse2dense.end(), num_row, vector<unsigned short>(num_column, 0));

		size_t i, j;

		for (i = 0; i < num_row; i ++){
			for (j = 0; j < num_column; j ++){
				m_table_coarse2dense[i][j] = (reinterpret_cast<unsigned short *>(head_buffer+2*sizeof(unsigned int)))[i*num_column + j];
			}
		}
		delete[] head_buffer;
	}

	void write_bin_matching_table(string pm_filename)
	{
        ofstream out_file (pm_filename.c_str(), ios::out|ios::binary);
		unsigned short* out_buffer = new unsigned short [m_table.size() * m_table[0].size()];
		char* head_buffer = reinterpret_cast<char *>(out_buffer);
		size_t i, j;

		for (i = 0; i < m_table.size(); i ++){
			for (j = 0; j < m_table[i].size(); j ++){
				*(out_buffer) = m_table[i][j];
				out_buffer ++;
			}
		}

		unsigned int len = m_table.size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));
		len = m_table[0].size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));

		out_file.write(head_buffer, m_table.size() * m_table[0].size() * sizeof(unsigned short));
		out_file.close();
	}

	void read_bin_matching_table()
	{
		m_table.clear();
		ifstream in_file ("matching_table.bin", ios::in | ios::binary | ios::ate);
		ifstream::pos_type in_size = in_file.tellg();
		char* head_buffer = new char [in_size];
		in_file.seekg(0, ios::beg);
		in_file.read(head_buffer, in_size);
		in_file.close();

		unsigned int num_row = *(reinterpret_cast<unsigned int *>(head_buffer));
		unsigned int num_column = *(reinterpret_cast<unsigned int *>(head_buffer+sizeof(unsigned int)));
		m_table.insert(m_table.end(), num_row, vector<unsigned short>(num_column, 0));

		size_t i, j;

		for (i = 0; i < num_row; i ++){
			for (j = 0; j < num_column; j ++){
				m_table[i][j] = (reinterpret_cast<unsigned short *>(head_buffer+2*sizeof(unsigned int)))[i*num_column + j];
			}
		}

		delete[] head_buffer;
	}

	void write_bin_rotation_matrix(string pm_filename)
	{
        ofstream out_file (pm_filename.c_str(), ios::out|ios::binary);
		double* out_buffer = new double [m_rmatrix.size() * 9];
		char* head_buffer = reinterpret_cast<char *>(out_buffer);
		size_t i, j, k;

		for (i = 0; i < m_rmatrix.size(); i ++){
			for (j = 0; j < m_rmatrix[i].size(); j ++){
				for (k = 0; k < m_rmatrix[i][j].size(); k ++){
					*(out_buffer) = m_rmatrix[i][j][k];
					out_buffer ++;
				}
			}
		}
		unsigned int len = m_rmatrix.size();
		out_file.write(reinterpret_cast<char *>(&len), sizeof(len));
		out_file.write(head_buffer, m_rmatrix.size() * 9 * sizeof(double));
		out_file.close();
	}

	void read_bin_rotation_matrix()
	{
		m_rmatrix.clear();
		ifstream in_file ("rotation_matrix.bin", ios::in | ios::binary | ios::ate);
		ifstream::pos_type in_size = in_file.tellg();
		char* head_buffer = new char [in_size];
		in_file.seekg(0, ios::beg);
		in_file.read(head_buffer, in_size);
		in_file.close();

		size_t num_matrix = *(reinterpret_cast<unsigned int *>(head_buffer));
		m_rmatrix.insert(m_rmatrix.end(), num_matrix, vector<vector<double> >(3, vector<double>(3, 0)));
		size_t i, j, k;

		for (i = 0; i < num_matrix; i ++){
			for (j = 0; j < 3; j ++){
				for (k = 0; k < 3; k ++){
					m_rmatrix[i][j][k] = (reinterpret_cast<double *>(head_buffer+sizeof(unsigned int)))[i*9 + j*3 + k];
				}
			}
		}
		delete[] head_buffer;
	}

	void print_rotation_matrix()
	{
		size_t i;
		//stringstream out_buffer;
		//ofstream out_file("rotation_matrix.txt");
		//out_buffer << m_rmatrix.size() << endl;
		FILE*	f = fopen ("rotation_matrix.txt","w");
		fprintf(f, "%d\n", m_rmatrix.size());
		for (i = 0; i < m_rmatrix.size(); i ++){
			fprintf(f, "%+1.16e %+1.16e %+1.16e ",m_rmatrix[i][0][0], m_rmatrix[i][0][1], m_rmatrix[i][0][2]);
			fprintf(f, "%+1.16e %+1.16e %+1.16e ",m_rmatrix[i][1][0], m_rmatrix[i][1][1], m_rmatrix[i][1][2]);
			fprintf(f, "%+1.16e %+1.16e %+1.16e\n",m_rmatrix[i][2][0], m_rmatrix[i][2][1], m_rmatrix[i][2][2]);

			//out_buffer << m_rmatrix[i][0][0] << " ";
			//out_buffer << m_rmatrix[i][0][1] << " ";
			//out_buffer << m_rmatrix[i][0][2] << " ";
			//out_buffer << m_rmatrix[i][1][0] << " ";
			//out_buffer << m_rmatrix[i][1][1] << " ";
			//out_buffer << m_rmatrix[i][1][2] << " ";
			//out_buffer << m_rmatrix[i][2][0] << " ";
			//out_buffer << m_rmatrix[i][2][1] << " ";
			//out_buffer << m_rmatrix[i][2][2] << endl;
		}
		//out_file << out_buffer.str();
		//out_file.close();
	}

	void load_rotation_matrix()
	{
		m_rmatrix.clear();

		ifstream in_file("rotation_matrix.txt");
		int num;
		in_file >> num;

		vector<vector<double> > tmp_matrix(3, vector<double>(3, 0));
		int i;

		for (i = 0; i < num; i ++){
			in_file >> tmp_matrix[0][0]
			        >> tmp_matrix[0][1]
					>> tmp_matrix[0][2]
					>> tmp_matrix[1][0]
			        >> tmp_matrix[1][1]
					>> tmp_matrix[1][2]
					>> tmp_matrix[2][0]
			        >> tmp_matrix[2][1]
					>> tmp_matrix[2][2];
			m_rmatrix.push_back(tmp_matrix);
		}

		in_file.close();
	}

	void load_matching_table()
	{
		m_table.clear();

		ifstream in_file("matching_table.txt");
		int num_pose, num_coarse;
		in_file >> num_pose >> num_coarse;

		vector<unsigned short>	tmp_row;
		unsigned short id_ray;
		int i, j;
		for (i = 0; i < num_pose; i ++){
			tmp_row.clear();
			for (j = 0; j < num_coarse; j ++){
				in_file >> id_ray;
				tmp_row.push_back(id_ray);
			}
			m_table.push_back(tmp_row);
		}
		in_file.close();
	}

	void load_matching_table_coarse2dense()
	{
		m_table_coarse2dense.clear();

		ifstream in_file("matching_table_coarse2dense.txt");
		int num_pose, num_coarse;
		in_file >> num_pose >> num_coarse;

		vector<unsigned short>	tmp_row;
		unsigned short id_ray;
		int i, j;
		for (i = 0; i < num_pose; i ++){
			tmp_row.clear();
			for (j = 0; j < num_coarse; j ++){
				in_file >> id_ray;
				tmp_row.push_back(id_ray);
			}
			m_table_coarse2dense.push_back(tmp_row);
		}
		in_file.close();
	}

	void load_matching_table_dense2coarse()
	{
		m_table_dense2coarse.clear();

		ifstream in_file("matching_table_dense2coarse.txt");
		int num_pose, num_coarse;
		in_file >> num_pose >> num_coarse;

		vector<unsigned short>	tmp_row;
		unsigned short id_ray;
		int i, j;
		for (i = 0; i < num_pose; i ++){
			tmp_row.clear();
			for (j = 0; j < num_coarse; j ++){
				in_file >> id_ray;
				tmp_row.push_back(id_ray);
			}
			m_table_dense2coarse.push_back(tmp_row);
		}
		in_file.close();
	}

	void init()
	{
		load_unitball();
		m_coarse.init();
		m_dense.init();
		m_pose.init();
//cerr << "m_pose.init_pose();" << endl;
		m_pose.init_pose();
	}

	void load_unitball()
	{
		vector<double>	tmp_point(3, 0);
		size_t i;

		m_unitball_coarse.clear();
		for (i = 0; i < points_default_coarse; i ++){
			tmp_point[0] = vertex_default_coarse[i][0];
			tmp_point[1] = vertex_default_coarse[i][1];
			tmp_point[2] = vertex_default_coarse[i][2];
			m_unitball_coarse.push_back(tmp_point);
		}

		m_unitball_dense.clear();
		for (i = 0; i < points_default_dense; i ++){
			tmp_point[0] = vertex_default_dense[i][0];
			tmp_point[1] = vertex_default_dense[i][1];
			tmp_point[2] = vertex_default_dense[i][2];
			m_unitball_dense.push_back(tmp_point);
		}

	}

	void count_matching_table()
	{
		vector<int>	tmp_counter(points_default_dense, 0);
		size_t i, j;

		for (i = 0; i < m_table.size(); i ++){
			for (j = 0; j < m_table[i].size(); j ++){
				tmp_counter[m_table[i][j]] += 1;
			}
		}

		sort(tmp_counter.begin(), tmp_counter.end());
		copy(tmp_counter.begin(), tmp_counter.end(), ostream_iterator<unsigned short>(cout, "\n"));
	}

	void print_matching_table_coarse2dense()
	{
		size_t i, j;
		stringstream out_buffer;
		out_buffer << m_table_coarse2dense.size() << " " << m_table_coarse2dense[0].size() << endl;
		for (i = 0; i < m_table_coarse2dense.size(); i ++){
			for (j = 0; j < m_table_coarse2dense[i].size(); j ++){
				out_buffer << m_table_coarse2dense[i][j] << " ";
			}
			out_buffer << endl;
		}

		ofstream out_file("matching_table_coarse2dense.txt");
		out_file << out_buffer.str();
		out_file.close();
	}

	void print_matching_table_dense2coarse()
	{
		size_t i, j;
		stringstream out_buffer;
		out_buffer << m_table_dense2coarse.size() << " " << m_table_dense2coarse[0].size() << endl;
		for (i = 0; i < m_table_dense2coarse.size(); i ++){
			for (j = 0; j < m_table_dense2coarse[i].size(); j ++){
				out_buffer << m_table_dense2coarse[i][j] << " ";
			}
			out_buffer << endl;
		}

		ofstream out_file("matching_table_dense2coarse.txt");
		out_file << out_buffer.str();
		out_file.close();
	}

	void print_matching_table()
	{
		size_t i, j;
		stringstream out_buffer;
		out_buffer << m_table.size() << " " << m_table[0].size() << endl;
		for (i = 0; i < m_table.size(); i ++){
			for (j = 0; j < m_table[i].size(); j ++){
				out_buffer << m_table[i][j] << " ";
			}
			out_buffer << endl;
		}

		ofstream out_file("matching_table.txt");
		out_file << out_buffer.str();
		out_file.close();
	}

	void compute_matching_table()
	{
		m_table.clear();
		m_rmatrix.clear();

		vector<unsigned short>	tmp_pose;
		vector<pair<double, pair<unsigned short, unsigned short> > >	tmp_pose_sort;
		vector<pair<unsigned short, unsigned short> >	tmp_pose_sort_row;
		vector<unsigned short>	tmp_coarse2dense;
		vector<unsigned short>	tmp_dense2coarse;
		unsigned short tmp_id_dense;
		size_t i, j;
		//double M[3][3];
		//RotationMatrix M;
		vector<vector<double> > M(3, vector<double>(3, 0));
		vector<double>	tmp_point_old(3, 0);
		vector<double>	tmp_point_new(3, 0);
		double			tmp_local_r_sq = 0.4;
		double			tmp_dist;

		for (i = 0; i < m_pose.m_arcs.size(); i ++){
if (i/100*100 == i) cerr << "i = " << i << endl;

			M[0][0] = m_pose.m_arcs[i][0][0];
			M[1][0] = m_pose.m_arcs[i][0][1];
			M[2][0] = m_pose.m_arcs[i][0][2];

			M[0][1] = m_pose.m_arcs[i][1][0];
			M[1][1] = m_pose.m_arcs[i][1][1];
			M[2][1] = m_pose.m_arcs[i][1][2];

			M[0][2] = m_pose.m_arcs[i][2][0];
			M[1][2] = m_pose.m_arcs[i][2][1];
			M[2][2] = m_pose.m_arcs[i][2][2];

			tmp_dist  = M[0][2] * M[0][2];
			tmp_dist += M[1][2] * M[1][2];
			tmp_dist += (M[2][2] - 1) * (M[2][2] - 1);
			if (tmp_dist > tmp_local_r_sq) continue;

			m_rmatrix.push_back(M);
			tmp_pose.clear();
			tmp_pose_sort.clear();

			for (j = 0; j < points_default_coarse; j++){
				tmp_point_old[0] = vertex_default_coarse[j][0];
				tmp_point_old[1] = vertex_default_coarse[j][1];
				tmp_point_old[2] = vertex_default_coarse[j][2];
				tmp_point_new[0] = M[0][0] * tmp_point_old[0] + M[0][1] * tmp_point_old[1] + M[0][2] * tmp_point_old[2];
				tmp_point_new[1] = M[1][0] * tmp_point_old[0] + M[1][1] * tmp_point_old[1] + M[1][2] * tmp_point_old[2];
				tmp_point_new[2] = M[2][0] * tmp_point_old[0] + M[2][1] * tmp_point_old[1] + M[2][2] * tmp_point_old[2];
				tmp_id_dense = m_dense.query_nn(tmp_point_new);
				tmp_pose.push_back(tmp_id_dense);
				tmp_pose_sort.push_back(pair<double, pair<unsigned short, unsigned short> >(2 * tmp_point_new[2] - tmp_point_old[2], pair<unsigned short, unsigned short>(j, tmp_id_dense)));
			}
			m_table.push_back(tmp_pose);

			sort(tmp_pose_sort.begin(), tmp_pose_sort.end(), less_than_univ<double, pair<unsigned short, unsigned short> >());
			tmp_pose_sort_row.clear();
			for (j = 0; j < tmp_pose_sort.size(); j ++){
				tmp_pose_sort_row.push_back(tmp_pose_sort[j].second);
			}
			m_table_sort.push_back(tmp_pose_sort_row);

			tmp_coarse2dense.clear();
			for (j = 0; j < points_default_pose; j++){
				tmp_point_old[0] = vertex_default_pose[j][0];
				tmp_point_old[1] = vertex_default_pose[j][1];
				tmp_point_old[2] = vertex_default_pose[j][2];
				tmp_point_new[0] = M[0][0] * tmp_point_old[0] + M[0][1] * tmp_point_old[1] + M[0][2] * tmp_point_old[2];
				tmp_point_new[1] = M[1][0] * tmp_point_old[0] + M[1][1] * tmp_point_old[1] + M[1][2] * tmp_point_old[2];
				tmp_point_new[2] = M[2][0] * tmp_point_old[0] + M[2][1] * tmp_point_old[1] + M[2][2] * tmp_point_old[2];
				tmp_coarse2dense.push_back(m_pose.query_nn(tmp_point_new));
			}
			m_table_coarse2dense.push_back(tmp_coarse2dense);
			tmp_dense2coarse = tmp_coarse2dense;
			for (j = 0; j < tmp_coarse2dense.size(); j ++){
				tmp_dense2coarse[tmp_coarse2dense[j]] = j;
			}
			m_table_dense2coarse.push_back(tmp_dense2coarse);
		}
	}

    //will generate full size table
	void compute_matching_table_v6()
	{
		m_table.clear();
		m_rmatrix.clear();

		vector<unsigned short>	tmp_pose;
		vector<unsigned short>	tmp_coarse2dense;
		vector<unsigned short>	tmp_dense2coarse;
		unsigned short tmp_id_dense;
		size_t i, j;
		vector<vector<double> > M(3, vector<double>(3, 0));
		vector<double>	tmp_point_old(3, 0);
		vector<double>	tmp_point_new(3, 0);

		for (i = 0; i < m_pose.m_arcs.size(); i ++){
if (i/100*100 == i) cerr << "i = " << i << endl;

			M[0][0] = m_pose.m_arcs[i][0][0];
			M[1][0] = m_pose.m_arcs[i][0][1];
			M[2][0] = m_pose.m_arcs[i][0][2];

			M[0][1] = m_pose.m_arcs[i][1][0];
			M[1][1] = m_pose.m_arcs[i][1][1];
			M[2][1] = m_pose.m_arcs[i][1][2];

			M[0][2] = m_pose.m_arcs[i][2][0];
			M[1][2] = m_pose.m_arcs[i][2][1];
			M[2][2] = m_pose.m_arcs[i][2][2];

			m_rmatrix.push_back(M);
			tmp_pose.clear();

			for (j = 0; j < points_default_coarse; j++){
				tmp_point_old[0] = vertex_default_coarse[j][0];
				tmp_point_old[1] = vertex_default_coarse[j][1];
				tmp_point_old[2] = vertex_default_coarse[j][2];
				tmp_point_new[0] = M[0][0] * tmp_point_old[0] + M[0][1] * tmp_point_old[1] + M[0][2] * tmp_point_old[2];
				tmp_point_new[1] = M[1][0] * tmp_point_old[0] + M[1][1] * tmp_point_old[1] + M[1][2] * tmp_point_old[2];
				tmp_point_new[2] = M[2][0] * tmp_point_old[0] + M[2][1] * tmp_point_old[1] + M[2][2] * tmp_point_old[2];
				tmp_id_dense = m_dense.query_nn(tmp_point_new);
				tmp_pose.push_back(tmp_id_dense);
			}
			m_table.push_back(tmp_pose);

			tmp_coarse2dense.clear();
			for (j = 0; j < points_default_dense; j++){
				tmp_point_old[0] = vertex_default_dense[j][0];
				tmp_point_old[1] = vertex_default_dense[j][1];
				tmp_point_old[2] = vertex_default_dense[j][2];
				tmp_point_new[0] = M[0][0] * tmp_point_old[0] + M[0][1] * tmp_point_old[1] + M[0][2] * tmp_point_old[2];
				tmp_point_new[1] = M[1][0] * tmp_point_old[0] + M[1][1] * tmp_point_old[1] + M[1][2] * tmp_point_old[2];
				tmp_point_new[2] = M[2][0] * tmp_point_old[0] + M[2][1] * tmp_point_old[1] + M[2][2] * tmp_point_old[2];
				tmp_coarse2dense.push_back(m_dense.query_nn(tmp_point_new));
			}
			m_table_coarse2dense.push_back(tmp_coarse2dense);
			tmp_dense2coarse = tmp_coarse2dense;
			for (j = 0; j < tmp_coarse2dense.size(); j ++){
				tmp_dense2coarse[tmp_coarse2dense[j]] = j;
			}
			m_table_dense2coarse.push_back(tmp_dense2coarse);
		}

        cout << "m_table.size() = " << m_table.size() << endl;
        cout << "m_rmatrix.size() = " << m_rmatrix.size() << endl;
        cout << "m_table_coarse2dense.size() = " << m_table_coarse2dense.size() << endl;
        cout << "m_table_dense2coarse.size() = " << m_table_dense2coarse.size() << endl;
	}

	void compute_matching_table_v6_2(double pm_angle_degree)
	{
		m_table.clear();
		m_rmatrix.clear();

		vector<pair<double, pair<unsigned short, unsigned short> > >	tmp_pose_sort(points_default_coarse, pair<double, pair<unsigned short, unsigned short> >(0, pair<unsigned short, unsigned short>(0, 0)));
		vector<pair<unsigned short, unsigned short> >	tmp_pose_sort_row(points_default_coarse, pair<unsigned short, unsigned short>(0, 0));

		vector<unsigned short>	tmp_pose(points_default_coarse, 0);
		vector<unsigned short>	tmp_coarse2dense(points_default_dense, 0);
		vector<unsigned short>	tmp_dense2coarse(points_default_dense, 0);
		unsigned short tmp_id_dense;
		size_t i, j;
		vector<vector<double> > M(3, vector<double>(3, 0));
		vector<double>	tmp_point_old(3, 0);
		vector<double>	tmp_point_new(3, 0);

		double			tmp_local_r_sq = sin(M_PI*pm_angle_degree/360)*sin(M_PI*pm_angle_degree/360)*4;; //0.268 = 30 degree, 0.362 = 35, 0.468 = 40, 0.586 = 45,
		double			tmp_dist;

		m_table_sort.clear();

        cout << "Please wait ..." << endl;
		for (i = 0; i < m_pose.m_arcs.size(); i ++){
//if (i/100*100 == i) cerr << "i = " << i << endl;

			M[0][0] = m_pose.m_arcs[i][0][0];
			M[1][0] = m_pose.m_arcs[i][0][1];
			M[2][0] = m_pose.m_arcs[i][0][2];

			M[0][1] = m_pose.m_arcs[i][1][0];
			M[1][1] = m_pose.m_arcs[i][1][1];
			M[2][1] = m_pose.m_arcs[i][1][2];

			M[0][2] = m_pose.m_arcs[i][2][0];
			M[1][2] = m_pose.m_arcs[i][2][1];
			M[2][2] = m_pose.m_arcs[i][2][2];

			tmp_dist  = M[0][2] * M[0][2];
			tmp_dist += M[1][2] * M[1][2];
			tmp_dist += (M[2][2] - 1) * (M[2][2] - 1);
			if (tmp_dist > tmp_local_r_sq) continue;

			m_rmatrix.push_back(M);

			for (j = 0; j < points_default_coarse; j++){
				tmp_point_old[0] = vertex_default_coarse[j][0];
				tmp_point_old[1] = vertex_default_coarse[j][1];
				tmp_point_old[2] = vertex_default_coarse[j][2];
				tmp_point_new[0] = M[0][0] * tmp_point_old[0] + M[0][1] * tmp_point_old[1] + M[0][2] * tmp_point_old[2];
				tmp_point_new[1] = M[1][0] * tmp_point_old[0] + M[1][1] * tmp_point_old[1] + M[1][2] * tmp_point_old[2];
				tmp_point_new[2] = M[2][0] * tmp_point_old[0] + M[2][1] * tmp_point_old[1] + M[2][2] * tmp_point_old[2];
				tmp_id_dense = m_dense.query_nn(tmp_point_new);
				tmp_pose[j] = tmp_id_dense;
				tmp_pose_sort[j] = pair<double, pair<unsigned short, unsigned short> >(2 * tmp_point_new[2] - tmp_point_old[2], pair<unsigned short, unsigned short>(j, tmp_id_dense));
			}
			m_table.push_back(tmp_pose);

			sort(tmp_pose_sort.begin(), tmp_pose_sort.end(), less_than_univ<double, pair<unsigned short, unsigned short> >());
			for (j = 0; j < tmp_pose_sort.size(); j ++){
				tmp_pose_sort_row[j] = tmp_pose_sort[j].second;
			}
			m_table_sort.push_back(tmp_pose_sort_row);

			for (j = 0; j < points_default_dense; j++){
				tmp_point_old[0] = vertex_default_dense[j][0];
				tmp_point_old[1] = vertex_default_dense[j][1];
				tmp_point_old[2] = vertex_default_dense[j][2];
				tmp_point_new[0] = M[0][0] * tmp_point_old[0] + M[0][1] * tmp_point_old[1] + M[0][2] * tmp_point_old[2];
				tmp_point_new[1] = M[1][0] * tmp_point_old[0] + M[1][1] * tmp_point_old[1] + M[1][2] * tmp_point_old[2];
				tmp_point_new[2] = M[2][0] * tmp_point_old[0] + M[2][1] * tmp_point_old[1] + M[2][2] * tmp_point_old[2];
				tmp_coarse2dense[j] = m_dense.query_nn(tmp_point_new);
			}
			m_table_coarse2dense.push_back(tmp_coarse2dense);

			for (j = 0; j < tmp_coarse2dense.size(); j ++){
				tmp_dense2coarse[tmp_coarse2dense[j]] = j;
			}
			m_table_dense2coarse.push_back(tmp_dense2coarse);
		}

        cout << "m_table.size() = " << m_table.size() << endl;
        cout << "m_rmatrix.size() = " << m_rmatrix.size() << endl;
        cout << "m_table_coarse2dense.size() = " << m_table_coarse2dense.size() << endl;
        cout << "m_table_dense2coarse.size() = " << m_table_dense2coarse.size() << endl;
	}

};

void task_map_sphere2circle(int argc, char *argv[])
{
	if (argc < 3){
		cerr << "USAGE: " << argv[0] << " <out_vtk_filename>" << endl;
		return;
	}

    MyUnitBall myBall;

    myBall.init();

	//m_unitball_dense
	size_t i;
	double x, y, z, r, s, u, v;
	vector<vector<double> >	tmp_points;
	vector<double>			tmp_point(2, 0);

	for (i = 0; i < myBall.m_unitball_dense.size(); i ++){
		x = myBall.m_unitball_dense[i][0];
		y = myBall.m_unitball_dense[i][1];
		z = myBall.m_unitball_dense[i][2];
		if (y < 0 || z < 0 || x < 0 || x == -1) continue;
		s = 0.5 * (1 - x);
		r = 2 * sqrt(1 - s);
		u = y / r;
		v = z / r;
		tmp_point[0] = u;
		tmp_point[1] = v;
		tmp_points.push_back(tmp_point);
	}

	//write to vtk file
	string out_fn = argv[2];
	ofstream out_file(out_fn.c_str());
	stringstream out_buffer;

	out_buffer << "# vtk DataFile Version 3.0" << endl;
	out_buffer << argv[2] << endl;
	out_buffer << "ASCII" << endl;
	out_buffer << "DATASET POLYDATA" << endl;
	out_buffer << "POINTS " << tmp_points.size() << " float" << endl;

	size_t j;
	for (j = 0; j < tmp_points.size(); j ++){
		out_buffer << tmp_points[j][0] << " "
				   << tmp_points[j][1] << " "
			       << 0 << endl;
	}

	out_buffer << "VERTICES " << tmp_points.size() << " " << tmp_points.size() * 2 << endl;
	for(j = 0; j < tmp_points.size(); j ++){
		out_buffer << "1 " << j << endl;
	}

	out_file << out_buffer.str();
	out_file.close();

}

void task_mtgen_v6(int argc, char *argv[])
{
    MyUnitBall myBall;

    myBall.init();
cerr << "myBall.compute_matching_table_v6(); " << endl;
	myBall.compute_matching_table_v6();

cerr << "serializing the tables..." << endl;
	myBall.ssave_matching_table("ssave_matching_table_v6.txt");
	myBall.ssave_matching_table_rmatrix("ssave_matching_table_v6_rmatrix.txt");
	//myBall.ssave_matching_table_sort("ssave_matching_table_v6_sort.txt");
	myBall.ssave_matching_table_dense2coarse("ssave_matching_table_v6_dense2coarse.txt");
	myBall.ssave_matching_table_coarse2dense("ssave_matching_table_v6_coarse2dense.txt");

//cerr << "myBall.write_bin_rotation_matrix();" << endl;
//	myBall.write_bin_rotation_matrix("matching_table_rotation_matrix_v6.bin");
//
//cerr << "myBall.write_bin_matching_table();" << endl;
//	myBall.write_bin_matching_table("matching_table_v6.bin");
//
//cerr << "myBall.write_bin_matching_table_coarse2dense();" << endl;
//	myBall.write_bin_matching_table_coarse2dense("matching_table_coarse2dense_v6.bin");
//
//cerr << "myBall.write_bin_matching_table_dense2coarse();" << endl;
//	myBall.write_bin_matching_table_dense2coarse("matching_table_dense2coarse_v6.bin");

}

void task_mtgen_v6_2(int argc, char *argv[])
{
    MyUnitBall myBall;

    myBall.init();
//cerr << "myBall.compute_matching_table_v6_2(); " << endl;
	myBall.compute_matching_table_v6_2(atof(argv[1]));

		string fn_table = string("ssave_matching_table_") + string(argv[1]) + string(".txt");
		string fn_table_rmatrix = string("ssave_matching_table_") + string(argv[1]) + string("_rmatrix.txt");
		string fn_table_sort = string("ssave_matching_table_") + string(argv[1]) + string("_sort.txt");
		string fn_table_dense2coarse = string("ssave_matching_table_") + string(argv[1]) + string("_dense2coarse.txt");
		string fn_table_coarse2dense = string("ssave_matching_table_") + string(argv[1]) + string("_coarse2dense.txt");

cerr << "serializing the tables..." << endl;
	myBall.ssave_matching_table(fn_table.c_str());
	myBall.ssave_matching_table_rmatrix(fn_table_rmatrix.c_str());
	myBall.ssave_matching_table_sort(fn_table_sort.c_str());
	myBall.ssave_matching_table_dense2coarse(fn_table_dense2coarse.c_str());
	myBall.ssave_matching_table_coarse2dense(fn_table_coarse2dense.c_str());
cerr << "Done!" << endl;

//cerr << "myBall.write_bin_rotation_matrix();" << endl;
//	myBall.write_bin_rotation_matrix("matching_table_rotation_matrix_v6.bin");
//
//cerr << "myBall.write_bin_matching_table();" << endl;
//	myBall.write_bin_matching_table("matching_table_v6.bin");
//
//cerr << "myBall.write_bin_matching_table_coarse2dense();" << endl;
//	myBall.write_bin_matching_table_coarse2dense("matching_table_coarse2dense_v6.bin");
//
//cerr << "myBall.write_bin_matching_table_dense2coarse();" << endl;
//	myBall.write_bin_matching_table_dense2coarse("matching_table_dense2coarse_v6.bin");

}

void task_mtgen(int argc, char *argv[])
{
	MyUnitBall myBall;

    myBall.init();
cerr << "myBall.compute_matching_table(); " << endl;
	myBall.compute_matching_table();

cerr << "serializing the tables..." << endl;
	myBall.ssave_matching_table("ssave_matching_table_full.txt");
	myBall.ssave_matching_table_rmatrix("ssave_matching_table_full_rmatrix.txt");
	myBall.ssave_matching_table_sort("ssave_matching_table_full_sort.txt");
	myBall.ssave_matching_table_dense2coarse("ssave_matching_table_full_dense2coarse.txt");
	myBall.ssave_matching_table_coarse2dense("ssave_matching_table_full_coarse2dense.txt");

//cerr << "myBall.write_bin_rotation_matrix();" << endl;
//	myBall.write_bin_rotation_matrix("rotation_matrix.bin");
//
//cerr << "myBall.write_bin_matching_table();" << endl;
//	myBall.write_bin_matching_table("matching_table.bin");
//
//cerr << "myBall.write_bin_matching_table_coarse2dense();" << endl;
//	myBall.write_bin_matching_table_coarse2dense("matching_table_coarse2dense.bin");
//
//cerr << "myBall.write_bin_matching_table_dense2coarse();" << endl;
//	myBall.write_bin_matching_table_dense2coarse("matching_table_dense2coarse.bin");
//
//cerr << "myBall.write_bin_matching_table_sort();" << endl;
//	myBall.write_bin_matching_table_sort("matching_table_sort.bin");

}

int main(int argc, char *argv[])
{
    //MyUnitBall myBall;
    //myBall.init();

		if (argc != 2){
				cerr << "Usage: " << argv[0] << " <angle>" << endl;
				return -1;
		}

		task_mtgen_v6_2(argc, argv);

/*
    if (argc < 2){
        task_mtgen(argc, argv);
    }
    else if (atoi(argv[1]) == 6){
        task_mtgen_v6(argc, argv);
    }
    else if (atoi(argv[1]) == 62){
        task_mtgen_v6_2(argc, argv);
    }
		else if (atoi(argv[1]) == 5){
				task_map_sphere2circle(argc, argv);
		}
*/

/*
	myBall.load_rotation_matrix();
	myBall.load_matching_table();
	myBall.load_matching_table_coarse2dense();
	myBall.load_matching_table_dense2coarse();

	myBall.write_bin_rotation_matrix();
	myBall.write_bin_matching_table();
	myBall.write_bin_matching_table_coarse2dense();
	myBall.write_bin_matching_table_dense2coarse();
*/

/*
	cout << "myBall.read_bin_matching_table();" << endl;
	myBall.read_bin_matching_table();

	//cout << "myBall.print_matching_table();" << endl;
	//myBall.print_matching_table();

	cout << "myBall.read_bin_matching_table();" << endl;
	myBall.read_bin_matching_table_sort();

	cout << "myBall.read_bin_matching_table_coarse2dense();" << endl;
	myBall.read_bin_matching_table_coarse2dense();

	//cout << "myBall.print_matching_table_coarse2dense();" << endl;
	//myBall.print_matching_table_coarse2dense();

	cout << "myBall.read_bin_matching_table_dense2coarse();" << endl;
	myBall.read_bin_matching_table_dense2coarse();

	//cout << "myBall.print_matching_table_dense2coarse();" << endl;
	//myBall.print_matching_table_dense2coarse();

	cout << "myBall.read_bin_rotation_matrix();" << endl;
	myBall.read_bin_rotation_matrix();

	//cout << "myBall.print_rotation_matrix();" << endl;
	//myBall.print_rotation_matrix();

	myBall.check_table_sort();
*/

    return 0;
}


#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
// #include <pcl/search/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>
#include <iostream>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Vector3.h>
#include <math.h>
#include <nav_msgs/Odometry.h>
#include <ros/console.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <Eigen/Eigen>
#include <random>

using namespace std;

// pcl::search::KdTree<pcl::PointXYZ> kdtreeLocalMap;
pcl::KdTreeFLANN<pcl::PointXYZ> kdtreeLocalMap;
vector<int> pointIdxRadiusSearch;
vector<float> pointRadiusSquaredDistance;

random_device rd;
//default_random_engine eng(0);
default_random_engine eng(rd());
uniform_real_distribution<double> rand_x;
uniform_real_distribution<double> rand_y;
uniform_real_distribution<double> rand_w;
uniform_real_distribution<double> rand_h;
uniform_real_distribution<double> rand_inf;

ros::Publisher _local_map_pub;
ros::Publisher _all_map_pub;
ros::Publisher click_map_pub_;
ros::Subscriber _odom_sub;

vector<double> _state;

int _obs_num;
double _x_size, _y_size, _z_size;
double _x_l, _x_h, _y_l, _y_h, _w_l, _w_h, _h_l, _h_h;
double _z_limit, _sensing_range, _resolution, _sense_rate, _init_x, _init_y;
double _min_dist;

bool _map_ok = false;
bool _has_odom = false;

int circle_num_;
double radius_l_, radius_h_, z_l_, z_h_;
double theta_;
uniform_real_distribution<double> rand_radius_;
uniform_real_distribution<double> rand_radius2_;
uniform_real_distribution<double> rand_theta_;
uniform_real_distribution<double> rand_z_;

sensor_msgs::PointCloud2 globalMap_pcd;
pcl::PointCloud<pcl::PointXYZ> cloudMap;

sensor_msgs::PointCloud2 localMap_pcd;
pcl::PointCloud<pcl::PointXYZ> clicked_cloud_;

void RandomMapGenerate() {
  pcl::PointXYZ pt_random;

  rand_x = uniform_real_distribution<double>(_x_l, _x_h);
  rand_y = uniform_real_distribution<double>(_y_l, _y_h);
  rand_w = uniform_real_distribution<double>(_w_l, _w_h);
  rand_h = uniform_real_distribution<double>(_h_l, _h_h);

  rand_radius_ = uniform_real_distribution<double>(radius_l_, radius_h_);
  rand_radius2_ = uniform_real_distribution<double>(radius_l_, 1.2);
  rand_theta_ = uniform_real_distribution<double>(-theta_, theta_);
  rand_z_ = uniform_real_distribution<double>(z_l_, z_h_);

  // generate polar obs
  for (int i = 0; i < _obs_num; i++) {
    double x, y, w, h;
    x = rand_x(eng);
    y = rand_y(eng);
    w = rand_w(eng);

    if (sqrt(pow(x - _init_x, 2) + pow(y - _init_y, 2)) < 2.0) {
      i--;
      continue;
    }

    if (sqrt(pow(x - 19.0, 2) + pow(y - 0.0, 2)) < 2.0) {
      i--;
      continue;
    }

    x = floor(x / _resolution) * _resolution + _resolution / 2.0;
    y = floor(y / _resolution) * _resolution + _resolution / 2.0;

    int widNum = ceil(w / _resolution);

    for (int r = -widNum / 2.0; r < widNum / 2.0; r++)
      for (int s = -widNum / 2.0; s < widNum / 2.0; s++) {
        h = rand_h(eng);
        int heiNum = ceil(h / _resolution);
        for (int t = -20; t < heiNum; t++) {
          pt_random.x = x + (r + 0.5) * _resolution + 1e-2;
          pt_random.y = y + (s + 0.5) * _resolution + 1e-2;
          pt_random.z = (t + 0.5) * _resolution + 1e-2;
          cloudMap.points.push_back(pt_random);
        }
      }
  }

  // generate circle obs
  for (int i = 0; i < circle_num_; ++i) {
    double x, y, z;
    x = rand_x(eng);
    y = rand_y(eng);
    z = rand_z_(eng);

    if (sqrt(pow(x - _init_x, 2) + pow(y - _init_y, 2)) < 2.0) {
      i--;
      continue;
    }

    if (sqrt(pow(x - 19.0, 2) + pow(y - 0.0, 2)) < 2.0) {
      i--;
      continue;
    }

    x = floor(x / _resolution) * _resolution + _resolution / 2.0;
    y = floor(y / _resolution) * _resolution + _resolution / 2.0;
    z = floor(z / _resolution) * _resolution + _resolution / 2.0;

    Eigen::Vector3d translate(x, y, z);

    double theta = rand_theta_(eng);
    Eigen::Matrix3d rotate;
    rotate << cos(theta), -sin(theta), 0.0, sin(theta), cos(theta), 0.0, 0, 0,
        1;

    double radius1 = rand_radius_(eng);
    double radius2 = rand_radius2_(eng);

    // draw a circle centered at (x,y,z)
    Eigen::Vector3d cpt;
    for (double angle = 0.0; angle < 6.282; angle += _resolution / 2) {
      cpt(0) = 0.0;
      cpt(1) = radius1 * cos(angle);
      cpt(2) = radius2 * sin(angle);

      // inflate
      Eigen::Vector3d cpt_if;
      for (int ifx = -0; ifx <= 0; ++ifx)
        for (int ify = -0; ify <= 0; ++ify)
          for (int ifz = -0; ifz <= 0; ++ifz) {
            cpt_if = cpt + Eigen::Vector3d(ifx * _resolution, ify * _resolution,
                                           ifz * _resolution);
            cpt_if = rotate * cpt_if + Eigen::Vector3d(x, y, z);
            pt_random.x = cpt_if(0);
            pt_random.y = cpt_if(1);
            pt_random.z = cpt_if(2);
            cloudMap.push_back(pt_random);
          }
    }
  }

  cloudMap.width = cloudMap.points.size();
  cloudMap.height = 1;
  cloudMap.is_dense = true;

  ROS_WARN("Finished generate random map ");

  kdtreeLocalMap.setInputCloud(cloudMap.makeShared());

  _map_ok = true;
}

void RandomMapGenerateCylinder() {
  pcl::PointXYZ pt_random;

  vector<Eigen::Vector2d> obs_position;

  rand_x = uniform_real_distribution<double>(_x_l, _x_h);
  rand_y = uniform_real_distribution<double>(_y_l, _y_h);
  rand_w = uniform_real_distribution<double>(_w_l, _w_h);
  rand_h = uniform_real_distribution<double>(_h_l, _h_h);
  rand_inf = uniform_real_distribution<double>(0.5, 1.5);

  rand_radius_ = uniform_real_distribution<double>(radius_l_, radius_h_);
  rand_radius2_ = uniform_real_distribution<double>(radius_l_, 1.2);
  rand_theta_ = uniform_real_distribution<double>(-theta_, theta_);
  rand_z_ = uniform_real_distribution<double>(z_l_, z_h_);

  // generate polar obs
  for (int i = 0; i < _obs_num && ros::ok(); i++) {
    double x, y, w, h, inf;
    x = rand_x(eng);
    y = rand_y(eng);
    w = rand_w(eng);
    inf = rand_inf(eng);

    if (sqrt(pow(x - _init_x, 2) + pow(y - _init_y, 2)) < 2.0) {
      i--;
      continue;
    }

    if (sqrt(pow(x - 19.0, 2) + pow(y - 0.0, 2)) < 2.0) {
      i--;
      continue;
    }

    bool flag_continue = false;
    for ( auto p : obs_position )
      if ( (Eigen::Vector2d(x,y) - p).norm() < _min_dist /*metres*/ )
      {
        i--;
        flag_continue = true;
        break;
      }
    if ( flag_continue ) continue;

    obs_position.push_back( Eigen::Vector2d(x,y) );

    x = floor(x / _resolution) * _resolution + _resolution / 2.0;
    y = floor(y / _resolution) * _resolution + _resolution / 2.0;

    int widNum = ceil((w*inf) / _resolution);
    double radius = (w*inf) / 2;

    for (int r = -widNum / 2.0; r < widNum / 2.0; r++)
      for (int s = -widNum / 2.0; s < widNum / 2.0; s++) {
        h = rand_h(eng);
        int heiNum = ceil(h / _resolution);
        for (int t = -30; t < heiNum; t++) {
          double temp_x = x + (r + 0.5) * _resolution + 1e-2;
          double temp_y = y + (s + 0.5) * _resolution + 1e-2;
          double temp_z = (t + 0.5) * _resolution + 1e-2;
          if ( (Eigen::Vector2d(temp_x,temp_y) - Eigen::Vector2d(x,y)).norm() <= radius )
          {
            pt_random.x = temp_x;
            pt_random.y = temp_y;
            pt_random.z = temp_z;
            cloudMap.points.push_back(pt_random);
          }
        }
      }
  }

  // generate circle obs
  for (int i = 0; i < circle_num_; ++i) {
    double x, y, z;
    x = rand_x(eng);
    y = rand_y(eng);
    z = rand_z_(eng);

    if (sqrt(pow(x - _init_x, 2) + pow(y - _init_y, 2)) < 2.0) {
      i--;
      continue;
    }

    if (sqrt(pow(x - 19.0, 2) + pow(y - 0.0, 2)) < 2.0) {
      i--;
      continue;
    }

    x = floor(x / _resolution) * _resolution + _resolution / 2.0;
    y = floor(y / _resolution) * _resolution + _resolution / 2.0;
    z = floor(z / _resolution) * _resolution + _resolution / 2.0;

    Eigen::Vector3d translate(x, y, z);

    double theta = rand_theta_(eng);
    Eigen::Matrix3d rotate;
    rotate << cos(theta), -sin(theta), 0.0, sin(theta), cos(theta), 0.0, 0, 0,
        1;

    double radius1 = rand_radius_(eng);
    double radius2 = rand_radius2_(eng);

    // draw a circle centered at (x,y,z)
    Eigen::Vector3d cpt;
    for (double angle = 0.0; angle < 6.282; angle += _resolution / 2) {
      cpt(0) = 0.0;
      cpt(1) = radius1 * cos(angle);
      cpt(2) = radius2 * sin(angle);

      // inflate
      Eigen::Vector3d cpt_if;
      for (int ifx = -0; ifx <= 0; ++ifx)
        for (int ify = -0; ify <= 0; ++ify)
          for (int ifz = -0; ifz <= 0; ++ifz) {
            cpt_if = cpt + Eigen::Vector3d(ifx * _resolution, ify * _resolution,
                                           ifz * _resolution);
            cpt_if = rotate * cpt_if + Eigen::Vector3d(x, y, z);
            pt_random.x = cpt_if(0);
            pt_random.y = cpt_if(1);
            pt_random.z = cpt_if(2);
            cloudMap.push_back(pt_random);
          }
    }
  }

  // generate floor
  // pcl::PointXYZ pt;
  // pt.z = 0.1;
  // for ( pt.x = _x_l; pt.x <= _x_h; pt.x += _resolution )
  //   for ( pt.y = _y_l; pt.y <= _y_h; pt.y += _resolution )
  //   {
  //     cloudMap.push_back(pt);
  //   }

  cloudMap.width = cloudMap.points.size();
  cloudMap.height = 1;
  cloudMap.is_dense = true;

  ROS_WARN("Finished generate random map ");

  kdtreeLocalMap.setInputCloud(cloudMap.makeShared());

  _map_ok = true;
}

void rcvOdometryCallbck(const nav_msgs::Odometry odom) {
  if (odom.child_frame_id == "X" || odom.child_frame_id == "O") return;
  _has_odom = true;

  _state = {odom.pose.pose.position.x,
            odom.pose.pose.position.y,
            odom.pose.pose.position.z,
            odom.twist.twist.linear.x,
            odom.twist.twist.linear.y,
            odom.twist.twist.linear.z,
            0.0,
            0.0,
            0.0};
}

int i = 0;
void pubSensedPoints() {
  // if (i < 10) {
  pcl::toROSMsg(cloudMap, globalMap_pcd);
  globalMap_pcd.header.frame_id = "world";
  _all_map_pub.publish(globalMap_pcd);
  // }

  return;

  /* ---------- only publish points around current position ---------- */
  if (!_map_ok || !_has_odom) return;

  pcl::PointCloud<pcl::PointXYZ> localMap;

  pcl::PointXYZ searchPoint(_state[0], _state[1], _state[2]);
  pointIdxRadiusSearch.clear();
  pointRadiusSquaredDistance.clear();

  pcl::PointXYZ pt;

  if (isnan(searchPoint.x) || isnan(searchPoint.y) || isnan(searchPoint.z))
    return;

  if (kdtreeLocalMap.radiusSearch(searchPoint, _sensing_range,
                                  pointIdxRadiusSearch,
                                  pointRadiusSquaredDistance) > 0) {
    for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i) {
      pt = cloudMap.points[pointIdxRadiusSearch[i]];
      localMap.points.push_back(pt);
    }
  } else {
    ROS_ERROR("[Map server] No obstacles .");
    return;
  }

  localMap.width = localMap.points.size();
  localMap.height = 1;
  localMap.is_dense = true;

  pcl::toROSMsg(localMap, localMap_pcd);
  localMap_pcd.header.frame_id = "world";
  _local_map_pub.publish(localMap_pcd);
}

void clickCallback(const geometry_msgs::PoseStamped& msg) {
  double x = msg.pose.position.x;
  double y = msg.pose.position.y;
  double w = rand_w(eng);
  double h;
  pcl::PointXYZ pt_random;

  x = floor(x / _resolution) * _resolution + _resolution / 2.0;
  y = floor(y / _resolution) * _resolution + _resolution / 2.0;

  int widNum = ceil(w / _resolution);

  for (int r = -widNum / 2.0; r < widNum / 2.0; r++)
    for (int s = -widNum / 2.0; s < widNum / 2.0; s++) {
      h = rand_h(eng);
      int heiNum = ceil(h / _resolution);
      for (int t = -1; t < heiNum; t++) {
        pt_random.x = x + (r + 0.5) * _resolution + 1e-2;
        pt_random.y = y + (s + 0.5) * _resolution + 1e-2;
        pt_random.z = (t + 0.5) * _resolution + 1e-2;
        clicked_cloud_.points.push_back(pt_random);
        cloudMap.points.push_back(pt_random);
      }
    }
  clicked_cloud_.width = clicked_cloud_.points.size();
  clicked_cloud_.height = 1;
  clicked_cloud_.is_dense = true;

  pcl::toROSMsg(clicked_cloud_, localMap_pcd);
  localMap_pcd.header.frame_id = "world";
  click_map_pub_.publish(localMap_pcd);

  cloudMap.width = cloudMap.points.size();

  return;
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "random_map_sensing");
  ros::NodeHandle n("~");

  _local_map_pub = n.advertise<sensor_msgs::PointCloud2>("/map_generator/local_cloud", 1);
  _all_map_pub = n.advertise<sensor_msgs::PointCloud2>("/map_generator/global_cloud", 1);

  _odom_sub = n.subscribe("odometry", 50, rcvOdometryCallbck);

  click_map_pub_ =
      n.advertise<sensor_msgs::PointCloud2>("/pcl_render_node/local_map", 1);
  // ros::Subscriber click_sub = n.subscribe("/goal", 10, clickCallback);

  n.param("init_state_x", _init_x, 0.0);
  n.param("init_state_y", _init_y, 0.0);

  n.param("map/x_size", _x_size, 50.0);
  n.param("map/y_size", _y_size, 50.0);
  n.param("map/z_size", _z_size, 5.0);
  n.param("map/obs_num", _obs_num, 30);
  n.param("map/resolution", _resolution, 0.1);
  n.param("map/circle_num", circle_num_, 30);

  n.param("ObstacleShape/lower_rad", _w_l, 0.3);
  n.param("ObstacleShape/upper_rad", _w_h, 0.8);
  n.param("ObstacleShape/lower_hei", _h_l, 3.0);
  n.param("ObstacleShape/upper_hei", _h_h, 7.0);

  n.param("ObstacleShape/radius_l", radius_l_, 7.0);
  n.param("ObstacleShape/radius_h", radius_h_, 7.0);
  n.param("ObstacleShape/z_l", z_l_, 7.0);
  n.param("ObstacleShape/z_h", z_h_, 7.0);
  n.param("ObstacleShape/theta", theta_, 7.0);

  n.param("sensing/radius", _sensing_range, 10.0);
  n.param("sensing/radius", _sense_rate, 10.0);

  n.param("min_distance", _min_dist, 1.0);

  _x_l = -_x_size / 2.0;
  _x_h = +_x_size / 2.0;

  _y_l = -_y_size / 2.0;
  _y_h = +_y_size / 2.0;

  _obs_num = min(_obs_num, (int)_x_size * 10);
  _z_limit = _z_size;

  ros::Duration(0.5).sleep();

  // RandomMapGenerate();
  RandomMapGenerateCylinder();

  ros::Rate loop_rate(_sense_rate);

  while (ros::ok()) {
    pubSensedPoints();
    ros::spinOnce();
    loop_rate.sleep();
  }
}