/**
 * @file    testImplicitSchurFactor.cpp
 * @brief   unit test implicit jacobian factors
 * @author  Frank Dellaert
 * @date    Oct 20, 2013
 */

//#include <gtsam_unstable/slam/ImplicitSchurFactor.h>
#include <gtsam/slam/ImplicitSchurFactor.h>
//#include <gtsam_unstable/slam/JacobianFactorQ.h>
#include <gtsam/slam/JacobianFactorQ.h>
//#include "gtsam_unstable/slam/JacobianFactorQR.h"
#include "gtsam/slam/JacobianFactorQR.h"

#include <gtsam/base/timing.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/GaussianFactor.h>

#include <boost/assign/list_of.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/range/adaptor/map.hpp>
#include <CppUnitLite/TestHarness.h>

using namespace std;
using namespace boost::assign;
using namespace gtsam;

// F
typedef Eigen::Matrix<double, 2, 6> Matrix26;
const Matrix26 F0 = Matrix26::Ones();
const Matrix26 F1 = 2 * Matrix26::Ones();
const Matrix26 F3 = 3 * Matrix26::Ones();
const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
    (make_pair(0, F0))(make_pair(1, F1))(make_pair(3, F3));
// RHS and sigmas
const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.);

//*************************************************************************************
TEST( implicitSchurFactor, creation ) {
  // Matrix E = Matrix::Ones(6,3);
  Matrix E = zeros(6, 3);
  E.block<2,2>(0, 0) = eye(2);
  E.block<2,3>(2, 0) = 2 * ones(2, 3);
  Matrix3 P = (E.transpose() * E).inverse();
  ImplicitSchurFactor<6> expected(Fblocks, E, P, b);
  Matrix expectedP = expected.getPointCovariance();
  EXPECT(assert_equal(expectedP, P));
}

/* ************************************************************************* */
TEST( implicitSchurFactor, addHessianMultiply ) {

  Matrix E = zeros(6, 3);
  E.block<2,2>(0, 0) = eye(2);
  E.block<2,3>(2, 0) = 2 * ones(2, 3);
  E.block<2,2>(4, 1) = eye(2);
  Matrix3 P = (E.transpose() * E).inverse();

  double alpha = 0.5;
  VectorValues xvalues = map_list_of //
  (0, gtsam::repeat(6, 2))//
  (1, gtsam::repeat(6, 4))//
  (2, gtsam::repeat(6, 0))// distractor
  (3, gtsam::repeat(6, 8));

  VectorValues yExpected = map_list_of//
  (0, gtsam::repeat(6, 27))//
  (1, gtsam::repeat(6, -40))//
  (2, gtsam::repeat(6, 0))// distractor
  (3, gtsam::repeat(6, 279));

  // Create full F
  size_t M=4, m = 3, d = 6;
  Matrix F(2 * m, d * M);
  F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3;

  // Calculate expected result F'*alpha*(I - E*P*E')*F*x
  FastVector<Key> keys;
  keys += 0,1,2,3;
  Vector x = xvalues.vector(keys);
  Vector expected = zero(24);
  ImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected);
  EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8));

  // Create ImplicitSchurFactor
  ImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b);

  VectorValues zero = 0 * yExpected;// quick way to get zero w right structure
  { // First Version
    VectorValues yActual = zero;
    implicitFactor.multiplyHessianAdd(alpha, xvalues, yActual);
    EXPECT(assert_equal(yExpected, yActual, 1e-8));
    implicitFactor.multiplyHessianAdd(alpha, xvalues, yActual);
    EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
    implicitFactor.multiplyHessianAdd(-1, xvalues, yActual);
    EXPECT(assert_equal(zero, yActual, 1e-8));
  }

  typedef Eigen::Matrix<double, 24, 1> DeltaX;
  typedef Eigen::Map<DeltaX> XMap;
  double* y = new double[24];
  double* xdata = x.data();

  { // Raw memory Version
    std::fill(y, y + 24, 0);// zero y !
    implicitFactor.multiplyHessianAdd(alpha, xdata, y);
    EXPECT(assert_equal(expected, XMap(y), 1e-8));
    implicitFactor.multiplyHessianAdd(alpha, xdata, y);
    EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
    implicitFactor.multiplyHessianAdd(-1, xdata, y);
    EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
  }

  // Create JacobianFactor with same error
  const SharedDiagonal model;
  JacobianFactorQ<6> jf(Fblocks, E, P, b, model);

  { // error
    double expectedError = jf.error(xvalues);
    double actualError = implicitFactor.errorJF(xvalues);
    DOUBLES_EQUAL(expectedError,actualError,1e-7)
  }

  { // JacobianFactor with same error
    VectorValues yActual = zero;
    jf.multiplyHessianAdd(alpha, xvalues, yActual);
    EXPECT(assert_equal(yExpected, yActual, 1e-8));
    jf.multiplyHessianAdd(alpha, xvalues, yActual);
    EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
    jf.multiplyHessianAdd(-1, xvalues, yActual);
    EXPECT(assert_equal(zero, yActual, 1e-8));
  }

  { // check hessian Diagonal
    VectorValues diagExpected = jf.hessianDiagonal();
    VectorValues diagActual = implicitFactor.hessianDiagonal();
    EXPECT(assert_equal(diagExpected, diagActual, 1e-8));
  }

  { // check hessian Block Diagonal
    map<Key,Matrix> BD = jf.hessianBlockDiagonal();
    map<Key,Matrix> actualBD = implicitFactor.hessianBlockDiagonal();
    LONGS_EQUAL(3,actualBD.size());
    EXPECT(assert_equal(BD[0],actualBD[0]));
    EXPECT(assert_equal(BD[1],actualBD[1]));
    EXPECT(assert_equal(BD[3],actualBD[3]));
  }

  { // Raw memory Version
    std::fill(y, y + 24, 0);// zero y !
    jf.multiplyHessianAdd(alpha, xdata, y);
    EXPECT(assert_equal(expected, XMap(y), 1e-8));
    jf.multiplyHessianAdd(alpha, xdata, y);
    EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
    jf.multiplyHessianAdd(-1, xdata, y);
    EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
  }

  { // Check gradientAtZero
    VectorValues expected = jf.gradientAtZero();
    VectorValues actual = implicitFactor.gradientAtZero();
    EXPECT(assert_equal(expected, actual, 1e-8));
  }

  // Create JacobianFactorQR
  JacobianFactorQR<6> jfq(Fblocks, E, P, b, model);
  {
    const SharedDiagonal model;
    VectorValues yActual = zero;
    jfq.multiplyHessianAdd(alpha, xvalues, yActual);
    EXPECT(assert_equal(yExpected, yActual, 1e-8));
    jfq.multiplyHessianAdd(alpha, xvalues, yActual);
    EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
    jfq.multiplyHessianAdd(-1, xvalues, yActual);
    EXPECT(assert_equal(zero, yActual, 1e-8));
  }

  { // Raw memory Version
    std::fill(y, y + 24, 0);// zero y !
    jfq.multiplyHessianAdd(alpha, xdata, y);
    EXPECT(assert_equal(expected, XMap(y), 1e-8));
    jfq.multiplyHessianAdd(alpha, xdata, y);
    EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
    jfq.multiplyHessianAdd(-1, xdata, y);
    EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
  }
  delete [] y;
}

/* ************************************************************************* */
TEST(implicitSchurFactor, hessianDiagonal)
{
  /* TESTED AGAINST MATLAB
   *  F = [ones(2,6) zeros(2,6) zeros(2,6)
        zeros(2,6) 2*ones(2,6) zeros(2,6)
        zeros(2,6) zeros(2,6) 3*ones(2,6)]
      E = [[1:6] [1:6] [0.5 1:5]];
      E = reshape(E',3,6)'
      P = inv(E' * E)
      H = F' * (eye(6) - E * P * E') * F
      diag(H)
   */
  Matrix E(6,3);
  E.block<2,3>(0, 0) << 1,2,3,4,5,6;
  E.block<2,3>(2, 0) << 1,2,3,4,5,6;
  E.block<2,3>(4, 0) << 0.5,1,2,3,4,5;
  Matrix3 P = (E.transpose() * E).inverse();
  ImplicitSchurFactor<6> factor(Fblocks, E, P, b);

  // hessianDiagonal
  VectorValues expected;
  expected.insert(0, 1.195652*ones(6));
  expected.insert(1, 4.782608*ones(6));
  expected.insert(3, 7.043478*ones(6));
  EXPECT(assert_equal(expected, factor.hessianDiagonal(),1e-5));

  // hessianBlockDiagonal
  map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
  LONGS_EQUAL(3,actualBD.size());
  Matrix FtE0 = F0.transpose() * E.block<2,3>(0, 0);
  Matrix FtE1 = F1.transpose() * E.block<2,3>(2, 0);
  Matrix FtE3 = F3.transpose() * E.block<2,3>(4, 0);

  // variant one
  EXPECT(assert_equal(F0.transpose()*F0-FtE0*P*FtE0.transpose(),actualBD[0]));
  EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1]));
  EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));

  // variant two
  Matrix I2 = eye(2);
  Matrix E0 = E.block<2,3>(0, 0);
  Matrix F0t = F0.transpose();
  EXPECT(assert_equal(F0t*F0-F0t*E0*P*E0.transpose()*F0,actualBD[0]));
  EXPECT(assert_equal(F0t*(F0-E0*P*E0.transpose()*F0),actualBD[0]));

  Matrix M1 = F0t*(F0-E0*P*E0.transpose()*F0);
  Matrix M2 = F0t*F0-F0t*E0*P*E0.transpose()*F0;

  EXPECT(assert_equal(  M1 , actualBD[0] ));
  EXPECT(assert_equal(  M1 , M2 ));

  Matrix M1b = F0t*(E0*P*E0.transpose()*F0);
  Matrix M2b = F0t*E0*P*E0.transpose()*F0;
  EXPECT(assert_equal(  M1b , M2b ));

  EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0]));
  EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1]));
  EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));
}

/* ************************************************************************* */
int main(void) {
  TestResult tr;
  int result = TestRegistry::runAllTests(tr);
  return result;
}
//*************************************************************************************


#include <gtest/gtest.h>
#include "../util/util.h"
#include <Eigen/Core>
#include <string>
#include <algorithm>

#ifndef _MSC_VER
extern "C" {
#include <csim/memory_ops.h>
#include <csim/stat_ops.h>
#include <csim/update_ops.h>
#include <csim/init_ops.h>
}
#else
#include <csim/memory_ops.h>
#include <csim/stat_ops.h>
#include <csim/update_ops.h>
#include <csim/init_ops.h>
#endif
#include <csim/update_ops_cpp.hpp>

void test_single_diagonal_matrix_gate(std::function<void(UINT, const CTYPE*, CTYPE*, ITYPE)> func) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;

	Eigen::MatrixXcd Identity(2, 2), Z(2, 2);
	Identity << 1, 0, 0, 1;
	Z << 1, 0, 0, -1;

	Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U;

	UINT target;
	double icoef, zcoef, norm;

	auto state = allocate_quantum_state(dim);
	initialize_Haar_random_state(state, dim);
	Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
	for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

	Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

	for (UINT rep = 0; rep < max_repeat; ++rep) {
		// single qubit diagonal matrix gate
		target = rand_int(n);
		icoef = rand_real(); zcoef = rand_real();
		norm = sqrt(icoef * icoef + zcoef * zcoef);
		icoef /= norm; zcoef /= norm;
		U = icoef * Identity + 1.i*zcoef * Z;
		Eigen::VectorXcd diag = U.diagonal();
		func(target, (CTYPE*)diag.data(), state, dim);
		test_state = get_expanded_eigen_matrix_with_identity(target, U, n) * test_state;
		state_equal(state, test_state, dim, "single diagonal gate");
	}
	release_quantum_state(state);
}

TEST(UpdateTest, SingleDiagonalMatrixTest) {
	test_single_diagonal_matrix_gate(single_qubit_diagonal_matrix_gate);
	test_single_diagonal_matrix_gate(single_qubit_diagonal_matrix_gate_single_unroll);
#ifdef _OPENMP
	test_single_diagonal_matrix_gate(single_qubit_diagonal_matrix_gate_parallel_unroll);
#endif
#ifdef _USE_SIMD
	test_single_diagonal_matrix_gate(single_qubit_diagonal_matrix_gate_single_simd);
#ifdef _OPENMP
	test_single_diagonal_matrix_gate(single_qubit_diagonal_matrix_gate_parallel_simd);
#endif
#endif
}

void test_single_phase_gate(std::function<void(UINT, CTYPE, CTYPE*, ITYPE)> func) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;

	Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U;

	UINT target;
	double angle;

	auto state = allocate_quantum_state(dim);
	initialize_Haar_random_state(state, dim);
	Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
	for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

	Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

	for (UINT rep = 0; rep < max_repeat; ++rep) {
		// single qubit phase matrix gate
		target = rand_int(n);
		angle = rand_real();
		U << 1, 0, 0, cos(angle) + 1.i*sin(angle);
#ifdef _MSC_VER
		CTYPE t = cos(angle) + 1.i*sin(angle);
#else
		CTYPE t = cos(angle) + 1.j*sin(angle);
#endif
		func(target, t, state, dim);
		test_state = get_expanded_eigen_matrix_with_identity(target, U, n) * test_state;
		state_equal(state, test_state, dim, "single phase gate");
	}
	release_quantum_state(state);
}

TEST(UpdateTest, SinglePhaseGateTest) {
	test_single_phase_gate(single_qubit_phase_gate);
	test_single_phase_gate(single_qubit_phase_gate_single_unroll);
#ifdef _OPENMP
	test_single_phase_gate(single_qubit_phase_gate_parallel_unroll);
#endif
#ifdef _USE_SIMD
	test_single_phase_gate(single_qubit_phase_gate_single_simd);
#ifdef _OPENMP
	test_single_phase_gate(single_qubit_phase_gate_parallel_simd);
#endif
#endif
}


// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/model.h>
#include <franka/robot.h>
#include <Eigen/Dense>
#include <array>
#include <cmath>
#include <functional>
#include <iostream>
#include "examples_common.h"

/**
 * @example cartesian_impedance_control.cpp
 * An example showing a simple cartesian impedance controller without inertia shaping
 * that renders a spring damper system where the equilibrium is the initial configuration.
 * After starting the controller try to push the robot around and try different stiffness levels.
 *
 * @warning collision thresholds are set to high values. Make sure you have the user stop at hand!
 */

void print_position(std::array<double, 42> initial_pose) {
  std::cout << "this is jacobian position" << std::endl;
  for (int i = 0; i < 42; i++) {
    std::cout << initial_pose[i] << "  ";
    if ((i + 1) % 7 == 0)
      std::cout << std::endl;
  }
}

Eigen::MatrixXf pseudoinverse(Eigen::MatrixXf m)
{
    //Eigen::Matrix<float,2,3> m;
  //  m<<0.68,0.597,-0.211, 0.823,0.566,-0.605;
   // m<<1,2,3,4,5,6;
    Eigen::JacobiSVD<Eigen::MatrixXf> svd =m.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
   // Eigen::JacobiSVD<Eigen::MatrixXf> svd(m, Eigen::ComputeThinU | Eigen::ComputeThinV);
    const Eigen::MatrixXf singularValues = svd.singularValues();
	Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> singularValuesInv(m.cols(), m.rows());
	singularValuesInv.setZero();
	double  pinvtoler = 1.e-6; // choose your tolerance wisely
	for (unsigned int i = 0; i < singularValues.size(); ++i) {
	 	if (singularValues(i) > pinvtoler)
	 		singularValuesInv(i, i) = 1.0f / singularValues(i);
	 	else
	 		singularValuesInv(i, i) = 0.f;
	 }
    Eigen::MatrixXf pinvmat = svd.matrixV() * singularValuesInv * svd.matrixU().transpose();
    std::cout << pinvmat << std::endl;
	return pinvmat;
}

int main(int argc, char** argv) {
  // Check whether the required arguments were passed
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
    return -1;
  }

  // Compliance parameters
  const double translational_stiffness{150.0};
  const double rotational_stiffness{10.0};
  Eigen::MatrixXd stiffness(6, 6), damping(6, 6);
  stiffness.setZero();
  stiffness.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3);
  damping.setZero();
  damping.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) *
                                     Eigen::MatrixXd::Identity(3, 3);
  damping.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) *
                                         Eigen::MatrixXd::Identity(3, 3);

  try {
    // connect to robot
    franka::Robot robot(argv[1]);
    setDefaultBehavior(robot);
    // load the kinematics and dynamics model
    franka::Model model = robot.loadModel();
    franka::RobotState initial_state = robot.readOnce();

    // equilibrium point is the initial position
    Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
    Eigen::Vector3d position_d(initial_transform.translation());
    Eigen::Quaterniond orientation_d(initial_transform.linear());
    // set collision behavior
    robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                               {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});

    // define callback for the torque control loop
    std::function<franka::Torques(const franka::RobotState&, franka::Duration)>
        impedance_control_callback = [&](const franka::RobotState& robot_state,
                                         franka::Duration /*duration*/) -> franka::Torques {
      // get state variables

      std::array<double, 7> coriolis_array = model.coriolis(robot_state);
      std::array<double, 42> jacobian_array =
          model.zeroJacobian(franka::Frame::kEndEffector, robot_state);
      // print the jacobian array
      // print_position(jacobian_array);

      // convert to Eigen
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data());
      // jacobian matrix
      Eigen::Map<const Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());

      Eigen::Matrix<double, 3, 7> half_jacobian;
      // only get three lines
      for (int i = 0; i < half_jacobian.rows(); i++) {
        for (int j = 0; j < half_jacobian.cols(); j++) {
          half_jacobian(i, j) = jacobian(i, j);
        }
      }

      Eigen::Map<const Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
      Eigen::Map<const Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
      Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
      Eigen::Vector3d position(transform.translation());
      Eigen::Quaterniond orientation(transform.linear());

      // compute error to desired equilibrium pose
      Eigen::MatrixXf pse_inverse_jacobian=pseudoinverse(half_jacobian);

      double phi_sin = 2 * M_PI * std::sin(0.5 * M_PI * time / 10);
      double phi_sinDot = M_PI * M_PI * std::cos(0.5 * M_PI * time / 10) / 10;
      double phi = phi_sin * std::sin(0.5 * M_PI * time / 10);
      double phiDot = phi_sin * M_PI * std::cos(0.5 * M_PI * time / 10) / 10;
      double phiDotDot = M_PI * phi_sinDot * std::cos(0.5 * M_PI * time / 10) / 10 -
                         M_PI * M_PI * phi_sin * std::sin(0.5 * M_PI * time / 10) / (2 * 10 * 10);
      double rx = r * std::cos(2 * phi + alpha) + 0 - r * std::cos(alpha);
      double ry = r * std::sin(phi + alpha) + 0 - r * std::sin(alpha);

    double drx = r * std::cos(2 * phi + alpha) + 0 - r * std::cos(alpha);
      double dry = r * std::sin(phi + alpha) + 0 - r * std::sin(alpha);

  //     float  drx(jj,1)=-2*r*sin(2*phi+alpha)*phiDot;
  //  dry(jj,1)=r*cos(phi+alpha)*phiDot;
  //  drz(jj,1)=0;
      pse_inverse_jacobian*(half_jacobian*dq-dq)

      // position error

      std::cout << "position: " << position << std::endl;
      std::cout << "positiond: " << position_d << std::endl;
      std::cout << "positiond: " << position_d << std::endl;

      for (int i = 0; i < dq.size(); i++) {
        std::cout << "dq " << dq[i] << "  " << std::endl;
      }

      Eigen::Matrix<double, 6, 1> error;
      error.head(3) << position - position_d;

      // orientation error
      // "difference" quaternion
      if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
        orientation.coeffs() << -orientation.coeffs();
      }
      // "difference" quaternion
      Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d);
      error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
      // Transform to base frame
      error.tail(3) << -transform.linear() * error.tail(3);

      // compute control
      Eigen::VectorXd tau_task(7), tau_d(7);

      // Spring damper system with damping ratio=1
      // matrix transpose multiple
      tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq));
      tau_d << tau_task + coriolis;

      std::array<double, 7> tau_d_array{};
      Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
      return tau_d_array;
    };
    // start real-time control loop
    std::cout << "WARNING: Collision thresholds are set to high values. "
              << "Make sure you have the user stop at hand!" << std::endl
              << "After starting try to push the robot and see how it reacts." << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();
    robot.control(impedance_control_callback);

  } catch (const franka::Exception& ex) {
    // print exception
    std::cout << ex.what() << std::endl;
  }

  return 0;
}


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <boost/python.hpp>

#include <scitbx/sym_mat3.h>
#include <scitbx/array_family/versa.h>
#include <scitbx/array_family/shared.h>
#include <scitbx/array_family/accessors/c_grid.h>
#include <boost/scoped_array.hpp>

namespace boost_python_meta_ext { struct holder {}; }

extern "C" {

  void
  dgesdd_(
    char const* jobz,
    int const* m,
    int const* n,
    double* a,
    int const* lda,
    double* s,
    double* u,
    int const* ldu,
    double* vt,
    int const* ldvt,
    double* work,
    int const* lwork,
    int* iwork,
    int* info,
    int jobz_len);

  void
  dgesvd_(
    char const* jobu,
    char const* jobvt,
    int const* m,
    int const* n,
    double* a,
    int const* lda,
    double* s,
    double* u,
    int const* ldu,
    double* vt,
    int const* ldvt,
    double* work,
    int const* lwork,
    int* info,
    int jobu_len,
    int jobvt_len);

  void
  dsyev_(
    char const* jobz,
    char const* uplo,
    int const* n,
    double* a,
    int const* lda,
    double* w,
    double* work,
    int const* lwork,
    int* info,
    int jobz_len,
    int uplo_len);

} // extern "C"

#if defined(SCITBX_LAPACK_FEM)
#  include <lapack_fem/selected.hpp>
#endif

namespace scitbx { namespace lapack { namespace boost_python {

#if defined(SCITBX_LAPACK_FEM)
  lapack_fem::common cmn(0, 0);
#endif

  boost::python::object
  dgesdd_wrapper(
    af::ref<double, af::c_grid<2> > const& a,
    bool use_fortran=false)
  {
    int m = a.accessor()[1];
    int n = a.accessor()[0];
    SCITBX_ASSERT(m > 0);
    SCITBX_ASSERT(n > 0);
    boost::python::object result;
#if defined(SCITBX_LAPACK_FEM) || defined(SCITBX_LAPACK_FOR)
    int p = std::min(m,n);
    af::shared<double> s(p, 0.);
    af::versa<double, af::c_grid<2> > u(af::c_grid<2>(p, m), 0.);
    af::versa<double, af::c_grid<2> > vt(af::c_grid<2>(n, p), 0.);
    boost::scoped_array<int> iwork(new int[8*p]);
    int lwork = -1;
    int info;
    for(unsigned i_pass=0;i_pass<2;i_pass++) {
      boost::scoped_array<double> work(new double[std::max(1,lwork)]);
#endif
      if (!use_fortran) {
#if defined(SCITBX_LAPACK_FEM)
        lapack_fem::dgesdd(
          cmn,
          /*jobz*/ "S",
          m,
          n,
          a[0],
          /*lda*/ m,
          s[0],
          u[0],
          /*ldu*/ m,
          vt[0],
          /*ldvt*/ p,
          work[0],
          lwork,
          iwork[0],
          info);
#else
        return result;
#endif
      }
      else {
#if defined(SCITBX_LAPACK_FOR)
        dgesdd_(
          /*jobz*/ "S",
          &m,
          &n,
          &a[0],
          /*lda*/ &m,
          &s[0],
          &u[0],
          /*ldu*/ &m,
          &vt[0],
          /*ldvt*/ &p,
          &work[0],
          &lwork,
          &iwork[0],
          &info,
          /*jobz_len*/ 1);
#else
        return result;
#endif
      }
#if defined(SCITBX_LAPACK_FEM) || defined(SCITBX_LAPACK_FOR)
      if (i_pass == 0) {
        lwork = static_cast<int>(work[0]);
      }
    }
    result = boost::python::object(boost_python_meta_ext::holder());
    result.attr("s") = s;
    result.attr("u") = u;
    result.attr("vt") = vt;
    result.attr("info") = info;
#endif
    return result;
  }

  boost::python::object
  dgesvd_wrapper(
    af::ref<double, af::c_grid<2> > const& a,
    bool use_fortran=false)
  {
    int m = a.accessor()[1];
    int n = a.accessor()[0];
    SCITBX_ASSERT(m > 0);
    SCITBX_ASSERT(n > 0);
    boost::python::object result;
#if defined(SCITBX_LAPACK_FEM) || defined(SCITBX_LAPACK_FOR)
    int p = std::min(m,n);
    af::shared<double> s(p, 0.);
    af::versa<double, af::c_grid<2> > u(af::c_grid<2>(p, m), 0.);
    af::versa<double, af::c_grid<2> > vt(af::c_grid<2>(n, p), 0.);
    int lwork = -1;
    int info;
    for(unsigned i_pass=0;i_pass<2;i_pass++) {
      boost::scoped_array<double> work(new double[std::max(1,lwork)]);
#endif
      if (!use_fortran) {
#if defined(SCITBX_LAPACK_FEM)
        lapack_fem::dgesvd(
          cmn,
          /*jobu*/ "S",
          /*jobvt*/ "S",
          m,
          n,
          a[0],
          /*lda*/ m,
          s[0],
          u[0],
          /*ldu*/ m,
          vt[0],
          /*ldvt*/ p,
          work[0],
          lwork,
          info);
#else
        return result;
#endif
      }
      else {
#if defined(SCITBX_LAPACK_FOR)
        dgesvd_(
          /*jobu*/ "S",
          /*jobvt*/ "S",
          &m,
          &n,
          &a[0],
          /*lda*/ &m,
          &s[0],
          &u[0],
          /*ldu*/ &m,
          &vt[0],
          /*ldvt*/ &p,
          &work[0],
          &lwork,
          &info,
          /*jobu_len*/ 1,
          /*jobvt_len*/ 1);
#else
        return result;
#endif
      }
#if defined(SCITBX_LAPACK_FEM) || defined(SCITBX_LAPACK_FOR)
      if (i_pass == 0) {
        lwork = static_cast<int>(work[0]);
      }
    }
    result = boost::python::object(boost_python_meta_ext::holder());
    result.attr("s") = s;
    result.attr("u") = u;
    result.attr("vt") = vt;
    result.attr("info") = info;
#endif
    return result;
  }

  int
  dsyev_wrapper(
    std::string const& jobz,
    std::string const& uplo,
    af::ref<double, af::c_grid<2> > const& a,
    af::ref<double> const& w,
    bool use_fortran=false)
  {
    SCITBX_ASSERT(a.accessor().is_square());
    int n = a.accessor()[0];
    SCITBX_ASSERT(w.size() == n);
    int info = 99;
#if defined(SCITBX_LAPACK_FEM) || defined(SCITBX_LAPACK_FOR)
    int lwork = -1;
    bool active = false;
    for(unsigned i_pass=0;i_pass<2;i_pass++) {
      boost::scoped_array<double> work(new double[std::max(1,lwork)]);
#endif
      if (!use_fortran) {
#if defined(SCITBX_LAPACK_FEM)
        lapack_fem::dsyev(
          cmn,
          fem::str_cref(jobz.data(), jobz.size()),
          fem::str_cref(uplo.data(), uplo.size()),
          n,
          a[0],
          /*lda*/ n,
          w[0],
          work[0],
          lwork,
          info);
        active = true;
#endif
      }
      else {
#if defined(SCITBX_LAPACK_FOR)
        dsyev_(
          jobz.data(),
          uplo.data(),
          &n,
          &a[0],
          /*lda*/ &n,
          &w[0],
          &work[0],
          &lwork,
          &info,
          jobz.size(),
          uplo.size());
        active = true;
#endif
      }
#if defined(SCITBX_LAPACK_FEM) || defined(SCITBX_LAPACK_FOR)
      if (!active) break;
      if (i_pass == 0) {
        TBXX_ASSERT(info == 0);
        lwork = static_cast<int>(work[0]);
      }
    }
#endif
    return info;
  }

  // simlar to time_eigensystem_real_symmetric()
  scitbx::vec3<double>
  time_dsyev(
    scitbx::sym_mat3<double> const& m,
    std::size_t n_repetitions,
    bool use_fortran=false)
  {
    SCITBX_ASSERT(n_repetitions % 2 == 0);
    scitbx::vec3<double> result(0,0,0);
    int info = 99;
    for(std::size_t i=0;i<n_repetitions/2;i++) {
      for(std::size_t j=0;j<2;j++) {
        scitbx::vec3<double> w;
        scitbx::mat3<double> a(m);
        info = dsyev_wrapper("V", "U",
          af::ref<double, af::c_grid<2> >(a.begin(), af::c_grid<2>(3,3)),
          w.ref(), use_fortran);
        if (j == 0) result += w;
        else        result -= w;
      }
    }
    SCITBX_ASSERT(info == 0);
    return result / static_cast<double>(n_repetitions);
  }

  bool
  fem_is_available()
  {
#if defined(SCITBX_LAPACK_FEM)
    return true;
#else
    return false;
#endif
  }

  bool
  for_is_available()
  {
#if defined(SCITBX_LAPACK_FOR)
    return true;
#else
    return false;
#endif
  }

  void
  wrap()
  {
    using namespace boost::python;

    def("fem_is_available", fem_is_available);
    def("for_is_available", for_is_available);

    def("lapack_dgesdd", dgesdd_wrapper, (
      arg("a"), arg("use_fortran")=false));
    def("lapack_dgesvd", dgesvd_wrapper, (
      arg("a"), arg("use_fortran")=false));
    def("lapack_dsyev", dsyev_wrapper, (
      arg("jobz"), arg("uplo"), arg("a"), arg("w"), arg("use_fortran")=false));

    def("time_lapack_dsyev", time_dsyev, (
      arg("m"), arg("n_repetitions"), arg("use_fortran")=false));
  }

}}} // namespace scitbx::lapack::boost_python


#include "polyscope/polyscope.h"

#include <iostream>

#include "geometrycentral/geometry.h"
#include "geometrycentral/halfedge_mesh.h"
#include "geometrycentral/linear_solvers.h"
#include "geometrycentral/polygon_soup_mesh.h"

#include <Eigen/SparseLU>

#include "args/args.hxx"
#include "json/json.hpp"

#define GLM_ENABLE_EXPERIMENTAL
#include "glm/gtx/string_cast.hpp"

#define STB_IMAGE_IMPLEMENTATION
#include "stb_image.h"

using namespace geometrycentral;
using std::cerr;
using std::cout;
using std::endl;
using std::string;

class CatData {
  // Initialized stuff
  Geometry<Euclidean>* geom;
  HalfedgeMesh* mesh;
  std::string niceName;

  // Original mesh information
  VertexData<size_t> vInd;
  size_t nVerts;
  size_t nHalfedges;
  size_t dim;
  VertexData<double> angleDefects;
  HalfedgeData<size_t> hInd;
  CornerData<size_t> cInd;

public:
  // Derived Information
  HalfedgeData<double> finalCurvature;
  VertexData<double> multiplier;

  HalfedgeData<double> d;
  HalfedgeData<double> alpha;

  EdgeData<double> netEdgeCurvature;
  CornerData<double> cornerAngle;
  CornerData<char> badCorners;

  // Solves the optimization problem
  void solveOptMatrix() {
    Eigen::SparseMatrix<double> d0 = Eigen::SparseMatrix<double>(dim, dim);
    std::vector<Eigen::Triplet<double>> tripletList;
    Vector<double> rhs = Vector<double>(dim);
    // cout << dim << endl;
    for (size_t i = 0; i < nHalfedges; i++) {
      tripletList.emplace_back(i, i, 1.);
      rhs[i] = 0.;
    }

    for (size_t i = nHalfedges; i < dim; i++) {
      rhs[i] = angleDefects[mesh->vertex(i - nHalfedges)];
    }
    for (EdgePtr e : mesh->edges()) {
      HalfedgePtr h1 = e.halfedge();
      HalfedgePtr h2 = h1.twin();
      size_t v1 = vInd[h1.vertex()];
      size_t v2 = vInd[h2.vertex()];
      tripletList.emplace_back(nHalfedges + v1, hInd[h1], 1);
      tripletList.emplace_back(nHalfedges + v1, hInd[h2], 1);
      tripletList.emplace_back(nHalfedges + v2, hInd[h1], 1);
      tripletList.emplace_back(nHalfedges + v2, hInd[h2], 1);

      tripletList.emplace_back(hInd[h1], nHalfedges + v1, 1);
      tripletList.emplace_back(hInd[h2], nHalfedges + v1, 1);
      tripletList.emplace_back(hInd[h1], nHalfedges + v2, 1);
      tripletList.emplace_back(hInd[h2], nHalfedges + v2, 1);
    }
    d0.setFromTriplets(tripletList.begin(), tripletList.end());
    cout << "Matrix built" << endl;

    Eigen::SparseLU<Eigen::SparseMatrix<double>> solver;
    solver.compute(d0);
    if (solver.info() != Eigen::Success) {
      cout << "solving failed" << endl;
    }
    Vector<double> solution = solver.solve(rhs);
    if (solver.info() != Eigen::Success) {
      cout << "solving failed";
    }
    // cout << "Matrix solved";
    for (size_t i = 0; i < nHalfedges; i++) {
      finalCurvature[i] = solution[i];
      //cout << solution[i] << endl;
    }
    for (size_t i = nHalfedges; i < dim; i++) {
      multiplier[i - nHalfedges] = solution[i];
    }
    polyscope::getSurfaceMesh(niceName)->addQuantity("Original Curvature", angleDefects);
    polyscope::getSurfaceMesh(niceName)->addQuantity("Curvature change", finalCurvature);
    polyscope::getSurfaceMesh(niceName)->addQuantity("Lagrange Multiplier", multiplier);
    return;
  }
  void checkAngles()
  {
    size_t bad = 0;
    for (CornerPtr c: mesh->corners())
    {
      cornerAngle[c] =  geom->angle(c);
    }
    for (CornerPtr c: mesh->corners())
    {
      HalfedgePtr h = c.halfedge();
      cornerAngle[c] += finalCurvature[h];
      cornerAngle[c.next()] += finalCurvature[h];
    }
    cout << bad << endl;
    for (CornerPtr c: mesh->corners())
    {
      if (cornerAngle[c] < 0 || cornerAngle[c] > 2 * M_PI)
      {
        bad++;
        badCorners[c] = true;
      }
    }
    cout << bad << endl;
  }
  CatData(std::string filename) {
    niceName = polyscope::utilities::guessNiceNameFromPath(filename);
    mesh = new HalfedgeMesh(PolygonSoupMesh(filename), geom);
    polyscope::registerSurfaceMesh(niceName, geom);

    vInd = mesh->getVertexIndices();
    nVerts = mesh->nVertices();
    hInd = mesh->getHalfedgeIndices();
    nHalfedges = mesh->nHalfedges();
    cInd = mesh->getCornerIndices();
    dim = nVerts + nHalfedges;

    finalCurvature = HalfedgeData<double>(mesh);
    multiplier = VertexData<double>(mesh);
    d = HalfedgeData<double>(mesh);
    alpha = HalfedgeData<double>(mesh);
    netEdgeCurvature = EdgeData<double>(mesh);
    cornerAngle = CornerData<double>(mesh);
    badCorners = CornerData<char>(mesh, false);

    geom->getVertexAngleDefects(angleDefects);
    solveOptMatrix();
    checkAngles();
    polyscope::getSurfaceMesh(niceName)->addQuantity("Central angles", alpha);
    delete geom;
    delete mesh;
  }
};

int main(int argc, char** argv) {
  // Configure the argument parser
  /*args::ArgumentParser parser("A simple demo of Polyscope.\nBy "
                              "Nick Sharp (nsharp@cs.cmu.edu)",
                              "");
  args::PositionalList<string> files(parser, "files", "One or more files to visualize");
  */
  // Options
  polyscope::options::autocenterStructures = true;
  // Initialize polyscope
  polyscope::init();
  CatData* c = new CatData("C:/spot1.obj");
  delete c;
  // Show the gui
  polyscope::show();

  return 0;
}


#include <boost/test/unit_test.hpp>
#include "functions/unary_minus.hh"
#include "functions/addition.hh"
#include "functions/std_functions.hh"
#include "functions/zero.hh"
#include "functions/polynomial.hh"
#include "functions/function_matrix.hh"
#include "functions/division.hh"
#include "functions/all_simplifications.hh"

BOOST_AUTO_TEST_CASE(simplification_ambiguities_test) {
  using namespace manifolds;

// commas in template parameter packs in macros are hard...
// hence this otherwise dumb-looking define
#define C ,

#define TESTCASE(code)                                                         \
  static_assert(Simplifies<code>::value, "Simplification failed")

  // Tests ambiguity of add_nadd and nadd_add and var_add
  TESTCASE(Addition<UnaryMinus<UnaryMinus<Sin> > C UnaryMinus<Sin> >);
  // Tests ambiguity of add_f_z and add_z_f and add_f_f and var_add
  TESTCASE(Addition<Zero C Zero>);
  // Tests ambiguity of add_f_p_1 and var_add
  TESTCASE(Addition<Sin C Polynomial<double C int_<1> > >);
  // Tests ambiguity of com_add and var_com
  TESTCASE(Composition<Addition<Sin C Cos> C Tan>);
  // Tests com_fm_fm and var_com
  TESTCASE(
      Composition<FunctionMatrix<int_<1> C int_<2> C Variable<0> C Variable<1> >
                      C FunctionMatrix<int_<1> C int_<2> C Sin C Cos> >);
  // Tests com_v_0_f and com_v_fm and var_com
  TESTCASE(Composition<
      Variable<0> C FunctionMatrix<int_<2> C int_<1> C Tan C Cos> >);
  // Tests com_mult_f and var_com
  TESTCASE(Composition<Multiplication<Variable<1> C Variable<0> > C
                           FunctionMatrix<int_<2> C int_<1> C Sin C Cos> >);
  // Tests com_p_1_fs and com_p1_p2 and var_com
  TESTCASE(
      Composition<Polynomial<double C int_<1> > C Polynomial<int C int_<4> > >);
  // Tests com_p_1_fs and and com_p_add_fs and var_com
  TESTCASE(Composition<Polynomial<int C int_<1> > C Addition<Sin C Sin> >);
  // Tests com_t_fm and var_com
  TESTCASE(
      Composition<Transpose C FunctionMatrix<int_<2> C int_<1> C Cos C Sin> >);
  // Tests com_z_f and var_com
  TESTCASE(Composition<Zero C Zero>);
  // Tests mult_div_dif and mult_div_f
  TESTCASE(Multiplication<Division<Sin C Cos> C Division<Cos C Sin> >);
  // Tests mult_f1_com_pow_fm_f2_f1 and var_mult and mult_um_f1_f2
  TESTCASE(Multiplication<UnaryMinus<Cos> C Composition<
      Pow C FunctionMatrix<int_<1> C int_<2> C Cos C UnaryMinus<Cos> > > >);
  // Tests add_p1_p2 and add_f_p_1
  TESTCASE(Addition<Polynomial<int C int_<12> > C Polynomial<int C int_<1> > >);
  // Tests um_um_f and um_f
  TESTCASE(UnaryMinus<UnaryMinus<Sin> >);
  // Tests mult_com_p1_fs_p2_1 and mult_com_p_f_f and
  // var_com_f1_fs_com_f2_fs
  TESTCASE(Multiplication<Composition<Polynomial<int C int_<2> > C Composition<
      Polynomial<int C int_<2> > C Sin> > C
                              Composition<Polynomial<int C int_<2> > C Sin> >);
  // Tests mult_z_f and var_f_com_p_f
  TESTCASE(
      Multiplication<Zero C Composition<Polynomial<int C int_<2> > C Zero> >);
  // Tests add_z_f and add_nadd
  TESTCASE(Addition<Zero C UnaryMinus<Zero> >);
  // Tests add_z_f and add_f_p_1
  TESTCASE(Addition<Zero C Polynomial<int C int_<1> > >);
  // Tests add_f_p_1 and add_f_f
  TESTCASE(Addition<Polynomial<int C int_<1> > C Polynomial<int C int_<1> > >);
  // Tests um_com_f_fs and um_f
  TESTCASE(UnaryMinus<Composition<Polynomial<int C int_<2> > C Sin> >);
  // Tests com_div_fs, var_com
  TESTCASE(Composition<Division<Sin C Cos> C Polynomial<int C int_<12> > >);
  // Tests mult_cos_cos and mult_f_f and var_mult
  TESTCASE(Multiplication<Cos C Cos>);
  // Tests var_com_f1_fs_com_f2_fs and var_add and add_f_f
  TESTCASE(Addition<Composition<Polynomial<int C int_<3> > C Cos> C
                        Composition<Polynomial<int C int_<3> > C Cos> >);
  // Tests var_com_f1_fs_com_f2_fs and var_com
  TESTCASE(Composition<Composition<Polynomial<int C int_<3> > C Cos> C
                           Composition<Polynomial<int C int_<3> > C Cos> >);
  // Tests var_grp and var_com_f1_fs_com_f2_fs
  TESTCASE(Group<Composition<Polynomial<int C int_<4> > C Cos> C
                     Composition<Polynomial<int C int_<4> > C Cos> >);
  // Tests var_com_f1_fs_com_f2_fs and var_mult and mult_f_f
  TESTCASE(Multiplication<Composition<Polynomial<int C int_<3> > C Cos> C
                              Composition<Polynomial<int C int_<3> > C Cos> >);
  // Tests var_com_f1_fs_com_f2_fs and add_com_p_f_f
  TESTCASE(
      Addition<Composition<Polynomial<int C int_<3> > C
                               Composition<Polynomial<int C int_<2> > C Sin> > C
                   Composition<Polynomial<int C int_<2> > C Sin> >);
  // Tests var_com_f1_fs_com_f2_fs and mult_com_p_f_f
  TESTCASE(Multiplication<Composition<Polynomial<int C int_<3> > C Composition<
      Polynomial<int C int_<2> > C Sin> > C
                              Composition<Polynomial<int C int_<2> > C Sin> >);
  // Tests add_f_z and nadd_add
  TESTCASE(Addition<UnaryMinus<Zero> C Zero>);
  // Tests com_z_z
  TESTCASE(Composition<Zero C Zero>);

  TESTCASE(Multiplication<Zero C IntegralPolynomial<1> >);
}


/**
    @file bayes_classifier.cpp

    @author Terence Henriod

    Project 1: Bayesian Minimum Error Classification

    @brief Class implementations for the BayesClassifier defined in
           bayes_classifier.h.

    @version Original Code 1.00 (3/8/2014) - T. Henriod
*/

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   HEADER FILES / NAMESPACES
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

// Class Declaration
#include "bayes_classifier.h"

// Other Dependencies
#include <cassert>
#include <iostream>
#include <fstream>
#include <Eigen/Dense>  // -I /home/thenriod/Desktop/cpp_libs/Eigen_lib

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
================================================================================
                   CLASS FUNCTION IMPLEMENTATIONS
================================================================================
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   CONSTRUCTOR(S) / DESTRUCTOR
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

/**
GameState

The default constructor for a game state. Constructs and initilizes an empty
GameState.

@pre
-# The GameState object is given an appropriate identifier.

@post
-# A new, empty GameState will be initialized.

@code
@endcode
*/
BayesClassifier::BayesClassifier()
{
  // variables
  int ndx = 0;
  Eigen::Vector2d temp_mean;
    temp_mean << 1, 1;
  Eigen::Matrix2d temp_matrix;
    temp_matrix << 1, 0,
                   0, 1;

  // initialize all members
  for( ndx = 0; ndx < NUM_FEATURES; ndx++ )
  {
    prior_probabilities.push_back( double( 1.0 / NUM_FEATURES ) );
  }
  for( ndx = 0; ndx < NUM_FEATURES; ndx++ )
  {
    mean_vectors.push_back( temp_mean );
  }
  for( ndx = 0; ndx < NUM_FEATURES; ndx++ )
  {
    covariance_matrices.push_back( temp_matrix );
  }
  assumption_case_ = CASE_THREE;

  // no return - constructor
}

BayesClassifier::BayesClassifier( const BayesClassifier& other )
{
  // no return - copy constructor
}

BayesClassifier& BayesClassifier::operator=( const BayesClassifier& other )
{
  // return *this
  return *this;
}

BayesClassifier::~BayesClassifier()
{
  // no return - destructor
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   MUTATORS
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

void BayesClassifier::clear()
{
  // no return - void
}

void BayesClassifier::setMean( const Eigen::Vector2d& new_mean_vector,
                               const int which_class )
{
  // variables
    // none

  // set the appropriate mean vector
  mean_vectors[which_class] = new_mean_vector;

  // no return - void
}

void BayesClassifier::setCovariance(
    const Eigen::Matrix2d& new_covariance_matrix,
    const int which_class )
{
  // variables
    // none

  // set the appropriate covariance matrix
  covariance_matrices[which_class] = new_covariance_matrix;

  // case: the new case was case 1
  if( assumption_case_ == CASE_ONE )
  {
    // update the variance
    variance = covariance_matrices[CLASS_ONE]( 0, 0 );
  }

  // no return - void
}

void BayesClassifier::setPriorProbabilities( const double class_one_prior )
{
  // assert pre-conditions
  assert( ( class_one_prior >= 0.0 ) && ( class_one_prior <= 1.0 ) );
  // assert( class_one_prior == ( 1 - class_one_prior ) );  // hopefully doubles don't screw this up
    // they do

  // variables
    // none

  // set the new prior probability
  prior_probabilities[0] = class_one_prior;
  prior_probabilities[1] = 1 - class_one_prior;

  // no return - void
}

void BayesClassifier::setAssumptionCase( const int new_case )
{
  // set the new case   TODO: automate this
  assumption_case_ = new_case;

  // case: the new case was case 1
  if( assumption_case_ == CASE_ONE )
  {
    // update the variance
    variance = covariance_matrices[CLASS_ONE]( 0, 0 );
  }
}

/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
                   ACCESSORS
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
void BayesClassifier::performAnalysis( const string input_file,
                                       const string output_file )
{
  // variables
  fstream file;
  TestData temp;
  char delimiter;
  int num_data = 0;
  int ndx = 0;
  int num_misclassified = 0;
  double test_error_rate = 0.5;
  double beta_start = 0.5;
  vector<TestData> data;
  Chernoff chernoff_bound;

  // read in all of the data
  file.clear();
  file.open( input_file.c_str(), fstream::in );
  while( file.good() )
  {
    // read in a line of data
    file >> temp.feature_vector(0) >> delimiter
         >> temp.feature_vector(1) >> delimiter
         >> temp.actual_class;

    // store the data
    data.push_back( temp );

    // count the data
    num_data++;
  }
  file.close();

  // classify all of the data
  for( ndx = 0; ndx < num_data; ndx++ )
  {
    // classify an object
    data[ndx].classified_as = assignToClass( data[ndx].feature_vector );

    // case: it was classified correctly
    if( data[ndx].classified_as == data[ndx].actual_class )
    {
      // mark and count this as a correct classification
      data[ndx].correctly_classified = CORRECT;
    }
    // case: it was not classified correctly
    else
    {
      // mark this as an incorrect classification
      data[ndx].correctly_classified = INCORRECT;
      num_misclassified++;
    }
  }

  // compute the error rate of the test
  test_error_rate = double( num_misclassified ) / double( num_data );

  // find chernoff bound
  chernoff_bound = findChernoffBound( beta_start, 0 );

  // output the results to file
  file.clear();
  file.open( output_file.c_str(), fstream::out );
  file << "Number of data: " << ( num_data - 1 ) << endl
       << "Number of incorrect classifications: "
           << num_misclassified << endl
       << "Test Sample Error Rate: " << test_error_rate << endl
       << "Battacharyya bound: " << findBattacharyyaBound() << endl
       << "Chernoff bound: " << chernoff_bound.bound << endl
       << "         beta*: " << chernoff_bound.beta << endl;
  for( ndx = 0; ndx < num_data; ndx++ )
  {
    // write the delimited data to the file
    file << data[ndx].feature_vector(0) << ", "
         << data[ndx].feature_vector(1) << ", "
         << data[ndx].actual_class << ", "
         << data[ndx].classified_as << ", "
         << data[ndx].correctly_classified << endl;
  }
  file.close();

  // no return - void
}

string BayesClassifier::assignToClass( Eigen::Vector2d& input_vector )
{
  // variables
  string classification_result;
  double discriminant_difference = 0;

  // calculate the difference of the discriminants
  discriminant_difference = calculateDiscriminant( input_vector, CLASS_ONE ) -
                            calculateDiscriminant( input_vector, CLASS_TWO );

  // case: the difference has a positive result
  if( discriminant_difference > 0 )
  {
    // the object is likely in class one
    classification_result = "ONE";
  }
  // case: the difference has a negative result
  else
  {
    // the object is likely in class two
    classification_result = "TWO";
  }

  // return the resulting assignment
  return classification_result;
}

double BayesClassifier::calculateDiscriminant(
    const Eigen::Vector2d& input_vector,
    const int which_class )
{
  // variables
  double discriminant_result = 0;
  double first_sum_term = 0;
  double second_sum_term = 0;
  double third_sum_term = 0;
  double fourth_sum_term = 0;
  double fifth_sum_term = 0;
  Eigen::Vector2d mean;
  Eigen::Matrix2d inverse_covariance_matrix;
  Eigen::Vector2d intermediate_row;
  Eigen::Vector2d intermediate_col;
  Eigen::Matrix2d intermediate_mat;

  // get the appropriate mean ready
  mean = mean_vectors[which_class];

  // prepare an inverse of the covariance matrix for the computations
  inverse_covariance_matrix = covariance_matrices[which_class].inverse();

  // case: the assumptions are not that of case 1
  if( assumption_case_ != CASE_ONE )
  {
    // compute the first summative term of the discriminant function
    intermediate_mat = -0.5 * inverse_covariance_matrix;
    intermediate_row = ( input_vector.transpose() * intermediate_mat );
    first_sum_term = intermediate_row.dot( input_vector );

    // compute the second summative term of the discriminant function
    second_sum_term = ( inverse_covariance_matrix * mean ).transpose().dot( input_vector );

    // compute the third summative term of the discriminant function
    intermediate_row = -0.5 * mean;
    intermediate_row = intermediate_row.transpose() * inverse_covariance_matrix;
    third_sum_term = intermediate_row.transpose().dot( mean );

    // compute the fourth summative term of the discriminant function
    fourth_sum_term = -0.5 *
                      log( covariance_matrices[which_class].determinant() );
  }
  // case: we are assuming case 1 assumptions
  else
  {
    // compute the first term ( 1/s^2 * mean * x )
    intermediate_row = ( 1.0 / variance ) * mean;
    first_sum_term = intermediate_row.transpose().dot( input_vector );

    // compute the second term
    intermediate_row = ( -1.0 / ( 2 * variance ) ) * mean;
    second_sum_term = intermediate_row.transpose().dot( mean );
  }

  // compute the last summative term of the discriminant function
  fifth_sum_term = log( prior_probabilities[which_class] );

  // sum the terms to get the discriminant result
  discriminant_result = first_sum_term + second_sum_term + third_sum_term +
                        fourth_sum_term + fifth_sum_term;

  // return the discriminant result
  return discriminant_result;
}

Chernoff BayesClassifier::findChernoffBound( double beta_star, int level )
{
  // variables
  Chernoff chernoff_bound;
    chernoff_bound.beta = beta_star;
  Chernoff left_attempt;
  Chernoff right_attempt;
  double beta_increment = 0.0249999;
  double prior_product = 0.0;
  double kappa_of_beta = 0;

  // compute the prior product
  prior_product = pow( prior_probabilities[0], beta_star ) *
                  pow( prior_probabilities[1], ( 1.0 - beta_star ) );

  // kappa( beta* )
  kappa_of_beta = kappaF( beta_star );

  // compute the Chernoff bound
  chernoff_bound.bound = prior_product * exp( -1.0 * kappa_of_beta );

  // case: we aren't 1000 levels deep
  if( level < 20 )
  {
    // find two different possible bounds
    left_attempt = findChernoffBound( beta_star - beta_increment, level + 1 );
    right_attempt = findChernoffBound( beta_star + beta_increment, level + 1 );

    // test to find the lowest bound
    if( left_attempt.bound < chernoff_bound.bound )
    {
      chernoff_bound = left_attempt;
    }
    if( right_attempt.bound < chernoff_bound.bound )
    {
      chernoff_bound = right_attempt;
    }
  }

  // return the Chernoff bound
  return chernoff_bound;
}

double BayesClassifier::findBattacharyyaBound()
{
  // variables
  double battacharyya_bound = 1;
  double kappa_of_beta = 0;
  double root_prior_product = 0;
  double root_covariance_det_product = 0;
  Eigen::Vector2d mean_difference;
  Eigen::Matrix2d covariance_sum;

  // compute the square root term
  root_prior_product = sqrt( prior_probabilities[0] * prior_probabilities[1] );

  // compute kappa( 0.5 )
  kappa_of_beta = kappaF( 0.5 );

  // compute sqrt( P( w1 ) * P( w2 ) ) * e^( -kappa( 0.5 ) )
  battacharyya_bound = root_prior_product * exp( -1.0 * kappa_of_beta );

  // return the Battacharrya bound
  return battacharyya_bound;
}

double BayesClassifier::kappaF( const double beta )
{
  // variables
  double kappa_of_beta = 0;
  double beta_complement = 1.0 - beta;
  double beta_product_over_two = 0;
  double root_prior_product = 0;
  double root_covariance_det_product = 0;
  double log_denominator = 0;
  Eigen::Vector2d mean_difference;
  Eigen::Vector2d intermediate_row;
  Eigen::Matrix2d scaled_covariance_sum;

  // compute (beta * beta^c) / 2
  beta_product_over_two = ( beta * beta_complement ) / 2;

  // compute the mean difference u2 - u1 (to be used later)
  mean_difference = mean_vectors[0] - mean_vectors[1];

  // compute the scaled covariance sum beta^c * E1 + beta * E2
  // (to be used later)
  scaled_covariance_sum = ( beta_complement * covariance_matrices[0] ) +
                          ( beta * covariance_matrices[1] );

  // compute the logarithm denominator (to be used later)
  log_denominator = pow( covariance_matrices[0].determinant(),
                         beta_complement );
  log_denominator *= pow( covariance_matrices[1].determinant(),
                          beta );

  // compute the first term in the sum
  intermediate_row = beta_product_over_two * mean_difference;
  intermediate_row = intermediate_row.transpose() *
                     scaled_covariance_sum.inverse();
  kappa_of_beta = intermediate_row.transpose() * mean_difference;

  // compute the second term in the sum
  kappa_of_beta += 0.5 * log( scaled_covariance_sum.determinant() /
                              log_denominator );

  // return the result
  return kappa_of_beta;
}


// g++ -fopenmp -I .. -O3 -DNDEBUG -finline-limit=1000 benchmarkX.cpp -o b && time ./b
#include <Eigen/Core>

using namespace std;
USING_PART_OF_NAMESPACE_EIGEN

#ifndef MATTYPE
#define MATTYPE MatrixXLd
#endif

#ifndef MATSIZE
#define MATSIZE 400
#endif

#ifndef REPEAT
#define REPEAT 100
#endif

int main(int argc, char *argv[])
{
	MATTYPE I = MATTYPE::Ones(MATSIZE,MATSIZE);
	MATTYPE m(MATSIZE,MATSIZE);
	for(int i = 0; i < MATSIZE; i++) for(int j = 0; j < MATSIZE; j++)
	{
		m(i,j) = (i+j+1)/(MATSIZE*MATSIZE);
	}
	for(int a = 0; a < REPEAT; a++)
	{
		m = I + 0.0001 * (m + m*m);
	}
	cout << m(0,0) << endl;
	return 0;
}


// Copyright (C) 2011  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "tester.h"
#include <dlib/svm_threaded.h>
#include <dlib/data_io.h>
#include "create_iris_datafile.h"
#include <vector>
#include <map>
#include <sstream>

namespace
{
    using namespace test;
    using namespace dlib;
    using namespace std;
    dlib::logger dlog("test.svm_multiclass_trainer");

    class test_svm_multiclass_trainer : public tester
    {
        /*!
            WHAT THIS OBJECT REPRESENTS
                This object represents a unit test.  When it is constructed
                it adds itself into the testing framework.
        !*/
    public:
        test_svm_multiclass_trainer (
        ) :
            tester (
                "test_svm_multiclass_trainer",       // the command line argument name for this test
                "Run tests on the svm_multiclass_linear_trainer stuff.", // the command line argument description
                0                     // the number of command line arguments for this test
            )
        {
        }

        void test_prior ()
        {
            print_spinner();
            typedef matrix<double,4,1> sample_type;
            typedef linear_kernel<sample_type> kernel_type;

            std::vector<sample_type> samples;
            std::vector<int> labels;

            for (int i = 0; i < 4; ++i)
            {
                if (i==2)
                    ++i;
                for (int iter = 0; iter < 5; ++iter)
                {
                    sample_type samp;
                    samp = 0;
                    samp(i) = 1;
                    samples.push_back(samp);
                    labels.push_back(i);
                }
            }

            svm_multiclass_linear_trainer<kernel_type,int> trainer;

            multiclass_linear_decision_function<kernel_type,int> df = trainer.train(samples, labels);

            //cout << "test: \n" << test_multiclass_decision_function(df, samples, labels) << endl;
            //cout << df.weights << endl;
            //cout << df.b << endl;

            std::vector<sample_type> samples2;
            std::vector<int> labels2;
            int i = 2;
            for (int iter = 0; iter < 5; ++iter)
            {
                sample_type samp;
                samp = 0;
                samp(i) = 1;
                samples2.push_back(samp);
                labels2.push_back(i);
                samples.push_back(samp);
                labels.push_back(i);
            }

            trainer.set_prior(df);
            trainer.set_c(0.1);
            df = trainer.train(samples2, labels2);

            matrix<double> res = test_multiclass_decision_function(df, samples, labels);
            dlog << LINFO << "test: \n" << res;
            dlog << LINFO << df.weights;
            dlog << LINFO << df.b;
            DLIB_TEST((unsigned int)sum(diag(res))==samples.size());
        }

        void test_prior_sparse ()
        {
            print_spinner();
            typedef std::map<unsigned long,double> sample_type;
            typedef sparse_linear_kernel<sample_type> kernel_type;

            std::vector<sample_type> samples;
            std::vector<int> labels;

            for (int i = 0; i < 4; ++i)
            {
                if (i==2)
                    ++i;
                for (int iter = 0; iter < 5; ++iter)
                {
                    sample_type samp;
                    samp[i] = 1;
                    samples.push_back(samp);
                    labels.push_back(i);
                }
            }

            svm_multiclass_linear_trainer<kernel_type,int> trainer;

            multiclass_linear_decision_function<kernel_type,int> df = trainer.train(samples, labels);

            //cout << "test: \n" << test_multiclass_decision_function(df, samples, labels) << endl;
            //cout << df.weights << endl;
            //cout << df.b << endl;

            std::vector<sample_type> samples2;
            std::vector<int> labels2;
            int i = 2;
            for (int iter = 0; iter < 5; ++iter)
            {
                sample_type samp;
                samp[i] = 1;
                samp[i+10] = 1;
                samples2.push_back(samp);
                labels2.push_back(i);
                samples.push_back(samp);
                labels.push_back(i);
            }

            trainer.set_prior(df);
            trainer.set_c(0.1);
            df = trainer.train(samples2, labels2);

            matrix<double> res = test_multiclass_decision_function(df, samples, labels);
            dlog << LINFO << "test: \n" << res;
            dlog << LINFO << df.weights;
            dlog << LINFO << df.b;
            DLIB_TEST((unsigned int)sum(diag(res))==samples.size());
        }

        template <typename sample_type>
        void run_test()
        {
            print_spinner();

            typedef typename sample_type::value_type::second_type scalar_type;

            std::vector<sample_type> samples;
            std::vector<scalar_type> labels;

            load_libsvm_formatted_data("iris.scale",samples, labels);

            DLIB_TEST(samples.size() == 150);
            DLIB_TEST(labels.size() == 150);

            typedef sparse_linear_kernel<sample_type> kernel_type;
            svm_multiclass_linear_trainer<kernel_type> trainer;
            trainer.set_c(100);
            trainer.set_epsilon(0.000001);

            randomize_samples(samples, labels);
            matrix<double> cv = cross_validate_multiclass_trainer(trainer, samples, labels, 4);

            dlog << LINFO << "confusion matrix: \n" << cv;
            const scalar_type cv_accuracy = sum(diag(cv))/sum(cv);
            dlog << LINFO << "cv accuracy: " << cv_accuracy;
            DLIB_TEST(cv_accuracy > 0.97);

            {
                print_spinner();
                typedef matrix<scalar_type,0,1> dsample_type;
                std::vector<dsample_type> dsamples = sparse_to_dense(samples);
                DLIB_TEST(dsamples.size() == 150);

                typedef linear_kernel<dsample_type> kernel_type;
                svm_multiclass_linear_trainer<kernel_type> trainer;
                trainer.set_c(100);

                cv = cross_validate_multiclass_trainer(trainer, dsamples, labels, 4);

                dlog << LINFO << "dense confusion matrix: \n" << cv;
                const scalar_type cv_accuracy = sum(diag(cv))/sum(cv);
                dlog << LINFO << "dense cv accuracy: " << cv_accuracy;
                DLIB_TEST(cv_accuracy > 0.97);
            }

        }

        void perform_test (
        )
        {
            print_spinner();
            create_iris_datafile();

            run_test<std::map<unsigned int, double> >();
            run_test<std::map<unsigned int, float> >();
            run_test<std::vector<std::pair<unsigned int, float> > >();
            run_test<std::vector<std::pair<unsigned long, double> > >();

            test_prior();
            test_prior_sparse();
        }
    };

    test_svm_multiclass_trainer a;

}


#ifndef SFGE_MATH_INTERSECTIONS_HPP
#define SFGE_MATH_INTERSECTIONS_HPP

#include <boost/array.hpp>

#include <SFML/System/Vector2.hpp>

#include "sfge/math/circle.hpp"
#include "sfge/math/edge.hpp"
#include "sfge/math/numerics.hpp"

namespace sfge
{
	enum IntersectionResult
	{
		IR_None,
		IR_Tangent,
		IR_TwoIntersections
	};

	template <typename VectorStorageT>
	struct EdgeCircleIntersectionCont
	{
		boost::array<VectorStorageT, 2> mTs;
		sf::Vector2f					mStartPoint;
		sf::Vector2f					mDir;
	};

	template <typename VectorStorageT>
	IntersectionResult	intersect(const Edge2<VectorStorageT> &e, const Circle<VectorStorageT> &c,
								  EdgeCircleIntersectionCont<VectorStorageT> &out);

#include "intersections.inl"
}

#endif


#ifndef HAZEN_CORE
#define HAZEN_CORE
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <Eigen/QR>
#include <cmath>
#include <iostream>
#include <type_traits>
#include <utility>

namespace hazen {

// Implementation --------------------------------------------------------------
namespace Unit_impl {
using fraction = std::pair<int, int>;
constexpr int gcd(int a, int b) {
  if (a == 0)
    return b;
  return gcd(b % a, a);
}
constexpr fraction simplify(fraction frac) {
  int common_factor = gcd(frac.first, frac.second);
  fraction result = {frac.first / common_factor, frac.second / common_factor};
  if (result.second < 0) {
    result.first *= -1;
    result.second *= -1;
  }
  return result;
}
constexpr fraction add_fraction(fraction frac1, fraction frac2) {
  int den = gcd(frac1.second, frac2.second);
  den = frac1.second * frac2.second / den;
  int num =
      frac1.first * (den / frac1.second) + frac2.first * (den / frac2.second);
  return simplify({num, den});
}
constexpr fraction subtract_fraction(fraction frac1, fraction frac2) {
  frac2.first *= -1;
  return add_fraction(frac1, frac2);
}
} // namespace Unit_impl

// Unit Templates --------------------------------------------------------------
template <int L_num, int L_den, int T_num, int T_den, int M_num, int M_den,
          int Theta_num, int Theta_den>
struct Unit {
  static constexpr Unit_impl::fraction L = {L_num, L_den};
  static constexpr Unit_impl::fraction T = {T_num, T_den};
  static constexpr Unit_impl::fraction M = {M_num, M_den};
  static constexpr Unit_impl::fraction Theta = {Theta_num, Theta_den};
};

template <typename U1, typename U2> struct UPlus {
  using type = Unit<Unit_impl::add_fraction(U1::L, U2::L).first,
                    Unit_impl::add_fraction(U1::L, U2::L).second,
                    Unit_impl::add_fraction(U1::T, U2::T).first,
                    Unit_impl::add_fraction(U1::T, U2::T).second,
                    Unit_impl::add_fraction(U1::M, U2::M).first,
                    Unit_impl::add_fraction(U1::M, U2::M).second,
                    Unit_impl::add_fraction(U1::Theta, U2::Theta).first,
                    Unit_impl::add_fraction(U1::Theta, U2::Theta).second>;
};
template <typename U1, typename U2> struct UMinus {
  using type = Unit<Unit_impl::subtract_fraction(U1::L, U2::L).first,
                    Unit_impl::subtract_fraction(U1::L, U2::L).second,
                    Unit_impl::subtract_fraction(U1::T, U2::T).first,
                    Unit_impl::subtract_fraction(U1::T, U2::T).second,
                    Unit_impl::subtract_fraction(U1::M, U2::M).first,
                    Unit_impl::subtract_fraction(U1::M, U2::M).second,
                    Unit_impl::subtract_fraction(U1::Theta, U2::Theta).first,
                    Unit_impl::subtract_fraction(U1::Theta, U2::Theta).second>;
};
template <typename U, int divisor> struct UDivide {
  static constexpr Unit_impl::fraction L =
      Unit_impl::add_fraction({U::L.first, U::L.second *divisor}, {0, 1});
  static constexpr Unit_impl::fraction T =
      Unit_impl::add_fraction({U::T.first, U::T.second *divisor}, {0, 1});
  static constexpr Unit_impl::fraction M =
      Unit_impl::add_fraction({U::M.first, U::M.second *divisor}, {0, 1});
  static constexpr Unit_impl::fraction Theta = Unit_impl::add_fraction(
      {U::Theta.first, U::Theta.second *divisor}, {0, 1});
  using type = Unit<L.first, L.second, T.first, T.second, M.first, M.second,
                    Theta.first, Theta.second>;
};
template <typename U, int multiplier> struct UMultiply {
  static constexpr Unit_impl::fraction L =
      Unit_impl::add_fraction({U::L.first * multiplier, U::L.second}, {0, 1});
  static constexpr Unit_impl::fraction T =
      Unit_impl::add_fraction({U::T.first * multiplier, U::T.second}, {0, 1});
  static constexpr Unit_impl::fraction M =
      Unit_impl::add_fraction({U::M.first * multiplier, U::M.second}, {0, 1});
  static constexpr Unit_impl::fraction Theta = Unit_impl::add_fraction(
      {U::Theta.first * multiplier, U::Theta.second}, {0, 1});
  using type = Unit<L.first, L.second, T.first, T.second, M.first, M.second,
                    Theta.first, Theta.second>;
};

template <typename U1, typename U2>
using Unit_Plus = typename UPlus<U1, U2>::type;
template <typename U1, typename U2>
using Unit_Minus = typename UMinus<U1, U2>::type;
template <typename U, int divisor>
using Unit_Divide = typename UDivide<U, divisor>::type;
template <typename U, int multiplier>
using Unit_Multiply = typename UMultiply<U, multiplier>::type;

// Unit Definitions ------------------------------------------------------------
using Dimensionless_Unit = Unit<0, 1, 0, 1, 0, 1, 0, 1>;
using Length_Unit = Unit<1, 1, 0, 1, 0, 1, 0, 1>;
using Time_Unit = Unit<0, 1, 1, 1, 0, 1, 0, 1>;
using Mass_Unit = Unit<0, 1, 0, 1, 1, 1, 0, 1>;
using Temperature_Unit = Unit<0, 1, 0, 1, 0, 1, 1, 1>;
using Velocity_Unit = Unit_Minus<Length_Unit, Time_Unit>;
using Acceleration_Unit = Unit_Minus<Velocity_Unit, Time_Unit>;
using Area_Unit = Unit_Plus<Length_Unit, Length_Unit>;
using Volume_Unit = Unit_Plus<Area_Unit, Length_Unit>;
using Density_Unit = Unit_Minus<Mass_Unit, Volume_Unit>;
using Force_Unit = Unit_Plus<Mass_Unit, Acceleration_Unit>;
using Pressure_Unit = Unit_Minus<Force_Unit, Area_Unit>;
using Dynamic_Viscosity_Unit = Unit_Plus<Pressure_Unit, Time_Unit>;
using Flow_Unit = Unit_Minus<Volume_Unit, Time_Unit>;

// Scalar Templates ------------------------------------------------------------
template <typename U> class Scalar {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  Scalar<U> &operator+=(const Scalar<U> &x) {
    val += x.val;
    return *this;
  }
  Scalar<U> &operator-=(const Scalar<U> &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar<U> &s) {
    os << s.val;
    return os;
  }

  double val;
};

// Binary Arithmetic Operations
template <typename U> inline Scalar<U> operator+(Scalar<U> x, Scalar<U> y) {
  return Scalar<U>{x.val + y.val};
}
template <typename U> inline Scalar<U> operator-(Scalar<U> x, Scalar<U> y) {
  return Scalar<U>{x.val - y.val};
}
template <typename U1, typename U2>
inline Scalar<Unit_Plus<U1, U2>> operator*(Scalar<U1> x, Scalar<U2> y) {
  return Scalar<Unit_Plus<U1, U2>>{x.val * y.val};
}
template <typename U1, typename U2>
inline Scalar<Unit_Minus<U1, U2>> operator/(Scalar<U1> x, Scalar<U2> y) {
  return Scalar<Unit_Minus<U1, U2>>{x.val / y.val};
}

// Use doubles as dimensionless scalars
template <typename U> inline Scalar<U> operator*(Scalar<U> x, double y) {
  return Scalar<U>{x.val * y};
}
template <typename U> inline Scalar<U> operator*(double x, Scalar<U> y) {
  return Scalar<U>{x * y.val};
}
template <typename U> inline Scalar<U> operator/(Scalar<U> x, double y) {
  return Scalar<U>{x.val / y};
}
template <typename U>
inline Scalar<Unit_Minus<Unit<0, 1, 0, 1, 0, 1, 0, 1>, U>>
operator/(double x, Scalar<U> y) {
  return Scalar<Unit_Minus<Unit<0, 1, 0, 1, 0, 1, 0, 1>, U>>{x / y.val};
}

// Binary Comparison Operations
template <typename U> inline bool operator==(Scalar<U> x, Scalar<U> y) {
  return x.val == y.val;
}
template <typename U> inline bool operator!=(Scalar<U> x, Scalar<U> y) {
  return x.val != y.val;
}
template <typename U> inline bool operator>=(Scalar<U> x, Scalar<U> y) {
  return x.val >= y.val;
}
template <typename U> inline bool operator<=(Scalar<U> x, Scalar<U> y) {
  return x.val <= y.val;
}
template <typename U> inline bool operator>(Scalar<U> x, Scalar<U> y) {
  return x.val > y.val;
}
template <typename U> inline bool operator<(Scalar<U> x, Scalar<U> y) {
  return x.val < y.val;
}

// Scalar Functions ------------------------------------------------------------
template <int pow_num, int pow_den, typename U>
constexpr inline Scalar<Unit_Divide<Unit_Multiply<U, pow_num>, pow_den>>
power(Scalar<U> x) {
  return Scalar<Unit_Divide<Unit_Multiply<U, pow_num>, pow_den>>(std::pow(
      x.val, static_cast<double>(pow_num) / static_cast<double>(pow_den)));
}

template <typename U>
constexpr inline Scalar<Unit_Divide<U, 2>> sqrt(Scalar<U> x) {
  return Scalar<Unit_Divide<U, 2>>(std::sqrt(x.val));
}
template <typename U> constexpr inline Scalar<U> abs(Scalar<U> x) {
  return Scalar<U>(std::abs(x.val));
}
template <typename U>
constexpr inline Scalar<Unit_Multiply<U, 2>> abs2(Scalar<U> x) {
  return x * x;
}

// Scalar Result Types ---------------------------------------------------------
// Product type of two scalars
template <typename T1, typename T2>
using product_type = decltype(std::declval<T1 &>() * std::declval<T2 &>());
// Quotient type of two scalars
template <typename T1, typename T2>
using quotient_type = decltype(std::declval<T1 &>() / std::declval<T2 &>());
// Root of a scalar
template <typename T> using root_type = decltype(sqrt(std::declval<T>()));

// Scalar Definitions ----------------------------------------------------------
using Dimensionless = Scalar<Dimensionless_Unit>;
using Angle = Scalar<Dimensionless_Unit>;
using Length = Scalar<Length_Unit>;
using Time = Scalar<Time_Unit>;
using Mass = Scalar<Mass_Unit>;
using Temperature = Scalar<Temperature_Unit>;
using Velocity = Scalar<Velocity_Unit>;
using Acceleration = Scalar<Acceleration_Unit>;
using Area = Scalar<Area_Unit>;
using Volume = Scalar<Volume_Unit>;
using Density = Scalar<Density_Unit>;
using Force = Scalar<Force_Unit>;
using Pressure = Scalar<Pressure_Unit>;
using Dynamic_Viscosity = Scalar<Dynamic_Viscosity_Unit>;
using Flow = Scalar<Flow_Unit>;

template <> class Scalar<Dimensionless_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Angle Radians(double d) { return Angle(d); }
  static constexpr Angle Degrees(double d) { return Angle(d * M_PI / 180.0); }
  static Angle Slope(double rise, double run) {
    return Angle(std::atan2(rise, run));
  }
  static Angle Slope(double slope) { return Angle(std::atan(slope)); }
  static Angle Percent(double d) { return Angle(std::atan(d * 0.01)); }
  constexpr double as_radians() const { return val; }
  constexpr double as_degrees() const { return val * 180.0 / M_PI; }
  double as_slope() const { return std::tan(val); }
  double as_percent() const { return std::tan(val) * 100.0; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Length_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Length Feet(double d) { return Length(d * 0.3048); }
  static constexpr Length Yards(double d) { return Length(d * 0.3048 * 3); }
  static constexpr Length Inches(double d) { return Length(d * 0.3048 / 12); }
  static constexpr Length Meters(double d) { return Length(d); }
  constexpr double as_feet() const { return val / 0.3048; }
  constexpr double as_yards() const { return val / 0.3048 / 3; }
  constexpr double as_inches() const { return val / 0.3048 * 12; }
  constexpr double as_meters() const { return val; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Time_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Time Seconds(double d) { return Time(d); }
  constexpr double as_seconds() const { return val; }
  constexpr double as_minutes() const { return val / 60.0; }
  constexpr double as_hours() const { return val / 60.0 / 60.0; }
  constexpr double as_days() const { return val / 60.0 / 60.0 / 24.0; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Mass_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Mass Kilograms(double d) { return Mass(d); }
  static constexpr Mass Slug(double d) { return Mass(d * 14.593903); }
  constexpr double as_kilogram() const { return val; }
  constexpr double as_slug() const { return val / 14.593903; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Temperature_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Temperature Kelvin(double d) { return Temperature(d); }
  static constexpr Temperature Celcius(double d) {
    return Temperature(d + 273.15);
  }
  static constexpr Temperature Fahrenheit(double d) {
    return Temperature((d - 32) * 5 / 9 + 237.15);
  }
  constexpr double as_kelvin(double d) const { return val; }
  constexpr double as_celcius(double d) const { return val - 273.15; }
  constexpr double as_fahrenheit(double d) const {
    return (val - 273.15) * 9 / 5 + 32;
  }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Velocity_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Velocity MPS(double d) { return Velocity(d); }
  static constexpr Velocity FPS(double d) { return Velocity(d * 0.3048); }
  constexpr double as_meters_per_second() const { return val; }
  constexpr double as_feet_per_second() const { return val / 0.3048; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Acceleration_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Acceleration MPS2(double d) { return Acceleration(d); }
  static constexpr Acceleration FPS2(double d) {
    return Acceleration(d * 0.3048);
  }
  constexpr double as_meters_per_second_squared() const { return val; }
  constexpr double as_feet_per_second_squared() const { return val / 0.3048; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Area_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Area Square_Meters(double d) { return Area(d); }
  static constexpr Area Square_Feet(double d) {
    return Area(d * 0.3048 * 0.3048);
  }
  static constexpr Area Square_Inches(double d) {
    return Area(d * 0.3048 * 0.3048 / 12 / 12);
  }
  static constexpr Area Square_Yards(double d) {
    return Area(d * 0.3048 * 0.3048 * 3 * 3);
  }
  static constexpr Area Acres(double d) { return Area(d * 4046.8564224); }
  constexpr double as_square_meters() const { return val; }
  constexpr double as_square_feet() const { return val / 0.3048 / 0.3048; }
  constexpr double as_square_inches() const {
    return 12 * 12 * val / 0.3048 / 0.3048;
  }
  constexpr double as_square_yards() const {
    return val / 0.3048 / 0.3048 / 3 / 3;
  }
  constexpr double as_acres() const { return val / 4046.8564224; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Volume_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Volume Cubic_Meters(double d) { return Volume(d); }
  static constexpr Volume Cubic_Feet(double d) {
    return Volume(d * 0.3048 * 0.3048 * 0.3048);
  }
  static constexpr Volume Gallons(double d) { return Volume(d * 0.0037854118); }
  static constexpr Volume Liters(double d) { return Volume(d * 0.001); }
  constexpr double as_cubic_meters() const { return val; }
  constexpr double as_cubic_feet() const {
    return val / 0.3048 / 0.3048 / 0.3048;
  }
  constexpr double as_gallons() const { return val / 0.0037854118; }
  constexpr double as_liters() const { return val / 0.001; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Density_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Density Kilograms_per_Cubic_Meter(double d) {
    return Density(d);
  }
  static constexpr Density Slugs_per_Cubic_Foot(double d) {
    return Density(d * 515.3788184);
  }
  constexpr double as_kilograms_per_cubic_meter() { return val; }
  constexpr double as_slugs_per_cubic_foot() { return val / 515.3788184; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Force_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Force Newtons(double d) { return Force(d); }
  static constexpr Force Pounds(double d) { return Force(d * 4.4482216); }
  constexpr double as_newtons(double d) const { return val; }
  constexpr double as_pounds(double d) const { return val / 4.4482216; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Pressure_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Pressure Pascals(double d) { return Pressure(d); }
  static constexpr Pressure PSF(double d) {
    return Pressure(d * 47.880258888889);
  }
  static constexpr Pressure PSI(double d) { return Pressure(d * 6894.7572932); }
  constexpr double as_pascals() const { return val; }
  constexpr double as_psf() const { return val / 47.880258888889; }
  constexpr double as_psi() const { return val / 6894.7572932; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Dynamic_Viscosity_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Dynamic_Viscosity Pascal_Seconds(double d) {
    return Dynamic_Viscosity(d);
  }
  static constexpr Dynamic_Viscosity PSF_Seconds(double d) {
    return Dynamic_Viscosity(d * 47.880258888889);
  }
  constexpr double as_pascal_seconds() const { return val; }
  constexpr double as_psf_seconds() const { return val / 47.880258888889; }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};
template <> class Scalar<Flow_Unit> {
public:
  constexpr Scalar() : val{} {}
  explicit constexpr Scalar(double d) : val{d} {}
  static constexpr Flow CMS(double d) { return Flow(d); }
  static constexpr Flow LPS(double d) { return Flow(d * 0.001); }
  static constexpr Flow CFS(double d) {
    return Flow(d * 0.3048 * 0.3048 * 0.3048);
  }
  static constexpr Flow GPM(double d) { return Flow(d * 0.0037854118 / 60); }
  static constexpr Flow GPD(double d) {
    return Flow(d * 0.0037854118 / 60 / 60 / 24);
  }
  static constexpr Flow MGD(double d) {
    return Flow(d * 0.0037854118 / 60 / 60 / 24 * 1000000);
  }
  constexpr double as_cubic_meters_per_second() const { return val; }
  constexpr double as_liters_per_second() const { return val / 0.001; }
  constexpr double as_cubic_feet_per_second() const {
    return val / 0.3048 / 0.3048 / 0.3048;
  }
  constexpr double as_gallons_per_minute() const {
    return val * 60 / 0.0037854118;
  }
  constexpr double as_gallons_per_day() const {
    return val * 60 * 60 * 24 / 0.0037854118;
  }
  constexpr double as_million_gallons_per_day() const {
    return val * 60 * 60 * 24 / 0.0037854118 / 1000000;
  }

  Scalar &operator+=(const Scalar &x) {
    val += x.val;
    return *this;
  }
  Scalar &operator-=(const Scalar &x) {
    val -= x.val;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Scalar &s) {
    os << s.val;
    return os;
  }

  double val;
};

// Unit Literals ---------------------------------------------------------------
// Meters
constexpr Length operator""_m(long double d) {
  return Length::Meters(static_cast<double>(d));
}
// Yard
constexpr Length operator""_yd(long double d) {
  return Length::Yards(static_cast<double>(d));
}
// Feet
constexpr Length operator""_ft(long double d) {
  return Length::Feet(static_cast<double>(d));
}
// Inches
constexpr Length operator""_in(long double d) {
  return Length::Inches(static_cast<double>(d));
}
// Kilograms
constexpr Mass operator""_kg(long double d) {
  return Mass::Kilograms(static_cast<double>(d));
}
// Slug
constexpr Mass operator""_slug(long double d) {
  return Mass::Slug(static_cast<double>(d));
}
// Seconds
constexpr Time operator""_s(long double d) {
  return Time::Seconds(static_cast<double>(d));
}
// Kelvin
constexpr Temperature operator""_K(long double d) {
  return Temperature::Kelvin(static_cast<double>(d));
}
// Celcius
constexpr Temperature operator""_C(long double d) {
  return Temperature::Celcius(static_cast<double>(d));
}
// Fahrenheit
constexpr Temperature operator""_F(long double d) {
  return Temperature::Fahrenheit(static_cast<double>(d));
}
// Cubic Meters
constexpr Volume operator""_m3(long double d) {
  return Volume::Cubic_Meters(static_cast<double>(d));
}
// Cubic feet
constexpr Volume operator""_ft3(long double d) {
  return Volume::Cubic_Feet(static_cast<double>(d));
}
// Liters
constexpr Volume operator""_l(long double d) {
  return Volume::Liters(static_cast<double>(d));
}
// Gallons
constexpr Volume operator""_gal(long double d) {
  return Volume::Gallons(static_cast<double>(d));
}
// Cubic Meters per Second
constexpr Flow operator""_cms(long double d) {
  return Flow::CMS(static_cast<double>(d));
}
// Cubic Feet per Second
constexpr Flow operator""_cfs(long double d) {
  return Flow::CFS(static_cast<double>(d));
}
// Gallons per Minute
constexpr Flow operator""_gpm(long double d) {
  return Flow::GPM(static_cast<double>(d));
}
// Gallons per Day
constexpr Flow operator""_gpd(long double d) {
  return Flow::GPD(static_cast<double>(d));
}
// Million Gallons per Day
constexpr Flow operator""_mgd(long double d) {
  return Flow::MGD(static_cast<double>(d));
}
// Meters per Second
constexpr Velocity operator""_mps(long double d) {
  return Velocity::MPS(static_cast<double>(d));
}
// Feet per Second
constexpr Velocity operator""_fps(long double d) {
  return Velocity::FPS(static_cast<double>(d));
}
// Meters per Second Squared
constexpr Acceleration operator""_mps2(long double d) {
  return Acceleration::MPS2(static_cast<double>(d));
}
// Feet per Second Squared
constexpr Acceleration operator""_fps2(long double d) {
  return Acceleration::FPS2(static_cast<double>(d));
}
// Kilograms per Cubic Meter
constexpr Density operator""_kgm3(long double d) {
  return Density::Kilograms_per_Cubic_Meter(static_cast<double>(d));
}
// Slug per Cubic Feet
constexpr Density operator""_slugcf(long double d) {
  return Density::Slugs_per_Cubic_Foot(static_cast<double>(d));
}
// Pascal Seconds
constexpr Dynamic_Viscosity operator""_pas(long double d) {
  return Dynamic_Viscosity::Pascal_Seconds(static_cast<double>(d));
}
// Pounds per Square Foot Second
constexpr Dynamic_Viscosity operator""_psfs(long double d) {
  return Dynamic_Viscosity::PSF_Seconds(static_cast<double>(d));
}
// Newtons
constexpr Force operator""_n(long double d) {
  return Force::Newtons(static_cast<double>(d));
}
// Pounds (Force)
constexpr Force operator""_lbf(long double d) {
  return Force::Pounds(static_cast<double>(d));
}
// Pounds (Force) per Cubic Foot
constexpr Scalar<Unit_Minus<Force_Unit, Volume_Unit>>
operator""_pcf(long double d) {
  return Scalar<Unit_Minus<Force_Unit, Volume_Unit>>{
      static_cast<double>(d * 157.0874606377)};
}
// Radians
constexpr Angle operator""_rads(long double d) {
  return Angle::Radians(static_cast<double>(d));
}
// Degrees
constexpr Angle operator""_degrees(long double d) {
  return Angle::Degrees(static_cast<double>(d));
}
// Dimensionless
constexpr Dimensionless operator""_pure(long double d) {
  return Dimensionless(static_cast<double>(d));
}

template <typename T, int rows, int cols> class Matrix {
public:
  using value_type = T;

  Matrix() = default;
  Matrix(const Matrix &) = default;
  Matrix(Matrix &&) = default;
  Matrix &operator=(const Matrix &) = default;
  Matrix &operator=(Matrix &&) = default;

  Matrix(size_t m, size_t n)
      : elems{Eigen::Matrix<double, rows, cols>::Zero(m, n)} {}
  Matrix(size_t n) : elems{Eigen::Matrix<double, rows, cols>::Zero(n)} {}
  Matrix(Eigen::Matrix<double, rows, cols> m) : elems{m} {}
  Matrix &operator=(const T &s) {
    elems.setConstant(s.val);
    return *this;
  }
  Matrix &operator=(const Eigen::Matrix<double, rows, cols> &m) {
    elems = m;
    return *this;
  }
  Matrix &operator+=(const Matrix &m) {
    elems += m.elems;
    return *this;
  }
  Matrix &operator-=(const Matrix &m) {
    elems -= m.elems;
    return *this;
  }
  Matrix &operator-() {
    elems = -elems;
    return *this;
  }
  friend std::ostream &operator<<(std::ostream &os, const Matrix &m) {
    os << m.elems;
    return os;
  }

  operator T &() { return elems(0, 0); }
  operator T() const { return elems(0, 0); }
  size_t size() const { return elems.size(); }
  size_t n_rows() const { return elems.rows(); }
  size_t n_cols() const { return elems.cols(); }

  T operator()(size_t i, size_t j = 0) const { return T(elems(i, j)); }

  static Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Constant(int m, int n,
                                                            const T &s) {
    return Matrix<T, Eigen::Dynamic, Eigen::Dynamic>{
        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Constant(m, n,
                                                                        s.val)};
  }
  static Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Random(int m, int n) {
    return Matrix<T, Eigen::Dynamic, Eigen::Dynamic>{
        Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Random(m, n)};
  }
  static Matrix<T, Eigen::Dynamic, 1> Random(int n) {
    return Matrix<T, Eigen::Dynamic, 1>{
        Eigen::Matrix<double, Eigen::Dynamic, 1>::Random(n)};
  }

  Eigen::Matrix<double, rows, cols> elems;
};

// Binary Arithmetic Operations
template <typename T, int rows, int cols>
inline Matrix<T, rows, cols> operator+(const Matrix<T, rows, cols> &x,
                                       const Matrix<T, rows, cols> &y) {
  return Matrix<T, rows, cols>{x.elems + y.elems};
}
template <typename T, int rows, int cols>
inline Matrix<T, rows, cols> operator-(const Matrix<T, rows, cols> &x,
                                       const Matrix<T, rows, cols> &y) {
  return Matrix<T, rows, cols>{x.elems - y.elems};
}
template <typename T1, typename T2, int m, int n, int o>
inline Matrix<product_type<T1, T2>, m, o> operator*(const Matrix<T1, m, n> &x,
                                                    const Matrix<T2, n, o> &y) {
  return Matrix<product_type<T1, T2>, m, o>{x.elems * y.elems};
}

// Use doubles as dimensionless scalars
template <typename T, int rows, int cols>
inline Matrix<T, rows, cols> operator*(const Matrix<T, rows, cols> &x,
                                       double y) {
  return Matrix<T, rows, cols>{x.elems * y};
}
template <typename T, int rows, int cols>
inline Matrix<T, rows, cols> operator*(double x,
                                       const Matrix<T, rows, cols> &y) {
  return Matrix<T, rows, cols>{x * y.elems};
}
template <typename T, int rows, int cols>
inline Matrix<T, rows, cols> operator/(const Matrix<T, rows, cols> &x,
                                       double y) {
  return Matrix<T, rows, cols>{x.elems / y};
}
template <typename T, int rows, int cols>
inline Matrix<quotient_type<Dimensionless, T>, rows, cols>
operator/(double x, const Matrix<T, rows, cols> &y) {
  return Matrix<quotient_type<Dimensionless, T>, rows, cols>{x / y.elems};
}

// Scaling
template <typename T, typename U, int rows, int cols>
inline Matrix<product_type<T, Scalar<U>>, rows, cols>
operator*(const Matrix<T, rows, cols> &x, Scalar<U> y) {
  return Matrix<product_type<T, Scalar<U>>, rows, cols>{x.elems * y.val};
}
template <typename T, typename U, int rows, int cols>
inline Matrix<product_type<T, Scalar<U>>, rows, cols>
operator*(Scalar<U> x, const Matrix<T, rows, cols> &y) {
  return Matrix<product_type<T, Scalar<U>>, rows, cols>{x.val * y.elems};
}
template <typename T, typename U, int rows, int cols>
inline Matrix<quotient_type<T, Scalar<U>>, rows, cols>
operator/(const Matrix<T, rows, cols> &x, Scalar<U> y) {
  return Matrix<quotient_type<T, Scalar<U>>, rows, cols>{x.elems / y.val};
}
template <typename T, typename U, int rows, int cols>
inline Matrix<quotient_type<Scalar<U>, T>, rows, cols>
operator/(Scalar<U> x, const Matrix<T, rows, cols> &y) {
  return Matrix<quotient_type<Scalar<U>, T>, rows, cols>{x.val / y.elems};
}

// Binary Comparison Operations
template <typename T, int rows, int cols>
inline bool operator==(const Matrix<T, rows, cols> &x,
                       const Matrix<T, rows, cols> &y) {
  return x.elems == y.elems;
}
template <typename T, int rows, int cols>
inline bool operator!=(const Matrix<T, rows, cols> &x,
                       const Matrix<T, rows, cols> &y) {
  return x.elems != y.elems;
}
template <typename T, int rows, int cols>
inline bool operator>=(const Matrix<T, rows, cols> &x,
                       const Matrix<T, rows, cols> &y) {
  return x.elems >= y.elems;
}
template <typename T, int rows, int cols>
inline bool operator<=(const Matrix<T, rows, cols> &x,
                       const Matrix<T, rows, cols> &y) {
  return x.elems <= y.elems;
}
template <typename T, int rows, int cols>
inline bool operator>(const Matrix<T, rows, cols> &x,
                      const Matrix<T, rows, cols> &y) {
  return x.elems > y.elems;
}
template <typename T, int rows, int cols>
inline bool operator<(const Matrix<T, rows, cols> &x,
                      const Matrix<T, rows, cols> &y) {
  return x.elems < y.elems;
}

// Matrix Functions ------------------------------------------------------------
template <typename T, int rows, int cols>
T norm(const Matrix<T, rows, cols> &m) {
  return T{m.elems.norm()};
}
template <typename T, int rows, int cols>
Matrix<T, rows, cols> abs(const Matrix<T, rows, cols> &m) {
  return Matrix<T, rows, cols>{m.elems.cwiseAbs()};
}

template <typename T, int rows, int cols>
product_type<T, T> squared_norm(const Matrix<T, rows, cols> &m) {
  return product_type<T, T>{m.elems.squaredNorm()};
}
template <typename T, int rows, int cols>
Matrix<T, cols, rows> transpose(const Matrix<T, rows, cols> &m) {
  return m.elems.transpose();
}
template <typename T1, typename T2, int rows>
product_type<T1, T2> dot_product(const Matrix<T1, rows, 1> &u,
                                 const Matrix<T2, rows, 1> &v) {
  return product_type<T1, T2>(u.elems.dot(v.elems));
}
template <typename T1, typename T2, int n, int m>
Matrix<quotient_type<T2, T1>, m, 1> solve(const Matrix<T1, n, m> &A,
                                          const Matrix<T2, n, 1> &b) {
  return Matrix<quotient_type<T2, T1>, m, 1>{A.elems.lu().solve(b.elems)};
}
template <typename T1, typename T2, int n, int m>
Matrix<quotient_type<T2, T1>, m, 1>
solve_least_squares(const Matrix<T1, n, m> &A, const Matrix<T2, n, 1> &b) {
  // return Matrix<quotient_type<T2, T1>, m, 1>{
  //     A.elems.householderQr().solve(b.elems)};
  return Matrix<quotient_type<T2, T1>, m, 1>{
      A.elems.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b.elems)};
}

// Matrix and Vector aliases
template <typename T> using Mat = Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
template <typename T> using Vec = Matrix<T, Eigen::Dynamic, 1>;

} // namespace hazen

#endif