// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
 *
 *  This is an example illustrating the use the general purpose non-linear
 *  optimization routines from the dlib C++ Library.
 *
 *  The library provides implementations of many popular algorithms such as L-BFGS
 *  and BOBYQA.  These algorithms allow you to find the minimum or maximum of a
 *  function of many input variables.  This example walks though a few of the ways
 *  you might put these routines to use.
 *
 */

#include <iostream>
#include <dlib/optimization.h>
#include <dlib/global_optimization.h>

#ifdef __cplusplus
extern "C" {
#endif
#include "ViennaRNA/utils/basic.h"
#ifdef __cplusplus
}
#endif

#include "wrap_dlib.h"

using namespace std;
using namespace dlib;

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

// In dlib, most of the general purpose solvers optimize functions that take a
// column vector as input and return a double.  So here we make a typedef for a
// variable length column vector of doubles.  This is the type we will use to
// represent the input to our objective functions which we will be minimizing.
typedef matrix<double, 0, 1> column_vector;

/*
 *  function to minimize to obtain equlibrium concentrations
 *  of multistrand systems. We use the transformation
 *
 *  L_a = lambda_a + ln Z_a
 *
 *  such that h(L) reads
 *
 *  h(L) = -\sum_a (c_a L_a - exp(L_a)) + sum_k K_k exp(sum_b L_b A_{b,k}
 *
 *  with total concentration c_a of strand a, equilibrium constant
 *  K_k of strand k, and membership matrix A[b][k] denoting the number
 *  of strands b in complex k
 *
 *  Note, here we minimize h(L) due to implementation issues whereas
 *  in our publication we've written h'(L) = -h(L) to effectively
 *  maximize the function instead.
 */
PRIVATE double
h(const column_vector&  L,
  const double          *eq_constants,
  const double          *concentration_strands_tot,
  const unsigned int    **A,
  size_t                strands,
  size_t                complexes)
{
  double h, hh, *K, maxK;

  K = (double *)vrna_alloc(sizeof(double) * complexes);
  h = 0.;
  maxK = (double)(-INF);

  for (size_t a = 0; a < strands; a++) {
//    printf("L[%u] = %g\n", a, L(a));
    maxK = (maxK < L(a)) ? L(a) : maxK;
  }

  for (size_t k = 0; k < complexes; k++) {
    K[k] = log(eq_constants[k]);

    for (size_t a = 0; a < strands; a++) {
      K[k] += L(a) *
              (double)A[a][k];
    }

    maxK = (maxK < K[k]) ? K[k] : maxK;
  }

  for (size_t a = 0; a < strands; a++)
    h -= concentration_strands_tot[a] *
         L(a);

//  printf("h = %g\n", h);
  hh = 0;

  for (size_t a = 0; a < strands; a++)
    hh += exp(L(a) - maxK);

  for (size_t k = 0; k < complexes; k++)
    hh += exp(K[k] - maxK);

  h += exp(maxK + log(hh));
//  printf("h = %g\n", h);

  free(K);

  return h;
}

/*
 *  Get gradient of h(L)
 */
PRIVATE const column_vector
h_derivative(const column_vector& L,
             const double         *eq_constants,
             const double         *concentration_strands_tot,
             const unsigned int   **A,
             size_t               strands,
             size_t               complexes)
{
  double        *K, *maxK;
  column_vector g(strands);

  K     = (double *)vrna_alloc(sizeof(double) * complexes);
  maxK  = (double *)vrna_alloc(sizeof(double) * strands);

  for (size_t a = 0; a < strands; a++)
    maxK[a] = L(a);

  for (size_t k = 0; k < complexes; k++) {
    K[k] = log(eq_constants[k]);
    for (size_t a = 0; a < strands; a++)
      K[k] += L(a) *
              (double)A[a][k];

    for (size_t a = 0; a < strands; a++)
      if (A[a][k] > 0)
        maxK[a] = (maxK[a] < K[k] + log((double)A[a][k])) ?
                  K[k] + log((double)A[a][k]) :
                  maxK[a];
  }

  for (size_t a = 0; a < strands; a++) {
    g(a) = -concentration_strands_tot[a];

    double hh = exp(L(a) - maxK[a]);
    for (size_t k = 0; k < complexes; k++)
      if (A[a][k] > 0)
        hh += exp(log((double)A[a][k]) +
                  K[k] -
                  maxK[a]);

    g(a) += exp(maxK[a] + log(hh));
//    printf("g(%u) = %g\n", a, g(a));
  }

  free(K);
  free(maxK);

  return g;
}

/*
 *  Get Hessian of h(L)
 */
PRIVATE matrix<double>
h_hessian(const column_vector&  L,
          const double          *eq_constants,
          const unsigned int    **A,
          size_t                strands,
          size_t                complexes)
{
  double                  *K, **xs;

  PRIVATE matrix<double>  H(strands, strands);

  K = (double *)vrna_alloc(sizeof(double) * complexes);
  xs = (double **)vrna_alloc(sizeof(double *) * strands);

  for (size_t a = 0; a < strands; a++) {
    xs[a] = (double *)vrna_alloc(sizeof(double) * strands);
    for (size_t b = 0; b < strands; b++)
      xs[a][b] = (a == b) ? L(a) : (double)(-INF);
  }

  for (size_t k = 0; k < complexes; k++) {
    K[k] = log(eq_constants[k]);
    for (size_t a = 0; a < strands; a++)
      K[k] += L(a) *
              (double)A[a][k];

    for (size_t a = 0; a < strands; a++)
      for (size_t b = 0; b < strands; b++) {
        if ((A[a][k] > 0) && (A[b][k] > 0))
          xs[a][b] = (xs[a][b] < K[k] + log((double)A[a][k]) + log((double)A[b][k])) ?
                      K[k] + log((double)A[a][k]) + log((double)A[b][k]) :
                      xs[a][b];
      }
  }

  for (size_t a = 0; a < strands; a++) {
    for (size_t b = 0; b < strands; b++) {
      double hh = (a == b) ? exp(L(a) - xs[a][b]) : 0.;

      for (size_t k = 0; k < complexes; k++)
        if ((A[a][k] > 0) && (A[b][k] > 0))
          hh += exp(log((double)A[a][k]) +
                    log((double)A[b][k]) +
                    K[k] -
                    xs[a][b]);

      H(a,b) = exp(xs[a][b] + log(hh));
//      H(b,a) = H(a,b);
//      printf("H(%u,%u) = %g\n", a, b, H(a,b));
    }
  }

  free(K);
  for (size_t a = 0; a < strands; a++)
    free(xs[a]);
  free(xs);

  return H;
}

class h_model
{
/*!
 *  This object is a "function model" which can be used with the
 *  find_min_trust_region() routine.
 * !*/

public:
typedef ::column_vector column_vector;
typedef matrix<double> general_matrix;

const double *eq_constants;
const double *concentration_strands_tot;
const unsigned int **A;
size_t strands;
size_t complexes;

double
operator()(const column_vector& x) const
{
  return h(x, eq_constants, concentration_strands_tot, A, strands, complexes);
}

void
init(const double       *eq_constants,
     const double       *concentration_strands_tot,
     const unsigned int **A,
     size_t             strands,
     size_t             complexes)
{
  this->eq_constants              = eq_constants;
  this->concentration_strands_tot = concentration_strands_tot;
  this->A                         = A;
  this->strands                   = strands;
  this->complexes                 = complexes;
}

void
get_derivative_and_hessian(const column_vector& x,
                           column_vector&       der,
                           general_matrix&      hess) const
{
  der   = h_derivative(x, eq_constants, concentration_strands_tot, A, strands, complexes);
  hess  = h_hessian(x, eq_constants, A, strands, complexes);
}
};

/*
 *  Get concentrations of single strands from
 *  a given vector L (that minimizes h(L))
 */
PRIVATE double *
conc_single_strands(const column_vector&  L,
                    size_t                strands)
{
  double *c = (double *)vrna_alloc(sizeof(double) * strands);

  for (size_t a = 0; a < strands; a++)
    c[a] = exp(L(a));

  return c;
}

/*
 *  Get concentrations of complexes from
 *  a given vector L (that minimizes h(L))
 */
PRIVATE double *
conc_complexes(const column_vector& L,
               const double         *eq_const,
               const unsigned int   **A,
               size_t               strands,
               size_t               complexes)
{
  double *c;

  c = (double *)vrna_alloc(sizeof(double) * complexes);

  for (size_t k = 0; k < complexes; k++) {
    c[k] = log(eq_const[k]);

    for (size_t a = 0; a < strands; a++)
      c[k] += (double)A[a][k] * L(a);

    c[k] = exp(c[k]);
  }

  return c;
}

double *
vrna_equilibrium_conc(const double        *eq_constants,
                      double              *concentration_strands,
                      const unsigned int  **A,
                      size_t              num_strands,
                      size_t              num_complexes)
{
  double        *r = NULL;

  column_vector starting_point;

  h_model       h;

  h.init(eq_constants,
         concentration_strands,
         A,
         num_strands,
         num_complexes);

  starting_point.set_size(num_strands);

  for (size_t a = 0; a < num_strands; a++)
    starting_point(a) = 0.;

  find_min_trust_region(objective_delta_stop_strategy(1e-18),
                        h,
                        starting_point,
                        1   // initial trust region radius
                        );

  double *conc_monomers = conc_single_strands(starting_point, num_strands);

  for (size_t a = 0; a < num_strands; a++)
    concentration_strands[a] = conc_monomers[a];

  r = conc_complexes(starting_point, eq_constants, A, num_strands, num_complexes);

  free(conc_monomers);

  return r;
}


/*
 *  Distributed under the MIT License (See accompanying file /LICENSE )
 */
#include <doctest/doctest.h>  // for ResultBuilder

#include <array>                             // for operator==
#include <boost/multiprecision/cpp_int.hpp>  // for cpp_int
#include <ostream>                           // for operator<<
#include <tuple>                             // for tuple
#include <type_traits>                       // for move

#include "projgeom/ck_plane.hpp"            // for ellck, hyck
#include "projgeom/common_concepts.h"       // for Value_type
#include "projgeom/fractions.hpp"           // for operator*
#include "projgeom/persp_plane.hpp"         // for persp_eucl...
#include "projgeom/pg_common.hpp"           // for cross
#include "projgeom/pg_line.hpp"             // for pg_line
#include "projgeom/pg_object.hpp"           // for operator*
#include "projgeom/pg_point.hpp"            // for pg_point
#include "projgeom/proj_plane.hpp"          // for coincident
#include "projgeom/proj_plane_measure.hpp"  // for x_ratio
// #include <iostream>

using namespace fun;

static const auto Zero = doctest::Approx(0).epsilon(0.01);

/**
 * @brief
 *
 * @param[in] a
 * @return true
 * @return false
 */
template <typename T> inline auto ApproxZero(const T& a) -> bool {
    return a[0] == Zero && a[1] == Zero && a[2] == Zero;
}

template <typename PG> void chk_ck(const PG& myck) {
    using P = typename PG::point_t;
    using K = Value_type<P>;

    auto a1 = P{1, -2, 3};
    auto a2 = P{4, 0, 6};
    auto a3 = P{-7, 1, 2};
    const auto triangle = std::tuple{std::move(a1), std::move(a2), std::move(a3)};
    const auto trilateral = tri_dual(triangle);
    const auto& [l1, l2, l3] = trilateral;
    const auto [t1, t2, t3] = myck.tri_altitude(triangle);

    auto o = myck.orthocenter(triangle);
    const auto tau = myck.reflect(l1);
    const auto Q = std::tuple{myck.tri_quadrance(triangle)};
    const auto S = std::tuple{myck.tri_spread(trilateral)};

    const auto a4 = P{3, 0, 2};

    if constexpr (Integral<K>) {
        CHECK(incident(l1, a2));
        CHECK(myck.is_perpendicular(t1, l1));
        CHECK(coincident(t1 * t2, t3));
        CHECK(o == t2 * t3);
        CHECK(a1 == myck.orthocenter(std::tuple{std::move(o), std::move(a2), std::move(a3)}));
        CHECK(tau(tau(a4)) == a4);
        // CHECK(myck.spread(l2, l2) == K(0));
        // CHECK(myck.spread(l3, l3) == K(0));
        // CHECK(myck.quadrance(a1, a1) == K(0));
        CHECK(check_sine_law(Q, S));
        CHECK(check_sine_law(S, Q));
    } else {
        CHECK(l1.dot(a2) == Zero);
        CHECK(l1.dot(myck.perp(t1)) == Zero);
        CHECK(t1.dot(t2 * t3) == Zero);
        CHECK(ApproxZero(cross(o, t2 * t3)));
        const auto o2 = myck.orthocenter(std::tuple{std::move(o), std::move(a2), std::move(a3)});
        CHECK(ApproxZero(cross(a1, o2)));
        CHECK(ApproxZero(cross(tau(tau(a4)), a4)));
        CHECK(myck.measure(l2, l2) == Zero);
        CHECK(myck.measure(l3, l3) == Zero);
        CHECK(myck.measure(a1, a1) == Zero);
        const auto& [q1, q2, q3] = Q;
        const auto& [s1, s2, s3] = S;

        const auto r1 = q1 * s2 - q2 * s1;
        const auto r2 = q2 * s3 - q3 * s2;
        CHECK(r1 == Zero);
        CHECK(r2 == Zero);
    }
}

template <typename P, typename L = typename P::dual>
requires Projective_plane_prim<P, L>  // c++20 concept
struct myck : ck<P, L, myck> {
    [[nodiscard]] constexpr auto perp(const P& v) const -> L {
        return L(-2 * v[0], v[1], -2 * v[2]);
    }

    [[nodiscard]] constexpr auto perp(const L& v) const -> P { return P(-v[0], 2 * v[1], -v[2]); }

    template <Projective_plane2 _P>
    [[nodiscard]] constexpr auto measure(const _P& a1, const _P& a2) const {
        auto x = x_ratio(a1, a2, this->perp(a2), this->perp(a1));
        // using Q_t = decltype(x);
        return 1 - x;
    }
};

TEST_CASE("CK plane chk_ck (int)") {
    // using boost::multiprecision::cpp_int;
    // namespace mp = boost::multiprecision;
    using boost::multiprecision::cpp_int;

    chk_ck(myck<pg_point<cpp_int>>());
    chk_ck(myck<pg_line<cpp_int>>());
    chk_ck(ellck<pg_point<cpp_int>>());
    chk_ck(ellck<pg_line<cpp_int>>());
    chk_ck(hyck<pg_point<cpp_int>>());
    chk_ck(hyck<pg_line<cpp_int>>());

    auto Ire = pg_point<cpp_int>{0, 1, 1};
    auto Iim = pg_point<cpp_int>{1, 0, 0};
    auto l_inf = pg_line<cpp_int>{0, -1, 1};

    auto P = persp_euclid_plane{std::move(Ire), std::move(Iim), std::move(l_inf)};
    chk_ck(P);
}

TEST_CASE("CK plane chk_ck (float)") {
    chk_ck(myck<pg_point<double>>());
    chk_ck(myck<pg_line<double>>());
    chk_ck(ellck<pg_point<float>>());
    chk_ck(ellck<pg_line<float>>());
    chk_ck(hyck<pg_point<double>>());
    chk_ck(hyck<pg_line<double>>());

    auto Ire = pg_point{0., 1., 1.};
    auto Iim = pg_point{1., 0., 0.};
    auto l_inf = pg_line{0., -1., 1.};

    auto P = persp_euclid_plane{std::move(Ire), std::move(Iim), std::move(l_inf)};
    chk_ck(P);
}


#include <iostream>
#include <complex>
#include <cstdlib>
#include <Eigen/Eigen>

int main(int argc, char const *argv[])
{
  Eigen::Matrix<float, 2, 2> a = Eigen::Matrix<float, 2, 2>::Zero();
  a(1,1) = 1;
  Eigen::Matrix<std::complex<double>, 2, 2> b, c;
  b << std::complex<double>(), std::complex<double>(1,1), std::complex<double>(2,2), std::complex<double>(3,3);
  c = a.cast<std::complex<double>>()*b;
  std::cout << "a = " << std::endl << a << std::endl;
  std::cout << "b = " << std::endl << b << std::endl;
  std::cout << "c = " << std::endl << c << std::endl;
  return EXIT_SUCCESS;
}


#define _USE_MATH_DEFINES

#include <igvc_msgs/velocity_pair.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include "SmoothControl.h"

ros::Publisher cmd_pub;
ros::Publisher target_pub;
ros::Publisher trajectory_pub;

nav_msgs::PathConstPtr path;

double lookahead_dist, maximum_vel;

SmoothControl controller;

void path_callback(const nav_msgs::PathConstPtr& msg)
{
  ROS_INFO("Follower got path");
  path = msg;
}

double get_distance(double x1, double y1, double x2, double y2)
{
  return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
}

void position_callback(const nav_msgs::OdometryConstPtr& msg)
{
  if (path.get() == nullptr)
  {
    return;
  }
  if (path->poses.empty() || path->poses.size() < 2)
  {
    ROS_INFO("Path empty.");
    igvc_msgs::velocity_pair vel;
    vel.left_velocity = 0.;
    vel.right_velocity = 0.;
    cmd_pub.publish(vel);
    path.reset();
    return;
  }

  float cur_x = msg->pose.pose.position.x;
  float cur_y = msg->pose.pose.position.y;
  tf::Quaternion q;
  tf::quaternionMsgToTF(msg->pose.pose.orientation, q);
  float cur_theta = tf::getYaw(q);

  float tar_x, tar_y, tar_theta;
  geometry_msgs::Point end = path->poses[path->poses.size() - 1].pose.position;
  double path_index = 0;
  double closest = std::abs(get_distance(cur_x, cur_y, path->poses[0].pose.position.x, path->poses[0].pose.position.y));
  double temp = std::abs(
      get_distance(cur_x, cur_y, path->poses[path_index].pose.position.x, path->poses[path_index].pose.position.y));
  while (path_index < path->poses.size() && temp <= closest)
  {
    if (temp < closest)
    {
      closest = temp;
    }
    path_index++;
    temp = std::abs(
        get_distance(cur_x, cur_y, path->poses[path_index].pose.position.x, path->poses[path_index].pose.position.y));
  }

  if (get_distance(cur_x, cur_y, end.x, end.y) > lookahead_dist)
  {
    double distance = 0;
    bool cont = true;
    while (cont && path_index < path->poses.size() - 1)
    {
      geometry_msgs::Point point1, point2;
      point1 = path->poses[path_index].pose.position;
      point2 = path->poses[path_index + 1].pose.position;
      double increment = get_distance(point1.x, point1.y, point2.x, point2.y);
      if (distance + increment > lookahead_dist)
      {
        cont = false;
        Eigen::Vector3d first(point1.x, point1.y, 0);
        Eigen::Vector3d second(point2.x, point2.y, 0);
        Eigen::Vector3d slope = second - first;
        // ROS_INFO_STREAM("first = " << first[0] << ", " << first[1]);
        // ROS_INFO_STREAM("slope = " << slope[0] << ", " << slope[1]);
        // ROS_INFO_STREAM("look = " << lookahead_dist << " dista = " << distance);
        // ROS_INFO_STREAM("increment = " << increment << " look - dist = " << (distance - lookahead_dist) + increment);
        slope /= increment;
        slope *= (distance - lookahead_dist) + increment;
        // ROS_INFO_STREAM("slope2 = " << slope[0] << ", " << slope[1]);
        slope += first;
        tar_x = slope[0];
        tar_y = slope[1];
      }
      else
      {
        path_index++;
        distance += increment;
      }
    }
  }
  else
  {
    tar_x = end.x;
    tar_y = end.y;
  }

  double yDiff = tar_y - cur_y;
  double xDiff = tar_x - cur_x;

  if (xDiff == 0)
  {
    tar_theta = yDiff > 0 ? M_PI : -M_PI;
  }
  else
  {
    tar_theta = atan2((yDiff), (xDiff));
  }

  ros::Time time = ros::Time::now();

  geometry_msgs::PointStamped target_point;
  target_point.header.frame_id = "/odom";
  target_point.header.stamp = time;
  target_point.point.x = tar_x;
  target_point.point.y = tar_y;
  target_pub.publish(target_point);

  igvc_msgs::velocity_pair vel;
  vel.header.stamp = time;

  nav_msgs::Path trajectory_msg;
  trajectory_msg.header.stamp = ros::Time::now();
  trajectory_msg.header.frame_id = "/odom";

  Eigen::Vector3d cur_pos(cur_x, cur_y, cur_theta);
  Eigen::Vector3d target(tar_x, tar_y, tar_theta);
  controller.getTrajectory(vel, trajectory_msg, cur_pos, target);

  ROS_INFO_STREAM("distance = " << get_distance(tar_x, tar_y, cur_x, cur_y));

  if (vel.right_velocity > maximum_vel || vel.left_velocity > maximum_vel)
  {
    ROS_ERROR_STREAM("Large velocity output stopping " << vel.right_velocity << ", " << vel.left_velocity);
    vel.right_velocity = 0;
    vel.left_velocity = 0;
  }
  // ROS_INFO_STREAM("target " << tar_x << " " << tar_y << "\n");

  cmd_pub.publish(vel);
  trajectory_pub.publish(trajectory_msg);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "path_follower");

  ros::NodeHandle nh;
  ros::NodeHandle pNh("~");

  pNh.param(std::string("target_v"), controller.v, 1.0);
  pNh.param(std::string("axle_length"), controller.axle_length, 0.52);
  pNh.param(std::string("k1"), controller.k1, 1.0);
  pNh.param(std::string("k2"), controller.k2, 3.0);
  pNh.param(std::string("roll_out_time"), controller.rollOutTime, 2.0);
  pNh.param(std::string("lookahead_dist"), lookahead_dist, 2.0);
  pNh.param(std::string("maximum_vel"), maximum_vel, 1.6);

  ros::Subscriber path_sub = nh.subscribe("/path", 1, path_callback);

  ros::Subscriber pose_sub = nh.subscribe("/odometry/filtered", 1, position_callback);

  cmd_pub = nh.advertise<igvc_msgs::velocity_pair>("/motors", 1);

  target_pub = nh.advertise<geometry_msgs::PointStamped>("/target_point", 1);

  trajectory_pub = nh.advertise<nav_msgs::Path>("/trajectory", 1);

  ros::spin();

  return 0;
}


#include <boost/test/unit_test.hpp>
#include "line.h"
#include "capsule.h"
#include "sphere.h"

BOOST_AUTO_TEST_SUITE(test_sphere)

BOOST_AUTO_TEST_CASE (test_sphere_line_intersection)
{
	math::sphere<2> s;
	s.centre.set(1, 0);
	s.radius = 1;

	math::line<2> l;
	l.A.set(0, 1);
	l.B.set(0, -1);

	math::vec<2> p1, p2;
	int npoints = s.query_intersection(l, p1, p2);

	BOOST_REQUIRE (npoints == 1);
	BOOST_REQUIRE ((p1 - math::vec<2>(0, 0)).length() < math::EPSILON);
}

BOOST_AUTO_TEST_CASE (test_sphere_capsule_intersection_1)
{
	math::sphere<3> s(math::vec<3>(0, 0, 0), 0.5f);
	math::capsule<3> c(math::vec<3>(0.7f, -1, 0), math::vec<3>(0.7f, 1, 0), 0.5f);

	BOOST_REQUIRE (s.test_intersection(c) == true);
}

BOOST_AUTO_TEST_CASE (test_sphere_capsule_intersection_2)
{
	math::sphere<3> s(math::vec<3>(0, 0, 0), 0.5f);
	math::capsule<3> c(math::vec<3>(1.7f, -1, 0), math::vec<3>(1.7f, 1, 0), 0.5f);

	BOOST_REQUIRE (s.test_intersection(c) == false);
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (c) 2020 Chris Richardson & Matthew Scroggs
// FEniCS Project
// SPDX-License-Identifier:    MIT

#include "brezzi-douglas-marini.h"
#include "core/dof-permutations.h"
#include "core/element-families.h"
#include "core/mappings.h"
#include "core/moments.h"
#include "core/polyset.h"
#include "core/quadrature.h"
#include "lagrange.h"
#include "nedelec.h"
#include <Eigen/Dense>
#include <numeric>
#include <vector>

using namespace basix;

//----------------------------------------------------------------------------
FiniteElement basix::create_bdm(cell::type celltype, int degree)
{
  if (celltype != cell::type::triangle and celltype != cell::type::tetrahedron)
    throw std::runtime_error("Unsupported cell type");

  const int tdim = cell::topological_dimension(celltype);

  const cell::type facettype
      = (tdim == 2) ? cell::type::interval : cell::type::triangle;

  // The number of order (degree) scalar polynomials
  const int npoly = polyset::dim(celltype, degree);
  const int ndofs = npoly * tdim;

  // Create coefficients for order (degree-1) vector polynomials
  Eigen::MatrixXd wcoeffs = Eigen::MatrixXd::Identity(ndofs, ndofs);

  // Dual space
  Eigen::MatrixXd dual = Eigen::MatrixXd::Zero(ndofs, ndofs);

  // quadrature degree
  int quad_deg = 5 * degree;

  // Add rows to dualmat for integral moments on facets
  const int facet_count = tdim + 1;
  const int facet_dofs = polyset::dim(facettype, degree);

  dual.block(0, 0, facet_count * facet_dofs, ndofs)
      = moments::make_normal_integral_moments(
          create_dlagrange(facettype, degree), celltype, tdim, degree,
          quad_deg);

  const int internal_dofs = ndofs - facet_count * facet_dofs;

  // Add rows to dualmat for integral moments on interior
  if (degree > 1)
  {
    // Interior integral moment
    dual.block(facet_count * facet_dofs, 0, internal_dofs, ndofs)
        = moments::make_dot_integral_moments(
            create_nedelec(celltype, degree - 1), celltype, tdim, degree,
            quad_deg);
  }

  const std::vector<std::vector<std::vector<int>>> topology
      = cell::topology(celltype);

  int perm_count = 0;
  for (int i = 1; i < tdim; ++i)
    perm_count += topology[i].size() * i;

  std::vector<Eigen::MatrixXd> base_permutations(
      perm_count, Eigen::MatrixXd::Identity(ndofs, ndofs));
  if (tdim == 2)
  {
    Eigen::ArrayXi edge_ref = dofperms::interval_reflection(degree + 1);
    Eigen::ArrayXXd edge_dir
        = dofperms::interval_reflection_tangent_directions(degree + 1);
    for (int edge = 0; edge < facet_count; ++edge)
    {
      const int start = edge_ref.size() * edge;
      for (int i = 0; i < edge_ref.size(); ++i)
      {
        base_permutations[edge](start + i, start + i) = 0;
        base_permutations[edge](start + i, start + edge_ref[i]) = 1;
      }
      Eigen::MatrixXd directions = Eigen::MatrixXd::Identity(ndofs, ndofs);
      directions.block(edge_dir.rows() * edge, edge_dir.cols() * edge,
                       edge_dir.rows(), edge_dir.cols())
          = edge_dir;
      base_permutations[edge] *= directions;
    }
  }
  else if (tdim == 3)
  {
    Eigen::ArrayXi face_ref = dofperms::triangle_reflection(degree + 1);
    Eigen::ArrayXi face_rot = dofperms::triangle_rotation(degree + 1);

    for (int face = 0; face < facet_count; ++face)
    {
      const int start = face_ref.size() * face;
      for (int i = 0; i < face_rot.size(); ++i)
      {
        base_permutations[6 + 2 * face](start + i, start + i) = 0;
        base_permutations[6 + 2 * face](start + i, start + face_rot[i]) = 1;
        base_permutations[6 + 2 * face + 1](start + i, start + i) = 0;
        base_permutations[6 + 2 * face + 1](start + i, start + face_ref[i])
            = -1;
      }
    }
  }

  // BDM has facet_dofs dofs on each facet, and ndofs-facet_count*facet_dofs in
  // the interior
  std::vector<std::vector<int>> entity_dofs(topology.size());
  for (int i = 0; i < tdim - 1; ++i)
    entity_dofs[i].resize(topology[i].size(), 0);
  entity_dofs[tdim - 1].resize(topology[tdim - 1].size(), facet_dofs);
  entity_dofs[tdim] = {internal_dofs};

  Eigen::MatrixXd coeffs = compute_expansion_coefficients(wcoeffs, dual);

  return FiniteElement(element::family::BDM, celltype, degree, {tdim}, coeffs,
                       entity_dofs, base_permutations, {}, {},
                       mapping::type::contravariantPiola);
}
//-----------------------------------------------------------------------------