/*! \file	geometry.hpp
 *  \brief	Declarations for geometry-related calculations.
*/

#ifndef THERMALVIS_GEOMETRY_H
#define THERMALVIS_GEOMETRY_H

#ifdef _USE_EIGEN_

#include "core/tools.hpp"

#include "core/general_resources.hpp"
#include "core/ros_resources.hpp"

#include <opencv2/core.hpp>

#ifdef _USE_OPENCV_VIZ_
#include <opencv2/viz.hpp>
#endif

#include <Eigen/Geometry>

typedef Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Matrix3frm;
typedef Eigen::Quaternion<double>   QuaternionDbl;
typedef Eigen::Quaternion<double>   Quaterniond;
typedef Eigen::Matrix<double, 4, 1, Eigen::DontAlign> Vector4d;

//#include <math.h>

// FLAGS for geometric distance measurement (F & H assessment)
#define SAMPSON_DISTANCE			0
#define ALGEBRAIC_DISTANCE			1
#define EPIPOLAR_DISTANCE			2
#define LOURAKIS_DISTANCE			3

#define DEFAULT_ASSIGN_MODE			0
#define MAPPER_ASSIGN_MODE			1

#define MAX_TIME_GAP_FOR_INTERP					0.5

#define MAX_RVIZ_DISPLACEMENT 					1000

#define CLUSTER_MEAN_MODE			1
#define DEFAULT_MEAN_MODE			0

void assignPose(geometry_msgs::PoseStamped& pPose, cv::Mat& C, int idx, ros::Time timestamp, int mode = DEFAULT_ASSIGN_MODE);
void convertPoseFormat(const geometry_msgs::Pose& pose, cv::Mat& t, Eigen::Quaternion<double>& Q);
void convertPoseFormat(const cv::Mat& t, const Eigen::Quaternion<double>& Q, geometry_msgs::Pose& pose);
void convertAndShiftPoseFormat(const geometry_msgs::Pose& pose, cv::Mat& t, Eigen::Quaternion<double>& Q);
void convertAndShiftPoseFormat(const cv::Mat& t, const Eigen::Quaternion<double>& Q, geometry_msgs::Pose& pose);

/// \brief 		Converts translation vector from OpenCV to Eigen format
void convertTvecToEigenvec(const cv::Mat& T_src, Eigen::Vector3f& T_dst);

/// \brief 		Converts rotation matrix from OpenCV to Eigen format
void convertRmatTo3frm(const cv::Mat& R_src, Matrix3frm& R_dst);

/// \brief 		Converts translation vector from Eigen to OpenCV format
void convertEigenvecToTvec(const Eigen::Vector3f& T_src, cv::Mat& T_dst);

#ifdef _USE_OPENCV_VIZ_
/// \brief 		Converts from 4x4 OpenCV matrix to 3x4 Affine representation
void convertMatToAffine(const cv::Mat mat, cv::Affine3f& affine);
#endif

/// \brief 		Finds centroid of 3D cloud
cv::Point3d findCentroid(vector<cv::Point3d>& cloud);

/// \brief 		Finds the length of the diagonal from extreme corners of minimum enclosing prism (perpendicular do X,Y,Z axes)
double findPrismDiagonal(vector<cv::Point3d>& cloud);

/// \brief 		Converts rotation matrix from Eigen to OpenCV format
void convert3frmToRmat(const Matrix3frm& R_src, cv::Mat& R_dst);

/// \brief 		Splits OpenCV transformation matrix into rotation matrix and translation vector
void decomposeTransform(const cv::Mat& c, cv::Mat& R, cv::Mat& t);

/// \brief		Find the average point position of the dominant cluster
bool findClusterMean(const vector<cv::Point3d>& estimatedLocations, cv::Point3d& pt3d, int mode = DEFAULT_MEAN_MODE, int minEstimates = 3, double maxStandardDev = 0.1);

/// \brief 		Possible duplicate of 'composeTransform'
void findP1Matrix(cv::Mat& P1, const cv::Mat& R, const cv::Mat& t);

/// \brief 		Combines OpenCV rotation matrix and translation vector into a transformation matrix
void composeTransform(const cv::Mat& R, const cv::Mat& t, cv::Mat& c);

/// \brief 		Converts OpenCV rotation matrix to Eigen quaternion
void matrixToQuaternion(const cv::Mat& mat, Eigen::Quaternion<double>& quat);

/// \brief 		Converts Eigen quaternion to OpenCV rotation matrix
void quaternionToMatrix(const Eigen::Quaternion<double>& quat, cv::Mat& mat, bool handedness = false);

/// \brief 		Transform a 3D point cloud according to a different coordinate system convention
void transformPoints(std::vector<cv::Point3d>& pts, unsigned int option = 0);

/// \brief 		Transform a 3D point cloud according to a different coordinate system convention
void transformPoints(std::vector<cv::Point3d>& pts, int *options);

/// \brief 		Get minimum number of projections required to achieve the specified number of pairs
int minProjections(int pairs);

/// \brief 		Get maximum possible number of pairs able to be achieved with specified number of projections
int possiblePairs(int projections);

/// \brief 		Convert from OpenCV 4x4 transformation matrix to 3x4 projection matrix
void transformationToProjection(const cv::Mat& trans, cv::Mat& proj);

/// \brief 		Convert from OpenCV 3x4 projection matrix to 4x4 transformation matrix
void projectionToTransformation(const cv::Mat& proj, cv::Mat& trans);

/// \brief 		Extract OpenCV 3x3 rotation matrix from OpenCV 3x4 projection matrix
void projectionToRotation(const cv::Mat& src, cv::Mat& dst);

/// \brief 		Convert OpenCV 3x3 rotation matrix to OpenCV 3x4 projection matrix
void rotationToProjection(const cv::Mat& src, cv::Mat& dst);

/// \brief 		Extract just the 3 translation magnitudes (X,Y,Z) between two poses
void getTranslationBetweenCameras(cv::Mat& C1, cv::Mat& C2, double *translations);

/// \brief 		Set the 3x4 pose matrix to the equivalent of the identity pose
void initializeP0(cv::Mat& P);

/// \brief 		Get the W and Z matrices, which are useful for some geometric operations
void getWandZ(cv::Mat& W, cv::Mat& Winv, cv::Mat& Z);

/// \brief 		Shift a 3D point according to a provided transformation
void transfer3dPoint(const cv::Point3d& src, cv::Point3d& dst, const cv::Mat& C);

/// \brief 		Shift a set of 3D points according to a provided transformation
void transfer3DPoints(const std::vector<cv::Point3d>& src, std::vector<cv::Point3d>& dst, const cv::Mat& C);

/// \brief 		Determine rotation relative to identity in degrees
double getRotationInDegrees(const cv::Mat& R);

/// \brief 		Get magnitude of translation vector
double getDistanceInUnits(const cv::Mat& t);

/// \brief 		Get angle difference between two quaternions
double getQuaternionAngle(const Eigen::Quaterniond& q1, const Eigen::Quaterniond& q2);

/// \brief 		Convert 3D point from OpenCV point format to matrix format
void convertPoint3dToMat(const cv::Point3d& src, cv::Mat& dst);

/// \brief 		Create default quaternion
Quaterniond defaultQuaternion();

/// \brief 		Converts OpenCV projection matrix to Eigen projection matrix
void convertProjectionMatCVToEigen(const cv::Mat& mat, Eigen::Matrix< double, 3, 4 >& m);

/// \brief 		Converts Eigen projection matrix to OpenCV projection matrix
void convertProjectionMatEigenToCV(const Eigen::Matrix< double, 3, 4 >& m, cv::Mat& mat);

/// \brief 		Calculate the dot product between two vectors
double dotProduct(const cv::Mat& vec1, const cv::Mat& vec2);

/// \brief 		Calculate the dot product between two quaternions
double dotProduct(const Quaterniond& q1, const Quaterniond& q2);

/// \brief 		Convert from Eigen vector to OpenCV matrix
void convertVec4dToMat(const Vector4d& vec4, cv::Mat& mat);

// Can't find where these are actually implemented!
/*
void compileTransform(cv::Mat& c, const cv::Mat& R, const cv::Mat& t);
void combineTransforms(cv::Mat& CN, const cv::Mat& C0, const cv::Mat& C1);
void decomposeTransform(const cv::Mat& c, cv::Mat& R, cv::Mat& t);
*/

bool interpolatePose(const geometry_msgs::Pose& pose1, ros::Time time1, const geometry_msgs::Pose& pose2, ros::Time time2, geometry_msgs::Pose& finalPose, ros::Time time3);
void shiftPose(const geometry_msgs::Pose& pose_src, geometry_msgs::Pose& pose_dst, cv::Mat transformation);

#endif // THERMALVIS_GEOMETRY_H

#endif // _USE_EIGEN_


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

using namespace Eigen;
using namespace std;

int main()
{
  MatrixXd m = MatrixXd::Random(3,3);
  m = (m + MatrixXd::Constant(3,3,1.2)) * 50;
  cout << "m =" << endl << m << endl;
  VectorXd v(3);
  v << 1, 2, 3;
  cout << "m * v =" << endl << m * v << endl;
}


#include "plan.h"

#include "../Util.h"
#include "../log.h"
#include "../simulator/constants.h"
#include "../simulator/graphics.h"

#include <iostream>
#include <queue>
#include <set>

#include <Eigen/Core>

constexpr double EPSILON = 1.2; // heuristic weight for weighted A*

using namespace NavSim;

// TODO implement goal orientations?
double heuristic(int x, int y, const point_t& goal) {
	double dx = goal(0) - x * PLAN_RESOLUTION;
	double dy = goal(1) - y * PLAN_RESOLUTION;
	return sqrt(dx * dx + dy * dy);
}

struct Node {
	int x;
	int y;
	int theta;		 // allows values 0..7; gives angle in increments of pi/4
	double acc_cost; // accumulated cost
	double heuristic_to_goal;
	int acc_steps; // for knowing how long (in discrete steps) the plan will be
	struct Node* parent;
	// action: the action taken to reach this node from the parent node
	// Tracking this makes it easier to construct the plan later.
	action_t action;

	// Note: In the robot frame, starting pose is always (0,0,0).
	Node(Node* p, action_t& a, const point_t& goal)
		: x(0), y(0), theta(0), acc_cost(0.), heuristic_to_goal(0.), acc_steps(0), parent(p),
		  action(a) {
		if (p != nullptr) {
			acc_steps = p->acc_steps + 1;
			bool moving_forward = action(1) > 0.0;
			double step_cost = moving_forward ? action(1) : RADIAN_COST * fabs(action(0));
			acc_cost = p->acc_cost + step_cost;
			if (moving_forward) {
				theta = p->theta;
				int dx(0), dy(0);
				if (theta >= 1 && theta <= 3)
					dy = 1;
				if (theta >= 3 && theta <= 5)
					dx = -1;
				if (theta >= 5 && theta <= 7)
					dy = -1;
				if (theta == 7 || theta <= 1)
					dx = 1;
				x = p->x + dx;
				y = p->y + dy;
			} else {
				x = p->x;
				y = p->y;
				theta = (p->theta + ((action(0) > 0.0) ? 1 : 7)) % 8;
			}
		}
		heuristic_to_goal = heuristic(x, y, goal);
	}
};

std::ostream& operator<<(std::ostream& out, const Node& n) {
	out << "(" << n.x << " " << n.y << " " << n.theta << ") ";
	return out;
}

class NodeEqualityCompare {
public:
	bool operator()(const Node* lhs, const Node* rhs) const {
		bool res = lhs->x < rhs->x || (lhs->x == rhs->x && lhs->y < rhs->y) ||
				   (lhs->x == rhs->x && lhs->y == rhs->y && lhs->theta < rhs->theta);
		return res;
	}
};

class NodeCompare {
public:
	bool operator()(const Node* lhs, const Node* rhs) {
		return (lhs->acc_cost + EPSILON * (lhs->heuristic_to_goal)) >
			   (rhs->acc_cost + EPSILON * (rhs->heuristic_to_goal));
	}
};

using pqueue_t = std::priority_queue<Node*, std::vector<Node*>, NodeCompare>;
using set_t = std::set<Node*, NodeEqualityCompare>;
using collides_predicate_t = std::function<bool(double x, double y, double radius)>;

bool is_valid(const Node* n, const collides_predicate_t& collides) {
	// To get away from obstacles, turning is always allowed.
	if (n->action(1) == 0.0)
		return true;
	double x = n->x * PLAN_RESOLUTION;
	double y = n->y * PLAN_RESOLUTION;
	return !collides(x, y, SAFE_RADIUS);
}

plan_t getPlan(const collides_predicate_t& collides, const point_t& goal, double goal_radius) {
	util::ScopedTimer timer;
	log(LOG_DEBUG, "Planning... ");
	action_t action = action_t::Zero();
	std::vector<Node*> allocated_nodes;
	Node* start = new Node(nullptr, action, goal);
	allocated_nodes.push_back(start);
	pqueue_t fringe; // If you haven't guessed, we'll be using A*
	set_t visited_set;
	fringe.push(start);
	visited_set.insert(start);
	Node* n = start;
	plan_t valid_actions(3, 2);
	valid_actions << 0., 0., M_PI / 4, 0., -M_PI / 4, 0.;
	int counter = 0;
	bool success = false;
	while (fringe.size() > 0) {
		if (counter++ > MAX_ITERS) {
			break;
		}
		n = fringe.top();
		fringe.pop();
		if (n->heuristic_to_goal < goal_radius) {
			success = true;
			break;
		} else {
			double forward_dist =
				(n->theta % 2 == 1) ? PLAN_RESOLUTION * 1.414 : PLAN_RESOLUTION;
			valid_actions(0, 1) = forward_dist;
			for (int i = 0; i < valid_actions.rows(); i++) {
				action = valid_actions.row(i);
				Node* next = new Node(n, action, goal);
				allocated_nodes.push_back(next);
				if (is_valid(next, collides) && visited_set.count(next) == 0) {
					visited_set.insert(next);
					fringe.push(next);
				}
			}
		}
	}
	if (success == false)
		n = start;

	plan_t plan(n->acc_steps, 2);
	for (int i = n->acc_steps - 1; i >= 0; i--) {
		plan.row(i) = n->action;
		n = n->parent;
	}

	std::chrono::milliseconds endTime = std::chrono::duration_cast<std::chrono::milliseconds>(
		std::chrono::system_clock::now().time_since_epoch());

	log(LOG_DEBUG, "finished in %d iterations (%d visited nodes), Time: %dms\n", counter,
		allocated_nodes.size(), timer.elapsedTime().count() / 1000);
	for (Node* p : allocated_nodes) {
		free(p);
	}

	return plan;
}

// Goal given in robot frame
plan_t getPlan(const points_t& lidar_hits, const point_t& goal, double goal_radius) {
	collides_predicate_t collidesPredicate = [&](double x, double y, double radius) {
		pose_t p = {x, y, 0};
		transform_t trf = toTransform(p);
		return collides(trf, lidar_hits, SAFE_RADIUS);
	};
	return getPlan(collidesPredicate, goal, goal_radius);
}

double planCostFromIndex(plan_t& plan, int idx) {
	double cost = 0;
	for (int i = idx; i < plan.rows(); i++) {
		action_t action = plan.row(i);
		cost += fabs(action(1)) + RADIAN_COST * fabs(action(0));
	}
	return cost;
}


/*
 Copyright (C) 2020 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file qle/termstructures/spreadedsmilesection2.hpp
    \brief smile section with linear interpolated vol spreads
    \ingroup termstructures
*/

#pragma once

#include <ql/math/interpolation.hpp>
#include <ql/quote.hpp>
#include <ql/termstructures/volatility/smilesection.hpp>

#include <boost/shared_ptr.hpp>

namespace QuantExt {
using namespace QuantLib;

class SpreadedSmileSection2 : public SmileSection {
public:
    SpreadedSmileSection2(const boost::shared_ptr<SmileSection>& base, const std::vector<Real>& volSpreads,
                          const std::vector<Real>& strikes, const bool strikesRelativeToAtm = false,
                          const Real atmLevel = Null<Real>());
    Rate minStrike() const;
    Rate maxStrike() const;
    Rate atmLevel() const;

protected:
    Volatility volatilityImpl(Rate strike) const;

private:
    boost::shared_ptr<SmileSection> base_;
    std::vector<Real> volSpreads_;
    std::vector<Real> strikes_;
    bool strikesRelativeToAtm_;
    Real atmLevel_;
    Interpolation volSpreadInterpolation_;
};

} // namespace QuantExt


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/QR>
#include <Eigen/Eigenvalues>
#include <chrono>
#include <cxxopts.hpp>

#include "DavidsonSolver.hpp"
#include "DavidsonOperator.hpp"
#include "MatrixFreeOperator.hpp"

#include <iostream>
#include <fstream>
#include <string>

using namespace std;

#define MAXBUFSIZE  ((int) 1e6)

Eigen::MatrixXd readMatrix(const char *filename)
    {
    int cols = 0, rows = 0;
    double buff[MAXBUFSIZE];

    // Read numbers from file into buffer.
    ifstream infile;
    infile.open(filename);
    while (! infile.eof())
        {
        string line;
        getline(infile, line);

        int temp_cols = 0;
        stringstream stream(line);
        while(! stream.eof())
            stream >> buff[cols*rows+temp_cols++];

        if (temp_cols == 0)
            continue;

        if (cols == 0)
            cols = temp_cols;

        rows++;
        }

    infile.close();

    rows--;

    // Populate matrix with numbers.
    Eigen::MatrixXd result(rows,cols);
    for (int i = 0; i < rows; i++)
        for (int j = 0; j < cols; j++)
            result(i,j) = buff[ cols*i+j ];

    return result;
    };

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

    // parse the input
    cxxopts::Options options(argv[0],  "Eigen Davidson Iterative Solver");
    options.positional_help("[optional args]").show_positional_help();
    options.add_options()
        ("size", "dimension of the matrix", cxxopts::value<std::string>()->default_value("100"))
        ("eps", "sparsity of the matrix", cxxopts::value<std::string>()->default_value("0.01"))
        ("neigen", "number of eigenvalues required", cxxopts::value<std::string>()->default_value("5"))
        ("corr", "correction method", cxxopts::value<std::string>()->default_value("DPR"))
        ("mf", "use matrix free", cxxopts::value<bool>())
        ("diag", "diagonal elements are ordered" , cxxopts::value<bool>())
        ("reorder", "reorder diagonal elements" , cxxopts::value<bool>())
        ("linsolve", "method to solve the linear system of JOCC (CG, GMRES, LLT)", cxxopts::value<std::string>()->default_value("CG"))
        ("init", "method to itialize the eigenvector (target, indentity, random)", cxxopts::value<std::string>()->default_value("target"))
        ("tol", "tolerance on the residue norm", cxxopts::value<std::string>()->default_value("1E-4"))
        ("lstol", "tolerance of the linear solver", cxxopts::value<std::string>()->default_value("0.01"))
        ("help", "Print the help", cxxopts::value<bool>());
    auto result = options.parse(argc,argv);

    if (result.count("help"))
    {
        std::cout << options.help({""}) << std::endl;
        exit(0);
    }

    int size = std::stoi(result["size"].as<std::string>(),nullptr);
    int neigen = std::stoi(result["neigen"].as<std::string>(),nullptr);
    bool mf = result["mf"].as<bool>();
    bool odiag = result["diag"].as<bool>();
    bool reorder = result["reorder"].as<bool>();
    std::string linsolve = result["linsolve"].as<std::string>();
    std::string eigen_init = result["init"].as<std::string>();
    std::string correction = result["corr"].as<std::string>();
    bool help = result["help"].as<bool>();
    double eps = std::stod(result["eps"].as<std::string>(),nullptr);
    double davidson_tol = std::stod(result["tol"].as<std::string>(),nullptr);
    double lsolve_tol = std::stod(result["lstol"].as<std::string>(),nullptr);

    // chrono
    std::chrono::time_point<std::chrono::system_clock> start, end;
    std::chrono::duration<double> elapsed_time;

    std::cout << "Matrix size : " << size << "x" << size << std::endl;
    std::cout << "Num Threads : " <<  Eigen::nbThreads() << std::endl;
    std::cout << "eps : " <<  eps << std::endl;

    // Create Operator
    DavidsonOperator Aop(size,eps,odiag,reorder);
    Eigen::MatrixXd Afull = Aop.get_full_mat();
    std::cout << "Afull" << std::endl << Afull.block(0,0,5,5) << std::endl;

    // Davidosn Solver
    start = std::chrono::system_clock::now();
    DavidsonSolver DS;

    DS.set_guess_vectors(eigen_init);
    DS.set_correction(correction);
    DS.set_tolerance(davidson_tol);

    if (correction == "JACOBI") {
        DS.set_jacobi_linsolve(linsolve);
        DS.set_linsolve_tol(lsolve_tol);
    }

    if (mf) DS.solve(Aop,neigen);
    else  DS.solve(Afull,neigen);

    end = std::chrono::system_clock::now();
    elapsed_time = end-start;
    std::cout << std::endl << "Davidson               : " << elapsed_time.count() << " secs" <<  std::endl;

    // normal eigensolver
    start = std::chrono::system_clock::now();
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es2(Afull);
    end = std::chrono::system_clock::now();
    elapsed_time = end-start;
    std::cout << "Eigen                  : " << elapsed_time.count() << " secs" <<  std::endl;

    auto dseigop = DS.eigenvalues();
    auto eig2 = es2.eigenvalues().head(neigen);
    std::cout << std::endl <<  "      Davidson  \tEigen \t\t Error" << std::endl;
    for(int i=0; i< neigen; i++)
        printf("#% 4d %8.7f \t%8.7f \t %4.2e\n",i,dseigop(i),eig2(i),abs(eig2(i)-dseigop(i)));

}


/* ****************** */
/* Include packages   */
/* ****************** */
#include <math.h>
#include <vector>
#include <stdio.h>
#include <stdlib.h>
#include <fstream>
#include <algorithm>
#include <random>
#include <iostream>
#include <time.h>
#include <stack>
#include <assert.h>
//include Eigen
// #include "eigen3/Eigen/Dense"
// #include "eigen3/Eigen/Sparse"
// #include "eigen3/Eigen/SparseLU"
// #include "eigen3/Eigen/SparseQR"
// #include "eigen3/Eigen/SparseCholesky"
// #include "eigen3/Eigen/IterativeLinearSolvers"
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <Eigen/SparseQR>
#include <Eigen/SparseCholesky>
#include <Eigen/IterativeLinearSolvers>
typedef Eigen::SparseMatrix<double > SpMat;
typedef Eigen::Triplet<double> T;
#include <typeinfo>
#include "MarketIO.h"
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/eigen.h>

//include MEX related files
// #include "mex.h"
// #include "matrix.h"

/* ******************** */
/* State Variable Class */
/* ******************** */

Eigen::MatrixXd empty;
Eigen::ArrayXd emptyAry;

namespace py = pybind11;
using namespace std;
using MatrixXdR = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

/* Timer functions                                    */
/******************************************************/

std::stack<clock_t> tictoc_stack;

void tic() {
    tictoc_stack.push(clock());
}

void toc() {
    std::cout << "Time elapsed: "
    << ((double)(clock() - tictoc_stack.top())) / CLOCKS_PER_SEC
    << std::endl;
    tictoc_stack.pop();
}

class stateVars {

public:
    Eigen::MatrixXd stateMat; //matrix to store state variables
    Eigen::MatrixXd stateMatNorm; //matrix to store normalized state variables [-1,1]
    Eigen::ArrayXd increVec; //vector to record steps
    Eigen::ArrayXd dVec; //vector to record steps
    int N; // num of dimensions
    int S; // number of rows for the grid
    Eigen::ArrayXd upperLims;
    Eigen::ArrayXd lowerLims;
    Eigen::ArrayXd gridSizes;

    stateVars (Eigen::ArrayXd, Eigen::ArrayXd, Eigen::ArrayXd); //constructors with arrays of upper/lower bounds and gridsizes
    stateVars (Eigen::MatrixXd); //constructors by loading in data

};

stateVars::stateVars (Eigen::ArrayXd upper, Eigen::ArrayXd lower, Eigen::ArrayXd gridSizes) {

    upperLims = upper;
    lowerLims = lower;
    N = upperLims.size();
    S = gridSizes.prod();
    stateMat.resize(S,N);
    dVec.resize(N);
    increVec.resize(N);
    increVec(0) = 1;

    //fill in the state object; similar to the ndgrid function in MATLAB

    for (int n = 0; n < N; ++n) {

        if (n != 0) {
            increVec(n) = gridSizes(n - 1) * increVec(n - 1);
        }
        dVec(n) = (upper(n) - lower(n)) / (gridSizes(n) - 1);

        for (int i = 0; i < S; ++i) {
            stateMat(i,n) = lower(n) + dVec(n) * ( int(i /  increVec(n) ) % int( gridSizes(n) ) );
        }

    }

}

stateVars::stateVars (Eigen::MatrixXd preLoad) {

    //fill in stateMat based on data loaded
    N = preLoad.cols();
    S = preLoad.rows();
    stateMat.resize(S,N);
    dVec.resize(N); dVec.setZero();
    increVec.resize(N); increVec.setZero();
    upperLims.resize(N); lowerLims.resize(N);
    for (int j = 0; j < preLoad.cols(); ++j) {
        upperLims(j) = preLoad.col(j).maxCoeff();
        lowerLims(j) = preLoad.col(j).minCoeff();
    }

    stateMat = preLoad;

    //figure out dVec and increVec
    for (int i = 1; i < S; ++i) {
        for (int n = 0; n < N; ++n ) {
            double diff = stateMat(i,n) - stateMat(i-1,n);
            if (diff > 0 && dVec(n) == 0 && increVec(n) == 0) {
                dVec(n) = diff;
                increVec(n) = i;
            }
        }

    }

}

struct bc {
    double a0;
    double a0S;
    bool natural;
    Eigen::ArrayXd level;
    Eigen::ArrayXd first;
    Eigen::ArrayXd second;

    bc(int d) {
        level.resize(d); first.resize(d); second.resize(d);
    }
};

struct elas {
    Eigen::MatrixXd elas1sc;
    Eigen::MatrixXd elas1c; //exposure elas
    Eigen::MatrixXd elas2sc;
    Eigen::MatrixXd elas2c; //exposure elas
    Eigen::MatrixXd elas1p; //price elas
    Eigen::MatrixXd elas2p;  //price elas

    elas(int T, int S) {
        elas1sc.resize(S,T); elas1c.resize(S,T); elas1p.resize(S,T);
        elas2sc.resize(S,T); elas2c.resize(S,T); elas2p.resize(S,T);
    }
};

class linearSysVars {

public:
    double dt;
    int k;
    Eigen::MatrixXd A;
    Eigen::MatrixXd B;
    Eigen::MatrixXd C;
    Eigen::MatrixXd D;

    Eigen::ArrayXd atBoundIndicators;

    std::vector<T> matList;
    std::string solverType;
    SpMat Le;

    //member functions

    //constructor
    linearSysVars(stateVars & state_vars, Eigen::MatrixXd A, Eigen::MatrixXd B, Eigen::MatrixXd C, Eigen::MatrixXd D, double dt);

    //function to construt matrix

    void constructMatFT(stateVars & state_vars);
    void constructMatFK(stateVars & state_vars);

};

linearSysVars::linearSysVars(stateVars & state_vars, Eigen::MatrixXd AInput, Eigen::MatrixXd BInput, Eigen::MatrixXd CInput, Eigen::MatrixXd DInput, double dtInput) {

    Le.resize(state_vars.S,state_vars.S);
    A.resize(state_vars.S,1); B.resize(state_vars.S,state_vars.N);
    C.resize(state_vars.S,state_vars.N); D.resize(state_vars.S,1);
    A = AInput; B = BInput; C = CInput; D = DInput;
    dt = dtInput;

}

void linearSysVars::constructMatFT(stateVars & state_vars) {
    matList.clear();
    matList.reserve(10 * state_vars.S);
    atBoundIndicators.resize(state_vars.N);
    double atBound = -1;
    double upperBound = -1;
    //construct matrix

    for (int i = 0; i < state_vars.S; ++i) {
        //level and time deriv

        atBound = -1;
        //check boundaries

        matList.push_back(T(i,i, (1.0 - dt * A(i,0))  ));

        for (int n  = (state_vars.N - 1); n >=0; --n ) {

            atBoundIndicators(n) = -1.0;

            double firstCoefE = B(i,n);

            double secondCoefE = C(i,n);

            //check whether it's at upper or lower boundary
            if ( std::abs(state_vars.stateMat(i,n) - state_vars.upperLims(n)) < state_vars.dVec(n)/2.0 ) {  //upper boundary
                atBoundIndicators(n) = 1.0;

                atBound = 1.0;
                upperBound = 1.0;
                /* Uncomment this section if you want natural boundaries */

                 matList.push_back(T(i, i, - dt * ( firstCoefE/state_vars.dVec(n) + secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                 matList.push_back(T(i, i - state_vars.increVec(n), - dt * ( - firstCoefE/state_vars.dVec(n) - 2 * secondCoefE / pow(state_vars.dVec(n), 2) ) ));
                 matList.push_back(T(i, i - 2*state_vars.increVec(n), - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );

                /* Uncomment this section if you want first derivatives = constant
                 matList.push_back(T(i,i, - (1.0 - dt * A(i,0) ) ));
                /*
                 matList.push_back(T(i, i, - dt * ( 1.0/state_vars.dVec(n)  ) ) );
                 matList.push_back(T(i, i - state_vars.increVec(n), - dt * ( - 1.0/state_vars.dVec(n)  ) ));*/
                 /*
                if ((n == 0) && atBoundIndicators(1) > 0 ) {
                 matList.push_back(T(i, i,  dt * ( firstCoefE/state_vars.dVec(n)  ) ) );
                 matList.push_back(T(i, i - state_vars.increVec(n),  dt * ( - firstCoefE/state_vars.dVec(n)  ) ));
                }*/
                /* Uncomment this section if you want second derivatives = constant  */
                //matList.push_back(T(i,i, - (1.0 - dt * A(i,0) )  ));
                /*
                matList.push_back(T(i, i, - dt * (  secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                matList.push_back(T(i, i - state_vars.increVec(n), - dt * ( - 2 * secondCoefE / pow(state_vars.dVec(n), 2) ) ));
                matList.push_back(T(i, i - 2*state_vars.increVec(n), - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                */
                /*
                matList.push_back(T(i, i, - dt * (  1.0 / pow(state_vars.dVec(n), 2) ) ) );
                matList.push_back(T(i, i - state_vars.increVec(n), - dt * ( - 2 *  1.0 / pow(state_vars.dVec(n), 2) ) ));
                matList.push_back(T(i, i - 2*state_vars.increVec(n), - dt * ( 1.0 / pow(state_vars.dVec(n), 2) ) ) );
                */
            } else if ( std::abs(state_vars.stateMat(i,n) - state_vars.lowerLims(n)) < state_vars.dVec(n)/2.0 ) { //lower boundary

                atBoundIndicators(n) = 1.0;
                atBound = 1.0;

                ///* Uncomment this section if you want natural boundaries

                 matList.push_back(T(i, i, - dt * ( - firstCoefE/state_vars.dVec(n) + secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                 matList.push_back(T(i, i + state_vars.increVec(n), - dt * ( firstCoefE/state_vars.dVec(n) - 2 * secondCoefE / pow(state_vars.dVec(n), 2) ) ));
                 matList.push_back(T(i, i + 2*state_vars.increVec(n), - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );

                //*/
                /* Uncomment this section if you want first derivatives = constant
                 */
                // matList.push_back(T(i,i, - (1.0 - dt * A(i,0) ) ));
                // matList.push_back(T(i, i, - dt * ( - 1/state_vars.dVec(n)  ) ) );
                // matList.push_back(T(i, i + state_vars.increVec(n), - dt * ( 1/state_vars.dVec(n) ) ));
                 /*
                if ((n == 0) && atBoundIndicators(1) > 0 ) {
                 matList.push_back(T(i, i,  dt * ( - firstCoefE/state_vars.dVec(n)  ) ) );
                 matList.push_back(T(i, i + state_vars.increVec(n),  dt * ( firstCoefE/state_vars.dVec(n) ) ));
                }*/
                /* Uncomment this section if you want second derivatives = constant  */
                //matList.push_back(T(i,i, - (1.0 - dt * A(i,0) )/ state_vars.N ));
                /*
                matList.push_back(T(i, i, - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                matList.push_back(T(i, i + state_vars.increVec(n), - dt * (  - 2 * secondCoefE / pow(state_vars.dVec(n), 2) ) ));
                matList.push_back(T(i, i + 2*state_vars.increVec(n), - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                */
                /*
                    matList.push_back(T(i, i, - dt * ( 1.0 / pow(state_vars.dVec(n), 2) ) ) );
                    matList.push_back(T(i, i + state_vars.increVec(n), - dt * (  - 2 * 1.0 / pow(state_vars.dVec(n), 2) ) ));
                    matList.push_back(T(i, i + 2*state_vars.increVec(n), - dt * ( 1.0 / pow(state_vars.dVec(n), 2) ) ) );
                */

            }

        }

        if (atBound < 0 ) {
          // matList.push_back(T(i,i, (1.0 - dt * A(i,0))  ));
        }
        for (int n = (state_vars.N - 1); n >= 0; --n) {

            //add elements to the vector of triplets for matrix construction
            if ( atBoundIndicators(n) < 0) {
                double firstCoefE = B(i,n);
                double secondCoefE = C(i,n);

                //first derivative
                 matList.push_back(T(i,i, - dt * ( -firstCoefE * ( firstCoefE > 0) + firstCoefE * ( firstCoefE < 0) ) / state_vars.dVec(n)  ) );
                 matList.push_back(T(i,i + state_vars.increVec(n), - dt * firstCoefE * ( firstCoefE > 0) / state_vars.dVec(n) ));
                 matList.push_back(T(i,i - state_vars.increVec(n), - dt *  - firstCoefE * ( firstCoefE < 0) / state_vars.dVec(n) ));

                    matList.push_back(T(i, i, - dt * -2 * secondCoefE / ( pow(state_vars.dVec(n), 2) ) ));
                    matList.push_back(T(i, i + state_vars.increVec(n), - dt * secondCoefE / ( pow(state_vars.dVec(n), 2) ) ));
                    matList.push_back(T(i, i - state_vars.increVec(n), - dt * secondCoefE / ( pow(state_vars.dVec(n), 2) ) ));

            }

        }

    }
    //form matrices

    Le.setFromTriplets(matList.begin(), matList.end());

    //compress
    Le.makeCompressed();
}

void linearSysVars::constructMatFK(stateVars & state_vars) {
    matList.clear();
    matList.reserve(10 * state_vars.S);
    atBoundIndicators.resize(state_vars.N);
    double atBound = -1;
    double upperBound = -1;
    //construct matrix

    for (int i = 0; i < state_vars.S; ++i) {
        //level and time deriv

        atBound = -1;
        //check boundaries

        matList.push_back(T(i,i, (0.0 - dt * A(i,0))  ));

        for (int n  = (state_vars.N - 1); n >=0; --n ) {

            atBoundIndicators(n) = -1.0;

            double firstCoefE = B(i,n);

            double secondCoefE = C(i,n);

            //check whether it's at upper or lower boundary
            if ( std::abs(state_vars.stateMat(i,n) - state_vars.upperLims(n)) < state_vars.dVec(n)/2.0 ) {  //upper boundary
                atBoundIndicators(n) = 1.0;

                atBound = 1.0;
                upperBound = 1.0;
                /* Uncomment this section if you want natural boundaries */

                 matList.push_back(T(i, i, - dt * ( firstCoefE/state_vars.dVec(n) + secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                 matList.push_back(T(i, i - state_vars.increVec(n), - dt * ( - firstCoefE/state_vars.dVec(n) - 2 * secondCoefE / pow(state_vars.dVec(n), 2) ) ));
                 matList.push_back(T(i, i - 2*state_vars.increVec(n), - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );

                /* Uncomment this section if you want first derivatives = constant
                 matList.push_back(T(i,i, - (1.0 - dt * A(i,0) ) ));
                /*
                 matList.push_back(T(i, i, - dt * ( 1.0/state_vars.dVec(n)  ) ) );
                 matList.push_back(T(i, i - state_vars.increVec(n), - dt * ( - 1.0/state_vars.dVec(n)  ) ));*/
                 /*
                if ((n == 0) && atBoundIndicators(1) > 0 ) {
                 matList.push_back(T(i, i,  dt * ( firstCoefE/state_vars.dVec(n)  ) ) );
                 matList.push_back(T(i, i - state_vars.increVec(n),  dt * ( - firstCoefE/state_vars.dVec(n)  ) ));
                }*/
                /* Uncomment this section if you want second derivatives = constant  */
                //matList.push_back(T(i,i, - (1.0 - dt * A(i,0) )  ));
                /*
                matList.push_back(T(i, i, - dt * (  secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                matList.push_back(T(i, i - state_vars.increVec(n), - dt * ( - 2 * secondCoefE / pow(state_vars.dVec(n), 2) ) ));
                matList.push_back(T(i, i - 2*state_vars.increVec(n), - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                */
                /*
                matList.push_back(T(i, i, - dt * (  1.0 / pow(state_vars.dVec(n), 2) ) ) );
                matList.push_back(T(i, i - state_vars.increVec(n), - dt * ( - 2 *  1.0 / pow(state_vars.dVec(n), 2) ) ));
                matList.push_back(T(i, i - 2*state_vars.increVec(n), - dt * ( 1.0 / pow(state_vars.dVec(n), 2) ) ) );
                */
            } else if ( std::abs(state_vars.stateMat(i,n) - state_vars.lowerLims(n)) < state_vars.dVec(n)/2.0 ) { //lower boundary

                atBoundIndicators(n) = 1.0;
                atBound = 1.0;

                ///* Uncomment this section if you want natural boundaries

                 matList.push_back(T(i, i, - dt * ( - firstCoefE/state_vars.dVec(n) + secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                 matList.push_back(T(i, i + state_vars.increVec(n), - dt * ( firstCoefE/state_vars.dVec(n) - 2 * secondCoefE / pow(state_vars.dVec(n), 2) ) ));
                 matList.push_back(T(i, i + 2*state_vars.increVec(n), - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );

                //*/
                /* Uncomment this section if you want first derivatives = constant
                 */
                // matList.push_back(T(i,i, - (1.0 - dt * A(i,0) ) ));
                // matList.push_back(T(i, i, - dt * ( - 1/state_vars.dVec(n)  ) ) );
                // matList.push_back(T(i, i + state_vars.increVec(n), - dt * ( 1/state_vars.dVec(n) ) ));
                 /*
                if ((n == 0) && atBoundIndicators(1) > 0 ) {
                 matList.push_back(T(i, i,  dt * ( - firstCoefE/state_vars.dVec(n)  ) ) );
                 matList.push_back(T(i, i + state_vars.increVec(n),  dt * ( firstCoefE/state_vars.dVec(n) ) ));
                }*/
                /* Uncomment this section if you want second derivatives = constant  */
                //matList.push_back(T(i,i, - (1.0 - dt * A(i,0) )/ state_vars.N ));
                /*
                matList.push_back(T(i, i, - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                matList.push_back(T(i, i + state_vars.increVec(n), - dt * (  - 2 * secondCoefE / pow(state_vars.dVec(n), 2) ) ));
                matList.push_back(T(i, i + 2*state_vars.increVec(n), - dt * ( secondCoefE / pow(state_vars.dVec(n), 2) ) ) );
                */
                /*
                    matList.push_back(T(i, i, - dt * ( 1.0 / pow(state_vars.dVec(n), 2) ) ) );
                    matList.push_back(T(i, i + state_vars.increVec(n), - dt * (  - 2 * 1.0 / pow(state_vars.dVec(n), 2) ) ));
                    matList.push_back(T(i, i + 2*state_vars.increVec(n), - dt * ( 1.0 / pow(state_vars.dVec(n), 2) ) ) );
                */

            }

        }

        if (atBound < 0 ) {
          // matList.push_back(T(i,i, (1.0 - dt * A(i,0))  ));
        }
        for (int n = (state_vars.N - 1); n >= 0; --n) {

            //add elements to the vector of triplets for matrix construction
            if ( atBoundIndicators(n) < 0) {
                double firstCoefE = B(i,n);
                double secondCoefE = C(i,n);

                //first derivative
                 matList.push_back(T(i,i, - dt * ( -firstCoefE * ( firstCoefE > 0) + firstCoefE * ( firstCoefE < 0) ) / state_vars.dVec(n)  ) );
                 matList.push_back(T(i,i + state_vars.increVec(n), - dt * firstCoefE * ( firstCoefE > 0) / state_vars.dVec(n) ));
                 matList.push_back(T(i,i - state_vars.increVec(n), - dt *  - firstCoefE * ( firstCoefE < 0) / state_vars.dVec(n) ));

                    matList.push_back(T(i, i, - dt * -2 * secondCoefE / ( pow(state_vars.dVec(n), 2) ) ));
                    matList.push_back(T(i, i + state_vars.increVec(n), - dt * secondCoefE / ( pow(state_vars.dVec(n), 2) ) ));
                    matList.push_back(T(i, i - state_vars.increVec(n), - dt * secondCoefE / ( pow(state_vars.dVec(n), 2) ) ));

            }

        }

    }
    //form matrices

    Le.setFromTriplets(matList.begin(), matList.end());

    //compress
    Le.makeCompressed();
}

py::tuple solveFT(Eigen::Ref<MatrixXdR> preLoadMat, Eigen::Ref<MatrixXdR> A, Eigen::Ref<MatrixXdR> B, Eigen::Ref<MatrixXdR> C,  Eigen::Ref<MatrixXdR> D, Eigen::Ref<MatrixXdR> v0, double dt, int tol)
{
    py::tuple data(3);
    stateVars stateSpace(preLoadMat);

    linearSysVars linearSys_vars(stateSpace, A,B,C,D,dt);
    linearSys_vars.constructMatFT(stateSpace);

    Eigen::VectorXd rhs;

    rhs = v0.array() + dt * D.array(); // transform v0 into rhs
    /*********************************************/
    /* Change RHS to reflect boundary conditions */
    /*********************************************/

    //construct matrix
    /* uncomment this section if you want to set the boundary conditions to a constant */
    // for (int i = 0; i < stateSpace.S; ++i) {

    //     for (int n = (stateSpace.N - 1); n >=0; --n ) {

    //         //check whether it's at upper or lower boundary
    //         if ( std::abs(stateSpace.stateMat(i,n) - stateSpace.upperLims(n)) < stateSpace.dVec(n)/2 ) {  //upper boundary
    //          //   v0(i) = 0.0001;
    //         } else if ( std::abs( stateSpace.stateMat(i,n) - stateSpace.lowerLims(n)) < stateSpace.dVec(n)/2 ) { //lower boundary
    //          //v0(i) = 0.0001;
    //         }
    //     }
    // }

    /* Initialize Eigen's cg solver */

    Eigen::VectorXd XiEVector;
    Eigen::LeastSquaresConjugateGradient<SpMat > cgE;
    // cgE.setMaxIterations(10000);
    cgE.setTolerance( pow(10,tol) );
    cgE.compute(linearSys_vars.Le);

    XiEVector = cgE.solveWithGuess(rhs, v0);
    data[0] = int(cgE.iterations());
    data[1] = cgE.error();
    data[2] = XiEVector;
    return data;

}

py::tuple solveFK(Eigen::Ref<MatrixXdR> preLoadMat, Eigen::Ref<MatrixXdR> A, Eigen::Ref<MatrixXdR> B, Eigen::Ref<MatrixXdR> C,  Eigen::Ref<MatrixXdR> D, Eigen::Ref<MatrixXdR> v0, int iters)
{
    py::tuple data(3);
    stateVars stateSpace(preLoadMat);
    double dt(1.0);
    linearSysVars linearSys_vars(stateSpace, A,B,C,D,dt);
    linearSys_vars.constructMatFK(stateSpace);

    Eigen::VectorXd rhs;
    rhs =  dt * D.array(); // transform v0 into rhs
    /*********************************************/
    /* Change RHS to reflect boundary conditions */
    /*********************************************/

    //construct matrix
    /* uncomment this section if you want to set the boundary conditions to a constant */
    for (int i = 0; i < stateSpace.S; ++i) {

        for (int n = (stateSpace.N - 1); n >=0; --n ) {

            //check whether it's at upper or lower boundary
            if ( std::abs(stateSpace.stateMat(i,n) - stateSpace.upperLims(n)) < stateSpace.dVec(n)/2 ) {  //upper boundary
             //   v0(i) = 0.0001;
            } else if ( std::abs( stateSpace.stateMat(i,n) - stateSpace.lowerLims(n)) < stateSpace.dVec(n)/2 ) { //lower boundary
             //v0(i) = 0.0001;
            }
        }
    }

    /* Initialize Eigen's cg solver */
    Eigen::VectorXd XiEVector;
    Eigen::LeastSquaresConjugateGradient<SpMat > cgE;
    cgE.setMaxIterations(iters);
    cgE.setTolerance( 0.000001 );
    cgE.compute(linearSys_vars.Le);  // update with Sparse matrix A
    XiEVector = cgE.solveWithGuess(rhs,v0);  // (rhs, guess)
    data[0] = int(cgE.iterations());
    data[1] = cgE.error();
    data[2] = XiEVector;

    return data;

}
/*************************************/
/* Using pybind11 to interface       */
/* with python                       */
/*************************************/

PYBIND11_MODULE(SolveLinSys,m){
    m.doc() = "PDE Solver in cpp";

    m.def("solveFT", &solveFT, py::arg("stateSpace"),
        py::arg("A"), py::arg("B"), py::arg("C"), py::arg("D"),
        py::arg("v0"), py::arg("dt"), py::arg("tol"));

    m.def("solveFK", &solveFK, py::arg("stateSpace"),
        py::arg("A"), py::arg("B"), py::arg("C"), py::arg("D"),
        py::arg("v0"), py::arg("iters"));

}


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    References
 *      Press W.H., et al. Numerical Recipes in C++: The Art of Scientific Computing. Cambridge
 *          University Press, February 2002.
 *
 */

#include <boost/multi_array.hpp>

#include "Tudat/Mathematics/Interpolators/linearInterpolator.h"

namespace tudat
{
namespace interpolators
{

//! Compute linear interpolation.
double computeLinearInterpolation( const Eigen::VectorXd& sortedIndependentVariables,
                                   const Eigen::VectorXd& associatedDependentVariables,
                                   const double targetIndependentVariableValue )
{
    // Declare local variables.
    // Declare nearest neighbor.
    int nearestNeighbor;
    double locationTargetIndependentVariableValueInInterval;

    // Compute nearest neighbor in sorted vector of independent variables.
    // Result is always to the left of the target independent variable value.
    nearestNeighbor = basic_mathematics::computeNearestLeftNeighborUsingBinarySearch(
            sortedIndependentVariables, targetIndependentVariableValue );

    // Compute location of target independent variable value in interval
    // between nearest neighbors.
    locationTargetIndependentVariableValueInInterval
            = ( targetIndependentVariableValue
              - sortedIndependentVariables[ nearestNeighbor ] )
             / ( sortedIndependentVariables[ nearestNeighbor + 1 ]
                 - sortedIndependentVariables[ nearestNeighbor ] );

    // Return the computed value of the dependent variable.
    return ( associatedDependentVariables[ nearestNeighbor ]
             * ( 1 - locationTargetIndependentVariableValueInInterval )
             + associatedDependentVariables[ nearestNeighbor + 1 ]
             * locationTargetIndependentVariableValueInInterval );
}

//! Compute linear interpolation.
Eigen::VectorXd computeLinearInterpolation(
        const std::map < double, Eigen::VectorXd >& sortedIndepedentAndDependentVariables,
        const double targetIndependentVariableValue )
{
    // Declare local variables.
    // Declare nearest neighbor.
    int nearestLeftNeighbor;

    // Declare location of target independent variable value in interval.
    double locationTargetIndependentVariableValueInInterval;

    // Declare map iterators
    std::map< double, Eigen::VectorXd >::const_iterator mapIteratorIntervalLeft;
    std::map< double, Eigen::VectorXd >::const_iterator mapIteratorIntervalRight;

    // Compute nearest neighbor in map of data.
    // Result is always to the left of the target independent variable value.
    nearestLeftNeighbor = basic_mathematics::computeNearestLeftNeighborUsingBinarySearch(
                sortedIndepedentAndDependentVariables, targetIndependentVariableValue );

    // Compute location of target independent variable value in interval
    // between nearest neighbors.
    mapIteratorIntervalLeft = sortedIndepedentAndDependentVariables.begin( );
    advance( mapIteratorIntervalLeft, nearestLeftNeighbor );
    mapIteratorIntervalRight = sortedIndepedentAndDependentVariables.begin( );
    advance( mapIteratorIntervalRight, nearestLeftNeighbor + 1 );
    locationTargetIndependentVariableValueInInterval
            = ( targetIndependentVariableValue
              - mapIteratorIntervalLeft->first )
             / ( mapIteratorIntervalRight->first
                 - mapIteratorIntervalLeft->first );

    // Return the computed value of the dependent variable.
    return ( mapIteratorIntervalLeft->second
             * ( 1 - locationTargetIndependentVariableValueInInterval )
             + mapIteratorIntervalRight->second
             * locationTargetIndependentVariableValueInInterval );
}

template class LinearInterpolator< double, Eigen::VectorXd >;
template class LinearInterpolator< double, Eigen::Vector6d >;
template class LinearInterpolator< double, Eigen::MatrixXd >;

template class LinearInterpolator< double, Eigen::Matrix< long double, Eigen::Dynamic, 1 > >;
template class LinearInterpolator< double, Eigen::Matrix< long double, Eigen::Dynamic, 6 > >;
template class LinearInterpolator< double, Eigen::Matrix< long double, Eigen::Dynamic,  Eigen::Dynamic > >;

} // namespace interpolators
} // mamespace tudat


/**
 * @file radauthreetimestepping_main.cc
 * @brief NPDE homework RadauThreeTimestepping
 * @author Erick Schulz
 * @date 08/04/2019
 * @copyright Developed at ETH Zurich
 */

#include "radauthreetimestepping.h"
#include "radauthreetimesteppingode.h"

#include <iostream>
#include <memory>

#include <Eigen/Core>

#include <lf/assemble/assemble.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

using namespace RadauThreeTimestepping;

int main(int /*argc*/, char ** /*argv*/) {
  /* Solving the ODE problem */
  // This function prints to the terminal the convergence rates and average rate
  // of a convergence study performed for the ODE (d/dt)y = -y.
  testConvergenceTwoStageRadauLinScalODE();

  /* Solving the parabolic heat equation */
  // Create a Lehrfem++ square tensor product mesh
  lf::mesh::hybrid2d::TPTriagMeshBuilder builder(
      std::make_unique<lf::mesh::hybrid2d::MeshFactory>(2));
  // Set mesh parameters following the Builder pattern
  // Domain is the unit square
  builder.setBottomLeftCorner(Eigen::Vector2d{-1.0, -1.0})
      .setTopRightCorner(Eigen::Vector2d{1, 1})
      .setNumXCells(50)
      .setNumYCells(50);
  auto mesh_p = builder.Build();

  /* SAM_LISTING_BEGIN_1 */
#if SOLUTION
  // Generate the linear lagrange FE data
  // Finite element space
  auto fe_space =
      std::make_shared<lf::uscalfe::FeSpaceLagrangeO1<double>>(mesh_p);
  // Obtain local->global index mapping for current finite element space
  const lf::assemble::DofHandler &dofh{fe_space->LocGlobMap()};
  // Dimension of finite element space
  const lf::uscalfe::size_type N_dofs(dofh.NumDofs());

  // Solve heat evolution with zero initial and boundary conditions
  double final_time = 1.0;
  unsigned int m = 50;
  Eigen::VectorXd discrete_heat_solution =
      solveHeatEvolution(dofh, m, final_time);
  LF_ASSERT_MSG(
      discrete_heat_solution.size() == N_dofs,
      "Size of discrete solution and dimension of FE space mismatch.");

  // Output results to vtk file
  lf::io::VtkWriter vtk_writer(
      mesh_p, CURRENT_BINARY_DIR "/discrete_heat_solution.vtk");
  // Write nodal data taking the values of the discrete solution at the vertices
  auto nodal_data = lf::mesh::utils::make_CodimMeshDataSet<double>(mesh_p, 2);
  for (int global_idx = 0; global_idx < N_dofs; global_idx++) {
    nodal_data->operator()(dofh.Entity(global_idx)) =
        discrete_heat_solution[global_idx];
  };
  vtk_writer.WritePointData("discrete_heat_solution", *nodal_data);
  /* SAM_LISTING_END_1 */
  std::cout << "\n The discrete_heat_solution was written to:" << std::endl;
  std::cout << ">> discrete_heat_solution.vtk\n" << std::endl;
#else
  //====================
  // Your code goes here
  //====================
#endif

  return 0;
}


#include "Solvers.h"
#include <Eigen/PardisoSupport>

int Temporalsolver(string typeSolver, double t_Incre, int maxIter, MatrixXd& T)
{
    // ****************************************************************************************************************************************************
    //                          EULER               //                     Solved by Euler integration. It is the fastest but less stable method.
    // ****************************************************************************************************************************************************
    if (typeSolver == "Euler")
    {
        int size;                         //Number on nodes
        int boundaryNodes;                //Number of nodes with boundary condition

        SparseMatrix<double> kl_S;        //Vectors and matrices incluiding boundary conditions
        SparseMatrix<double> kr_S;
        VectorXd T0;
        VectorXd QL;
        VectorXd c;
        double time = 0;

        int count;
        VectorXd T_4;
        VectorXd T_col;
        MatrixXd c_diago;

        count = 0;

        do
        {
            ObjectsDefinition(size, boundaryNodes, kl_S, kr_S, T0, QL, c, T.col(count), time);

            if (count == 0)
            {
                T.col(0) = T0;                   // Initial conditions
            }

            c = c.cwiseInverse();
            c_diago = c.asDiagonal();
            T_col = T.col(count);
            T_4 = T_col.array().pow(4);

            T.col(count + 1) = T.col(count) + t_Incre * c_diago * (kl_S.selfadjointView<Upper>() * T.col(count) + QL + kr_S.selfadjointView<Upper>() * T_4);          //Finite differential equation

            time += t_Incre;
            count++;
            std::cout << "count" << count << "\n";
        } while (count + 1 < maxIter);
    }

    // ****************************************************************************************************************************************************
    //      ADAMS BASHFORTH 2      //      Second order explicit method --- U(n+1) <- U(n) + Dt/2 ( 3 F(n)-F(U(n-1) ) --- x2 times slower than Euler
    // ****************************************************************************************************************************************************
    else if (typeSolver == "AB2")
    {
        int size;                         //Number on nodes
        int boundaryNodes;                //Number of nodes with boundary condition

        SparseMatrix<double> kl_S;        //Vectors and matrices incluiding boundary conditions
        SparseMatrix<double> kr_S;
        VectorXd T0;
        VectorXd QL;
        VectorXd c;
        double time = 0;

        int count;
        VectorXd T_4_0;
        VectorXd T_col_0;
        VectorXd F_0;
        MatrixXd c_diago;

        VectorXd T_4_1;
        VectorXd T_col_1;
        VectorXd F_1;

        count = 0;

        do
        {
            ObjectsDefinition(size, boundaryNodes, kl_S, kr_S, T0, QL, c, T.col(count), time);

            c = c.cwiseInverse();
            c_diago = c.asDiagonal();

            if (count == 0)
            {
                T.col(0) = T0;                             // Initial conditions
            }

            //count
            T_col_0 = T.col(count);
            T_4_0 = T_col_0.array().pow(4);
            F_0 = c_diago * (kl_S.selfadjointView<Upper>() * T.col(count) + QL + kr_S.selfadjointView<Upper>() * T_4_0);

            if (count == 0)
            {
                T.col(1) = T0 + t_Incre * F_0;             //Aproximation of second temperature term with Euler
            }

            //count + 1
            T_col_1 = T.col(count + 1);
            T_4_1 = T_col_1.array().pow(4);
            F_1 = c_diago * (kl_S.selfadjointView<Upper>() * T.col(count + 1) + QL + kr_S.selfadjointView<Upper>() * T_4_1);

            T.col(count + 2) = T.col(count + 1) + t_Incre * (3. / 2. * F_1 - 1. / 2. * F_0);          //Finite differences equation

            time += t_Incre;
            count++;
            std::cout << "count" << count << "\n";
        } while (count + 2 < maxIter);

    }

    // ****************************************************************************************************************************************************
    //                                            RUNGE KUTTA 4         // Fourth order explicit method. x4 times slower than Euler
    // ****************************************************************************************************************************************************
    else if (typeSolver == "RK4")
    {

        int size;                         //Number on nodes
        int boundaryNodes;                //Number of nodes with boundary condition

        SparseMatrix<double> kl_S;        //Vectors and matrices incluiding boundary conditions
        SparseMatrix<double> kr_S;
        VectorXd T0;
        VectorXd QL;
        VectorXd c;
        double time = 0;

        int count;
        VectorXd T_4;
        VectorXd T_col;
        MatrixXd c_diago;

        VectorXd k1, k2, k3, k4;

        count = 0;

        do
        {
            ObjectsDefinition(size, boundaryNodes, kl_S, kr_S, T0, QL, c, T.col(count), time);

            if (count == 0)
            {
                T.col(0) = T0;                   // Initial conditions
            }

            c = c.cwiseInverse();
            c_diago = c.asDiagonal();

            // k1
            T_col = T.col(count);
            T_4 = T_col.array().pow(4);
            k1 = c_diago * (kl_S.selfadjointView<Upper>() * T_col + QL + kr_S.selfadjointView<Upper>() * T_4);

            // k2
            T_col = T.col(count) + t_Incre / 2 * k1;
            T_4 = T_col.array().pow(4);
            k2 = c_diago * (kl_S.selfadjointView<Upper>() * T_col + QL + kr_S.selfadjointView<Upper>() * T_4);

            // k3
            T_col = T.col(count) + t_Incre / 2 * k2;
            T_4 = T_col.array().pow(4);
            k3 = c_diago * (kl_S.selfadjointView<Upper>() * T_col + QL + kr_S.selfadjointView<Upper>() * T_4);

            // k4
            T_col = T.col(count) + t_Incre * k3;
            T_4 = T_col.array().pow(4);
            k4 = c_diago * (kl_S.selfadjointView<Upper>() * T_col + QL + kr_S.selfadjointView<Upper>() * T_4);

            T.col(count + 1) = T.col(count) + 1. / 6. * t_Incre * (k1 + 2 * k2 + 2 * k3 + k4);          //Finite differential equation

            time += t_Incre;
            count++;
            std::cout << "count" << count << "\n";
        } while (count + 1 < maxIter);
    }

    else if (typeSolver == "CN")
    {
        int size;                         //Number on nodes
        int boundaryNodes;                //Number of nodes with boundary condition

        SparseMatrix<double> kl_S;          //Vectors and matrices incluiding boundary conditions
        SparseMatrix<double> kr_S;
        VectorXd T0;
        VectorXd QL;
        VectorXd c;
        double time = 0;

        SparseMatrix<double> kle_S;
        SparseMatrix<double> kre_S;
        VectorXd QLe;

        int count;
        VectorXd T_4;
        VectorXd T_col;
        SparseMatrix<double> c_diago;

        count = 0;

        do
        {
            ObjectsDefinition(size, boundaryNodes, kl_S, kr_S, T0, QL, c, T.col(count), time);

            if (count == 0)
            {
                T.col(0) = T0;                    //Initial conditions
            }

            size = T0.size();

            SparseMatrix<double> AuxS;
            AuxS = kl_S.selfadjointView<Upper>();
            kl_S = AuxS;
            AuxS = kr_S.selfadjointView<Upper>();
            kr_S = AuxS;

            MatrixXd Aux;
            c = c.cwiseInverse();
            Aux = c.asDiagonal();
            c_diago = Aux.sparseView();

            // QLe
            T_col = T.col(count);
            T_4 = T_col.array().pow(4);
            QLe = t_Incre / 2. * c_diago * (kl_S * T_col + 2 * QL + kr_S * T_4) + T_col;

            //kle
            SparseMatrix<double> I(size, size);
            I.setIdentity();
            kle_S = t_Incre / 2. * c_diago * kl_S - I;

            // kre
            kre_S = t_Incre / 2. * c_diago * kr_S;

            StableStationarySolverInternal(kle_S, kre_S, QLe, T_col);
            time += t_Incre;
            count++;

            T.col(count) = T_col;

        } while (count + 1 < maxIter);

    }

    return 0;
}


//******************************************************************************
//  Copyright(C) 2008-2013 Intel Corporation. All Rights Reserved.
//
//  The source code, information  and  material ("Material") contained herein is
//  owned  by Intel Corporation or its suppliers or licensors, and title to such
//  Material remains  with Intel Corporation  or its suppliers or licensors. The
//  Material  contains proprietary information  of  Intel or  its  suppliers and
//  licensors. The  Material is protected by worldwide copyright laws and treaty
//  provisions. No  part  of  the  Material  may  be  used,  copied, reproduced,
//  modified, published, uploaded, posted, transmitted, distributed or disclosed
//  in any way  without Intel's  prior  express written  permission. No  license
//  under  any patent, copyright  or  other intellectual property rights  in the
//  Material  is  granted  to  or  conferred  upon  you,  either  expressly,  by
//  implication, inducement,  estoppel or  otherwise.  Any  license  under  such
//  intellectual  property  rights must  be express  and  approved  by  Intel in
//  writing.
//
//  *Third Party trademarks are the property of their respective owners.
//
//  Unless otherwise  agreed  by Intel  in writing, you may not remove  or alter
//  this  notice or  any other notice embedded  in Materials by Intel or Intel's
//  suppliers or licensors in any way.
//
//******************************************************************************
// Content:
//     Intel(R) Math Kernel Library (MKL) overloaded Boost/uBLAS prod()
//******************************************************************************

#ifndef _MKL_BOOST_UBLAS_MATRIX_PROD_
#define _MKL_BOOST_UBLAS_MATRIX_PROD_

#ifdef NDEBUG

#include <boost/version.hpp>
#if defined (BOOST_VERSION) && (BOOST_VERSION >= 103401)

#include <boost/numeric/ublas/matrix.hpp>

#include "mkl_boost_ublas_gemm.hpp"

namespace boost { namespace numeric { namespace ublas {

    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( m1, m2 )
    prod(const matrix<T,F,A> &m1,
         const matrix<T,F,A> &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasNoTrans, CblasNoTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(m1), m2 )
    prod(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m1,
         const matrix<T,F,A> &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasTrans, CblasNoTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(conj(m1)), m2 )
    prod(const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m1,
         const matrix<T,F,A> &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasNoTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( conj(trans(m1)), m2 )
    prod(const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m1,
         const matrix<T,F,A> &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasNoTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( m1, trans(m2) )
    prod(const matrix<T,F,A> &m1,
         const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasNoTrans, CblasTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(m1), trans(m2) )
    prod(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m1,
         const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasTrans, CblasTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(conj(m1)), trans(m2) )
    prod(const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m1,
         const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( conj(trans(m1)), trans(m2) )
    prod(const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m1,
         const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( m1, trans(conj(m2)) )
    prod(const matrix<T,F,A> &m1,
         const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasNoTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(m1), trans(conj(m2)) )
    prod(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m1,
         const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(conj(m1)), trans(conj(m2)) )
    prod(const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m1,
         const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( conj(trans(m1)), trans(conj(m2)) )
    prod(const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m1,
         const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( m1, conj(trans(m2)) )
    prod(const matrix<T,F,A> &m1,
         const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasNoTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(m1), conj(trans(m2)) )
    prod(const matrix_unary2<matrix<T,F,A>,scalar_identity<T> > &m1,
         const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( trans(conj(m1)), conj(trans(m2)) )
    prod(const matrix_unary2<matrix_unary1<matrix<T,F,A>,scalar_conj<T> > const ,scalar_identity<T> > &m1,
         const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }
    template<class T, class F, class A>
    MKL_BOOST_UBLAS_INLINE
    matrix<T,F,A>    // prod( conj(trans(m1)), conj(trans(m2)) )
    prod(const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m1,
         const matrix_unary1<matrix_unary2<matrix<T,F,A>,scalar_identity<T> >,scalar_conj<T> > &m2)
    {
        matrix<T,F,A> temporary(m1.size1(),m2.size2());
        mkl::gemm(CblasConjTrans, CblasConjTrans, m1, m2, temporary);
        return temporary;
    }

}}}
#endif  // BOOST_VERSION
#endif  // NDEBUG
#endif  // _MKL_BOOST_UBLAS_MATRIX_PROD_


///////////////////////////////////////////////////////////////////////////////
// vector_space::example::difference.cpp                                     //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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 <vector>
#include <algorithm>
#include <functional>
#include <iterator>
#include <boost/bind.hpp>
#include <boost/range.hpp>
#include <boost/assert.hpp>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/vector_space/data/lazy_difference.hpp>

void example_difference(std::ostream& out){
    std::cout << "-> example_difference : ";
    using namespace boost;
    namespace vs = statistics::detail::vector_space;
    typedef double                          val_;
    typedef std::vector<val_>               vec_;
    typedef vs::lazy_difference<vec_,vec_>  range_diff_;

    const val_ eps = math::tools::epsilon<val_>();
    const val_ delta = eps * static_cast<val_>(2);

    vec_ vec;
    {
        using namespace assign;
        vec += 0.0, 1.1, 2.2, 3.3;
    }
    vec_ vec1(vec);
    std::transform(
        boost::begin(vec),
        boost::end(vec),
        begin(vec1),
        boost::bind<val_>(
            std::minus<val_>(),
            _1,
            delta
        )
    );

    range_diff_ range_diff(vec,vec1);
    vec_ copy_diff(size(vec));
    std::copy(
        const_begin(range_diff),
        const_end(range_diff),
        begin(copy_diff)
    );

   BOOST_FOREACH(const val_& z, copy_diff){
        BOOST_ASSERT(fabs(z-delta)<eps);
    }

    out << "<- " << std::endl;
}


#ifndef ALM_HERMITE_CURVE_VEC_H
#define ALM_HERMITE_CURVE_VEC_H

#include <Eigen/Dense>
#include <avatar_locomanipulation/helpers/hermite_curve.hpp>

class HermiteCurveVec{
public:
	HermiteCurveVec();
	HermiteCurveVec(const Eigen::VectorXd & start_pos, const Eigen::VectorXd & start_vel,
				   const Eigen::VectorXd & end_pos, const Eigen::VectorXd & end_vel);
	~HermiteCurveVec();
	Eigen::VectorXd evaluate(const double & s_in);
	Eigen::VectorXd evaluateFirstDerivative(const double & s_in);
	Eigen::VectorXd evaluateSecondDerivative(const double & s_in);

private:
	Eigen::VectorXd p1;
	Eigen::VectorXd v1;
	Eigen::VectorXd p2;
	Eigen::VectorXd v2;

	std::vector<HermiteCurve> curves;
 	Eigen::VectorXd output;
};

#endif


// -*- C++ -*-

#include <mtl/matrix.h>
#include <mtl/mtl.h>
#include <mtl/utils.h>

using namespace mtl;

typedef matrix< double, rectangle<>, dense<>, column_major>::type MATRIX;

int
main (int , char *[])
{
  MATRIX A(3, 5), B(5, 3);
  for (int j = 0; j < 5; ++j)
    for (int i = 0; i < 3; ++i)
      A(i,j) = i * 5 + j;

  transpose(A, B);
  print_all_matrix(A);
  print_all_matrix(B);
  exit (0);
}


/**
 * test eigensystem calculation
 * @author Tobias Weber <tweber@ill.fr>
 * @date 25-jul-20
 * @license GPLv3, see 'LICENSE' file
 *
 * g++ -std=c++20 -DUSE_LAPACK -I.. -I/usr/include/lapacke -Iext/lapacke/include -Lext/lapacke/lib -o eig eig.cpp -llapacke
 *
 * ----------------------------------------------------------------------------
 * tlibs
 * Copyright (C) 2017-2021  Tobias WEBER (Institut Laue-Langevin (ILL),
 *                          Grenoble, France).
 * Copyright (C) 2015-2017  Tobias WEBER (Technische Universitaet Muenchen
 *                          (TUM), Garching, Germany).
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3 of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * ----------------------------------------------------------------------------
 */

#define BOOST_TEST_MODULE Eigenvector Test
#include <boost/test/included/unit_test.hpp>
namespace test = boost::unit_test;
namespace testtools = boost::test_tools;

#include <iostream>
#include <vector>
#include <random>

#include "libs/maths.h"

// LinearAlgebra.eigen([-1.5 0.01 0.02; -0.04 1.0 0.03; -2.05 0.06 0.5])

using t_types = std::tuple<double, float>;
BOOST_AUTO_TEST_CASE_TEMPLATE(test_eig, t_real, t_types)
{
	using namespace tl2_ops;

	using t_cplx = std::complex<t_real>;
	using t_vec = tl2::vec<t_real, std::vector>;
	using t_mat = tl2::mat<t_real, std::vector>;
	using t_vec_cplx = tl2::vec<t_cplx, std::vector>;
	using t_mat_cplx = tl2::mat<t_cplx, std::vector>;

	t_real eps = 1e-4;
	std::size_t dim = 3;

	std::mt19937 rndgen{tl2::epoch<unsigned int>()};
	std::uniform_real_distribution<t_real> rnddist{-100, 100};

	// real version
	{
		std::cout << "--------------------------------------------------------------------------------" << std::endl;
		auto mat = tl2::zero<t_mat>(dim,dim);
		for(std::size_t i=0; i<dim; ++i)
			for(std::size_t j=0; j<dim; ++j)
				mat(i,j) = rnddist(rndgen);
		std::cout << mat << std::endl;

		bool sym = 0;
		auto [ok, evals_re, evals_im, evecs_re, evecs_im] =
			tl2_la::eigenvec<t_mat, t_vec, t_real>(mat, false, sym, true);
		std::cout << "ok = " << std::boolalpha << ok << "\n" << std::endl;

		for(std::size_t i=0; i<evals_re.size(); ++i)
			std::cout << "Re(eval) " << i+1 << ": " << evals_re[i] << std::endl;
		for(std::size_t i=0; i<evals_im.size(); ++i)
			std::cout << "Im(eval) " << i+1 << ": " << evals_im[i] << std::endl;
		std::cout << std::endl;

		for(std::size_t i=0; i<evecs_re.size(); ++i)
			std::cout << "Re(evec) " << i+1 << ": " << evecs_re[i] << std::endl;
		std::cout << std::endl;
		for(std::size_t i=0; i<evecs_im.size(); ++i)
			std::cout << "Im(evec) " << i+1 << ": " << evecs_im[i] << std::endl;
		std::cout << std::endl;

		BOOST_TEST(ok);

		t_mat_cplx mat_cplx = tl2::zero<t_mat_cplx>(dim, dim);
		for(std::size_t i=0; i<dim; ++i)
			for(std::size_t j=0; j<dim; ++j)
				mat_cplx(i,j) = mat(i,j);

		for(std::size_t i=0; i<dim; ++i)
		{
			t_cplx eval = t_cplx{evals_re[i], evals_im[i]};
			t_vec_cplx evec = tl2::zero<t_vec_cplx>(dim);

			for(std::size_t j=0; j<dim; ++j)
				evec[j] = t_cplx{evecs_re[i][j], evecs_im[i][j]};

			t_vec_cplx tstvec1 = mat_cplx * evec;
			t_vec_cplx tstvec2 = eval * evec;
			bool is_equal = tl2::equals<t_vec_cplx>(tstvec1, tstvec2, eps);
			std::cout << tstvec1 << " == " << tstvec2 << ": " << std::boolalpha << is_equal << std::endl;
			BOOST_TEST(is_equal);
		}

		std::cout << "--------------------------------------------------------------------------------\n" << std::endl;
	}

	// real version, symmetric
	{
		std::cout << "--------------------------------------------------------------------------------" << std::endl;
		auto mat = tl2::zero<t_mat>(dim,dim);
		for(std::size_t i=0; i<dim; ++i)
			for(std::size_t j=i; j<dim; ++j)
				mat(j,i) = mat(i,j) = rnddist(rndgen);
		std::cout << mat << std::endl;

		bool sym = 1;
		auto [ok, evals_re, evals_im, evecs_re, evecs_im] =
			tl2_la::eigenvec<t_mat, t_vec, t_real>(mat, false, sym, true);
		std::cout << "ok = " << std::boolalpha << ok << "\n" << std::endl;

		for(std::size_t i=0; i<evals_re.size(); ++i)
			std::cout << "Re(eval) " << i+1 << ": " << evals_re[i] << std::endl;
		for(std::size_t i=0; i<evals_im.size(); ++i)
			std::cout << "Im(eval) " << i+1 << ": " << evals_im[i] << std::endl;
		std::cout << std::endl;

		for(std::size_t i=0; i<evecs_re.size(); ++i)
			std::cout << "Re(evec) " << i+1 << ": " << evecs_re[i] << std::endl;
		std::cout << std::endl;
		for(std::size_t i=0; i<evecs_im.size(); ++i)
			std::cout << "Im(evec) " << i+1 << ": " << evecs_im[i] << std::endl;
		std::cout << std::endl;

		BOOST_TEST(ok);

		t_mat_cplx mat_cplx = tl2::zero<t_mat_cplx>(dim, dim);
		for(std::size_t i=0; i<dim; ++i)
			for(std::size_t j=0; j<dim; ++j)
				mat_cplx(i,j) = mat(i,j);

		for(std::size_t i=0; i<dim; ++i)
		{
			t_cplx eval = t_cplx{evals_re[i], evals_im[i]};
			t_vec_cplx evec = tl2::zero<t_vec_cplx>(dim);

			for(std::size_t j=0; j<dim; ++j)
				evec[j] = t_cplx{evecs_re[i][j], evecs_im[i][j]};

			t_vec_cplx tstvec1 = mat_cplx * evec;
			t_vec_cplx tstvec2 = eval * evec;
			bool is_equal = tl2::equals<t_vec_cplx>(tstvec1, tstvec2, eps);
			std::cout << tstvec1 << " == " << tstvec2 << ": " << std::boolalpha << is_equal << std::endl;
			BOOST_TEST(is_equal);
		}

		std::cout << "--------------------------------------------------------------------------------\n" << std::endl;
	}

	// complex version
	{
		std::cout << "--------------------------------------------------------------------------------" << std::endl;

		auto mat = tl2::zero<t_mat_cplx>(dim, dim);
		for(std::size_t i=0; i<dim; ++i)
			for(std::size_t j=0; j<dim; ++j)
				mat(i,j) = rnddist(rndgen) + rnddist(rndgen)*t_cplx{0, 1};
		std::cout << mat << std::endl;

		bool herm = 0;
		auto [ok, evals, evecs] =
			tl2_la::eigenvec<t_mat_cplx, t_vec_cplx, t_cplx>(mat, false, herm, true);
		std::cout << "ok = " << std::boolalpha << ok << "\n" << std::endl;

		for(std::size_t i=0; i<evecs.size(); ++i)
			std::cout << "eval " << i+1 << ": " << evals[i] << std::endl;
		std::cout << std::endl;
		for(std::size_t i=0; i<evecs.size(); ++i)
			std::cout << "evec " << i+1 << ": " << evecs[i] << std::endl;
		std::cout << std::endl;

		BOOST_TEST(ok);
		for(std::size_t i=0; i<dim; ++i)
		{
			t_vec_cplx tstvec1 = mat*evecs[i];
			t_vec_cplx tstvec2 = evals[i]*evecs[i];
			bool is_equal = tl2::equals<t_vec_cplx>(tstvec1, tstvec2, eps);
			std::cout << tstvec1 << " == " << tstvec2 << ": " << std::boolalpha << is_equal << std::endl;
			BOOST_TEST(is_equal);
		}

		std::cout << "--------------------------------------------------------------------------------\n" << std::endl;
	}

	// complex version, hermitian
	{
		std::cout << "--------------------------------------------------------------------------------" << std::endl;

		auto mat = tl2::zero<t_mat_cplx>(dim, dim);
		for(std::size_t i=0; i<dim; ++i)
		{
			for(std::size_t j=i+1; j<dim; ++j)
			{
				mat(i,j) = rnddist(rndgen) + rnddist(rndgen)*t_cplx{0, 1};
				mat(j,i) = std::conj(mat(i,j));
			}
		}
		for(std::size_t i=0; i<dim; ++i)
			mat(i,i) = rnddist(rndgen);
		std::cout << mat << std::endl;

		bool herm = 1;
		auto [ok, evals, evecs] =
			tl2_la::eigenvec<t_mat_cplx, t_vec_cplx, t_cplx>(mat, false, herm, true);
		std::cout << "ok = " << std::boolalpha << ok << "\n" << std::endl;

		for(std::size_t i=0; i<evecs.size(); ++i)
			std::cout << "eval " << i+1 << ": " << evals[i] << std::endl;
		std::cout << std::endl;
		for(std::size_t i=0; i<evecs.size(); ++i)
			std::cout << "evec " << i+1 << ": " << evecs[i] << std::endl;
		std::cout << std::endl;

		BOOST_TEST(ok);
		for(std::size_t i=0; i<dim; ++i)
		{
			t_vec_cplx tstvec1 = mat*evecs[i];
			t_vec_cplx tstvec2 = evals[i]*evecs[i];
			bool is_equal = tl2::equals<t_vec_cplx>(tstvec1, tstvec2, eps);
			std::cout << tstvec1 << " == " << tstvec2 << ": " << std::boolalpha << is_equal << std::endl;
			BOOST_TEST(is_equal);
		}

		std::cout << "--------------------------------------------------------------------------------\n" << std::endl;
	}
}


#include "ScatterDraw.h"
#include <Eigen/Eigen.h>

namespace Upp {
using namespace Eigen;

struct Equation_functor : NonLinearOptimizationFunctor<double> {
	DataSource *series;
	ExplicitEquation *fSource;
	Equation_functor() {}

	int operator()(const VectorXd &b, VectorXd &fvec) const {
		ASSERT(b.size() == unknowns);
		ASSERT(fvec.size() == datasetLen);
		for (int i = 0; i < unknowns; ++i)
			(*fSource).SetCoeffVal(i, b(i));
		for(int64 i = 0; i < datasetLen; i++)
			fvec(ptrdiff_t(i)) = (*fSource).f((*series).x(i)) - (*series).y(i);
		return 0;
	}
};

void ExplicitEquation::SetNumCoeff(int num) {
	coeff.SetCount(num);
	for (int i = 0; i < num; ++i)
		coeff[i] = 0;
}

ExplicitEquation::FitError ExplicitEquation::Fit(DataSource &serie, double &r2) {
	r2 = Null;

	if (serie.IsExplicit() || serie.IsParam())
		return InadequateDataSource;

	if (serie.GetCount() < coeff.GetCount())
		return SmallDataSource;

	ptrdiff_t numUnknowns = coeff.GetCount();

	VectorXd x(numUnknowns);
	for (int i = 0; i < numUnknowns; ++i)
		x(i) = coeff[i];

	Equation_functor functor;
	functor.series = &serie;
	functor.fSource = this;
	functor.unknowns = numUnknowns;
	functor.datasetLen = Eigen::Index(serie.GetCount());

	NumericalDiff<Equation_functor> numDiff(functor);
	LevenbergMarquardt<NumericalDiff<Equation_functor> > lm(numDiff);
// 	ftol is a nonnegative input variable that measures the relative error desired in the sum of squares
	lm.parameters.ftol = 1.E4*NumTraits<double>::epsilon();
//  xtol is a nonnegative input variable that measures the relative error desired in the approximate solution
	lm.parameters.xtol = 1.E4*NumTraits<double>::epsilon();
	lm.parameters.maxfev = maxFitFunctionEvaluations;
	int ret = lm.minimize(x);
	if (ret == LevenbergMarquardtSpace::ImproperInputParameters)
		return ExplicitEquation::ImproperInputParameters;
	else if (ret == LevenbergMarquardtSpace::TooManyFunctionEvaluation)
		return TooManyFunctionEvaluation;

	r2 = R2Y(serie);

	return NoError;
}

double ExplicitEquation::R2Y(DataSource &serie, double mean) {
	if (!IsNum(mean))
		mean = serie.AvgY();
	double sse = 0, sst = 0;
	for (int64 i = 0; i < serie.GetCount(); ++i) {
		double y = serie.y(i);
		if (!!IsNum(y)) {
			double err = y - f(serie.x(i));
			sse += err*err;
			double d = y - mean;
			sst += d*d;
		}
	}
	if (sst < 1E-50 || sse > sst)
		return 0;
	return 1 - sse/sst;
}

int ExplicitEquation::maxFitFunctionEvaluations = 2000;

double PolynomialEquation::f(double x) {
	if (x < 0)
		return Null;
	double y = 0;
	for (int i = 0; i < coeff.GetCount(); ++i)
		y += coeff[i]*pow(x, i);
	return y;
}

String PolynomialEquation::GetEquation(int numDigits) {
	if (coeff.IsEmpty())
		return String();
	String ret = FormatCoeff(0, numDigits);
	if (coeff.GetCount() == 1)
		return ret;
	ret += Format(" + %s*x", FormatCoeff(1, numDigits));
	for (int i = 2; i < coeff.GetCount(); ++i)
		ret += Format(" + %s*x^%s", FormatCoeff(i, numDigits), FormatInt(i));
	ret.Replace("+ -", "- ");
	return ret;
}

double FourierEquation::f(double x) {
	double y = coeff[0];
	double w = coeff[1];
	for (int i = 2; i < coeff.GetCount(); i += 2) {
		int n = 1 + (i - 2)/2;
		y += coeff[i]*cos(n*w*x) + coeff[i+1]*sin(n*w*x);
	}
	return y;
}

String FourierEquation::GetEquation(int numDigits) {
	if (coeff.GetCount() < 4)
		return String();
	String ret = FormatCoeff(0, numDigits);

	for (int i = 2; i < coeff.GetCount(); i += 2) {
		int n = 1 + (i - 2)/2;
		String nwx = Format("%d*%s*x", n, FormatCoeff(1, numDigits));
		ret += Format(" + %s*cos(%s)", FormatCoeff(i, numDigits), nwx);
		ret += Format(" + %s*sin(%s)", FormatCoeff(i + 1, numDigits), nwx);
	}
	ret.Replace("+ -", "- ");
	return ret;
}

static inline double DegToRad(double deg) {return deg*M_PI/180.;}
static inline double RadToDeg(double rad) {return rad*180./M_PI;}

void EvalExpr::EvalThrowError(CParserPP &p, const char *s) {
	CParserPP::Pos pos = p.GetPos();
	CParserPP::Error err(Format("(%d): ", pos.GetColumn()) + String(s));
	throw err;
}

doubleUnit usqrt(doubleUnit val) {
	val.Sqrt();
	return val;
}

doubleUnit ufabs(doubleUnit val) {
	val.val = fabs(val.val);
	return val;
}

doubleUnit uceil(doubleUnit val) {
	val.val = ceil(val.val);
	return val;
}

doubleUnit ufloor(doubleUnit val) {
	val.val = floor(val.val);
	return val;
}

doubleUnit uround(doubleUnit val) {
	val.val = round(val.val);
	return val;
}

doubleUnit usin(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = sin(val.val);
	return val;
}

doubleUnit ucos(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = cos(val.val);
	return val;
}

doubleUnit utan(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = tan(val.val);
	return val;
}

doubleUnit uasin(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = asin(val.val);
	return val;
}

doubleUnit uacos(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = acos(val.val);
	return val;
}

doubleUnit uatan(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = atan(val.val);
	return val;
}

doubleUnit usinh(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = sinh(val.val);
	return val;
}

doubleUnit ucosh(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = cosh(val.val);
	return val;
}

doubleUnit utanh(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = tanh(val.val);
	return val;
}

doubleUnit uexp(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = exp(val.val);
	return val;
}

doubleUnit uDegToRad(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = DegToRad(val.val);
	return val;
}

doubleUnit uRadToDeg(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = RadToDeg(val.val);
	return val;
}

doubleUnit ulog(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = log(val.val);
	return val;
}

doubleUnit ulog10(doubleUnit val) {
	if (!val.unit.IsAdim())
		return Null;
	val.val = log10(val.val);
	return val;
}

EvalExpr::EvalExpr() {
	noCase = false;
	errorIfUndefined = false;
	allowString = false;

	constants.Add("pi", doubleUnit(M_PI));
	constants.Add("e", doubleUnit(M_E));

	functions.Add("abs", ufabs);
	functions.Add("ceil", uceil);
	functions.Add("floor", ufloor);
	functions.Add("round", uround);
	functions.Add("sqrt", usqrt);
	functions.Add("sin", usin);
	functions.Add("cos", ucos);
	functions.Add("tan", utan);
	functions.Add("asin", uasin);
	functions.Add("acos", uacos);
	functions.Add("atan", uatan);
	functions.Add("sinh", usinh);
	functions.Add("cosh", ucosh);
	functions.Add("tanh", utanh);
	functions.Add("log", ulog);
	functions.Add("log10", ulog10);
	functions.Add("exp", uexp);
	functions.Add("degToRad", uDegToRad);
	functions.Add("radToDeg", uRadToDeg);
}

doubleUnit EvalExpr::Term(CParserPP& p) {
	p.Char('+');
	bool isneg = p.Char('-');
	if (p.IsId()) {
		String strId = p.ReadIdPP();
		if(doubleUnit (*function)(doubleUnit) = functions.Get(strId, 0)) {
			p.PassChar('(');
			doubleUnit x(Exp(p));
			p.PassChar(')');
			doubleUnit ret(function(x));
			if (IsNull(ret))
				EvalThrowError(p, Format(t_("Error in %s(%f)"), strId, x.val));
			if (isneg)
				ret.Neg();
			return ret;
		}
		String strIdSearch;
		if (noCase)
			strIdSearch = ToLower(strId);
		else
			strIdSearch = strId;
		doubleUnit ret(constants.Get(strIdSearch, Null));
		if (IsNull(ret)) {
			int id = FindVariable(strIdSearch);
			if (id >= 0)
				ret = variables[id];
			else {
				if (errorIfUndefined) {
					lastError = Format(t_("Unknown identifier '%s'"), strId);
					return Null;
				}
					//EvalThrowError(p, Format(t_("Unknown identifier '%s'"), strId));
				lastVariableSetId = variables.FindAdd(strIdSearch, 0);
				ret = variables[lastVariableSetId];
			}
		}
		if (isneg)
			ret.Neg();
		return ret;
	} else if (p.Char('(')) {
		doubleUnit x(Exp(p));
		p.PassChar(')');
		if (isneg)
			x.Neg();
		return x;
	} else {
		if (p.IsChar2('.', '.'))
			p.ThrowError("missing number");
		doubleUnit x(p.ReadDouble());
		if (isneg)
			x.Neg();
		return x;
	}
}

doubleUnit EvalExpr::Pow(CParserPP& p) {
	doubleUnit x(Term(p));
	for(;;)
		if(p.Char('^')) {
			//if (x.val < 0)
			//	EvalThrowError(p, t_("Complex number"));
			x.Exp(Term(p));
		} else
			return x;
}

doubleUnit EvalExpr::Mul(CParserPP& p) {
	doubleUnit x(Pow(p));
	for(;;)
		if(p.Char('*'))
			x.Mult(Pow(p));
		else if (p.Char2('|', '|'))
			x.ResParallel(Pow(p));
		else if(memcmp(p.GetPtr(), "·", strlen("·")) == 0) {
			CParserPP::Pos pos = p.GetPos();
			pos.ptr += strlen("·");
			p.SetPos(pos);
			p.Spaces();
			x.Mult(Pow(p));
		} else if(p.Char('/')) {
			x.Div(Pow(p));
		} else if(memcmp(p.GetPtr(), "º", strlen("º")) == 0) {
			CParserPP::Pos pos = p.GetPos();
			pos.ptr += strlen("º");
			p.SetPos(pos);
			p.Spaces();
			x.Mult(doubleUnit(M_PI/180.));
		} else
			return x;
}

doubleUnit EvalExpr::Exp(CParserPP& p) {
	doubleUnit x(Mul(p));
	for(;;)
		if(p.Char('+'))
			x.Sum(Mul(p));
		else if(p.Char('-'))
			x.Sub(Mul(p));
		else if(p.Char(':')) {
			x.Mult(doubleUnit(60));
			x.Sum(Mul(p));
		} else
			return x;
}

doubleUnit EvalExpr::AssignVariable(String var, String expr) {
	doubleUnit ret;
	if (noCase)
		var = ToLower(var);
	int idalloc = FindAddVariable(var);
	try {
		p.Set(expr);

		ret.Set(Exp(p));
		if (!IsNull(ret)) {
			SetVariable(idalloc, ret);
			return ret;
		} else {
			if (allowString) {
				ret.sval = expr;
				SetVariable(idalloc, ret);
				return ret;
			}
			return Null;
		}
	} catch(CParserPP::Error e) {
		if (allowString) {
			ret.sval = expr;
			SetVariable(idalloc, ret);
			return ret;
		}
		lastError = e;
		return Null;
	} catch(Exc e) {
		lastError = e;
		return Null;
	} catch(...) {
		lastError = "Unknown error";
		return Null;
	}
}

void EvalExpr::RenameVariable(String varname, String newvarname) {
	if (noCase) {
		varname = ToLower(varname);
		newvarname = ToLower(newvarname);
	}
	try {
		int id = variables.Find(varname);
		if (id >= 0)
			variables.SetKey(id, newvarname);
	} catch(CParserPP::Error e) {
		lastError = e;
	} catch(Exc e) {
		lastError = e;
	}
}

doubleUnit EvalExpr::AssignVariable(String var, double d) {
	if (noCase)
		var = ToLower(var);
	try {
		doubleUnit ret(d);
		SetVariable(var, ret);
		return ret;
	} catch(CParserPP::Error e) {
		lastError = e;
		return Null;
	} catch(Exc e) {
		lastError = e;
		return Null;
	} catch(...) {
		lastError = "Unknown error";
		return Null;
	}
}

doubleUnit EvalExpr::Eval(String line) {
	line = TrimBoth(line);
	if (line.IsEmpty())
		return Null;

	p.Set(line);
	try {
		if(p.IsId()) {
			CParserPP::Pos pos = p.GetPos();
			String var = p.ReadIdPP();
			if(p.Char('=')) {
				if (noCase)
					var = ToLower(var);
				doubleUnit ret(Exp(p));
				SetVariable(var, ret);
				return ret;
			} else {
				p.SetPos(pos);
				return Exp(p);
			}
		} else
			return Exp(p);
	} catch(CParserPP::Error e) {
		lastError = e;
		return Null;
	} catch(Exc e) {
		lastError = e;
		return Null;
	}
}

String EvalExpr::TermStr(CParserPP& p, int numDigits) {
	if(p.IsId()) {
		String strId = p.ReadIdPP();
		if(functions.Find(strId) >= 0) {
			p.PassChar('(');
			String x = ExpStr(p, numDigits);
			p.PassChar(')');
			return strId + "(" + x + ")";
		}
		if (noCase)
			strId = ToLower(strId);
		if (IsNull(numDigits)) {
			if (constants.Find(strId) < 0)
				lastVariableSetId = variables.FindAdd(strId, 0);
			return strId;
		} else {
			if (constants.Find(strId) >= 0)
				return strId;
			else {
				lastVariableSetId = variables.FindAdd(strId, 0);
				return FormatDoubleFix(variables[lastVariableSetId].val, numDigits);
			}
		}
	}
	if(p.Char('(')) {
		String x = ExpStr(p, numDigits);
		p.PassChar(')');
		return "(" + x + ")";
	}
	return FormatDoubleFix(p.ReadDouble(), IsNull(numDigits) ? 3 : numDigits);
}

String EvalExpr::PowStr(CParserPP& p, int numDigits) {
	String x = TermStr(p, numDigits);
	for(;;)
		if(p.Char('^'))
			x = x + "^" + TermStr(p, numDigits);
		else
			return x;
}

String EvalExpr::MulStr(CParserPP& p, int numDigits) {
	String x = PowStr(p, numDigits);
	for(;;)
		if(p.Char('*'))
			x = x + "*" + MulStr(p, numDigits);
		else if(p.Char('/'))
			x = x + "/" + PowStr(p, numDigits);
		else
			return x;
}

String EvalExpr::ExpStr(CParserPP& p, int numDigits) {
	String x = MulStr(p, numDigits);
	for(;;)
		if(p.Char('+'))
			x = x + " + " + MulStr(p, numDigits);
		else if(p.Char('-'))
			x = x + " - " + MulStr(p, numDigits);
		else if(p.Char(':'))
			x = x + ":" + MulStr(p, numDigits);
		else {
			x.Replace("+ -", "- ");
			return x;
	}
}

String EvalExpr::EvalStr(String line, int numDigits) {
	line = TrimBoth(line);
	if (line.IsEmpty())
		return Null;

	CParserPP p(line);
	try {
		if(p.IsId()) {
			CParserPP::Pos pos = p.GetPos();
			String var = p.ReadIdPP();
			if(p.Char('=')) {
				String ret = ExpStr(p, numDigits);
				lastVariableSetId = variables.FindAdd(var, 0);
				return var + " = " + ret;
			} else {
				p.SetPos(pos);
				return ExpStr(p, numDigits);
			}
		} else
			return ExpStr(p, numDigits);
	} catch(CParserPP::Error e) {
		lastError = Format(t_("Error evaluating '%s': %s"), line, e);
		return Null;
	} catch(Exc e) {
		lastError = Format(t_("Error: %s"), e);
		return Null;
	} catch(String e) {
		lastError = Format(t_("Error: %s"), e);
		return Null;
	} catch(...) {
		lastError = t_("Unknown error");
		return Null;
	}
}

void EvalExpr::ClearVariables() {
	variables.Clear();
}

Vector<int> EvalExpr::FindPattern(String yes, String no) const {
	Vector<int> ret;
	yes = ToLower(yes);
	no = ToLower(no);
	for (int i = 0; i < variables.GetCount(); ++i) {
		String name = ToLower(variables.GetKey(i));
		if (PatternMatch(yes, name) && !PatternMatch(no, name))
			ret << i;
	}
	return ret;
}

Vector<int> EvalExpr::FindPattern(String yes, String no, String yes2) const {
	Vector<int> ret = FindPattern(yes, no);
	if (ret.IsEmpty())
		ret = FindPattern(yes2, no);
	return ret;
}

ExplicitEquation::FitError SplineEquation::Fit(DataSource &data, double &r2) {
	Vector<Pointf> seriesRaw;
	for (int64 i = 0; i < data.GetCount(); ++i) {		// Remove Nulls
		if (!!IsNum(data.x(i)) && !!IsNum(data.y(i)))
			seriesRaw << Pointf(data.x(i), data.y(i));
	}

	if(seriesRaw.IsEmpty())
        return SmallDataSource;

    r2 = 1;

	PointfLess less;
	Sort(seriesRaw, less);								// Sort

	Vector<double> x, y;
	x.Reserve(seriesRaw.GetCount());
	y.Reserve(seriesRaw.GetCount());
	x << seriesRaw[0].x;
	y << seriesRaw[0].y;
	for (int i = 1; i < seriesRaw.GetCount(); ++i) {	// Remove points with duplicate x
		if (seriesRaw[i].x != seriesRaw[i - 1].x) {
			x << seriesRaw[i].x;
			y << seriesRaw[i].y;
		}
	}

	if (x.GetCount() < 2)
		return SmallDataSource;

	Init(x, y);

	coeff.SetCount(1);

	return NoError;
}

void Spline::Init(const double *x, const double *y, int num) {
    nscoeff = num - 1;

    Buffer<double> h(nscoeff);
    for(int i = 0; i < nscoeff; ++i)
        h[i] = x[i+1] - x[i];

    Buffer<double> alpha(nscoeff);
    for(int i = 1; i < nscoeff; ++i)
        alpha[i] = 3*(y[i+1] - y[i])/h[i] - 3*(y[i] - y[i-1])/h[i-1];

    Buffer<double> c(nscoeff+1), l(nscoeff+1), mu(nscoeff+1), z(nscoeff+1);
    l[0] = 1;
    mu[0] = 0;
    z[0] = 0;

    for(int i = 1; i < nscoeff; ++i) {
        l[i] = 2*(x[i+1] - x[i-1]) - h[i-1]*mu[i-1];
        mu[i] = h[i]/l[i];
        z[i] = (alpha[i] - h[i-1]*z[i-1])/l[i];
    }

    l[nscoeff] = 1;
    z[nscoeff] = 0;
    c[nscoeff] = 0;

	scoeff.Alloc(nscoeff);
    for(int i = nscoeff-1; i >= 0; --i) {
        c[i] = z[i] - mu[i] * c[i+1];
        scoeff[i].b = (y[i+1] - y[i])/h[i] - h[i]*(c[i+1] + 2*c[i])/3;
        scoeff[i].d = (c[i+1] - c[i])/3/h[i];
    }

    for(int i = 0; i < nscoeff; ++i) {
        scoeff[i].x = x[i];
        scoeff[i].a = y[i];
        scoeff[i].c = c[i];
    }
    xlast = x[num-1];
}

int Spline::GetPieceIndex(double x) const {
	ASSERT(nscoeff > 0);
    int j;
    for (j = 0; j < nscoeff; j++) {
        if (scoeff[j].x > x) {
            if (j == 0)
                j = 1;
            break;
        }
    }
    return --j;
}

double Spline::f(double x) const {
	int j = GetPieceIndex(x);

    double dx = x - scoeff[j].x;
    double dx2 = dx*dx;
    return scoeff[j].a + scoeff[j].b*dx + scoeff[j].c*dx*dx + scoeff[j].d*dx*dx2;
}

double Spline::df(double x) const {
	int j = GetPieceIndex(x);

    double dx = x - scoeff[j].x;
    return scoeff[j].b + scoeff[j].c*2.*dx + scoeff[j].d*3.*dx*dx;
}

double Spline::d2f(double x) const {
	int j = GetPieceIndex(x);

    double dx = x - scoeff[j].x;
    return scoeff[j].c*2. + scoeff[j].d*6.*dx;
}

double Spline::Integral0(const Coeff &c, double x) {
	double x2 = x*x;
	return c.a*x + c.b*x2/2 + c.c*x*x2/3 + c.d*x2*x2/4;
}

double Spline::Integral(double from, double to) const {
	int ifrom;
	if (!IsNum(from)) {
		ifrom = 0;
		from = scoeff[0].x;
	} else
		ifrom = GetPieceIndex(from);
	int ito;
	if (!IsNum(to)) {
		ito = nscoeff-1;
		to = xlast;
	} else
		ito = GetPieceIndex(to);

	ASSERT(ifrom <= ito);
	if (ifrom > ito)
		return 0;

	double res = 0;
	for (int i = ifrom; i < ito; ++i) {
		double val = Integral0(scoeff[i], scoeff[i+1].x - scoeff[i].x) - Integral0(scoeff[i], from - scoeff[i].x);
		res += val;
		from = scoeff[i+1].x;
	}
	double bal =  Integral0(scoeff[ito], to - scoeff[ito].x) - Integral0(scoeff[ito], from - scoeff[ito].x);
	res += bal;
	return res;
}

INITBLOCK {
	ExplicitEquation::Register<LinearEquation>("LinearEquation");
	ExplicitEquation::Register<PolynomialEquation2>("PolynomialEquation2");
	ExplicitEquation::Register<PolynomialEquation3>("PolynomialEquation3");
	ExplicitEquation::Register<PolynomialEquation4>("PolynomialEquation4");
	ExplicitEquation::Register<PolynomialEquation5>("PolynomialEquation5");
	ExplicitEquation::Register<SinEquation>("SinEquation");
	ExplicitEquation::Register<DampedSinEquation>("DampedSinusoidal");
	ExplicitEquation::Register<Sin_DampedSinEquation>("Sin_DampedSinusoidal");
	ExplicitEquation::Register<ExponentialEquation>("ExponentialEquation");
	ExplicitEquation::Register<RealExponentEquation>("RealExponentEquation");
	ExplicitEquation::Register<Rational1Equation>("Rational1Equation");
	ExplicitEquation::Register<FourierEquation1>("FourierEquation1");
	ExplicitEquation::Register<FourierEquation2>("FourierEquation2");
	ExplicitEquation::Register<FourierEquation3>("FourierEquation3");
	ExplicitEquation::Register<FourierEquation4>("FourierEquation4");
	ExplicitEquation::Register<WeibullEquation>("WeibullEquation");
	ExplicitEquation::Register<WeibullCumulativeEquation>("WeibullCumulativeEquation");
	ExplicitEquation::Register<NormalEquation>("NormalEquation");
}

}


#include <boost/mpl/plus.hpp>
#include <boost/mpl/int.hpp>
#include <functional>
#include <cassert>

using namespace boost::mpl;

/**
 * metafunction class
 */
struct plus_mf {
    template <class T1, class T2>
        struct apply {
            typedef typename plus<T1, T2>::type type;
        };

    /* metafunction forward */
    //template <class T1, class T2>
    //    struct apply: plus<T1, T2> { };
};

/**
 * function object(functor)
 */
//struct plus_f {
//    int operator () (int x, int y) {
//        return x + y;
//    }
//};
struct plus_f: std::plus<int> {};

/**
 */
template <int lhs, int rhs>
struct plus_ff {
    int operator() () {
        return lhs + rhs;
    }
};

/**
 */
template <int lhs, int rhs>
struct plus_fff {
    const static int value = lhs + rhs;
};

int main()
{
    static_assert((plus<int_<1>, int_<2>>::value == int_<3>::value), "metafunction");
    static_assert((plus_mf::apply<int_<1>, int_<2>>::type::value == int_<3>::value), "metafunction-class");
    assert((plus_f()(1, 2) == 3));
    assert((plus_ff<1, 2>()() == 3));
    static_assert((plus_fff<1, 2>::value == 3), "metafunction2");
    return 0;
}