#include <socialgraph.h>
#include <math.h>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/bc_clustering.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>

Graph createGraph(std::ifstream &fin){
	facebook::Name name;
	facebook::Data data = facebook::readFile(fin, name);

	facebook::Map map = facebook::createMap(data);

	Graph g = facebook::createGraph(data, map);

	return g;
}

WeightedGraph createWeightedGraph(Graph g){
	WeightedGraph wg(num_vertices(g));

	property_map<WeightedGraph, edge_weight_t>::type weightmap = get(edge_weight, wg);
	EdgeIterator e, e_end;
	for(tie(e, e_end) = edges(g); e != e_end; e++){
		WeightedEdgeDescriptor e1; bool inserted;
		tie(e1, inserted) = add_edge(source(*e, g), target(*e, g), wg);
		weightmap[e1] = 1;
	}

	return wg;
}

Matrix createAllPairsShortestPaths(WeightedGraph wg){
	int V = num_vertices(wg);
	Matrix D(V, std::vector<int>(V));
	johnson_all_pairs_shortest_paths(wg, D);

	return D;
}

//have to corresponding vertices
Edges newEdgeList(Graph g, Graph g2){
	Edges edges;
	for(int i=0; i<num_vertices(g); i++){
		for(int j=i+1; j<num_vertices(g); j++){
			if(edge(i, j, g).second == 0){
				if(edge(i, j, g2).second == 1){
					Edge e(i, j);
					edges.push_back(e);
				}
			}
		}
	}
	return edges;
}

Matrix newList2Matrix(Graph g, Edges edges){
	Matrix matrix(num_vertices(g));
	for(int i=0; i<num_vertices(g); i++){
		std::vector<int> temp(num_vertices(g), 0);
		matrix[i] = temp;
	}
	for(int i=0; i<edges.size(); i++){
		matrix[edges[i].first][edges[i].second] = 1;
		matrix[edges[i].second][edges[i].first] = 1;
	}
	return matrix;
}

MatrixScore initializeScore(Graph g){
	MatrixScore scores(num_vertices(g));

	for(int i=0; i<num_vertices(g); i++){
		std::vector<double> score(num_vertices(g), 0.0);
		scores[i] = score;
	}
	return scores;
}

std::vector<double> sortDouble(std::vector<double> scores){
	std::vector<double> temps;
	for(int i=0; i<scores.size(); i++){
		if(scores[i] > 0)
			temps.push_back(scores[i]);
	}
	std::sort(temps.begin(), temps.end());
	return temps;
}

bool comparisonSort(Score i, Score j){
	if(i.first < j.first) return false;
	if(j.first < i.first) return true;
	return j.second.first < i.second.first;
}

Scores SortScores(MatrixScore mscores){
	Scores scores;

	for(int i=0; i<mscores.size(); i++){
		for(int j=i+1; j<mscores.size(); j++){
			scores.push_back(Score(mscores[i][j], Edge(i,j)));
		}
	}
	std::sort(scores.begin(), scores.end(), comparisonSort);

	return scores;
}

Scores SortIndividualScores(MatrixScore mscores, int node){
	Scores scores;

	for(int i=0; i<mscores.size(); i++){
		scores.push_back(Score(mscores[node][i], Edge(node, i)));
	}

	std::sort(scores.begin(), scores.end(), comparisonSort);

	return scores;
}

float AvgDegree(Graph g){
	float avg=0.0;

	for(int i=0; i<num_vertices(g); i++)
		avg += out_degree(i,g);

	avg = avg / num_vertices(g);

	return avg;
}

MatrixScore ExtAdamicAdar(Graph g){
	Graph temp_g = g;
	WeightedGraph wg = createWeightedGraph(temp_g);
	Matrix distance = createAllPairsShortestPaths(wg);
	MatrixScore scores = initializeScore(g);

	for(int i=0; i<num_vertices(g); i++){
		for(int j=0; j<num_vertices(g); j++){
			if(edge(i,j,g).second == 1)
				scores[i][j] = 1;
		}
	}

	float avg = AvgDegree(g);
	float thresh = (float)avg/log(2);

	for(int k=0; k<2; k++){

	for(int x=0; x<num_vertices(temp_g); x++){
		for(int y=0; y<num_vertices(temp_g); y++){
			if(distance[x][y] == 2){
				for(int i=0; i<num_vertices(g); i++){
					if(distance[x][i] == 1 && distance[i][y] == 1){
						scores[x][y] += (double)(scores[x][i] * scores[y][i])/log(out_degree(i,g));
					}
				}
				if(scores[x][y] > thresh)
					scores[x][y] = 1.0;
				else
					scores[x][y] = scores[x][y] / thresh;
				add_edge(x,y,temp_g);
			}
		}
	}
	wg = createWeightedGraph(temp_g);
	distance = createAllPairsShortestPaths(wg);

	}

	return scores;
}

MatrixScore AdamicAdar(Graph g){
	Matrix distance = createAllPairsShortestPaths(createWeightedGraph(g));
	MatrixScore scores = initializeScore(g);

	for(int x=0; x<num_vertices(g); x++){
		#pragma omp parallel for
		for(int y=x+1; y<num_vertices(g); y++){
			if(distance[x][y] == 2){
				for(int i=0; i<num_vertices(g); i++){
					if(x!=i && y!=i && out_degree(i,g) > 1){
						int d = distance[x][i] + distance[y][i];
						if(d == distance[x][y])
							scores[x][y] += (double)1/log(out_degree(i,g));
					}
				}
			}
		}
	}

	return scores;
}

void normalize(MatrixScore scores, Graph g){
	//mean
	int count=0;
	double avg=0.0;
	for(int x=0; x<num_vertices(g); x++){
		for(int y=x+1; y<num_vertices(g); y++){
			if(edge(x,y,g).second == 0){
				avg += scores[x][y];
				count++;
			}
		}
	}
	avg = avg/count;

	double dev=0.0;
	for(int x=0; x<num_vertices(g); x++){
		for(int y=x+1; y<num_vertices(g); y++){
			if(edge(x,y,g).second == 0){
				dev += pow(scores[x][y] - avg, 2);
			}
		}
	}
	dev = sqrt(dev/count);
	std::cout << avg << " " << dev << std::endl;
}

MatrixScore MultiTimeAdamicAdar(Graph g, Matrix distance, Matrix new_matrix){
	MatrixScore scores(num_vertices(g));
	for(int i=0; i<num_vertices(g); i++){
		std::vector<double> score(num_vertices(g), 0.0);
		scores[i] = score;
	}

	std::vector<int> is_new(num_vertices(g), 0);
	for(int i=0; i<num_vertices(g); i++){
		for(int j=0; j<num_vertices(g); j++){
			if(new_matrix[i][j] == 1){
				is_new[i] = 1;
				break;
			}
		}
	}

	for(int s=0; s<num_vertices(g); s++){
		for(int t=s+1; t<num_vertices(g); t++){
			if(distance[s][t] == 2){
				for(int i=0; i<num_vertices(g); i++){
					if(distance[s][i] == 1 && distance[t][i] == 1){
						float si = (new_matrix[s][i] == 1)?1.5:1;
						float ti = (new_matrix[t][i] == 1)?1.5:1;
						scores[s][t] += (si*ti)/log10(out_degree(i,g));
					}
				}
			}
		}
	}

	return scores;
}

float AvgDegreeAA(Graph g, Edges new_edges, Matrix distance){
	int count=0; float temp_avg=0.0;
	for(int i=0; i<new_edges.size(); i++){
		int s = new_edges[i].first;
		int t = new_edges[i].second;

		if(distance[s][t] == 2){
			for(int j=0; j<num_vertices(g); j++){
				if(distance[s][j] + distance[j][t] == distance[s][t]){
					temp_avg += out_degree(j,g);
					count++;
				}
			}
		}
	}

	return temp_avg/count;
}

Matrix createMatrix(Graph g){
	Matrix matrix(num_vertices(g));

	#pragma omp parallel for
	for(int i=0; i<num_vertices(g); i++){
		std::vector<int> list(num_vertices(g));
		for(int j=0; j<num_vertices(g); j++){
			if(edge(i,j,g).second == 1)
				list[j] = 1;
			else
				list[j] = 0;
		}
		matrix[i] = list;
	}

	return matrix;
}

Matrix multiplyMatrix(Matrix A, Matrix B){
	Matrix matrix(A.size());

	#pragma omp parallel for
	for(int i=0; i<A.size(); i++){
		std::vector<int> list(A.size(), 0);
		for(int j=0; j<A.size(); j++){
			for(int k=0; k<A.size(); k++){
				list[j] += A[i][k] * B[k][j];
			}
		}
		matrix[i] = list;
	}

	return matrix;
}

std::vector<double> VertexBetweennessCentrality(Graph g, std::vector<double> &v_centrality_vec, std::vector<double> &e_centrality_vec){
	StdEdgeIndexMap my_e_index;
	EdgeIndexMap e_index(my_e_index);
	int i=0;
	BGL_FORALL_EDGES(edge, g, Graph){
		my_e_index.insert(std::pair<EdgeDescriptor, int>(edge,i));
		++i;
	}

	//std::vector<double>e_centrality_vec(num_edges(g), 0.0);
	iterator_property_map<std::vector<double>::iterator, EdgeIndexMap> e_centrality_map(e_centrality_vec.begin(), e_index);

	VertexIndexMap v_index = get(vertex_index, g);
	//std::vector<double> v_centrality_vec(num_vertices(g), 0.0);
	iterator_property_map<std::vector<double>::iterator, VertexIndexMap> v_centrality_map(v_centrality_vec.begin(), v_index);
	brandes_betweenness_centrality(g, v_centrality_map, e_centrality_map);
	//relative_betweenness_centrality(g, v_centrality_map);

	return v_centrality_vec;
}

MatrixScore BCBasedLP(Graph g, Matrix distance, std::vector<double> v_centrality_vec, float beta){
	MatrixScore score = initializeScore(g);
	//Matrix distance = createAllPairsShortestPaths(createWeightedGraph(g));

	#pragma omp parallel for
	for(int x=0; x<num_vertices(g); x++){
		for(int y=x+1; y<num_vertices(g); y++){
		/*
			if(distance[x][y] == 3){

			std::vector<float> z;
			double avg=0.0;
			for(int i=0; i<num_vertices(g); i++){
				if((distance[x][i] + distance[i][y] == distance[x][y]) && x!=i && y!=i){
					z.push_back(v_centrality_vec[i]);
					avg += v_centrality_vec[i];
				}
			}

			avg = avg/z.size();

			for(int i=0; i<z.size(); i++){
				score[x][y] += pow(avg - z[i],2);
			}

			if(score[x][y] != 0)
				score[x][y] = sqrt(score[x][y]);
			else{
				double tavg = (v_centrality_vec[x] + v_centrality_vec[y])/2;
				tavg = pow(v_centrality_vec[x]-tavg,2) + pow(v_centrality_vec[y]-tavg,2);
				score[x][y] = sqrt(tavg);
			}

			}
		*/

			for(int i=0; i<num_vertices(g); i++){
				if((distance[x][i] + distance[i][y] == distance[x][y])
				&& x!=i && y!=i){
					//score[x][y] += 1/v_centrality_vec[i];
					//score[x][y] += 1/pow(v_centrality_vec[i],2);
					//score[x][y] += 1/log(v_centrality_vec[i]);
					//score[x][y] += 1/log(1+v_centrality_vec[i]);
					score[x][y] += 1/(pow(log(1+v_centrality_vec[i]),2));
				}
			}

			if(distance[x][y] >= 2)
				score[x][y] *= pow(beta, distance[x][y]-2);
		}
	}

	return score;
}

float BCBasedLPst(Graph g, Matrix distance, std::vector<double> v_centrality_vec, float beta, int s, int t){

	float score=0.0;
	int count=0;
	std::vector<float> z;
	double avg=0.0;
	for(int i=0; i<num_vertices(g); i++){
		if((distance[s][i] + distance[i][t] == distance[s][t]) && s!=i && t!=i){
			//score += v_centrality_vec[i];
			z.push_back(v_centrality_vec[i]);
			avg += v_centrality_vec[i];
			//count++;
		}
	}

	avg = avg/z.size();

	return avg;

	/*
	for(int i=0; i<z.size(); i++){
		score += pow(avg - z[i],2);
	}

	score = sqrt(score/z.size());

	return score;
	*/
}

/*
	Matrix distance = createAllPairsShortestPaths(createWeightedGraph(g));

	MatrixScore score = initializeScore(g);

	MatrixList ml;
	ml.push_back(createMatrix(g));

	for(int i=0; i<num_vertices(g); i++){
		for(int j=i+1; j<num_vertices(g); j++){
			if(distance[i][j] > 1 && distance[i][j] < num_vertices(g)){
				while(distance[i][j] > ml.size())
					ml.push_back(multiplyMatrix(ml[0], ml[ml.size()-1]));

				int num = ml[distance[i][j]-1][i][j];

				for(int k=0; k<num_vertices(g); k++){
					if(distance[i][k] + distance[k][j] == distance[i][j] && k!=i && k!=j){
						score[i][j] += (float)(ml[distance[i][k]-1][i][k] * ml[distance[k][j]-1][k][j])/num;
					}
				}
			}
		}
	}

	return score;
*/

Graph ClusterEdgeCentrality(Graph g, double max_centrality){
	Graph cg = g;
	StdEdgeIndexMap my_e_index;
	EdgeIndexMap e_index(my_e_index);
	int i=0;
	BGL_FORALL_EDGES(edge, cg, Graph){
		my_e_index.insert(std::pair<EdgeDescriptor, int>(edge, i));
		++i;
	}

	std::vector<double>e_centrality_vec(num_edges(cg), 0.0);
	iterator_property_map<std::vector<double>::iterator, EdgeIndexMap> e_centrality_map(e_centrality_vec.begin(), e_index);

	bc_clustering_threshold<double>terminate(max_centrality, cg, false);
	betweenness_centrality_clustering(cg, terminate, e_centrality_map);

	for(int i=0; i<num_vertices(cg); i++){
		for(int j=0; j<num_vertices(cg); j++){
			if(edge(i, j, cg).second == 1){
			}
		}
	}

	return cg;
}

Graph EdgesMST(Graph g, std::vector<double> ebc){
	MatrixScore weights = initializeScore(g);

	int k=0;
	BGL_FORALL_EDGES(e, g, Graph){
		int s = source(e, g);
		int t = target(e, g);
		if(s < t)
			weights[s][t] += ebc[k];
		else if(t < s)
			weights[t][s] += ebc[k];
		k++;
	}

	for(int i=0; i<num_vertices(g); i++){
		for(int j=i+1; j<num_vertices(g); j++){
			weights[i][j] = weights[i][j]/2;
			weights[j][i] = weights[i][j];
		}
	}

	UndirectedGraph ug(num_vertices(g));

	property_map<UndirectedGraph, edge_weight_t>::type weightmap = get(edge_weight,ug);

	BGL_FORALL_EDGES(e, g, Graph){
		int s = source(e, g);
		int t = target(e, g);
		if(edge(s,t,ug).second != 1){
			UndirectedEdgeDescriptor ue;
			bool inserted;
			tie(ue, inserted) = add_edge(s,t,ug);
			weightmap[ue] = (size_t)weights[s][t];
		}
	}

	std::vector<UndirectedEdgeDescriptor> spanning_tree;
	kruskal_minimum_spanning_tree(ug, std::back_inserter(spanning_tree));

	Graph tempg(num_vertices(g));
	for(int i=0; i<spanning_tree.size(); i++){
		int s = source(spanning_tree[i], ug);
		int t = target(spanning_tree[i], ug);

		add_edge(s,t,tempg);
		add_edge(t,s,tempg);
	}

	return tempg;
}

MatrixScore EdgeCentrality(Graph g, std::vector<double> ebc){
	MatrixScore score = initializeScore(g);

	int k=0;
	BGL_FORALL_EDGES(e, g, Graph){
		int s = source(e, g);
		int t = target(e, g);
		score[s][t] = ebc[k];
		score[t][s] = ebc[k];
		k++;
	}

	return score;
}

void NormalizePr(MatrixScore &scores, Matrix distance){
	//mean
	int count=0;
	double avg=0.0;
	for(int x=0; x<scores.size(); x++){
		for(int y=x+1; y<scores[x].size(); y++){
			if(distance[x][y] == 2){
				avg += scores[x][y];
				count++;
			}
		}
	}

	avg = avg/count;

	double dev=0.0;
	for(int x=0; x<scores.size(); x++){
		for(int y=x+1; y<scores[x].size(); y++){
			if(distance[x][y] == 2){
				dev += pow(scores[x][y] - avg, 2);
			}
		}
	}
	dev = sqrt(dev/count);

	count =0;

	for(int x=0; x<scores.size(); x++){
		for(int y=x+1; y<scores[x].size(); y++){
			if(distance[x][y] == 2){
				scores[x][y] = (scores[x][y] - avg)/dev;
				scores[y][x] = scores[x][y];
				if(scores[x][y] >= 1.0)
					count++;
			}
		}
	}
	std::cout << "number of scores above 1: " << count << std::endl;
}


#include <Eigen/Dense>
#include <iostream>

class Kalman {
    public:

        Kalman(){
            is_init = false;
        }

        ~Kalman(){
            ;
        }

        bool isInit(){
            return is_init;
        }

        // 初始化状态向量
        void init(Eigen::VectorXd x_in){
            x_ = x_in;
            is_init = true;
        }

        // 状态转移矩阵
        void setF(Eigen::MatrixXd F_in){
            F_ = F_in;
        }

        // 系统的不确定度
        void setP(Eigen::MatrixXd P_in){
            P_ = P_in;
        }

        // 过程噪声 （一般为单位矩阵）
        void setQ(Eigen::MatrixXd Q_in){
            Q_ = Q_in;
        }

        // 测量矩阵
        void setH(Eigen::MatrixXd H_in){
            H_ = H_in;
        }

         // 测量噪声矩阵（一般由传感器厂家提供）
        void setR(Eigen::MatrixXd R_in){
            R_ = R_in;
        }

        // 计算预测值
        void predict(){
            x_ = F_ * x_;
            // std::cout << x_ << std::endl;
            //printf("predict ok, x_ shape is %d %d\n", x_.rows(), x_.cols());
            P_ = F_*P_*F_.transpose();
            //printf("predict ok, P_ shape is %d %d\n", P_.rows(), P_.cols());
        }

        // 观测
        void measurement_update(const Eigen::VectorXd &z){
            // 观测值z与预测值x的差值y
            Eigen::VectorXd y = z - H_ * x_;
            //printf("update ok, y_ shape is %d %d\n", y.rows(), y.cols());
            // 求解卡尔曼增益k，差值y的权重
            Eigen::MatrixXd S_ = H_ * P_ * H_.transpose() + R_;
            //printf("update ok, S_ shape is %d %d\n", S_.rows(), S_.cols());
            Eigen::MatrixXd K_ = P_ * H_.transpose() * S_.inverse();
            //printf("update ok, K_ shape is %d %d\n", K_.rows(), K_.cols());
            // 更新状态向量，考虑了测量值，预测值，整个系统的噪声
            x_ = x_ + K_ * y;
            int size = x_.size();
            Eigen::MatrixXd I = Eigen::MatrixXd::Identity(size, size);
            P_ = (I - K_ * H_) * P_;
        }

        Eigen::VectorXd getX(){
            return x_;
        }

    private:
        bool is_init;
        Eigen::VectorXd x_;
        Eigen::MatrixXd F_, P_, Q_, H_, R_;
};

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

    Kalman kalman;

    int iter=0;
    double mx = 1.0, my = 1.0, dt=0.1;
    while (iter++ < 1070)
    {

        if (!kalman.isInit())
        {
            Eigen::VectorXd x_in(4, 1);
            x_in << mx, my, 0.0, 0.0;
            kalman.init(x_in);

            Eigen::MatrixXd P_in(4, 4);
            P_in << 1.0, 0.0, 0.0, 0.0,
                    0.0, 1.0, 0.0, 0.0,
                    0.0, 0.0, 100.0, 0.0,
                    0.0, 0.0, 0.0, 100.0;
            kalman.setP(P_in);

            Eigen::MatrixXd Q_in(4, 4);
            Q_in << 1.0, 0.0, 0.0, 0.0,
                    0.0, 1.0, 0.0, 0.0,
                    0.0, 0.0, 1.0, 0.0,
                    0.0, 0.0, 0.0, 1.0;

            kalman.setQ(Q_in);

            Eigen::MatrixXd H_in(2, 4);
            H_in << 1.0, 0.0, 0.0, 0.0,
                    0.0, 1.0, 0.0, 0.0;

            kalman.setH(H_in);

            Eigen::MatrixXd R_in(2, 2);
            R_in << 0.0225, 0.0,
                    0.0, 0.0225;

            kalman.setR(R_in);
            continue;
        }

        Eigen::MatrixXd F_in(4, 4);
        F_in << 1.0, 0.0, dt, 0.0,
                0.0, 1.0, 0.0, dt,
                0.0, 0.0, 1.0, 0.0,
                0.0, 0.0, 0.0, 1.0;

        kalman.setF(F_in);
        kalman.predict();

        Eigen::VectorXd z(2, 1);
        z << mx, my;
        kalman.measurement_update(z);

        Eigen::VectorXd x_out = kalman.getX();
        std::cout << "x: " << x_out(0) << ", y:" << x_out(1) << std::endl;
        // std::cout << "vx: " << x_out(2) << ", vy:" << x_out(3) << std::endl;

        std::cout << "mx: " << mx << ", my:" << my << std::endl;

        // dt = 0.1;
        mx += 0.3;
        my += 0.4;

        if(iter == 998) mx = 1, my = 1;
        if(iter == 999) mx = 10, my = 10;
    }

    return 0;
}


#include <fmt/core.h>

#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <cassert>
#include <iostream>
#include <vector>

#include "NumpySaver.hpp"
#include "Timer.hpp"
#include "YAMLUtils.hpp"
#include "bsplines.hpp"

using namespace Eigen;
using std::cout, std::endl, std::cerr;

void test_bsplines(double x = .1, Index order = 3, Index nknots = 11) {
  std::vector<double> std_knots(nknots);
  Map<ArrayXd> knots(std_knots.data(), std_knots.size());
  knots = ArrayXd::LinSpaced(nknots, 0, 1);

  auto [values, index] = bsplines::bsplines(x, std_knots, order);

  Map<VectorXd> values_eigen(values.data(), values.size());
  auto sum = values_eigen.sum();
  if (std::abs(sum - 1) > 1e-15) {
    cerr << "The knots are: " << knots.transpose() << endl;
    cerr << "The spline value at " << x << " is: " << values_eigen.transpose()
         << endl;
    cerr << "Summing all spline values (should be 1): " << sum << endl;
  }
}

SparseMatrix<double> build_matrix(const std::vector<double>& knots,
                                  std::size_t order = 3) {
  auto n = knots.size();

  SparseMatrix<double> matrix(n + 1, n + order - 2);
  matrix.reserve(order * (n + 1) - 2);

  auto [deriv_0, _index] = bsplines::ndx_bsplines(knots[0], knots, order, 2);
  for (int col = 0; col < int(order) - 1; col++)
    matrix.insert(0, col) = deriv_0[col + 1];

  for (int row = 1; row < int(n); row++) {
    auto [deriv, index] = bsplines::ndx_bsplines(knots[row], knots, order, 2);
    for (int col = row - 1; col < row - 1 + int(order); col++)
      matrix.insert(row, col) = deriv[col - row + 2];
  }

  auto [deriv_last, __index] =
      bsplines::ndx_bsplines(knots[n - 1], knots, order, 1);
  matrix.insert(n, n + order - 3) = deriv_last[order];
  matrix.insert(n, n + order - 4) = deriv_last[order - 1];

  return matrix;
}

double rho_uniform_sphere(double r, double R, double q) {
  if (r <= R) return q / (4. * M_PI / 3. * R * R * R);

  return 0;
}

double rho_uniform_shell(double r, double R1, double R2, double q) {
  if (r < R1) return 0;
  if (r <= R2) return q / (4. * M_PI / 3. * (R2 * R2 * R2 - R1 * R1 * R1));

  return 0;
}

double rho_hydrogen_ground_state(double r, double e, double r_bohr = 1) {
  return e / M_PI / (r_bohr * r_bohr * r_bohr) * std::exp(-2 * r / r_bohr);
}

VectorXd build_rhs(const std::vector<double>& knots,
                   std::function<double(double)>& rho) {
  auto n = knots.size();
  VectorXd rhs(n + 1);
  for (std::size_t i = 0; i < n; i++) {
    rhs[i] = -knots[i] * 4 * M_PI * rho(knots[i]);
  }

  rhs[n] = 0;

  return rhs;
}

double evaluate_spline(const double& x, const std::vector<double>& knots,
                       const VectorXd& weights, std::size_t order = 3) {
  auto n = knots.size();
  assert(int(n + order) - 1 == weights.size());

  auto [values, index] = bsplines::bsplines(x, knots, order);

  double sum = 0;
  for (std::size_t i = 0; i <= order; i++)
    sum += values[i] * weights[index + i];

  return sum;
}

VectorXd evaluate_spline(const VectorXd& x, const std::vector<double>& knots,
                         const VectorXd& weights, std::size_t order = 3) {
  auto n = x.size();
  VectorXd ret(n);
  for (Index i = 0; i < n; i++) {
    ret[i] = evaluate_spline(x[i], knots, weights, order);
  }
  return ret;
}

void solve_problem(YAML::Node node) {
  using namespace std::placeholders;

  auto name = node["name"].as<std::string>();
  auto R = node["R"].as<double>();
  auto R1 = node["R1"].as<double>();
  auto R2 = node["R2"].as<double>();
  auto q = node["q"].as<double>();
  auto e = node["e"].as<double>();
  auto r_bohr = node["r_bohr"].as<double>();

  YAML::Node knot_node = node["knots"];
  auto knots = get_yaml_values<double>(knot_node);

  auto matrix = build_matrix(knots);

  std::vector<std::tuple<const char*, std::function<double(double)>>> rhos = {
      {"uniform_sphere", std::bind(rho_uniform_sphere, _1, R, q)},
      {"uniform_shell", std::bind(rho_uniform_shell, _1, R1, R2, q)},
      {"hydrogen", std::bind(rho_hydrogen_ground_state, _1, e, r_bohr)},
  };

  if (knots.size() < 30) {
    cout << "Knots = ";
    for (std::size_t i = 0; i < knots.size(); i++) cout << knots[i] << ", ";
    cout << "\n";
  }

  // first do the (sparse) LU decomposition
  Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>
      solver;
  solver.analyzePattern(matrix);
  solver.factorize(matrix);

  VectorXd x = VectorXd::LinSpaced(1000, knots[0], knots[knots.size() - 1]);

  for (std::size_t i = 0; i < rhos.size(); i++) {
    auto rho_name = std::get<0>(rhos[i]);
    auto rho = std::get<1>(rhos[i]);

    cout << "\nProblem = " << rho_name << "\n";

    auto rhs = build_rhs(knots, rho);
    auto solution = solver.solve(rhs);
    if (knots.size() < 30) {
      cout << "rhs vector: " << rhs.transpose() << "\n";
      cout << "solution vector: " << solution.transpose() << "\n";
    }

    VectorXd weights(solution.size() + 1);
    weights << 0, solution;

    VectorXd sol = evaluate_spline(x, knots, weights);

    NumpySaver(fmt::format("build/output/solution_{}_{}.npy", name, rho_name))
        << x << sol;
  }
  cout << "\n";
}

void plot_bsplines(std::size_t order = 3) {
  std::vector<double> std_knots(11);
  Map<ArrayXd> knots(std_knots.data(), std_knots.size());
  knots = ArrayXd::LinSpaced(11, 0, 1);
  VectorXd xs = VectorXd::LinSpaced(1000, knots[0], knots[knots.size() - 1]);
  MatrixXd spline(xs.size(), order + 1);
  MatrixXd dspline(xs.size(), order + 1);
  MatrixXd ddspline(xs.size(), order + 1);

  for (Index i = 0; i < xs.size(); i++) {
    auto [x_spline, index_0] = bsplines::bsplines(xs[i], std_knots, order);
    auto [x_dspline, index_1] =
        bsplines::ndx_bsplines(xs[i], std_knots, order, 1);
    auto [x_ddspline, index_2] =
        bsplines::ndx_bsplines(xs[i], std_knots, order, 2);
    for (std::size_t j = 0; j < order + 1; j++) {
      spline(i, j) = x_spline[j];
      dspline(i, j) = x_dspline[j];
      ddspline(i, j) = x_ddspline[j];
    }
  }
  NumpySaver saver("build/output/plot_splines.npy");
  saver << xs;
  for (std::size_t j = 0; j < order + 1; j++) saver << spline.col(j);
  for (std::size_t j = 0; j < order + 1; j++) saver << dspline.col(j);
  for (std::size_t j = 0; j < order + 1; j++) saver << ddspline.col(j);
}

int main() {
  cout << "Testing if all spline values at each point sum to 1, no error means "
          "it worked."
       << endl;
  for (double x = 0; x <= 1; x += .001) {
    test_bsplines(x, 3, 11);
  }

  std::vector<double> std_knots(11);
  Map<ArrayXd> knots(std_knots.data(), std_knots.size());
  knots = ArrayXd::LinSpaced(11, 0, 1);

  cout << "Testing Matrix creation..." << endl;
  auto matrix = build_matrix(std_knots);
  cout << "Matrix size is " << matrix.rows() << "x" << matrix.cols() << endl;
  cout << matrix << endl;

  YAML::Node config = YAML::LoadFile("config.yaml");

  for (std::size_t i = 0; i < config.size(); i++) {
    cout << "\n\n*****************************************\n"
         << fmt::format("Running simulation {}/{}", i + 1, config.size())
         << endl;
    solve_problem(config[i]);
  }

  plot_bsplines();
  return 0;
}


#define CATCH_CONFIG_MAIN
#include "catch.hpp"

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include <unsupported/Eigen/KroneckerProduct>

#include <Spectra/MatOp/SparseSymMatProd.h>
#include <Spectra/SymEigsSolver.h>

#include <iostream>
#include <cassert>
#include <random>
#include <algorithm>

#include "edlib/EDP/LocalHamiltonian.hpp"
#include "edlib/EDP/ConstructSparseMat.hpp"

Eigen::SparseMatrix<double> getSX()
{
	Eigen::SparseMatrix<double> res(2,2);
	res.insert(0,1) = 1.0;
	res.insert(1,0) = 1.0;
	res.makeCompressed();
	return res;
}

Eigen::SparseMatrix<std::complex<double> > getSY()
{
	Eigen::SparseMatrix<std::complex<double> > res(2,2);
	constexpr std::complex<double> I(0., 1.);
	res.insert(0,1) = -I;
	res.insert(1,0) = I;
	res.makeCompressed();
	return res;
}

Eigen::SparseMatrix<double> getSZ()
{
	Eigen::SparseMatrix<double> res(2,2);
	res.insert(0,0) = 1.0;
	res.insert(1,1) = -1.0;
	res.makeCompressed();
	return res;
}

template<typename T>
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
twoQubitOp(int N, int pos1, int pos2,
		const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& v1,
		const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& v2)
{
	using namespace Eigen;
	const uint32_t dim = (1<<N);

	assert(pos1 < pos2);

	Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> res(1,1);
	res(0,0) = 1.0;

	for(int i = 0; i < pos1; i++)
	{
		res = Eigen::kroneckerProduct(MatrixXd::Identity(2,2), res).eval();
	}

	res = Eigen::kroneckerProduct(v1, res).eval();
	for(int i = pos1+1; i < pos2; i++)
	{
		res = Eigen::kroneckerProduct(MatrixXd::Identity(2,2), res).eval();
	}
	res = Eigen::kroneckerProduct(v2, res).eval();
	for(int i = pos2+1; i < N; i++)
	{
		res = Eigen::kroneckerProduct(MatrixXd::Identity(2,2), res).eval();
	}
	return res;
}

template<typename T>
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
singleQubitOp(int N, int pos,
		const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& v)
{
	using namespace Eigen;
	const uint32_t dim = (1<<N);

	using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> ;
	MatrixT res(1,1);
	res(0,0) = 1.0;

	for(int i = 0; i < pos; i++)
	{
		res = Eigen::kroneckerProduct(MatrixT::Identity(2,2), res).eval();
	}

	res = Eigen::kroneckerProduct(v, res).eval();
	for(int i = pos+1; i < N; i++)
	{
		res = Eigen::kroneckerProduct(MatrixT::Identity(2,2), res).eval();
	}
	return res;
}

Eigen::SparseMatrix<double> getSXXYY()
{
	Eigen::SparseMatrix<double> res(4,4);
	res.insert(1,2) = 2.0;
	res.insert(2,1) = 2.0;
	res.makeCompressed();
	return res;
}

Eigen::SparseMatrix<double> getSXX()
{
	Eigen::SparseMatrix<double> res(4,4);
	res.insert(0,3) = 1.0;
	res.insert(1,2) = 1.0;
	res.insert(2,1) = 1.0;
	res.insert(3,0) = 1.0;
	res.makeCompressed();
	return res;
}

Eigen::SparseMatrix<double> getSYY()
{
	Eigen::SparseMatrix<double> res(4,4);
	res.insert(0,3) = -1.0;
	res.insert(1,2) = 1.0;
	res.insert(2,1) = 1.0;
	res.insert(3,0) = -1.0;
	res.makeCompressed();
	return res;
}

Eigen::SparseMatrix<double> getSZZ()
{
	Eigen::SparseMatrix<double> res(4,4);
	res.insert(0,0) = 1.0;
	res.insert(1,1) = -1.0;
	res.insert(2,2) = -1.0;
	res.insert(3,3) = 1.0;
	res.makeCompressed();
	return res;
}

class GraphGenerator
{
private:
	int numVertices_;
	std::vector<std::pair<int,int> > allEdges_;
	std::vector<int> allVertices_;

public:
	GraphGenerator(int numVertices)
		: numVertices_{numVertices}
	{
		for(int i = 0; i < numVertices-1; ++i)
		{
			for(int j = i+1; j < numVertices; ++j)
			{
				allEdges_.emplace_back(i,j);
			}
		}
		for(int i = 0; i < numVertices_; ++i)
		{
			allVertices_.emplace_back(i);
		}
	}

	std::vector<std::pair<int,int> > createRandomGraph(int numEdges)
	{
		std::vector<std::pair<int,int> > edges = allEdges_;
		std::random_shuffle(edges.begin(), edges.end());
		edges.resize(numEdges);
		return edges;
	}

	std::vector<int> createRandomVertexSet(int n)
	{
		std::vector<int> v = allVertices_;
		std::random_shuffle(v.begin(), v.end());
		v.resize(n);
		return v;
	}
};

constexpr int N = 10;
TEST_CASE("Test single qubit operators", "[LocalHamSingle]") {
	std::random_device rd;
	std::default_random_engine re{rd()};
	std::uniform_int_distribution<> uid(0, N-1);
	std::uniform_real_distribution<> urd;

	using cx_double = std::complex<double>;

	SECTION("Test contructing pauli X") {
		edp::LocalHamiltonian<double> lh(N,2);
		for(int n = 0; n < 100; ++n)
		{
			lh.clearTerms();
			int idx = uid(re);
			double val = urd(re);
			lh.addOneSiteTerm(idx, val*getSX());
			auto mat1 = Eigen::MatrixXd(edp::constructSparseMat<double>(1<<N, lh));
			auto mat2 = singleQubitOp<double>(N, idx, val*getSX()) ;
			REQUIRE((mat1-mat2).squaredNorm() < 1e-8);
		}
	}
	SECTION("Test contructing pauli Z") {
		edp::LocalHamiltonian<double> lh(N,2);
		for(int n = 0; n < 100; ++n)
		{
			lh.clearTerms();
			int idx = uid(re);
			double val = urd(re);
			lh.addOneSiteTerm(idx, val*getSZ());
			auto mat1 = Eigen::MatrixXd(edp::constructSparseMat<double>(1<<N, lh));
			auto mat2 = singleQubitOp<double>(N, idx, val*getSZ()) ;
			REQUIRE((mat1-mat2).squaredNorm() < 1e-8);
		}
	}
	SECTION("Test contructing pauli Y") {
		edp::LocalHamiltonian<cx_double> lh(N,2);
		for(int n = 0; n < 100; ++n)
		{
			lh.clearTerms();
			int idx = uid(re);
			double val = urd(re);
			lh.addOneSiteTerm(idx, val*getSY());
			auto mat1 = Eigen::MatrixXcd(edp::constructSparseMat<cx_double>(1<<N, lh));
			auto mat2 = singleQubitOp<cx_double>(N, idx, val*getSY()) ;
			REQUIRE((mat1-mat2).squaredNorm() < 1e-8);
		}
	}
}

TEST_CASE("Test 2-local Hamiltonians", "[LocalHam2loc]") {
	std::random_device rd;
	std::default_random_engine re{rd()};
	std::uniform_int_distribution<> uid(0, N-1);
	std::uniform_int_distribution<> numEdgesRd(1, N*(N-1)/2);
	std::uniform_real_distribution<> urd;

	using cx_double = std::complex<double>;

	GraphGenerator ggen{N};

	SECTION("Random XYZ Hamiltonian") {
		edp::LocalHamiltonian<double> lh(N,2);

		for(int n = 0; n < 10; ++n)
		{
			lh.clearTerms();
			int numEdges = numEdgesRd(re);
			auto edges = ggen.createRandomGraph(numEdges);

			auto matEx = Eigen::MatrixXd(1<<N, 1<<N);
			matEx.setZero();

			for(const auto& edge: edges)
			{
				double v1 = urd(re);
				double v2 = urd(re);
				double v3 = urd(re);
				lh.addTwoSiteTerm(edge, v1*getSXX());
				lh.addTwoSiteTerm(edge, v2*getSYY());
				lh.addTwoSiteTerm(edge, v3*getSZZ());

				matEx += v1*twoQubitOp<double>(N, edge.first, edge.second, getSX(), getSX());
				auto matYY = twoQubitOp<cx_double>(N, edge.first, edge.second, getSY(), getSY());
				matEx += v2*matYY.real();
				matEx += v3*twoQubitOp<double>(N, edge.first, edge.second, getSZ(), getSZ());
			}
			auto mat = Eigen::MatrixXd(edp::constructSparseMat<double>(1<<N, lh));
			REQUIRE((mat-matEx).squaredNorm() < 1e-8);
		}
	}
	SECTION("Random Field Ising Hamiltonian") {
		edp::LocalHamiltonian<double> lh(N,2);

		for(int n = 0; n < 20; ++n)
		{
			lh.clearTerms();
			int numEdges = numEdgesRd(re);
			auto edges = ggen.createRandomGraph(numEdges);

			auto matEx = Eigen::MatrixXd(1<<N, 1<<N);
			matEx.setZero();

			for(const auto& edge: edges)
			{
				double v = urd(re);
				lh.addTwoSiteTerm(edge, v*getSZZ());

				matEx += v*twoQubitOp<double>(N, edge.first, edge.second, getSZ(), getSZ());
			}

			auto numFields = uid(re);
			auto vs = ggen.createRandomVertexSet(numFields);

			for(auto v: vs)
			{
				double h1 = urd(re);
				double h3 = urd(re);

				lh.addOneSiteTerm(v, h1*getSX());
				lh.addOneSiteTerm(v, h3*getSZ());

				matEx += h1*singleQubitOp<double>(N, v, getSX());
				matEx += h3*singleQubitOp<double>(N, v, getSZ());
			}

			auto mat = Eigen::MatrixXd(edp::constructSparseMat<double>(1<<N, lh));
			REQUIRE((mat-matEx).squaredNorm() < 1e-8);
		}
	}
}


#include <iostream>
#include <functional>
#include <numeric>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <map>
#include <Eigen\dense>

#include "markov.h"
#include "TransitionMatrix.h"

int main() {

	SetTransitionMatrix();
	setGameMatrix();

	//Output Vector
	v.setZero();
	v(0) = 1.0;

	// Print Results to File
	std::ofstream myfile;
	std::ofstream myfile2;
	myfile.open("markov_results.txt");
	myfile2.open("GameMatrix_results.txt");

	// TODO add Markov vector - Matrix multiplication
	for (int i = 0; i < 101; i++) {

	v = v.transpose() * TransitionMatrix;
	std::cout << v << std::endl; //this is just a sample, becareful how you print to file so you can mine useful stats
	myfile << v << std::endl;
	myfile << "*************" << std::endl;
	}

	v.setZero();
	v(0) = 1.0;

	for (int i = 0; i < 100; i++) {
		v = v.transpose() * GameMatrix;
		for (int j = 0; j < size - 1; j++)
		myfile2 << v(j) << std::endl;
		std::cout << v << std::endl; //this is just a sample, becareful how you print to file so you can mine useful stats
		myfile2 << "*************" << std::endl;
	}

	myfile.close();
	myfile2.close();

  return 1;
}


///
/// @file
/// @copyright Copyright (C) Fredrik Orderud
///
/// @brief Copied from: http://www.crystalclearsoftware.com/cgi-bin/boost_wiki/wiki.pl?LU_Matrix_Inversion
///
#ifndef INVERT_MATRIX_HPP_
#define INVERT_MATRIX_HPP_

// REMEMBER to update "lu.hpp" header includes from boost-CVS
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

/**
 * Matrix inversion routine.
 * Uses lu_factorize and lu_substitute in uBLAS to invert a matrix
 */
template<class T>
bool InvertMatrix(const boost::numeric::ublas::matrix<T>& input, boost::numeric::ublas::matrix<T>& inverse)
{
    using namespace boost::numeric::ublas;
    typedef permutation_matrix<std::size_t> pmatrix;
    // create a working copy of the input
    matrix<T> A(input);
    // create a permutation matrix for the LU-factorization
    pmatrix pm(A.size1());
    // perform LU-factorization
    int res = lu_factorize(A,pm);
    if( res != 0 ) return false;
    // create identity matrix of "inverse"
    inverse.assign(boost::numeric::ublas::identity_matrix<T>(A.size1()));
    // backsubstitute to get the inverse
    lu_substitute(A, pm, inverse);
    return true;
}

#endif  // INVERT_MATRIX_HPP_


//  (C) Copyright Nick Thompson 2021.
//  Use, modification and distribution are subject to 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)
#ifndef BOOST_MATH_TOOLS_QUARTIC_ROOTS_HPP
#define BOOST_MATH_TOOLS_QUARTIC_ROOTS_HPP
#include <array>
#include <cmath>
#include <boost/math/tools/cubic_roots.hpp>

namespace boost::math::tools {

namespace detail {

// Make sure the nans are always at the back of the array:
template<typename Real>
bool comparator(Real r1, Real r2) {
   using std::isnan;
   if (isnan(r2)) { return true; }
   return r1 < r2;
}

template<typename Real>
std::array<Real, 4> polish_and_sort(Real a, Real b, Real c, Real d, Real e, std::array<Real, 4>& roots) {
    // Polish the roots with a Halley iterate.
    using std::fma;
    using std::abs;
    for (auto &r : roots) {
        Real df = fma(4*a, r, 3*b);
        df = fma(df, r, 2*c);
        df = fma(df, r, d);
        Real d2f = fma(12*a, r, 6*b);
        d2f = fma(d2f, r, 2*c);
        Real f = fma(a, r, b);
        f = fma(f,r,c);
        f = fma(f,r,d);
        f = fma(f,r,e);
        Real denom = 2*df*df - f*d2f;
        if (abs(denom) > (std::numeric_limits<Real>::min)())
        {
            r -= 2*f*df/denom;
        }
    }
    std::sort(roots.begin(), roots.end(), detail::comparator<Real>);
    return roots;
}

}
// Solves ax^4 + bx^3 + cx^2 + dx + e = 0.
// Only returns the real roots, as these are the only roots of interest in ray intersection problems.
// Follows Graphics Gems V: https://github.com/erich666/GraphicsGems/blob/master/gems/Roots3And4.c
template<typename Real>
std::array<Real, 4> quartic_roots(Real a, Real b, Real c, Real d, Real e) {
    using std::abs;
    using std::sqrt;
    auto nan = std::numeric_limits<Real>::quiet_NaN();
    std::array<Real, 4> roots{nan, nan, nan, nan};
    if (abs(a) <= (std::numeric_limits<Real>::min)()) {
        auto cbrts = cubic_roots(b, c, d, e);
        roots[0] = cbrts[0];
        roots[1] = cbrts[1];
        roots[2] = cbrts[2];
        if (b == 0 && c == 0 && d == 0 && e == 0) {
           roots[3] = 0;
        }
        return detail::polish_and_sort(a, b, c, d, e, roots);
    }
    if (abs(e) <= (std::numeric_limits<Real>::min)()) {
        auto v = cubic_roots(a, b, c, d);
        roots[0] = v[0];
        roots[1] = v[1];
        roots[2] = v[2];
        roots[3] = 0;
        return detail::polish_and_sort(a, b, c, d, e, roots);
    }
    // Now solve x^4 + Ax^3 + Bx^2 + Cx + D = 0.
    Real A = b/a;
    Real B = c/a;
    Real C = d/a;
    Real D = e/a;
    Real Asq = A*A;
    // Let x = y - A/4:
    // Mathematica: Expand[(y - A/4)^4 + A*(y - A/4)^3 + B*(y - A/4)^2 + C*(y - A/4) + D]
    // We now solve the depressed quartic y^4 + py^2 + qy + r = 0.
    Real p = B - 3*Asq/8;
    Real q = C - A*B/2 + Asq*A/8;
    Real r = D - A*C/4 + Asq*B/16 - 3*Asq*Asq/256;
    if (abs(r) <= (std::numeric_limits<Real>::min)()) {
        auto [r1, r2, r3] = cubic_roots(Real(1), Real(0), p, q);
        r1 -= A/4;
        r2 -= A/4;
        r3 -= A/4;
        roots[0] = r1;
        roots[1] = r2;
        roots[2] = r3;
        roots[3] = -A/4;
        return detail::polish_and_sort(a, b, c, d, e, roots);
    }
    // Biquadratic case:
    if (abs(q) <= (std::numeric_limits<Real>::min)()) {
        auto [r1, r2] = quadratic_roots(Real(1), p, r);
        if (r1 >= 0) {
           Real rtr = sqrt(r1);
           roots[0] = rtr - A/4;
           roots[1] = -rtr - A/4;
        }
        if (r2 >= 0) {
           Real rtr = sqrt(r2);
           roots[2] = rtr - A/4;
           roots[3] = -rtr - A/4;
        }
        return detail::polish_and_sort(a, b, c, d, e, roots);
    }

    // Now split the depressed quartic into two quadratics:
    // y^4 + py^2 + qy + r = (y^2 + sy + u)(y^2 - sy + v) = y^4 + (v+u-s^2)y^2 + s(v - u)y + uv
    // So p = v+u-s^2, q = s(v - u), r = uv.
    // Then (v+u)^2 - (v-u)^2 = 4uv = 4r = (p+s^2)^2 - q^2/s^2.
    // Multiply through by s^2 to get s^2(p+s^2)^2 - q^2 - 4rs^2 = 0, which is a cubic in s^2.
    // Then we let z = s^2, to get
    // z^3 + 2pz^2 + (p^2 - 4r)z - q^2 = 0.
    auto z_roots = cubic_roots(Real(1), 2*p, p*p - 4*r, -q*q);
    // z = s^2, so s = sqrt(z).
    // No real roots:
    if (z_roots.back() <= 0) {
      return roots;
    }
    Real s = sqrt(z_roots.back());

    // s is nonzero, because we took care of the biquadratic case.
    Real v = (p + s*s + q/s)/2;
    Real u = v - q/s;
    // Now solve y^2 + sy + u = 0:
    auto [root0, root1] = quadratic_roots(Real(1), s, u);

    // Now solve y^2 - sy + v = 0:
    auto [root2, root3] = quadratic_roots(Real(1), -s, v);
    roots[0] = root0;
    roots[1] = root1;
    roots[2] = root2;
    roots[3] = root3;

    for (auto& r : roots) {
        r -= A/4;
    }
    return detail::polish_and_sort(a, b, c, d, e, roots);
}

}
#endif


#include <iostream>
#include <Eigen/Dense>
#include "uranus/Matrix.hpp"
#include "uranus/un-constrained.hpp"

int main()
{
	problem<2> f;	  // f = x_1^2 + 4x_2^2

	f.matirx_Jacobian_ << 2, 0, 0, 8;

    uranus::Vector<2> x0;
	x0 << 1, 1;                   // 初始点

	uranus::Vector<2> y = BFGS<2>(f,x0,0.001,true); // 用BFGS求解

    std::cout <<"zuiyoujie:\n" << y <<"\n";

    // dasds
    problem<2> f2;
    f2.matirx_Jacobian_ << 2, 0, 0, 2;
    uranus::Vector<2> x2;
    x2 << 5,5;

    double M_k = 0.1;                          // init M_k > 0
    uranus::Vector<2> x3;
    double a;
	do{
		M_k = 10 * M_k;                         // update M_k
		f2.matirx_Jacobian_(1,1) = 2 + M_k;     // update J mat
        cout << "M_k=============================\n"<<M_k<<"\n";
		x3 = BFGS(f2, x2, 0.001, true);       // min f

        a = M_k * pow(x3(1)-1, 2);
        std::cout <<"x3: \n" << x3 <<"\n";
	}
	while(a > 0.01);

    std::cout <<"x3: \n" << x3 <<"\n";

    return 0;
}


#pragma once

#include <polyfem/Types.hpp>

#include <Eigen/Dense>
#include <Eigen/Sparse>

namespace polyfem
{

	// Show some stats about the matrix M: det, singular values, condition number, etc
	void show_matrix_stats(const Eigen::MatrixXd &M);

	template <typename T>
	T determinant(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, 0, 3, 3> &mat)
	{
		assert(mat.rows() == mat.cols());

		if (mat.rows() == 1)
			return mat(0);
		else if (mat.rows() == 2)
			return mat(0, 0) * mat(1, 1) - mat(0, 1) * mat(1, 0);
		else if (mat.rows() == 3)
			return mat(0, 0) * (mat(1, 1) * mat(2, 2) - mat(1, 2) * mat(2, 1)) - mat(0, 1) * (mat(1, 0) * mat(2, 2) - mat(1, 2) * mat(2, 0)) + mat(0, 2) * (mat(1, 0) * mat(2, 1) - mat(1, 1) * mat(2, 0));

		assert(false);
		return T(0);
	}

	template <typename T>
	bool read_matrix(const std::string &path, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &mat);

	template <typename T>
	bool read_matrix_binary(const std::string &path, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &mat);

	template <typename Mat>
	bool write_matrix_binary(const std::string &path, const Mat &mat);

	class SpareMatrixCache
	{
	public:
		SpareMatrixCache() {}
		SpareMatrixCache(const size_t size);
		SpareMatrixCache(const size_t rows, const size_t cols);
		SpareMatrixCache(const SpareMatrixCache &other);

		void init(const size_t size);
		void init(const size_t rows, const size_t cols);
		void init(const SpareMatrixCache &other);

		void set_zero();

		inline void reserve(const size_t size) { entries_.reserve(size); }
		inline size_t entries_size() const { return entries_.size(); }
		inline size_t capacity() const { return entries_.capacity(); }
		inline size_t non_zeros() const { return mapping_.empty() ? mat_.nonZeros() : values_.size(); }

		void add_value(const int i, const int j, const double value);
		StiffnessMatrix get_matrix(const bool compute_mapping = true);
		void prune();

		SpareMatrixCache operator+(const SpareMatrixCache &a) const;
		void operator+=(const SpareMatrixCache &o);

	private:
		size_t size_;
		StiffnessMatrix tmp_, mat_;
		std::vector<Eigen::Triplet<double>> entries_;
		std::vector<std::vector<std::pair<int, size_t>>> mapping_;
		std::vector<int> inner_index_, outer_index_;
		std::vector<double> values_;
	};

} // namespace polyfem


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_IEEE_FUNCTION_SCALAR_ULPDIST_HPP_INCLUDED
#define NT2_TOOLBOX_IEEE_FUNCTION_SCALAR_ULPDIST_HPP_INCLUDED
#include <nt2/sdk/constant/eps_related.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/constant/digits.hpp>
#include <boost/fusion/tuple.hpp>

#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/tofloat.hpp>
#include <nt2/include/functions/ldexp.hpp>
#include <nt2/include/functions/frexp.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/include/functions/dist.hpp>
#include <nt2/include/functions/is_nan.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ulpdist_, tag::cpu_,
                         (A0)(A1),
                         (arithmetic_<A0>)(arithmetic_<A1>)
                        )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ulpdist_(tag::arithmetic_,tag::arithmetic_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)> :
      std::tr1::result_of<meta::arithmetic(A0,A1)>{};

    NT2_FUNCTOR_CALL(2)
    {
      return dist(a0, a1);
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is bool_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ulpdist_, tag::cpu_,
                         (A0)(A1),
                         (bool_<A0>)(bool_<A1>)
                        )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ulpdist_(tag::bool_,tag::bool_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)> :
      std::tr1::result_of<meta::arithmetic(A0,A1)>{};

    NT2_FUNCTOR_CALL(2)
    {
      return dist(a0, is_nez(a1));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is real_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::ulpdist_, tag::cpu_,
                         (A0)(A1),
                         (real_<A0>)(real_<A1>)
                        )

namespace nt2 { namespace ext
{
  template<class Dummy>
  struct call<tag::ulpdist_(tag::real_,tag::real_),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0,A1)> :
      std::tr1::result_of<meta::arithmetic(A0,A1)>{};

    NT2_FUNCTOR_CALL(2)
    {
      typedef typename std::tr1::result_of<meta::arithmetic(A0, A1) >::type type;
      typedef typename meta::as_integer<A0>::type itype;
      if (a0 == a1)               return Zero<type>();
      if (is_nan(a0)&&is_nan(a1)) return Zero<type>();
      itype e1, e2;
      type m1, m2;
      boost::fusion::tie(m1, e1) = nt2::frexp(type(a0));
      boost::fusion::tie(m2, e2) = nt2::frexp(type(a1));
      itype expo = -nt2::max(e1, e2);
      double e = (e1 == e2) ? nt2::abs(m1-m2)
      : nt2::abs(nt2::ldexp(a0, expo)-nt2::ldexp(a1, expo));
      return e/Eps<type>();
    }
  };
} }

#endif
// modified by jt the 26/12/2010


// inverse_gamma_example.cpp

// Copyright Paul A. Bristow 2010.

// Use, modification and distribution are subject to 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)

// Example 1 of using inverse gamma functions.

#include <boost/math/special_functions/gamma.hpp>

using boost::math::gamma_p_inv; // Compute x given a
//using boost::math::gamma_q_inv;
//using boost::math::gamma_p_inva; // Compute a given x
//using boost::math::gamma_q_inva;

#include <iostream>
   using std::cout;    using std::endl;
#include <iomanip>
   using std::setprecision;
#include <cmath>
   using std::sqrt;
#include <limits>

int main()
{
  cout << "Example 1 using Inverse Gamma function. " << endl;

  #ifdef BOOST_NO_CXX11_NUMERIC_LIMITS
  int max_digits10 = 2 + (boost::math::policies::digits<double, boost::math::policies::policy<> >() * 30103UL) / 100000UL;
  cout << "BOOST_NO_CXX11_NUMERIC_LIMITS is defined" << endl;
#else
  int max_digits10 = std::numeric_limits<double>::max_digits10;
#endif
  cout << "Show all potentially significant decimal digits std::numeric_limits<double>::max_digits10 = "
    << max_digits10 << endl;
  cout.precision(max_digits10); //

  double x = 1.;
  double a = 10;

  double r = boost::math::gamma_q_inv(a ,x);

  cout << " x = " << x << ", = gamma_q_inv(a,x)" << r << endl; //

  return 0;
}  // int main()

/*

Output is:

*/


/**
 * File: random_var.cpp
 * Date: Mon Nov  9 10:31:35 CET 2020
 * Author: Open Risk  (www.openriskmanagement.com)
 *
 * Examples of using the tailRisk library to compute tail risk measures
 *
 */

#include <armadillo>
#include "random_var.h"

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

    // Reading in some data for a type 0 representation (discrete distribution)
    int LossGrid = 1000;
    int DataType = 0;
    RandomVar L(LossGrid, DataType);
    L.ReadFromJSON("../../data/example5.json");
    L.Print();

    // Calculate various measures
    double alpha = 0.8;
    int threshold = 0;

    std::cout << "Mean Value: " << L.Mean() << std::endl;
    std::cout << "Median Value: " << L.Median() << std::endl;
    std::cout << "STD Value: " << L.StandardDeviation() << std::endl;
    std::cout << "Kurtosis: " << L.Kurtosis() << std::endl;
    std::cout << "Skeweness: " << L.Skeweness() << std::endl;
    std::cout << "Quantile @ " << alpha << ": " << L.Quantile(alpha) << std::endl;
    std::cout << "Quantile Index @ " << alpha << ": " << L.Quantile_Index(alpha) << std::endl;
    std::cout << "VaR @ " << alpha << ": " << L.VaR(alpha) << std::endl;
    std::cout << "Expected Shortfall @ " << alpha << ": " << L.ExpectedShortFall(alpha) << std::endl;
    std::cout << "Exceedance Probability: " << L.ExceedanceProbability(threshold) << std::endl;
    std::cout << "Mean Excess: " << L.MeanExcess(threshold ) << std::endl;

    return 0;
}


//=======================================================================
// Copyright (c) 2014 Andrzej Pacuk
//
// 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)
//=======================================================================
/**
 * @file performance_measures.hpp
 * @brief
 * @author Andrzej Pacuk
 * @version 1.0
 * @date 2014-10-22
 */
#ifndef PALL_PERFORMANCE_MEASURES_HPP
#define PALL_PERFORMANCE_MEASURES_HPP

#include <boost/range/combine.hpp>
#include <boost/range/empty.hpp>
#include <boost/range/size.hpp>
#include <boost/tuple/tuple.hpp>

#include <algorithm>
#include <cmath>
#include <utility>

namespace paal {

/**
 * @brief
 *
 * @tparam FloatType
 * @tparam Probs
 * @tparam TestResults
 *
 * @param probs
 * @param test_results
 */
template<typename FloatType = double,
         typename Probs, typename TestResults>
FloatType log_loss(Probs &&probs, TestResults &&test_results) {
    assert(boost::size(probs) == boost::size(test_results));
    assert(!boost::empty(probs));

    FloatType loss{};
    static FloatType EPSILON{1e-6};
    for(auto prob_result : boost::combine(probs, test_results)) {
        FloatType prob, result;
        boost::tie(prob, result) = prob_result;
        loss -= std::log(std::max(result ? prob : 1 - prob,
                                  EPSILON));
    }

    return loss / boost::size(probs);
}

/**
 * @brief
 *
 * @tparam FloatType
 * @param log_loss
 *
 * @return
 */
template<typename FloatType>
FloatType likelihood_from_log_loss(FloatType log_loss) {
    return std::exp(-log_loss);
}

/**
 * @brief
 *
 * @tparam FloatType
 * @tparam Probs
 * @tparam TestResults
 *
 * @param probs
 * @param test_results
 */
template<typename FloatType = double,
         typename Probs, typename TestResults>
FloatType likelihood(Probs &&probs, TestResults &&test_results) {
    return likelihood_from_log_loss(log_loss<FloatType>(std::forward<Probs>(probs),
                                    std::forward<TestResults>(test_results)));
}

/**
 * @brief
 *
 * @tparam FloatType
 * @tparam Probs
 * @tparam TestResults
 *
 * @param probs
 * @param test_results
 */
template<typename FloatType = double,
typename Probs, typename TestResults>
FloatType mean_absolute_error(Probs &&probs, TestResults &&test_results) {
    assert(boost::size(probs) == boost::size(test_results));
    assert(!boost::empty(probs));

    FloatType loss{};
    for(auto prob_result : boost::combine(probs, test_results)) {
        FloatType prob, result;
        boost::tie(prob, result) = prob_result;
        loss += std::abs(prob - result);
    }

    return loss / boost::size(probs);
}

} //! paal

#endif /* PALL_PERFORMANCE_MEASURES_HPP */


#pragma once

#include <vector>
#include <cassert>

#include <iostream>
#include <iomanip>

#include <Eigen/Dense>

//! \file rkintegrator.hpp Solution for Problem 1, implementing RkIntegrator class

//! \brief Implements a Runge-Kutta explicit solver for a given Butcher tableau for autonomous ODEs
//! \tparam State a type representing the space in which the solution lies, e.g. R^d, represented by e.g. Eigen::VectorXd.
template <class State>
class RKIntegrator {
public:
    //! \brief Constructor for the RK method.
    //! Performs size checks and copies A and b into internal storage
    //! \param[in] A matrix containing coefficents of Butcher tableau, must be (strictly) lower triangular (no check)
    //! \param[in] b vector containing coefficients of lower part of Butcher tableau
    RKIntegrator(const Eigen::MatrixXd & A, const Eigen::VectorXd & b)
        : A(A), b(b), s(b.size()) {
        assert( A.cols() == A.rows() && "Matrix must be square.");
        assert( A.cols() == b.size() && "Incompatible matrix/vector size.");
    }

    //! \brief Perform the solution of the ODE
    //! Solve an autonomous ODE y' = f(y), y(0) = y0, using a RK scheme given in the Butcher tableau provided in the
    //! constructor. Performs N equidistant steps upto time T with initial data y0
    //! \tparam Function type for function implementing the rhs function. Must have State operator()(State x)
    //! \param[in] f function handle for rhs in y' = f(y), e.g. implemented using lambda funciton
    //! \param[in] T final time T
    //! \param[in] y0 initial data y(0) = y0 for y' = f(y)
    //! \param[in] N number of steps to perform. Step size is h = T / N. Steps are equidistant.
    //! \return vector containing all steps y^n (for each n) including initial and final value
    template <class Function>
    std::vector<State> solve(const Function &f, double T, const State & y0, unsigned int N) const {
        // Iniz step size
        double h = T / N;

        // Will contain all steps, reserve memory for efficiency
        std::vector<State> res;
        res.reserve(N+1);

        // Store initial data
        res.push_back(y0);

        // Initialize some memory to store temporary values
        State ytemp1 = y0;
        State ytemp2 = y0;
        // Pointers to swap previous value
        State * yold = &ytemp1;
        State * ynew = &ytemp2;

        // Loop over all fixed steps
        for(unsigned int k = 0; k < N; ++k) {
            // Compute, save and swap next step
            step(f, h, *yold, *ynew);
            res.push_back(*ynew);
            std::swap(yold, ynew);
        }

        return res;
    }

private:

    //! \brief Perform a single step of the RK method for the solution of the autonomous ODE
    //! Compute a single explicit RK step y^{n+1} = y_n + \sum ... starting from value y0 and storing next value in y1
    //! \tparam Function type for function implementing the rhs. Must have State operator()(State x)
    //! \param[in] f function handle for ths f, s.t. y' = f(y)
    //! \param[in] h step size
    //! \param[in] y0 initial state
    //! \param[out] y1 next step y^{n+1} = y^n + ...
    template <class Function>
    void step(const Function &f, double h, const State & y0, State & y1) const {
        // create vector holding next value
        y1 = y0;

        // Reserve space for increments
        std::vector<State> k;
        k.reserve(s);

        // Loop over the size of RK
        for(unsigned int i = 0; i < s; ++i) {
            // Compute increments and save them to k
            State incr = y0;
            for(unsigned int j = 0; j < i; ++j) {
                incr += h*A(i,j)*k.at(j);
            }
            k.push_back( f( incr ) );
            y1 += h * b(i) * k.back();
        }
    }

    //! Matrix A in Butcher scheme
    const Eigen::MatrixXd A;
    //! Vector b in Butcher scheme
    const Eigen::VectorXd b;
    //! Size of Butcher matrix and vector A and b
    unsigned int s;
};

Nummerical Methods for CSE/PS13/solutions_ps13/rkintegrator.hpp


//---------------------------------------------------------------------------//
//!
//! \file   Utility_GaussRadauQuadratureSet.cpp
//! \author Luke Kersting
//! \brief  Gauss-Radau quadrature set
//!
//---------------------------------------------------------------------------//

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// FRENSIE Includes
#include "Utility_GaussRadauQuadratureSet.hpp"
#include "Utility_GaussKronrodIntegrator.hpp"
#include "Utility_ContractException.hpp"

namespace Utility{

// Constructor
GaussRadauQuadratureSet::GaussRadauQuadratureSet(
                              boost::function<double (double, int)>
                                polynomial_expansion_function,
                              const double error_tol,
                              const int polynomial_order )
  : d_polynomial_expansion_function( polynomial_expansion_function ),
    d_error_tol( error_tol ),
    d_polynomial_order( polynomial_order )
{
  // Make sure the error tolerances are valid
  testPrecondition( error_tol >= 0.0 );

  // Make sure the work space size is valid
  testPrecondition( polynomial_order > 0 );
}

// Caluclate the nth order Jacobi Polynomial at x
/* \details The Jacobi Polynomials can be calculated by the following recursion
 * relationship:
 * a1_n P_{n+1}^{\alpha,\beta}(x) = (a2_n + a3_n x)P_n^{\alpha,\beta}(x) - a4_n P_{n-1}^{\alpha,\beta}(x)
 * where:
 * a1_n = 2(n+1)(n+\alpha+\beta+1)(2n+\alpha+\beta)
 * a2_n = (2n+\alpha+\beta+1)(\alpha^2-\beta^2)
 * a3_n = (2n + \alpha + \beta)(2n + \alpha + \beta + 1)(2n + \alpha +\beta + 2)
 * a4_n = 2(n+\alpha)(n+\beta)(2n+\alpha+\beta+2)
 */
double GaussRadauQuadratureSet::getJacobiPolynomial(
                              double x,
                              int n,
                              int alpha,
                              int beta ) const
{
  // Calculate the first two polynomials
  double P_0 = 1.0;
  double P_1 = 0.5*( alpha - beta + ( alpha + beta + 2 )* x );

  if (n==0)
  {
    return P_0;
  }
  if (n==1)
  {
    return P_1;
  }
  else
  {
    int a_b = alpha + beta;
    int a_b_1 = a_b + 1;
    int a_b_2 = a_b + 2;

    double p_i_minus_2 = P_0;
    double p_i_minus_1 = P_1;
    double p_i;

    // Use recursion relation to calculate higher order Jacobi polynomials
    for (int i = 1; i < n; ++i)
    {
      int two_i = 2*i;

      // Calculate the Jacobi coefficients
      double a1_i = 2.0*( i + 1.0 )*( i + a_b_1 )*( two_i + a_b );
      double a2_i = ( two_i + a_b_1 )*( alpha*alpha - beta*beta );
      double a3_i = ( two_i + a_b )*( two_i + a_b_1 )*( two_i + a_b_2 );
      double a4_i = 2.0*( i + alpha )*( i + beta )*( two_i + a_b_2 );

      // Calculate new P_n value
      p_i = 1.0/a1_i*( ( a2_i + a3_i*x )*p_i_minus_1 - a4_i*p_i_minus_2 );

      // Update P_n_minus_1 and P_n_minus_2 for next value on n
      p_i_minus_2 = p_i_minus_1;
      p_i_minus_1 = p_i;
    }

  return p_i;
  }
}

double GaussRadauQuadratureSet::getLegendrePolynomial(
                                        double x,
                                        int n ) const
{
  return getJacobiPolynomial( x, n, 0, 0 );
}

// Calculate the derivative of the nth order Jacobi Polynomial at x
/* \details The Jacobi Polynomials can be calculated by the following recursion
 * relationship:
 * 1/2( n + \alpha + \beta + 1 )P_{n-1}^{\alpha + 1,\beta + 1}(x)
 */
double GaussRadauQuadratureSet::getJacobiPolynomialDerivative(
                              double x,
                              int n,
                              int alpha,
                              int beta ) const
{

  if (n==0)
  {
    return 0.0;
  }
  if (n==1)
  {
    return 0.5*( alpha + beta + 2.0 );
  }
  else
  {
    return 0.5 * (alpha + beta + n + 1.0) * getJacobiPolynomial( x,
                                                                 n-1,
                                                                 alpha + 1,
                                                                 beta + 1 );
  }
}

// Estimate the roots of the Jacobi Polynomial
/* \details The roots of the Jacobi Polynomials can be estimated by the roots of
 * the Chebyshev Polynomials which are given by the relationship:
 * x_k = cos( (2k - 1)\pi/2n ) , k = 1, ... , n
 */
void GaussRadauQuadratureSet::getJacobiPolynomialRoots(
                                 Teuchos::Array<double>& roots,
                                 const int n,
                                 int alpha,
                                 int beta ) const
{
  // Max number of allowed iterations
  int max_iterations = 200;

  int iteration;
  double root_k, s, jacobi, jacobi_derivative, delta_root;

  // Iterate through all n roots ( 0 < k < n )
  for (int k = 0; k < n; k++)
  {
    // Make an initial guess that the roots are equal to the roots of the Chebyshev Polynomial
    root_k = -cos( ( 2.0*k + 1.0 )/( 2.0 * n )* PhysicalConstants::pi );

    // Actual root is known to be inbetween roots of Chebyshev Polynomial
    if (k > 0)
    {
      root_k = ( root_k + roots[k-1] )/2.0;
    }

    iteration = 0;

    // Iterate until you converge on root
    do
    {
      s = 0;

      for (int i = 0; i < k; i++)
      {
        s += 1.0/( root_k - roots[i] );
      }

      // Get error
      jacobi = getJacobiPolynomial( root_k, n, alpha, beta );
      jacobi_derivative =
               getJacobiPolynomialDerivative( root_k, n, alpha, beta );\

      delta_root = -jacobi/( jacobi_derivative - jacobi*s );

      // Update root value
      root_k += delta_root;

      // Update iteration
      ++iteration;

      if ( iteration > max_iterations )
         break;
/*
      TEST_FOR_EXCEPTION( iteration > max_iterations,
		          RadauQuadratueError,
		          "Error: the root of the Jacobi Polynomial "
                          "did not converge" );
*/
    }
    while ( fabs(delta_root) > d_error_tol );

    roots[k] = root_k;
  }
}

// Find the Radau nodes and wieghts including at end point -1 or 1
void GaussRadauQuadratureSet::findNodesAndWeights(
                            double end_point,
                            Teuchos::Array<double>& nodes,
                            Teuchos::Array<double>& weights ) const
{
  // Make sure end_point is either -1 or 1
  testPrecondition( fabs(end_point) == 1.0 );

  int n = d_polynomial_order;
  int alpha = 0;
  int beta = 1;
  double jacobi_derivative;

  Teuchos::Array<double> roots( n );

  // Calculate the roots of the Jacobi Polynomial
  getJacobiPolynomialRoots( roots, n, alpha, beta );

  // Check to see if the end point is the first node
  if ( end_point < roots[0] )
  {
    nodes[0] = end_point;
    weights[0] = findWeightAtEndPoint( end_point, n );

    // Iterate through the all nodes and weights
    for ( int i = 0; i < n; ++i )
    {
      nodes[i+1] = roots[i];

      weights[i+1] = findWeightAtNode( roots[i], end_point, n );
    }
  }
  else
  {
    nodes[n] = end_point;
    weights[n] = findWeightAtEndPoint( end_point, n );

    // Iterate through all other nodes and weights
    for ( int i = 0; i < n; ++i )
    {
      nodes[i] = roots[i];

      weights[i] = findWeightAtNode( roots[i], end_point, n );
    }
  }
}

// Find the Radau nodes and wieghts including at end point -1 or 1
void GaussRadauQuadratureSet::findNodesAndPositiveWeights(
                            double end_point,
                            Teuchos::Array<double>& nodes,
                            Teuchos::Array<double>& weights ) const
{
  // Make sure end_point is either -1 or 1
  testPrecondition( fabs(end_point) == 1.0 );

  int n = d_polynomial_order;
  int alpha = 0;
  int beta = 1;
  double jacobi_derivative;

  Teuchos::Array<double> roots( n );

  // Calculate the roots of the Jacobi Polynomial
  getJacobiPolynomialRoots( roots, n, alpha, beta );

  // Check to see if the end point is the first node
  if ( end_point < roots[0] )
  {
    nodes[0] = end_point;
    weights[0] = findWeightAtEndPoint( end_point, n );

    // Iterate through the all nodes and weights
    for ( int i = 0; i < n; ++i )
    {
      nodes[i+1] = roots[i];

      jacobi_derivative =
              getJacobiPolynomialDerivative( nodes[i+1], n, alpha, beta );

      weights[i+1] = findWeightAtNode( roots[i], end_point, n );
    }
  }
  else
  {
    nodes[n] = end_point;
    weights[n] = findWeightAtEndPoint( -end_point, n );

    // Iterate through all other nodes and weights
    for ( int i = 0; i < n; ++i )
    {
      nodes[n-1-i] = -roots[i];

      // Get the derivative of the Jacobi Polynomial of order n
      jacobi_derivative =
              getJacobiPolynomialDerivative( roots[i],
                                             n,
                                             alpha, beta );

      weights[n-1-i] = findWeightAtNode( roots[i], -end_point, n );
    }
  }
}

// Create the integrand function for the weight at the end point -1 or 1
double GaussRadauQuadratureSet::getFixedWeightIntegrand(
                               double end_point,
                               int n ) const
{
  return d_polynomial_expansion_function( end_point, n )*getJacobiPolynomial( end_point, n );
}

// Create the integrand function for the weight at a given node with end point -1 or 1
double GaussRadauQuadratureSet::getWeightIntegrand(
                               double x,
                               double node,
                               double end_point,
                               int n ) const
{
  return getFixedWeightIntegrand( x, n )*( x - end_point )/( x - node );
}

// Find the Radau wieght for the function at the end point -1 or 1
double GaussRadauQuadratureSet::findWeightAtEndPoint( double end_point,
                                                         int n ) const
{
  // Make sure end_point is either -1 or 1
  testPrecondition( fabs(end_point) == 1.0 );
  // Make sure n is positive
  testPrecondition( n > 0.0 );

  boost::function<double (double x)> weight_function =
    boost::bind<double>( &GaussRadauQuadratureSet::getFixedWeightIntegrand,
			 boost::cref( *this ),
			 _1,
			 n );

  double abs_error, result;
  double precision = 1e-12;

  Utility::GaussKronrodIntegrator integrator( precision );

  integrator.integrateAdaptively<15>(
					weight_function,
					-1.0,
					1.0,
					result,
					abs_error );

  return result/getJacobiPolynomial( end_point, n );

}

// Find the Radau wieght for the function at a given node
double GaussRadauQuadratureSet::findWeightAtNode( double node,
                                                     double end_point,
                                                     int n ) const
{
  // Make sure end_point is either -1 or 1
  testPrecondition( fabs(end_point) == 1.0 );
  // Make sure node is between -1 and 1
  testPrecondition( node < 1.0 );
  testPrecondition( node > -1.0 );
  // Make sure n is positive
  testPrecondition( n > 0.0 );

  boost::function<double (double x)> weight_function =
    boost::bind<double>( &GaussRadauQuadratureSet::getWeightIntegrand,
			 boost::cref( *this ),
			 _1,
                         node,
                         end_point,
			 n );

  double abs_error, result;
  double precision = 1e-12;

  Utility::GaussKronrodIntegrator integrator( precision );

  integrator.integrateAdaptively<15>(
					weight_function,
					-1.0,
					1.0,
					result,
					abs_error );
  return result/
           ( getJacobiPolynomialDerivative( node, n ) * ( node - end_point) );

}

} // end Utility namespace

//---------------------------------------------------------------------------//
// end Utility_GaussRadauQuadratureSet.cpp
//---------------------------------------------------------------------------//


/* Implementation of the algorithm
 *
  D. Conte, L. Ixaru, B. Paternoster, G. Santomauro:
    Exponentially-fitted Gauss–Laguerre quadrature rule for integrals over an unbounded interval.

  https://hpcquantlib.wordpress.com/2020/05/17/optimized-heston-model-integration-exponentially-fitted-gauss-laguerre-quadrature-rule/

    Copyright (c) 2020, Klaus Spanderen
    All rights reserved.

    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are met:

    1. Redistributions of source code must retain the above copyright notice, this
    list of conditions and the following disclaimer.

    2. Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the documentation
    and/or other materials provided with the distribution.

    3. Neither the names of the copyright holders nor the names of the QuantLib
    Group and its contributors may be used to endorse or promote products
    derived from this software without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
    AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
    IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
    DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
    FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
    DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
    SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
    CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
    OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
    OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <ql/math/integrals/gaussianquadratures.hpp>

#include <cmath>
#include <iostream>
#include <fstream>
#include <queue>

#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/unordered_map.hpp>
#include <boost/multiprecision/gmp.hpp>
#include <boost/thread.hpp>
#include <boost/bind.hpp>

using namespace boost::numeric::ublas;

//typedef boost::multiprecision::number<
//            boost::multiprecision::cpp_dec_float<400> > Float;

typedef boost::multiprecision::number<
            boost::multiprecision::gmp_float<600> > Float;

template <class real>
boost::numeric::ublas::vector<real> lu(
    const boost::numeric::ublas::matrix<real>& A,
    const boost::numeric::ublas::vector<real>& b) {

    matrix<real> A_fac = A;

    permutation_matrix<std::size_t> piv(b.size());
    int singular = lu_factorize(A_fac, piv);

    if (singular)
        throw std::runtime_error("lu: A is singular.");

    vector<real> x = b;
    lu_substitute(A_fac, piv, x);

    return x;
}

template <class real>
boost::numeric::ublas::matrix<real> inv(
    const boost::numeric::ublas::matrix<real>& A) {

    using namespace boost::numeric::ublas;

    matrix<real> A_fac = A;

    permutation_matrix<std::size_t> piv(A.size1());
    int singular = lu_factorize(A_fac, piv);

    if (singular)
        throw std::runtime_error("lu: A is singular.");

    matrix<real> inverse = identity_matrix<real>(A.size1());

    lu_substitute(A_fac, piv, inverse);

    return inverse;
}

Float pow_c(const Float& x, int n) {

    if (n == 0)
        return Float(1.0);

    if (n == 1)
        return x;

    typedef std::pair<int, Float> key_type;

    typedef boost::unordered_map<key_type, Float> ResultMap;

    static boost::mutex mutex;

    static std::size_t maxCacheSize = 16384;

    static ResultMap results;
    static std::queue<key_type> fifo;

    const key_type key(n, x);
    {
        boost::lock_guard<boost::mutex> lock(mutex);

        const typename ResultMap::const_iterator iter = results.find(key);

        if (iter != results.end())
            return iter->second;
    }

    const Float result = (n < 0)
                ? Float(1.0)/pow_c(x, -n)
                : Float(pow_c(x, n/2) * pow_c(x, n/2) * pow_c(x, n - 2*(n/2)));

    boost::lock_guard<boost::mutex> lock(mutex);

    results.emplace(key, result);
    fifo.push(key);

    while(fifo.size() > maxCacheSize) {
        results.erase(fifo.front());
        fifo.pop();
    }

    return result;
}

Float pow_i(long n) {
    if (n < 0)
        return pow_c(Float(2), n);
    else if (n < 8*sizeof(unsigned long))
        return Float(1uL << n);
    else
        return pow_c(Float(2), n);
}

template <class result_type, class float_type>
result_type eta(int m, float_type Z) {

    typedef std::pair<int, float_type> key_type;
    typedef boost::unordered_map<key_type, result_type> ResultMap;

    static boost::mutex mutex;

    static std::size_t maxCacheSize = 16384;

    static ResultMap results;
    static std::queue<key_type> fifo;

    const key_type key(m, Z);
    {
        boost::lock_guard<boost::mutex> lock(mutex);
        const typename ResultMap::const_iterator iter = results.find(key);

        if (iter != results.end()) {
            return iter->second;
        }
    }

    result_type result;

    if (m == 0) {
        if (Z < 0) {
            const result_type sz = sqrt(-Z);
            result = sin(sz)/sz;
        }
        else if (Z > 0) {
            const result_type sz = sqrt(Z);
            result = sinh(sz)/sz;
        }
        else
            result = result_type(1.0);
    }
    else if (m == -1)
        if (Z <= 0)
            result = cos(sqrt(-Z));
        else
            result = cosh(sqrt(Z));
    else
        result = (eta<result_type, float_type>(m-2, Z)
            - (2*m-1)*eta<result_type, float_type>(m-1, Z))/Z;

    boost::lock_guard<boost::mutex> lock(mutex);
    results.emplace(key, result);
    fifo.push(key);

    while(fifo.size() > maxCacheSize) {
        results.erase(fifo.front());
        fifo.pop();
    }

    return result;
}

template <class real>
real factorial(std::size_t n) {
    static std::vector<real> cache(1, real(1));

    if (cache.size() > n)
        return cache[n];

    const real val = real(n) * factorial<real>(n-1);
    cache.resize(n+1);

    return cache[n] = val;
}

template <class real>
vector<real> w_lin(const vector<real>& x, const real& Z) {
    const int N = x.size();

    const int s = N/2;
    const int r = N-s;

    matrix<real> A(N, N);
    int row = 0;

    vector<real> b(x.size());

    for (int n=r+1; n <= N; ++n) {
        for (int k=0; k < N; ++k)
            A(row, k) = pow_c(x(k), 2*n-2)*eta<real, real>(n-2, x(k)*x(k)*Z);

        b(row) = pow_i(n-1)*factorial<real>(n-1)/pow_c(1.0-Z, n);
        ++row;
    }

    for (int n=s+1; n <= N; ++n) {
        for (int k=0; k < N; ++k)
            A(row, k) = pow_c(x(k), 2*n-1)*eta<real, real>(n-1, x(k)*x(k)*Z);

        b(row) = pow_i(n-1)*factorial<real>(n-1)/pow_c(1.0-Z, n);
        ++row;
    }

    return lu(A,b);
}

class WorkerJxA {
  public:
    WorkerJxA(
        matrix<Float>& JxA, const vector<Float>& w,
        const vector<Float>& x, const Float& Z)
    : JxA_(JxA), w_(w), x_(x), Z_(Z) {}

    void run() const {
        const int N = JxA_.size1();
        const int s = N/2;
        const int r = N-s;

        for (int j=0; j < N; ++j) {
            const Float xxZ = x_(j)*x_(j)*Z_;

            for (int i=1; i <= N; ++i) {
                JxA_(i-1, j) = (i <= s)
                    ? pow_c(x_(j), 2*(i+r)-3)*(
                        2*(i+r-1)*eta<Float, Float>(i+r-2, xxZ)
                        + xxZ*eta<Float, Float>(i+r-1, xxZ))
                    : pow_c(x_(j), 2*(i-1))*(
                        (2*i-1)*eta<Float, Float>(i-1, xxZ)
                        + xxZ*eta<Float, Float>(i, xxZ));
                JxA_(i-1, j) *= -w_(j);
            }
        }
    }

  private:
    matrix<Float>& JxA_;
    const vector<Float>& w_, x_;
    const Float& Z_;
};

class WorkerInvA {
  public:
    WorkerInvA(matrix<Float>& A, const vector<Float>& x, const Float& Z)
    : A_(A), x_(x), Z_(Z) { }

    void run() const {
        const int N = A_.size1();
        const int s = N/2;
        const int r = N-s;

        for (int j=0; j < N; ++j) {
            const Float xxZ = x_(j)*x_(j)*Z_;
            for (int i=1; i <= N; ++i)
                A_(i-1, j) = (i <= s)
                   ? pow_c(x_(j), 2*(i+r-1))*eta<Float, Float>(i+r-2, xxZ)
                   : pow_c(x_(j), 2*i-1    )*eta<Float, Float>(i-1,   xxZ);
        }

        A_ = inv(A_);
    }

  private:
    matrix<Float>& A_;
    const vector<Float>& x_;
    const Float& Z_;
};

class WorkerC {
  public:
    WorkerC(matrix<Float>& C, const vector<Float>& w,
            const vector<Float>& x, const Float& Z)
    : C_(C), w_(w), x_(x), Z_(Z) {}

    void run() const {
        const int N = C_.size1();
        const int s = N/2;
        const int r = N-s;

        for (int k=0; k < N; ++k) {
            const Float xxZ = x_(k)*x_(k)*Z_;
            for (int i=1; i <= N; ++i) {
                C_(i-1, k) = (i <= r)
                    ? pow_c(x_(k), 2*i-3)*( (2*i-2)*eta<Float, Float>(i-2, xxZ)
                        + xxZ*eta<Float, Float>(i-1, xxZ) )
                    : pow_c(x_(k), 2*(i-r-1))*( (2*(i-r)-1)*eta<Float, Float>(i-r-1, xxZ)
                        + xxZ*eta<Float, Float>(i-r, xxZ) );
                C_(i-1, k) *= w_(k);
            }
        }
    }
  private:
    matrix<Float>& C_;
    const vector<Float>& w_, x_;
    const Float& Z_;
};

class WorkerD {
  public:
    WorkerD(matrix<Float>& D, vector<Float>& dZ, const vector<Float>& x, const Float& Z)
    : D_(D), dZ_(dZ), x_(x), Z_(Z) {}

    void run() const {
        const int N = D_.size1();
        const int s = N/2;
        const int r = N-s;

        for (int k=0; k < N; ++k) {
            const Float xxZ = x_(k)*x_(k)*Z_;
            for (int i=1; i <= N; ++i)
                D_(i-1, k) = ( i <= r)
                    ? pow_c(x_(k), 2*i-2)    *eta<Float, Float>(i-2, xxZ)
                    : pow_c(x_(k), 2*(i-r)-1)*eta<Float, Float>(i-r-1, xxZ);
        }

        const Float omz(1-Z_);

        for (int i=1; i <= N; ++i)
            dZ_(i-1) = (i <= r)
                ? pow_i(i-1)*factorial<Float>(i-1) / pow_c(omz, i)
                : pow_i(i-r-1)*factorial<Float>(i-r-1) / pow_c(omz, i-r);
    }

  private:
    matrix<Float>& D_;
    vector<Float>& dZ_;
    const vector<Float>& x_;
    const Float& Z_;
};

template <class real>
vector<real> newton_iter(const vector<real>& w, const vector<real>& x, const real& Z) {
    const int N = x.size();
    const int s = N/2;
    const int r = N-s;

    matrix<real> invA(N, N);
    WorkerInvA workerInvA(invA, x, Z);

    boost::thread invA_thread(&WorkerInvA::run, &workerInvA);

    matrix<real> JxA(N, N);

    WorkerJxA workerJxA(JxA, w, x, Z);
    boost::thread JxA_thread(&WorkerJxA::run, &workerJxA);

    matrix<real> C(N, N);
    WorkerC workerC(C, w, x, Z);
    boost::thread C_thread(&WorkerC::run, &workerC);

    matrix<real> D(N, N);
    vector<real> dZ(N);
    WorkerD workerD(D, dZ, x, Z);
    boost::thread D_thread(&WorkerD::run, &workerD);

    JxA_thread.join();
    invA_thread.join();
    const matrix<real> JxW = prod(invA, JxA);

    C_thread.join();
    D_thread.join();

    const matrix<real> B = C + prod(D, JxW);

    return lu(B, vector<real>(prod(D, w) - dZ));
}

template <class real>
vector<real> newton(vector<real>& x, real Z) {
    const static real eps = Float(1e-300);

    const std::size_t N = x.size();

    vector<real> w(N), dx;

    do {
        w = w_lin(x, Z);

        dx = newton_iter(w, x, Z);

        x = x - dx;

        std::cout << norm_2(dx) << std::endl;

        for (std::size_t i=0; i < N; ++i)
            if (x(i) < 0.0) {
                return vector<real>();
            }
    }
    while (norm_2(dx) > eps);

    return w;
}

bool greaterThan(vector<Float>& x, vector<Float>& y) {
    bool f = false;

    for (std::size_t i=0; i < x.size(); ++i) {
        if (x[i] >= y[i]) {
            f = true;
        }
    }
    return f;
}

int main() {

    const std::size_t n = 64;
    const std::size_t maxOrder = 45;

    const QuantLib::Array x_laguerre =
        QuantLib::GaussLaguerreIntegration(n).x();
    const QuantLib::Array w_laguerre =
        QuantLib::GaussLaguerreIntegration(n).weights();

    std::vector<vector<Float> > x(maxOrder, vector<Float>(n));
    std::copy(x_laguerre.begin(), x_laguerre.end(), x[0].begin());

    std::ofstream f("values.txt");
    f << std::setprecision(std::numeric_limits<double>::digits10 + 1)
        << "{ 0.0";
    for (std::size_t i = 0; i < n; ++i)
        f << ", " << x_laguerre[i];
    for (std::size_t i = 0; i < n; ++i)
        f << ", " << w_laguerre[i];
    f << " }," << std::endl;
    f.flush();

    vector<Float> xGuess;
    std::vector<Float> o(maxOrder, Float(0.0));
    o[0] = 0.01;

    std::size_t iter = 0;

    vector<Float> w;
    w = newton(x[0], Float(-o[0]*o[0]));

    while (o[0] < 50.0) {

        ++iter;
        const std::size_t order = std::min(maxOrder, iter);

        vector<Float> xTest(n);

        const Float m1 = o[0] + Float(0.01);
        const Float m2 = o[0]*(1 + 0.0075);

        Float nomega = (m1 > m2)? m1 : m2;

        do {
            const Float z = -nomega*nomega;

            for (std::size_t i=0; i < order; ++i) {
                Float l=1.0;
                for (std::size_t j=0; j < order; ++j)
                    if (i != j)
                        l *= (nomega-o[j])/(o[i]-o[j]);

                xTest += x[i]*l;
            }

            xGuess = xTest;

            w = newton(xTest, z);

            if (w.size() == 0) {
                std::cout << "opps, monotocity violation " << nomega;
                nomega = o[0] + 0.5*(nomega - o[0]);
                std::cout << " new " << nomega << std::endl;
            }
        } while (w.size() == 0);

        std::cout << "start norm " << nomega << " " << norm_2(xGuess - xTest) << std::endl;

        if (greaterThan(xTest, x[0]))
            std::cout << "wrong direction" << std::endl;

        for (int i=std::min(maxOrder-1, order); i > 0; --i) {
            x[i] = x[i-1];
            o[i] = o[i-1];
        }

        x[0] = xTest;
        o[0] = nomega;

        Float s=0;
        for (std::size_t i=0; i < n; ++i)
            s+=w(i)*(x[0](i)*cos(o[0]*x[0](i)) + x[0](i)*sin(o[0]*x[0](i)));

        const Float expected = (1+2*o[0]-o[0]*o[0])/(1+o[0]*o[0])/(1+o[0]*o[0]);
        if (abs(s - expected) > 1e-16) {
            std::cout << "integration error " << abs(s - expected) << std::endl;
            exit(-1);
        }

        f << std::setprecision(std::numeric_limits<double>::digits10 + 1)
            << "{ " << o[0];
        for (std::size_t i = 0; i < n; ++i)
            f << ", " << x[0](i);
        for (std::size_t i = 0; i < n; ++i)
            f << ", " << exp(x[0](i))*w(i);
        f << " }," << std::endl;
        f.flush();
    }
    f.close();
}


#pragma once
#include <Eigen/Core>
#include "datatypes.hpp"
#include <Eigen/Eigen>
#include "profile.hpp"
#include "integration.hpp"
#include "transformation.hpp"

namespace gd {

using namespace std;
USING_PART_OF_NAMESPACE_EIGEN

class Mesh1d {
public:
	virtual int findindex(double) { return 0; }
	virtual double indexto_x(int) { return 0; }
	//virtual void indexto_x(int) { return 0; }
	virtual bool inrange(double) { return false; }
	virtual int length() {return 0;}
};

template<class Base=Mesh1d>
class Mesh1dRegular : public Base {
public:
	Mesh1dRegular(double x1, double x2, int length) : x1(x1), x2(x2), _length(length) {}
	int findindex(double x) {
		return x < x1 ? 0 :
			(x >= x2 ? _length-1 : (int)((x-x1)/(x2-x1)*(_length)));
	}
	double indexto_x(int i) {
		return x1 + (x2-x1)/length()*i;
	}
	bool inrange(double r) {
		return (r >= x1) && (r < x2);
	}
	int length() { return _length; }
	double x1, x2;
	int _length;
};

template<class T=double>
class BasisTriLeft {
public:
	T operator()(T x) {
		return x >= -1 ? (x < 0 ? (x+1) : 0) : 0;
	}
};

template<class T=double>
class BasisTriRight {
public:
	T operator()(T x) {
		return x >= 0 ? (x < 1 ? (1-x) : 0) : 0;
	}
};

template<class T=double>
class BasisTriMesh1dRegular {
public:
	BasisTriMesh1dRegular(T _x1, T _x2, int _n_nodes) : x1(_x1), x2(_x2), n_nodes(_n_nodes), mesh(_x1, _x2, _n_nodes), ctrans(_n_nodes+1,_n_nodes+1) {
		MatrixXd m = MatrixXd::Zero(_n_nodes+1,_n_nodes+1);
		//m.setZero();
		m(0,0) = 1./3;
		m(_n_nodes,_n_nodes) = 1./3;
		for(int i = 1; i < (_n_nodes+1); i++) {
			m(i, i-1) = 1./6;
			m(i-1, i) = 1./6;
		}
		for(int i = 1; i < (_n_nodes); i++) {
			m(i, i) = 2./3;
		}
		T scale = (x2-x1)/n_nodes;
		m = m * scale;
		//cout << m << endl;
		ctrans = m.inverse();
		//cout << ctrans << endl;
	}
	void testprofile(Profile* profile, double_vector v, bool dotrans) {
		double* vp = v.data().begin();
		double* array = vp;
		//int size = x.size();
		//double scale = 1./size;
		//cout << "size = " << size << endl;
		for(int i = 0; i < n_nodes; i++) {
			double integral = 0;
			T xleft = mesh.indexto_x(i);
			T xright = mesh.indexto_x(i+1);
			T dx = (xright-xleft);
			{
				BasisTriRight<T> triright;
				auto f = [&](double x) { return triright((x-xleft)/dx) * profile->densityr(x); };
				IntegratorGSL<> integratorGSL(f); // the integrator
				integral = integratorGSL.integrate(xleft, xright);
				array[i] +=  integral;
			}
			{
				BasisTriLeft<T> trileft;
				auto f = [&](double x) { return trileft((x-xleft)/dx-1) * profile->densityr(x); };
				IntegratorGSL<> integratorGSL(f); // the integrator
				integral = integratorGSL.integrate(xleft, xright);
				array[i+1] +=  integral;
			}
		}
		if(dotrans) {
			VectorXd v_alias = VectorXd::Map(v.data().begin(), v.size());
			VectorXd vtrans = ctrans * v_alias;
			VectorXd::Map(v.data().begin(), v.size()) = vtrans;
		}
	}

	void test(double_vector x, double_vector y, double_vector v, bool dotrans) {
		double* xp = x.data().begin();
		double* yp = y.data().begin();
		double* vp = v.data().begin();
		int size = x.size();
		double scale = 1./size * (x2-x1);
		//cout << "size = " << size << endl;
		for(int i = 0; i < size; i++) {
			//cout << "i = " << i << endl;
			this->operator()(xp[i], yp[i]*scale, vp);

		}
		if(dotrans) {
			VectorXd v_alias = VectorXd::Map(v.data().begin(), v.size());
			VectorXd vtrans = ctrans * v_alias;
			VectorXd::Map(v.data().begin(), v.size()) = vtrans;
		}
	}

	template<class A>
	T operator()(T x, T y, A array) {
		if(mesh.inrange(x)) {
			int indexleft = mesh.findindex(x);
			T xleft = mesh.indexto_x(indexleft);
			T xright = mesh.indexto_x(indexleft+1);
			T fraction = (x-xleft)/(xright-xleft);
			//cout << "x = " << x << " xleft = " << xleft << " xright = " << xright << " fraction = " << fraction << endl;
			BasisTriLeft<T> trileft;
			BasisTriRight<T> triright;
			//cout << "y = " << y << " triright(fraction) = " << triright(fraction) << " trileft (fraction-1) = " << trileft (fraction-1) << endl;
			array[indexleft]   +=  triright(fraction) * y;
			array[indexleft+1] +=  trileft (fraction-1) * y;
			// TODO: use trileft(fraction) + triright(fraction) == 1
		}
		return 0; // TODO: not finished.., remove code?
	}

	T x1, x2;
	int n_nodes;
	Mesh1dRegular<> mesh;
	MatrixXd ctrans;
};

template<class Basis, class T=double>
class MeshRegularNodal1d {
public:
	typedef Basis basis_type;
	T x1, x2;
	int n_cells;
	Mesh1dRegular<> mesh;
	MatrixXd ctrans;
	Transformation1d_in_3d* transformation;
	int dof;
	enum { dof_per_cell = Basis::degree+1 };

	int get_dof() { return dof;}
	int get_n_cells() { return n_cells;}
	int dof_index(int cell_index, int local_index) {
		return  cell_index*(dof_per_cell-1)+local_index;
	}

	template<int I, class B=Basis>
	struct util {
		typedef util<I-1, typename B::next_type> next_type;
		next_type next;
		T integrate(int i, double xleft, double xright, double dx, Profile* profile) {
			if(i == I) {
				B basis;
				auto f = [&](double x) { return basis((x-xleft)/dx) * profile->densityr(x); };
				IntegratorGSL<> integratorGSL(f); // the integrator
				return integratorGSL.integrate(xleft, xright);
			} else {
				return next.integrate(i, xleft, xright, dx, profile);
			}
		}
		template<class F>
		T integrate2(int i, double xleft, double xright, double dx, F f) {
			if(i == I) {
				B basis;
				auto f2 = [&](double x) { return basis((x-xleft)/dx) * f(x); };
				IntegratorGSL<> integratorGSL(f2); // the integrator
				return integratorGSL.integrate(xleft, xright);
			} else {
				return next.integrate2(i, xleft, xright, dx, f);
			}
		}
		template<class Array>
		T eval(Array& array, int index, double xleft, double dx, double x) {
			B basis;
			//cout << "eval: " << index << " " << xleft << " " << dx << " " << x << " u=" << ((x-xleft)/dx) << " " << basis((x-xleft)/dx) << " " << array(index) << endl;
			return basis((x-xleft)/dx) * array(index) + next.eval(array, index-1, xleft, dx, x);
		}
		template<class Array>
		T gradient(Array& array, int index, double xleft, double dx, double x) {
			B basis;
			//cout << "grad: " << index << " " << xleft << " " << dx << " " << x << " u=" << ((x-xleft)/dx) << " " << basis((x-xleft)/dx) << " " << basis.dfdx((x-xleft)/dx) << " " << array(index) << endl;
			return basis.dfdx((x-xleft)/dx)/dx * array(index) + next.gradient(array, index-1, xleft, dx, x);
		}
	};
	template<class B>
	struct util<-1, B> {
		T integrate(int, double, double, double, Profile*) {
			return 0;
		}
		template<class F>
		T integrate2(int, double, double, double, F) {
			return 0;
		}
		template<class Array>
		T eval(Array&, int, double, double, double) {
			return 0;
		}
		template<class Array>
		T gradient(Array&, int, double, double, double) {
			return 0;
		}
	};

	template<int I, int J, class B1=Basis, class B2=Basis>
	struct selfintegrator {
		typedef selfintegrator<I-1, J, typename B1::next_type, B2> next_typeI;
		typedef selfintegrator<I, J-1, B1, typename B2::next_type> next_typeJ;
		next_typeI nextI;
		next_typeJ nextJ;
		T integrate(int i, int j) {
			if((i == I)) { // first search for right i
				if(j == J) { // then right j
					B1 basis1;
					B2 basis2;
					auto f = [&](double x) { return basis1(x) * basis2(x); };
					IntegratorGSL<> integratorGSL(f);
					return integratorGSL.integrate(0, 1);
				}  else {
					return nextJ.integrate(i, j);
				}
			} else {
				return nextI.integrate(i, j);
			}
		}
		T integrate_grad(int i, int j, double xleft, double xright, double dx, Transformation1d_in_3d* transformation) {
			if((i == I)) { // first search for right i
				if(j == J) { // then right j
					B1 basis1;
					B2 basis2;
					//auto f = [&](double x) { return basis1.dfdx((x-xleft)/dx) / dx * basis2.dfdx((x-xleft)/dx) / dx *  x * x;}; //*/; };
					auto f = [&](double x) -> double {
						//double r = tan(u*M_PI/2);
						double u = x;
						//double t = tan(u*M_PI/2);
						//double c = cos(u*M_PI/2);
						//return 16./2 * basis1.dfdx((x-xleft)/dx) / dx * basis2.dfdx((x-xleft)/dx) / dx * t * t * c * c;
						return basis1.dfdx((x-xleft)/dx) / dx * basis2.dfdx((x-xleft)/dx) / dx * transformation->laplace_u1_1_times_d3xdu(u) * transformation->laplace_u1_2(u);
					}; //*/; };
					IntegratorGSL<> integratorGSL(f);
					return integratorGSL.integrate(xleft, xright);
				}  else {
					return nextJ.integrate_grad(i, j, xleft, xright, dx, transformation);
				}
			} else {
				return nextI.integrate_grad(i, j, xleft, xright, dx, transformation);
			}
		}
	};
	template<int I, class B1, class B2>
	struct selfintegrator<I, -1, B1, B2> {
		T integrate(int, int) {
			return 0;
		}
		T integrate_grad(int, int, double, double, double, Transformation1d_in_3d*) {
			return 0;
		}
	};
	template<int J, class B1, class B2>
	struct selfintegrator<-1, J, B1, B2> {
		T integrate(int, int) {
			return 0;
		}
		T integrate_grad(int, int, double, double, double, Transformation1d_in_3d* ) {
			return 0;
		}
	};

	double integrate_gradshape(int cell_index, int i, int j) {
		typedef selfintegrator<Basis::degree, Basis::degree> selfintegrator_type;
		selfintegrator_type si;
		T xleft = mesh.indexto_x(cell_index);
		T xright = mesh.indexto_x(cell_index+1);
		T dx = (xright-xleft);
		//cout << "integrate: " << cell_index << " i " << i << " "  << xleft << " to " << xright << endl;
		return si.integrate_grad(i, j, xleft, xright, dx, transformation);
	}

	template<class F>
	double integrate_shape(int cell_index, int i, F f) {
		T xleft = mesh.indexto_x(cell_index);
		T xright = mesh.indexto_x(cell_index+1);
		T dx = (xright-xleft);
		util<Basis::degree, Basis> integrator;
		//cout << "integrate shape: " << cell_index << " i " << i << " " << xleft << " to " << xright << endl;
		return integrator.integrate2(i, xleft, xright, dx, f);
		//return -integrator.integrate2(dof_per_cell-1-i, xright, xleft, dx, f);
	}

	double eval(VectorXd& solution, double x) {
		int cell_index = mesh.findindex(x);
		T xleft = mesh.indexto_x(cell_index);
		T xright = mesh.indexto_x(cell_index+1);
		T dx = xright-xleft;
		double v = 0;
		util<Basis::degree, Basis> util;
		if(mesh.inrange(x))
			v = util.eval(solution, dof_index(cell_index, dof_per_cell-1), xleft, dx, x);
		//cout << "eval: " << v << endl;
		return v;
		/*for(int i = 0; i < dof_per_cell; i++) {
			ev
		}*/
	}

	double gradient(VectorXd& solution, double x) {
		int cell_index = mesh.findindex(x);
		T xleft = mesh.indexto_x(cell_index);
		T xright = mesh.indexto_x(cell_index+1);
		T dx = xright-xleft;
		double v = 0;
		util<Basis::degree, Basis> util;
		if(mesh.inrange(x))
			v = util.gradient(solution, dof_index(cell_index, dof_per_cell-1), xleft, dx, x);
		//cout << "grad: " << " " << cell_index << " " << n_cells << " " << v << endl;
		return v;
		/*for(int i = 0; i < dof_per_cell; i++) {
			ev
		}*/
	}

	MeshRegularNodal1d(T _x1, T _x2, int n_cells, Transformation1d_in_3d* transformation) : x1(_x1), x2(_x2), n_cells(n_cells), mesh(_x1, _x2, n_cells), ctrans(1, 1), transformation(transformation) {
		if(Basis::degree == 0) {
			//dof_per_cell = 1;
			dof = n_cells;
		} else {
			dof = 1 + n_cells + (dof_per_cell-2)*n_cells; // 1 dof per border + dofs inside the cel
		}
		//cout << "n_cells = " << n_cells << " dof = " << dof << " dof_per_cell = " << dof_per_cell << endl;
		MatrixXd m = MatrixXd::Zero(dof, dof);
		ctrans.resize(dof, dof);
		T scale = (x2-x1)/n_cells;

		T integrals[dof_per_cell][dof_per_cell];
		for(int j = 0; j < dof_per_cell; j++) {
			for(int k = 0; k < (j+1); k++) {
				typedef selfintegrator<Basis::degree, Basis::degree> selfintegrator_type;
				selfintegrator_type si;
				double integral = si.integrate(j,k);
				//cout << j << " " << k << " " << integral << endl;
				integrals[j][k] = integral;
				integrals[k][j] = integral;
			}
		}
		for(int i = 0; i < n_cells; i++) {
			for(int j = 0; j < dof_per_cell; j++) {
				for(int k = 0; k < dof_per_cell; k++) {
					int i1 = i*(dof_per_cell-1)+j;
					int i2 = i*(dof_per_cell-1)+k;
					m(i1, i2) = m(i1, i2) + integrals[j][k];
				}
			}
		}
		//cout << m << endl;
		m = m * scale;
		ctrans = m.inverse();
	}
	void testprofile(Profile* profile, double_vector v, bool dotrans) {
		double* vp = v.data().begin();
		assert((int)v.size() == dof);
		double* array = vp;
		//int size = x.size();
		//double scale = 1./size;
		//cout << "size = " << size << endl;
		for(int i = 0; i < n_cells; i++) {
			for(int j = 0; j < dof_per_cell; j++) {
				T xleft = mesh.indexto_x(i);
				T xright = mesh.indexto_x(i+1);
				T dx = (xright-xleft);
				util<Basis::degree, Basis> integrator;
				double integral = integrator.integrate(j, xleft, xright, dx, profile);
				/**/
				cout << "> " << i << " " << j << " " << (i*(dof_per_cell-1)+j) << " " << integral << endl;
				//array[i*(dof_per_cell-1)+(dof_per_cell-1-j)] += integral;
				array[i*(dof_per_cell-1)+j] += integral;

			}
		}
		if(dotrans) {
			VectorXd v_alias = VectorXd::Map(v.data().begin(), v.size());
			VectorXd vtrans = ctrans * v_alias;
			VectorXd::Map(v.data().begin(), v.size()) = vtrans;
		}
	}

	void test(double_vector x, double_vector y, double_vector v, bool dotrans) {
		/*double* xp = x.data().begin();
		double* yp = y.data().begin();
		double* vp = v.data().begin();
		int size = x.size();
		double scale = 1./size * (x2-x1);
		//cout << "size = " << size << endl;
		for(int i = 0; i < size; i++) {
			//cout << "i = " << i << endl;
			this->operator()(xp[i], yp[i]*scale, vp);

		}
		if(dotrans) {
			VectorXd v_alias = VectorXd::Map(v.data().begin(), v.size());
			VectorXd vtrans = ctrans * v_alias;
			VectorXd::Map(v.data().begin(), v.size()) = vtrans;
		}*/
	}

	template<class A>
	T operator()(T x, T y, A array) {
		/*if(mesh.inrange(x)) {
			int indexleft = mesh.findindex(x);
			T xleft = mesh.indexto_x(indexleft);
			T xright = mesh.indexto_x(indexleft+1);
			T fraction = (x-xleft)/(xright-xleft);
			//cout << "x = " << x << " xleft = " << xleft << " xright = " << xright << " fraction = " << fraction << endl;
			BasisTriLeft<T> trileft;
			BasisTriRight<T> triright;
			//cout << "y = " << y << " triright(fraction) = " << triright(fraction) << " trileft (fraction-1) = " << trileft (fraction-1) << endl;
			array[indexleft]   +=  triright(fraction) * y;
			array[indexleft+1] +=  trileft (fraction-1) * y;
			// TODO: use trileft(fraction) + triright(fraction) == 1
		}*/
	}

};

}