/**********************************************************************************************************************
This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
Licensed under Apache2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#pragma once

#include <Eigen/Dense>

namespace ct {
namespace models {
namespace quadrotor {

const double pi = 3.14159265;

// mass / inertia
const double mQ = 0.546;         // mass of quadcopter [ kg ]
const double Thxxyy = 2.32e-3;   // moment of inertia around x,y [ kg*m^2 ]
const double Thzz = 3e-4;        // moment of inertia around z [ kg*m^2 ]
const double arm_len = 0.175;    // length of quadcopter arm [ m ]
const double grav_const = 9.81;  // gravitational constant [ m/s^2 ]

const double f_hover = mQ * grav_const;

// Thrust parameters
const double kF = 6.17092e-8 * 3600 / (2 * pi * 2 * pi);  // rotor thrust coefficient [ N/rad^2 ]
const double kM = 1.3167e-9 * 3600 / (2 * pi * 2 * pi);   // rotor moment coefficient [ Nm/rad^2]
const double wmax = 7800.0 * 2 * pi / 60;                 // maximum rotor speed [ rad/s ]
const double wmin = 1200.0 * 2 * pi / 60;                 // minimum rotor speed [ rad/s ]
const double Fsat_min = kF * wmin * wmin;
const double Fsat_max = kF * wmax * wmax;

const Eigen::Vector4d kFs(kF, kF, kF, kF);
const Eigen::Vector4d kMs(kM, kM, kM, kM);

}  // namespace quadrotor
}  // namespace models
}  // namespace ct


//multiprecision long factorials handling
#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>
namespace mp = boost::multiprecision;
using namespace std;

mp :: cpp_int factorial(int n){
    mp :: cpp_int u = 1;
     for(int i = 2; i <= n; i++){
          u *= i;
      }
    return u;
}
int main(){

    int T;
		cin>>T;
		// cin.ignore(); must be there when using getline(cin, s)
		while(T--){
		    int n, r;
		    cin >> n >> r;
		    mp :: cpp_int a = factorial(n);
		    mp :: cpp_int b = factorial(n - r);
		    mp :: cpp_int c = factorial(r);
		    cout << (a / (b * c)) % 1000000007 << '\n';

		}

    return 0;
}


//This code is an implementation of the Parallel ILU algorithm described in Chow and Patel (2015)
//"Fine-Grained Parallel Incomplete LU Factorization," SIAM J. Sci Comput., Vol. 37, No.2, C169-C193

//C++ and the Eigen library are used for sparse linear algebra, OpenMP is used for parallelization
//This implementation assumes the input of a square, symmetric, positive definite matrix
//Terminology used from paper: "ILU residual norm", "nonlinear residual norm"

//Developed by Marcos Botto Tornielli, May 2021

//####################################################################################################//
//######################## Parallel Incomplete LU Factorization ######################################//
//####################################################################################################//
#include <iostream>
#include <Eigen/Sparse>
#include <unsupported/Eigen/SparseExtra>
#include <iomanip>

int main()
{
    //dummy parallel region to use up the overhead from starting first region
#pragma omp parallel
    {
#pragma omp single
        std::cout << "This is thread #" << omp_get_thread_num() << std::endl;
    }

    //################################################################################################//
    //##################################### Matrix I/O ###############################################//
    //################################################################################################//

    //Define common Eigen objects to be used throughout the code
    typedef Eigen::SparseMatrix<double> SpMat;
    typedef Eigen::SparseMatrix<double,Eigen::RowMajor> SpMatRow;
    typedef Eigen::Triplet<double> T;

    //Define timing variables
    double t1,t2;

    //Load input matrix
    SpMat A;
    Eigen::loadMarket(A, "../input/Pres_Poisson/Pres_Poisson.mtx");

    //Matrix information as read from MatrixMarket file
    //Unknown loadMarket bug causes only lower triangular part of symmetric matrix to be read
    int rows = A.rows();
    int cols = A.cols();
    int nonzs = A.nonZeros();
    double norm = A.norm();
    bool compr = A.isCompressed();
    std::cout << "Matrix is compressed: " << compr << std::endl;
    std::cout << "Number of rows    : " << rows << std::endl;
    std::cout << "Number of columns : " << cols << std::endl;
    std::cout << "Number of nonzeros: " << nonzs << std::endl;
    std::cout << "Frobenius norm    : " << norm << std::endl << std::endl;

    //Workaround for loadMarket symmetry issue
    //Add A to its transpose; diagonal will be divided by 2 to compensate later
    SpMat A_t = A.transpose();
    A = A + A_t;
    //Matrix information for true A matrix
    rows = A.rows();
    cols = A.cols();
    nonzs = A.nonZeros();
    norm = A.norm();
    compr = A.isCompressed();
    std::cout << "Matrix is compressed: " << compr << std::endl;
    std::cout << "Number of rows    : " << rows << std::endl;
    std::cout << "Number of columns : " << cols << std::endl;
    std::cout << "Number of nonzeros: " << nonzs << std::endl;
    std::cout << "Frobenius norm    : " << norm << std::endl << std::endl;

    //################################################################################################//
    //################################ Diagonal Scaling of A #########################################//
    //################################################################################################//

    //Scale the diagonal down by 2 for loadMarket issue workaround
    //Compute values for diagonal scaling matrix D
    //Chow and Patel assume that the matrix used in the algorithm has been scaled to have unit diagonal

    std::vector<T> dTriplets;
    dTriplets.reserve(rows);
    for (int i=0; i<A.outerSize(); ++i)
    {
        for (Eigen::SparseMatrix<double>::InnerIterator it(A,i); it; ++it)
        {
            if (it.row() == it.col())
            {
                it.valueRef() = it.value()/2.0;
                double diag_entry = it.value();
                double scale_diag_entry = 1.0/sqrt(diag_entry);
                dTriplets.push_back(T(it.row(),it.row(),scale_diag_entry));
            }

        }
    }

    SpMat D(A.rows(),A.cols());
    D.setFromTriplets(dTriplets.begin(),dTriplets.end());

    //Scale A matrix for unit diagonal
    A = D*A*D;

    //################################################################################################//
    //##################### Define Sparsity Structure (not supported) ################################//
    //################################################################################################//

    //This section is included for potential future development
    //But the higher ILU Levels are not currently supported by the code (5/8/21)
    //Unless this support is added, the rest of the code assumes ILU(0) sparsity structure

    int ilu_level = 0;
    SpMat A_sparsity = A;

    for (int i=0; i<ilu_level; i++)
    {
        A_sparsity = A_sparsity*A;
    }

    //################################################################################################//
    //################### Construct Initial Guesses for L and U ######################################//
    //################################################################################################//

    //"Standard" initial guess used:
    //L guess is the strictly lower triangular part of A with an enforced unit diagonal
    //U guess is the upper triangular part of A (also has a unit diagonal because A does)

    std::vector<T> lTriplets;
    std::vector<T> uTriplets;

    lTriplets.reserve(((A.nonZeros() - A.rows())/2.0)+A.rows());
    uTriplets.reserve(((A.nonZeros() - A.rows())/2.0)+A.rows());

    for (int i=0; i<A.outerSize(); ++i)
    {
        for (Eigen::SparseMatrix<double>::InnerIterator it(A,i); it; ++it)
        {
            if (it.row() == it.col())
            {
                lTriplets.push_back(T(it.row(),it.col(),1.0));
            }

            if (it.row() > it.col())
            {
                lTriplets.push_back(T(it.row(),it.col(),it.value()));
            }else{
                uTriplets.push_back(T(it.row(),it.col(),it.value()));
            }

        }
    }

    SpMatRow L(A.rows(),A.cols());
    SpMat U(A.rows(),A.cols());
    L.setFromTriplets(lTriplets.begin(),lTriplets.end());
    U.setFromTriplets(uTriplets.begin(),uTriplets.end());

    //Calculate ILU residual norm for 0 sweeps
    SpMat resM = A - L*U;
    double ilu_res_norm = resM.norm();
    std::cout << "ILU residual norm,       0 sweeps: " << ilu_res_norm << std::endl;

    //Calculate nonlinear residual norm for 0 sweeps

    double nonl_res_norm = 0.0;

    t1 = omp_get_wtime();

#pragma omp parallel for reduction(+:nonl_res_norm) schedule(dynamic,8)
    for (int i=0; i<A.outerSize(); ++i)
    {
        for (Eigen::SparseMatrix<double>::InnerIterator it(A,i); it; ++it)
        {
            if (it.row() > it.col())
            {
                SpMat in_prod_mat = L.block(it.row(),0,1,it.col()+1) * U.block(0,it.col(),it.col()+1,1);
                double in_prod = in_prod_mat.coeffRef(0,0);
                nonl_res_norm = nonl_res_norm + abs(it.value() - in_prod);
            }else{
                SpMat in_prod_mat = L.block(it.row(),0,1,it.row()+1) * U.block(0,it.col(),it.row()+1,1);
                double in_prod = in_prod_mat.coeffRef(0,0);
                nonl_res_norm = nonl_res_norm + abs(it.value() - in_prod);
            }

        }
    }

    t2 = omp_get_wtime();

    std::cout << "Nonlinear residual norm, 0 sweeps: " << nonl_res_norm << std::endl;
    std::cout << "Time to compute (s)              : " << t2-t1 << std::endl << std::endl;

    //################################################################################################//
    //################################# MAIN ALGORITHM LOOP ##########################################//
    //################################################################################################//

    int n_sweeps = 5;

    for (int sweep=0; sweep<n_sweeps; sweep++)
    {
        t1 = omp_get_wtime();

#pragma omp parallel for schedule(dynamic,8)
        for (int i=0; i<A.outerSize(); ++i)
        {
            for (Eigen::SparseMatrix<double>::InnerIterator it(A,i); it; ++it)
            {
                if (it.row() > it.col())
                {
                    double div = 1.0/U.coeffRef(it.col(),it.col());
                    SpMat in_prod_mat = L.block(it.row(),0,1,it.col()) * U.block(0,it.col(),it.col(),1);
                    double in_prod = in_prod_mat.coeffRef(0,0);
                    L.coeffRef(it.row(),it.col()) = (it.value() - in_prod) * div;
                }else{
                    SpMat in_prod_mat = L.block(it.row(),0,1,it.row()) * U.block(0,it.col(),it.row(),1);
                    double in_prod = in_prod_mat.coeffRef(0,0);
                    U.coeffRef(it.row(),it.col()) = it.value() - in_prod;
                }

            }
        }

        t2 = omp_get_wtime();
        std::cout << "Time to compute sweep " << sweep+1 << " (s)      : " << t2-t1 << std::endl << std::endl;

        //Calculate ILU residual norm for current number of sweeps
        resM = A - L*U;
        ilu_res_norm = resM.norm();
        std::cout << "ILU residual norm,       " << sweep+1 << " sweeps: " << ilu_res_norm << std::endl;

        //Calculate nonlinear residual norm for current number of sweeps
        nonl_res_norm = 0.0;

        t1 = omp_get_wtime();

#pragma omp parallel for reduction(+:nonl_res_norm) schedule(dynamic,8)
        for (int i=0; i<A.outerSize(); ++i)
        {
            for (Eigen::SparseMatrix<double>::InnerIterator it(A,i); it; ++it)
            {
                if (it.row() > it.col())
                {
                    SpMat in_prod_mat = L.block(it.row(),0,1,it.col()+1) * U.block(0,it.col(),it.col()+1,1);
                    double in_prod = in_prod_mat.coeffRef(0,0);
                    nonl_res_norm = nonl_res_norm + abs(it.value() - in_prod);
                }else{
                    SpMat in_prod_mat = L.block(it.row(),0,1,it.row()+1) * U.block(0,it.col(),it.row()+1,1);
                    double in_prod = in_prod_mat.coeffRef(0,0);
                    nonl_res_norm = nonl_res_norm + abs(it.value() - in_prod);
                }

            }
        }

        t2 = omp_get_wtime();

        std::cout << "Nonlinear residual norm, " << sweep+1 << " sweeps: " << nonl_res_norm << std::endl;
        std::cout << "Time to compute (s)              : " << t2-t1 << std::endl << std::endl <<std::endl;

    }

    return 0;
}
//####################################################################################################//
//####################################################################################################//
//####################################################################################################//


#include <Engine/MeshEdit/AXAP.h>

#include <Engine/Primitive/TriMesh.h>

#include <Eigen/Sparse>
#include <Eigen/LU>
#include <Eigen/SVD>

using namespace Ubpa;

using namespace std;
using namespace Eigen;

AXAP::AXAP(Ptr<TriMesh> triMesh)
	: heMesh(make_shared<HEMesh<V>>())
{
	Init(triMesh);
}

void AXAP::Clear() {
	heMesh->Clear();
	triMesh = nullptr;
}

bool AXAP::Init(Ptr<TriMesh> triMesh) {
	Clear();

	if (triMesh == nullptr)
		return true;

	if (triMesh->GetType() == TriMesh::INVALID)
	{
		printf("ERRIR::Minsurf::Init:\n"
			"\t""trimesh is invalid\n");
		return false;
	}

	// init half-edge structure
	nV = triMesh->GetPositions().size();
	nT = triMesh->GetTriangles().size();
	vector<vector<size_t>> triangles;
	triangles.reserve(nT);
	for (auto triangle : triMesh->GetTriangles())
	{
		triangles.push_back({ triangle->idx[0], triangle->idx[1], triangle->idx[2] });
	}
	heMesh->Reserve(nV);
	heMesh->Init(triangles);

	if (!heMesh->IsTriMesh() || !heMesh->HaveBoundary())
	{
		printf("ERROR::MinSurf::Init:\n"
			"\t""trimesh is not a triangle mesh or hasn't a boundary\n");
		heMesh->Clear();
		return false;
	}

	// positions of triangle mesh -> positions of half-edge structure
	for (size_t i = 0; i < nV; i++)
	{
		auto v = heMesh->Vertices().at(i);
		v->pos = triMesh->GetPositions()[i].cast_to<vecf3>();
	}

	this->triMesh = triMesh;
	return true;
}

bool AXAP::Run() {
	if (heMesh->IsEmpty() || !triMesh)
	{
		printf("ERROR::Minsurf::Run\n"
			"\t""heMesh->IsEmpty() || !triMesh\n");
		return false;
	}

	InitPara();
	InitFlatTri();
	InitParaSolver();
	iter_count_ = 0;

	UpdateTriMesh();
	return true;
}

void AXAP::UpdateTriMesh()
{
	// half-edge structure -> triangle mesh
	vector<pointf3> positions;
	vector<unsigned> indice;
	vector<normalf> normals = vector<normalf>();
	vector<pointf2> texcoords;
	positions.reserve(nV);
	indice.reserve(3 * nT);
	normals.reserve(nV);
	texcoords.reserve(nV);
	for (size_t i = 0; i < nV; i++)
	{
		//positions.push_back(heMesh->Vertices().at(i)->pos.cast_to<pointf3>());
		positions.push_back({ para_solution_(i, 0), para_solution_(i, 1), 0 });
		texcoords.push_back({ para_solution_(i, 0), para_solution_(i, 1) });
	}
	for (auto f : heMesh->Polygons())
	{
		for (auto v : f->BoundaryVertice())
		{
			indice.push_back(static_cast<unsigned>(heMesh->Index(v)));
		}
	}

	triMesh->Init(indice, positions, normals, texcoords);
}

void AXAP::InitPara()
{
	SparseMatrix<float> A(nV, nV);
	MatrixXf B(nV, 2);
	// construct matrix A, B

	// Initialize
	for (size_t i = 0; i < nV; i++)
	{
		A.insert(i, i) = 0;
		B.row(i) = RowVector2f::Zero();
		auto vi = heMesh->Vertices().at(i);
		for (auto vj : vi->AdjVertices())
		{
			A.insert(i, heMesh->Index(vj)) = 0;
		}
	}

	// add cotangent weight
	for (size_t t = 0; t < nT; t++)
	{
		auto triangle = heMesh->Polygons().at(t);
		auto edge = triangle->HalfEdge();
		for (int k = 0; k < 3; k++, edge = edge->Next())
		{
			auto vi = edge->Origin();
			auto vj = edge->End();
			auto vk = edge->Next()->End();
			int i = heMesh->Index(vi);
			int j = heMesh->Index(vj);
			vecf3 eki = vi->pos - vk->pos;
			vecf3 ekj = vj->pos - vk->pos;
			float cot_theta = eki.cos_theta(ekj) / eki.sin_theta(ekj);
			A.coeffRef(i, i) += cot_theta;
			A.coeffRef(i, j) -= cot_theta;
			A.coeffRef(j, i) -= cot_theta;
			A.coeffRef(j, j) += cot_theta;
		}
	}

	// set boundary to given value
	size_t nB = heMesh->Boundaries()[0].size();
	float length_count = 0;
	for (size_t k = 0; k < nB; k++)
	{
		auto edge = heMesh->Boundaries()[0][k];
		double length = (edge->End()->pos - edge->Origin()->pos).norm();
		length_count += length;
	}

	bool flag = false;
	float length_pos = 0;
	for (size_t k = 0; k < nB; k++)
	{
		auto edge = heMesh->Boundaries()[0][k];
		auto vi = edge->Origin();
		size_t i = heMesh->Index(vi);
		float ratio = length_pos / length_count;
		if (k == 0)
		{
			start = i;
		}
		else if ((ratio >= 0.5) && !flag)
		{
			end = i;
			flag = true;
		}

		for (auto vj : vi->AdjVertices())
		{
			A.coeffRef(i, heMesh->Index(vj)) = 0;
		}
		A.coeffRef(i, i) = 1;
		if (ratio < 0.25)
		{
			B(i, 0) = ratio * 4;
			B(i, 1) = 0;
		}
		else if (ratio >= 0.25 && ratio < 0.5)
		{
			B(i, 0) = 1;
			B(i, 1) = (ratio - 0.25) * 4;
		}
		else if (ratio >= 0.5 && ratio < 0.75)
		{
			B(i, 0) = 1 - (ratio - 0.5) * 4;
			B(i, 1) = 1;
		}
		else
		{
			B(i, 0) = 0;
			B(i, 1) = 1 - (ratio - 0.75) * 4;
		}
		double length = (edge->End()->pos - edge->Origin()->pos).norm();
		length_pos += length;
	}

	// solve sparse linear equations
	SparseLU<SparseMatrix<float>> solver;
	solver.compute(A);
	para_solution_ = solver.solve(B);

	cout << "Success::Parametrize::Parametrize:" << endl
		<< "\t" << "parametrization successfully constructed" << endl;
}

void AXAP::InitFlatTri()
{
	flat_tri_ = (Matrix<float, 3, 2>*)malloc(nT * sizeof(Matrix<float, 3, 2>));
	if (flat_tri_ == nullptr)
	{
		cout << "No enough memory!" << endl;
	}

	for (size_t i = 0; i < nT; i++)
	{
		auto ti = heMesh->Polygons().at(i);
		auto vi0 = ti->HalfEdge()->Origin();
		auto vi1 = ti->HalfEdge()->End();
		auto vi2 = ti->HalfEdge()->Next()->End();
		vecf3 ei1 = vi1->pos - vi0->pos;
		vecf3 ei2 = vi2->pos - vi0->pos;
		double cos_theta = ei1.cos_theta(ei2);
		flat_tri_[i] = Matrix<float, 3, 2>();
		flat_tri_[i].row(0) = RowVector2f::Zero();
		flat_tri_[i].row(1) = RowVector2f(ei1.norm(), 0);
		flat_tri_[i].row(2) = RowVector2f(ei2.norm() * cos_theta, ei2.norm() * sqrt(1 - pow(cos_theta, 2)));
	}
}

void AXAP::InitParaSolver()
{
	SparseMatrix<float> A(nV, nV);
	for (size_t i = 0; i < nV; i++)
	{
		if (i == start || i == end)
		{
			A.insert(i, i) = 1;
		}
		else
		{
			auto vi = heMesh->Vertices().at(i);
			A.insert(i, i) = 0;
			for (auto vj : vi->AdjVertices())
			{
				A.insert(i, heMesh->Index(vj)) = 0;
			}
		}
	}

	// add cotangent weight
	for (size_t t = 0; t < nT; t++)
	{
		auto triangle = heMesh->Polygons().at(t);
		auto edge = triangle->HalfEdge();
		for (int k = 0; k < 3; k++, edge = edge->Next())
		{
			auto vi = edge->Origin();
			auto vj = edge->End();
			auto vk = edge->Next()->End();
			int i = heMesh->Index(vi);
			int j = heMesh->Index(vj);
			vecf3 eki = vi->pos - vk->pos;
			vecf3 ekj = vj->pos - vk->pos;
			float cot_theta = eki.cos_theta(ekj) / eki.sin_theta(ekj);
			if (i != start && i != end)
			{
				A.coeffRef(i, i) += cot_theta;
				A.coeffRef(i, j) -= cot_theta;
			}
			if (j != start && j != end)
			{
				A.coeffRef(j, i) -= cot_theta;
				A.coeffRef(j, j) += cot_theta;
			}
		}
	}
	para_solver_.compute(A);
}

Matrix2f AXAP::Jacobian(Matrix<float, 3, 2>& x, Matrix<float, 3, 2>& u)
{
	Matrix2f A = Matrix2f();
	Matrix2f B = Matrix2f();
	A.row(0) = x.row(1) - x.row(0);
	A.row(1) = x.row(2) - x.row(0);
	B.row(0) = u.row(1) - u.row(0);
	B.row(1) = u.row(2) - u.row(0);
	Matrix2f J =  A.lu().solve(B);
	return J;
}

bool AXAP::Iterate()
{
	return true;
}

void AXAP::UpdatePara()
{

}


#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/math/tools/solve.hpp>
#include <iostream>
#include <iomanip>
#include <regex>
#include <omp.h>
#include "../lib/prim2d.h"
#include "../lib/de2d.h"

#define ub  boost::numeric::ublas

#define DEBUG 2

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

    if( !argv[1] )
    {
        std::cout << "set the input file name\n";
        exit(1);
    }
    std::string fileName0(argv[1]);

    std::vector<Segm2D> sigma_t;
    sigma_t.reserve(1000);

    load_border(sigma_t, fileName0);
    size_t n = sigma_t.size();
    #if DEBUG > 1
       std::cout << "\033[31;1mn = " << n << "\033[0m\n";
    #endif

    bool isMove = true;
    int cycle = 0;
    double t = 0.0;
    double dt = 0.01;
    const double T = 5.;
    const double lambda = 0.5;
    const double alpha = 0.2;
    const Vector2D rM1 = Vector2D(2.5,0.0);
    const double q1 = -M_PI;
    const double T0 = 1.0;
    const double VEps = 0.1;
    const size_t max_cycle = 20;

    ub::matrix<double> A(n,n);
    ub::vector<double> f(n);
    ub::vector<double> g(n);

    for(;cycle<max_cycle&&isMove;++cycle)
    {

        // ***********   stage 1 *****************//

        double ts1 = omp_get_wtime();
        #pragma omp parallel for
        for(size_t i=0; i<n; ++i)
        {
            Vector2D rM = (sigma_t[i].A + sigma_t[i].B)/2.0;
            for(size_t j=0; j<n; ++j)
            {
                if(i!=j)
                {
                    Vector2D rN = 0.5*(sigma_t[j].A + sigma_t[j].B );
                    Vector2D nN = normal(sigma_t[j]);
                    A(i,j)=-2.0*lambda*Omega(rM,rN,nN)*dl(sigma_t[j]);
                }
                else A(i,j)=1.0;
            }
            f(i)=2.0*lambda*phi0(rM,q1,rM1)+2.0*alpha*rM.y;
        }
        double te1 = omp_get_wtime();
        std::cout << "\t\033[33mstage 1: "
                  << std::fixed << std::showpoint<< std::setprecision(10)
                  << te1-ts1 << " sec.\033[0m";

        // ***********   stage 2 *****************//

        double ts2 = omp_get_wtime();
        g = boost::math::tools::solve(A,f);
        double te2 = omp_get_wtime();
        std::cout << "\t\033[34mstage 2: "
                  << std::fixed << std::showpoint<< std::setprecision(5)
                  << te2-ts2 << " sec.\033[0m";

        // ***********   stage 3 *****************//

        std::vector<Vector2D> vA(n), vB(n);

        double ts3 = omp_get_wtime();
        #pragma omp parallel for
        for(size_t i=0; i<n; ++i){
            Vector2D rMA = sigma_t[i].A;
            vA[i] = v0(rMA,q1,rM1);
            for(size_t j=0; j<n; ++j)
                vA[i] += g(j)*vSegmTheta(rMA,sigma_t[j],VEps);
        }// for_i

        #pragma omp parallel for
        for(size_t i=0; i<n; ++i){
            sigma_t[i].A += vA[i]*dt;
            sigma_t[(i+1)%n].B = sigma_t[i].A;

        }// for_i

        double te3 = omp_get_wtime();
        std::cout << "\t\033[35mstage 3: "
                  << std::fixed << std::showpoint<< std::setprecision(10)
                  << te3-ts3 << " sec. \033[0m\n";

        t+=dt/T0;

        #if DEBUG > 1
        for( size_t k=1; k<5; ++k)
            if(cycle==(k*(max_cycle-1)/5))
            {
                std::stringstream ss;
                ss << k;
                std::string suffix;
                ss >> suffix;

                std::string outFileName = std::regex_replace(
                    fileName0,
                    std::regex("0"),
                    suffix
                );
                std::cout << "\033[31;1mcycle=" << std::setw(4) << cycle
                          << "\tsave " << outFileName
                          << "\t\tt = " << t
                          << "\033[0m\n";
                save_border(sigma_t, outFileName);
            }
        #endif

        // set your conditions for the variable isMove here:
        // if ( u cond. )  isMove = false;

    } // cycle

    std::cout <<  "\033[32;1msimulation time T = " << T << "\033[0m\n";
    std::string outFileName = std::regex_replace(
        fileName0,
        std::regex("0"),
        "_end"
    );
    save_border(sigma_t, outFileName);

}//main


#define _USE_MATH_DEFINES

#include <Eigen/Core>
#include "../matplotlibcpp.h"

namespace plt = matplotlibcpp;

Eigen::ArrayXd fun(const Eigen::ArrayXd& t)
{
    using Eigen::cos;
    using Eigen::exp;
    using Eigen::sin;
    return 0.0981 * cos(0.1 * t - 0.007845) - exp(-0.3999 * t) * (0.0981 * cos(3.169 * t) + 0.0124 * sin(3.169 * t));
}

int main()
{
    Eigen::VectorXd x = Eigen::VectorXd::LinSpaced(100, 0, 10);
    Eigen::VectorXd y = fun(x.array());
    plt::stl::plot(x.data(), x.data() + x.size(), y.data(),
                   {{"label", "Eigen VectorXd"}, {"marker", "o"}, {"markersize", "5"}, {"markerfacecolor", "#3efab0"}});
    plt::legend();
    plt::show();
}


#include <Eigen/Dense>
#include <vector>
#include <array>
#include <string>
#include <ros/ros.h>
#include <swerve_math/Swerve.h>
#include <swerve_point_generator/profiler.h>
#include <swerve_point_generator/FullGenCoefs.h>
#include <swerve_point_generator/GenerateSwerveProfile.h>
#include <talon_swerve_drive_controller/MotionProfile.h> //Only needed for visualization
#include <talon_swerve_drive_controller/MotionProfilePoints.h> //Only needed for visualization
#include <talon_swerve_drive_controller/WheelPos.h>

//Get swerve info here:

std::shared_ptr<swerve> swerve_math;
std::shared_ptr<swerve_profile::swerve_profiler> profile_gen;

ros::ServiceClient graph_prof;
ros::ServiceClient get_pos;
ros::ServiceClient graph_swerve_prof;
double defined_dt;

double f_v;
double f_s;
double f_a;
double f_s_s;
double f_s_v;

swerveVar::driveModel model;

bool full_gen(swerve_point_generator::FullGenCoefs::Request &req, swerve_point_generator::FullGenCoefs::Response &res)
{
	//ROS_ERROR("running point gen");

	std::array<double, WHEELCOUNT> curPos;
	talon_swerve_drive_controller::WheelPos pos_msg;
	if (!get_pos.call(pos_msg))
	{
		ROS_ERROR("failed to get wheel pos in point gen, maybe asked swerve drive controller before it was started up?");
		return false;
	}
	for (int i = 0; i < WHEELCOUNT; i++)
		curPos[i] = pos_msg.response.positions[i]; //TODO: FILL THIS OUT SOMEHOW
	const int k_p = 1;
	res.points.resize(155 / defined_dt);
	int prev_point_count = 0;
	talon_swerve_drive_controller::MotionProfile graph_msg;
	for (size_t s = 0; s < req.spline_groups.size(); s++)
	{
		int priv_num = 0;
		if (s > 0)
		{
			priv_num = req.spline_groups[s - 1];
		}
		const int n = round(req.wait_before_group[s] / defined_dt);
		std::vector<swerve_profile::spline_coefs> x_splines;
		std::vector<swerve_profile::spline_coefs> y_splines;
		std::vector<swerve_profile::spline_coefs> orient_splines;

		const int neg_x = req.x_invert[s] ? -1 : 1;
		std::vector<double> end_points_holder;
		double shift_by = 0;
		if (s != 0)
		{
			shift_by = req.end_points[priv_num - 1];
		}

		for (int i = priv_num; i < req.spline_groups[s]; i++)
		{
			ROS_INFO_STREAM("orient_coefs[" << i << "].spline=" << req.orient_coefs[i].spline[0] << " " <<
							req.orient_coefs[i].spline[1] << " " <<
							req.orient_coefs[i].spline[2] << " " <<
							req.orient_coefs[i].spline[3] << " " <<
							req.orient_coefs[i].spline[4] << " " <<
							req.orient_coefs[i].spline[5]);

			orient_splines.push_back(swerve_profile::spline_coefs(
										 req.orient_coefs[i].spline[0] * neg_x,
										 req.orient_coefs[i].spline[1] * neg_x,
										 req.orient_coefs[i].spline[2] * neg_x,
										 req.orient_coefs[i].spline[3] * neg_x,
										 req.orient_coefs[i].spline[4] * neg_x,
										 req.orient_coefs[i].spline[5] * neg_x));
			ROS_INFO_STREAM("orient_coefs[" << i << "].spline=" << orient_splines.back());

			ROS_INFO_STREAM("x_coefs[" << i << "].spline=" << req.x_coefs[i].spline[0] << " " <<
							req.x_coefs[i].spline[1] << " " <<
							req.x_coefs[i].spline[2] << " " <<
							req.x_coefs[i].spline[3] << " " <<
							req.x_coefs[i].spline[4] << " " <<
							req.x_coefs[i].spline[5]);

			x_splines.push_back(swerve_profile::spline_coefs(
									req.x_coefs[i].spline[0] * neg_x,
									req.x_coefs[i].spline[1] * neg_x,
									req.x_coefs[i].spline[2] * neg_x,
									req.x_coefs[i].spline[3] * neg_x,
									req.x_coefs[i].spline[4] * neg_x,
									req.x_coefs[i].spline[5] * neg_x));
			ROS_INFO_STREAM("x_coefs[" << i << "].spline=" << x_splines.back());

			ROS_INFO_STREAM("y_coefs[" << i << "].spline=" << req.y_coefs[i].spline[0] << " " <<
							req.y_coefs[i].spline[1] << " " <<
							req.y_coefs[i].spline[2] << " " <<
							req.y_coefs[i].spline[3] << " " <<
							req.y_coefs[i].spline[4] << " " <<
							req.y_coefs[i].spline[5]);

			y_splines.push_back(swerve_profile::spline_coefs(
									req.y_coefs[i].spline[0],
									req.y_coefs[i].spline[1],
									req.y_coefs[i].spline[2],
									req.y_coefs[i].spline[3],
									req.y_coefs[i].spline[4],
									req.y_coefs[i].spline[5]));
			ROS_INFO_STREAM("y_coefs[" << i << "].spline=" << y_splines.back());

			ROS_INFO_STREAM("hrer: " << req.end_points[i] - shift_by << " r_s: " <<  req.spline_groups[s] <<  " s: " << s);
			end_points_holder.push_back(req.end_points[i] - shift_by);
		}

		const double t_shift = req.t_shift[s];
		const bool flip_dirc = req.flip[s];

		swerve_point_generator::GenerateSwerveProfile::Response srv_msg; //TODO FIX THIS, HACK
		//srv_msg.points.resize(0);
		ROS_INFO_STREAM("req.initial_v: " << req.initial_v << " req.final_v: " << req.final_v << " t_shift: " << t_shift);
		profile_gen->generate_profile(x_splines, y_splines, orient_splines, req.initial_v, req.final_v, srv_msg, end_points_holder, t_shift, flip_dirc);
		const int point_count = srv_msg.points.size();
		//ROS_WARN("TEST2");

		graph_msg.request.joint_trajectory.header = srv_msg.header;

		res.dt = defined_dt;

		//ROS_INFO_STREAM("dt: " << res.dt);

		//ROS_WARN("BUFFERING");
		//TODO: optimize code?

		std::array<bool, WHEELCOUNT> holder;

		//Do first point and initialize stuff

		/*
		TODO: IMPLEMENT BELOW
		if(motion_profile_mode == steering_joints_[0].getMode())
		{
			for(size_t i = 0; i < WHEELCOUNT; i++)
			{
				speed_joints_[i].setCommand(0);
				steering_joints_[i].setCommand(0);
			}
		}
		*/
		//ROS_INFO_STREAM("pos_0:" << srv_msg.points[0].positions[0] << "pos_1:" << srv_msg.points[0].positions[1] <<"pos_2:" <<  srv_msg.points[0].positions[2]);

		// Bounds checking - not safe to proceed with setting up angle
		// positions and velocities if data is not as expected.
		if (srv_msg.points.size() < 2)
		{
			ROS_ERROR("Need at least 2 points");
			return false;
		}
		for (const auto point : srv_msg.points)
		{
			if (point.positions.size() < 3)
			{
				ROS_ERROR("Not enough positions in point");
				return false;
			}
		}
		const std::array<Eigen::Vector2d, WHEELCOUNT> angles_positions  = swerve_math->motorOutputs({srv_msg.points[1].positions[0] - srv_msg.points[0].positions[0], srv_msg.points[1].positions[1] - srv_msg.points[0].positions[1]}, srv_msg.points[1].positions[2] - srv_msg.points[0].positions[2], srv_msg.points[1].positions[2], false, holder, false, curPos, false);
		//TODO: angles on the velocity array below are superfluous, could remove
		//std::array<Eigen::Vector2d, WHEELCOUNT> angles_velocities  = swerve_math->motorOutputs({srv_msg.points[1].velocities[0], srv_msg.points[1].velocities[1]}, -srv_msg.points[1].velocities[2], /*srv_msg.points[1].positions[2]*/ M_PI / 2.0, false, holder, false, curPos, false);
		for (size_t k = 0; k < WHEELCOUNT; k++)
			curPos[k] = angles_positions[k][1];

		//ROS_INFO_STREAM("pos_0:" << srv_msg.points[i+1].positions[0] << "pos_1:" << srv_msg.points[i+1].positions[1] <<"pos_2:" <<  srv_msg.points[i+1].positions[2] << " counts: " << point_count << " i: "<< i << " wheels: " << WHEELCOUNT);
		std::array<double, WHEELCOUNT> vel_sum;
		for (int i = prev_point_count; i < n + prev_point_count; i++)
		{
			graph_msg.request.joint_trajectory.points.push_back(srv_msg.points[0]);

			for (size_t k = 0; k < WHEELCOUNT; k++)
			{
				res.points[i].hold.push_back(true);

				if (s == 0)
				{
					res.points[i].drive_pos.push_back(angles_positions[k][0]);
				}
				else
				{
					res.points[i].drive_pos.push_back(res.points[i - 1].drive_pos[k]);

				}
				//ROS_WARN("re");

				res.points[i].drive_f.push_back(0);
				vel_sum[k] = 0;

				res.points[i].steer_pos.push_back(angles_positions[k][1]);
				res.points[i].steer_f.push_back(0);
				//ROS_INFO_STREAM("drive_pos: " << res.points[i+1].drive_pos[k] << "drive_f: " << res.points[i+1].drive_vel[k] << "steer_pos: " << res.points[i+1].steer_pos[i]);
			}
		}

		graph_msg.request.joint_trajectory.points.insert(graph_msg.request.joint_trajectory.points.end(), srv_msg.points.begin(), srv_msg.points.end());

		std::array<double, WHEELCOUNT> prev_vels;
		std::array<double, WHEELCOUNT> prev_steer_pos;
		for (size_t k = 0; k < WHEELCOUNT; k++)
		{
			prev_vels[k] = 0;
			prev_steer_pos[k] = angles_positions[k][1];
		}
		for (int i = 0; i < point_count - k_p; i++)
		{
			const std::array<Eigen::Vector2d, WHEELCOUNT> angles_positions  = swerve_math->motorOutputs({srv_msg.points[i + 1].positions[0] - srv_msg.points[i].positions[0], srv_msg.points[i + 1].positions[1] - srv_msg.points[i].positions[1]}, srv_msg.points[i + 1].positions[2] - srv_msg.points[i].positions[2], srv_msg.points[i + 1].positions[2], false, holder, false, curPos, false);
			//TODO: angles on the velocity array below are superfluous, could remove
			std::array<Eigen::Vector2d, WHEELCOUNT> angles_velocities  = swerve_math->motorOutputs({srv_msg.points[i + 1].velocities[0], srv_msg.points[i + 1].velocities[1]}, srv_msg.points[i + 1].velocities[2], srv_msg.points[i + 1].positions[2], false, holder, false, curPos, false);
			for (size_t k = 0; k < WHEELCOUNT; k++)
				curPos[k] = angles_positions[k][1];

			//ROS_INFO_STREAM("pos_0:" << srv_msg.points[i+1].positions[0] << "pos_1:" << srv_msg.points[i+1].positions[1] <<"pos_2:" <<  srv_msg.points[i+1].positions[2] << " counts: " << point_count << " i: "<< i << " wheels: " << WHEELCOUNT);
			for (size_t k = 0; k < WHEELCOUNT; k++)
			{
				res.points[i + n + prev_point_count].hold.push_back(false);

				//ROS_WARN("hhhhere");
				if (i != 0 || n != 0 || s != 0)
				{
					res.points[i + n + prev_point_count].drive_pos.push_back(angles_positions[k][0] + res.points[i + n - 1 + prev_point_count].drive_pos[k]);
				}
				else
				{
					res.points[i + n + prev_point_count ].drive_pos.push_back(angles_positions[k][0]);
				}

				if (i > point_count - k_p - 2)
				{
					//ROS_INFO_STREAM("final pos" << angles_positions[k][0] + res.points[i + n - 1 + prev_point_count].drive_pos[k]);
					//ROS_INFO_STREAM("vel sum" << vel_sum[k]);
					res.points[i + n + prev_point_count].drive_f.push_back(0);
					res.points[i + n + prev_point_count].steer_f.push_back(0);
				}
				else
				{
					int sign_v = angles_velocities[k][0] < 0 ? -1 : angles_velocities[k][0] > 0 ? 1 : 0;
					res.points[i + n + prev_point_count].drive_f.push_back(angles_velocities[k][0] * f_v + sign_v * f_s + f_a /* / ( -fabs(angles_velocities[k][0]) / (model.maxSpeed * 1.2) + 1.05 ) */ * (angles_velocities[k][0] - prev_vels[k]) / defined_dt);
					prev_vels[k] = angles_velocities[k][0];
					vel_sum[k] += angles_velocities[k][0];

					const double steer_v = (angles_positions[k][1] - prev_steer_pos[k]) / defined_dt;
					const int sign_steer_v = steer_v < 0 ? -1 : steer_v > 0 ? 1 : 0;
					res.points[i + n + prev_point_count].steer_f.push_back(steer_v * f_s_v + sign_steer_v * f_s_s);
				}

				res.points[i + n + prev_point_count].steer_pos.push_back(angles_positions[k][1]);

				prev_steer_pos[k] = angles_positions[k][1];

				//ROS_INFO_STREAM("drive_pos: " << res.points[i+n+prev_point_count].drive_pos[k] << "drive_f: " << res.points[i+n+prev_point_count].drive_f[k] << "steer_pos: " << res.points[i+n+prev_point_count].steer_pos[k]);
			}
		}
		prev_point_count += point_count + n - k_p;
		//ROS_ERROR_STREAM("l: " <<  prev_point_count << " P: " << point_count);
	}

	graph_prof.call(graph_msg);
	res.points.erase(res.points.begin() + prev_point_count, res.points.end());
	ROS_INFO_STREAM("profile time: " << res.points.size() * defined_dt);

	res.joint_trajectory = graph_msg.request.joint_trajectory;

	//talon_swerve_drive_controller::MotionProfilePoints graph_swerve_msg;
	//graph_swerve_msg.request.points = res.points;
	//graph_swerve_prof.call(graph_swerve_msg);
	//ROS_WARN("FIN");
	return true;
}
int main(int argc, char **argv)
{
	ros::init(argc, argv, "point_gen");
	ros::NodeHandle nh;

	ros::NodeHandle controller_nh(nh, "swerve_drive_controller");

	//double wheel_radius;
	bool invert_wheel_angle;
	swerveVar::ratios drive_ratios;
	swerveVar::encoderUnits units;
	double max_accel;
	double max_brake_accel;
	double ang_accel_conv;

	if (!controller_nh.getParam("f_s", f_s))
		ROS_ERROR("Could not read f_s in point gen");
	if (!controller_nh.getParam("f_a", f_a))
		ROS_ERROR("Could not read f_a in point gen");
	if (!controller_nh.getParam("f_v", f_v))
		ROS_ERROR("Could not read f_v in point gen");
	if (!controller_nh.getParam("f_s_v", f_s_v))
		ROS_ERROR("Could not read f_s_v in point gen");
	if (!controller_nh.getParam("f_s_s", f_s_s))
		ROS_ERROR("Could not read f_s_s in point gen");

	if (!controller_nh.getParam("wheel_radius", model.wheelRadius))
		ROS_ERROR("Could not read wheel_radius in point_gen");
	if (!controller_nh.getParam("max_accel", max_accel))
		ROS_ERROR("Could not read max_accel in point_gen");
	if (!controller_nh.getParam("max_brake_accel", max_brake_accel))
		ROS_ERROR("Could not read max_brake_accel in point_gen");
	if (!controller_nh.getParam("ang_accel_conv", ang_accel_conv))
		ROS_ERROR("Could not read ang_accel_conv in point_gen");
	if (!controller_nh.getParam("max_speed", model.maxSpeed))
		ROS_ERROR("Could not read max_speed in point_gen");
	if (!controller_nh.getParam("mass", model.mass))
		ROS_ERROR("Could not read mass in point_gen");
	if (!controller_nh.getParam("motor_free_speed", model.motorFreeSpeed))
		ROS_ERROR("Could not read motor_free_speed in point_gen");
	if (!controller_nh.getParam("motor_stall_torque", model.motorStallTorque))
		ROS_ERROR("Could not read motor_stall_torque in point_gen");
	// TODO : why not just use the number of wheels read from yaml?
	if (!controller_nh.getParam("motor_quantity", model.motorQuantity))
		ROS_ERROR("Could not read motor_quantity in point_gen");
	if (!controller_nh.getParam("invert_wheel_angle", invert_wheel_angle))
		ROS_ERROR("Could not read invert_wheel_angle in point_gen");
	if (!controller_nh.getParam("ratio_encoder_to_rotations", drive_ratios.encodertoRotations))
		ROS_ERROR("Could not read ratio_encoder_to_rotations in point_gen");
	if (!controller_nh.getParam("ratio_motor_to_rotations", drive_ratios.motortoRotations))
		ROS_ERROR("Could not read ratio_motor_to_rotations in point_gen");
	if (!controller_nh.getParam("ratio_motor_to_steering", drive_ratios.motortoSteering))
		ROS_ERROR("Could not read ratio_motor_to_steering in point_gen");
	if (!controller_nh.getParam("encoder_drive_get_V_units", units.rotationGetV))
		ROS_ERROR("Could not read encoder_drive_get_V_units in point_gen");
	if (!controller_nh.getParam("encoder_drive_get_P_units", units.rotationGetP))
		ROS_ERROR("Could not read encoder_drive_get_P_units in point_gen");
	if (!controller_nh.getParam("encoder_drive_set_V_units", units.rotationSetV))
		ROS_ERROR("Could not read encoder_drive_set_V_units in point_gen");
	if (!controller_nh.getParam("encoder_drive_set_P_units", units.rotationSetP))
		ROS_ERROR("Could not read encoder_drive_set_P_units in point_gen");
	if (!controller_nh.getParam("encoder_steering_get_units", units.steeringGet))
		ROS_ERROR("Could not read encoder_steering_get_units in point_gen");
	if (!controller_nh.getParam("encoder_steering_set_units", units.steeringSet))
		ROS_ERROR("Could not read encoder_steering_set_units in point_gen");
	std::array<Eigen::Vector2d, WHEELCOUNT> wheel_coords;
	if (!controller_nh.getParam("wheel_coords1x", wheel_coords[0][0]))
		ROS_ERROR("Could not read wheel_coords1x in point_gen");
	if (!controller_nh.getParam("wheel_coords2x", wheel_coords[1][0]))
		ROS_ERROR("Could not read wheel_coords2x in point_gen");
	if (!controller_nh.getParam("wheel_coords3x", wheel_coords[2][0]))
		ROS_ERROR("Could not read wheel_coords3x in point_gen");
	if (!controller_nh.getParam("wheel_coords4x", wheel_coords[3][0]))
		ROS_ERROR("Could not read wheel_coords4x in point_gen");
	if (!controller_nh.getParam("wheel_coords1y", wheel_coords[0][1]))
		ROS_ERROR("Could not read wheel_coords1y in point_gen");
	if (!controller_nh.getParam("wheel_coords2y", wheel_coords[1][1]))
		ROS_ERROR("Could not read wheel_coords2y in point_gen");
	if (!controller_nh.getParam("wheel_coords3y", wheel_coords[2][1]))
		ROS_ERROR("Could not read wheel_coords3y in point_gen");
	if (!controller_nh.getParam("wheel_coords4y", wheel_coords[3][1]))
		ROS_ERROR("Could not read wheel_coords4y in point_gen");

	//ROS_WARN("point_init");
	//ROS_INFO_STREAM("model max speed: " << model.maxSpeed << " radius: " << model.wheelRadius);

	XmlRpc::XmlRpcValue wheel_list;
	controller_nh.getParam("steering", wheel_list);

	std::vector<std::string> wheel_names;
	wheel_names.resize(wheel_list.size());
	for (int i = 0; i < wheel_list.size(); ++i)
	{
		wheel_names[i] = static_cast<std::string>(wheel_list[i]);
	}
	std::vector<double> offsets;
	for (auto it = wheel_names.cbegin(); it != wheel_names.cend(); ++it)
	{
		ros::NodeHandle nh(controller_nh, *it);
		double dbl_val = 0;
		if (!nh.getParam("offset", dbl_val))
			ROS_ERROR_STREAM("Can not read offset for " << *it);
		offsets.push_back(dbl_val);
	}

	swerve_math = std::make_shared<swerve>(wheel_coords, offsets, invert_wheel_angle, drive_ratios, units, model);
	defined_dt = .02;
	profile_gen = std::make_shared<swerve_profile::swerve_profiler>(hypot(wheel_coords[0][0], wheel_coords[0][1]), max_accel, model.maxSpeed, 1, 1, defined_dt, ang_accel_conv, max_brake_accel); //Fix last val
	//Something to get intial wheel position

	std::map<std::string, std::string> service_connection_header;
	service_connection_header["tcp_nodelay"] = "1";
	graph_prof = nh.serviceClient<talon_swerve_drive_controller::MotionProfile>("/visualize_profile", false, service_connection_header);
	graph_swerve_prof = nh.serviceClient<talon_swerve_drive_controller::MotionProfilePoints>("/visualize_swerve_profile", false, service_connection_header);

	ros::service::waitForService("swerve_drive_controller/wheel_pos");
	ROS_ERROR("DONE WAITING FOR wheel_pos");
	get_pos = nh.serviceClient<talon_swerve_drive_controller::WheelPos>("swerve_drive_controller/wheel_pos", false, service_connection_header);

	// Once everything this node needs is available, open
	// it up to connections from the outside
        //ROS_ERROR("BEFORE advertiseService");
	ros::ServiceServer service = nh.advertiseService("/point_gen/command", full_gen);
	//ROS_ERROR("AFTER advertiseService");

	ros::spin();
}


#include <qpOASES.hpp>
#include <iostream>
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>
//#include <OptimalControl/ControlsLibrary.hpp>

using namespace qpOASES;

using Eigen::MatrixXd;

int main()
{
    /*
    MatrixXd A(2,2);
    MatrixXd B(2,1);
    A(0,0) = 1;
    A(0,1) = 1;
    A(1,1) = 1;
    A(1,0) = 1;
    B(0,0) = 10;
    B(1,0) = 10;

    MatrixXd A_d, B_d;

    ControlsLibrary::ContinuousToDiscrete(A, B, 0.1, A_d, B_d);

    cout <<A_d << endl << B_d << endl; */

    // QProblem qp(2,1);

    // MatrixXd H(2,2);
    // MatrixXd A(1,2);
    // MatrixXd g(2,1);
    // MatrixXd lbA(1,1);

    // H = 2*MatrixXd::Identity(2,2);
    // A = MatrixXd::Ones(1,2);
    // g = MatrixXd::Zero(2,1);

    // lbA << 10;
    // /*

    // H << 0.4, 0,
    //      0, 1;
    // H = 2 * H;
    // cout << H << endl;
    // A << 1, -1,
    //      -0.3, -1;
    // ubA << -2, -8;
    // g << -5, -6;
    // lb << 0, 0;
    // ub << 10, 10; */

    // cout << H << endl;
    // cout << A << endl;
    // cout << lbA << endl;
    // cout << g << endl;

    // int nWSR = 10;
    // qp.setPrintLevel(qpOASES::PL_MEDIUM);
    // qp.init(H.data(), g.data(), A.data(), NULL, NULL, lbA.data(), NULL, nWSR);
    // MatrixXd x_out(2,1);

    // qp.getPrimalSolution(x_out.data());

    // cout << x_out << endl;
    //cout << qp.getObjVal() << endl;

     qpOASES::int_t vars = 2;
     qpOASES::int_t cons = 2;

    QProblem qp(vars,cons);

     Eigen::Matrix<double, 2, 2, Eigen::RowMajor> H;
     Eigen::Matrix<double, 2, 2, Eigen::RowMajor> A;

   // MatrixXd H(2,2);
   // MatrixXd A(2,2);
    MatrixXd g(2,1);
    MatrixXd ubA(2,1);
    //MatrixXd lbX(2,1);
    //MatrixXd ubX(2,1);

    H << 0.4, 0.0,
         0.0, 1.0;

    H = 2 * H;

    A << 1.0, -1.0,
         -0.3, -1.0;

    ubA << -2, -8;

    g << -5, -6;
    //lbX << 0, 0;
    //ubX << 10, 10;

    std::cout << H << std::endl;
    std::cout << H.data()[0] << std::endl;
    std::cout << H.data()[1] << std::endl;
    std::cout << H.data()[2] << std::endl;
    std::cout << H.data()[3] << std::endl;
    std::cout << A << std::endl;

    std::cout << A.data()[0] << std::endl;
    std::cout << A.data()[1] << std::endl;
    std::cout << A.data()[2] << std::endl;
    std::cout << A.data()[3] << std::endl;

    std::cout << g << std::endl;
 //   cout << ubA << endl;
    std::cout << ubA << std::endl;
    //cout << lbX << endl;
    //cout << ubX << endl;

     qpOASES::Options myOptions;
     //myOptions.enableRamping = BT_FALSE;
     //myOptions.maxPrimalJump = 1;
     //myOptions.setToMPC();
    qpOASES::int_t nWSR = 10;
    qp.setPrintLevel(qpOASES::PL_HIGH);
    qp.setOptions(myOptions);
    qp.init(H.data(), g.data(), A.data(), NULL, NULL, NULL, ubA.data(), nWSR);
    MatrixXd x_out(2,1);

    qp.getPrimalSolution(x_out.data());
    // qp.printOptions();
    std::cout << x_out << std::endl;
}


#include "stdafx.h"

#include "problem.hpp"

#include <fstream>
#include <functional>
#include <vector>

#include <boost/algorithm/string.hpp>

struct advent_2015_2 : problem
{
	advent_2015_2() noexcept : problem(2015, 2) {
	}

	struct box
	{
		std::size_t l, w, h;
	};

protected:
	std::vector<box> boxes;

	void prepare_input(std::ifstream& fin) override {
		for(std::string line; std::getline(fin, line); ) {
			std::vector<std::string> fragments;
			boost::split(fragments, line, [](char ch) { return ch == 'x'; });
			boxes.push_back(box{ std::stoull(fragments[0]), std::stoull(fragments[1]), std::stoull(fragments[2]) });
		}
	}

	std::size_t get_paper_area(box b) {
		const std::size_t areas[] = { b.l * b.w, b.w * b.h, b.h * b.l };
		const std::size_t smallest = *std::min_element(std::begin(areas), std::end(areas));
		return 2 * std::accumulate(std::begin(areas), std::end(areas), 0ui64) + smallest;
	}

	std::size_t get_ribbon_length(box b) noexcept {
		const std::size_t semiperimeters[] = { b.l + b.w, b.w + b.h, b.h + b.l };
		const std::size_t smallest = *std::min_element(std::begin(semiperimeters), std::end(semiperimeters));
		return (2 * smallest) + (b.l * b.w * b.h);
	}

	std::string part_1() override {
		std::size_t paper_required = 0;
		for(const box b : boxes) {
			paper_required += get_paper_area(b);
		}
		return std::to_string(paper_required);
	}

	std::string part_2() override {
		std::size_t ribbon_required = 0;
		for(const box b : boxes) {
			ribbon_required += get_ribbon_length(b);
		}
		return std::to_string(ribbon_required);
	}
};

REGISTER_SOLVER(2015, 2);


#include <math.h>
#include <float.h>

#include <Eigen/Sparse>

#include "HarmonicMap.h"

#ifndef M_PI
#define M_PI 3.141592653589793238462643383279
#endif

void MeshLib::CHarmonicMap::set_mesh(CHarmonicMapMesh* pMesh)
{
    m_pMesh = pMesh;

    // 1. compute the weights of edges
    _calculate_edge_weight();

    // 2. map the boundary to unit circle
    _set_boundary();

    // 3. initialize the map of interior vertices to (0, 0)
    using M = CHarmonicMapMesh;
    for (M::MeshVertexIterator viter(m_pMesh); !viter.end(); ++viter)
    {
        M::CVertex* pV = *viter;
        if (pV->boundary())
            continue;
		//insert your code here
        pV->uv() = CPoint2(0, 0);
    }
}

double MeshLib::CHarmonicMap::step_one()
{
    if (!m_pMesh)
    {
        std::cerr << "Should set mesh first!" << std::endl;
        return DBL_MAX;
    }

    using M = CHarmonicMapMesh;

    // move each interior vertex to its weighted center of neighbors
    double max_error = -DBL_MAX;
    for (M::MeshVertexIterator viter(m_pMesh); !viter.end(); ++viter)
    {
        M::CVertex* pV = *viter;
        if (pV->boundary())
            continue;

        double sw = 0;
        CPoint2 suv(0, 0);
        for (M::VertexVertexIterator vviter(pV); !vviter.end(); vviter++)
        {
            M::CVertex* pW = *vviter;
            M::CEdge* pE = m_pMesh->vertexEdge(pV, pW);
			//insert your code here
            suv += pW->uv() * pE->weight();
            sw += pE->weight();
        }
        suv /= sw;

        double error = (pV->uv() - suv).norm();
        max_error = (error > max_error) ? error : max_error;
        pV->uv() = suv;
    }

    printf("Current max error is %g\n", max_error);
    return max_error;
}

void MeshLib::CHarmonicMap::iterative_map(double epsilon)
{
    if (!m_pMesh)
    {
        std::cerr << "Should set mesh first!" << std::endl;
        return;
    }

    using M = CHarmonicMapMesh;

    // take steps until it converges.
    while (true)
    {
        double error = this->step_one();
        if (error < epsilon)
            break;
    }
}

void MeshLib::CHarmonicMap::map()
{
    if (!m_pMesh)
    {
        std::cerr << "Should set mesh first!" << std::endl;
        return;
    }

    using M = CHarmonicMapMesh;

    // 1. Initialize
    int vid = 0;  // interior vertex id
    int bid = 0;  // boundary vertex id
    for (M::MeshVertexIterator viter(m_pMesh); !viter.end(); ++viter)
    {
        M::CVertex* pV = *viter;

        if (pV->boundary())
            pV->idx() = bid++;
        else
            pV->idx() = vid++;
    }

    int interior_vertices = vid;
    int boundary_vertices = bid;

    // 2. Set the matrix A and B
    std::vector<Eigen::Triplet<double>> A_coefficients;
    std::vector<Eigen::Triplet<double>> B_coefficients;

    for (M::MeshVertexIterator viter(m_pMesh); !viter.end(); ++viter)
    {
        M::CVertex* pV = *viter;
        if (pV->boundary())
            continue;
        int vid = pV->idx();

        double sw = 0;
        for (M::VertexVertexIterator witer(pV); !witer.end(); ++witer)
        {
            M::CVertex* pW = *witer;
            int wid = pW->idx();

            M::CEdge* e = m_pMesh->vertexEdge(pV, pW);
            double w = e->weight();

			//insert your code here
			//construct one element of the matrix A and B, using
            sw += w;
			//there push_back the triplet to A or B coefficients
            if (pW->boundary())
            {
                B_coefficients.push_back(Eigen::Triplet<double>(vid, wid, w));
            }
            else
            {
                A_coefficients.push_back(Eigen::Triplet<double>(vid, wid, -1 * w));
            }
        }
		//insert the diagonal element
        A_coefficients.push_back(Eigen::Triplet<double>(vid, vid, sw));

    }

    Eigen::SparseMatrix<double> A(interior_vertices, interior_vertices);
    A.setZero();
    Eigen::SparseMatrix<double> B(interior_vertices, boundary_vertices);
    B.setZero();
    A.setFromTriplets(A_coefficients.begin(), A_coefficients.end());
    B.setFromTriplets(B_coefficients.begin(), B_coefficients.end());

    // 3. Solve the equations
    Eigen::ConjugateGradient<Eigen::SparseMatrix<double>> solver;
    std::cerr << "Eigen Decomposition" << std::endl;
    solver.compute(A);
    std::cerr << "Eigen Decomposition Finished" << std::endl;

    if (solver.info() != Eigen::Success)
    {
        std::cerr << "Waring: Eigen decomposition failed" << std::endl;
    }

    for (int k = 0; k < 2; k++)
    {
        Eigen::VectorXd b(boundary_vertices);
        // set boundary constraints vector b
        for (M::MeshVertexIterator viter(m_pMesh); !viter.end(); ++viter)
        {
            M::CVertex* pV = *viter;
            if (!pV->boundary())
                continue;
            int id = pV->idx();
            b(id) = pV->uv()[k];
        }

        Eigen::VectorXd c(interior_vertices);
        c = B * b;

        Eigen::VectorXd x = solver.solve(c); // Ax=c
        if (solver.info() != Eigen::Success)
        {
            std::cerr << "Waring: Eigen decomposition failed" << std::endl;
        }

        // set the images of the harmonic map to interior vertices
        for (M::MeshVertexIterator viter(m_pMesh); !viter.end(); ++viter)
        {
            M::CVertex* pV = *viter;
            if (pV->boundary())
                continue;
            int id = pV->idx();
            pV->uv()[k] = x(id);
        }
    }
}

void MeshLib::CHarmonicMap::_calculate_edge_weight()
{
    using M = CHarmonicMapMesh;

    // 1. compute edge length
    for (M::MeshEdgeIterator eiter(m_pMesh); !eiter.end(); ++eiter)
    {
        M::CEdge* pE = *eiter;
        M::CVertex* v1 = m_pMesh->edgeVertex1(pE);
        M::CVertex* v2 = m_pMesh->edgeVertex2(pE);
        pE->length() = (v1->point() - v2->point()).norm();
    }

    // 2. compute corner angle
    for (M::MeshFaceIterator fiter(m_pMesh); !fiter.end(); ++fiter)
    {
        M::CFace* pF = *fiter;
        M::CHalfEdge* pH[3];
		//insert your code here
        int i = 0;
        for (M::FaceHalfedgeIterator fhiter(pF); !fhiter.end(); ++fhiter)
        {
            pH[i] = *fhiter;
            i++;
            assert(i < 4);
        }
        assert(i == 3);
        assert(pH[0]->target() == pH[1]->source());
        assert(pH[1]->target() == pH[2]->source());
        assert(pH[2]->target() == pH[0]->source());
		//use inverse cosine law to compute the corner angles
        pH[0]->angle() = _inverse_cosine_law(static_cast<CHarmonicMapEdge *>(pH[1]->edge())->length(), static_cast<CHarmonicMapEdge*>(pH[2]->edge())->length(), static_cast<CHarmonicMapEdge*>(pH[0]->edge())->length());
        pH[1]->angle() = _inverse_cosine_law(static_cast<CHarmonicMapEdge*>(pH[0]->edge())->length(), static_cast<CHarmonicMapEdge*>(pH[2]->edge())->length(), static_cast<CHarmonicMapEdge*>(pH[1]->edge())->length());
        pH[2]->angle() = _inverse_cosine_law(static_cast<CHarmonicMapEdge*>(pH[1]->edge())->length(), static_cast<CHarmonicMapEdge*>(pH[0]->edge())->length(), static_cast<CHarmonicMapEdge*>(pH[2]->edge())->length());
    }

    // 3. compute edge weight
    for (M::MeshEdgeIterator eiter(m_pMesh); !eiter.end(); ++eiter)
    {
        M::CEdge* pE = *eiter;
		//insert your code here
        if (pE->halfedge(0) != NULL)
        {
            pE->weight() += std::tan(M_PI / 2 - static_cast<CHarmonicMapHalfEdge*>(pE->halfedge(0))->angle());
        }
        if (pE->halfedge(1) != NULL)
        {
            pE->weight() += std::tan(M_PI / 2 - static_cast<CHarmonicMapHalfEdge*>(pE->halfedge(1))->angle());
        }
		//set cotangent edge weight
    }
}

void MeshLib::CHarmonicMap::_set_boundary()
{
    using M = CHarmonicMapMesh;

    // 1. get the boundary half edge loop
    M::CBoundary boundary(m_pMesh);
    std::vector<M::CLoop*>& pLs = boundary.loops();
    if (pLs.size() != 1)
    {
        std::cerr << "Only topological disk accepted!" << std::endl;
        exit(EXIT_FAILURE);
    }
    M::CLoop* pL = pLs[0];
    std::list<M::CHalfEdge*>& pHs = pL->halfedges();

    // 2. compute the total length of the boundary
    double sum = 0.0;
    std::list<M::CHalfEdge*>::iterator it;
    for (it = pHs.begin(); it != pHs.end(); ++it)
    {
        M::CHalfEdge* pH = *it;
        sum += m_pMesh->halfedgeEdge(pH)->length();
    }

    // 3. parameterize the boundary using arc length parameter
    double len = 0.0;
    for (it = pHs.begin(); it != pHs.end(); ++it)
    {
        M::CHalfEdge* pH = *it;
        M::CVertex* pV = m_pMesh->halfedgeVertex(pH);

        len += m_pMesh->halfedgeEdge(pH)->length();
        double angle = len / sum * 2.0 * M_PI;
        pV->uv() = CPoint2(cos(angle), sin(angle));
    }
}

double MeshLib::CHarmonicMap::_inverse_cosine_law(double a, double b, double c)
{
    double cs = (a * a + b * b - c * c) / (2.0 * a * b);
    assert(cs <= 1.0 && cs >= -1.0);
    return std::acos(cs);
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2010 Sebastian Schlenkrich

*/

#ifndef quantlib_gausslobatto_hpp
#define quantlib_gausslobatto_hpp

#include <ql/types.hpp>
#include <boost/function.hpp>

namespace TemplateAuxilliaries {

    //! Template for Gauss-Lobatto integration, see gausslobattointegral.hpp/.cpp
    template <class Type>
    class GaussLobatto {
    private:
        //! local types
        typedef double Real;
        //! attributes
        mutable size_t maxEvaluations_;
        mutable Type absAccuracy_;
        mutable Type relAccuracy_;
        mutable Type absError_;
        mutable size_t evaluations_;
        const bool useConvergenceEstimate_;
        //! constants
        const static double alpha_, beta_, x1_, x2_, x3_;
        //! specific integration routines
        Type calculateAbsTolerance(const boost::function<Type (Type)>& f, Type a, Type b) const;
        Type adaptivGaussLobattoStep(const boost::function<Type (Type)>& f, Type a, Type b, Type fa, Type fb, Type acc) const;
    public:
        //! constructor
        GaussLobatto( size_t maxEvaluations,
                      Type absAccuracy,
                      Type relAccuracy = 0,
                      bool useConvergenceEstimate = true )
            : maxEvaluations_(maxEvaluations), absAccuracy_(absAccuracy), relAccuracy_(relAccuracy),
              absError_(0), evaluations_(0), useConvergenceEstimate_(useConvergenceEstimate) {}
        //! inspectors
        Type absoluteAccuracy() const { return absAccuracy_;      }
        Type absoluteError()    const { return absError_;         }
        size_t maxEvaluations()   const { return maxEvaluations_;   }
        size_t evaluations()      const { return evaluations_;      }
        //! modifiers
        void setNumberOfEvaluations(size_t number)         const { evaluations_ = number; }
        void increaseNumberOfEvaluations(size_t increase)  const { evaluations_ += increase; }
        //! integrate function interface
        Type integrate(const boost::function<Type (Type)>& f, Type a, Type b) const {
            //setNumberOfEvaluations(0);
            const Type calcAbsTolerance = calculateAbsTolerance(f, a, b);
            //increaseNumberOfEvaluations(2);
            return adaptivGaussLobattoStep(f, a, b, f(a), f(b), calcAbsTolerance);
        }
    };

    template <class Type> const double GaussLobatto<Type>::alpha_ = std::sqrt(2.0/3.0);
    template <class Type> const double GaussLobatto<Type>::beta_  = 1.0/std::sqrt(5.0);
    template <class Type> const double GaussLobatto<Type>::x1_    = 0.94288241569547971906;
    template <class Type> const double GaussLobatto<Type>::x2_    = 0.64185334234578130578;
    template <class Type> const double GaussLobatto<Type>::x3_    = 0.23638319966214988028;

    template <class Type> Type
    GaussLobatto<Type>::calculateAbsTolerance( const boost::function<Type (Type)>& f, Type a, Type b) const {
        Type relTol = (relAccuracy_ > QL_EPSILON) ? relAccuracy_ : QL_EPSILON;

        const Type m = (a+b)/2;
        const Type h = (b-a)/2;
        const Type y1 = f(a);
        const Type y3 = f(m-alpha_*h);
        const Type y5 = f(m-beta_*h);
        const Type y7 = f(m);
        const Type y9 = f(m+beta_*h);
        const Type y11= f(m+alpha_*h);
        const Type y13= f(b);

        Type acc=h*(0.0158271919734801831*(y1+y13)
                  +0.0942738402188500455*(f(m-x1_*h)+f(m+x1_*h))
                  +0.1550719873365853963*(y3+y11)
                  +0.1888215739601824544*(f(m-x2_*h)+ f(m+x2_*h))
                  +0.1997734052268585268*(y5+y9)
                  +0.2249264653333395270*(f(m-x3_*h)+f(m+x3_*h))
                  +0.2426110719014077338*y7);

        increaseNumberOfEvaluations(13);
        QL_REQUIRE(acc != 0.0, "can not calculate absolute accuracy from "
                               "relative accuracy");

        Type r = 1.0;
        if (useConvergenceEstimate_) {
            const Type integral2 = (h/6)*(y1+y13+5*(y5+y9));
            const Type integral1 = (h/1470)*(77*(y1+y13)+432*(y3+y11)+
                                             625*(y5+y9)+672*y7);

            if (fabs(integral2-acc) != 0.0)
                r = fabs(integral1-acc)/fabs(integral2-acc);
            if (r == 0.0 || r > 1.0)
                r = 1.0;
        }

        if (relAccuracy_ != 0)
            return  ((absoluteAccuracy() < acc*relTol) ? absoluteAccuracy() : acc*relTol)/(r*QL_EPSILON);
                    //min(absoluteAccuracy(), acc*relTol)/(r*QL_EPSILON);
        else {
            return absoluteAccuracy()/(r*QL_EPSILON);
        }
    }

    template <class Type> Type
    GaussLobatto<Type>::adaptivGaussLobattoStep(const boost::function<Type (Type)>& f, Type a, Type b, Type fa, Type fb, Type acc) const {
        QL_REQUIRE(evaluations() < maxEvaluations(),
                   "max number of iterations reached");

        const Type h=(b-a)/2;
        const Type m=(a+b)/2;

        const Type mll=m-alpha_*h;
        const Type ml =m-beta_*h;
        const Type mr =m+beta_*h;
        const Type mrr=m+alpha_*h;

        const Type fmll= f(mll);
        const Type fml = f(ml);
        const Type fm  = f(m);
        const Type fmr = f(mr);
        const Type fmrr= f(mrr);
        increaseNumberOfEvaluations(5);

        const Type integral2=(h/6)*(fa+fb+5*(fml+fmr));
        const Type integral1=(h/1470)*(77*(fa+fb)
                                       +432*(fmll+fmrr)+625*(fml+fmr)+672*fm);

        // avoid 80 bit logic on x86 cpu
        Type dist = acc + (integral1-integral2);
        if(dist==acc || mll<=a || b<=mrr) {
            QL_REQUIRE(m>a && b>m,"Interval contains no more machine number");
            return integral1;
        }
        else {
            return  adaptivGaussLobattoStep(f,a,mll,fa,fmll,acc)
                  + adaptivGaussLobattoStep(f,mll,ml,fmll,fml,acc)
                  + adaptivGaussLobattoStep(f,ml,m,fml,fm,acc)
                  + adaptivGaussLobattoStep(f,m,mr,fm,fmr,acc)
                  + adaptivGaussLobattoStep(f,mr,mrr,fmr,fmrr,acc)
                  + adaptivGaussLobattoStep(f,mrr,b,fmrr,fb,acc);
        }
    }

}

#endif  /* ifndef quantlib_gausslobatto_hpp */


#include <iostream>
#include <fstream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include "Draw.h"

using namespace std;
using namespace Eigen;

int main(){
    string traj_file = "/home/leodu/learn_slambook/ch3/examples/trajectory.txt";
    vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
    ifstream fin(traj_file);
    if(!fin){
        cout << "File not found." << endl;
        return 1;
    }
    while(!fin.eof()){
        double time, tx, ty, tz, qw, qx, qy, qz;
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Isometry3d Twr(Quaterniond(qw,qx,qy,qz));
        Twr.pretranslate(Vector3d(tx,ty,tz));
        poses.push_back(Twr);
//        cout << "Twr = \n" << Twr.matrix() << endl;
    }
    Draw draw(poses);
    draw.drawTraj();

}


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <math.h>
#include <cassert>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include "metro/log_sum_exp.hpp"

namespace metro {
	template<>
	double log_sum_exp( std::vector< double > const& data ) {
		if( data.size() == 0 ) {
			return -std::numeric_limits< double >::infinity() ;
		}
		std::vector< double >::const_iterator max_i = std::max_element( data.begin(), data.end() ) ;
		double const max_value = *max_i ;
		if( max_value == -std::numeric_limits< double >::infinity() ) {
			return max_value ;
		}
		// exponentiate
		double result = max_value ;
		for( std::size_t i = 0; i < data.size(); ++i ) {
			result += std::exp(data[i] - max_value) ;
		}
		return max_value + std::log( result ) ;
	}

	void rowwise_log_sum_exp( Eigen::MatrixXd const& data, Eigen::VectorXd* result ) {
		assert( result ) ;
		assert( result->size() == data.rows() ) ;

		for( int i = 0; i < result->size(); ++i ) {
			(*result)(i) = log_sum_exp( data.row(i) ) ;
		}
	}
	void rowwise_log_sum_exp( Eigen::MatrixXd const& data, Eigen::MatrixXd const& nonmissingness, Eigen::VectorXd* result ) {
		assert( result ) ;
		assert( result->size() == data.rows() ) ;
		assert( data.rows() == nonmissingness.rows() ) ;
		assert( data.cols() == nonmissingness.cols() ) ;

		for( int i = 0; i < result->size(); ++i ) {
			(*result)(i) = log_sum_exp( data.row(i), nonmissingness.row(i) ) ;
		}
	}
}


#define _LIE_ALGEBRA_

#include <iostream>
#include <vector>
#include <armadillo>
#include <cmath>
#include <algorithm>

#ifdef _WEIGHTED_MAT_
#else
#include "weightedMat.hpp"
#endif

using arma::cx_mat;
using arma::kron;

using std::vector;
using std::cout;
using std::endl;

class LieAlgebra
{

  public:

    LieAlgebra(int n, double penalty)
    {
      basisGenerator(n, penalty, lieBasis);
    }

    ~LieAlgebra();

    vector<weightedMat> lieBasis;

    struct byCost
    {
        bool operator()(weightedMat const &a, weightedMat const &b)
        {
          		return a.cost < b.cost;
        }
    };

  private:

    cx_double imagI = cx_double(0.0, 1.0);

    void generatorLoop(int depth,
              vector<int> & indices,
              vector<int> & maxIndex,
              vector<cx_mat>& Paulis,
              vector<weightedMat>& lieBasis,
              double penalty
              )
    {
      if (depth>0){
         for(int i = 0; i < maxIndex[depth-1]; ++i){
            indices[depth-1] = i;
            generatorLoop(depth-1, indices, maxIndex, Paulis, lieBasis, penalty);
         }
      }
      else
      {
          // indices of tensor product
          //cout << "indices : ";
          int numNonZeros = 0;
          weightedMat temp;
          temp.lieMat = Paulis[indices[0]];

          for(int r = 0; r < indices.size(); ++r)
          {
            if(indices[r] != 0)
            {
              ++numNonZeros;
            }
            //cout << indices[r] <<" ";
          }

          if(numNonZeros < 3 )
          {
            temp.cost = 1;
          }
          else
          {
            temp.cost = penalty;
          }

          //make kronecker products
          for(int r = 1; r < indices.size(); ++r)
          {
            temp.lieMat = kron(temp.lieMat, Paulis[indices[r]]);
          }

          temp.lieMat *= imagI;

          //sub riemannian case
          lieBasis.push_back(temp);

       }
    }

    //n is 2^n in SU(2^n)
    void basisGenerator(int n, double penalty, vector<weightedMat>& lieBasis)
    {

      vector<cx_mat> Paulis;
      Paulis.reserve(4);

      Paulis[0] = eye<cx_mat>(2,2);
      Paulis[1] = { {0.0,0.0}, {1.0,0.0}, {1.0,0.0}, {0.0,0.0} };
      Paulis[2] = { {0.0,0.0}, {0.0,1.0}, {0.0,-1.0}, {0.0,0.0} };
      Paulis[3] = { {1.0,0.0}, {0.0,0.0}, {0.0,0.0}, {-1.0,0.0} };

      for(int s = 1; s < 4; ++s)
      {
        Paulis[s].reshape(2,2);
      }

      vector<int> indices(n,0);
      vector<int> maxIndex;

      for(int i=0; i<n; ++i)
      {
        maxIndex.push_back(4);
      }

      generatorLoop(indices.size(),indices, maxIndex, Paulis, lieBasis, penalty);

      //for SU remove the identity product I otimes I .... otimes I
      lieBasis.erase(lieBasis.begin());
      std::sort(lieBasis.begin(), lieBasis.end(), byCost());

      cout << "number of basis elements : " << lieBasis.size() << endl;
      for(int i=0; i< lieBasis.size(); ++i)
      {
        cout << "basis element :" << i << " with cost :" << lieBasis[i].cost << endl << lieBasis[i].lieMat << endl;
      }
    }
};


// number_systems.cpp: performance comparison between different number systems
//
// Copyright (C) 2017-2020 Stillwater Supercomputing, Inc.
//
// This file is part of the HPRBLAS project, which is released under an MIT Open Source license.
#include <boost/multiprecision/cpp_bin_float.hpp>
//#define POSIT_FAST_SPECIALIZATION 1
#define POSIT_FAST_POSIT_16_1 1
#define POSIT_FAST_POSIT_32_2 1
#include <universal/number/posit/posit.hpp>
#include <universal/performance/number_system.hpp>

// define a true 256-bit IEEE floating point type
constexpr size_t bits_in_octand = 113 + 128;
using cpp_bin_float_octand = boost::multiprecision::number<boost::multiprecision::backends::cpp_bin_float<bits_in_octand, boost::multiprecision::backends::digit_base_2, void, boost::int16_t, -16382, 16383>, boost::multiprecision::expression_template_option::et_off>;
// define the floating point types (single, double, quad, octand)
using sp = boost::multiprecision::cpp_bin_float_single;
using dp = boost::multiprecision::cpp_bin_float_double;
using qp = boost::multiprecision::cpp_bin_float_quad;
using op = cpp_bin_float_octand;

int main(int argc, char** argv)
try {
	using namespace std;
	using namespace sw::universal;

	cout << "Arithmetic performance comparison" << endl;

	posit<16,1> p16;
	float f;
	sp boostsp;
	posit<32, 2> p32;

	OperatorPerformance report;
	GeneratePerformanceReport(f, report);
	cout << ReportPerformance(f, report) << endl;
//	GeneratePerformanceReport(boostsp, report);
//	cout << ReportPerformance(boostsp, report) << endl;
	GeneratePerformanceReport(p16, report);
	cout << ReportPerformance(p16, report) << endl;
	GeneratePerformanceReport(p32, report);
	cout << ReportPerformance(p32, report) << endl;

	return EXIT_SUCCESS;
}
catch (char const* msg) {
	std::cerr << msg << std::endl;
	return EXIT_FAILURE;
}
catch (const sw::universal::posit_arithmetic_exception& err) {
	std::cerr << "Uncaught posit arithmetic exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const sw::universal::quire_exception& err) {
	std::cerr << "Uncaught quire exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const sw::universal::posit_internal_exception& err) {
	std::cerr << "Uncaught posit internal exception: " << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (std::runtime_error& err) {
	std::cerr << err.what() << std::endl;
	return EXIT_FAILURE;
}
catch (...) {
	std::cerr << "Caught unknown exception" << std::endl;
	return EXIT_FAILURE;
}


#include "config.hpp"
#include "include/common.hpp"
#include "include/pi.hpp"
#include <boost/mpi.hpp>
#include <boost/program_options.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <iomanip>
#include <iostream>
#include <string>

namespace mpi = boost::mpi;
namespace po = boost::program_options;
namespace mp = boost::multiprecision;

using number = mp::number<mp::cpp_dec_float<10000>>;
constexpr int ROOT = 0;

namespace mpi_pi {

int main(int argc, char **argv) {
  mpi::environment env(argc, argv, false);
  mpi::communicator world;

  config::Configuration config;
  po::options_description description("options");
  if (world.rank() == ROOT) {
    description.add_options()("help,h", "print help message");
    description.add_options()(
        "method,m", po::value<std::string>()->default_value("area_integral"),
        "method to calculate pi");
    description.add_options()("terms,t",
                              po::value<std::size_t>()->default_value(1000),
                              "terms number to calculate");

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, description), vm);
    po::notify(vm);

    if (vm.count("help"))
      config.show_help = true;
    if (vm.count("method"))
      config.method = vm["method"].as<std::string>();
    if (vm.count("terms"))
      config.terms = vm["terms"].as<std::size_t>();
  }

  mpi::broadcast(world, config, ROOT);

  if (config.show_help) {
    if (world.rank() == ROOT) {
      std::cout << description << std::endl;
    }
    return 0; // all processes exit
  }

  number result;
  if (config.method == "area_integral") {
    result = area_integral::pi<number>(world, ROOT, config.terms);
  } else if (config.method == "power_series") {
    result = power_series::pi<number>(world, ROOT, config.terms);
  } else if (config.method == "improved_power_series") {
    result = improved_power_series::pi<number>(world, ROOT, config.terms);
  } else if (config.method == "monte_carlo") {
    result = monte_carlo::pi<number>(world, ROOT, config.terms);
  } else if (config.method == "monte_carlo_integral") {
    result = monte_carlo_integral::pi<number>(world, ROOT, config.terms);
  } else if (config.method == "random_integral") {
    result = random_integral::pi<number>(world, ROOT, config.terms);
  } else if (config.method == "borwein1987") {
    result = borwein1987::pi<number>(world, ROOT, config.terms);
  } else if (config.method == "yasumasa2002") {
    result = yasumasa2002::pi<number>(world, ROOT, config.terms);
  } else if (config.method == "chudnovsky") {
    result = chudnovsky::pi<number>(world, ROOT, config.terms);
  } else if (config.method == "bbp") {
    result = bbp::pi<number>(world, ROOT, config.terms);
  } else {
    std::cout << "invalid method \"" << config.method << "\"" << std::endl;
  }
  if (world.rank() == ROOT) {
    std::cout << std::setprecision(std::numeric_limits<number>::max_digits10)
              << result << std::endl;
  }
}

} // namespace mpi_pi

int main(int argc, char **argv) { return mpi_pi::main(argc, argv); }


//
//  multivector_AWT.cpp
//
//  Created by r. on 18/05/14
//

// Disables checks in boost
#ifndef NDEBUG
#define NDEBUG
#endif

//
#include "../include/reporter.hpp"

#include <iostream>
#include "../include/stopwatch.hpp"
#include "../include/multivector.hpp"
#include "../include/multivector_AWT.hpp"
#include "../include/tFEM.hpp"
#include "../include/AWT.hpp"

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>

namespace mypara
{
	using namespace std;
	namespace mpi = boost::mpi;
	namespace ublas = boost::numeric::ublas;

	void test_multivector_AWT_basic()
	{
		cout << "Enter: mypara::test_multivector_AWT_basic()" << endl;
		stopwatch::Time time(stopwatch::watches, "mypara::test_multivector_AWT_basic()");

		mpi::communicator world;

		typedef ublas::matrix<double> matrix;

		using namespace mypara;

		unsigned int size1 = 3; // Number of rows
		unsigned int size2 = 9; // Number of columns

		// Create AWT
		double T = 4;
		spacetime::tmesh te;
		te.makeuniform(0, T, size2);

		spacetime::tFEM tfem(te);

		unsigned int nu = 2;
		mypara::AWT awt(tfem.AtE, tfem.MtE, nu);

		// Check generalized "eigenvalues" of awt
		{
			spacetime::AWT awt2(tfem.AtE, tfem.MtE, nu);
			bool ok = true;
			for (auto i = 0; i != awt.gamma.size(); ++i) {
				ok = ok && (fabs(awt.gamma[i] - awt2.gamma[i]) <= 1e-8);
			}
			cout << "Eigenvalues OK? " << (ok ? "YES" : "NO") << endl;
			assert(ok);
		}

		matrix v_ref(size1, size2); v_ref.clear();
		// Create matrix to be distributed
		{
			for (unsigned int j = 0; j != size2; ++j)
			{
				v_ref(j % size1, j) = 1;
//				v_ref(j, j) = 1;
			}
		}

		multivector v(size1, size2, world);
		// Distributed matrix
		{
			for (unsigned int j = v.getja(); j < v.getjb(); ++j) {
				v.colio_local(j) = ublas::column(v_ref, j);
			}
		}

		// Check multivector::get_whole()
		{
			matrix z = (v_ref - v.getwhole());
			bool ok = (ublas::norm_frobenius(z) == 0);
			cout << "Get whole OK? " << (ok ? "YES" : "NO") << endl;
			assert(ok);
			world.barrier();
		}

		// Print info
		{
			for (unsigned int j = v.getja(); j < v.getjb(); ++j) {
				cout << "Proc " << world.rank() << ": " << "column #" << j << " is " << v.colio_local(j) << endl;
			}
			cout.flush(); world.barrier();
		}

		// Multiplication 1
		{
			// Print info
			{
				if (!world.rank()) cout << "Testing right multiplication by T" << endl;
				cout.flush(); world.barrier();
			}

			multivector w = awt.uT(v);

			matrix w_ref;
			{
				spacetime::AWT awt(tfem.AtE, tfem.MtE, nu);
				w_ref = ublas::prod(v_ref, awt.V);
			}

			// Print info
			{
				for (unsigned int j = w.getja(); j < w.getjb(); ++j) {
					cout << "Proc " << world.rank() << ": " << "column #" << j << " is " << w.colio_local(j) << endl;
				}

				multivector::dense_matrix w_whole = w.getwhole();
				if (!world.rank()) cout << "w_whole: " << w_whole << endl;

				cout.flush(); world.barrier();
			}

			// Check multiplication result
			{
				if (!world.rank()) cout << "w_ref: " << w_ref << endl;
				matrix z = (w_ref - w.getwhole());
				bool ok = (ublas::norm_frobenius(z) <= 1e-10);
				cout << "Multiplication 1 OK? " << (ok ? "YES" : "NO") << endl;
				assert(ok);
			}
		}

		// Multiplication 2
		{
			// Print info
			{
				if (!world.rank()) cout << "Testing right multiplication by Tt" << endl;
				cout.flush(); world.barrier();
			}

			multivector w = awt.uTt(v);

			matrix w_ref;
			{
				spacetime::AWT awt(tfem.AtE, tfem.MtE, nu);
				w_ref = ublas::prod(v_ref, ublas::trans(awt.V));
			}

			// Print info
			{
				for (unsigned int j = w.getja(); j < w.getjb(); ++j) {
					cout << "Proc " << world.rank() << ": " << "column #" << j << " is " << w.colio_local(j) << endl;
				}

				multivector::dense_matrix w_whole = w.getwhole();
				if (!world.rank()) cout << "w_whole: " << w_whole << endl;

				cout.flush(); world.barrier();
			}

			// Check multiplication result
			{
				if (!world.rank()) cout << "w_ref: " << w_ref << endl;
				matrix z = (w_ref - w.getwhole());
				bool ok = (ublas::norm_frobenius(z) <= 1e-10);
				cout << "Multiplication 2 OK? " << (ok ? "YES" : "NO") << endl;
				assert(ok);
			}
		}

		//
		{
			multivector::dense_matrix v_whole = v.getwhole();

			// Print info
			{
				if (!world.rank()) cout << "v_whole: " << v_whole << endl;
				cout.flush(); world.barrier();
			}

		}

		cout << "Exit: mypara::test_multivector_AWT_basic()" << endl;
	}

//	void test_multivector_AWT_large()
//	{
//		cout << "Enter: mypara::test_multivector_AWT_large()" << endl;
//		stopwatch::Time time(stopwatch::watches, "mypara::test_multivector_AWT_large()");
//
//		mpi::communicator world;
//
//		unsigned int size1 = 100000;
//		unsigned int size2 = (1 << 12);
//
//		multivector v(size1, size2, world);
//
//		{
//			typedef ublas::compressed_matrix<double> sparse_matrix;
//
//			sparse_matrix m(size2, size2, 3*size2); m.clear();
//			m = ublas::identity_matrix<>(size2);
//
//			stopwatch::StopWatch w;
//			w.tic();
//			v * m;
//			w.add();
//
//			// Print timings
//			{
//				cout.flush(); world.barrier();
//				if (!world.rank())
//				{
//					cout << "@iden: ";
//					cout << "size2: " << size2 << " ";
//					cout << "time(ms): " << w.ms().count() << " ";
//					cout << endl;
//				}
//				world.barrier();
//			}
//		}
//
//		cout << "Exit: mypara::test_multivector_AWT_large()" << endl;
//	}

	void test_multivector_AWT_scale()
	{
		cout << "Enter: mypara::test_multivector_AWT_scale()" << endl;
		stopwatch::Time time(stopwatch::watches, "mypara::test_multivector_AWT_scale()");

		reporter::note("Parallel AWT scalability test");
		reporter::note("Timings are in milliseconds");

		typedef ublas::matrix<double> matrix;
		typedef ublas::compressed_matrix<double> sparse_matrix;

		mpi::communicator world;

		//unsigned int n1 = 12; // size1 = dimV increases up to (2 ^ n1)
		unsigned int n2 = 14; // size2 = dimE increases up to (2 ^ n2)

		//for (unsigned int size1 = (1 << n1); size1 <= (1 << n1); size1 *= 2)
		unsigned size1 = 1953;
		reporter::note["dimV"] = size1;
		reporter::note("Number of temporal elements")["N = []"];
		reporter::note("Multiplication by T [ms]")["T = []"];
		reporter::note("Multiplication by V [ms]")["V = []"];
		{
			for (unsigned int size2 = 1; size2 <= (1 << n2); size2 *= 2)
			{
				reporter::note[""];
				reporter::note["n"] = size2;

				multivector v(size1, 1 + size2, world);

				// Create AWT
				double T = 2;
				spacetime::tmesh te;
				te.makeuniform(0, T, v.getot());

				spacetime::tFEM tfem(te);

				unsigned int nu = 2;
				AWT awt(tfem.AtE, tfem.MtE, nu);

				double t_max = 0;

				// Multiply by T
				{
					stopwatch::StopWatch w;
					//awt.uT(v); // compute jplan
					w.tic();
					awt.uT(v);
					w.add();
					double t = w.ms().count();
					t_max = max(t, t_max);

					// Print timings
					{
						cout.flush(); world.barrier();
						if (!world.rank())
						{
							cout << "@T: ";
							cout << "size1: " << size1 << " ";
							cout << "size2: " << size2 << " ";
							cout << "time(ms): " << t << " ";
							cout << endl;
						}
						world.barrier();
					}

					reporter::note["t"] = t;
				}

				// Multiply by Tt
				{
					stopwatch::StopWatch w;
					//awt.uTt(v); // compute jplan
					w.tic();
					awt.uTt(v);
					w.add();
					double v = w.ms().count();
					t_max = max(v, t_max);

					// Print timings
					{
						cout.flush(); world.barrier();
						if (!world.rank())
						{
							cout << "@V: ";
							cout << "size1: " << size1 << " ";
							cout << "size2: " << size2 << " ";
							cout << "time(ms): " << v << " ";
							cout << endl;
						}
						world.barrier();
					}

					reporter::note["v"] = v;
				}

				reporter::note["N.append(n);"];
				reporter::note["T.append(t);"];
				reporter::note["V.append(v);"];

				// Abort loop if the trafo is taking too long
				double t_max_ok = 4 * 60e3; // seconds
				if (t_max >= t_max_ok) break;
			} // for size2
		} // for size1

		cout << "Exit: mypara::test_multivector_AWT_scale()" << endl;
	}
}

void test()
{
	reporter::note.is_quiet = (boost::mpi::communicator().rank() != 0);

	mypara::test_multivector_AWT_basic();
//    mypara::test_multivector_AWT_large();
    mypara::test_multivector_AWT_scale();
}