#include <common_robotics_utilities/conversions.hpp>

#include <vector>

#include <Eigen/Geometry>

namespace common_robotics_utilities
{
namespace conversions
{
Eigen::Quaterniond QuaternionFromRPY(const double R,
                                     const double P,
                                     const double Y)
{
  const Eigen::AngleAxisd roll(R, Eigen::Vector3d::UnitX());
  const Eigen::AngleAxisd pitch(P, Eigen::Vector3d::UnitY());
  const Eigen::AngleAxisd yaw(Y, Eigen::Vector3d::UnitZ());
  const Eigen::Quaterniond quat(roll * pitch * yaw);
  return quat;
}

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Quaterniond QuaternionFromUrdfRPY(const double R,
                                         const double P,
                                         const double Y)
{
  const Eigen::AngleAxisd roll(R, Eigen::Vector3d::UnitX());
  const Eigen::AngleAxisd pitch(P, Eigen::Vector3d::UnitY());
  const Eigen::AngleAxisd yaw(Y, Eigen::Vector3d::UnitZ());
  const Eigen::Quaterniond quat(yaw * pitch * roll);
  return quat;
}

// Returns XYZ Euler angles
Eigen::Vector3d EulerAnglesFromRotationMatrix(
    const Eigen::Matrix3d& rot_matrix)
{
  // Use XYZ angles
  const Eigen::Vector3d euler_angles = rot_matrix.eulerAngles(0, 1, 2);
  return euler_angles;
}

// Returns XYZ Euler angles
Eigen::Vector3d EulerAnglesFromQuaternion(const Eigen::Quaterniond& quat)
{
  return EulerAnglesFromRotationMatrix(quat.toRotationMatrix());
}

// Returns XYZ Euler angles
Eigen::Vector3d EulerAnglesFromIsometry3d(const Eigen::Isometry3d& trans)
{
  return EulerAnglesFromRotationMatrix(trans.rotation());
}

Eigen::Isometry3d TransformFromXYZRPY(const double x,
                                      const double y,
                                      const double z,
                                      const double roll,
                                      const double pitch,
                                      const double yaw)
{
  const Eigen::Isometry3d transform = Eigen::Translation3d(x, y, z)
                                      * QuaternionFromRPY(roll, pitch, yaw);
  return transform;
}

Eigen::Isometry3d TransformFromRPY(const Eigen::Vector3d& translation,
                                   const Eigen::Vector3d& rotation)
{
  const Eigen::Isometry3d transform = (Eigen::Translation3d)translation
                                      * QuaternionFromRPY(rotation.x(),
                                                          rotation.y(),
                                                          rotation.z());
  return transform;
}

Eigen::Isometry3d TransformFromRPY(const Eigen::VectorXd& components)
{
  if (components.size() == 6)
  {
    return Eigen::Translation3d(components(0),
                                components(1),
                                components(2))
           * QuaternionFromRPY(components(3),
                               components(4),
                               components(5));
  }
  else
  {
    throw std::invalid_argument(
          "VectorXd source vector is not 6 elements in size");
  }
}

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Isometry3d TransformFromUrdfXYZRPY(const double x,
                                          const double y,
                                          const double z,
                                          const double roll,
                                          const double pitch,
                                          const double yaw)
{
  const Eigen::Isometry3d transform = Eigen::Translation3d(x, y, z)
                                      * QuaternionFromUrdfRPY(roll, pitch, yaw);
  return transform;
}

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Isometry3d TransformFromUrdfRPY(const Eigen::Vector3d& translation,
                                       const Eigen::Vector3d& rotation)
{
  const Eigen::Isometry3d transform = (Eigen::Translation3d)translation
                                      * QuaternionFromUrdfRPY(rotation.x(),
                                                              rotation.y(),
                                                              rotation.z());
  return transform;
}

// URDF RPY IS ACTUALLY APPLIED Y*P*R
Eigen::Isometry3d TransformFromUrdfRPY(const Eigen::VectorXd& components)
{
  if (components.size() == 6)
  {
    return Eigen::Translation3d(components(0),
                                components(1),
                                components(2))
           * QuaternionFromUrdfRPY(components(3),
                                   components(4),
                                   components(5));
  }
  else
  {
    throw std::invalid_argument(
          "VectorXd source vector is not 6 elements in size");
  }
}

Eigen::VectorXd TransformToRPY(const Eigen::Isometry3d& transform)
{
  Eigen::VectorXd components = Eigen::VectorXd::Zero(6);
  const Eigen::Vector3d translation = transform.translation();
  const Eigen::Vector3d rotation
      = EulerAnglesFromRotationMatrix(transform.rotation());
  components << translation, rotation;
  return components;
}

Eigen::Vector3d StdVectorDoubleToEigenVector3d(
    const std::vector<double>& vector)
{
  if (vector.size() == 3)
  {
    return Eigen::Vector3d(vector[0], vector[1], vector[2]);
  }
  else
  {
    throw std::invalid_argument(
          "Vector3d source vector is not 3 elements in size");
  }
}

Eigen::VectorXd StdVectorDoubleToEigenVectorXd(
    const std::vector<double>& vector)
{
  Eigen::VectorXd eigen_vector(vector.size());
  for (size_t idx = 0; idx < vector.size(); idx++)
  {
    const double val = vector[idx];
    eigen_vector((ssize_t)idx) = val;
  }
  return eigen_vector;
}

std::vector<double> EigenVector3dToStdVectorDouble(
    const Eigen::Vector3d& point)
{
  return std::vector<double>{point.x(), point.y(), point.z()};
}

std::vector<double> EigenVectorXdToStdVectorDouble(
    const Eigen::VectorXd& eigen_vector)
{
  std::vector<double> vector((size_t)eigen_vector.size());
  for (size_t idx = 0; idx < (size_t)eigen_vector.size(); idx++)
  {
    const double val = eigen_vector[(ssize_t)idx];
    vector[idx] = val;
  }
  return vector;
}

// Takes <x, y, z, w> as is the ROS custom!
Eigen::Quaterniond StdVectorDoubleToEigenQuaterniond(
    const std::vector<double>& vector)
{
  if (vector.size() == 4)
  {
    return Eigen::Quaterniond(vector[3], vector[0], vector[1], vector[2]);
  }
  else
  {
    throw std::invalid_argument(
          "Quaterniond source vector is not 4 elements in size");
  }
}

// Returns <x, y, z, w> as is the ROS custom!
std::vector<double> EigenQuaterniondToStdVectorDouble(
    const Eigen::Quaterniond& quat)
{
  return std::vector<double>{quat.x(), quat.y(), quat.z(), quat.w()};
}

Eigen::Vector3d GeometryPointToEigenVector3d(
    const geometry_msgs::Point& point)
{
  Eigen::Vector3d eigen_point(point.x, point.y, point.z);
  return eigen_point;
}

geometry_msgs::Point EigenVector3dToGeometryPoint(
    const Eigen::Vector3d& point)
{
  geometry_msgs::Point geom_point;
  geom_point.x = point.x();
  geom_point.y = point.y();
  geom_point.z = point.z();
  return geom_point;
}

Eigen::Vector4d GeometryPointToEigenVector4d(
    const geometry_msgs::Point& point)
{
  Eigen::Vector4d eigen_point(point.x, point.y, point.z, 1.0);
  return eigen_point;
}

geometry_msgs::Point EigenVector4dToGeometryPoint(
    const Eigen::Vector4d& point)
{
  geometry_msgs::Point geom_point;
  geom_point.x = point(0);
  geom_point.y = point(1);
  geom_point.z = point(2);
  return geom_point;
}

geometry_msgs::PointStamped EigenVector3dToGeometryPointStamped(
    const Eigen::Vector3d& point, const std::string& frame_id)
{
  geometry_msgs::PointStamped point_stamped;
  point_stamped.header.frame_id = frame_id;
  point_stamped.point = EigenVector3dToGeometryPoint(point);
  return point_stamped;
}

Eigen::Vector3d GeometryVector3ToEigenVector3d(
    const geometry_msgs::Vector3& vector)
{
  Eigen::Vector3d eigen_vector(vector.x, vector.y, vector.z);
  return eigen_vector;
}

geometry_msgs::Vector3 EigenVector3dToGeometryVector3(
    const Eigen::Vector3d& vector)
{
  geometry_msgs::Vector3 geom_vector;
  geom_vector.x = vector.x();
  geom_vector.y = vector.y();
  geom_vector.z = vector.z();
  return geom_vector;
}

Eigen::Vector4d GeometryVector3ToEigenVector4d(
    const geometry_msgs::Vector3& vector)
{
  Eigen::Vector4d eigen_vector(vector.x, vector.y, vector.z, 0.0);
  return eigen_vector;
}

geometry_msgs::Vector3 EigenVector4dToGeometryVector3(
    const Eigen::Vector4d& vector)
{
  geometry_msgs::Vector3 geom_vector;
  geom_vector.x = vector(0);
  geom_vector.y = vector(1);
  geom_vector.z = vector(2);
  return geom_vector;
}

Eigen::Quaterniond GeometryQuaternionToEigenQuaterniond(
    const geometry_msgs::Quaternion& quat)
{
  Eigen::Quaterniond eigen_quaternion(quat.w, quat.x, quat.y, quat.z);
  return eigen_quaternion;
}

geometry_msgs::Quaternion EigenQuaterniondToGeometryQuaternion(
    const Eigen::Quaterniond& quat)
{
  geometry_msgs::Quaternion geom_quaternion;
  geom_quaternion.w = quat.w();
  geom_quaternion.x = quat.x();
  geom_quaternion.y = quat.y();
  geom_quaternion.z = quat.z();
  return geom_quaternion;
}

Eigen::Isometry3d GeometryPoseToEigenIsometry3d(
    const geometry_msgs::Pose& pose)
{
  const Eigen::Translation3d trans(pose.position.x,
                                   pose.position.y,
                                   pose.position.z);
  const Eigen::Quaterniond quat(pose.orientation.w,
                                pose.orientation.x,
                                pose.orientation.y,
                                pose.orientation.z);
  const Eigen::Isometry3d eigen_pose = trans * quat;
  return eigen_pose;
}

geometry_msgs::Pose EigenIsometry3dToGeometryPose(
    const Eigen::Isometry3d& transform)
{
  const Eigen::Vector3d trans = transform.translation();
  const Eigen::Quaterniond quat(transform.rotation());
  geometry_msgs::Pose geom_pose;
  geom_pose.position.x = trans.x();
  geom_pose.position.y = trans.y();
  geom_pose.position.z = trans.z();
  geom_pose.orientation.w = quat.w();
  geom_pose.orientation.x = quat.x();
  geom_pose.orientation.y = quat.y();
  geom_pose.orientation.z = quat.z();
  return geom_pose;
}

geometry_msgs::PoseStamped EigenIsometry3dToGeometryPoseStamped(
    const Eigen::Isometry3d& transform, const std::string& frame_id)
{
  geometry_msgs::PoseStamped pose_stamped;
  pose_stamped.header.frame_id = frame_id;
  pose_stamped.pose = EigenIsometry3dToGeometryPose(transform);
  return pose_stamped;
}

Eigen::Isometry3d GeometryTransformToEigenIsometry3d(
    const geometry_msgs::Transform& transform)
{
  const Eigen::Translation3d trans(transform.translation.x,
                                   transform.translation.y,
                                   transform.translation.z);
  const Eigen::Quaterniond quat(transform.rotation.w,
                                transform.rotation.x,
                                transform.rotation.y,
                                transform.rotation.z);
  const Eigen::Isometry3d eigen_transform = trans * quat;
  return eigen_transform;
}

geometry_msgs::Transform EigenIsometry3dToGeometryTransform(
    const Eigen::Isometry3d& transform)
{
  const Eigen::Vector3d trans = transform.translation();
  const Eigen::Quaterniond quat(transform.rotation());
  geometry_msgs::Transform geom_transform;
  geom_transform.translation.x = trans.x();
  geom_transform.translation.y = trans.y();
  geom_transform.translation.z = trans.z();
  geom_transform.rotation.w = quat.w();
  geom_transform.rotation.x = quat.x();
  geom_transform.rotation.y = quat.y();
  geom_transform.rotation.z = quat.z();
  return geom_transform;
}

geometry_msgs::TransformStamped EigenIsometry3dToGeometryTransformStamped(
    const Eigen::Isometry3d& transform, const std::string& frame_id,
    const std::string& child_frame_id)
{
  geometry_msgs::TransformStamped transform_stamped;
  transform_stamped.header.frame_id = frame_id;
  transform_stamped.child_frame_id = child_frame_id;
  transform_stamped.transform = EigenIsometry3dToGeometryTransform(transform);
  return transform_stamped;
}

Eigen::Matrix3Xd VectorGeometryPointToEigenMatrix3Xd(
    const std::vector<geometry_msgs::Point>& vector_geom)
{
  Eigen::Matrix3Xd eigen_matrix = Eigen::MatrixXd(3, vector_geom.size());
  for (size_t idx = 0; idx < vector_geom.size(); idx++)
  {
    eigen_matrix.block<3,1>(0, (ssize_t)idx)
        = GeometryPointToEigenVector3d(vector_geom[idx]);
  }
  return eigen_matrix;
}

std::vector<geometry_msgs::Point> EigenMatrix3XdToVectorGeometryPoint(
    const Eigen::Matrix3Xd& eigen_matrix)
{
  std::vector<geometry_msgs::Point> vector_geom((size_t)eigen_matrix.cols());
  for (size_t idx = 0; idx < vector_geom.size(); idx++)
  {
    vector_geom[idx]
        = EigenVector3dToGeometryPoint(
            eigen_matrix.block<3,1>(0, (ssize_t)idx));
  }
  return vector_geom;
}

std::vector<geometry_msgs::Point>
VectorEigenVector3dToVectorGeometryPoint(
    const common_robotics_utilities::math::VectorVector3d& vector_eigen)
{
  std::vector<geometry_msgs::Point> vector_geom(vector_eigen.size());
  for (size_t idx = 0; idx < vector_eigen.size(); idx++)
  {
    vector_geom[idx] = EigenVector3dToGeometryPoint(vector_eigen[idx]);
  }
  return vector_geom;
}

common_robotics_utilities::math::VectorVector3d
VectorGeometryPointToVectorEigenVector3d(
    const std::vector<geometry_msgs::Point>& vector_geom)
{
  common_robotics_utilities::math::VectorVector3d vector_eigen(
      vector_geom.size());
  for (size_t idx = 0; idx < vector_geom.size(); idx++)
  {
    vector_eigen[idx] = GeometryPointToEigenVector3d(vector_geom[idx]);
  }
  return vector_eigen;
}

common_robotics_utilities::math::VectorVector3d
VectorGeometryVector3ToEigenVector3d(
    const std::vector<geometry_msgs::Vector3>& vector_geom)
{
  common_robotics_utilities::math::VectorVector3d vector_eigen(
      vector_geom.size());
  for (size_t idx = 0; idx < vector_geom.size(); idx++)
  {
    vector_eigen[idx] = GeometryVector3ToEigenVector3d(vector_geom[idx]);
  }
  return vector_eigen;
}

common_robotics_utilities::math::VectorIsometry3d
VectorGeometryPoseToVectorIsometry3d(
    const std::vector<geometry_msgs::Pose>& vector_geom)
{
  common_robotics_utilities::math::VectorIsometry3d vector_eigen(
      vector_geom.size());
  for (size_t idx = 0; idx < vector_geom.size(); idx++)
  {
    vector_eigen[idx] = GeometryPoseToEigenIsometry3d(vector_geom[idx]);
  }
  return vector_eigen;
}

common_robotics_utilities::math::VectorIsometry3d
VectorGeometryPoseToVectorIsometry3d(
    const std::vector<geometry_msgs::Transform>& vector_geom)
{
  common_robotics_utilities::math::VectorIsometry3d vector_eigen(
      vector_geom.size());
  for (size_t idx = 0; idx < vector_geom.size(); idx++)
  {
    vector_eigen[idx] = GeometryTransformToEigenIsometry3d(vector_geom[idx]);
  }
  return vector_eigen;
}

std::vector<geometry_msgs::Pose> VectorIsometry3dToVectorGeometryPose(
    const common_robotics_utilities::math::VectorIsometry3d& vector_eigen)
{
  std::vector<geometry_msgs::Pose> vector_geom(vector_eigen.size());
  for (size_t idx = 0; idx < vector_eigen.size(); idx++)
  {
    vector_geom[idx] = EigenIsometry3dToGeometryPose(vector_eigen[idx]);
  }
  return vector_geom;
}

std::vector<geometry_msgs::Transform> VectorIsometry3dToVectorGeometryTransform(
    const common_robotics_utilities::math::VectorIsometry3d& vector_eigen)
{
  std::vector<geometry_msgs::Transform> vector_geom(vector_eigen.size());
  for (size_t idx = 0; idx < vector_eigen.size(); idx++)
  {
    vector_geom[idx] = EigenIsometry3dToGeometryTransform(vector_eigen[idx]);
  }
  return vector_geom;
}
}  // namespace conversions
}  // namespace common_robotics_utilities


#include "utils.hpp"
#include <iostream>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/random.hpp>
#include <cmath>
using namespace std;
using Bint = boost::multiprecision::cpp_int;

string RSAPrivateKeyFactorizer_cppfunc(string s, string d_, string e_){
    Bint n(s);
    Bint d(d_);
    Bint e(e_);
    Bint t = e*d-1;
    Bint u = 0;
    using engine = boost::random::independent_bits_engine<boost::mt19937, 16384, Bint>;
    engine gen;
    while(true){
        Bint quotient = t/2;
        Bint remainder = t%2;
        if(remainder!=0){break;}
        u += 1;
        t = quotient;
    }
    bool found = false;
    Bint c1, c2;
    while(!found){
        Bint i("1");
        Bint a = (gen()+1)%n;
        while(i<=u and !found){
            Bint c1_ = powm(Bint("2"),i-1,n);
            c1 = powm(a,c1_*t,n);
            Bint c2_ = powm(Bint("2"),i,n);
            c2 = powm(a,c2_*t,n);
            found = (c1!=1) and (c1!=-1%n) and (c2==1);
            i+=1;
        }
    }
    Bint retval =  gcd(c1-1, n);
    return retval.str();

}


/*
 * warp_statistics.hpp
 *
 *  Created on: Nov 14, 2018
 *      Author: Gregory Kramida
 *   Copyright: 2018 Gregory Kramida
 *
 *   Licensed under the Apache License, Version 2.0 (the "License");
 *   you may not use this file except in compliance with the License.
 *   You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 *   Unless required by applicable law or agreed to in writing, software
 *   distributed under the License is distributed on an "AS IS" BASIS,
 *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *   See the License for the specific language governing permissions and
 *   limitations under the License.
 */

#pragma once

//libraries
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

//local
#include "typedefs.hpp"

namespace math {

/**
 * Locates the maximum L2 norm (length) of the vector in the given field.
 * @param[out] max_norm length of the longest vector
 * @param[out] coordinate the location of the longest vector
 * @param[in] vector_field the vector field to look at
 */
template<typename Scalar>
void locate_max_norm(typename Scalar::Scalar& max_norm, math::Vector2i& coordinates,
		const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& vector_field);
/**
 * Locates the maximum L2 norm (length) of the vector in the given field.
 * @overload
 */
template<typename Scalar>
void locate_max_norm(typename Scalar::Scalar& max_norm, math::Vector3i& coordinates,
		const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& vector_field);
/**
 * Locates the minimum L2 norm (length) of the vector in the given field.
 * @param[out] max_norm length of the longest vector
 * @param[out] coordinate the location of the longest vector
 * @param[in] vector_field the vector field to look at
 */
template<typename Scalar>
void locate_min_norm(typename Scalar::Scalar& min_norm, math::Vector2i& coordinates,
		const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& vector_field);

/**
 * Locates the minimum L2 norm (length) of the vector in the given field.
 * @overload
 */
template<typename Scalar>
void locate_min_norm(typename Scalar::Scalar& min_norm, math::Vector3i& coordinates,
		const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& vector_field);
/**
 * Locates the maximum in the given field.
 * @param[out] maximum the maximum coefficient
 * @param[out] coordinates coordinate of the maximum coefficient
 * @param[in] scalar_field the scalar field to look at
 */
template<typename Scalar>
void locate_maximum(Scalar& maximum, math::Vector2i& coordinates,
		const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& scalar_field);
/**
 * Locates the maximum in the given field.
 * @overload
 */
template<typename Scalar>
void locate_maximum(Scalar& maximum, math::Vector3i& coordinates,
		const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& scalar_field);

template<typename Scalar>
inline
Scalar maximum(const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& scalar_field){
	return static_cast<Eigen::Tensor<Scalar,0,Eigen::ColMajor> >(scalar_field.maximum())(0);
}

template<typename Scalar>
inline
Scalar maximum(const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& scalar_field){
	return scalar_field.maxCoeff();
}

template<typename Scalar>
inline
Scalar minimum(const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& scalar_field){
	return static_cast<Eigen::Tensor<Scalar,0,Eigen::ColMajor> >(scalar_field.minimum())(0);
}

template<typename Scalar>
inline
Scalar minimum(const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& scalar_field){
	return scalar_field.minCoeff();
}

//uses traversal function/functor combo, otherwise the same as locate_max_norm
void locate_max_norm2(float& max_norm, math::Vector2i& coordinates, const math::MatrixXv2f& vector_field);

/**
 * Locate the maximum L2 norm (length) of the vector in the given field.
 * @param[out] min_norm length of the shortest vector
 * @param[out] coordinate the location of the shortest vector
 * @param[in] vector_field the field to look at
 */
template<typename Scalar>
typename Scalar::Scalar max_norm(const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& vector_field);
/**
 * Locate the maximum L2 norm (length) of the vector in the given field.
 * @override
 */
template<typename Scalar>
typename Scalar::Scalar max_norm(const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& vector_field);
/**
 * Locate the minimum L2 norm (length) of the vector in the given field.
 * @param[out] min_norm length of the shortest vector
 * @param[out] coordinate the location of the shortest vector
 * @param[in] vector_field the field to look at
 */
template<typename Scalar>
typename Scalar::Scalar min_norm(const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& vector_field);
/**
 * Locate the minimum L2 norm (length) of the vector in the given field.
 * @override
 */
template<typename Scalar>
typename Scalar::Scalar min_norm(const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& vector_field);

template<typename Scalar>
Scalar mean(const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& field);

template<typename Scalar>
Scalar mean(const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& field);

template<typename Scalar>
Scalar std(const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>& field);

template<typename Scalar>
Scalar std(const Eigen::Tensor<Scalar, 3, Eigen::ColMajor>& field);

float ratio_of_vector_lengths_above_threshold(const math::MatrixXv2f& vector_field, float threshold);
float mean_vector_length(const math::MatrixXv2f& vector_field);
void mean_and_std_vector_length(float& mean, float& standard_deviation, const math::MatrixXv2f& vector_field);

}


//
// Multivariate Guassian Class.
// Original from Humphrey Hu
// (https://github.com/Humhu/argus_utils/blob/devel/include/argus_utils/random/MultivariateGaussian.hpp)
// Adapted by Arun Venkatraman
//

#pragma once

#include <Eigen/Dense>
#include <Eigen/Cholesky>

#include <cassert>
#include <random>

namespace math
{

class MultivariateGaussian
{
public:

	// Seeds the engine using a true random number. Sets mean_ to zero
	// and covariance to identity.
	MultivariateGaussian(const int dim);

	// Seeds the engine using the specified seed. Sets mean_ to zero
	// and _covariance to identity.
	MultivariateGaussian(const int dim, const int seed);

	// Sets the mean to mu and covariance to sigma.
	MultivariateGaussian(const Eigen::VectorXd& mu, const Eigen::MatrixXd& sigma);

	// Sets the mean to mu and covariance to sigma. Seeds the engine using a specified seed.
	MultivariateGaussian(const Eigen::VectorXd& mu, const Eigen::MatrixXd& sigma, const int seed);

	MultivariateGaussian(const MultivariateGaussian& other);
	MultivariateGaussian& operator=(const MultivariateGaussian& other);

    void set_mean(const Eigen::VectorXd& mu);
    void set_covariance(const Eigen::MatrixXd& sigma);
	void set_information(const Eigen::MatrixXd& inv_sigma);

	unsigned int get_dimension() const { return mean_.size(); }
	const Eigen::VectorXd& mean() const { return mean_; }
	const Eigen::MatrixXd covariance() const { return llt_.reconstructedMatrix(); }
	const Eigen::MatrixXd cholesky() const { return llt_.matrixL(); }

	// Generate a sample truncated at a specified number of standard deviations.
	Eigen::VectorXd sample(double max_var = 3.0);

	// Evaluate the multivariate normal PDF for the specified sample.
    double evaluate_probability(const Eigen::VectorXd& x) const;

protected:

    std::mt19937 gen_;
	std::normal_distribution<double> distribution_;

    // Mean of the distribution.
	Eigen::VectorXd mean_;
    // Store covariance as its cholesky decomposition.
	Eigen::LLT<Eigen::MatrixXd> llt_;

    // Normalization constant;
	double z_;

	void initialize_cov(const Eigen::MatrixXd& cov);
};

}


/*
Part of the Fluid Corpus Manipulation Project (http://www.flucoma.org/)
Copyright 2017-2019 University of Huddersfield.
Licensed under the BSD-3 License.
See license.md file in the project root for full license information.
This project has received funding from the European Research Council (ERC)
under the European Union’s Horizon 2020 research and innovation programme
(grant agreement No 725899).
*/

// modified version of Normalization.hpp code
#pragma once

#include "../util/FluidEigenMappings.hpp"
#include "../../data/TensorTypes.hpp"
#include <Eigen/Core>
#include <cassert>
#include <cmath>

namespace fluid {
namespace algorithm {

class RobustScaling
{
public:
  using ArrayXd = Eigen::ArrayXd;
  using ArrayXXd = Eigen::ArrayXXd;

  void init(double low, double high, RealMatrixView in)
  {
    using namespace Eigen;
    using namespace _impl;
    const double epsilon = std::numeric_limits<double>::epsilon();
    mLow = low;
    mHigh = high;
    ArrayXXd input = asEigen<Array>(in);
    mDataLow.resize(input.cols());
    mDataHigh.resize(input.cols());
    mMedian.resize(input.cols());
    mRange.resize(input.cols());
    index length = input.rows();
    for (index i = 0; i < input.cols(); i++)
    {
      ArrayXd sorted = input.col(i);
      std::sort(sorted.data(), sorted.data() + length);
      mMedian(i) = sorted(lrint(0.5 * (length - 1)));
      mDataLow(i) = sorted(lrint((mLow / 100.0) * (length - 1)));
      mDataHigh(i) = sorted(lrint((mHigh / 100.0) * (length - 1)));
    }
    mRange = mDataHigh - mDataLow;
    mRange = mRange.max(epsilon);
    mInitialized = true;
  }

  void init(double low, double high, RealVectorView dataLow,
            RealVectorView dataHigh, RealVectorView median,
            RealVectorView range)
  {
    using namespace Eigen;
    using namespace _impl;
    const double epsilon = std::numeric_limits<double>::epsilon();
    mLow = low;
    mHigh = high;
    mDataLow = asEigen<Array>(dataLow);
    mDataHigh = asEigen<Array>(dataHigh);
    mMedian = asEigen<Array>(median);
    mRange = asEigen<Array>(range);
    mRange =
        mRange.max(epsilon); // in case it is imported from the outside world
    mInitialized = true;
  }

  void processFrame(const RealVectorView in, RealVectorView out,
                    bool inverse = false) const
  {
    using namespace Eigen;
    using namespace _impl;
    ArrayXd input = asEigen<Array>(in);
    ArrayXd result;
    if (!inverse) { result = (input - mMedian) / mRange; }
    else
    {
      result = (input * mRange) + mMedian;
    }
    out = asFluid(result);
  }

  void process(const RealMatrixView in, RealMatrixView out,
               bool inverse = false) const
  {
    using namespace Eigen;
    using namespace _impl;
    ArrayXXd input = asEigen<Array>(in);
    ArrayXXd result;
    if (!inverse)
    {
      result = (input.rowwise() - mMedian.transpose());
      result = result.rowwise() / mRange.transpose();
    }
    else
    {
      result = (input.rowwise() * mRange.transpose());
      result = (result.rowwise() + mMedian.transpose());
    }
    out = asFluid(result);
  }

  void setLow(double low) { mLow = low; }
  void setHigh(double high) { mHigh = high; }
  bool initialized() const { return mInitialized; }

  double getLow() const { return mLow; }
  double getHigh() const { return mHigh; }

  void getDataLow(RealVectorView out) const
  {
    using namespace _impl;
    out = asFluid(mDataLow);
  }

  void getDataHigh(RealVectorView out) const
  {
    using namespace _impl;
    out = asFluid(mDataHigh);
  }

  void getMedian(RealVectorView out) const
  {
    using namespace _impl;
    out = asFluid(mMedian);
  }

  void getRange(RealVectorView out) const
  {
    using namespace _impl;
    out = asFluid(mRange);
  }

  index dims() const { return mMedian.size(); }
  index size() const { return 1; }

  void clear()
  {
    mLow = 0;
    mHigh = 1.0;
    mMedian.setZero();
    mRange.setZero();
    mRange.setZero();
    mInitialized = false;
  }

  double  mLow{0.0};
  double  mHigh{1.0};
  ArrayXd mDataHigh;
  ArrayXd mDataLow;
  ArrayXd mMedian;
  ArrayXd mRange;
  bool    mInitialized{false};
};
}; // namespace algorithm
}; // namespace fluid


#include <vector>
#include <chrono>
#include <Eigen/Dense>

#include "homography/compute_homography_model.hpp"
#include "homography/compute_homography_error.hpp"
#include "ransac.hpp"

using namespace std;
using namespace Eigen;

void random_measurements(Matrix3d& H, vector<Point2f>& src, vector<Point2f>& dst, int n) {
  src.clear();
  dst.clear();
  H = Matrix3d::Random();
  H /= H(2,2);
  VectorXd x = 1000*VectorXd::Random(n);
  VectorXd y = 1000*VectorXd::Random(n);
  for(int i = 0; i < n; i++) {
    Vector3d p(x(i), y(i), 1);
    Vector3d pp = H*p+Vector3d(1.*rand()/RAND_MAX, 1.*rand()/RAND_MAX, 0);
    src.emplace_back(p.x()/p.z(), p.y()/p.z());
    dst.emplace_back(pp.x()/pp.z(), pp.y()/pp.z());
  }
}

int main() {
  ComputeHomographyModel chm;
  ComputeHomographyError che;
  vector<HomographyMeasurement> measurements;

  measurements.emplace_back(Point2f(0,0), Point2f(1,1));
  measurements.emplace_back(Point2f(1,1), Point2f(2,2));
  measurements.emplace_back(Point2f(2,1), Point2f(3,4));
  measurements.emplace_back(Point2f(4,3), Point2f(1,5));

  auto start = chrono::high_resolution_clock::now();
  HomographyModel hm = chm(measurements);
  auto finish = chrono::high_resolution_clock::now();

  // cout << che(hm, measurements[0]) << endl;

  cout << "Simple homography calculation\n";
  cout << "Both matrices should be equal\n";
  cout << "Ours: " << chrono::duration<double>(finish-start).count() << " s" << endl;
  hm.print();

  vector<Point2f> src, dst;
  src.emplace_back(0,0);
  src.emplace_back(1,1);
  src.emplace_back(2,1);
  src.emplace_back(4,3);
  dst.emplace_back(1,1);
  dst.emplace_back(2,2);
  dst.emplace_back(3,4);
  dst.emplace_back(1,5);

  start = chrono::high_resolution_clock::now();
  Mat H = findHomography(src, dst);
  finish = chrono::high_resolution_clock::now();
  cout << "OpenCV's: " << chrono::duration<double>(finish-start).count() << " s" << endl;
  cout << H << endl;

  Matrix3d real_H;
  double opencv = 0, our = 0;
  for(int i = 0; i < 1000; i++) {
    random_measurements(real_H, src, dst, 10000);
    start = chrono::high_resolution_clock::now();
    H = findHomography(src, dst, CV_RANSAC);
    finish = chrono::high_resolution_clock::now();
    opencv += chrono::duration<double>(finish-start).count();
    // cout << "Real H:\n" << real_H << endl;
    // cout << "OpenCV's H: " << chrono::duration<double>(finish - start).count()
        //  << " s" << endl;
    // cout << H << endl;

    measurements.clear();
    for (size_t i = 0; i < src.size(); i++) {
      measurements.emplace_back(src[i], dst[i]);
    }

    start = chrono::high_resolution_clock::now();
    auto my_H =
        ransac<HomographyModel, HomographyMeasurement, ComputeHomographyModel,
               ComputeHomographyError, 4>(measurements);
    finish = chrono::high_resolution_clock::now();
    our += chrono::duration<double>(finish-start).count();

    // cout << "Our H: " << chrono::duration<double>(finish - start).count()
        //  << " s" << endl;
    // my_H.print();
  }
  cout << "OpenCV's: " << opencv << " s" << endl;
  cout << "Ours: " << our << " s" << endl;

  return EXIT_SUCCESS;
}


// Copyright Paul A. Bristow 2016
// Copyright John Z. Maddock 2016

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

/*! brief Example of using Lambert W function to compute current through a diode connected transistor with preset series resistance.
  \details T. C. Banwell and A. Jayakumar,
  Exact analytical solution of current flow through diode with series resistance,
  Electron Letters, 36(4):291-2 (2000)
  DOI:  doi.org/10.1049/el:20000301

  The current through a diode connected NPN bipolar junction transistor (BJT)
  type 2N2222 (See https://en.wikipedia.org/wiki/2N2222 and
  https://www.fairchildsemi.com/datasheets/PN/PN2222.pdf Datasheet)
  was measured, for a voltage between 0.3 to 1 volt, see Fig 2 for a log plot,
  showing a knee visible at about 0.6 V.

  The transistor parameter isat was estimated to be 25 fA and the ideality factor = 1.0.
  The intrinsic emitter resistance re was estimated from the rsat = 0 data to be 0.3 ohm.

  The solid curves in Figure 2 are calculated using equation 5 with rsat included with re.

  http://www3.imperial.ac.uk/pls/portallive/docs/1/7292572.PDF
*/

#include <boost/math/special_functions/lambert_w.hpp>
using boost::math::lambert_w0;

#include <iostream>
// using std::cout;
// using std::endl;
#include <exception>
#include <stdexcept>
#include <string>
#include <array>
#include <vector>

/*!
Compute thermal voltage as a function of temperature,
about 25 mV at room temperature.
https://en.wikipedia.org/wiki/Boltzmann_constant#Role_in_semiconductor_physics:_the_thermal_voltage

\param temperature Temperature (degrees centigrade).
*/
const double v_thermal(double temperature)
{
  constexpr const double boltzmann_k = 1.38e-23; // joules/kelvin.
  const double charge_q = 1.6021766208e-19; // Charge of an electron (columb).
  double temp =+ 273; // Degrees C to K.
  return boltzmann_k * temp / charge_q;
} // v_thermal

/*!
Banwell & Jayakumar, equation 2
*/
double i(double isat, double vd, double vt, double nu)
{
  double i = isat * (exp(vd / (nu * vt)) - 1);
  return i;
} //

/*!

Banwell & Jayakumar, Equation 4.
i current flow = isat
v voltage source.
isat reverse saturation current in equation 4.
(might implement equation 4 instead of simpler equation 5?).
vd voltage drop = v - i* rs  (equation 1).
vt  thermal voltage, 0.0257025 = 25 mV.
nu junction ideality factor (default = unity), also known as the emission coefficient.
re intrinsic emitter resistance, estimated to be 0.3 ohm from low current.
rsat reverse saturation current

\param v Voltage V to compute current I(V).
\param vt Thermal voltage, for example 0.0257025 = 25 mV, computed from boltzmann_k * temp / charge_q;
\param rsat Resistance in series with the diode.
\param re Instrinsic emitter resistance (estimated to be 0.3 ohm from the Rs = 0 data)
\param isat Reverse saturation current (See equation 2).
\param nu Ideality factor (default = unity).

\returns I amp as function of V volt.

*/
double iv(double v, double vt, double rsat, double re, double isat, double nu = 1.)
{
  // V thermal 0.0257025 = 25 mV
  // was double i = (nu * vt/r) * lambert_w((i0 * r) / (nu * vt)); equ 5.

  rsat = rsat + re;
  double i = nu * vt / rsat;
  std::cout << "nu * vt / rsat = " << i << std::endl; // 0.000103223

  double x = isat * rsat / (nu * vt);
  std::cout << "isat * rsat / (nu * vt) = " << x << std::endl;

  double eterm = (v + isat * rsat) / (nu * vt);
  std::cout << "(v + isat * rsat) / (nu * vt) = " << eterm << std::endl;

  double e = exp(eterm);
  std::cout << "exp(eterm) = " << e << std::endl;

  double w0 = lambert_w0(x * e);
  std::cout << "w0 = " << w0 << std::endl;
  return i * w0 - isat;

} // double iv

std::array<double, 5> rss = {0., 2.18, 10., 51., 249};  // series resistance (ohm).
std::array<double, 8> vds = { 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9 };  // Diode voltage.

int main()
{
  try
  {
    std::cout << "Lambert W diode current example." << std::endl;

    //[lambert_w_diode_example_1
    double x = 0.01;
    //std::cout << "Lambert W (" << x << ") = " << lambert_w(x) << std::endl; // 0.00990147

    double nu = 1.0;                  // Assumed ideal.
    double vt = v_thermal(25);        // v thermal, Shockley equation, expect about 25 mV at room temperature.
    double boltzmann_k = 1.38e-23;    // joules/kelvin
    double temp = 273 + 25;
    double charge_q = 1.6e-19;        // column
    vt = boltzmann_k * temp / charge_q;
    std::cout << "V thermal "
       << vt << std::endl;            // V thermal 0.0257025 = 25 mV
    double rsat = 0.;
    double isat = 25.e-15;            //  25 fA;
    std::cout << "Isat = " << isat << std::endl;

    double re = 0.3;  // Estimated from slope of straight section of graph (equation 6).

    double v = 0.9;
    double icalc = iv(v, vt, 249., re, isat);

    std::cout << "voltage = " << v << ", current = " << icalc << ", " << log(icalc) << std::endl; // voltage = 0.9, current = 0.00108485, -6.82631
 //] [/lambert_w_diode_example_1]
  }
  catch (std::exception& ex)
  {
    std::cout << ex.what() << std::endl;
  }
}  // int main()

/*
   Output:
//[lambert_w_output_1
   Lambert W diode current example.
   V thermal 0.0257025
   Isat = 2.5e-14
   nu * vt / rsat = 0.000103099
   isat * rsat / (nu * vt) = 2.42486e-10
   (v + isat * rsat) / (nu * vt) = 35.016
   exp(eterm) = 1.61167e+15
   w0 = 10.5225
   voltage = 0.9, current = 0.00108485, -6.82631

//] [/lambert_w_output_1]
*/


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

#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix2f m = Matrix2f::Random();
m = (m + m.adjoint()).eval();
JacobiRotation<float> J;
J.makeJacobi(m, 0, 1);
cout << "Here is the matrix m:" << endl << m << endl;
m.applyOnTheLeft(0, 1, J.adjoint());
m.applyOnTheRight(0, 1, J);
cout << "Here is the matrix J' * m * J:" << endl << m << endl;
  return 0;
}


// Copyright (c) 2022 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <Eigen/Core>
#include <Eigen/LU>
#include <cmath>
#include <functional>

#include "pyinterp/eigen.hpp"

namespace pyinterp::detail::math {

/// Known radial functions.
enum RadialBasisFunction : uint8_t {
  Cubic,
  Gaussian,
  InverseMultiquadric,
  Linear,
  Multiquadric,
  ThinPlate
};

//// A radial basis function (RBF) is a real-valued function φ whose value
/// depends only on the distance between the input and some fixed point,
/// either the origin, so that φ(x) = φ(║x║), or some other fixed point
/// c, called a center, so that φ(x) = φ(║x - c║). Any function φ that
/// satisfies the property φ(x) = φ(║x║) is a radial function.
template <typename T>
class RBF {
 public:
  /// Pointer to the Radial function used
  using PtrRadialBasisFunction =
      Matrix<T> (*)(const Eigen::Ref<const Matrix<T>>& r, const T);

  /// Default constructor
  ///
  /// @param epsilon Adjustable constant for gaussian or multiquadrics
  /// functions - defaults to approximate average distance between nodes
  /// (which is a good start).
  /// @param smooth Values greater than zero increase the smoothness of the
  /// approximation. 0 is for interpolation (default), the function will always
  /// go through the nodal points in this case.
  /// @param rbf The radial basis function, based on the radius, r, given by the
  /// norm (Euclidean distance)
  RBF(const T& epsilon, const T& smooth, const RadialBasisFunction rbf)
      : epsilon_(T(1) / epsilon), smooth_(smooth) {
    switch (rbf) {
      case RadialBasisFunction::Cubic:
        function_ = &RBF::cubic;
        break;
      case RadialBasisFunction::Gaussian:
        function_ = &RBF::gaussian;
        break;
      case RadialBasisFunction::InverseMultiquadric:
        function_ = &RBF::inverse_multiquadric;
        break;
      case RadialBasisFunction::Linear:
        function_ = &RBF::linear;
        break;
      case RadialBasisFunction::Multiquadric:
        function_ = &RBF::multiquadric;
        break;
      case RadialBasisFunction::ThinPlate:
        function_ = &RBF::thin_plate;
        break;
      default:
        throw std::invalid_argument("Radial function unknown: " +
                                    std::to_string(static_cast<int>(rbf)));
    }
  }

  /// Calculates the interpolated values
  ///
  /// @param xk Coordinates of the nodes
  /// @param yk Values of the nodes
  /// @param xi Coordinates to evaluate the interpolant at.
  /// @return interpolated values for each coordinates provided.
  [[nodiscard]] auto interpolate(const Eigen::Ref<const Matrix<T>>& xk,
                                 const Eigen::Ref<const Vector<T>>& yk,
                                 const Eigen::Ref<const Matrix<T>>& xi) const
      -> Vector<T> {
    // Matrix of distances between the coordinates provided.
    const auto r = RBF::distance_matrix(xk, xk);

    // Default epsilon to approximate average distance between nodes
    const auto epsilon =
        std::isnan(epsilon_) ? 1 / RBF<T>::average(r) : epsilon_;

    // TODO(fbriol)
    auto A = function_(r, epsilon);

    // Apply smoothing factor if needed
    if (smooth_) {
      A -= Matrix<T>::Identity(xk.cols(), xk.cols()) * smooth_;
    }

    return function_(distance_matrix(xk, xi), epsilon) *
           RBF<T>::solve_linear_system(A, yk);
  }

 private:
  /// Adjustable constant for gaussian or multiquadrics functions
  T epsilon_;

  /// Smooth factor
  T smooth_;

  /// Radial bassis function, based on the radius
  PtrRadialBasisFunction function_;

  // Calculates the distance average excluding the diagonal
  static auto average(const Eigen::Ref<const Matrix<T>>& distance) -> T {
    assert(distance.cols() == distance.rows());
    auto sum = T(0);
    auto n = distance.cols();
    for (Eigen::Index ix = 0; ix < distance.rows() - 1; ++ix) {
      for (Eigen::Index jx = ix + 1; jx < distance.cols(); ++jx) {
        sum += distance(ix, jx);
      }
    }
    return static_cast<T>(sum / (n * (n - 1) * 0.5));
  };

  /// Returns the distance between two points in a euclidean space
  static auto euclidean_distance(const Eigen::Ref<const Vector<T>>& x,
                                 const Eigen::Ref<const Vector<T>>& y) -> T {
    return std::sqrt((x - y).array().pow(2).sum());
  }

  /// Multiquadric
  static auto multiquadric(const Eigen::Ref<const Matrix<T>>& r,
                           const T epsilon) -> Matrix<T> {
    return ((epsilon * r).array().pow(2) + 1).sqrt();
  }

  /// Inverse multiquadric
  static auto inverse_multiquadric(const Eigen::Ref<const Matrix<T>>& r,
                                   const T epsilon) -> Matrix<T> {
    return 1.0 / multiquadric(r, epsilon).array();
  }

  /// Gauss
  static auto gaussian(const Eigen::Ref<const Matrix<T>>& r, const T epsilon)
      -> Matrix<T> {
    return (-(epsilon * r).array().pow(2)).exp();
  }

  /// Linear spline
  static auto linear(const Eigen::Ref<const Matrix<T>>& r, const T /*epsilon*/)
      -> Matrix<T> {
    return r;
  }

  /// Cubic spline
  static auto cubic(const Eigen::Ref<const Matrix<T>>& r, const T /*epsilon*/)
      -> Matrix<T> {
    return r.array().pow(3);
  }

  /// Thin plate spline
  static auto thin_plate(const Eigen::Ref<const Matrix<T>>& r,
                         const T /*epsilon*/) -> Matrix<T> {
    return (r.array() == 0).select(0, r.array().pow(2) * r.array().log());
  }

  /// Calculation of distances between the coordinates provided.
  static auto distance_matrix(const Eigen::Ref<const Matrix<T>>& xk,
                              const Eigen::Ref<const Matrix<T>>& xi)
      -> Matrix<T> {
    assert(xk.rows() == xi.rows());

    auto result = Matrix<T>(xi.cols(), xk.cols());

    for (Eigen::Index i0 = 0; i0 < xi.cols(); ++i0) {
      for (Eigen::Index i1 = 0; i1 < xk.cols(); ++i1) {
        result(i0, i1) = euclidean_distance(xi.col(i0), xk.col(i1));
      }
    }
    return result;
  }

  /// Resolution of the linear system
  static auto solve_linear_system(const Matrix<T>& A, const Vector<T>& di)
      -> Vector<T> {
    Eigen::FullPivLU<Matrix<T>> lu(A);
    return lu.solve(di);
  }
};

}  // namespace pyinterp::detail::math


/**
 * @file
 * @brief NPDE homework ElementMatrixComputation code
 * @author Janik Schüttler, edited by Oliver Rietmann
 * @date 03.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "mylinearloadvector.h"

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

#include <Eigen/Core>
#include <functional>

namespace ElementMatrixComputation {

namespace {

/* SAM_LISTING_BEGIN_1 */
Eigen::Vector4d computeLoadVector(
    const Eigen::MatrixXd &vertices,
    std::function<double(const Eigen::Vector2d &)> f) {
  // Number of nodes of the element: triangles = 3, rectangles = 4
  const int num_nodes = vertices.cols();
  // Vector for returning element vector
  Eigen::Vector4d elem_vec = Eigen::Vector4d::Zero();

  //====================

  // first two midpoints of edges of cell
  Eigen::Vector2d m1 = 0.5*(vertices.col(0) + vertices.col(1));
  Eigen::Vector2d m2 = 0.5*(vertices.col(1) + vertices.col(2));

  // == TRIANGLE ==
  if (num_nodes == 3) {
    // Third midpoint of edge
    Eigen::Vector2d m3 = 0.5*(vertices.col(2) + vertices.col(0));

    // Area
    double K = 0.5*( (vertices(0,1) - vertices(0,0)) * (vertices(1,2) - vertices(1,0)) -
                     (vertices(1,1) - vertices(1,0)) * (vertices(0,2) - vertices(0,0)));
    K /= num_nodes;

    elem_vec(0) = f(m1) + f(m3);
    elem_vec(1) = f(m1) + f(m2);
    elem_vec(2) = f(m2) + f(m3);
    elem_vec *= K/2.0;

    // == QUADRILITERA ==
    } else if (num_nodes == 4) {
    // Third and forth midpoints of edge
    Eigen::Vector2d m3 = 0.5*(vertices.col(2) + vertices.col(3));
    Eigen::Vector2d m4 = 0.5*(vertices.col(3) + vertices.col(0));

    // Area
    double K = ( vertices(0,1) - vertices(0,0) )*
               ( vertices(1,3) - vertices(1,0) );
    K /= num_nodes;

    elem_vec(0) = f(m1) + f(m4);
    elem_vec(1) = f(m1) + f(m2);
    elem_vec(2) = f(m2) + f(m3);
    elem_vec(3) = f(m3) + f(m4);
    elem_vec *= K/2.0;
    }

  //====================

  return elem_vec;
}
/* SAM_LISTING_END_1 */

}  // namespace

Eigen::Vector4d MyLinearLoadVector::Eval(const lf::mesh::Entity &cell) {
  // Topological type of the cell
  const lf::base::RefEl ref_el{cell.RefEl()};
  const lf::base::size_type num_nodes{ref_el.NumNodes()};

  // Obtain the vertex coordinates of the cell, which completely
  // describe its shape.
  const lf::geometry::Geometry *geo_ptr = cell.Geometry();

  // Matrix storing corner coordinates in its columns
  auto vertices = geo_ptr->Global(ref_el.NodeCoords());

  return computeLoadVector(vertices, f_);
}

}  // namespace ElementMatrixComputation


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

using namespace Eigen;
using namespace std;

int main(int, char**)
{
  cout.precision(3);
  Matrix4f A = MatrixXf::Random(4,4);
cout << "Here is a random 4x4 matrix:" << endl << A << endl;
HessenbergDecomposition<MatrixXf> hessOfA(A);
MatrixXf H = hessOfA.matrixH();
cout << "The Hessenberg matrix H is:" << endl << H << endl;
MatrixXf Q = hessOfA.matrixQ();
cout << "The orthogonal matrix Q is:" << endl << Q << endl;
cout << "Q H Q^T is:" << endl << Q * H * Q.transpose() << endl;

  return 0;
}


#include "problemes.h"
#include "arithmetique.h"
#include "premiers.h"

#include <fstream>
#include <boost/algorithm/string.hpp>

typedef unsigned long long nombre;
typedef std::vector<nombre> vecteur;

ENREGISTRER_PROBLEME(108, "Diophantine reciprocals I") {
    // In the following equation x, y, and n are positive integers.
    //
    //                                          1/x + 1/y = 1/n
    //
    // For n = 4 there are exactly three distinct solutions:
    //
    //      1/5 + 1/20 = 1/4
    //      1/6 + 1/12 = 1/4
    //      1/8 + 1/8 = 1/4
    //
    // What is the least value of n for which the number of distinct solutions exceeds one-thousand?
    //
    // NOTE: This problem is an easier version of Problem 110; it is strongly advised that you solve this one first.
    vecteur premiers;
    premiers::crible23<nombre>(1000000, std::back_inserter(premiers));

    nombre resultat = 0;
    for (nombre n = 2;; ++n) {
        std::map<nombre, nombre> decomposition;
        arithmetique::decomposition(n, premiers, decomposition);
        nombre solutions = 1;
        for (const auto &d: decomposition) {
            solutions *= (2 * d.second + 1);
        }
        solutions = (solutions + 1) / 2;
        if (solutions > 1000) {
            resultat = n;
            break;
        }
    }

    return std::to_string(resultat);
}


#include <dlib/matrix.h>
#include <dlib/optimization.h>
#include <iostream>
#include <vector>

using namespace std;

using column_vector = dlib::matrix<double, 0, 1>;

double f(const column_vector& col) {
    assert(col.nr() == 2);

    const double x1 = col(0);
    const double x2 = col(1);

    const double ret = -(x1 - 2) * (x1 - 2) - (x2 - 3) * (x2 - 3) + 5;

    printf("func called at x1 = %f, x2 = %f, f = %f\n", x1, x2, ret);

    return ret;
}

int main() {
    column_vector x = {0, 0};

    double ret = dlib::find_max_bobyqa(
        f, x, 5, dlib::uniform_matrix<double>(2, 1, -100.0),
        dlib::uniform_matrix<double>(2, 1, 100.0), 10, 1.E-6, 100);

    cout << x << endl;
    cout << ret << endl;

    return 0;
}


#include "BoostOdeint.hpp"

#include <Eigen/Core>

#include <tuple>
#include <iostream>

using namespace sodes::odeint;
using namespace Eigen;

// System to be solved
ArrayXd lorenz(const Ref<ArrayXd> x, Ref<ArrayXd> dxdt, const double &t)
{
    double sigma = 10.0;
    double R = 28.0;
    double b = 8.0 / 3.0;

    dxdt[0] = sigma * (x[1] - x[0]);
    dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
    dxdt[2] = -b * x[2] + x[0] * x[1];

    return dxdt;
}

int main (){
    // Defining the state vector with initial conditions
    int num_of_states = 3;
    ArrayXd X(num_of_states);
    X << 0., 1., 0.1;

    // Set time range and increment
    auto t_init = 0.0;
    auto t_final = 10.0;
    auto t_span = std::make_tuple(t_init, t_final);
    auto dt = 0.01;

    // Containers to store solution
    std::vector<ArrayXd> x_sol;
    std::vector<double> times;
    std::tie(times, x_sol) = solve_ivp_const(lorenz, t_span, dt, X, "runge_kutta_cash_karp54");

    // Displaying result on terminal
    auto steps = times.size();
    for (size_t i = 0; i < steps; i++)
    {
        std::cout << times[i] << '\t' << x_sol[i][0] << '\t' << x_sol[i][1] << '\t' << x_sol[i][2] << '\n';
    }
}


#pragma once

#include <cmath>
#include <cstdint>
#include <random>

#include <Eigen/Geometry>

namespace common_robotics_utilities
{
namespace gaussian_distributions
{
/// See https://people.sc.fsu.edu/~jburkardt/presentations/truncated_normal.pdf
/// for details on the implementations here.

/// Evaluate the value of the CDF of the gaussian distribution specified by
/// @param mean and @param std_dev at @param val.
inline double EvaluateGaussianCDF(
    const double mean, const double std_dev, const double val)
{
  return 0.5 * (1.0 + std::erf(((val - mean) / std_dev) / std::sqrt(2.0)));
}

/// Evaluate the value of the PDF of the gaussian distribution specified by
/// @param mean and @param std_dev at @param val.
inline double EvaluateGaussianPDF(
    const double mean, const double std_dev, const double val)
{
  const double exponent = ((val - mean) * (val - mean))
                          / (2.0 * std_dev * std_dev);
  const double fraction = 1.0 / (std_dev * std::sqrt(2.0 * M_PI));
  const double pdf = fraction * std::exp(-exponent);
  return pdf;
}

/// Evaluate the value of the CDF of the truncated gaussian distribution
/// specified by @param mean and @param std_dev and bounds @param lower_bound
/// and @param upper_bound at @param val.
inline double EvaluateTruncatedGaussianCDF(
    const double mean, const double lower_bound, const double upper_bound,
    const double std_dev, const double val)
{
  if (lower_bound > upper_bound)
  {
    throw std::invalid_argument("lower_bound > upper_bound");
  }
  if (val <= lower_bound)
  {
    return 0.0;
  }
  else if (val >= upper_bound)
  {
    return 1.0;
  }
  else
  {
    const double cdf_lower_bound
        = EvaluateGaussianCDF(mean, std_dev, lower_bound);
    const double numerator
        = EvaluateGaussianCDF(mean, std_dev, val) - cdf_lower_bound;
    const double denominator
        = EvaluateGaussianCDF(mean, std_dev, upper_bound) - cdf_lower_bound;
    return numerator / denominator;
  }
}

/// Evaluate the value of the PDF of the truncated gaussian distribution
/// specified by @param mean and @param std_dev and bounds @param lower_bound
/// and @param upper_bound at @param val.
inline double EvaluateTruncatedGaussianPDF(
    const double mean, const double lower_bound, const double upper_bound,
    const double std_dev, const double val)
{
  if (lower_bound > upper_bound)
  {
    throw std::invalid_argument("lower_bound > upper_bound");
  }
  if (val <= lower_bound)
  {
    return 0.0;
  }
  else if (val >= upper_bound)
  {
    return 0.0;
  }
  else
  {
    const double cdf_upper = EvaluateGaussianCDF(mean, std_dev, upper_bound);
    const double cdf_lower = EvaluateGaussianCDF(mean, std_dev, lower_bound);
    const double probability_enclosed = cdf_upper - cdf_lower;
    const double gaussian_pdf = EvaluateGaussianPDF(mean, std_dev, val);
    const double pdf = gaussian_pdf / probability_enclosed;
    return pdf;
  }
}

/// Integrate the probability contained between @param lower_limit and @param
/// upper_limit for the gaussian distribution specified by @param mean and
/// @param std_dev.
inline double IntegrateGaussian(
    const double mean, const double std_dev, const double lower_limit,
    const double upper_limit)
{
  if (lower_limit > upper_limit)
  {
    throw std::invalid_argument("lower_limit > upper_limit");
  }
  const double upper_limit_cdf
      = EvaluateGaussianCDF(mean, std_dev, upper_limit);
  const double lower_limit_cdf
      = EvaluateGaussianCDF(mean, std_dev, lower_limit);
  const double probability = upper_limit_cdf - lower_limit_cdf;
  return probability;
}

/// Integrate the probability contained between @param lower_limit and @param
/// upper_limit for the truncated gaussian distribution specified by @param mean
/// and @param std_dev and bounds @param lower_bound and @param upper_bound.
inline double IntegrateTruncatedGaussian(
    const double mean, const double lower_bound, const double upper_bound,
    const double std_dev, const double lower_limit, const double upper_limit)
{
  if (lower_bound > upper_bound)
  {
    throw std::invalid_argument("lower_bound > upper_bound");
  }
  if (lower_limit > upper_limit)
  {
    throw std::invalid_argument("lower_limit > upper_limit");
  }
  const double lower_limit_cdf
      = EvaluateTruncatedGaussianCDF(
          mean, lower_bound, upper_bound, std_dev, lower_limit);
  const double upper_limit_cdf
      = EvaluateTruncatedGaussianCDF(
          mean, lower_bound, upper_bound, std_dev, upper_limit);
  const double probability = upper_limit_cdf - lower_limit_cdf;
  return probability;
}

/// Note that comments come from (and refer to) parts of the original R plugin
/// that this implementation was derived from.
class TruncatedGaussianDistribution
{
private:

  double mean_ = 0.0;
  double stddev_ = 0.0;
  double std_lower_bound_ = 0.0;
  double std_upper_bound_ = 0.0;

  enum class DrawCases {TYPE_1, TYPE_2, TYPE_3, TYPE_4, NONE};
  DrawCases case_ = NONE;
  std::uniform_real_distribution<double> uniform_unit_dist_;
  std::uniform_real_distribution<double> uniform_range_dist_;
  std::exponential_distribution<double> exponential_dist_;
  std::normal_distribution<double> normal_dist_;

  bool CheckSimple(const double lower_bound, const double upper_bound) const
  {
    // Init Values Used in Inequality of Interest
    const double val1
        = (2 * std::sqrt(std::exp(1)))
          / (lower_bound + std::sqrt(std::pow(lower_bound, 2) + 4));
    const double val2
        = std::exp((std::pow(lower_bound, 2)
                    - lower_bound * std::sqrt(std::pow(lower_bound, 2) + 4))
                   / (4));
    if (upper_bound > lower_bound + val1 * val2)
    {
      return true;
    }
    else
    {
      return false;
    }
  }

  // Naive Accept-Reject algorithm
  template<typename Generator>
  double NaiveAcceptReject(
      const double lower_bound, const double upper_bound, Generator& prng)
  {
    while (true)
    {
      // In the R plugin, this was a call to Rf_rnorm(0.0, 1.0), in pure C++ it
      // is a call to std::normal_distribution<double>.
      const double draw = normal_dist_(prng);
      if ((draw <= upper_bound) && (draw >= lower_bound))
      {
          return draw;
      }
    }
  }

  // Accept-Reject Algorithm
  template<typename Generator>
  double SimpleAcceptReject(const double lower_bound, Generator& prng)
  {
    // Init Values
    const double alpha
        = (lower_bound + std::sqrt(std::pow(lower_bound, 2) + 4.0)) / (2.0);
    while (true)
    {
      // In the R plugin, this was a call to Rf_rexp(1.0), in pure C++ it is a
      // call to std::exponential_distribution<double>.
      const double e = exponential_dist_(prng);
      const double z = lower_bound + e / alpha;
      const double rho = std::exp(-std::pow(alpha - z, 2) / 2);
      // In the R plugin, this was a call to Rf_rnorm(0.0, 1.0), in pure C++ it
      // is a call to std::normal_distribution<double>.
      const double u = uniform_unit_dist_(prng);
      if (u <= rho)
      {
        return z;
      }
    }
  }

  // Accept-Reject Algorithm
  template<typename Generator>
  double ComplexAcceptReject(
      const double lower_bound, const double upper_bound, Generator& prng)
  {
    while (true)
    {
      // In the R plugin, this was a call to Rf_rnorm(lower_bound, upper_bound),
      // in pure C++ it is a call to std::normal_distribution<double>.
      const double z = uniform_range_dist_(prng);
      double rho = 0.0;
      if (0 < lower_bound)
      {
        rho = std::exp((std::pow(lower_bound, 2) - std::pow(z, 2)) / 2);
      }
      else if (upper_bound < 0)
      {
        rho = std::exp((std::pow(upper_bound, 2) - std::pow(z, 2)) / 2);
      }
      else if (0 < upper_bound && lower_bound < 0)
      {
        rho = std::exp(-std::pow(z, 2) / 2);
      }
      // In the R plugin, this was a call to Rf_rnorm(0.0, 1.0), in pure C++ it
      // is a call to std::normal_distribution<double>.
      const double u = uniform_unit_dist_(prng);
      if (u <= rho)
      {
        return z;
      }
    }
  }

  template<typename Generator>
  double Sample(Generator& prng)
  {
    if (case_ == DrawCases::TYPE_1)
    {
      const double draw
          = NaiveAcceptReject(std_lower_bound_, std_upper_bound_, prng);
      return mean_ + stddev_ * draw;
    }
    else if (case_ == DrawCases::TYPE_2)
    {
      const double draw = SimpleAcceptReject(std_lower_bound_, prng);
      return mean_ + stddev_ * draw;
    }
    else if (case_ == DrawCases::TYPE_3)
    {
      while (true)
      {
        const double draw = SimpleAcceptReject(std_lower_bound_, prng);
        if (draw <= std_upper_bound_)
        {
          return mean_ + stddev_ * draw;
        }
      }
    }
    else if (case_ == DrawCases::TYPE_4)
    {
      const double draw
          = ComplexAcceptReject(std_lower_bound_, std_upper_bound_, prng);
      return mean_ + stddev_ * draw;
    }
    else
    {
      if (case_ == DrawCases::NONE)
      {
        return mean_;
      }
      else
      {
        throw std::runtime_error("Invalid case");
      }
    }
  }

public:

  TruncatedGaussianDistribution(
      const double mean, const double stddev, const double lower_bound,
      const double upper_bound)
    : uniform_unit_dist_(0.0, 1.0),
      uniform_range_dist_(lower_bound, upper_bound),
      exponential_dist_(1.0), normal_dist_(0.0, 1.0)
  {
    // Set operating parameters
    mean_ = mean;
    stddev_ = stddev;
    if (std::abs(stddev_) == 0.0)
    {
      case_ = DrawCases::NONE;
    }
    else
    {
      // Standardize the lower and upper bounds
      std_lower_bound_ = (lower_bound - mean_) / stddev_;
      std_upper_bound_ = (upper_bound - mean_) / stddev_;
      // Set the operating case - i.e. which sampling method we will use
      case_ = DrawCases::NONE;
      if (0.0 <= std_upper_bound_ && 0.0 >= std_lower_bound_)
      {
        case_ = DrawCases::TYPE_1;
      }
      if (0.0 < std_lower_bound_
          && std_upper_bound_ == std::numeric_limits<double>::infinity())
      {
        case_ = DrawCases::TYPE_2;
      }
      if (0.0 > std_upper_bound_
          && std_lower_bound_ == -std::numeric_limits<double>::infinity())
      {
        std_lower_bound_ = -1 * std_upper_bound_;
        std_upper_bound_ = std::numeric_limits<double>::infinity();
        stddev_ = -1 * stddev_;
        case_ = DrawCases::TYPE_2;
      }
      if ((0.0 > std_upper_bound_ || 0.0 < std_lower_bound_)
          && !(std_upper_bound_ == std::numeric_limits<double>::infinity()
               || std_lower_bound_ == -std::numeric_limits<double>::infinity()))
      {
        if (CheckSimple(std_lower_bound_, std_upper_bound_))
        {
          case_ = DrawCases::TYPE_3;
        }
        else
        {
          case_ = DrawCases::TYPE_4;
        }
      }
      if ((case_ != DrawCases::TYPE_1) && (case_ != DrawCases::TYPE_2)
          && (case_ != DrawCases::TYPE_3) && (case_ != DrawCases::TYPE_4))
      {
        throw std::invalid_argument("case cannot be NONE with stddev == 0");
      }
    }
  }

  template<typename Generator>
  double operator()(Generator& prng)
  {
    return Sample(prng);
  }
};

/// Simple multivariate gaussian distribution.
class MultivariteGaussianDistribution
{
private:
  const Eigen::VectorXd mean_;
  const Eigen::MatrixXd norm_transform_;

  std::normal_distribution<double> unit_gaussian_dist_;

  template<typename Generator>
  Eigen::VectorXd Sample(Generator& prng)
  {
    Eigen::VectorXd draw;
    draw.resize(mean_.rows());
    for (ssize_t idx = 0; idx < draw.rows(); idx++)
    {
      draw(idx) = unit_gaussian_dist_(prng);
    }
    return norm_transform_ * draw + mean_;
  }

  static Eigen::MatrixXd CalculateNormTransform(
      const Eigen::MatrixXd& covariance)
  {
    Eigen::MatrixXd norm_transform;
    Eigen::LLT<Eigen::MatrixXd> chol_solver(covariance);
    if (chol_solver.info() == Eigen::Success)
    {
      // Use cholesky solver
      norm_transform = chol_solver.matrixL();
    }
    else
    {
      // Use eigen solver
      Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigen_solver(covariance);
      norm_transform
          = eigen_solver.eigenvectors()
            * eigen_solver.eigenvalues().cwiseMax(0.0).cwiseSqrt().asDiagonal();
    }
    return norm_transform;
  }

public:
  MultivariteGaussianDistribution(
      const Eigen::VectorXd& mean, const Eigen::MatrixXd& covariance)
      : mean_(mean), norm_transform_(CalculateNormTransform(covariance)),
        unit_gaussian_dist_(0.0, 1.0)
  {
    if (mean_.rows() != covariance.rows() || mean_.cols() != covariance.cols())
    {
      throw std::invalid_argument("mean and covariance are different sizes");
    }
    const auto nan_check = [] (const double& val) { return std::isnan(val); };
    if ((norm_transform_.unaryExpr(nan_check)).any())
    {
      throw std::runtime_error(
          "NaN Found in norm_transform in MultivariateGaussianDistribution");
    }
    const auto inf_check = [] (const double& val) { return std::isinf(val); };
    if ((norm_transform_.unaryExpr(inf_check)).any())
    {
      throw std::runtime_error(
          "Inf Found in norm_transform in MultivariateGaussianDistribution");
    }
  }

  template<typename Generator>
  Eigen::VectorXd operator()(Generator& prng)
  {
    return Sample(prng);
  }
};
}  // namespace gaussian_distributions
}  // namespace common_robotics_utilities


#include <iostream>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

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

    Matrix<float, 10, 10> originMat = Matrix<float, 10, 10>::Random(10, 10);
    cout << "original matrix: \n" << originMat << endl;

    Matrix<float, 3, 3> eyeMat = Matrix<float, 3, 3>::Identity(3,3);
    cout << "3x3 identity matrix: \n" << eyeMat << endl;

    originMat.block<3, 3>(0, 0) = eyeMat;
    cout << "original matrix with 3x3 identity block: \n" << originMat << endl;

    return 0;
}


/*!
 * @file basis_q1.hpp
 * @brief Contains implementation of \f$Q_1\f$-basis functions for a given quadrilateral.
 * @author Konrad Simon
 * @date August 2019
 */

#ifndef INCLUDE_BASIS_Q1_HPP_
#define INCLUDE_BASIS_Q1_HPP_

// Deal.ii
#include <deal.II/base/function.h>

#include <deal.II/lac/full_matrix.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

// STL
#include <cmath>
#include <fstream>

// My Headers
#include "coefficients.h"

namespace Coefficients
{
using namespace dealii;

/*!
 * @class BasisQ1
 * @brief Class implements scalar \f$Q_1\f$-basis functions for a given quadrilateral.
 */
template <int dim>
class BasisQ1 : public Function<dim>
{
public:
	BasisQ1();

	void set_index (unsigned int index);

	void set_coeff(const typename Triangulation<dim>::active_cell_iterator &cell);

	virtual double value(const Point<dim> &p,
						const unsigned int component = 0) const override;
	virtual void value_list(const std::vector<Point<dim>> &points,
								std::vector<double>  &values,
								const unsigned int component = 0) const override;

private:
	/*!
	 * Index of current basis function to be evaluated.
	 */
	unsigned int 		index_basis;

	/*!
	 * Matrix columns hold coefficients of basis functions.
	 */
	FullMatrix<double>	coeff_matrix;
	bool is_set_coeff_matrix;
};

/*!
 * Constructor.
 * @param cell
 */
template<int dim>
BasisQ1<dim>::BasisQ1 ()
:
Function<dim>(),
index_basis(0),
coeff_matrix(FullMatrix<double>(std::pow(2,dim),std::pow(2,dim))),
is_set_coeff_matrix(false)
{
}

/*!
 * Set the coefficients. Template specialization \f$dim=2\f$.
 * @param cell
 *
 * Build up coefficient matrix \f$A=(a_{ij})\f$ for basis polynomial
 * \f$\varphi_i(x,y)=a_i^0 + a_i^1 x + a_i^2 y + a_i^3 xy \f$. The \f$i\f$-th column
 * of the matrix hence contains the coefficients for the \f$i\f$-th basis associated
 * to the \f$i\f$-th vertex.
 */
template<>
void
BasisQ1<2>::set_coeff (const typename Triangulation<2>::active_cell_iterator &cell)

{
	FullMatrix<double>	point_matrix(4,4);

	for (unsigned int i=0; i<4; ++i)
	{
		const Point<2>& p = cell->vertex(i);

		point_matrix(i,0) = 1;
		point_matrix(i,1) = p(0);
		point_matrix(i,2) = p(1);
		point_matrix(i,3) = p(0)*p(1);
	}

	// Columns of coeff_matrix are the coefficients of the polynomial
	coeff_matrix.invert (point_matrix);

	is_set_coeff_matrix = true;
}

/*!
 * Set the coefficients. Template specialization \f$dim=3\f$.
 * @param cell
 *
 * Build up coefficient matrix \f$A=(a_{ij})\f$ for basis polynomial
 * \f$\varphi_i(x,y)=a_i^0 + a_i^1 x + a_i^2 y + a_i^3 xy \f$. The \f$i\f$-th column
 * of the matrix hence contains the coefficients for the \f$i\f$-th basis associated
 * to the \f$i\f$-th vertex.
 */
template<>
void
BasisQ1<3>::set_coeff (const typename Triangulation<3>::active_cell_iterator &cell)
{
	FullMatrix<double>	point_matrix(8,8);

	for (unsigned int i=0; i<8; ++i)
	{
		const Point<3>& p = cell->vertex(i);

		point_matrix(i,0) = 1;
		point_matrix(i,1) = p(0);
		point_matrix(i,2) = p(1);
		point_matrix(i,3) = p(2);
		point_matrix(i,4) = p(0)*p(1);
		point_matrix(i,5) = p(1)*p(2);
		point_matrix(i,6) = p(0)*p(2);
		point_matrix(i,7) = p(0)*p(1)*p(2);
	}

	// Columns of coeff_matrix are the coefficients of the polynomial
	coeff_matrix.invert (point_matrix);

	is_set_coeff_matrix = true;
}

/*!
 * Set the index of the basis function to be evaluated.
 *
 * @param index
 */
template<int dim>
void
BasisQ1<dim>::set_index (unsigned int index)
{
	index_basis = index;
}

/*!
 * Evaluate a basis function with a preset index at one given point in 2D.
 *
 * @param p
 * @param component
 */
template<>
double
BasisQ1<2>::value (const Point<2> &p,
					const unsigned int /* component */) const
{
	Assert (is_set_coeff_matrix,
			ExcMessage ("Coefficient matrix must be set first."));

	double value = coeff_matrix(0,index_basis)
					+ coeff_matrix(1,index_basis)*p(0)
					+ coeff_matrix(2,index_basis)*p(1)
					+ coeff_matrix(3,index_basis)*p(0)*p(1);

	return value;
}

/*!
 * Evaluate a basis function with a preset index at one given point in 3D.
 *
 * @param p
 * @param component
 */
template<>
double
BasisQ1<3>::value (const Point<3> &p,
					const unsigned int /* component */) const
{
	Assert (is_set_coeff_matrix,
			ExcMessage ("Coefficient matrix must be set first."));

	double value = coeff_matrix(0,index_basis)
					+ coeff_matrix(1,index_basis)*p(0)
					+ coeff_matrix(2,index_basis)*p(1)
					+ coeff_matrix(3,index_basis)*p(2)
					+ coeff_matrix(4,index_basis)*p(0)*p(1)
					+ coeff_matrix(5,index_basis)*p(1)*p(2)
					+ coeff_matrix(6,index_basis)*p(0)*p(2)
					+ coeff_matrix(7,index_basis)*p(0)*p(1)*p(2);

	return value;
}

/*!
 * Evaluate a basis function with a preset index on a given set of points in 2D.
 *
 * @param points
 * @param values
 */
template<>
void
BasisQ1<2>::value_list(const std::vector<Point<2>> &points,
								std::vector<double>  &values,
								const unsigned int /*component = 0*/) const
{
	Assert (points.size() == values.size(),
			ExcDimensionMismatch (points.size(), values.size()) );
	Assert (is_set_coeff_matrix,
			ExcMessage ("Coefficient matrix must be set first."));

	for ( unsigned int p=0; p<points.size(); ++p)
	{
		values[p] = coeff_matrix(0,index_basis)
							+ coeff_matrix(1,index_basis)*points[p](0)
							+ coeff_matrix(2,index_basis)*points[p](1)
							+ coeff_matrix(3,index_basis)*points[p](0)*points[p](1);

	} // end ++p
}

/*!
 * Evaluate a basis function with a preset index on a given set of points in 3D.
 *
 * @param points
 * @param values
 */
template<>
void
BasisQ1<3>::value_list(const std::vector<Point<3>> &points,
								std::vector<double>  &values,
								const unsigned int /*component = 0*/) const
{
	Assert (points.size() == values.size(),
			ExcDimensionMismatch (points.size(), values.size()) );
	Assert (is_set_coeff_matrix,
			ExcMessage ("Coefficient matrix must be set first."));

	for ( unsigned int p=0; p<points.size(); ++p)
	{
		values[p] = coeff_matrix(0,index_basis)
							+ coeff_matrix(1,index_basis)*points[p](0)
							+ coeff_matrix(2,index_basis)*points[p](1)
							+ coeff_matrix(3,index_basis)*points[p](2)
							+ coeff_matrix(4,index_basis)*points[p](0)*points[p](1)
							+ coeff_matrix(5,index_basis)*points[p](1)*points[p](2)
							+ coeff_matrix(6,index_basis)*points[p](0)*points[p](2)
							+ coeff_matrix(7,index_basis)*points[p](0)*points[p](1)*points[p](2);

	} // end ++p
}

} // end namespace Coefficients

#endif /* INCLUDE_BASIS_Q1_HPP_ */


#pragma once
#include "dirichlet_boundary.hpp"
#include "dofs.hpp"
#include "load_vector_assembly.hpp"
#include "stiffness_matrix_assembly.hpp"
#include <Eigen/Core>
#include <igl/slice.h>
#include <igl/slice_into.h>
#include <string>
#include <tuple>

typedef Eigen::VectorXd Vector;

//----------------solveBegin----------------
//! Solve the FEM system.
//!
//! @param[out] u will at the end contain the FEM solution.
//! @param[in] quadraticDofs containing mesh data
//! @param[in] f the RHS f (as in the exercise)
//! return number of degrees of freedom (without the boundary dofs)
int solveFiniteElement(Vector &     u,
                       const QDofs &quadraticDofs,
                       const std::function<double(double, double)> &f) {
	// get quadratic dofs
	Eigen::MatrixXi qdof;
	int             N = quadraticDofs.get_dofs(qdof);

	// get mesh vertices
	const Eigen::MatrixXd &vertices = quadraticDofs.get_vertices();

	SparseMatrix A;
	// (write your solution here)
	assembleStiffnessMatrix(A, vertices, qdof, N);

	Vector F;
	// (write your solution here)
	assembleLoadVector(F, vertices, qdof, N, f);

	u.resize(N);
	u.setZero();
	Eigen::VectorXi interiorDofs;

	auto zerobc = [](double x, double y) {
		std::ignore = x;
		std::ignore = y;
		return 0;
	};
	// set homogeneous Dirichlet Boundary conditions
	setDirichletBoundary(u, interiorDofs, quadraticDofs, zerobc);
	F -= A * u;

	SparseMatrix AInterior;
	igl::slice(A, interiorDofs, interiorDofs, AInterior);
	Eigen::SimplicialLDLT<SparseMatrix> solver;

	Vector FInterior;
	igl::slice(F, interiorDofs, FInterior);

	//initialize solver for AInterior
	// (write your solution here)
	solver.compute(AInterior);

	if (solver.info() != Eigen::Success) {
		throw std::runtime_error("Could not decompose the matrix");
	}

	//solve interior system
	// (write your solution here)
	Vector uInterior = solver.solve(FInterior);
	igl::slice_into(uInterior, interiorDofs, u);

	return interiorDofs.size();
}
//----------------solveEnd----------------


#include "reverse.hpp"
#include "forward.hpp"
#include <Eigen/Dense>
#include <vector>
#include <iostream>

namespace autodiff {

/**
 * Set the value and derivative of the specified function at the
 * specified input.  The functor must define the method
 * `dual<double> operator()(const dual<double>& x) const;`
 *
 * @tparam F type of function
 * @param[in] f function
 * @param[in] x input
 * @param[out] y function applied to input
 * @param[out] dy_dx derivative of y w.r.t. x
 */
template <typename F>
void derivative(const F& f, double x, double& y, double& dy_dx) {
  dual<double> xd{ x, 1 };
  auto yd = f(xd);
  y = yd.val();
  dy_dx = yd.tan();
}

/**
 * Set the value and gradient of the specified function at the
 * specified input.  The functor must define the method
 * `adj operator()(const Eigen::Matrix<adj, -1, 1>& x) const;`.
 *
 * @tparam F type of function
 * @param[in] f function
 * @param[in] x input
 * @param[out] fx function applied to input
 * @param[out] grad gradient of function applied to input
 */
template <typename F>
void gradient(const F& f, const Eigen::VectorXd& x,
              double& fx, Eigen::RowVectorXd& grad) {
  Eigen::Matrix<adj, -1, 1> xa(x.size());
  for (int i = 0; i < xa.size(); ++i)
    xa(i) = x(i);
  adj y = f(xa);
  std::vector<double> adjoints = chain(y);
  fx = y.val();
  grad.resize(x.size());
  for (int i = 0; i < x.size(); ++i)
    grad(i) = adjoints[xa(i).idx()];
}

/**
 * Set the value of the specified function at the specified value and
 * the product of the gradient of the function at the specified value
 * and the specified vector.
 *
 * @tparam F type of function
 * @param[in] f function
 * @param[in] x input
 * @param[in] v vector to multiply
 * @param[out] fx value of f(x)
 * @param[out] gradv product of gradient of f(x) and v
 */
template <typename F>
void vector_gradient_product(const F& f, const Eigen::VectorXd& x,
                             const Eigen::VectorXd v, double& fx,
                             double& gradv) {
  Eigen::Matrix<dual<double>, -1, 1> xd(x.size());
  for (int i = 0; i < xd.size(); ++i)
    xd(i) = { x(i), v(i) };
  dual<double> fxd = f(xd);
  fx = fxd.val();
  gradv = fxd.tan();
}

/**
 * Set the specified Hessian matrix to the Hessian of the specified
 * function at the specified value.
 *
 * @tparam F type of function
 * @param[in] f function
 * @param[in] x input
 * @param[out] H set to Hessian of function at input
 */
template <typename F>
void hessian(const F& f, const Eigen::VectorXd& x, Eigen::MatrixXd& H) {
  int N = x.size();
  H.resize(N, N);
  for (int m = 0; m < N; ++m) {  // m-th row of H
    Eigen::Matrix<dual<adj>, -1, 1> xd(N);
    for (int n = 0; n < N; ++n)
      xd(n) = { x(n), m == n };  // d.x(n)/d.x(m) = 1 iff m == n
    auto y = f(xd);
    std::vector<double> adjoints = chain(y.tan());
    for (int n = 0; n < N; ++n)
      H(m, n) = adjoints[xd(n).val().idx()];  // = bar(dot(x[m, n]))
  }
}

}


#include <Eigen/Core>

#include "quaternion_product.h"

/*
  Assume quaternion data format is (x, y, z, w) and translational format is (x,
  y, z)

  Verifies against Eigen cross product implementation
*/

__attribute__((always_inline)) Eigen::Vector3f crossProduct(Eigen::Vector3f lhs,
                                     Eigen::Vector3f rhs) {

  Eigen::Vector3f result(
      lhs.coeff(1) * rhs.coeff(2) - lhs.coeff(2) * rhs.coeff(1),
      lhs.coeff(2) * rhs.coeff(0) - lhs.coeff(0) * rhs.coeff(2),
      lhs.coeff(0) * rhs.coeff(1) - lhs.coeff(1) * rhs.coeff(0));
  return result;
}

/*
  Computes the point product
*/
__attribute__((always_inline)) Eigen::Vector3f pointProduct(Eigen::Vector4f &q,
                                     Eigen::Vector3f &p) {
  Eigen::Vector3f qvec(q(0), q(1), q(2));

  Eigen::Vector3f uv = crossProduct(qvec, p); // qvec.cross(p);
  for (int i = 0; i < 3; i++) {
    uv(i) = uv(i) * 2;
  }
  Eigen::Vector3f qxuv = crossProduct(qvec, uv); // qvec.cross(uv);

  Eigen::Vector3f result;
  result = p + (q(3) * uv) + qxuv;

  return result;
}

/*
  Takes two quaternions and multiplies them together

  Verified against the SE3 product in Sophus
*/
SE3T quaternionProduct(SE3T &a, SE3T &b) {
  Eigen::Vector3f resultTranslation;
  Eigen::Vector4f resultQuaternion;

  Eigen::Vector4f aq = a.quaternion;
  Eigen::Vector4f bq = b.quaternion;

  // Hamilton product
  resultQuaternion(3) =
      aq(3) * bq(3) - aq(0) * bq(0) - aq(1) * bq(1) - aq(2) * bq(2);
  resultQuaternion(0) =
      aq(3) * bq(0) + aq(0) * bq(3) + aq(1) * bq(2) - aq(2) * bq(1);
  resultQuaternion(1) =
      aq(3) * bq(1) + aq(1) * bq(3) + aq(2) * bq(0) - aq(0) * bq(2);
  resultQuaternion(2) =
      aq(3) * bq(2) + aq(2) * bq(3) + aq(0) * bq(1) - aq(1) * bq(0);

  Eigen::Vector3f r = pointProduct(a.quaternion, b.translation);

  resultTranslation = a.translation + r;

  SE3T result = {resultQuaternion, resultTranslation};
  return result;
}


/*
Copyright (C) 2012 Mathias Eitz and Ronald Richter.
All rights reserved.

This file is part of the imdb library and is made available under
the terms of the BSD license (see the LICENSE file).
*/

#ifndef DISTANCE_HPP
#define DISTANCE_HPP

#include <cmath>
#include <limits>
#include <iterator>

#include <boost/function.hpp>

#include "../util/types.hpp"

// ----------------------------------------------------------------
// Note: only distance metrics allowed here, i.e. functions that
// describe the distance between two descriptors and thus smaller
// distances denote more similar objects. The dotproduct itself
// e.g. is NOT a distance measure, it is a similarity measure
// ----------------------------------------------------------------

namespace imdb
{

/**
 * \addtogroup distfn
 * @{
 */

/// L1 norm
template <class T, class R = float>
struct l1norm
{
    // stl
    typedef T first_argument_type;
    typedef T second_argument_type;
    typedef R result_type;

    /// L1 distance between a and b.
    R operator() (const T& a, const T& b) const
    {
        R s = 0;
        for (typename T::const_iterator ai = a.begin(), bi = b.begin(); ai != a.end(); ++ai, ++bi)
        {
            R d = static_cast<R>(*ai) - static_cast<R>(*bi);
            s += std::abs(d);
        }
        return s;
    }
};

/**
 * @brief Squared L2 (Euclidean) distance function.
 *
 * Using the squared Euclidean distance is a bit faster than the L2 norm as we avoid the call to sqrt.
 */
template <class T, class R = float>
struct l2norm_squared
{
    // stl
    typedef T first_argument_type;
    typedef T second_argument_type;
    typedef R result_type;

    /// Squared L2 distance between a and b.
    R operator() (const T& a, const T& b) const
    {
        R s = 0;
        for (typename T::const_iterator ai = a.begin(), bi = b.begin(); ai != a.end(); ++ai, ++bi)
        {
            R d = static_cast<R>(*ai) - static_cast<R>(*bi);
            s += d*d;
        }
        return s;
    }
};

/**
 * @brief L2 (Euclidean) distance function
 */
template <class T, class R = float>
struct l2norm
{
    // stl
    typedef T first_argument_type;
    typedef T second_argument_type;
    typedef R result_type;

    l2norm_squared<T,R> n;

    /// L2 (Euclidean) distance between a and b.
    R operator() (const T& a, const T& b) const
    {
        return std::sqrt(n(a,b));
    }
};

/**
 * @brief One minus dot product distance function.
 *
 * 1 - <a,b> distance function. Assumes a and b are two vectors in R^d
 * having \b unit length
 */
template <class T, class R = float>
struct one_minus_dot
{
    // stl
    typedef T first_argument_type;
    typedef T second_argument_type;
    typedef R result_type;

    /// Computes 1 - <a,b>. As we assume that both a and b have unit length, the
    /// result is guaranteed to lie in [0,2]
    R operator() (const T& a, const T& b) const
    {
        R s = 0;
        for (typename T::const_iterator ai = a.begin(), bi = b.begin(); ai != a.end(); ++ai, ++bi)
        {
            s += static_cast<R>(*ai) * static_cast<R>(*bi);
        }
        return (1.0 - s);
    }
};

/// Jensen-Shannon divergence
template <class T, class R = float>
struct jsd
{
    // stl
    typedef T first_argument_type;
    typedef T second_argument_type;
    typedef R result_type;

    R operator() (const T& a, const T& b) const
    {
        R s = 0;
        for (typename T::const_iterator ai = a.begin(), bi = b.begin(); ai != a.end(); ++ai, ++bi)
        {
            R v0 = *ai;
            R v1 = *bi;
            R n = 2.0 / (v0 + v1);

            s += ((v0 > 0.0) ? v0 * std::log(v0*n):0.0) + ((v1 > 0.0) ? v1 * std::log(v1*n):0.0);
        }
        return s;
    }
};

/**
 * @brief Chi squared distance function.
 *
 * Chi squared distance function between two vectors k and h; d = sum_i[(ai - bi)^2 / (ai+bi)].
 * We avoid division through zero by adding a tiny value to the denominator:
 * -# if ki == hi == 0 then the nominator is zero and the overall result is zero as desired
 * -# if ki or hi != 0 then ki + hi == ki + hi + float_min
 */
template <class T, class R = float>
struct chi2
{

    // stl
    typedef T first_argument_type;
    typedef T second_argument_type;
    typedef R result_type;

    R operator() (const T& a, const T& b) const
    {
        R s = 0;
        for (typename T::const_iterator ai = a.begin(), bi = b.begin(); ai != a.end(); ++ai, ++bi)
        {
            R v0 = *ai;
            R v1 = *bi;
            R nom = v0-v1;
            R denom = v0+v1+std::numeric_limits<R>::epsilon();
            s += nom*nom / denom;
        }
        return s;
    }
};

/**
 * @brief Distance function from 'Eitz et al. - An evaluation of descriptors for large-scale image retrieval from sketched feature lines'
 *
 * Used to compare two Tensor descriptors generated by a tensor_generator. Note that this distance function
 * is different from all others in that before you can actually use operator() you need to set the member
 * variable mask, a vector<bool> of size a.size()/3 which contains a boolean for each grid cell of the
 * Tensor descriptor. mask[i] = true indicates that cell i is masked out, i.e. this cell is ignored when
 * computing the distance.
 */
template <class T, class R = float>
struct dist_frobenius
{
    // stl
    typedef T first_argument_type;
    typedef T second_argument_type;
    typedef R result_type;

    R operator() (const T& a, const T& b) const
    {
        assert(a.size() % 3 == 0);
        assert(a.size() == b.size());

        size_t numCells = a.size() / 3;
        assert(mask->size() == numCells);
        size_t index = 0;
        R dist = 0;

        for (size_t i = 0; i < numCells; i++)
        {
            // masking vector provided by query sketch, if
            // a cell in the query is empty (i.e. contains
            // no lines, we ignore it in the distance computation
            if ((*mask)[i]) continue;

            // difference tensor
            R dE = a[index] - b[index]; index++;
            R dF = a[index] - b[index]; index++;
            R dG = a[index] - b[index]; index++;

            // frobenius norm of difference tensor
            dist += std::sqrt(dE*dE + 2*dF*dF + dG*dG);
        }

        // normalization not required since the number of cells that are masked out
        // is defined by the query sketch and thus is constant for all comparison
        // required for one query
        return dist;
    }

    // if you use this distance function, you will need to manually set the correct
    // mask to be used for the query image
    const vector<bool>* mask;
};

/**
 * @brief Distance function from 'Lee & Funkhouser - Sketch-Based Search and Composition of 3D Models'.
 */
template <class T, class R = float>
struct dist_df
{
    // stl
    typedef T first_argument_type;
    typedef T second_argument_type;
    typedef R result_type;

    /**
     * @brief a and b are two descriptors we want to compare. First half of the vectors
     * is expected to contain the distance transform, second half the image/sketch itself.
     *
     * @param a
     * @param b
     * @return Computes <a,dt(b)> + <b,dt(a)>, result range is [0,inf] where 0 means that a and b are equal
     */
    R operator() (const T& a, const T& b) const
    {
        assert(a.size() % 2 == 0);
        assert(a.size() == b.size());

        result_type dist = 0;
        size_t offset = a.size() / 2;
        for (size_t i = 0; i < offset; i++) {
            dist += a[i]*b[i+offset];
            dist += b[i]*a[i+offset];
        }
        return dist;
    }
};

/**
 * @brief 'Base' template factory class for generating a distance function
 * by name, we typically use the partially specialized version for vector<T>, though.
 */
template <class T, class R = float>
struct distance_functions
{
    typedef boost::function<R (const T&, const T&)> distfn_t;

    distfn_t make(const std::string& /*name*/)
    {
        return distfn_t();
    }
};

/**
 * @brief Factory class that generates a distance function from its name.
 *
 * Note that this is a partial specialization of distance_functions
 * for data of type vector<T>. This makes constructing our typical
 * distance function that actually work on a vector<float> just
 * a little bit more convenient than if we would use the more general version.
 */
template <class X, class R>
struct distance_functions<std::vector<X>, R>
{
    typedef std::vector<X> T;
    typedef boost::function<R (const T&, const T&)> distfn_t;

    distfn_t make(const std::string& name)
    {
        if (name == "l1norm") return l1norm<T>();
        if (name == "l2norm") return l2norm<T>();
        if (name == "l2norm_squared") return l2norm_squared<T>();
        if (name == "jsd") return jsd<T>();
        if (name == "chi2") return chi2<T>();
        if (name == "one_minus_dot") return one_minus_dot<T>();
        if (name == "df") return dist_df<T>();
        if (name == "frobenius") return dist_frobenius<T>();

        return distfn_t();
    }
};

/** @} */

} // namespace imdb

#endif // DISTANCE_HPP


// Eigen lib Tutorial
// Source: http://eigen.tuxfamily.org/dox/group__TutorialMatrixArithmetic.html
// Compile: g++ -I /usr/include/eigen3/ eigen_ex4.cpp -o eigen_ex4

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

using namespace Eigen;

int main(void)
{
    // addition and subtraction
    Matrix2d a;
    a << 1, 2,
         3, 4;
    MatrixXd b(2,2);
    b << 2, 3,
         1, 4;
    std::cout << "a + b =\n" << a + b << std::endl;
    std::cout << "a - b =\n" << a - b << std::endl;
    std::cout << "Doing a += b;" << std::endl;
    a += b;
    std::cout << "Now a = \n" << a << std::endl;
    Vector3d v(1,2,3);
    Vector3d w(1,0,0);
    std::cout << "-v + w - v =\n" << -v + w - v << std::endl;

    // Scalar multiplication and division
    a -= b;
    std::cout << "a * 2,5 =\n" << a * 2.5  << std::endl;
    std::cout << "0.1 * v =\n" << 0.1 * v  << std::endl;
    std::cout << "Doing v *= 2;" << std::endl;
    v *= 2;
    std::cout << "Now v =\n" << v << std::endl;

    // Transposition and conjugation
    MatrixXcf c = MatrixXcf::Random(2,2);
    std::cout << "Here is the matrix c\n" << c  << std::endl;
    std::cout << "Here is the matrix c^T\n" << c.transpose() << std::endl;
    std::cout << "Here is the conjugate of c\n" <<
        c.conjugate() << std::endl;
    std::cout << "Here is the matrix c^*\n" << c.adjoint() << std::endl;

    // Matrix-matrix and matrix-vector multiplication
    Matrix2d mat;
    mat << 1, 2,
           3, 4;
    Vector2d u(-1,1), x(2,0);
    std::cout << "Here is mat*mat:\n" << mat*mat  << std::endl;
    std::cout << "Here is mat*u:\n" << mat*u << std::endl;
    std::cout << "Here is u^T*mat:\n" << u.transpose()*mat << std::endl;
    std::cout << "Here is u^T*x\n" << u.transpose()*x << std::endl;
    std::cout << "Here is u*x^T\n" << u*x.transpose() << std::endl;
    std::cout << "let's multiply mat by itself" << std::endl;
    mat = mat*mat;
    std::cout << "Now mat is mat:\n" << mat << std::endl;

    // Dot product and cross product
    Vector3d e(1,2,3);
    Vector3d f(0,1,2);

    std::cout << "Dot product: " << e.dot(f) << std::endl;
    double dp = e.adjoint()*f;  // automatic converiosn of the inner product to a scalar
    std::cout << "Dot product via a matrix product:" << dp << std::endl;
    std::cout << "Cross product:\n" << e.cross(f) << std::endl;

    // Basic arithmetic reduction operations
    Matrix2d mat2;
    mat2 << 1, 2,
            3, 4;
    std::cout << "Here is mat2.sum():       " << mat2.sum() << std::endl;
    std::cout << "Here is mat2.prod():      " << mat2.prod() << std::endl;
    std::cout << "Here is mat2.mean():      " << mat2.mean() << std::endl;
    std::cout << "Here is mat2.minCoeff():  " << mat2.minCoeff() << std::endl;
    std::cout << "Here is mat2.maxCoeff():  " << mat2.maxCoeff() << std::endl;
    std::cout << "Here is mat2.trace():     " << mat2.trace() << std::endl;

    return 0;
}


/**
 * Barrier functions that grow to infinity as x -> 0+. Includes gradient and
 * hessian functions, too. These barrier functions can be used to impose
 * inequlity constraints on a function.
 */

#pragma once

#include <array>

#include <Eigen/Core>

namespace ipc::rigid {

/// @brief Barrier functions to choose from.
enum BarrierType {
    IPC,
    POLY_LOG,
    SPLINE,
};

///////////////////////////////////////////////////////////////////////////
// Choose your barrier!

/**
 * @brief Function that grows to infinity as x approaches 0 from the right.
 *
 * @param x             The x value at which to evaluate.
 * @param s             Activation value of the barrier.
 * @param barrier_type  Barrier function type to use.
 * @return The value of the barrier function at x.
 */
template <typename T> T barrier(const T& x, double s, BarrierType barrier_type);

double barrier_gradient(double x, double s, BarrierType barrier_type);

double barrier_hessian(double x, double s, BarrierType barrier_type);

///////////////////////////////////////////////////////////////////////////
// Poly-Log Barrier

/// y := x/eps; b(x, ϵ) = -log(y)*(2*y^3-3*y^2+1)
template <typename T> T poly_log_barrier(const T& x, double s);

double poly_log_barrier_gradient(double x, double s);

double poly_log_barrier_hessian(double x, double s);

///////////////////////////////////////////////////////////////////////////
// Spline Barrier

/**
 * @brief Function that grows to infinity as x approaches 0 from the right.
 *
 * \begin{align}
 * g(x, s) = \frac{1}{s^3}x^3 - \frac{3}{s^2}x^2 + \frac{3}{s}x
 * \end{align}
 * \begin{align}
 * \phi_{\text{spline}}(x, s) = \frac{1}{g(x, s)} - 1
 * \end{align}
 *
 * where \f$s\f$ is a constant. See here
 * for the original definition and a discussion.
 *
 * @param x The x value at which to evaluate \f$\phi_{\text{spline}}(x)\f$.
 * @param s Denominator inside x which steepens the function.
 * @return The value of the barrier function at x.
 */
template <typename T> T spline_barrier(const T& x, double s);

/**
 * @brief Derivative of the spline_barrier function with respect to x.
 *
 * \begin{align}
 * g'(x, s) = \frac{3}{s^3}x^2 - \frac{6}{s^2}x + \frac{3}{s}
 * \end{align}
 * \begin{align}
 * \phi'_{\text{spline}}(x, s) = \frac{-1}{[g(x)]^2}g'(x)
 * \end{align}
 *
 * where \f$s\f$ is a constant. See here
 * for the original definition and a discussion.
 *
 * @param x The x value at which to evaluate \f$\phi_{\text{spline}}(x)\f$.
 * @param s Denominator inside x which steepens the function.
 * @return The value of the derivative of the barrier function at x.
 */
double spline_barrier_gradient(double x, double s);

/**
 * @brief Second derivative of the spline_barrier function with respect to
 * x.
 *
 * \begin{align}
 * g''(x, s) = \frac{6}{s^3}x - \frac{6}{s^2}
 * \end{align}
 * \begin{align}
 * \phi''_{\text{spline}}(x, s) = \frac{2}{[g(x)]^3}[g'(x)]^2 +
 * \frac{-1}{[g(x)]^2}g''(x) \end{align}
 *
 * where \f$s\f$ is a constant. See here
 * for the original definition and a discussion.
 *
 * @param x The x value at which to evaluate \f$\phi_{\text{spline}}(x)\f$.
 * @param s Denominator inside x which steepens the function.
 * @return The value of the second derivative of the barrier function at x.
 */
double spline_barrier_hessian(double x, double s);

} // namespace ipc::rigid

#include "barrier.tpp"


#include <iostream>
using namespace std;

#include <ctime>

// use eigin in c++
// Eigen Core part
#include <Eigen/Core>
// 稠密矩阵的代数运算
#include <Eigen/Dense>

#define MATRIX_SIZE    50

//---------------------介绍eigen库的基本用法-----------------------
//内置类型：Eigen::Matrix<>，适用于所有向量和矩阵
//这是一个模版类，三个参数为：数据类型，行，列

int main(int argc, char const *argv[])
{
   //---------------------创建不同类型的向量或者矩阵------------------

   //声明一个2*3的float矩阵
   Eigen::Matrix<float, 2, 3> matrix_23;

   //Eigen 通过 typedef 提供了许多内置类型，不过底层仍然是 Eigen::Matrix
   //例如， Vector3d三位向量 就是 Eigen::Matrix<double,3,1>
   Eigen::Vector3d            v_3d;   //这两个等价, 但是这个数据类型是double
   Eigen::Matrix<float, 3, 1> v_3d_1; //这两个等价，这个数据类型是float

   //Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
   Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零

   //动态大小的矩阵，矩阵大小不确定
   //第一行用最基本的类型，第二行是eigen用typedef提供的
   Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
   Eigen::MatrixXd matrix_dynamic_1;

   //---------------------对Eigen矩阵的操作------------------

   //数据初始化
   matrix_23 << 1, 2, 3, 4, 5, 6;

   //输出
   cout << "直接用cout输出\n" << matrix_23 << endl;
   //遍历矩阵元素
   //
   // for (size_t i = 0; i < 2; i++)
   // {
   //    for (size_t j = 0; j < 3; j++)
   //    {
   //       cout << matrix_23(i, j) << "\t";
   //    }
   //    cout << endl;
   // }

   //矩阵间乘法，或者矩阵和向量
   //要注意的：
   //1. 两个对象内元素的数据类型要一样
   //   Wrong example:
   //   Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
   //2. 遵循矩阵乘法规则, 例如矩阵维度
   //   Wrong example:
   //   Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;
   v_3d << 3, 2, 1;   //v_3d数据类型是double
   v_3d_1 << 4, 5, 6; //v_3d_1数据类型是float

   //四则运算 用 + - * /
   //这里展示乘法
   //matrix_23数据类型是float，如果要和v_3d相乘，需要显示转换
   //Eigen为了性能，不会提供隐士转换，转换成员函数是 .cast<double>()
   Eigen::Matrix<double, 2, 1> result  = matrix_23.cast<double>() * v_3d;
   Eigen::Matrix<float, 2, 1>  result2 = matrix_23 * v_3d_1;

   //矩阵运算
   matrix_33 = Eigen::Matrix3d::Random();                             //随机3x3的矩阵，但是感觉怎么都不变...
   cout << "matrix \n" << matrix_33 << endl << endl;
   cout << "matrix transpose\n" << matrix_33.transpose() << endl;     // 转置
   cout << "matrix sum\n" << matrix_33.sum() << endl;                 // 各元素和
   cout << "matrix trace\n" << matrix_33.trace() << endl;             // 迹
   cout << "matrix inverse\n" << matrix_33.inverse() << endl;         // 逆
   cout << "matrix determinant\n" << matrix_33.determinant() << endl; // 行列式

   //----------------------eigenvalues & eigenvectors-----------------
   // Computes eigenvalues and eigenvectors of selfadjoint matrices.
   // define a selfadjoint Matrix
   Eigen::Matrix3d S = matrix_33.transpose() * matrix_33;
   // use solver on 3x3 matrix S
   Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es(S);
   cout << "Eigen values = \n" << es.eigenvalues() << endl;
   cout << "Eigen vectors = \n" << es.eigenvectors() << endl;

   //-------------------------解方程-----------------------------
   //Solve matrix_NN * x = v_Nd
   //直接求逆求解 与 QR decomposition求解
   //define matrix_NN and v_Nd
   Eigen::Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN;
   matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE); //另一种使用random的用法
   Eigen::Matrix<double, MATRIX_SIZE, 1> v_Nd;
   v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE, 1);
   Eigen::Matrix<double, MATRIX_SIZE, 1> x;
   //set timer
   clock_t time_start = clock();
   //1. solve by inverse, time used is4.151ms
   x = matrix_NN.inverse() * v_Nd;
   cout << "time used is" << 1000 * (clock() - time_start) / (double)CLOCKS_PER_SEC << "ms" << endl;
   //2. solve by QR decomposition, time used is0.082ms
   time_start = clock();
   x          = matrix_NN.colPivHouseholderQr().solve(v_Nd);
   cout << "time used is" << 1000 * (clock() - time_start) / (double)CLOCKS_PER_SEC << "ms" << endl;

   return 0;
}


/**
 * @file exponentialintegrator_main.cc
 * @brief NPDE homework ExponentialIntegrator code
 * @author Unknown, Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>
/**
 * @file exponentialintegrator_main.cc
 * @brief NPDE homework ExponentialIntegrator code
 * @author Unknown, Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <vector>

#include "exponentialintegrator.h"

int main() {
  /* SAM_LISTING_BEGIN_0 */
  // Final time
  double T = 1.0;
  // Initial value
  Eigen::VectorXd y0(1);
  y0 << 0.1;
  // Function and Jacobian and exact solution
  auto f = [](const Eigen::VectorXd &y) { return y(0) * (1.0 - y(0)); };
  auto df = [](const Eigen::VectorXd &y) {
    Eigen::VectorXd dfy(1);
    dfy << 1.0 - 2.0 * y(0);
    return dfy;
  };
  double exactyT = y0(0) / (y0(0) + (1.0 - y0(0)) * std::exp(-T));

  // Container for errors
  std::vector<double> error(15);

  // Test many step sizes
  for (int j = 0; j < 15; ++j) {
    int N = std::pow(2, j + 1);
    Eigen::VectorXd y = y0;
    double h = T / N;
#if SOLUTION
    for (int k = 0; k < N; ++k) {
      y = ExponentialIntegrator::exponentialEulerStep(y, f, df, h);
    }
#else
    //====================
    // Your code goes here
    // TODO: Perform N timesteps with inital data y0 and store the result in y.
    //====================
#endif

    error[j] = std::abs(y(0) - exactyT);
    std::cout << std::left << std::setfill(' ') << std::setw(3)
              << "N = " << std::setw(7) << N << std::setw(8)
              << "Error = " << std::setw(13) << error[j];
    if (j > 0) {
      std::cout << std::left << std::setfill(' ') << std::setw(10)
                << "Approximated order = " << std::log2(error[j - 1] / error[j])
                << std::endl;
    } else
      std::cout << std::endl;
  }
  /* SAM_LISTING_END_0 */
  return 0;
}