#include <iostream>
#include <string>
#include <fstream>
#include <cmath>
#include <map>
#include <vector>
#include <ctime>
#include <windows.h>
#include <sstream>
#include <float.h>
#include <pcl/point_types.h>
//#include <pcl/io/pcd_io.h>

#include <Eigen/Dense>
#include <vector>
#include <math.h>

using namespace Eigen;
using namespace Eigen::internal;
using namespace Eigen::Architecture;
using namespace pcl;
using namespace std;

const int ERROR_RANGE = 5;
const int COUNT_RANGE = 10;
int TTTTT = 0;
int KKKKK = 0;

inline  int string2int(std::string s)
{
    int n = atoi(s.c_str());
	return n;
}

inline float string2float(std::string str)
{
	float result = atof(str.c_str());
	return result;
}

inline double string2double(std::string str)
{
	double result;
	stringstream sstr(str);
	sstr >> result;
	return result;
}

inline long string2long(std::string str)
{
   long result = atol(str.c_str());
   return result;
}

std::string double2String(double d)
{
	std::ostringstream os;
	 if (os << d)
	    return os.str();
	 else
		 return "double2string error!";
}

inline void cutstring(std::string s ,int a[])
{
	std::string flag = ":";
	std::string::size_type position=0;
	int i=0;
	while((position=s.find_first_of(flag,position)) != std::string::npos)
	{
		//	  std::cout<< "position  " << i << " : " << position << std::endl;
		a[i] = position;
		position++;
		i++;
	}
}

//Akima方法
class  akima
{
private:
	int n, k;
	double  *x, *y, t, z, s[4];
public:
	akima (int nn)
	{
		n = nn;
		x = new double[n];    //动态分配内存
		y = new double[n];
	}
	void input(const std::vector<double> &X, const std::vector<double> &Y);          //由文件读入n个数据点(x, y)
	double interp(double);  //计算插值点t所在子区间上的三次多项式
	//并计算t点的近似值z
	void output(); //输出三次多项式系数以及t点的近似值z到文件并显示
	~akima()
	{
		delete []x;
	    delete []y;
	}
};

void akima::input(const std::vector<double> &X, const std::vector<double> &Y)
{
    for (int i = 0; i < X.size(); i++)
	{
	 x[i] = X[i];
	 y[i] = Y[i];
	}
}

double akima::interp(double tt)    ////计算插值点t所在子区间上的三次多项式
{
	//并计算t点的近似值z
	int m,kk;
	double u[5],p,q;
	t = tt;
	z=0.0; s[0]=0.0; s[1]=0.0; s[2]=0.0; s[3]=0.0;
	if (n<1) { k = 0; return z; }
	if (n==1) { k = 0; s[0]=y[0]; z=y[0]; return z;}
	if (n==2)
	{
		k = 0;
		s[0]=y[0]; s[1]=(y[1]-y[0])/(x[1]-x[0]);
		z=(y[0]*(t-x[1])-y[1]*(t-x[0]))/(x[0]-x[1]);
		return z;
	}
	if (t<=x[1]) k=0;
	else if (t>=x[n-1]) k=n-2;
	else
	{
		k=1; m=n;
		while (((k-m)!=1)&&((k-m)!=-1))
		{
			kk=(k+m)/2;
			if (t<x[kk-1]) m=kk;
			else k=kk;
		}
		k=k-1;
	}
	u[2]=(y[k+1]-y[k])/(x[k+1]-x[k]);
	if (n==3)
	{
		if (k==0)
		{
			u[3]=(y[2]-y[1])/(x[2]-x[1]);
			u[4]=2.0*u[3]-u[2];
			u[1]=2.0*u[2]-u[3];
			u[0]=2.0*u[1]-u[2];
		}
		else
		{
			u[1]=(y[1]-y[0])/(x[1]-x[0]);
			u[0]=2.0*u[1]-u[2];
			u[3]=2.0*u[2]-u[1];
			u[4]=2.0*u[3]-u[2];
		}
	}
	else
	{
		if (k<=1)
		{
			u[3]=(y[k+2]-y[k+1])/(x[k+2]-x[k+1]);
			if (k==1)
			{
				u[1]=(y[1]-y[0])/(x[1]-x[0]);
				u[0]=2.0*u[1]-u[2];
				if (n==4) u[4]=2.0*u[3]-u[2];
				else u[4]=(y[4]-y[3])/(x[4]-x[3]);
			}
			else
			{
				u[1]=2.0*u[2]-u[3];
				u[0]=2.0*u[1]-u[2];
				u[4]=(y[3]-y[2])/(x[3]-x[2]);
			}
		}
		else if (k>=(n-3))
		{
			u[1]=(y[k]-y[k-1])/(x[k]-x[k-1]);
			if (k==(n-3))
			{
				u[3]=(y[n-1]-y[n-2])/(x[n-1]-x[n-2]);
				u[4]=2.0*u[3]-u[2];
				if (n==4) u[0]=2.0*u[1]-u[2];
				else u[0]=(y[k-1]-y[k-2])/(x[k-1]-x[k-2]);
			}
			else
			{
				u[3]=2.0*u[2]-u[1];
				u[4]=2.0*u[3]-u[2];
				u[0]=(y[k-1]-y[k-2])/(x[k-1]-x[k-2]);
			}
		}
		else
		{
			u[1]=(y[k]-y[k-1])/(x[k]-x[k-1]);
			u[0]=(y[k-1]-y[k-2])/(x[k-1]-x[k-2]);
			u[3]=(y[k+2]-y[k+1])/(x[k+2]-x[k+1]);
			u[4]=(y[k+3]-y[k+2])/(x[k+3]-x[k+2]);
		}
	}
	s[0]=fabs(u[3]-u[2]);
	s[1]=fabs(u[0]-u[1]);
	if ((s[0]+1.0==1.0)&&(s[1]+1.0==1.0))
		p=(u[1]+u[2])/2.0;
	else p=(s[0]*u[1]+s[1]*u[2])/(s[0]+s[1]);
	s[0]=fabs(u[3]-u[4]);
	s[1]=fabs(u[2]-u[1]);
	if ((s[0]+1.0==1.0)&&(s[1]+1.0==1.0))
		q=(u[2]+u[3])/2.0;
	else q=(s[0]*u[2]+s[1]*u[3])/(s[0]+s[1]);
	s[0]=y[k];
	s[1]=p;
	s[3]=x[k+1]-x[k];
	s[2]=(3.0*u[2]-2.0*p-q)/s[3];
	s[3]=(q+p-2.0*u[2])/(s[3]*s[3]);
	p=t-x[k];
	z=s[0]+s[1]*p+s[2]*p*p+s[3]*p*p*p;
	return z;
}

void akima::output ()//输出三次多项式系数以及t点的近似值z到文件并显示
{
	char str2[20];
	cout <<"\n输出文件名:  ";
	cin >>str2;
	ofstream fout (str2, ios::app);
	if (!fout)
	{ cout <<"\n不能打开这个文件 " <<str2 <<endl; exit(1); }
	fout <<endl;  cout <<endl;
	fout <<k <<":" <<endl;
	fout <<s[0] <<"   " <<s[1] <<"   " <<s[2] <<"   " <<s[3] <<endl;
	cout <<k <<":" <<endl;
	cout <<s[0] <<"   " <<s[1] <<"   " <<s[2] <<"   " <<s[3] <<endl;
	fout <<endl <<t <<"   " <<z <<endl;
	cout <<endl <<t <<"   " <<z <<endl;
	fout.close ();
}
/*
void main ()      //主函数
{
	akima  solution(11);
	solution.input ();          //由文件读入n个数据点(x, y)
	solution.interp (-0.85);         //执行Akima方法
	solution.output ();//输出三次多项式系数以及t点的近似值z到文件并显示
	solution.interp (0.15);         //执行Akima方法
	solution.output ();//输出三次多项式系数以及t点的近似值z到文件并显示
}
*/

double Assum(vector<double> &vec)
{
	std::size_t len = vec.size();
	double sum = 0;
	for (int i = 0; i < len; i++)
	{
	 sum += vec[i];
	}

	return (static_cast<double>(sum) / len);
}

void Seprate2GetTrans(const string recorder_str,
	                     Matrix4d &rotation,
					     Vector3d &Origanl_XYZ,
				    const string previous_time,
				    const string current_time)
{
	int a[10] = {0};
	cutstring(recorder_str,a);
	Vector4d q;
	Vector3d vel;
/*
    //////////////////////////////////////////////////////////
		 2𝑞02+2𝑞12−1  2𝑞1𝑞2−2𝑞0𝑞3   2𝑞1𝑞3+2𝑞0𝑞2
		 2𝑞1𝑞2+2𝑞0𝑞3  2𝑞02+2𝑞22−1   2𝑞2𝑞3−2𝑞0𝑞1
		 2𝑞1𝑞3−2𝑞0𝑞2  2𝑞2𝑞3+2𝑞0𝑞1   2𝑞02+2𝑞32−1
*/
	q(0) = string2double(recorder_str.substr(a[0]+1, a[1] - a[0] - 1).c_str());
	q(1) = string2double(recorder_str.substr(a[1]+1, a[2] - a[1] - 1).c_str());
	q(2) = string2double(recorder_str.substr(a[2]+1, a[3] - a[2] - 1).c_str());
	q(3) = string2double(recorder_str.substr(a[3]+1, a[4] - a[3] - 1).c_str());

    int delta_time = string2int(current_time.substr(4).c_str()) - string2int(previous_time.substr(4).c_str());

	vel(0) =  string2double(recorder_str.substr(a[4]+1, a[5] - a[4] - 1).c_str());
	vel(1) =  string2double(recorder_str.substr(a[5]+1, a[6] - a[5] - 1).c_str());
	vel(2) =  string2double(recorder_str.substr(a[6]+1).c_str());

	Origanl_XYZ += static_cast<double>(delta_time)*vel / 3 ;
	double  w = q(0), x = q(1) , y = q(2) , z = q(3);

	rotation(0,0) = 2 * ( pow(q(0),2) + pow(q(1),2) ) - 1;
	rotation(0,1) = 2 * q(1) * q(2) - 2 * q(0) * q(3);
	rotation(0,2) = 2 * q(1) * q(3) + 2 * q(0) * q(2);
	rotation(0,3) = Origanl_XYZ(0);

	rotation(1,0) = 2 * q(1) * q(2) + 2 * q(0) * q(3);
	rotation(1,1) = 2 * ( pow(q(0),2) + pow(q(2),2) ) - 1;
	rotation(1,2) = 2 * q(2) * q(3) - 2 * q(0) * q(1);
	rotation(1,3) = Origanl_XYZ(1);

	rotation(2,0) = 2 * q(1) * q(3) - 2 * q(0) * q(2);
	rotation(2,1) = 2 * q(2) * q(3) + 2 * q(0) * q(1);
	rotation(2,2) = 2 * ( pow(q(0),2) + pow(q(3),2) ) - 1;
	//rotation(2,3) = Origanl_XYZ(2);
	rotation(2,3) = 0;

  /*
    | 1−2(y2+z2)  2(xy+zw)      2(xz−yw)
	| 2(xy−zw)    1−2(x2+z2)   2(yz+xw)
	| 2(xz+yw)     2(yz−xw)    1−2(x2+y2)
   */
	/*
	rotation(0,0) = - 2 * ( pow(q(2),2) + pow(q(3),2) ) + 1;
	rotation(0,1) = 2 * q(1) * q(2) - 2 * q(0) * q(3);
	rotation(0,2) = 2 * q(1) * q(3) - 2 * q(0) * q(2);
	rotation(0,3) = Origanl_XYZ(0);

	rotation(1,0) = 2 * q(1) * q(2) - 2 * q(0) * q(3);
	rotation(1,1) = - 2 * ( pow(q(1),2) - pow(q(3),2) ) + 1;
	rotation(1,2) = 2 * q(2) * q(3) + 2 * q(0) * q(1);
	rotation(1,3) = Origanl_XYZ(1);

	rotation(2,0) = 2 * q(1) * q(3) + 2 * q(0) * q(2);
	rotation(2,1) = 2 * q(2) * q(3) + 2 * q(0) * q(1);
	rotation(2,2) = - 2 * ( pow(q(1),2) + pow(q(2),2) ) + 1;
	rotation(2,3) = Origanl_XYZ(2);
	//rotation(2,3) = 0;
	*/
	////////////////////////////////////
	/*
	r(1,1)=1-2*y*y-2*z*z;
	r(1,2)=2*x*y+2*w*z;
	r(1,3)=2*x*z-2*w*y;

	r(2,1)=2*x*y-2*w*z;
	r(2,2)=1-2*x*x-2*z*z;
	r(2,3)=2*z*y+2*w*x;

	r(3,1)=2*x*z+2*w*y;
	r(3,2)=2*y*z-2*w*x;
	r(3,3)=1-2*x*x-2*y*y;
	*/
	/*
	rotation(0,0) = 1 - ( 2*y*y + 2*z*z);
	rotation(0,1) = 2*x*y - 2*w*z;
	rotation(0,2) = 2*x*z - 2*w*y;
	rotation(0,3) = Origanl_XYZ(0);

	rotation(1,0) = 2*x*y - 2*w*z;
	rotation(1,1) = 1 + ( 2*x*x - 2*z*z );
	rotation(1,2) = 2*z*y + 2*w*x;
	rotation(1,3) = Origanl_XYZ(1);

	rotation(2,0) = 2*x*z + 2*w*y;
	rotation(2,1) = 2*y*z + 2*w*x;
	rotation(2,2) = 1 - ( 2*x*x + 2*y*y );
	rotation(2,3) = Origanl_XYZ(2);
	//rotation(2,3) = 0;

	rotation(3,0) = 0;
	rotation(3,1) = 0;
	rotation(3,2) = 0;
	rotation(3,3) = 1;
	*/
}

void Seprate2GetPCvec(const string str, Vector4d &point,const int signal[5],const Vector3d &coor)
{
	point(0) = string2double(str.substr(0, signal[0]).c_str());
	point(1) = string2double(str.substr(signal[0] + 1, signal[1] - signal[0] -1).c_str());
    point(2) = 0;
 // point(2) = coor(2);
	point(3) = 1;
}

void Combine2PCL(std::map<std::string, std::string> &mymap,const char *str)
{
    if (str == NULL)
	{
	    std::cout << __FILE__<<":"<<__LINE__ <<"  File ERROR!"<<std::endl;
	    return ;
	}

	std::ifstream input(str);
	int number = -1;
	if (input.is_open())
	{
		std::string str = "";
		std::string prefix_timestamp = "";

		pcl::PointCloud<pcl::PointXYZI> TotalCloud;
		pcl::PointCloud<pcl::PointXYZI> midcloud;

		Matrix4d rotation;//旋转变换的矩阵，为4*4的矩阵，多一维是为了添加进入平移变换。
		Vector4d f; f(3) = 1; //每一个点在局部坐标系中的位置
		Vector3d current_position ;//第k帧时刻的坐标系距离原点的位置
		current_position(0) = current_position(1) = current_position(2) = 0.0;
		long int i = 0;

		string current_timestamp;
		string previous_timestamp;

		while ( getline(input,str) )
		{
			if (str.find("x86isnice") != std::string::npos)
			{
				number++;
				TotalCloud += midcloud;
				std::size_t count  = string2int(str.substr(0,str.find_first_of(" ")));

				midcloud.clear();
				pcl::PointCloud<pcl::PointXYZI>().swap(midcloud);

				i = 0;
				midcloud.height = 1;
				midcloud.width = count;
				midcloud.is_dense = true;
				midcloud.resize(midcloud.width  *  midcloud.height);

				getline(input ,str);

				int signal[5] = {0};
				cutstring(str,signal);
				prefix_timestamp = str.substr(signal[1]+1,signal[2] - signal[1] - 1);
				string  recorder_str = mymap[prefix_timestamp];
				/*分割出参数，获得转换矩阵的更新*///Seprate2GetTrans(string recorder_str, Vector4d transform, Vector3d Origanl_XYZ,string previous_time,string current_time)
				                              //Seprate2GetPCvec(string str, Vector4d point,int signal[5])
				if ( 0 == number )
				{
				   current_timestamp = previous_timestamp = prefix_timestamp;
				}
				else
				{
				   previous_timestamp = current_timestamp;
				   current_timestamp = prefix_timestamp;
				}

				Seprate2GetTrans(recorder_str,rotation, current_position, previous_timestamp,current_timestamp);
				/*分割出参数，获得转换矩阵的更新*/
				//Seprate2GetPCvec( str, f, signal);//QQXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
				Seprate2GetPCvec( str, f, signal,current_position);//QQ

				f = rotation * f;
				midcloud.points[i].x = f(0);
				midcloud.points[i].y = f(1);
				midcloud.points[i].z = f(2);
				midcloud.points[i].intensity = string2double(str.substr(signal[2] + 1 , signal[3] - signal[2] - 1));
				i++;
			}
			else
			{
				int signal[5] = {0};
				cutstring(str,signal);
				//Seprate2GetPCvec( str, f, signal);QQXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
				Seprate2GetPCvec( str, f, signal,current_position);//QQ
				f = rotation * f;
				midcloud.points[i].x = f(0);
				midcloud.points[i].y = f(1);
				midcloud.points[i].z = f(2);
				midcloud.points[i].intensity = string2double(str.substr(signal[2] + 1 , signal[3] - signal[2] - 1));
				i++;
			}
		}

		TotalCloud += midcloud;

		ofstream outpcd("../mypcd.pcd");

		outpcd << "# .PCD v.7 - Point Cloud Data file format"<<std::endl;
		outpcd << "VERSION 0.7" << std::endl;
		outpcd << "FIELDS x y z intensity" << std::endl;
		outpcd << "SIZE 4 4 4 4" <<std::endl;
		outpcd << "TYPE F F F F" <<std::endl;
		outpcd << "COUNT 1 1 1 1" <<std::endl;
		outpcd << "WIDTH " << TotalCloud.size() << std::endl;
		outpcd << "HEIGHT 1" <<std::endl;
		outpcd << "VIEWPOINT 0 0 0 1 0 0 0" <<std::endl;
		outpcd << "POINTS " << TotalCloud.size() << std::endl;
		outpcd << "DATA ascii" << std::endl;

		for (std::size_t pp = 0; pp < static_cast<std::size_t>(TotalCloud.size()); pp++)
		{
		     outpcd << TotalCloud.points[pp].x<<"  "<< TotalCloud.points[pp].y << "  " << TotalCloud.points[pp].z<< "  "<< TotalCloud.points[pp].intensity << std::endl;
		}
		outpcd.close();
	//	pcl::io::savePCDFile("../pp.pcd",TotalCloud);

		input.close();
		std::cerr<<"Research completed!  "<<TotalCloud.size() << std::endl;
	}
	else
	{
		std::cerr << "One of the file is not Open ,please check!" << endl;
		exit(EXIT_FAILURE);
	}
}

#if 1

int main()//动态规划匹配
{
	 ifstream xsens;
	 ifstream laser;
	 std::map<string , string> recorder;

	 clock_t start , finish;

	 string laserfile = "E:/binding/1450857349-laser.txt";
	 string xsensfile = "E:/binding/1450857349-Xsnes.txt";
	// xsens.open("D:\\1448246245-Xsnes.txt");
	// laser.open("D:\\test.txt");
	 laser.open(laserfile);
	 xsens.open(xsensfile);

	 if (laser.is_open() && xsens.is_open())
	 {
		 start = clock();
	     string str;
		 vector<string> log;

		 vector<string> xsens_log; //record th log of xsens to interplot value @ "ERROR"

		 while ( getline(laser,str) )
		 {
		    if (str.find("x86isnice") != std::string::npos)
			{
				string xx;
				getline(laser,str);
				int laser_signal[5] = {0};
				cutstring(str,laser_signal);
				int laser_result = string2int(str.substr(laser_signal[1]+5,laser_signal[2] - laser_signal[1] - 5));//@@@@@@@@@@@@@@@@@@@@
			//	int laser_result = string2int(str.substr(laser_signal[1]+4,laser_signal[2] - laser_signal[1] - 5));
				int xsens_signal[10] = {0};
				int xsens_result = 0;
				int count = 0;

			    bool flag = 0;

				if ( !log.empty() )
				{
				  for (vector<string>::iterator it = log.begin(); it != log.end(); it++)
				  {
					   cutstring(*it ,xsens_signal);
					   xsens_result = string2int(it->substr( 4, 10));
					   if ( abs(laser_result - xsens_result) <= ERROR_RANGE)
					   {
					    // recorder.insert(pair<string,string>(str.substr(laser_signal[1]+1,laser_signal[2] - laser_signal[1] - 1), it->substr(0,13)));
						   recorder.insert(pair<string,string>(str.substr(laser_signal[1]+1,laser_signal[2] - laser_signal[1] - 1), *it));
						   flag = 1;
						  break;
					   }
				  }
	              if ( flag == 1)
				  {
				//	  log.clear();
				//	  vector<string>().swap(log);

					  vector<string>::iterator it = log.begin();
					  vector<string>::iterator xl = xsens_log.begin();
					  //while (it != log.end() && it->compare(xx.substr(0,13)) < 0)
					 // while (it != log.end() && it->compare(str.substr(laser_signal[1]+1,laser_signal[2] - laser_signal[1] - 1)) < 0)
					  while (it != log.end() && (it->substr(0,13)).compare(str.substr(laser_signal[1]+1,laser_signal[2] - laser_signal[1] - 1)) < 0)
					  {//(it->substr(0,13)).compare(str.substr(laser_signal[1]+1,laser_signal[2] - laser_signal[1] - 1);
						  it = log.erase(it);
						  xl = xsens_log.erase(xl);
					  }
					  //xsens_log to clear-------------------------------------------//
				  }
				}

				if ( flag == 0 )
				{
					bool tag = 0;
					do
					{
						count++;

						getline( xsens, xx);

						if ( xx == "")
						{
							tag = 1;
							break;
						}

						cutstring( xx, xsens_signal);
						//log.push_back(xx.substr(0,13));
						log.push_back(xx);
						xsens_log.push_back(xx);//xsens_log的记录，并查集合//////////////////////////////////////////////

						xsens_result = string2int(xx.substr( 4, 10));
					} while (abs(laser_result - xsens_result) > ERROR_RANGE  && !xsens.eof() && count < COUNT_RANGE);

					//	if (count < COUNT_RANGE )
					if (count < COUNT_RANGE && tag != 1)
					{
					   //while (it != log.end() && it->compare(xx.substr(0,13)) < 0)
					    vector<string>::iterator it = log.begin();
						vector<string>::iterator xl = xsens_log.begin();
						while (it != log.end() && it->compare(str.substr(laser_signal[1]+1,laser_signal[2] - laser_signal[1] - 1)) < 0)
						{
								it = log.erase(it);
						        xl = xsens_log.erase(xl);//////////////////////////////并查集合剔除
						}
					//   @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@//
					//	recorder.insert(pair<string,string>(str.substr(laser_signal[1]+1,laser_signal[2] - laser_signal[1] - 1), xx.substr(0,13)));
					//   recorder.insert(pair<string,string>(str, xx));
						recorder.insert(pair<string,string>(str.substr(laser_signal[1]+1,laser_signal[2] - laser_signal[1] - 1), xx));
					}
					else
					{
						TTTTT++;
                     #if 0
						recorder.insert(pair<string,string>(str.substr(laser_signal[1]+1,laser_signal[2] - laser_signal[1] - 1),"ERROR"));
                     #else
                        /////////////

					   	 for (int cheap = 0 ; cheap <= 3 && !xsens.eof(); cheap++)
						 {
						 getline( xsens, xx);

							 if ( xx == "")
							 {
								 break;
							 }

						 cutstring( xx, xsens_signal);
						 log.push_back(xx);

						 xsens_log.push_back(xx);//xsens_log的记录，并查集合//////////////////////////////////////////////
						 }

						////////////
						vector<vector<double>> YY(7);
						vector<double> XX;

						for (vector<string>::iterator i = xsens_log.begin(); i != xsens_log.end(); i++)
						{
						  int pp[10] = {0};
						  ::cutstring(*i , pp);

						  XX.push_back(static_cast<double>(::string2int(i->substr(4,10).c_str())));

						  YY[0].push_back(string2double(i->substr(pp[0]+1, pp[1] - pp[0] - 1).c_str()));
						  YY[1].push_back(string2double(i->substr(pp[1]+1, pp[2] - pp[1] - 1).c_str()));
						  YY[2].push_back(string2double(i->substr(pp[2]+1, pp[3] - pp[2] - 1).c_str()));
						  YY[3].push_back(string2double(i->substr(pp[3]+1, pp[4] - pp[3] - 1).c_str()));
						  YY[4].push_back(string2double(i->substr(pp[4]+1, pp[5] - pp[4] - 1).c_str()));
						  YY[5].push_back(string2double(i->substr(pp[5]+1, pp[6] - pp[5] - 1).c_str()));
						  YY[6].push_back(string2double(i->substr(pp[6]+1).c_str()));
						}//遍历获得数值

						double interplot[7] = {0.0};
						int num_of_element = XX.size();
						for (int i = 0; i < 7; i++)
						{
						  akima computing(num_of_element);

						  computing.input(XX,YY[i]);

						  double x = string2double(str.substr(laser_signal[1]+5,laser_signal[2] - laser_signal[1] - 5));
						  interplot[i] = computing.interp(x);
						  if (_isnan(interplot[i]))
						  {
						     interplot[i] = Assum(YY[i]);
						  }
						//  computing.~akima();
						}
					//	std::cout << interplot[0]<<" "<< interplot[1]<<" "<<interplot[2]<<" "<< interplot[3]<<" " << interplot[4]<<" " << interplot[5]<<" "<<interplot[6]<<endl;
						string result = str.substr(laser_signal[1]+1,laser_signal[2] - laser_signal[1] - 1) +":"+
							                                               double2String(interplot[0]) +":"+
																		   double2String(interplot[1]) +":"+
																		   double2String(interplot[2]) +":"+
																		   double2String(interplot[3]) +":"+
																		   double2String(interplot[4]) +":"+
																		   double2String(interplot[5]) +":"+
																		   double2String(interplot[6]) ;
					  //@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@//
					  recorder.insert(pair<string,string>(str.substr(laser_signal[1]+1,laser_signal[2] - laser_signal[1] - 1),result));
				      //recorder.insert(pair<string,string>(str,result + " == x86isnice"));
						////////@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
					 //   recorder.insert(pair<string,string>(str.substr(laser_signal[1]+1,laser_signal[2] - laser_signal[1] - 1),result + " == x86isnice"));

                     #endif
					}
				}

				str.clear();
			}
			else
			{
				str.clear();
				continue;
			}
		 }

	 }
	 else
	 {
		 std::cerr << "One of the file is not Open ,please check!" << endl;
		 exit(EXIT_FAILURE);
	 }

	 laser.close();
	 xsens.close();

	 for (std::map<string,string>::iterator it = recorder.begin(); it != recorder.end(); it++)
	 {
	    std::cout << it->first << " <--> " << it->second << endl;
	 }

	 finish = clock();
	 cout <<"recorder.size()::"<< recorder.size()<<"   "<<"NOT HIT:: " << TTTTT<<"  OUT_OF_RANGE:"<<KKKKK/7<<"  The Cost Of Time is:"<<finish - start<<" ms" <<endl;

     Combine2PCL(recorder, laserfile.c_str());

	 std::cout << "Complete All" << std::endl;
	 return 0;
}

#else
int main()
{
	ifstream laser;
	ofstream output("D:\\test.txt");
	laser.open("D:\\1448246245-laser.txt");

	if (laser.is_open())
	{
		string str;
		while ( getline(laser,str)  && output.good())
		{
			if (str.find("x86isnice") != std::string::npos)
			{
				output << str <<endl;

				getline(laser,str);

				output << str<< endl;
/*
				getline(laser,str);
				output << str << endl;

				getline(laser,str);
				output << str << endl;
*/
			}
			else
				continue;
		}
	}
	else
	{
		std::cerr << "laser is not open!" <<endl;
		exit(EXIT_FAILURE);
	}

	laser.close();
	output.close();
   return 0;
}
#endif


#include <Eigen/CholmodSupport>
#include <fstream>
#include <iomanip>
#include "../../include/Optimization/LineSearch.h"
#include "../../include/Optimization/AugmentedLagrangian.h"
#include "../../include/timer.h"
#include <iostream>

void OptSolver::augmentedLagrangianSolver(
	std::function<double(const Eigen::VectorXd&, Eigen::VectorXd*, Eigen::SparseMatrix<double>*, bool)> objFunc,
	std::function<double(const Eigen::VectorXd&, const Eigen::VectorXd&)> findMaxStep,
	std::function<double(const Eigen::VectorXd&, const Eigen::VectorXd&, Eigen::VectorXd*, Eigen::SparseMatrix<double>*, bool, Eigen::VectorXd*)> constraintsFunc,
	std::function<double(const Eigen::VectorXd&, const double&, Eigen::VectorXd*, Eigen::SparseMatrix<double>*, bool)> penaltyFunc,
	Eigen::VectorXd& x0, Eigen::VectorXd& lambda, double& mu,
	int numIter, double gradTol, double xTol, double cTol, bool displayInfo, std::function<void(const Eigen::VectorXd&, double&, double&)> getNormFunc, std::string* savingFolder, std::function<void(Eigen::VectorXd&)> postProcess)
{
	const int DIM = x0.rows();
	Eigen::VectorXd randomVec = x0;
	randomVec.setRandom();
	x0 += 1e-6 * randomVec;
	Eigen::VectorXd grad = Eigen::VectorXd::Zero(DIM);
	Eigen::SparseMatrix<double> hessian;

	Eigen::VectorXd neggrad, delta_x;
	double maxStepSize = 1.0;
	double reg = 1e-8;

	bool isProj = true;
	Timer<std::chrono::high_resolution_clock> totalTimer;
	double totalAssemblingTime = 0;
	double totalSolvingTime = 0;
	double totalLineSearchTime = 0;

	totalTimer.start();
	std::ofstream optInfo;

	Eigen::VectorXd constVec;
	double init = constraintsFunc(x0, lambda, NULL, NULL, false, &constVec);
	std::cout << "constraint violation: " << constVec.minCoeff() << ", " << constVec.maxCoeff() << std::endl;

	int i = 0;
	for (; i < numIter; i++)
	{
		if (displayInfo)
			std::cout << "\niter: " << i << std::endl;

		auto lagrangian = [&](const Eigen::VectorXd& x, Eigen::VectorXd* grad, Eigen::SparseMatrix<double>* hess, bool isProj) {
			Eigen::VectorXd deriv, derivc, derivp;
			Eigen::SparseMatrix<double> H, Hc, Hp;
			double E = objFunc(x, grad ? &deriv : NULL, hess ? &H : NULL, isProj);
			double Ec = constraintsFunc(x, lambda, grad ? &derivc : NULL, hess ? &Hc : NULL, isProj, NULL);
			double Ep = penaltyFunc(x, mu, grad ? &derivp : NULL, hess ? &Hp : NULL, isProj);

			if (grad)
			{
				(*grad) = deriv + derivc + derivp;
			}

			if (hess)
			{
				(*hess) = H + Hc + Hp;
			}

			return E + Ec + Ep;
		};

		Timer<std::chrono::high_resolution_clock> localTimer;
		localTimer.start();
		double f = lagrangian(x0, &grad, &hessian, isProj);
		localTimer.stop();
		double localAssTime = localTimer.elapsed<std::chrono::milliseconds>() * 1e-3;
		totalAssemblingTime += localAssTime;

		localTimer.start();
		Eigen::SparseMatrix<double> H = hessian;
		Eigen::SparseMatrix<double> I(DIM, DIM);
		I.setIdentity();
		std::cout << "num of nonzeros: " << H.nonZeros() << ", rows: " << H.rows() << ", cols: " << H.cols() << std::endl;
		Eigen::CholmodSupernodalLLT<Eigen::SparseMatrix<double>> solver(H);

		//		Eigen::SimplicialLLT<Eigen::SparseMatrix<double> > solver(hessian);

		while (solver.info() != Eigen::Success)
		{
			if (displayInfo)
			{
				if (isProj)
					std::cout << "some small perturb is needed to remove round-off error, current reg = " << reg << std::endl;
				else
					std::cout << "Matrix is not positive definite, current reg = " << reg << std::endl;
			}

			H = hessian + reg * I;
			solver.compute(H);
			reg = std::max(2 * reg, 1e-16);
		}

		neggrad = -grad;
		delta_x = solver.solve(neggrad);

		localTimer.stop();
		double localSolvingTime = localTimer.elapsed<std::chrono::milliseconds>() * 1e-3;
		totalSolvingTime += localSolvingTime;

		maxStepSize = findMaxStep(x0, delta_x);

		localTimer.start();
		double rate = LineSearch::backtrackingArmijo(x0, grad, delta_x, lagrangian, maxStepSize);
		localTimer.stop();
		double localLinesearchTime = localTimer.elapsed<std::chrono::milliseconds>() * 1e-3;
		totalLineSearchTime += localLinesearchTime;

		if (!isProj)
		{
			reg *= 0.5;
			reg = std::max(reg, 1e-16);
		}

		x0 = x0 + rate * delta_x;

		double fnew = lagrangian(x0, &grad, NULL, isProj);

		//Eigen::VectorXd constVec;
		double tmpE = constraintsFunc(x0, lambda, NULL, NULL, false, &constVec);
		bool iskeepsame = true;

		if (rate * delta_x.lpNorm<Eigen::Infinity>() < std::max(1.0, xTol))
		{
			if (std::max(std::abs(constVec.minCoeff()), std::abs(constVec.maxCoeff())) > 10 * cTol && mu < 1e4)
			{
				iskeepsame = false;
				mu *= 2;
			}
			if (iskeepsame)
			{
				for (int j = 0; j < constVec.rows(); j++)
				{
					lambda(j) += mu * constVec(j);
				}
			}

		}
		double fnewAfterUpdate = lagrangian(x0, NULL, NULL, isProj);	// after update lambda and mu
		double Ec = constraintsFunc(x0, lambda, NULL, NULL, isProj, NULL);
		double Ep = penaltyFunc(x0, mu, NULL, NULL, isProj);
		if (displayInfo)
		{
			std::cout << "line search rate : " << rate << ", actual hessian : " << !isProj << ", reg = " << reg << std::endl;
			std::cout << "f_old: " << f << ", after update x, f_new: " << fnew << ", after update lambda and mu, f_new: " << fnewAfterUpdate << ", grad norm: " << grad.norm() << ", delta x: " << rate * delta_x.norm() << ", delta_f: " << f - fnew << std::endl;
			std::cout << "constraint violation: " << constVec.minCoeff() << ", " << constVec.maxCoeff() << std::endl;
			std::cout << "lambda: " << lambda.minCoeff() << ", " << lambda.maxCoeff() << ", mu: " << mu << std::endl;
			std::cout << "E const: " << Ec << ", E penalty: " << Ep << std::endl;
			if (getNormFunc)
			{
				double gradz, gradw;
				getNormFunc(grad, gradz, gradw);

				double updatez, updatew;
				getNormFunc(rate * delta_x, updatez, updatew);
				std::cout << "z grad: " << gradz << ", w grad: " << gradw << ", z change: " << updatez << ", w change: " << updatew << std::endl;
			}
			std::cout << "timing info (in total seconds): " << std::endl;
			std::cout << "assembling took: " << totalAssemblingTime << ", LLT solver took: " << totalSolvingTime << ", line search took: " << totalLineSearchTime << std::endl;
		}

		if (savingFolder)
		{
			optInfo << "line search rate : " << rate << ", actual hessian : " << !isProj << ", reg = " << reg << std::endl;
			optInfo << "f_old: " << f << ", f_new: " << fnew << ", grad norm: " << grad.norm() << ", delta x: " << rate * delta_x.norm() << ", delta_f: " << f - fnew << std::endl;
			if (getNormFunc)
			{
				double gradz, gradw;
				getNormFunc(grad, gradz, gradw);

				double updatez, updatew;
				getNormFunc(rate * delta_x, updatez, updatew);
				optInfo << "z grad: " << gradz << ", w grad: " << gradw << ", z change: " << updatez << ", w change: " << updatew << std::endl;
			}
			optInfo << "timing info (in total seconds): " << std::endl;
			optInfo << "assembling took: " << totalAssemblingTime << ", LLT solver took: " << totalSolvingTime << ", line search took: " << totalLineSearchTime << std::endl;

			if (i % 100 == 0)
			{
				std::string fileName = (*savingFolder) + "intermediate.txt";
				std::ofstream ofs(fileName);
				if (ofs)
				{
					ofs << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << x0 << std::endl;
				}
			}
		}

		if (iskeepsame)
		{
			if (std::abs(f - fnew) / f < 1e-5 || rate * delta_x.norm() < 1e-5 || grad.norm() < 1e-4)
			{
				isProj = false;
			}
			if (reg > 1e3)
				isProj = true;
		}

		if (rate < 1e-8)
		{
			std::cout << "terminate with small line search rate (<1e-8): L2-norm = " << grad.norm() << std::endl;
			break;
		}

		if (grad.norm() < gradTol && std::max(std::abs(constVec.minCoeff()), std::abs(constVec.maxCoeff())) < cTol)
		{
			std::cout << "terminate with gradient L2-norm = " << grad.norm() << std::endl;
			break;
		}

		if (rate * delta_x.norm() < xTol)
		{
			std::cout << "terminate with small variable change, gradient L2-norm = " << grad.norm() << std::endl;
			break;
		}
	}
	if (i >= numIter)
		std::cout << "terminate with reaching the maximum iteration, with gradient L2-norm = " << grad.norm() << std::endl;

	totalTimer.stop();
	if (displayInfo)
	{
		std::cout << "total time costed (s): " << totalTimer.elapsed<std::chrono::milliseconds>() * 1e-3 << ", within that, assembling took: " << totalAssemblingTime << ", LLT solver took: " << totalSolvingTime << ", line search took: " << totalLineSearchTime << std::endl;
	}

	if (savingFolder)
	{
		optInfo << "total time costed (s): " << totalTimer.elapsed<std::chrono::milliseconds>() * 1e-3 << ", within that, assembling took: " << totalAssemblingTime << ", LLT solver took: " << totalSolvingTime << ", line search took: " << totalLineSearchTime << std::endl;

		std::string fileName = (*savingFolder) + "final_res.txt";
		std::ofstream ofs(fileName);
		if (ofs)
		{
			ofs << std::setprecision(std::numeric_limits<long double>::digits10 + 1) << x0 << std::endl;
		}

	}

}


/**
 * \file ButterworthFilter.hxx
 */

#include "ButterworthFilter.h"
#include <ATK/EQ/helpers.h>

#include <boost/math/constants/constants.hpp>
#include <boost/math/tools/polynomial.hpp>

namespace ButterworthUtilities
{
  template<typename DataType>
  void create_butterworth_analog_coefficients(int order, EQUtilities::ZPK<DataType>& zpk)
  {
    zpk.k = 1;
    zpk.z.clear(); // no zeros for this filter type
    zpk.p.clear();
    for(gsl::index i = -order+1; i < order; i += 2)
    {
      zpk.p.push_back(std::complex<DataType>(-std::cos(boost::math::constants::pi<DataType>() * i / (2 * order)), -std::sin(boost::math::constants::pi<DataType>() * i / (2 * order))));
    }
  }

  template<typename DataType, typename Container>
  void create_default_coeffs(size_t order, DataType Wn, Container& coefficients_in, Container& coefficients_out)
  {
    EQUtilities::ZPK<DataType> zpk;

    int fs = 2;
    create_butterworth_analog_coefficients(static_cast<int>(order), zpk);
    EQUtilities::populate_lp_coeffs(Wn, fs, order, zpk, coefficients_in, coefficients_out);
  }

  template<typename DataType, typename Container>
  void create_bp_coeffs(size_t order, DataType wc1, DataType wc2, Container& coefficients_in, Container& coefficients_out)
  {
    EQUtilities::ZPK<DataType> zpk;

    int fs = 2;
    create_butterworth_analog_coefficients(static_cast<int>(order/2), zpk);
    EQUtilities::populate_bp_coeffs(wc1, wc2, fs, order, zpk, coefficients_in, coefficients_out);
  }

  template<typename DataType, typename Container>
  void create_bs_coeffs(size_t order, DataType wc1, DataType wc2, Container& coefficients_in, Container& coefficients_out)
  {
    EQUtilities::ZPK<DataType> zpk;

    int fs = 2;
    create_butterworth_analog_coefficients(static_cast<int>(order/2), zpk);
    EQUtilities::populate_bs_coeffs(wc1, wc2, fs, order, zpk, coefficients_in, coefficients_out);
  }
}

namespace ATK
{
  template <typename DataType>
  ButterworthLowPassCoefficients<DataType>::ButterworthLowPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void ButterworthLowPassCoefficients<DataType_>::set_cut_frequency(CoeffDataType cut_frequency)
  {
    if(cut_frequency <= 0)
    {
      throw std::out_of_range("Frequency can't be negative");
    }
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  typename ButterworthLowPassCoefficients<DataType_>::CoeffDataType ButterworthLowPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType>
  void ButterworthLowPassCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  unsigned int ButterworthLowPassCoefficients<DataType>::get_order() const
  {
    return in_order;
  }

  template <typename DataType>
  void ButterworthLowPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    ButterworthUtilities::create_default_coeffs(in_order, 2 * cut_frequency / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  ButterworthHighPassCoefficients<DataType>::ButterworthHighPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void ButterworthHighPassCoefficients<DataType_>::set_cut_frequency(CoeffDataType cut_frequency)
  {
    if(cut_frequency <= 0)
    {
      throw std::out_of_range("Frequency can't be negative");
    }
    this->cut_frequency = cut_frequency;
    setup();
  }

  template <typename DataType_>
  typename ButterworthHighPassCoefficients<DataType_>::CoeffDataType ButterworthHighPassCoefficients<DataType_>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template <typename DataType>
  void ButterworthHighPassCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = order;
    setup();
  }

  template <typename DataType>
  unsigned int ButterworthHighPassCoefficients<DataType>::get_order() const
  {
    return in_order;
  }

  template <typename DataType>
  void ButterworthHighPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    ButterworthUtilities::create_default_coeffs(in_order, (input_sampling_rate - 2 * cut_frequency) / input_sampling_rate, coefficients_in, coefficients_out);
    for(gsl::index i = in_order - 1; i >= 0; i -= 2)
    {
      coefficients_in[i] = - coefficients_in[i];
      coefficients_out[i] = - coefficients_out[i];
    }
  }

  template <typename DataType>
  ButterworthBandPassCoefficients<DataType>::ButterworthBandPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void ButterworthBandPassCoefficients<DataType_>::set_cut_frequencies(std::pair<CoeffDataType, CoeffDataType> cut_frequencies)
  {
    if(cut_frequencies.first <= 0 || cut_frequencies.second <= 0)
    {
      throw std::out_of_range("Frequencies can't be negative");
    }
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void ButterworthBandPassCoefficients<DataType_>::set_cut_frequencies(CoeffDataType f0, CoeffDataType f1)
  {
    set_cut_frequencies(std::make_pair(f0, f1));
  }

  template <typename DataType_>
  std::pair<typename ButterworthBandPassCoefficients<DataType_>::CoeffDataType, typename ButterworthBandPassCoefficients<DataType_>::CoeffDataType> ButterworthBandPassCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType>
  void ButterworthBandPassCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  unsigned int ButterworthBandPassCoefficients<DataType>::get_order() const
  {
    return in_order / 2;
  }

  template <typename DataType>
  void ButterworthBandPassCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    ButterworthUtilities::create_bp_coeffs(in_order, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }

  template <typename DataType>
  ButterworthBandStopCoefficients<DataType>::ButterworthBandStopCoefficients(gsl::index nb_channels)
  :Parent(nb_channels, nb_channels)
  {
  }

  template <typename DataType_>
  void ButterworthBandStopCoefficients<DataType_>::set_cut_frequencies(std::pair<CoeffDataType, CoeffDataType> cut_frequencies)
  {
    if(cut_frequencies.first <= 0 || cut_frequencies.second <= 0)
    {
      throw std::out_of_range("Frequencies can't be negative");
    }
    this->cut_frequencies = cut_frequencies;
    setup();
  }

  template <typename DataType_>
  void ButterworthBandStopCoefficients<DataType_>::set_cut_frequencies(CoeffDataType f0, CoeffDataType f1)
  {
    set_cut_frequencies(std::make_pair(f0, f1));
  }

  template <typename DataType_>
  std::pair<typename ButterworthBandStopCoefficients<DataType_>::CoeffDataType, typename ButterworthBandStopCoefficients<DataType_>::CoeffDataType> ButterworthBandStopCoefficients<DataType_>::get_cut_frequencies() const
  {
    return cut_frequencies;
  }

  template <typename DataType>
  void ButterworthBandStopCoefficients<DataType>::set_order(unsigned int order)
  {
    if(order == 0)
    {
      throw std::out_of_range("Order can't be null");
    }
    in_order = out_order = 2 * order;
    setup();
  }

  template <typename DataType>
  unsigned int ButterworthBandStopCoefficients<DataType>::get_order() const
  {
    return in_order / 2;
  }

  template <typename DataType>
  void ButterworthBandStopCoefficients<DataType>::setup()
  {
    Parent::setup();
    coefficients_in.assign(in_order+1, 0);
    coefficients_out.assign(out_order, 0);

    ButterworthUtilities::create_bs_coeffs(in_order, 2 * cut_frequencies.first / input_sampling_rate, 2 * cut_frequencies.second / input_sampling_rate, coefficients_in, coefficients_out);
  }
}


#include <iostream>
#include <algorithm>
#include <cmath>
#include <vector>
#include <map>
#include <set>
#include <tuple>
#include <stdbool.h>
#include <bitset>
#include <string>
#include <boost/multiprecision/cpp_int.hpp>

namespace mp = boost::multiprecision;
using namespace std;

int main(void) {
    int n,k;
    cin >> n;
    cin >> k;
    k -= 1;

    vector<mp::cpp_int> trees(n);
    for(int i = 0 ; i < n ; ++i) {
        cin >> trees[i];
    }
    sort(trees.begin(), trees.end());
    // 距離の差分で作り直す
    // ソートしてアレばk本分離れた位置がhmax-hminそのものになりそう
    mp::cpp_int min = 9999999999999999;
    for(int i = 0 ; i < n - k ; ++i) {
        auto d = abs(trees[i] - trees[i + k]);
        if (d < min) {
            min = d;
        }
    }
    cout << min << endl;
    return 0;
}


#include <iostream>
#include <vector>
#include <string>
#include <map>
#include <algorithm>
#include <Eigen/Dense>
#include "../matplotlibcpp.h"

using namespace Eigen;
namespace plt = matplotlibcpp;

MatrixXd sigmoid(MatrixXd&);
// MatrixXd ReLU(MatrixXd&);
// MatrixXd tanh(MatrixXd&);

int main()
{
    using std::cout;
    using std::endl;
    using std::map;
    using std::vector;
    using std::string;

    int node_num = 100;
    MatrixXd input_data = MatrixXd::Random(1000, node_num);
    int hidden_layer_size = 5;
    map<int, MatrixXd> activations;

    MatrixXd x = input_data;
    MatrixXd w, a, z;

    for (int i = 0; i < hidden_layer_size; i++)
    {
        if (i != 0)
        {
            x = activations[i-1];
        }

        w = MatrixXd::Random(node_num, node_num) * 0.1; // ここの重みを変更する
        a = x * w;
        z = sigmoid(a);

        activations[i] = z;
    }

    // vectorにEigenを詰めなおして、プロットを作成すること
    // for (auto activation : activations)
    // {
    //     cout << activation.first << ":" << endl;
    //     cout << activation.second << endl;
    // }

    vector<double> a_vec(1000*node_num);
    Map<MatrixXd>(&a_vec[0], 1000, node_num) = activations[0];

    int bins = 30;
    string title = "Layer1";

    plt::hist(a_vec, bins);
    plt::xlim(0, 1);
    plt::title(title);
    plt::save("activation_hist_xavier0.png");

    // vector<double> tmp_a(node_num*node_num);
    for (auto activation : activations)
    {
        if (activation.first != 0)
        {
            string save_filename = "activation_hist_xavier";
            save_filename += std::to_string(activation.first);
            save_filename += ".png";

            Map<MatrixXd>(&a_vec[0], 1000, node_num) = activation.second;

            title = "Layer";
            title += std::to_string(activation.first + 1);
            plt::hist(a_vec, bins);
            plt::xlim(0, 1);
            plt::title(title);
            plt::save(save_filename);
        }
    }

    return 0;
}

MatrixXd sigmoid(MatrixXd& X)
{
    return X.unaryExpr([](double p){return 1 / (1 + exp(p));});
}

// MatrixXd ReLU(MatrixXd& X)
// {
//     return X.unaryExpr([](double p){return std::max(0, p);});
// }

// MatrixXd tanh(MatrixXd& X)
// {
//     return X.unaryExpr([](double p){return tanh(p);});
// }


/**
 * @file advectionfv2d.cc
 * @brief NPDE homework AdvectionFV2D code
 * @author Philipp Egg
 * @date 21.06.2020
 * @copyright Developed at ETH Zurich
 */

#include "advectionfv2d.h"

#include <lf/geometry/geometry.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/utils/utils.h>

#include <Eigen/Core>
#include <Eigen/LU>
#include <algorithm>
#include <array>
#include <memory>
#include <stdexcept>
#include <vector>

namespace AdvectionFV2D {

/* SAM_LISTING_BEGIN_1 */
Eigen::Matrix<double, 2, 3> gradbarycoordinates(
    const Eigen::Matrix<double, 2, 3> &triangle) {
  Eigen::Matrix3d X;

  // solve for the coefficients of the barycentric coordinate functions
  X.block<3, 1>(0, 0) = Eigen::Vector3d::Ones();
  X.block<3, 2>(0, 1) = triangle.transpose();
  return X.inverse().block<2, 3>(1, 0);
}
/* SAM_LISTING_END_1 */

/* SAM_LISTING_BEGIN_2 */
std::shared_ptr<
    lf::mesh::utils::CodimMeshDataSet<Eigen::Matrix<double, 2, Eigen::Dynamic>>>
computeCellNormals(std::shared_ptr<const lf::mesh::Mesh> mesh_p) {
  //====================
  // Your code goes here
  //====================
  return nullptr;
}
/* SAM_LISTING_END_2 */

/* SAM_LISTING_BEGIN_3 */
std::shared_ptr<
    lf::mesh::utils::CodimMeshDataSet<std::array<const lf::mesh::Entity *, 4>>>
getAdjacentCellPointers(std::shared_ptr<const lf::mesh::Mesh> mesh_p) {
  //====================
  // Your code goes here
  //====================
  return nullptr;
}
/* SAM_LISTING_END_3 */

// Function returning the barycenter of TRIA or QUAD
/* SAM_LISTING_BEGIN_4 */
Eigen::Vector2d barycenter(const Eigen::MatrixXd corners) {
  Eigen::Vector2d midpoint;
  if (corners.cols() == 3) {
    midpoint = (corners.col(0) + corners.col(1) + corners.col(2)) / 3.0;
  } else if (corners.cols() == 4) {
    midpoint =
        (corners.col(0) + corners.col(1) + corners.col(2) + corners.col(3)) /
        4.0;
  } else {
    throw std::runtime_error("Wrong geometrie in barycenter()");
  }
  return midpoint;
}
/* SAM_LISTING_END_4 */

/* SAM_LISTING_BEGIN_5 */
double computeHmin(std::shared_ptr<const lf::mesh::Mesh> mesh_p) {
  //====================
  // Your code goes here
  // Replace the dummy return value below:
  return 0.0;
  //====================
}
/* SAM_LISTING_END_5 */

}  // namespace AdvectionFV2D


#include "nearest_neighbor_brute_force.h"
#include "point_AABBTree_squared_distance.h"
#include "CloudPoint.h"
#include "Object.h"
#include "AABBTree.h"
#include "warnings.h"
#include "VectorXb.h"
#include "tictoc.h"
#include "visualize_aabbtree.h"
#include <igl/read_triangle_mesh.h>
#include <Eigen/Core>
#include <iostream>
#include <string> // std::stoi
#include <iomanip> // std::setw
#include <memory> // std::shared_ptr

int main(int argc, char * argv[])
{
  /////////////////////////////////////////////////////////////////////////////
  // POINT CLOUD DISTANCE QUERY
  /////////////////////////////////////////////////////////////////////////////
  std::cout<<"# Point Cloud Distance Queries"<<std::endl;
  // Prepare a random list of points in our set and random queries
  Eigen::MatrixXd points =
    Eigen::MatrixXd::Random(argc>1?std::stoi(argv[1]):100000,3);
  Eigen::MatrixXd queries =
    Eigen::MatrixXd::Random(argc>2?std::stoi(argv[2]):10000,3);

  std::cout<<"    |points|: "<< points.rows()<<std::endl;
  std::cout<<"  |querires|: "<<queries.rows()<<std::endl<<std::endl;
  // Brute Force
  tic(); // Start the clock!
  Eigen::VectorXd bf_sqrD(queries.rows());
  Eigen::VectorXi bf_I   (queries.rows());
  // Loop over queries
  for(int i = 0;i < queries.rows(); i++)
  {
    nearest_neighbor_brute_force(points,queries.row(i),bf_I(i),bf_sqrD(i));
  }
  std::cout<<"  | Method      | Time in seconds |"<<std::endl;
  std::cout<<"  |:------------|----------------:|"<<std::endl;
  std::cout<<"  | brute force | " << FLOAT15 << toc() << " |"<<std::endl;

  tic();
  // Build a tree
  std::vector<std::shared_ptr<Object> > point_indices;
  // Put a reference to each point in a boxable object
  point_indices.reserve(points.rows());
  for(int i = 0;i<points.rows();i++)
  {
    point_indices.emplace_back(std::make_shared<CloudPoint>(points,i));
  }
  // Build tree
  std::shared_ptr<AABBTree> root = std::make_shared<AABBTree>(point_indices);
  std::cout<<"  | build tree  | " << FLOAT15 << toc() << " |"<<std::endl;

  tic();
  Eigen::VectorXd tree_sqrD(queries.rows());
  Eigen::VectorXi tree_I   (queries.rows());
  // Loop over queries
  for(int i = 0;i < queries.rows(); i++)
  {
    Eigen::RowVector3d query = queries.row(i);
    std::shared_ptr<Object> closest_object;
    const double inf = std::numeric_limits<double>::infinity();
    point_AABBTree_squared_distance(
      query,root,0,inf,tree_sqrD(i),closest_object);
    if(closest_object)
    {
      tree_I(i) = std::static_pointer_cast<CloudPoint>(closest_object)->i;
    }else
    {
      tree_I(i) = -1;
    }
  }
  std::cout<<"  | use tree    | " << FLOAT15 << toc() << " |"<<std::endl;

  // Check that solutions match.
  for(int i = 0;i < queries.rows(); i++)
  {
    WARN_IF_NOT_EQUAL(bf_I,tree_I,i);
    WARN_IF_NOT_APPROX(bf_sqrD,tree_sqrD,i);
  }
  visualize_aabbtree(points,root);

}


// Copyright 2018 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

//[ guide_custom_accumulators

#include <array>
#include <boost/format.hpp>
#include <boost/histogram.hpp>
#include <boost/histogram/accumulators/mean.hpp>
#include <iostream>

int main() {
  using namespace boost::histogram;
  const auto axis = axis::regular<>(3, 0.0, 1.0);

  // Create a 1D-profile, which computes the mean of samples in each bin. The
  // factory function `make_profile` is provided by the library as a shorthand.
  auto h1 = make_histogram_with(dense_storage<accumulators::mean<>>(), axis);

  // Argument of `sample` is passed to accumulator.
  h1(0.0, sample(2)); // sample 2 goes to first bin
  h1(0.1, sample(2)); // sample 2 goes to first bin
  h1(0.4, sample(3)); // sample 3 goes to second bin
  h1(0.5, sample(4)); // sample 4 goes to second bin

  std::ostringstream os1;
  for (auto x : indexed(h1)) {
    // Accumulators usually have methods to access their state. Use the arrow
    // operator to access them. Here, `count()` gives the number of samples,
    // `value()` the mean, and `variance()` the variance estimate of the mean.
    os1 << boost::format("%i count %i mean %.1f variance %.1f\n") % x.index() %
               x->count() % x->value() % x->variance();
  }
  std::cout << os1.str() << std::flush;
  assert(os1.str() == "0 count 2 mean 2.0 variance 0.0\n"
                      "1 count 2 mean 3.5 variance 0.5\n"
                      "2 count 0 mean 0.0 variance 0.0\n");

  // Let's make a custom accumulator, which tracks the maximum of the samples. It must
  // have a call operator that accepts the argument of the `sample` function. The return
  // value of the call operator is ignored.
  struct max {
    void operator()(double x) {
      if (x > value) value = x;
    }
    double value = 0;
  };

  // Create a histogram with the custom accumulator, initialize the accumulators
  // to 1.
  auto h2 = make_histogram_with(dense_storage<max>(), axis);
  h2(0.0, sample(2));   // sample 2 goes to first bin
  h2(0.1, sample(2.5)); // sample 2.5 goes to first bin
  h2(0.4, sample(3));   // sample 3 goes to second bin
  h2(0.5, sample(4));   // sample 4 goes to second bin

  std::ostringstream os2;
  for (auto x : indexed(h2)) {
    os2 << boost::format("%i value %.1f\n") % x.index() % x->value;
  }
  std::cout << os2.str() << std::flush;
  assert(os2.str() == "0 value 2.5\n"
                      "1 value 4.0\n"
                      "2 value 0.0\n");
}

//]


// Copyright (c) 2015
// Author: Chrono Law
#include <std.hpp>
//using namespace std;

#include <boost/random.hpp>
using namespace boost;

//////////////////////////////////////////

void case1()
{
    mt19937 rng(time(0));

    std::cout << mt19937::min() << "<->"
        << mt19937::max() << std::endl;

    for (int i = 0;i < 100;++i)
    {
        std::cout << rng() << ",";
    }

    rng.discard(5);
    std::vector<int> vec(10);
    rng.generate(vec.begin(), vec.end());

}

//////////////////////////////////////////

void case2()
{
    mt19937 rng(time(0));
    std::cout <<  rng() << std::endl;

    mt19937 rng2(rng);

    for (int i = 0;i < 10;++i)
    {   assert(rng() == rng2());    }
}

//////////////////////////////////////////

void case3()
{
    //using namespace boost::random;
    mt19937 rng(time(0));

    random::uniform_int_distribution<> ui(0, 255);
    for (int i = 0;i < 10;++i)
    {   std::cout << ui(rng) << ","; }
    assert(ui.a() == 0 && ui.b() == 255);
    std::cout << std::endl;

    uniform_01<> u01;
    for (int i = 0;i < 10;++i)
    {   std::cout << u01(rng) << ",";    }
    std::cout << std::endl;

    normal_distribution<> nd(1, 2);
    int count = 0;
    for (int i = 0;i < 10000;++i)
    {
        if (abs(nd(rng) - 1) <= 2.0)
        {   ++count;    }
    }
    std::cout << 1.0 * count / 10000 << std::endl;
}

//////////////////////////////////////////

void case4()
{
    mt19937 rng((int32_t)time(0));
    uniform_smallint<>  us(1,100);

    variate_generator<mt19937&, uniform_smallint<>> gen(rng, us);
    for (int i = 0; i < 10 ; ++i)
    {       std::cout << gen() << std::endl;  }

}

template<typename Rng >
void rand_bytes(unsigned char *buf, int buf_len)
{
    typedef variate_generator<Rng, uniform_smallint<>> var_gen_t;
    static var_gen_t
        gen(Rng((typename Rng::result_type)time(0)),
                uniform_smallint<>(1,255));

    generate_n(buf, buf_len, std::ref(gen));
    //for (int i = 0; i < buf_len; ++i)
    //{   buf[i] = gen();}
}

void case5()
{
    unsigned char buf[10];

    rand_bytes<mt19937>(buf, 10);
    for (int i = 0;i < 10 ;++i)
    {       std::cout << (short)buf[i] << ",";   }
    std::cout << std::endl;

    rand_bytes<rand48>(buf, 10);
    for (int i = 0;i < 10 ;++i)
    {       std::cout << (short)buf[i] << ",";   }

    std::cout << std::endl;
}

//////////////////////////////////////////

#include <boost/nondet_random.hpp>

class boost::random_device::impl
{
    private:
        rand48 rng;
    public:
        impl():rng(time(0))
    {   std::cout << "random_device::impl ctor\n";   }

    ~impl()
    {   std::cout << "random_device::impl dtor\n";   }

    unsigned int operator()()
    {   return rng();   }
};

boost::random_device::random_device()
: pimpl(new impl)
{}

boost::random_device::~random_device()
{   delete pimpl;}

double boost::random_device::entropy() const
{   return 10;}

unsigned int boost::random_device::operator()()
{   return (*pimpl)();}

void case6()
{
    random_device rng;
    for (int i = 0 ;i < 10 ; ++i)
    {   std::cout << rng() << ",";   }
    std::cout << std::endl;

    uniform_real<> ur(1.0, 2.0);
    for (int i = 0 ;i < 10 ; ++i)
    {   std::cout << ur(rng) << ","; }
    std::cout << std::endl;

    variate_generator<random_device&, uniform_smallint<>>
        gen(rng, uniform_smallint<>(0,255));
    for (int i = 0 ;i < 10 ; ++i)
    {   std::cout << gen() << ",";   }
    std::cout << std::endl;
}

int main()
{
    case1();
    case2();
    case3();
    case4();
    case5();
    case6();
}


//          Copyright Carl Philipp Reh 2009 - 2016.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <fcppt/math/box/comparison.hpp>
#include <fcppt/math/box/intersection.hpp>
#include <fcppt/math/box/null.hpp>
#include <fcppt/math/box/object_impl.hpp>
#include <fcppt/math/box/output.hpp>
#include <fcppt/preprocessor/disable_gcc_warning.hpp>
#include <fcppt/preprocessor/pop_warning.hpp>
#include <fcppt/preprocessor/push_warning.hpp>
#include <fcppt/config/external_begin.hpp>
#include <boost/test/unit_test.hpp>
#include <fcppt/config/external_end.hpp>

FCPPT_PP_PUSH_WARNING
FCPPT_PP_DISABLE_GCC_WARNING(-Weffc++)

BOOST_AUTO_TEST_CASE(
	math_box_intersection
)
{
FCPPT_PP_POP_WARNING

	typedef fcppt::math::box::object<
		int,
		2
	> box_i2;

	box_i2 const
		box1(
			box_i2::vector(
				1,
				1
			),
			box_i2::dim(
				2,
				2
			)
		),
		box2(
			box_i2::vector(
				0,
				0
			),
			box_i2::dim(
				4,
				4
			)
		),
		box3(
			box_i2::vector(
				2,
				2
			),
			box_i2::dim(
				2,
				2
			)
		),
		box4(
			box_i2::vector(
				5,
				5
			),
			box_i2::dim(
				1,
				1
			)
		),
		intersection1(
			fcppt::math::box::intersection(
				box1,
				box2
			)
		),
		intersection2(
			fcppt::math::box::intersection(
				box1,
				box3
			)
		),
		intersection3(
			fcppt::math::box::intersection(
				box1,
				box4
			)
		);

	BOOST_CHECK_EQUAL(
		intersection1,
		box1
	);

	box_i2 const result2(
		box_i2::vector(
			2,
			2
		),
		box_i2::dim(
			1,
			1
		)
	);

	BOOST_CHECK_EQUAL(
		intersection2,
		result2
	);

	BOOST_CHECK_EQUAL(
		intersection3,
		fcppt::math::box::null<
			box_i2
		>()
	);
}


#include <Eigen/Dense>
#include <igl/readOFF.h>
#include <igl/writeOFF.h>
#include <igl/exact_geodesic.h>
#include <igl/colormap.h>

int main()
{
    Eigen::MatrixXd V;
    Eigen::MatrixXi F;
    igl::readOFF("./data/mesh/bunny.off", V, F);

    // Compute geodesic distances from all vertices to the source vertex 0.
    Eigen::VectorXi VS, FS, VT, FT;
    VS.resize(1);
    VS << 0;
    VT.setLinSpaced(V.rows(), 0, V.rows() - 1);
    Eigen::VectorXd D;
    igl::exact_geodesic(V, F, VS, FS, VT, FT, D);

    // Visualize the geodesics as colors using a colormap.
    Eigen::MatrixXd C;
    igl::colormap(igl::COLOR_MAP_TYPE_JET, D, true, C);
    igl::writeOFF("./bunny_heat_geodesics.off", V, F, C);
}


#include "StableFluidsSim.h"
#include <Eigen/LU>

scalar lerp(scalar a, scalar b, scalar x) {
    return (1 - x) * a + x * b;
}

void StableFluidsSim::diffuseD(int N, ArrayXs * x, ArrayXs * x0, scalar diff, scalar dt)
{
  assert((*x0 == *x0).all());

  scalar a = diff * dt * N * N;
  *x = *x0;

  scalar dD;
  for (int k = 0; k < 20; k++)
  {
    for (int i = 1; i <= N; i++)
    {
      for (int j = 1; j <= N; j++) // IMPORTANT: DO NOT MODIFY THE LOOP ORDER
      {
        // STUDENTS: You will certainly need code here, do diffuse for ([1, N], [1, N])
        dD = a * ((*x)(i - 1, j) + (*x)(i + 1, j) + (*x)(i, j - 1) + (*x)(i, j + 1));
        (*x)(i, j) = ((*x0)(i, j) + dD) / (1 + 4 * a);
      }
    }
  }
}

void StableFluidsSim::diffuseU(int N, ArrayXs * x, ArrayXs * x0, scalar diff, scalar dt)
{
    assert((*x0 == *x0).all());

    scalar a = diff * dt * N * N;
    *x = *x0;
    scalar du;
    int nTerms;
    for (int k = 0; k < 20; k++)
    {
        for (int i = 1; i <= N; i++)
        {
            for (int j = 0; j <= N; j++) // IMPORTANT: DO NOT MODIFY THE LOOP ORDER
            {
                // STUDENTS: You will certainly need code here, do diffuse for ([1, N], [0, N]), note the case when (j == 0) or (j == N) need special treatment
                if (j == 0) {
                    du = a * ((*x)(i - 1, j) + (*x)(i + 1, j) + (*x)(i, j + 1));
                    nTerms = 3;
                }
                else if (j == N) {
                    du = a * ((*x)(i - 1, j) + (*x)(i + 1, j) + (*x)(i, j - 1));
                    nTerms = 3;
                }
                else {
                    du = a * ((*x)(i - 1, j) + (*x)(i + 1, j) + (*x)(i, j - 1) + (*x)(i, j + 1));
                    nTerms = 4;
                }
                (*x)(i, j) = ((*x0)(i, j) + du) / (1 + nTerms * a);
            }
        }
    }
}

void StableFluidsSim::diffuseV(int N, ArrayXs * x, ArrayXs * x0, scalar diff, scalar dt)
{
    assert((*x0 == *x0).all());

    scalar a = diff * dt * N * N;
    *x = *x0;
    scalar du;
    int nTerms;
    for (int k = 0; k < 20; k++)
    {
        for (int i = 0; i <= N; i++)
        {
            for (int j = 1; j <= N; j++) // IMPORTANT: DO NOT MODIFY THE LOOP ORDER
            {
                if (i == 0) {
                    du = a * ((*x)(i + 1, j) + (*x)(i, j - 1) + (*x)(i, j + 1));
                    nTerms = 3;
                }
                else if (i == N) {
                    du = a * ((*x)(i - 1, j) + (*x)(i, j - 1) + (*x)(i, j + 1));
                    (*x)(i, j)= ((*x0)(i, j) + du) / (1 + 3 * a);
                    nTerms = 3;
                }
                else {
                    du = a * ((*x)(i - 1, j) + (*x)(i + 1, j) + (*x)(i, j - 1) + (*x)(i, j + 1));
                    nTerms = 4;
                }
                (*x)(i, j) = ((*x0)(i, j) + du) / (1 + nTerms * a);
            }
        }
    }
}

void StableFluidsSim::advectD(int N, ArrayXs * x, ArrayXs * x0, ArrayXs * u, ArrayXs * v, scalar dt)
{
    assert((*x0 == *x0).all());
    assert((*u == *u).all());
    assert((*v == *v).all());

    // STUDENTS: You will certainly need code here, advect for ([1, N], [1, N])
    scalar dt0 = dt * N;

    for (int i = 1; i <= N; i++)
    {
        for (int j = 1; j <= N; j++)
        {
            scalar i0 = i - dt0 * interpolateV(v, i, j);
            scalar j0 = j - dt0 * interpolateU(u, i, j);
            (*x)(i, j) = interpolateD(x0, i0, j0);

        }
    }
}

scalar StableFluidsSim::interpolateD(ArrayXs * d, scalar i, scalar j)
{
    // STUDENTS: You will certainly need code here, note the indices should be CLAMP-ed to [0, m_N], since we have to use (i + 1) and (j + 1)
    int i1 = CLAMP((int) i, 0, m_N);
    int i2 = i1 + 1;
    int j1 = CLAMP((int) j, 0, m_N);
    int j2 = j1 + 1;
    scalar s = CLAMP(i - i1, 0, 1);
    scalar t = CLAMP(j - j1, 0, 1);
    scalar d1 = lerp((*d)(i1, j1), (*d)(i2, j1), s);
    scalar d2 = lerp((*d)(i1, j2), (*d)(i2, j2), s);
    return lerp(d1, d2, t);
}

scalar StableFluidsSim::interpolateU(ArrayXs * u, scalar i, scalar j)
{
    // STUDENTS: You will certainly need code here, note the i index should be CLAMP-ed to [0, m_N], while j index should be CLAMP-ed to [0, m_N-1], since we have to use (i + 1) and (j + 1)
    int i1 = CLAMP((int) i, 0, m_N);
    int i2 = i1 + 1;
    int j1 = CLAMP((int) (j - 0.5), 0, m_N - 1);
    int j2 = j1 + 1;
    scalar s = CLAMP(i - i1, 0, 1);
    scalar t = CLAMP(j - j1 - 0.5, 0, 1);
    scalar u1 = lerp((*u)(i1, j1), (*u)(i2, j1), s);
    scalar u2 = lerp((*u)(i1, j2), (*u)(i2, j2), s);
    return lerp(u1, u2, t);
}

scalar StableFluidsSim::interpolateV(ArrayXs * v, scalar i, scalar j)
{
    // STUDENTS: You will certainly need code here
    int i1 = CLAMP((int) (i - 0.5), 0, m_N - 1);
    int i2 = i1 + 1;
    int j1 = CLAMP((int) j, 0, m_N);
    int j2 = j1 + 1;
    scalar s = CLAMP(i - i1 - 0.5, 0, 1);
    scalar t = CLAMP(j - j1, 0, 1);
    scalar v1 = lerp((*v)(i1, j1), (*v)(i2, j1), s);
    scalar v2 = lerp((*v)(i1, j2), (*v)(i2, j2), s);
    return lerp(v1, v2, t);
}

void StableFluidsSim::advectU(int N, ArrayXs * x, ArrayXs * x0, ArrayXs * u, ArrayXs * v, scalar dt)
{
    assert((*x0 == *x0).all());
    assert((*u == *u).all());
    assert((*v == *v).all());

    scalar dt0 = dt * N;
    for (int i = 1; i <= N; i++)
    {
        for (int j = 0; j <= N; j++)
        {
            // STUDENTS: You will certainly need code here,
            // add the origin of U grid to the coordinate before sampling, for example, sample at (i + 0, j + 0.5) when you need backtracing the old velocity at (i, j)
            scalar i0 = i - dt0 * interpolateV(v, i, j + 0.5);
            scalar j0 = j + 0.5 - dt0 * interpolateU(u, i, j + 0.5);
            // now you have the backward-traced velocity, minus it from the current position (i + 0, j + 0.5), then sample the velocity again.
            (*x)(i, j) = interpolateU(x0, i0, j0);
        }
    }
}

void StableFluidsSim::advectV(int N, ArrayXs * x, ArrayXs * x0, ArrayXs * u, ArrayXs * v, scalar dt)
{
    assert((*x0 == *x0).all());
    assert((*u == *u).all());
    assert((*v == *v).all());

    scalar dt0 = dt * N;
    for (int i = 0; i <= N; i++)
    {
        for (int j = 1; j <= N; j++)
        {
            // STUDENTS: You will certainly need code here
            scalar i0 = i + 0.5 - dt0 * interpolateV(v, i + 0.5, j);
            scalar j0 = j - dt0 * interpolateU(u, i + 0.5, j);
            (*x)(i, j) = interpolateV(x0, i0, j0);
        }
    }
}

void StableFluidsSim::project(int N, ArrayXs * u, ArrayXs * v, ArrayXs * u0, ArrayXs * v0)
{
    if (VERBOSE) std::cout << "u0: " << std::endl << *u0 << std::endl << std::endl;
    if (VERBOSE) std::cout << "v0: " << std::endl << *v0 << std::endl << std::endl;

    ArrayXs div(N + 2, N + 2);
    ArrayXs p(N + 2, N + 2);
    div.setZero();
    p.setZero();
    scalar h = 1.0 / N;

    // STUDENTS: You will certainly need code here

    // set solid boundary conditions, 0 the most top and bottom row / left and right column of u0, v0
    for (int i = 0; i <= N; ++i) {
        // Top and bottom rows
        (*u0)(0, i) = 0;
        (*u0)(N + 1, i) = 0;
        (*v0)(0, i) = 0;
        (*v0)(N, i) = 0;

        // Left and right columns
        (*u0)(i, 0) = 0;
        (*u0)(i, N) = 0;
        (*v0)(i, 0) = 0;
        (*v0)(i, N + 1) = 0;

    }
    // Edge cases
    (*v0)(0, N + 1) = 0;
    (*v0)(N, N + 1) = 0;
    (*u0)(N + 1, 0) = 0;
    (*u0)(N + 1, N) = 0;

    for (int i = 1; i <= N; i++)
    {
        for (int j = 1; j <= N; j++)
        {
          // compute divergence of the velocity field, note the divergence field is available from ([1, N], [1, N])
          div(i, j) = -h * ((*v0)(i, j) - (*v0)(i - 1, j) + (*u0)(i, j) - (*u0)(i, j - 1));
        }
    }

    for (int k = 0; k < 20; k++)
    {
        for (int i = 1; i <= N; i++)
        {
              for (int j = 1; j <= N; j++) // IMPORTANT: DO NOT MODIFY THE LOOP ORDER
              {
                  // solve for pressure inside the region ([1, N], [1, N])
                  int nTerms = 4;
                  scalar terms = p(i - 1, j) + p(i + 1, j) + p(i, j - 1) + p(i, j + 1);

                  if (i == 1) {
                      terms -= p(i - 1, j);
                      --nTerms;
                  }
                  else if (i == N) {
                      terms -= p(i + 1, j);
                      --nTerms;
                  }

                  if (j == 1) {
                      terms -= p(i, j - 1);
                      --nTerms;
                  }
                  else if (j == N) {
                      terms -= p(i, j + 1);
                      --nTerms;
                  }
                  p(i, j) = (div(i, j) + terms) / nTerms;
              }
        }
    }

    (*u) = (*u0);
    (*v) = (*v0);

    for (int i = 1; i <= N; i++)
    {
        for (int j = 1; j < N; j++)
        {
            // apply pressure to correct velocities ([1, N], [1, N)) for u, ([1, N), [1, N]) for v
            (*u)(i, j) -= (p(i, j + 1) - p(i, j)) / h;
            (*v)(j, i) -= (p(j + 1, i) - p(j, i)) / h;
        }
    }
}


//####### Test module for quadrature ####################################

//Define Module name
 #define BOOST_TEST_MODULE "quadratures"

//Will automatically define a main for this test
 #define BOOST_TEST_DYN_LINK

//Include Boost unit tests library & library for floating point comparison
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

 //Units choice. Not relevant here, but avoids compile-time warning
 #define PXRMP_USE_SI_UNITS

#include "quadrature.hpp"

using namespace picsar::multi_physics;

// ------------- Tests --------------

//Tolerance for double precision calculations
const double double_tolerance = 1.0e-6;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-3;

//Test quadrature with double precision
BOOST_AUTO_TEST_CASE( quadrature_finite_interval_double_1 )
{
    auto sin2 = [](double x)
        {return sin(x)*sin(x);};

    const double a = 0.0;
    const double b = 2.0*pi;

    const double exp_res_sin2 = pi;

    double res_sin2 = quad_a_b<double>(sin2, a, b);

    BOOST_CHECK_SMALL((res_sin2-exp_res_sin2)/exp_res_sin2, double_tolerance);
}

BOOST_AUTO_TEST_CASE( quadrature_infinite_interval_double_1 )
{
    auto expx2 = [](double x)
        {return exp(-x*x);};

    const double a = 0.0;

    const double exp_expx2 = sqrt(pi)/2.0;

    double res_expx2 = quad_a_inf<double>(expx2, a);

    BOOST_CHECK_SMALL((res_expx2-exp_expx2)/exp_expx2, double_tolerance);
}

BOOST_AUTO_TEST_CASE( quadrature_infinite_interval_double_2 )
{
    auto datan = [](double x)
        {return 1.0/(1.0+x*x);};

    const double a = 0.0;

    const double exp_datan = pi/2.0;

    double res_datan = quad_a_inf<double>(datan, a);

    BOOST_CHECK_SMALL((res_datan-exp_datan)/exp_datan, double_tolerance);
}

//Test Bessel functions with single precision
BOOST_AUTO_TEST_CASE( quadrature_finite_interval_float_1 )
{
    auto fsin2 = [](float x)
        {return sin(x)*sin(x);};

        const float fa = static_cast<float>(0.0);
        const float fb = static_cast<float>(2.0*pi);

        const float fexp_res_sin2 = static_cast<float>(pi);

        float fres_sin2 = quad_a_b<float>(fsin2, fa, fb);

        BOOST_CHECK_SMALL((fres_sin2-fexp_res_sin2)/fexp_res_sin2,
            float_tolerance);
}

BOOST_AUTO_TEST_CASE( quadrature_infinite_interval_float_1 )
{
    auto fexpx2 = [](float x)
        {return exp(-x*x);};

    const float fa = static_cast<float>(0.0);

    const float fexp_expx2 = static_cast<float>(sqrt(pi)/2.0);

    float fres_expx2 = quad_a_inf<float>(fexpx2, fa);

    BOOST_CHECK_SMALL((fres_expx2-fexp_expx2)/fexp_expx2, float_tolerance);
}

BOOST_AUTO_TEST_CASE( quadrature_infinite_interval_float_2 )
{
    auto fdatan = [](float x)
        {return 1.0f/(1.0f+x*x);};

    const float fa = static_cast<float>(0.0);

    const float fexp_datan = static_cast<float>(pi/2.0);

    float fres_datan = quad_a_inf<float>(fdatan, fa);

    BOOST_CHECK_SMALL((fres_datan-fexp_datan)/fexp_datan, float_tolerance);
}


#include "SHTools.h"
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>
#include <Eigen/Dense>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <ctime>
#include <fstream>
#include <iostream>
#include <math.h>
#include <random>
#include <string>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_vertex_base_with_info_2<unsigned, K> Vb;
typedef CGAL::Triangulation_data_structure_2<Vb> Tds;
typedef CGAL::Delaunay_triangulation_2<K, Tds> Delaunay;
typedef Delaunay::Face_circulator Face_circulator;
typedef Delaunay::Face_handle Face_handle;
typedef Delaunay::Point Point;
typedef Eigen::Vector3d Vector3d;
typedef Eigen::VectorXd VectorXd;
typedef Eigen::Matrix3d Matrix3d;
typedef Eigen::Matrix3Xd Matrix3Xd;
typedef Eigen::Map<Matrix3Xd> MapM3Xd;
typedef Eigen::Ref<Matrix3Xd> RefM3Xd;

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

  // We expect three command line arguments:
  // 1. Path to the data file
  // 2. Output directory where output files will be created
  if (argc < 3) {
    std::cout << "Usage: ./postProcess <data_file> <output_dir>" << std::endl;
    return 0;
  }

  clock_t t = clock();

  // Open the data file
  std::ifstream inputfile(argv[1]);
  if (!inputfile.is_open()) {
    std::cout << "Unable to open file " << argv[1] << std::endl;
    return 0;
  }

  //**********************************************************************//
  // Create directory for storing the output files f.dat, Alm.dat and
  // spectrum.dat
  //

  auto outputDir = boost::filesystem::path(std::string(argv[2]));
  if (!boost::filesystem::create_directory(outputDir)) {
    std::cout << "Could not create output directory " << outputDir << std::endl;
    return 0;
  }

  auto f_path = std::string(argv[2]) + "/f0.dat";
  std::ofstream f_file(f_path);
  auto df2_path = std::string(argv[2]) + "/df2.dat";
  std::ofstream df2_file(df2_path);
  auto u_path = std::string(argv[2]) + "/u2.dat";
  std::ofstream u_file(u_path);
  auto Alm_mean_path = std::string(argv[2]) + "/Alm_mean.dat";
  std::ofstream Alm_mean_file(Alm_mean_path);
  auto Alm_var_path = std::string(argv[2]) + "/Alm_var.dat";
  std::ofstream Alm_var_file(Alm_var_path);
  auto spec_path = std::string(argv[2]) + "/spectrum.dat";
  std::ofstream spec_file(spec_path);

  if (!f_file) {
    std::cout << "Failed to create output file " << f_path;
    return 0;
  }
  if (!df2_file) {
    std::cout << "Failed to create output file " << df2_path;
    return 0;
  }
  if (!u_file) {
    std::cout << "Failed to create output file " << u_path;
    return 0;
  }
  if (!Alm_mean_file) {
    std::cout << "Failed to create output file " << Alm_mean_path;
    return 0;
  }
  if (!Alm_var_file) {
    std::cout << "Failed to create output file " << Alm_var_path;
    return 0;
  }
  if (!spec_file) {
    std::cout << "Failed to create output file " << spec_path;
    return 0;
  }

  //**********************************************************************//
  // Now read the first( or the header ) line of the data file
  // The first line contains N, gamma and beta information
  std::string line, ignore;
  size_t N;
  double_t gamma = 0, beta = 0;
  std::getline(inputfile, line);
  std::istringstream header(line);
  header >> ignore >> N >> ignore >> gamma >> ignore >> beta;
  int lmax = std::floor(std::sqrt(N) - 1);

  // Write column names in the output files
  f_file << "f0" << std::endl;
  df2_file << "df2" << std::endl;
  u_file << "mean,variance" << std::endl;
  for (auto i = 0; i < lmax; ++i) {
    Alm_mean_file << "l" << i << ",";
    Alm_var_file << "l" << i << ",";
    spec_file << "l" << i << ",";
  }
  Alm_mean_file << "l" << lmax << std::endl;
  Alm_var_file << "l" << lmax << std::endl;
  spec_file << "l" << lmax << std::endl;

  //**********************************************************************//
  // Create the Gauss-Lengendre grid
  //
  int nlat, nlong;
  Eigen::VectorXd latglq(lmax + 1);
  Eigen::VectorXd longlq(2 * lmax + 1);
  glqgridcoord_wrapper_(latglq.data(), longlq.data(), &lmax, &nlat, &nlong);
  auto numQ = nlat * nlong;

  Eigen::VectorXd gridglq(numQ);
  Eigen::VectorXd plx((lmax + 1) * (lmax + 1) * (lmax + 2) / 2);
  Eigen::VectorXd w(lmax + 1), zero(lmax + 1);
  Eigen::VectorXd cilm(2 * (lmax + 1) * (lmax + 1));
  Eigen::VectorXd pspectrum(lmax + 1);

  // Pre-compute all the matrices needed for expansion
  shglq_wrapper_(&lmax, zero.data(), w.data(), plx.data());

  //**********************************************************************//
  // Some useful lambda functions
  //

  // A function to read N rows of a 3xN matrix from a stringstream
  auto readMatrix3Xd = [](std::istringstream &ss, RefM3Xd X) {
    auto N = X.cols();
    size_t valCount = 0;
    while (valCount < 3 * N && ss.good()) {
      std::string value;
      std::getline(ss, value, ',');
      X(valCount % 3, valCount / 3) = std::stod(value);
      valCount++;
    }
  };

  // Calculate Alm from cilm
  auto A_lm = [&cilm, &lmax](const size_t l, const int m) {
    int j = 0, n = m;
    if (m < 0) {
      j = 1;
      n = -m;
    }
    return cilm(j + 2 * (l + (lmax + 1) * n));
  };

  // The first row of data is ALWAYS the zero temperature particle position
  // and rotation vectors. We will use it to locate the triangles containing
  // the quadrature points for interpolation and also the zero temperature
  // radius
  Matrix3Xd zeroTempX(3, N);
  std::getline(inputfile, line);
  std::istringstream zeroTempRow(line);
  readMatrix3Xd(zeroTempRow, zeroTempX);

  // Calculate the zero temperature radius
  double_t R0 = zeroTempX.colwise().norm().mean();

  // Now we need to project the zeroTempX to a sphere of radius R0
  Matrix3Xd X0(3, N);
  X0 = R0 * zeroTempX.colwise().normalized();

  // The following matrix is unit normals associated with X0
  Matrix3Xd x0(3, N);
  x0 = X0.normalized();

  // Quadrature point coordinates in Cartesian on a sphere of radius R0
  Matrix3Xd Q0(3, numQ);
  size_t index = 0;
  for (auto ip = 0; ip < nlong; ++ip) {
    auto phi = longlq(ip) * M_PI / 180.0;
    auto sin_p = std::sin(phi);
    auto cos_p = std::cos(phi);
    for (auto it = 0; it < nlat; ++it) {
      auto theta = (90.0 - latglq(it)) * M_PI / 180.0;
      auto sin_t = std::sin(theta);
      Q0.col(index++) << R0 * sin_t * cos_p, R0 * sin_t * sin_p,
          R0 * std::cos(theta);
    }
  }

  // Rotate all points of the shell so that the 0th point is along z-axis
  Vector3d c = X0.col(0);
  double_t cos_t = c(2) / c.norm();
  double_t sin_t = std::sin(std::acos(cos_t));
  Vector3d axis;
  axis << c(1), -c(0), 0.;
  axis.normalize();
  Matrix3d rotMat, axis_cross, outer;
  axis_cross << 0., -axis(2), axis(1), axis(2), 0., -axis(0), -axis(1), axis(0),
      0.;
  outer.noalias() = axis * axis.transpose();
  rotMat =
      cos_t * Matrix3d::Identity() + sin_t * axis_cross + (1 - cos_t) * outer;
  Matrix3Xd rPts(3, N);
  rPts = rotMat * X0;

  // Calculate the stereographic projections
  Vector3d p0;
  p0 << 0, 0, -R0; // Point on the plane of projection
  c = rPts.col(0); // The point from which we are projecting

  MapM3Xd l0(&(rPts(0, 1)), 3, N - 1);
  Matrix3Xd l(3, N - 1), proj(3, N - 1);
  l = (l0.colwise() - c).colwise().normalized(); // dirns of projections
  for (auto j = 0; j < N - 1; ++j) {
    proj.col(j) = ((p0(2) - l0(2, j)) / l(2, j)) * l.col(j) + l0.col(j);
  }

  // Insert the projected points in a CGAL vertex_with_info vector
  std::vector<std::pair<Point, unsigned>> verts;
  for (auto j = 0; j < N - 1; ++j) {
    verts.push_back(std::make_pair(Point(proj(0, j), proj(1, j)), j + 1));
  }

  // Triangulate
  Delaunay dt(verts.begin(), verts.end());

  // Rotate the quadrature points
  Matrix3Xd Qr(3, numQ);
  Qr = rotMat * Q0;

  // Stereographic projection of quadrature points
  Eigen::Matrix3Xd lQ(3, numQ), Qsp(3, numQ);
  lQ = (Qr.colwise() - c).colwise().normalized();
  for (auto j = 0; j < numQ; ++j) {
    Qsp.col(j) = ((p0(2) - Qr(2, j)) / lQ(2, j)) * lQ.col(j) + Qr.col(j);
  }
  //**********************************************************************//

  //******************** Determine interpolation weights ******************//
  std::vector<std::vector<std::pair<size_t, double_t>>> interpolData;
  for (auto q = 0; q < numQ; ++q) {
    auto query = Point(Qsp(0, q), Qsp(1, q));
    Delaunay::Locate_type lt;
    int li;
    Face_handle face = dt.locate(query, lt, li);
    std::vector<std::pair<size_t, double_t>> nodesAndWeights;
    switch (lt) {
    case Delaunay::FACE: {
      auto i = face->vertex(0)->info();
      auto j = face->vertex(1)->info();
      auto k = face->vertex(2)->info();
      Eigen::Vector3d v0 = X0.col(i);
      Eigen::Vector3d v1 = X0.col(j);
      Eigen::Vector3d v2 = X0.col(k);
      Eigen::Vector3d qp = Q0.col(q);
      auto A0 = ((v1 - qp).cross((v2 - qp))).norm();
      auto A1 = ((v2 - qp).cross((v0 - qp))).norm();
      auto A2 = ((v0 - qp).cross((v1 - qp))).norm();
      auto A = A0 + A1 + A2;
      nodesAndWeights.push_back(std::make_pair(i, A0 / A));
      nodesAndWeights.push_back(std::make_pair(j, A1 / A));
      nodesAndWeights.push_back(std::make_pair(k, A2 / A));
      break;
    }
    case Delaunay::OUTSIDE_CONVEX_HULL: {
      auto i = dt.is_infinite(face->vertex(0)) ? 0 : face->vertex(0)->info();
      auto j = dt.is_infinite(face->vertex(1)) ? 0 : face->vertex(1)->info();
      auto k = dt.is_infinite(face->vertex(2)) ? 0 : face->vertex(2)->info();
      Eigen::Vector3d v0 = X0.col(i);
      Eigen::Vector3d v1 = X0.col(j);
      Eigen::Vector3d v2 = X0.col(k);
      Eigen::Vector3d qp = Q0.col(q);
      auto A0 = ((v1 - qp).cross((v2 - qp))).norm();
      auto A1 = ((v2 - qp).cross((v0 - qp))).norm();
      auto A2 = ((v0 - qp).cross((v1 - qp))).norm();
      auto A = A0 + A1 + A2;
      nodesAndWeights.push_back(std::make_pair(i, A0 / A));
      nodesAndWeights.push_back(std::make_pair(j, A1 / A));
      nodesAndWeights.push_back(std::make_pair(k, A2 / A));
      break;
    }
    case Delaunay::EDGE: {
      auto id1 = face->vertex((li + 1) % 3)->info();
      auto id2 = face->vertex((li + 2) % 3)->info();
      Eigen::Vector3d v1, v2, qp;
      v1 = X0.col(id1);
      v2 = X0.col(id2);
      qp = Q0.col(q);
      double_t ratio = (qp - v1).norm() / (qp - v2).norm();
      nodesAndWeights.push_back(std::make_pair(id1, 1.0 / (1 + ratio)));
      nodesAndWeights.push_back(std::make_pair(id2, ratio / (1 + ratio)));
      break;
    }
    case Delaunay::VERTEX:
      nodesAndWeights.push_back(std::make_pair(li, 1.0));
      break;
    default:
      std::cout << "Quadrature point " << q << " not found!" << std::endl;
    }
    interpolData.push_back(nodesAndWeights);
  }
  //**********************************************************************//

  //***************************** MAIN LOOP ******************************//
  // Now iterate over the remaining rows
  auto f0_mean = 0.0;
  auto rowCount = 1;
  while (std::getline(inputfile, line)) {

    std::istringstream rowStream(line);

    // We will extract 3*N doubles representing particle positions
    // from the stream and ignore the 3*N rotation vector components
    Matrix3Xd Xt(3, N);
    readMatrix3Xd(rowStream, Xt);

    // Calculate finput for all particles
    VectorXd finput(N);
    // VectorXd u_squared(N);
    finput = ((Xt - X0).array() * x0.array()).matrix().colwise().sum();
    auto f0 = finput.mean();
    f_file << f0 << std::endl;
    f0_mean += f0;

    // Calculate tangent fluctuations
    VectorXd u_squared(N);
    u_squared = ((Xt - X0) - x0 * finput.asDiagonal()).colwise().squaredNorm();
    u_file << u_squared.mean() << ","
           << (u_squared - u_squared.mean() * VectorXd::Ones(N))
                  .array()
                  .square()
                  .mean()
           << std::endl;

    // Now calculate f at quadrature points from finput by interpolation
    gridglq.setZero(numQ);
    for (auto q = 0; q < numQ; ++q) {
      auto nodeWeights = interpolData[q];
      for (const auto &nw : nodeWeights) {
        gridglq(q) += finput(nw.first) * nw.second;
      }
    }

    // Expand using Gauss-Legendre Quadrature
    shexpandglq_wrapper_(cilm.data(), &lmax, gridglq.data(), w.data(),
                         plx.data());

    // Calculate Alm coefficients and their mean and variance
    for (auto l = 0; l < lmax; ++l) {
      std::vector<double_t> Alm_vec;
      double_t Alm_mean = 0.0;
      for (auto m = -l; m <= l; ++m) {
        auto Alm = A_lm(l, m);
        Alm_mean += Alm;
        Alm_vec.push_back(Alm);
      }
      auto n = Alm_vec.size();
      Alm_mean /= n;
      double_t Alm_var = 0.0;
      for (const auto &alm : Alm_vec) {
        Alm_var += (alm - Alm_mean) * (alm - Alm_mean);
      }
      Alm_var /= n;
      // Write the mean and variance to file
      Alm_mean_file << Alm_mean << ",";
      Alm_var_file << Alm_var << ",";
    }

    {
      std::vector<double_t> Alm_vec;
      double_t Alm_mean = 0.0;
      for (auto m = -lmax; m <= lmax; ++m) {
        auto Alm = A_lm(lmax, m);
        Alm_mean += Alm;
        Alm_vec.push_back(Alm);
      }
      auto n = Alm_vec.size();
      Alm_mean /= n;
      double_t Alm_var = 0.0;
      for (const auto &alm : Alm_vec) {
        Alm_var += (alm - Alm_mean) * (alm - Alm_mean);
      }
      Alm_var /= n;
      // Write the mean and variance to file
      Alm_mean_file << Alm_mean << std::endl;
      Alm_var_file << Alm_var << std::endl;
    }

    // Get the power spectrum and write it to file
    shpowerspectrum_wrapper_(cilm.data(), &lmax, pspectrum.data());
    for (auto l = 0; l < lmax; ++l)
      spec_file << pspectrum(l) << ",";
    spec_file << pspectrum(lmax) << std::endl;

    rowCount++;
  }
  // Time average of f0
  f0_mean /= rowCount;

  // Close all the open files
  spec_file.close();
  f_file.close();
  u_file.close();
  Alm_mean_file.close();
  Alm_var_file.close();

  // Now we will read the input data file again to calculate variance of
  // finput over all time steps
  inputfile.clear();
  inputfile.seekg(0, std::ios::beg);
  std::getline(inputfile, line); // Eat the header line
  std::getline(inputfile, line); // Eat the zero temperature line

  while (std::getline(inputfile, line)) {

    std::istringstream rowStream(line);

    // We will extract 3*N doubles representing particle positions
    // from the stream and ignore the 3*N rotation vector components
    Matrix3Xd Xt(3, N);
    readMatrix3Xd(rowStream, Xt);

    // Calculate finput for all particles
    VectorXd finput(N);
    finput = ((Xt - X0).array() * x0.array()).matrix().colwise().sum();
    df2_file << (finput - f0_mean * VectorXd::Ones(N)).array().square().mean()
             << std::endl;
  }

  df2_file.close();

  std::cout << "Time elapsed = " << ((float)(clock() - t)) / CLOCKS_PER_SEC
            << " seconds." << std::endl;

  return 1;
}


// weighted_die.cpp
//
// Copyright (c) 2009
// Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[weighted_die
/*`
    For the source of this example see
    [@boost://libs/random/example/weighted_die.cpp weighted_die.cpp].
*/
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/discrete_distribution.hpp>

boost::mt19937 gen;

/*`
   This time, instead of a fair die, the probability of
   rolling a 1 is 50% (!).  The other five faces are all
   equally likely.

   __discrete_distribution works nicely here by allowing
   us to assign weights to each of the possible outcomes.

   [tip If your compiler supports `std::initializer_list`,
   you can initialize __discrete_distribution directly with
   the weights.]
*/
double probabilities[] = {
    0.5, 0.1, 0.1, 0.1, 0.1, 0.1
};
boost::random::discrete_distribution<> dist(probabilities);

/*`
  Now define a function that simulates rolling this die.
*/
int roll_weighted_die() {
    /*<< Add 1 to make sure that the result is in the range [1,6]
         instead of [0,5].
    >>*/
    return dist(gen) + 1;
}

//]

#include <iostream>

int main() {
    for(int i = 0; i < 10; ++i) {
        std::cout << roll_weighted_die() << std::endl;
    }
}


//==================================================================================================
//  BiLinearInterpolation.hpp
//
//  Created by Zachary Clawson on 7/19/2015
//  Copyright (c) 2015 Zachary Clawson. All rights reserved.
//==================================================================================================

#ifndef __BiLinearInterpolation_hpp
#define __BiLinearInterpolation_hpp

#include <Eigen/Dense>
#include "config/GlobalTypes.hpp"

//------------------------------------------------------------------------------------------------//
//------------------------------------------------------------------------------------------------//

/** BiLinearInterpolation
 *  * This implementation follows the way it is written on Wikipedia with the exception that we
 *    reverse the meaning of eta and xi (i.e. on Wikipedia eta <- 1-eta and xi <- 1-xi
 *    https://en.wikipedia.org/wiki/Bilinear_interpolation
 */

class BiLinearInterpolationPrivate;

class BiLinearInterpolation {
public:
	BiLinearInterpolation(
        const ArrayXreal_t & values,
        const Vector2real_t bounds_min,
        const Vector2real_t bounds_max
    );

	~BiLinearInterpolation();

    real_t operator()(const Vector2real_t & x) const;

private:
    std::unique_ptr<BiLinearInterpolationPrivate> p;
};

//------------------------------------------------------------------------------------------------//

//------------------------------------------------------------------------------------------------//
//------------------------------------------------------------------------------------------------//

#endif /* BILINEARINTERPOLATION */


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2020 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Authors: Wolfgang Bangerth, 1999,
 *          Guido Kanschat, 2011
 *          Luca Heltai, 2021
 */
#include "step-3.h"

#include <deal.II/base/function.h>
#include <deal.II/base/quadrature_lib.h>

#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_values.h>

#include <deal.II/grid/grid_generator.h>

#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/solver_cg.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>

#include <fstream>
#include <iostream>

using namespace dealii;

Step3::Step3()
  : ParameterAcceptor("Step3")
  , dof_handler(triangulation)
{
  add_parameter("Number of global refinements", n_refinements);
  add_parameter("Output filename", output_name);
  add_parameter("Finite element degree", fe_degree);
  add_parameter("Forcing term expression", forcing_term_expression);
  add_parameter("Boundary condition expression", boundary_contition_expression);
  add_parameter("Problem constants", function_constants);
  add_parameter("Grid generator function", grid_generator_function);
  add_parameter("Grid generator arguments", grid_generator_arguments);
}

void
Step3::make_grid(const std::string &params)
{
  ParameterAcceptor::initialize(params);
  // GridGenerator::hyper_cube(triangulation, -1, 1);
  // GridGenerator::hyper_L(triangulation);
  GridGenerator::generate_from_name_and_arguments(triangulation,
                                                  grid_generator_function,
                                                  grid_generator_arguments);
  // triangulation.begin_active()->face(0)->set_boundary_id(1);
  // // for (auto &face : triangulation.active_face_iterators())
  // //   if (((std::fabs(face->center()[0]) < 1e-12 &&
  // //         std::fabs(face->center()[1] - 0.5) < 1e-12) ||
  // //        (std::fabs(face->center()[1]) < 1e-12 &&
  // //         std::fabs(face->center()[0] - 0.5) < 1e-12)))
  //     face->set_boundary_id(1);
  triangulation.refine_global(n_refinements);
  std::cout << "Number of active cells: " << triangulation.n_active_cells()
            << std::endl;
}

void
Step3::setup_system()
{
  FE_Q<2> fe{fe_degree};
  dof_handler.distribute_dofs(fe);
  std::cout << "Number of degrees of freedom: " << dof_handler.n_dofs()
            << std::endl;
  DynamicSparsityPattern dsp(dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern(dof_handler, dsp);
  sparsity_pattern.copy_from(dsp);
  system_matrix.reinit(sparsity_pattern);
  solution.reinit(dof_handler.n_dofs());
  system_rhs.reinit(dof_handler.n_dofs());

  forcing_term.initialize("x,y", forcing_term_expression, function_constants);
  boundary_condition.initialize("x,y",
                                boundary_contition_expression,
                                function_constants);
}

void
Step3::assemble_system()
{
  QGauss<2>          quadrature_formula(fe_degree + 1);
  FEValues<2>        fe_values(dof_handler.get_fe(),
                        quadrature_formula,
                        update_values | update_gradients | update_JxW_values |
                          update_quadrature_points);
  const unsigned int dofs_per_cell = dof_handler.get_fe().n_dofs_per_cell();
  FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
  Vector<double>     cell_rhs(dofs_per_cell);
  std::vector<types::global_dof_index> local_dof_indices(dofs_per_cell);
  for (const auto &cell : dof_handler.active_cell_iterators())
    {
      fe_values.reinit(cell);
      cell_matrix = 0;
      cell_rhs    = 0;
      for (const unsigned int q_index : fe_values.quadrature_point_indices())
        {
          for (const unsigned int i : fe_values.dof_indices())
            for (const unsigned int j : fe_values.dof_indices())
              cell_matrix(i, j) +=
                (fe_values.shape_grad(i, q_index) * // grad phi_i(x_q)
                 fe_values.shape_grad(j, q_index) * // grad phi_j(x_q)
                 fe_values.JxW(q_index));           // dx
          for (const unsigned int i : fe_values.dof_indices())
            cell_rhs(i) += (fe_values.shape_value(i, q_index) * // phi_i(x_q)
                            forcing_term.value(
                              fe_values.quadrature_point(q_index)) * // f(x_q)
                            fe_values.JxW(q_index));                 // dx
        }
      cell->get_dof_indices(local_dof_indices);
      for (const unsigned int i : fe_values.dof_indices())
        for (const unsigned int j : fe_values.dof_indices())
          system_matrix.add(local_dof_indices[i],
                            local_dof_indices[j],
                            cell_matrix(i, j));
      for (const unsigned int i : fe_values.dof_indices())
        system_rhs(local_dof_indices[i]) += cell_rhs(i);
    }
  std::map<types::global_dof_index, double> boundary_values;
  // VectorTools::interpolate_boundary_values(dof_handler,
  //                                          0,
  //                                          Functions::ZeroFunction<2>(),
  //                                          boundary_values);
  VectorTools::interpolate_boundary_values(dof_handler,
                                           0,
                                           boundary_condition,
                                           boundary_values);
  MatrixTools::apply_boundary_values(boundary_values,
                                     system_matrix,
                                     solution,
                                     system_rhs);
}

void
Step3::solve()
{
  SolverControl            solver_control(1000, 1e-12);
  SolverCG<Vector<double>> solver(solver_control);
  solver.solve(system_matrix, solution, system_rhs, PreconditionIdentity());
}

void
Step3::output_results() const
{
  DataOut<2> data_out;
  data_out.attach_dof_handler(dof_handler);
  data_out.add_data_vector(solution, "solution");
  data_out.build_patches();
  std::ofstream output(output_name);
  data_out.write_vtk(output);
}

void
Step3::run(const std::string &params)
{
  make_grid(params);
  setup_system();
  assemble_system();
  solve();
  output_results();
}


#pragma once

#include <glad/glad.h>
#include <bound.hpp>
#include <Eigen/Dense>

namespace Config {

namespace Type {
    using real = float;
    using Point = Eigen::Matrix<real, 2, 1>;
    using Vertex = Point;
    using Color3f = Eigen::Vector3f;
    using Color4f = Eigen::Vector4f;
    using Bound2D = Bounds<real, 2>;

    using SData = Eigen::Matrix3i;

    template<int width, int height>
    struct Size2D final {
        static constexpr int width = width;
        static constexpr int height = height;
    };

    template<int width, int height, int depth>
    struct Size3D final {
        static constexpr int width = width;
        static constexpr int height = height;
        static constexpr int depth = depth;
    };

    struct Offset2D final {
        int x, y;
        bool operator!=(Offset2D other) {
            return x != other.x || y != other.y;
        }
    };

} // namespace Type

} // namespace Config


#include "SVD_pvfmm.hpp"

#include <chrono>
#include <iomanip>
#include <iostream>
#include <limits>
#include <vector>

#include <Eigen/Dense>

// Assume A=(m,n), m>n
// U = (m,n), S = (n,n), VT = (n,n)
void testSVD(const EMat &U, const EVec &Sdiag, const EMat &VT, const EMat &A, const EVec &x, const EVec &b) {
    EMat S(U.cols(), VT.rows());
    S = Sdiag.asDiagonal();

    // step 1, test if USVT==A
    EMat Arecon = U * (S * VT);
    EMat Aerror = Arecon - A;
    printf("Aerror max min %g, %g\n", Aerror.maxCoeff(), Aerror.minCoeff());

    // step 2, test backward error
    EVec Sdiaginv = Sdiag;
    for (int i = 0; i < Sdiaginv.size(); i++) {
        Sdiaginv[i] = Sdiaginv[i] < Sdiag[0] * eps ? 0 : 1.0 / Sdiaginv[i];
    }

    EMat V = VT.transpose();
    for (int i = 0; i < Sdiaginv.size(); i++) {
        V.col(i) *= Sdiaginv[i];
    }

    EVec x2 = V * (U.transpose() * b);
    EVec b2 = A * x2;
    EVec xerror = x2 - x;
    EVec berror = b2 - b;
    printf("xerror max min %g, %g\n", xerror.maxCoeff(), xerror.minCoeff());
    printf("berror max min %g, %g\n", berror.maxCoeff(), berror.minCoeff());
}

inline double pot(const EVec3 &target, const EVec3 &source) {
    EVec3 rst = target - source;
    double rnorm = rst.norm();
    return rnorm < eps ? 0 : 1 / rnorm;
}

int main(int argc, char **argv) {
    Eigen::initParallel();

    const int pEquiv = atoi(argv[1]); // (8-1)^2*6 + 2 points
    const int pCheck = atoi(argv[1]);
    const double scaleEquiv = 1.05;
    const double scaleCheck = 2.95;
    const double pCenterEquiv[3] = {-(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2, -(scaleEquiv - 1) / 2};
    const double pCenterCheck[3] = {-(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2, -(scaleCheck - 1) / 2};

    auto pointMEquiv = surface(pEquiv, (double *)&(pCenterEquiv[0]), scaleEquiv, 0);
    auto pointMCheck = surface(pCheck, (double *)&(pCenterCheck[0]), scaleCheck, 0);

    // Aup for solving MEquiv
    const int equivN = pointMEquiv.size() / 3;
    const int checkN = pointMCheck.size() / 3;
    EMat Aup(checkN, equivN);
    for (int k = 0; k < checkN; k++) {
        EVec3 Cpoint(pointMCheck[3 * k], pointMCheck[3 * k + 1], pointMCheck[3 * k + 2]);
        for (int l = 0; l < equivN; l++) {
            const EVec3 Lpoint(pointMEquiv[3 * l], pointMEquiv[3 * l + 1], pointMEquiv[3 * l + 2]);
            Aup(k, l) = pot(Cpoint, Lpoint);
        }
    }

    EVec x(Aup.cols());
    x.setRandom();
    EVec b = Aup * x;

    // jacobi svd
    using std::cout;
    using std::endl;

    {
        cout << "JacobiSVD" << endl;
        Eigen::JacobiSVD<EMat> svd(Aup, Eigen::ComputeThinU | Eigen::ComputeThinV);
        EMat U = svd.matrixU();
        EMat VT = svd.matrixV().transpose();
        EVec Svec = svd.singularValues();
        testSVD(U, Svec, VT, Aup, x, b);
    }
    // this triggers error #13212make -j: Reference to ebx in function requiring stack alignment
    // {
    //     cout << "BDCSVD" << endl;
    //     Eigen::BDCSVD<EMat> svd(Aup, Eigen::ComputeThinU | Eigen::ComputeThinV);
    //     EMat U = svd.matrixU();
    //     EMat VT = svd.matrixV().transpose();
    //     EVec Svec = svd.singularValues();
    //     testSVD(U, Svec, VT, Aup, x, b);
    // }
    {
        cout << "HouseholderQR" << endl;
        EVec x2 = Aup.colPivHouseholderQr().solve(b);
        EVec b2 = Aup * x2;
        EVec xerror = x2 - x;
        EVec berror = b2 - b;
        printf("xerror max min %g, %g\n", xerror.maxCoeff(), xerror.minCoeff());
        printf("berror max min %g, %g\n", berror.maxCoeff(), berror.minCoeff());
    }

    return 0;
}