#include <iostream>
#include <cmath>
#include <Eigen/Dense>
#include "LCPSolve.h"

using namespace Eigen;
using namespace std;

// Forward declarations.
bool checkCompatability(const MatrixXd &M, const int &dim);
bool checkTrivialSol(const VectorXd &q);
MatrixXd constructTableau(const MatrixXd &M, const VectorXd &q);
void pivot(MatrixXd &tableau, const int &row, const int &col);
int initializeTableau(MatrixXd &tableau);
VectorXd basicVars(const MatrixXd &tableau);
bool checkSol(const MatrixXd &tableau);
bool checkRayTermination(const MatrixXd &tableu, const int &pivotCol);
int minRatioTest(const MatrixXd &tableau, const int &pivotCol);
MatrixXd extractSolution(const MatrixXd &tableau);

// Solve LCP by Lemke's Method.
LCP LCPSolve(MatrixXd M, VectorXd q) {
    const int dim = q.size();

    // Initialize LCP data structure.
    LCP solution {};
    solution.M = M;
    solution.q = q;

    // Check that inputs are compatable.
    bool compatable = checkCompatability(M, dim);
    if (!compatable) {
        solution.exitCond = 2;
        return solution;
    }

    // Check for trivial solution.
    bool trivial = checkTrivialSol(q);
    if (trivial) {
        solution.z = VectorXd::Zero(dim);
        solution.w = q;
        solution.exitCond = 0;
        return solution;
    }

    // Construct & initialize tableau.
    MatrixXd tableau = constructTableau(M, q);
    VectorXd index = VectorXd::LinSpaced(dim, 0, dim-1);

    int pivotCol = initializeTableau(tableau);
    int pivotRow = pivotCol - dim;
    index(pivotRow) = 2*dim;

    // Now, the tableau is initialized with a feasible basis. Pivot until there is a feasible basis w/o z0.
    const int maxIter {pow(2, dim)};
    int iter {0};
    bool solFound = false;
    while ((!solFound) && (iter < maxIter)) {
        // Check for ray termination.
        bool rayTermination = checkRayTermination(tableau, pivotCol);
        if (rayTermination) {
            solFound = true;
        } else {
            // Minimum ratio test to determine the pivot row (blocked/dropped variable).
            pivotRow = minRatioTest(tableau, pivotCol);
            pivot(tableau, pivotRow, pivotCol);
            int drop = index(pivotRow);
            index(pivotRow) = pivotCol;

            // Find next entering variable (pivotCol). Dropped variable is pivotRow.
            if (drop > dim-1) {
                pivotCol = drop - dim;
            } else {
                pivotCol = drop + dim;
            }

            // Check for solution.
            solFound = checkSol(tableau);
            iter++;
        }
    }

    // Return solution.
    if (solFound) {
        MatrixXd sols = extractSolution(tableau);
        solution.z = sols.col(0);
        solution.w = sols.col(1);
        solution.exitCond = 0;
    } else {
        MatrixXd sols = extractSolution(tableau);
        solution.z = sols.col(0);
        solution.w = sols.col(1);
        solution.exitCond = 3;
    }

    return solution;
}

// Check that the inputs M and q are compatable.
bool checkCompatability(const MatrixXd &M, const int &dim) {
    const int rows = M.rows();
    const int cols = M.cols();

    if (rows != cols)
        return false;
    else if (rows != dim)
        return false;
    else
        return true;
}

// Check for trivial solution where q is positive.
bool checkTrivialSol(const VectorXd &q) {
    const int dim = q.size();
    for (int i = 0; i < dim; i++)
        if (q[i] <= 0)
            return false;
    return true;
}

// Construct Lemke tableau with auxilillary variable.
MatrixXd constructTableau(const MatrixXd &M, const VectorXd &q) {
    const int dim = q.size();
    MatrixXd tableau = MatrixXd::Zero(dim, 2*dim + 2); // Initialize with zeros.
    VectorXd auxVar = -VectorXd::Ones(dim); // Auxillary variable z_0 = {-1, -1, ... -1}.
    tableau.topLeftCorner(dim, dim) = MatrixXd::Identity(dim, dim); // Enter identity matrix on left side (w_1, w_2, etc).
    tableau.middleCols(dim, dim) = -M; // Enter I-M (z_1, z_2, etc).
    tableau.col(2*dim) = auxVar; // Enter auxiliary variable.
    tableau.col(2*dim + 1) = q; // Enter q.
    return tableau;
}

// Pivot function.
void pivot(MatrixXd &tableau, const int &row, const int &col) {
    const int dim = tableau.rows();
    double pivotElement = tableau(row, col);
    VectorXd newPivotRow = (1/pivotElement)*tableau.row(row);
    tableau.row(row) = newPivotRow;

    VectorXd newNonPivotRow {};
    for (int i = 0; i < dim; i++) {
        if (i != row) {
            newNonPivotRow = tableau.row(i) - tableau(i, col)*tableau.row(row);
            tableau.row(i) = newNonPivotRow;
        }
    }
}

// Initialize tableau.
int initializeTableau(MatrixXd &tableau) {
    const int dim = tableau.rows();
    const int cols = 2*dim +2;

    // Pivot row of min element of q w.r.t. aux column.
    int minRow {0};
    double minVal = tableau.coeff(minRow, cols-1);
    for (int i = 1; i < dim; i++) {
        if (tableau.coeff(i, cols-1) < minVal) {
            minRow = i;
            minVal = tableau(minRow, cols-1);
        }
    }

    pivot(tableau, minRow, cols-2);

    // Return the next entering variable, which is the complement to the non-basic variable.
    int newPivotCol = minRow + dim;
    return newPivotCol;
}

// Return which variables are basic.
VectorXd basicVars(const MatrixXd &tableau) {
    int cols = tableau.cols();
    double varNorm {};
    VectorXd isBasic = VectorXd::Zero(cols - 1);

    double err {1e-8};
    for (int i = 0; i < cols - 1; i++) {
        varNorm = tableau.col(i).norm();
        if (abs(1 - varNorm) < err) {
            isBasic[i] = true;
        } else {
            isBasic[i] = false;
        }
    }

    return isBasic;
}

// Check for solution.
bool checkSol(const MatrixXd &tableau) {

    // Determine basic variables.
    VectorXd isBasic = basicVars(tableau);

    // Size information and initialize q & z0.
    bool solFound {false};
    int dim = tableau.rows();
    int cols = 2*dim + 2;
    VectorXd q = tableau.col(cols-1);
    VectorXd z0 = tableau.col(cols-2);

    // No solution if any element of q is negative.
    for (int i = 0; i <= dim-1; i++) {
        if (q[i] < 0) {
            return solFound;
        }
    }

    // No solution if auxillary variable z0 is basic.
    if (isBasic[cols-2] == 1) {
        return solFound;
    }

    // If q is positive and z0 is non-basic, solution has been found.
    solFound = true;
    return solFound;
}

// Check for secondary ray termination.
bool checkRayTermination(const MatrixXd &tableu, const int &pivotCol) {
    VectorXd column = tableu.col(pivotCol);
    int negTestSum {0};
    for (int i {0}; i < column.size(); i++) {
        if (column(i) <= 0) {
            negTestSum++;
        }
    }

    if (negTestSum == column.size()) {
        return true;
    } else {
        return false;
    }
}

// Minimum ratio test to determine pivot row.
int minRatioTest(const MatrixXd &tableau, const int &pivotCol) {
    const int dim = tableau.rows();

    VectorXd ratioTest = VectorXd::Zero(dim);
    for (int i = 0; i < dim; i++) {
        ratioTest[i] = tableau.coeff(i, 2*dim + 1) / tableau.coeff(i, pivotCol);
        if (ratioTest[i] < 0) {
            ratioTest[i] = 1e10;
        }
    }

    int pivotRow {0};
    double minRatio = ratioTest[0];
    for (int i = 1; i < dim; i++) {
        if (ratioTest[i] < minRatio) {
            minRatio = ratioTest[i];
            pivotRow = i;
        }
    }

    return pivotRow;
}

// Extract solution from tableau.
MatrixXd extractSolution(const MatrixXd &tableau) {
    const int dim = tableau.rows();
    MatrixXd A = MatrixXd::Zero(dim, 2*dim);
    VectorXd z = VectorXd::Zero(dim);
    VectorXd w = VectorXd::Zero(dim);
    VectorXd isBasic = basicVars(tableau);

    for (int i = 0; i < 2*dim; i++)
        if (isBasic[i] == 1)
            A.col(i) = tableau.col(i);

    for (int i = 0; i < 2*dim; i++) {
        if (i < dim) {
            w[i] = A.col(i).dot(tableau.col(2*dim + 1));
        } else {
            z[i-dim] = A.col(i).dot(tableau.col(2*dim + 1));
        }
    }

    MatrixXd sols = MatrixXd::Zero(dim, 2);
    sols.col(0) = z;
    sols.col(1) = w;
    return sols;
}


// Boost.Geometry
// Unit Test

// Copyright (c) 2019 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2019.
// Modifications copyright (c) 2019, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/detail/make/make.hpp>
#include <boost/geometry/geometries/infinite_line.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/util/math.hpp>

namespace
{
    // Boost.Test does not support BOOST_CHECK_CLOSE for integral types
    template <typename T>
    bool is_small(T const& value)
    {
        static long double const epsilon = 1.0e-5;
        return bg::math::abs(value) < epsilon;
    }
}

template <typename T, typename C>
void verify_point_on_line(bg::model::infinite_line<T> const& line,
                          C const& x, C const& y)
{
    BOOST_CHECK_MESSAGE(is_small(line.a * x + line.b * y + line.c),
                        "Point is not located on the line");
}

template <typename T>
void test_make()
{
    typedef bg::model::infinite_line<T> line_type;

    // Horizontal through origin
    line_type line = bg::detail::make::make_infinite_line<T>(0, 0, 10, 0);
    verify_point_on_line(line, 0, 0);
    verify_point_on_line(line, 10, 0);

    // Horizontal line above origin
    line = bg::detail::make::make_infinite_line<T>(0, 5, 10, 5);
    verify_point_on_line(line, 0, 5);
    verify_point_on_line(line, 10, 5);

    // Vertical through origin
    line = bg::detail::make::make_infinite_line<T>(0, 0, 0, 10);
    verify_point_on_line(line, 0, 0);
    verify_point_on_line(line, 0, 10);

    // Vertical line left from origin
    line = bg::detail::make::make_infinite_line<T>(5, 0, 5, 10);
    verify_point_on_line(line, 5, 0);
    verify_point_on_line(line, 5, 10);

    // Diagonal through origin
    line = bg::detail::make::make_infinite_line<T>(0, 0, 8, 10);
    verify_point_on_line(line, 0, 0);
    verify_point_on_line(line, 8, 10);

    // Diagonal not through origin
    line = bg::detail::make::make_infinite_line<T>(5, 2, -8, 10);
    verify_point_on_line(line, 5, 2);
    verify_point_on_line(line, -8, 10);
}

template <typename T>
void test_all()
{
    test_make<T>();
}

int test_main(int, char* [])
{
    test_all<double>();
    test_all<long double>();
    test_all<float>();
    test_all<int>();
    return 0;
}


/*
 utils.hxx

 Copyright (c) 2018 Guy Skinner

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#pragma once

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/range/numeric.hpp>
#include <functional>
#include <vector>

namespace ublas = boost::numeric::ublas;

double det3(ublas::matrix<double> a);

ublas::vector<double> cross3(ublas::vector<double> a,
                             ublas::vector<double> b);

ublas::matrix<double> inv3(ublas::matrix<double> a);

long linear_search(const std::vector<double>& vec,
		   double a,
		   double tol);

inline long product(ublas::vector<long> vec) {
  return boost::accumulate(vec,1,std::multiplies<long>());
}


//  (C) Copyright John Maddock 2006.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_SPECIAL_LEGENDRE_HPP
#define BOOST_MATH_SPECIAL_LEGENDRE_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/tools/config.hpp>

namespace boost{
namespace math{

// Recurrance relation for legendre P and Q polynomials:
template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type
   legendre_next(unsigned l, T1 x, T2 Pl, T3 Plm1)
{
   typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   return ((2 * l + 1) * result_type(x) * result_type(Pl) - l * result_type(Plm1)) / (l + 1);
}

namespace detail{

// Implement Legendre P and Q polynomials via recurrance:
template <class T, class Policy>
T legendre_imp(unsigned l, T x, const Policy& pol, bool second = false)
{
   static const char* function = "boost::math::legrendre_p<%1%>(unsigned, %1%)";
   // Error handling:
   if((x < -1) || (x > 1))
      return policies::raise_domain_error<T>(
         function,
         "The Legendre Polynomial is defined for"
         " -1 <= x <= 1, but got x = %1%.", x, pol);

   T p0, p1;
   if(second)
   {
      // A solution of the second kind (Q):
      p0 = (boost::math::log1p(x, pol) - boost::math::log1p(-x, pol)) / 2;
      p1 = x * p0 - 1;
   }
   else
   {
      // A solution of the first kind (P):
      p0 = 1;
      p1 = x;
   }
   if(l == 0)
      return p0;

   unsigned n = 1;

   while(n < l)
   {
      std::swap(p0, p1);
      p1 = boost::math::legendre_next(n, x, p0, p1);
      ++n;
   }
   return p1;
}

} // namespace detail

template <class T, class Policy>
inline typename boost::enable_if_c<policies::is_policy<Policy>::value, typename tools::promote_args<T>::type>::type
   legendre_p(int l, T x, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   static const char* function = "boost::math::legendre_p<%1%>(unsigned, %1%)";
   if(l < 0)
      return policies::checked_narrowing_cast<result_type, Policy>(detail::legendre_imp(-l-1, static_cast<value_type>(x), pol, false), function);
   return policies::checked_narrowing_cast<result_type, Policy>(detail::legendre_imp(l, static_cast<value_type>(x), pol, false), function);
}

template <class T>
inline typename tools::promote_args<T>::type
   legendre_p(int l, T x)
{
   return boost::math::legendre_p(l, x, policies::policy<>());
}

template <class T, class Policy>
inline typename boost::enable_if_c<policies::is_policy<Policy>::value, typename tools::promote_args<T>::type>::type
   legendre_q(unsigned l, T x, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::legendre_imp(l, static_cast<value_type>(x), pol, true), "boost::math::legendre_q<%1%>(unsigned, %1%)");
}

template <class T>
inline typename tools::promote_args<T>::type
   legendre_q(unsigned l, T x)
{
   return boost::math::legendre_q(l, x, policies::policy<>());
}

// Recurrence for associated polynomials:
template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type
   legendre_next(unsigned l, unsigned m, T1 x, T2 Pl, T3 Plm1)
{
   typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   return ((2 * l + 1) * result_type(x) * result_type(Pl) - (l + m) * result_type(Plm1)) / (l + 1 - m);
}

namespace detail{
// Legendre P associated polynomial:
template <class T, class Policy>
T legendre_p_imp(int l, int m, T x, T sin_theta_power, const Policy& pol)
{
   // Error handling:
   if((x < -1) || (x > 1))
      return policies::raise_domain_error<T>(
      "boost::math::legendre_p<%1%>(int, int, %1%)",
         "The associated Legendre Polynomial is defined for"
         " -1 <= x <= 1, but got x = %1%.", x, pol);
   // Handle negative arguments first:
   if(l < 0)
      return legendre_p_imp(-l-1, m, x, sin_theta_power, pol);
   if(m < 0)
   {
      int sign = (m&1) ? -1 : 1;
      return sign * boost::math::tgamma_ratio(static_cast<T>(l+m+1), static_cast<T>(l+1-m), pol) * legendre_p_imp(l, -m, x, sin_theta_power, pol);
   }
   // Special cases:
   if(m > l)
      return 0;
   if(m == 0)
      return boost::math::legendre_p(l, x, pol);

   T p0 = boost::math::double_factorial<T>(2 * m - 1, pol) * sin_theta_power;

   if(m&1)
      p0 *= -1;
   if(m == l)
      return p0;

   T p1 = x * (2 * m + 1) * p0;

   int n = m + 1;

   while(n < l)
   {
      std::swap(p0, p1);
      p1 = boost::math::legendre_next(n, m, x, p0, p1);
      ++n;
   }
   return p1;
}

template <class T, class Policy>
inline T legendre_p_imp(int l, int m, T x, const Policy& pol)
{
   BOOST_MATH_STD_USING
   // TODO: we really could use that mythical "pow1p" function here:
   return legendre_p_imp(l, m, x, static_cast<T>(pow(1 - x*x, T(abs(m))/2)), pol);
}

}

template <class T, class Policy>
inline typename tools::promote_args<T>::type
   legendre_p(int l, int m, T x, const Policy& pol)
{
   typedef typename tools::promote_args<T>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(detail::legendre_p_imp(l, m, static_cast<value_type>(x), pol), "bost::math::legendre_p<%1%>(int, int, %1%)");
}

template <class T>
inline typename tools::promote_args<T>::type
   legendre_p(int l, int m, T x)
{
   return boost::math::legendre_p(l, m, x, policies::policy<>());
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_SPECIAL_LEGENDRE_HPP


#include "srrg_geometry/geometry3d.h"
#include <iostream>
#include <Eigen/StdVector>
#include "srrg_autodiff/ad_multivariate.h"

using namespace std;
using namespace srrg2_core;
using namespace srrg2_core::geometry3d;

using namespace Eigen;

typedef srrg2_core::Vector2ad<float> Vector2adf;
typedef srrg2_core::Vector3ad<float> Vector3adf;
typedef srrg2_core::Vector6ad<float> Vector6adf;
typedef srrg2_core::Matrix3ad<float> Matrix3adf;
typedef srrg2_core::DualValue_<float> DualValuef;
typedef srrg2_core::Isometry3ad<float> Isometry3adf;

template <typename Scalar_>
class TransformPoint: public MultivariateFunction<Scalar_, 6, 3>{
public:
  void operator()(Scalar_* output, const Scalar_* input){

    // this maps the memory area in the input array to an
    // Eigen vector of dimension 6
    // if you feel uncomfy, you can
    // allocate an array
    //   Vector6<Scalar_> robot_pose;
    // fill the elements with a for loop
    //    for (int i=0; i<6; i++) robot_pose[i]=input[i];
    Eigen::Map<const Eigen::Matrix<Scalar_,6,1> > robot_pose(input);

    // this maps the memory area in the output array to an
    // Eigen vector of dimension 3
    // Changing the Eigen object would result
    // in doing side effect to the memory area
    Eigen::Map<Eigen::Matrix<Scalar_,3,1> > projected_point(output);

    // compute the rotation matrix and translation vector
    // encoded in robot_pose
    Isometry3_<Scalar_> robot_pose_matrix=v2t<Scalar_>(robot_pose);

    // compute the positionof the point w.r.t. the world,
    // by multiplying it by a transformation matrix
    projected_point=robot_pose_matrix*point;
  }

  // this is the parameter
  Eigen::Matrix<Scalar_, 3, 1> point;
};

// this is our function that supports the multivariate autodiff
ADMultivariateFunction<double, TransformPoint> ad_project_point;

template <typename Scalar_>
class TransformMovingPoint: public MultivariateFunction<Scalar_, 9, 3>{
public:
  void operator()(Scalar_* output, const Scalar_* input){

    // this maps the memory area in the input array to an
    // Eigen vector of dimension 6
    // if you feel uncomfy, you can
    // allocate an array
    //   Vector6<Scalar_> robot_pose;
    // fill the elements with a for loop
    //    for (int i=0; i<6; i++) robot_pose[i]=input[i];
    Eigen::Map<const Eigen::Matrix<Scalar_,6,1> > robot_pose(input);
    Eigen::Map<const Eigen::Matrix<Scalar_,3,1> > point(input+6);

    // this maps the memory area in the output array to an
    // Eigen vector of dimension 3
    // Changing the Eigen object would result
    // in doing side effect to the memory area
    Eigen::Map<Eigen::Matrix<Scalar_,3,1> > projected_point(output);

    // compute the rotation matrix and translation vector
    // encoded in robot_pose
    Isometry3_<Scalar_> robot_pose_matrix=v2t<Scalar_>(robot_pose);

    // compute the positionof the point w.r.t. the world,
    // by multiplying it by a transformation matrix
    projected_point=robot_pose_matrix*point;
  }
};

template <typename Scalar_>
class TransformMovingPoint2: public MultivariateFunction<Scalar_, 9, 3>{
public:
  void operator()(Scalar_* output, const Scalar_* input){

    // this maps the memory area in the input array to an
    // Eigen vector of dimension 6
    // if you feel uncomfy, you can
    // allocate an array
    //   Vector6<Scalar_> robot_pose;
    // fill the elements with a for loop
    //    for (int i=0; i<6; i++) robot_pose[i]=input[i];
    Eigen::Map<const Eigen::Matrix<Scalar_,6,1> > robot_pose_pert(input);
    Eigen::Map<const Eigen::Matrix<Scalar_,3,1> > point_pert(input+6);

    // this maps the memory area in the output array to an
    // Eigen vector of dimension 3
    // Changing the Eigen object would result
    // in doing side effect to the memory area
    Eigen::Map<Eigen::Matrix<Scalar_,3,1> > projected_point(output);

    // compute the rotation matrix and translation vector
    // encoded in robot_pose
    Isometry3_<Scalar_> robot_pose_matrix=v2t<Scalar_>(robot_pose_pert)*_robot_pose;

    // compute the positionof the point w.r.t. the world,
    // by multiplying it by a transformation matrix
    projected_point=robot_pose_matrix*(_point+point_pert);
  }
  Isometry3_<Scalar_> _robot_pose;
  Vector3_<Scalar_> _point;
};

// this is our function that supports the multivariate autodiff
ADMultivariateFunction<double, TransformMovingPoint> ad_move_point;

ADMultivariateFunction<double, TransformMovingPoint2> ad_move_point_manifold;

int main(int argc, char** argv){
  ad_project_point.point<<1,2,3;
  ad_project_point.point<<1,2,3;

  Eigen::Matrix<double, 6, 1> v;
  v << 0,0,0,0,0,0;

  Eigen::Matrix<double, 3, 1> output;
  Eigen::Matrix<double, 3, 6> jacobian;

  ad_project_point(&output[0], &v[0]);
  jacobian=ad_project_point.jacobian(&v[0]);

  cerr << "output: " << endl;
  cerr << output.transpose() << endl;
  cerr << "jacobian: " << endl;
  cerr << jacobian << endl;

  cerr << endl  << endl;
  Eigen::Matrix<double, 9, 1> pose_and_point;
  Eigen::Matrix<double, 3, 9> jacobian_move;
  pose_and_point.head<6>()=v;
  pose_and_point.tail<3>() << 1,2,3;
  cerr << "pose_and_point: " <<pose_and_point.transpose() << endl;
  ad_move_point(&output[0], &pose_and_point[0]);
  jacobian_move=ad_move_point.jacobian(&pose_and_point[0]);
  cerr << "jacobian: " << endl;
  cerr << jacobian_move << endl;

  cerr << "pose_and_point (zero pert): " <<pose_and_point.transpose() << endl;
  ad_move_point_manifold._robot_pose.setIdentity();
  ad_move_point_manifold._point << 1,2,3;
  ad_move_point_manifold(&output[0]);
  jacobian_move=ad_move_point_manifold.jacobian();
  cerr << "jacobian: " << endl;
  cerr << jacobian_move << endl;

}


// SGFilter.h
//
// Implenetation of the Savitzky-Golay Filter in cpp (Arduino/Teensy)
//

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <unistd.h>
#include <iostream>

namespace openvslam
{

    class SGFilter
    {
	public:
        struct DATA
		{
	    float yRaw = 0.0F;
            float y = 0.0F;
            float yDot = 0.0F;
		};

        // Methods
        SGFilter(uint32_t poly_order, uint32_t filter_size);

        void update(float new_time_float, float new_val);
        void debug_print_A(void);
        float powFast(float x, uint32_t n);

        // Attributes
        uint32_t poly_order_;
        uint32_t filter_size_;

        uint32_t filter_iter_;
        uint32_t current_line_;

        float zero_time_;
        float filter_value_;

        Eigen::VectorXf f_; // Vector of function values
        Eigen::VectorXf t_; // Vector of time slots
        Eigen::MatrixXf A_; // Matrix of Least-Squares coefficients
        Eigen::VectorXf c_; // Vector of Polynomial coefficients
        DATA data_;
    };
}


/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_SDK_MEMORY_META_IS_POWER_OF_2_HPP_INCLUDED
#define NT2_SDK_MEMORY_META_IS_POWER_OF_2_HPP_INCLUDED

#include <cstddef>
#include <boost/mpl/bool.hpp>

namespace nt2 { namespace meta
{
  //////////////////////////////////////////////////////////////////////////////
  // Boolean meta-function checking if a Integral Constant is a power of 2
  // Documentation: is_power_of_2.rst
  //////////////////////////////////////////////////////////////////////////////
  template<std::size_t N>
  struct is_power_of_2_c : boost::mpl::bool_<(!(N & (N - 1)) && N)> {};

  //////////////////////////////////////////////////////////////////////////////
  // Boolean meta-function checking if a compile-time integral is a power of 2
  // Documentation: is_power_of_2_c.rst
  //////////////////////////////////////////////////////////////////////////////
  template<class N>
  struct is_power_of_2 : is_power_of_2_c<N::value> {};
} }

#endif


#include "InfiniteRoots.h"
#include "HomotopyContinuation.h"
#include <boost/math/tools/polynomial.hpp>

using namespace boost::math;
using namespace boost::math::tools; // for polynomial

InfiniteRoots::InfiniteRoots(int L, int M) {
    _L = L;
    _M = M;
    _roots.resize(M);
}

std::vector<var_t>& InfiniteRoots::getRoot(int m)
{
    if (_roots[m-1].size() == 0) {
        computeRoots(m);
    }

    return _roots[m-1];
}

std::vector<var_t> InfiniteRoots::equationSolver(std::vector<var_t> &coefs, std::vector<var_t> &out) {
    if (coefs.size() == 2) {
        return { -coefs[0]/coefs[1] };
    }
    if (coefs.size() <= 1) {
        return {};
    }

    PolynomialFunction target(coefs);
    UnityEquation start(1, coefs.size() - 1);

    SimpleHomotopy hom(&start, &target);
    hom.setSteps(coefs.size() * 300);
    hom.setRandSeed(coefs.size());
    SimpleHomotopyContinuation hc;

    std::vector<var_t> ret;

    for (int i = 0; i < start.numberOfRoots(); i++) {
        Vector root = start.getRoot(i);
        Solution sol(root);
        hc.solve(hom, sol);
        bool found = false;
        for (int j = 0; j < ret.size(); j++) {
            if (std::abs(ret[j] - sol.get(0)) < EPS) {
                found = true;
                break;
            }
        }
        if (!found) {
            ret.push_back(sol.get(0));
        }
    }

    // it produced some duplicated roots
    if (ret.size() < start.numberOfRoots()) {
        polynomial<var_t> eq(coefs.begin(), coefs.end());
        polynomial<var_t> eq2 {{var_t(1.0L, 0.0L)}};
        for (int i = 0; i < ret.size(); i++) {
            polynomial<var_t> solved {{-ret[i], var_t(1.0L, 0)}};
            eq2 *= solved;
        }
        polynomial<var_t> eq3 = eq / eq2;
        out = eq3.data();
    }
    return ret;
}

void InfiniteRoots::computeRoots(int m) {
    std::vector<var_t> coefs(m + 1);
    elem_t r = 1.0L;
    int R = _L - 2 * _M + 2 * m;
    for (int i = 0; i <= m; i++) {
        coefs[i] = var_t(r, 0.0L);
        r *= -(m-i) * (2 * _L) / (elem_t)((R - i) *  (i+1));
    }
    reverse(coefs.begin(), coefs.end());
    while (true) {
        std::vector<var_t> remain;
        std::vector<var_t> res = equationSolver(coefs, remain);
        _roots[m-1].insert(_roots[m-1].end(), res.begin(), res.end());
//        std::cout << "roots=" << _roots[m-1] << " remain=" << remain << std::endl;
        if (remain.size() <= 1) break;
        coefs = remain;
    }
}


#include <SDKApplication.hpp>

#include <float.h>
#include <vector>
#include <map>

#include <boost/program_options.hpp>

#include <clutil.h>
#include <CMat.h>

using namespace std;

#define PROGRAM_FILE "matvec.cl"
#define KERNEL_FUNC "matvec_mult"

#define NROUND 64

bool Near(float t1, float t2, float div = 10000)
{
    return  fabs(t1-t2) < t1/div;
}

class MatVec: public SDKSample
{
public:
    ClContextPtr   pContext;
    ClKernelPtr pKernel, pKrnMult, pKrnSum, pKernelMatMat;
    ClCmdQPtr   pCmdQ;
    ClProgramPtr pProgram;
    ClDevice     *pDevice;
    const cl_uint M, N, K;
    cl_uint       RM, RN, RK;
    CMat<float> mat, vec, result, correct;
    ClMemPtr mat_mem, vec_mem, res_mem;
    int timer;
    cl_double gpuTime, cpuTime;
    MatVec(const char *name):
        SDKSample(name)
//            , M(8), N(10), K(10)
      ,M(1024), N(1024), K(1024)
      , RM(clCeiling<NROUND>(M)), RN(clCeiling<NROUND>(N)), RK(clCeiling<NROUND>(K))
      , mat((unsigned long)RM,(unsigned long)RN)
      , vec((unsigned long)RN, (unsigned long)RK)
      , result((unsigned long)RM, (unsigned long)RK)
      , correct((unsigned long)M, (unsigned long)K){}

    virtual int setup()
    {
        std::vector<ClKernelPtr> vecKrn;
        vecKrn = ClPlatform::createSimpleKernel(std::cout, platformId, PROGRAM_FILE, "matmat_mult");
        if(vecKrn.size() == 0) {
            printf("Failed to create kernel!\n");
            return SDK_FAILURE;
        }
        pKernel = vecKrn[0];
        pProgram = pKernel->pProgram;
        pContext =  pProgram->pContext;
        pDevice = pContext->devices[deviceId];
        pCmdQ = pContext->createCmdQ(pDevice);
        if( sizeof(float)*(M*N+N*K+M*K) >= pContext->devices[deviceId]->maxMemAllocSize ) {
            printf("Larger than device maxMemAllocSize %ld >= %ld\n", sizeof(float)*(M*N+N*K+M*K), pDevice->maxMemAllocSize);
            return SDK_FAILURE;
        }
        if( NULL == (pKernelMatMat = pProgram->createKernel("matmat_mult") ) ) {
            cout << "failed to created kernel matmat_mult" << endl;
            return SDK_FAILURE;
        }

        if( NULL == (pKrnMult = pProgram->createKernel("matvecmult_mult") ) ) {
            cout << "failed to created kernel matvecmult_mult" << endl;
            return SDK_FAILURE;
        }
        if( NULL == (pKrnSum = pProgram->createKernel("matvecmult_sum") ) ) {
            cout << "failed to created kernel matvecmult_sum" << endl;
            return SDK_FAILURE;
        }

        /* Initialize data to be processed by the kernel */
        mat = (float)0.0;
        vec = (float)0.0;
        for(int i=0; i<N; ++i) {
            for(int j=0; j<M; ++j)
                mat(j,i)  = (i+j) *2.0;
            for(int j=0; j<K; ++j)
                vec(i,j) = i*3.0;
        }
        timer = sampleCommon->createTimer();
        sampleCommon->resetTimer(timer);

        sampleCommon->startTimer(timer);
        correct = mat * vec;
        double t = mat(0,0) *vec(0,0);
//        for(int i=0;i<M; ++i)
//            for(int j=0; j<K; ++j) {
//                float *cor = &correct(i,j), *a = &mat(i,0);
//                cor[0] = 0;
//                for(int k=0; k<N; ++k, ++a)
//                    cor[0] += a[0] * vec.ptr()[k*N + j];
//            }
        sampleCommon->stopTimer(timer);
        cpuTime = sampleCommon->readTimer(timer);
        sampleCommon->resetTimer(timer);
        cout << "Matrix size: (" <<M << "," <<N <<"," <<K << ")    CPU time:" << cpuTime << endl;

        mat_mem = pContext->createMem(CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR, sizeof(float)*RM*RN, &mat[0]);
        if( ! mat_mem ) {
            printf("Failed to create mat_mem!\n");
            return SDK_FAILURE;
        }
        vec_mem = pContext->createMem(CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, sizeof(float)*RN*RK, &vec[0]);
        res_mem = pContext->createMem(CL_MEM_WRITE_ONLY, sizeof(float)*RM*RK, NULL);
        if( ! res_mem ) {
            printf("Failed to create res_mem!\n");
            return SDK_FAILURE;
        }

        return SDK_SUCCESS;

    }

    virtual int initialize()
    {
        // Call base class Initialize to get default configuration
        if(this->SDKSample::initialize())
            return SDK_FAILURE;
        return SDK_SUCCESS;
    }
    int runMatVec()
    {
        size_t globalsize;
        sampleCommon->startTimer(timer);
        if( CL_SUCCESS  != pKernel->setArgs(0, mat_mem, vec_mem, res_mem, M, N) ) {
            printf("Failed to set arg!\n");
            return SDK_FAILURE;
        }
        globalsize = M;
        cl_event evts[3];
        pCmdQ->enqueue(pKernel, 1, NULL, &globalsize, NULL, 0, NULL, &evts[0] );
        pCmdQ->enqueueRead(res_mem, CL_FALSE, 0, sizeof(float)*M, &result[0], 1, &evts[0], &evts[1]);
        clWaitForEvents(1, &evts[1]);
        for(int i=0; i < 2; ++i)
            clReleaseEvent(evts[i]);

        sampleCommon->stopTimer(timer);
        cl_double gpuTime = sampleCommon->readTimer(timer);
        cout << " Device Timing:" << gpuTime << endl;
        return SDK_SUCCESS;
    }
    int runMatMat()
    {
        cl_int status=CL_SUCCESS;
        bool USE_MAP = true;

        const size_t NDIM = 2;
        size_t globalsize[NDIM] = {RM/4, RK/4};
        size_t localsize[NDIM] = {16, 16};

        ExecKernel exKrn(pKernelMatMat, pCmdQ, NDIM, globalsize, localsize);
        MapMem     exMap(pKernelMatMat, pCmdQ, res_mem);
        ReadWriteMem    exRead(pKernelMatMat, pCmdQ, res_mem, &result[0], true);

        status = pKernelMatMat->validSizes(deviceId, NDIM, globalsize, localsize);
        CHECK_OPENCL_ERROR(status, "invalid localsize");

        sampleCommon->resetTimer(timer);
        sampleCommon->startTimer(timer);
        status = pKernelMatMat->setArgs(0, mat_mem, vec_mem, res_mem, RM, RN, RK);
        CHECK_OPENCL_ERROR(status, "Failed to set Args");
        CHECK_OPENCL_ERROR(exKrn(), "Failed to execute kernel");

        if( !USE_MAP ) {
            CHECK_OPENCL_ERROR(exRead(), "Failed to read mem");
        }else{

            CHECK_OPENCL_ERROR(exMap.map(), "Failed to map read mem");
            memcpy(&result[0], exMap.ptr, sizeof(cl_float) * RM  * RK);
            CHECK_OPENCL_ERROR(exMap.unmap(), "Failed to unmap read mem");
        }

        sampleCommon->stopTimer(timer);
        cl_double gpuTime = sampleCommon->readTimer(timer);
        cout << " Device Timing:" << gpuTime << endl;
        return SDK_SUCCESS;
    }
    int runMultiStage()
    {
        size_t globalsize;
        /* Data and buffers */
        if( CL_SUCCESS  != pKrnMult->setArgs(0, mat_mem, vec_mem,  M, N) )
        {
            printf("Failed to set arg!\n");
            return SDK_FAILURE;
        }
        if( CL_SUCCESS  != pKrnSum->setArgs(0, mat_mem, res_mem,  M, N) )
        {
            printf("Failed to set arg!\n");
            return SDK_FAILURE;
        }

        globalsize= M*N;
        cl_event evts[3];

        pCmdQ->enqueue(pKrnMult, 1, NULL, &globalsize, NULL, 0, NULL, &evts[0]);
        globalsize= M;
        pCmdQ->enqueue(pKrnSum, 1, NULL, &globalsize, NULL, 1, &evts[0], &evts[1]);

        pCmdQ->enqueueRead(res_mem, CL_TRUE, 0, sizeof(float)*M, &result[0], 2, &evts[0], &evts[2]);

        pCmdQ->flush();

        clWaitForEvents(1, &evts[2]);
        for(int i=0; i < 3; ++i)
            clReleaseEvent(evts[i]);

        sampleCommon->stopTimer(timer);
        gpuTime = sampleCommon->readTimer(timer);
        cout << " Device Timing:" << gpuTime << endl;
        return SDK_SUCCESS;
    }

    virtual int run() {
        if( K == 1 ){
            //            return runMatVec();
            //            return runMultiStage();
            return runMatMat();
        }else{
            return runMatMat();
        }
    }

    virtual int verifyResults(){
        bool ok = true;
        for(int i=0;i<M; ++i) {
            for(int j=0;j<K; ++j)
                if( !Near(result(i,j), correct(i,j)) ) {
                    ok = false;
                    printf("(%d,%d) ERROR %f != %f \n", i, j,result(i,j), correct(i,j));
                }else{
//                    printf("(%d,%d) ERROR %f = %f \n", i, j,result(i,j), correct(i,j));
                }
        }
        if( ok) printf("GPU computation is correct!\n");
        return SDK_SUCCESS;
    }
    virtual int genBinaryImage(){
        return SDK_SUCCESS;
    }
    virtual int cleanup(){
        return SDK_SUCCESS;
    }
    void printStats()
    {
    }
};

int main(int argc, char *argv[])
{
//    ClPlatform::createSimpleKernel(std::cout);
    MatVec app("OpenCL MatVec");

    if(app.initialize() != SDK_SUCCESS)
        return SDK_FAILURE;

    if(app.parseCommandLine(argc, argv) != SDK_SUCCESS)
        return SDK_FAILURE;

    if(app.isDumpBinaryEnabled())
    {
        return app.genBinaryImage();
    }
    else
    {
        if(app.setup() != SDK_SUCCESS)
            return SDK_FAILURE;

        if(app.run() != SDK_SUCCESS)
            return SDK_FAILURE;

        if(app.verifyResults() != SDK_SUCCESS)
            return SDK_FAILURE;

        if(app.cleanup() != SDK_SUCCESS)
            return SDK_FAILURE;

        app.printStats();
    }

    return SDK_SUCCESS;

}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"
#include <Eigen/SVD>

template<typename MatrixType> void svd(const MatrixType& m)
{
  /* this test covers the following files:
     SVD.h
  */
  int rows = m.rows();
  int cols = m.cols();

  typedef typename MatrixType::Scalar Scalar;
  typedef typename NumTraits<Scalar>::Real RealScalar;
  MatrixType a = MatrixType::Random(rows,cols);
  Matrix<Scalar, MatrixType::RowsAtCompileTime, 1> b =
    Matrix<Scalar, MatrixType::RowsAtCompileTime, 1>::Random(rows,1);
  Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> x(cols,1), x2(cols,1);

  RealScalar largerEps = test_precision<RealScalar>();
  if (ei_is_same_type<RealScalar,float>::ret)
    largerEps = 1e-3f;

  {
    SVD<MatrixType> svd(a);
    MatrixType sigma = MatrixType::Zero(rows,cols);
    MatrixType matU  = MatrixType::Zero(rows,rows);
    sigma.block(0,0,cols,cols) = svd.singularValues().asDiagonal();
    matU.block(0,0,rows,cols) = svd.matrixU();
    VERIFY_IS_APPROX(a, matU * sigma * svd.matrixV().transpose());
  }

  if (rows==cols)
  {
    if (ei_is_same_type<RealScalar,float>::ret)
    {
      MatrixType a1 = MatrixType::Random(rows,cols);
      a += a * a.adjoint() + a1 * a1.adjoint();
    }
    SVD<MatrixType> svd(a);
    svd.solve(b, &x);
    VERIFY_IS_APPROX(a * x,b);
  }

  if(rows==cols)
  {
    SVD<MatrixType> svd(a);
    MatrixType unitary, positive;
    svd.computeUnitaryPositive(&unitary, &positive);
    VERIFY_IS_APPROX(unitary * unitary.adjoint(), MatrixType::Identity(unitary.rows(),unitary.rows()));
    VERIFY_IS_APPROX(positive, positive.adjoint());
    for(int i = 0; i < rows; i++) VERIFY(positive.diagonal()[i] >= 0); // cheap necessary (not sufficient) condition for positivity
    VERIFY_IS_APPROX(unitary*positive, a);

    svd.computePositiveUnitary(&positive, &unitary);
    VERIFY_IS_APPROX(unitary * unitary.adjoint(), MatrixType::Identity(unitary.rows(),unitary.rows()));
    VERIFY_IS_APPROX(positive, positive.adjoint());
    for(int i = 0; i < rows; i++) VERIFY(positive.diagonal()[i] >= 0); // cheap necessary (not sufficient) condition for positivity
    VERIFY_IS_APPROX(positive*unitary, a);
  }
}

void test_eigen2_svd()
{
  for(int i = 0; i < g_repeat; i++) {
    CALL_SUBTEST_1( svd(Matrix3f()) );
    CALL_SUBTEST_2( svd(Matrix4d()) );
    CALL_SUBTEST_3( svd(MatrixXf(7,7)) );
    CALL_SUBTEST_4( svd(MatrixXd(14,7)) );
    // complex are not implemented yet
//     CALL_SUBTEST( svd(MatrixXcd(6,6)) );
//     CALL_SUBTEST( svd(MatrixXcf(3,3)) );
    SVD<MatrixXf> s;
    MatrixXf m = MatrixXf::Random(10,1);
    s.compute(m);
  }
}


// Copyright (c) 2014 Max Planck Society

#include "math_tools.h"
#include <Eigen/Dense>
#include <Eigen/Core>
#include <stdexcept>
#include <cmath>
#include <cstdint>

namespace math_tools {

Eigen::MatrixXd squareDistance(const Eigen::MatrixXd& a,
                               const Eigen::MatrixXd& b) {
  int aRows = a.rows();
  int aCols = a.cols();
  int bCols = b.cols();

  Eigen::MatrixXd am(aRows, aCols);    // mean-corrected a
  Eigen::MatrixXd bm(aRows, bCols);    // mean-corrected b
  // final result, aCols x bCols
  Eigen::MatrixXd result(aCols, bCols);

  Eigen::VectorXd mean(aRows);

  /*  If the two Matrices have the same address, it means the function was
      called from the overloaded version, and thus the mean only has to be
      computed once.
   */
  if (&a == &b) {  // Same address?
    mean = a.rowwise().mean();
    am = a.colwise() - mean;
    bm = am;
  } else {
    if (aRows != b.rows()) {
      throw std::runtime_error("Matrix dimension incorrect.");
    }

    mean = static_cast<double>(aCols) / (aCols + bCols) * a.rowwise().mean() +
      static_cast<double>(bCols) / (bCols + aCols) * b.rowwise().mean();

    // The mean of the two Matrices is subtracted beforehand, because the
    // squared error is independent of the mean and this makes the squares
    // smaller.
    am = a.colwise() - mean;
    bm = b.colwise() - mean;
  }

  Eigen::MatrixXd a_square =
    am.array().square().colwise()
    .sum().transpose().rowwise() .replicate(bCols);

  Eigen::MatrixXd b_square = bm.array().square().colwise().sum().colwise()
                             .replicate(aCols);

  Eigen::MatrixXd twoab = 2 * (am.transpose()) * bm;

  return (a_square.matrix() + b_square.matrix()) - twoab;
}

Eigen::MatrixXd squareDistance(const Eigen::MatrixXd& a) {
  return squareDistance(a, a);
}

Eigen::MatrixXd generate_random_sequence(int d, int n) {
  // x = randn(d,1); % starting sample
  Eigen::VectorXd x = math_tools::generate_normal_random_matrix(d, 1);

  Eigen::VectorXd t = math_tools::generate_normal_random_matrix(d, 1);

  return generate_random_sequence(n, x, t);
}

Eigen::MatrixXd generate_random_sequence(int n, Eigen::VectorXd x,
    Eigen::VectorXd t) {
// function X = GPanimation(d,n)
// % returns a matrix X of size [d,n], representing a grand circle on the
// % unit d-sphere in n steps, starting at a random location. Given a kernel
// % matrix K, this can be turned into a tour through the sample space, simply
// % by calling chol(K)’ * X;
// %
// % Philipp Hennig, September 2012

// r = sqrt(sum(x.^2));
  double r = std::sqrt(x.transpose() * x);

// x = x ./ r; % project onto sphere
  x = x / r;

// t = randn(d,1); % sample tangent direction

// t = t - (t'*x) * x; % orthogonalise by Gram-Schmidt.
  double tmp = t.adjoint() * x;
  t = t - tmp * x;

// t = t ./ sqrt(sum(t.^2)); % standardise
  t = t / std::sqrt(t.transpose() * t);

// s = linspace(0,2*pi,n+1); s = s(1:end-1); % space to span
  Eigen::VectorXd s(n + 1);
  s.setLinSpaced(n + 1, 0, 2 * M_PI);
  s.conservativeResize(s.rows() - 1);

// t = bsxfun(@times,s,t); % span linspace in direction of t
//     std::cout << (s.transpose().replicate(t.rows(),1)).format(OctaveFmt) <<
// std::endl;
//     std::cout << (t.replicate(1,s.rows())).format(OctaveFmt) <<
// std::endl;

  Eigen::MatrixXd T = s.transpose().replicate(t.rows(), 1)
                      .cwiseProduct(t.replicate(1, s.rows()));

// X = r.* exp_map(x,t); % project onto sphere, re-scale
  Eigen::MatrixXd X = r * exp_map(x, T);
// end
  return X;
}

Eigen::MatrixXd exp_map(const Eigen::VectorXd& mu, const Eigen::MatrixXd& E) {
// D = size(E,1);
  int D = E.rows();

// theta = sqrt(sum((E.^2)));
  Eigen::MatrixXd theta = E.array().pow(2).colwise().sum().sqrt();

// M = mu * cos(theta) + E .* repmat(sin(theta)./theta, D, 1);
  Eigen::MatrixXd M = mu * theta.array().cos().matrix() +
                      E.cwiseProduct(
                        (theta.array().sin() / theta.array())
                        .matrix().replicate(D, 1));

// if (any (abs (theta) <= 1e-7))
// for a = find (abs (theta) <= 1e-7)
// M (:, a) = mu;
// end % for
// end % if
  for (int i = 0; i < theta.cols(); i++) {
    if (theta(0, i) < MINIMAL_THETA) {
      M.col(i) = mu;
    }
  }

// end % function
  return M;
}

Eigen::MatrixXd generate_uniform_random_matrix_0_1(
    const size_t n,
    const size_t m) {
  Eigen::MatrixXd result = Eigen::MatrixXd(n, m);
  result.setRandom();
  Eigen::MatrixXd temp = result.array() + 1;
  result = temp / 2.0;
  result = result.array().max(1e-10);
  result = result.array().min(1.0);
  return result;
}

Eigen::MatrixXd box_muller(const Eigen::VectorXd &vRand) {
  size_t n = vRand.rows();
  size_t m = n / 2;

  Eigen::ArrayXd rand1 = vRand.head(m);
  Eigen::ArrayXd rand2 = vRand.tail(m);

  /* Implemented according to
   * http://en.wikipedia.org/wiki/Box%E2%80%93Muller_transform
   */

  rand1 = rand1.max(1e-10);
  rand1 = rand1.min(1.0);

  rand1 = -2 * rand1.log();
  rand1 = rand1.sqrt();

  rand2 = rand2 * 2 * M_PI;

  Eigen::MatrixXd result(2 * m, 1);
  Eigen::MatrixXd res1 = (rand1 * rand2.cos()).matrix();
  Eigen::MatrixXd res2 = (rand1 * rand2.sin()).matrix();
  result << res1, res2;

  return result;
}

Eigen::MatrixXd generate_normal_random_matrix(
    const size_t n,
    const size_t m) {
  // if n*m is odd, we need one random number extra!
  // therefore, we have to round up here.
  size_t N = static_cast<size_t>(std::ceil(n * m / 2.0));

  Eigen::MatrixXd result(2 * N, 1);
  // push random samples through the Box-Muller transform
  result = box_muller(generate_uniform_random_matrix_0_1(2 * N, 1));
  result.conservativeResize(n, m);
  return result;
}

double generate_normal_random_double() {
  Eigen::MatrixXd randomMatrix = generate_normal_random_matrix(1, 1);
  return randomMatrix(0,0);
}

}  // namespace math_tools


#include <iostream>
#include <vector>
#include <string>
#include "jefflib.h"
#include <boost/tokenizer.hpp>

using namespace std;

int getHundred(int x){
    string s = to_string(x);
    if(s.size()>= 3){
        return stoi(s.substr(s.size() - 3, 1));
    }
    else{
        return 0;
    }
}

int main()
{
    int rack = 0;
    int pwr = 0;
    int maxPwr = 0;
    int maxX = 0;
    int maxY = 0;
    int serialNum = 2568;
    int grid[301][301];

    for(int x = 1; x <= 300; x++){
        for(int y = 1; y <= 300; y++){
            rack = x + 10;
            pwr = rack * y;
            pwr += serialNum;
            pwr *= rack;
            pwr = getHundred(pwr) - 5;
            grid[x][y] = pwr;
        }
    }

    for(int x = 1; x <= 298; x++){
        for(int y = 1; y <= 298; y++){
            pwr = grid[x][y] + grid[x+1][y] + grid[x+2][y];
            pwr = pwr + grid[x][y+1] + grid[x+1][y+1] + grid[x+2][y+1];
            pwr = pwr + grid[x][y+2] + grid[x+1][y+2] + grid[x+2][y+2];
            if(pwr > maxPwr){
                maxPwr = pwr;
                maxX = x;
                maxY = y;
            }
        }
    }
    cout << "Part 1: Max power cell = " << maxX << ", " << maxY << endl;

    // Part 2
    maxPwr = 0;
    int maxSz = 0;
    for(int x = 1; x <= 300; x++){
        cout << x << ": ";
        for(int y = 1; y <= 300; y++){
            cout << y;
            // For each cell, calc power for all the valid square sizes
            int maxDim = 0;
            int maxXDim = 300 - x + 1;
            int maxYDim = 300 - y + 1;
            if(maxXDim > maxYDim){
                maxDim = maxYDim;
            }
            else{
                maxDim = maxXDim;
            }

            for(int sz = 1; sz <= maxDim; sz++){
                pwr = 0;
                for(int row = 0; row < sz; row++){
                    for(int col = 0; col < sz; col++){
                        pwr = pwr + grid[x+col][y+row];
                    }
                }
                if(pwr > maxPwr){
                    maxPwr = pwr;
                    maxSz = sz;
                    maxX = x;
                    maxY = y;
                }
            }
        }
        cout << endl;
    }
    cout << "Part 2: Max power cell = " << maxX << "," << maxY << "," << maxSz << endl;
}


#include "kak.hpp"
#include "IRProvider.hpp"
#include "xacc.hpp"
#include "xacc_service.hpp"
#include <Eigen/Eigenvalues>
#include <unsupported/Eigen/KroneckerProduct>
#include <unsupported/Eigen/MatrixFunctions>
#include "PauliOperator.hpp"

namespace {
constexpr std::complex<double> I { 0.0, 1.0 };
int getTempId()
{
  static int tempIdCounter = 0;
  tempIdCounter++;
  return tempIdCounter;
}

// Define some special matrices
const Eigen::MatrixXcd& KAK_MAGIC()
{
  static Eigen::MatrixXcd KAK_MAGIC(4, 4);
  static bool init = false;
  if (!init)
  {
    KAK_MAGIC <<  1, 0, 0, I,
                  0, I, 1, 0,
                  0, I, -1, 0,
                  1, 0, 0, -I;
    KAK_MAGIC = KAK_MAGIC * std::sqrt(0.5);
    init = true;
  }

  return KAK_MAGIC;
}

const Eigen::MatrixXcd& KAK_MAGIC_DAG()
{
  static Eigen::MatrixXcd KAK_MAGIC_DAG = KAK_MAGIC().adjoint();
  return KAK_MAGIC_DAG;
}

const Eigen::MatrixXcd& KAK_GAMMA()
{
  static Eigen::MatrixXcd KAK_GAMMA(4, 4);
  static bool init = false;
  if (!init)
  {
    KAK_GAMMA << 1, 1, 1, 1,
                1, 1, -1, -1,
                -1, 1, -1, 1,
                1, -1, -1, 1;
    KAK_GAMMA = 0.25 * KAK_GAMMA;
    init = true;
  }

  return KAK_GAMMA;
}

// Splits i = 0...length into approximate equivalence classes
// determine by the predicate
std::vector<std::pair<int, int>> contiguousGroups(int in_length, std::function<bool(int,int)> in_predicate)
{
  int start = 0;
  std::vector<std::pair<int, int>> result;
  while(start < in_length)
  {
    auto past = start + 1;
    while ((past < in_length) && in_predicate(start, past))
    {
      past++;
    }
    result.emplace_back(start, past);
    start = past;
  }
  return result;
}

Eigen::MatrixXd blockDiag(const Eigen::MatrixXd& in_first, const Eigen::MatrixXd& in_second)
{
  Eigen::MatrixXd bdm = Eigen::MatrixXd::Zero(in_first.rows() + in_second.rows(), in_first.cols() + in_second.cols());
  bdm.block(0, 0, in_first.rows(), in_first.cols()) = in_first;
  bdm.block(in_first.rows(), in_first.cols(), in_second.rows(), in_second.cols()) = in_second;
  return bdm;
}

inline bool isSquare(const Eigen::MatrixXcd& in_mat)
{
  return in_mat.rows() == in_mat.cols();
}

// If the matrix is finite: no NaN elements
template<typename Derived>
inline bool isFinite(const Eigen::MatrixBase<Derived>& x)
{
  return ((x - x).array() == (x - x).array()).all();
}

bool isDiagonal(const Eigen::MatrixXcd& in_mat, double in_tol = 1e-9)
{
  if (!isFinite(in_mat))
  {
    return false;
  }

  for (int i = 0; i < in_mat.rows(); ++i)
  {
    for (int j = 0; j < in_mat.cols(); ++j)
    {
      if (i != j)
      {
        if (std::abs(in_mat(i,j)) > in_tol)
        {
          return false;
        }
      }
    }
  }

  return true;
}

bool allClose(const Eigen::MatrixXcd& in_mat1, const Eigen::MatrixXcd& in_mat2, double in_tol = 1e-9)
{
  if (!isFinite(in_mat1) || !isFinite(in_mat2))
  {
    return false;
  }

  if (in_mat1.rows() == in_mat2.rows() && in_mat1.cols() == in_mat2.cols())
  {
    for (int i = 0; i < in_mat1.rows(); ++i)
    {
      for (int j = 0; j < in_mat1.cols(); ++j)
      {
        if (std::abs(in_mat1(i,j) - in_mat2(i, j)) > in_tol)
        {
          return false;
        }
      }
    }

    return true;
  }
  return false;
}

bool isHermitian(const Eigen::MatrixXcd& in_mat)
{
  if (!isSquare(in_mat) || !isFinite(in_mat))
  {
    return false;
  }
  return allClose(in_mat, in_mat.adjoint());
}

bool isUnitary(const Eigen::MatrixXcd& in_mat)
{
  if (!isSquare(in_mat) || !isFinite(in_mat))
  {
    return false;
  }

  Eigen::MatrixXcd Id = Eigen::MatrixXcd::Identity(in_mat.rows(), in_mat.cols());

  return allClose(in_mat * in_mat.adjoint(), Id);
}

bool isOrthogonal(const Eigen::MatrixXcd& in_mat, double in_tol = 1e-9)
{
  if (!isSquare(in_mat) || !isFinite(in_mat))
  {
    return false;
  }

  // Is real
  for (int i = 0; i < in_mat.rows(); ++i)
  {
    for (int j = 0; j < in_mat.cols(); ++j)
    {
      if (std::abs(in_mat(i,j).imag()) > in_tol)
      {
        return false;
      }
    }
  }
  // its transpose is its inverse
  return allClose(in_mat.inverse(), in_mat.transpose(), in_tol);
}
// Is Orthogonal and determinant == 1
bool isSpecialOrthogonal(const Eigen::MatrixXcd& in_mat, double in_tol = 1e-9)
{
  return isOrthogonal(in_mat, in_tol) && (std::abs(std::abs(in_mat.determinant()) - 1.0) < in_tol);
}

bool isCanonicalized(double x, double y, double z)
{
  // 0 ≤ abs(z) ≤ y ≤ x ≤ pi/4
  // if x = pi/4, z >= 0
  const double TOL = 1e-9;
  if (std::abs(z) >= 0 && y >= std::abs(z) && x >= y && x <= M_PI_4 + TOL)
  {
    if (std::abs(x - M_PI_4) < TOL)
    {
      return (z >= 0);
    }
    return true;
  }
  return false;
}
// Compute exp(i(x XX + y YY + z ZZ)) matrix
Eigen::Matrix4cd interactionMatrixExp(double x, double y, double z)
{
  Eigen::MatrixXcd X { Eigen::MatrixXcd::Zero(2, 2)};
  Eigen::MatrixXcd Y { Eigen::MatrixXcd::Zero(2, 2)};
  Eigen::MatrixXcd Z { Eigen::MatrixXcd::Zero(2, 2)};
  X << 0, 1, 1, 0;
  Y << 0, -I, I, 0;
  Z << 1, 0, 0, -1;
  auto XX = Eigen::kroneckerProduct(X, X);
  auto YY = Eigen::kroneckerProduct(Y, Y);
  auto ZZ = Eigen::kroneckerProduct(Z, Z);
  Eigen::MatrixXcd herm = x*XX + y*YY + z*ZZ;
  herm = I*herm;
  Eigen::MatrixXcd unitary = herm.exp();
  return unitary;
}

// Simplify the Z-Y-Z decomposition:
// i.e. combining rotations and removing trivial rotation
std::shared_ptr<xacc::CompositeInstruction> simplifySingleQubitSeq(double zAngleBefore, double yAngle, double zAngleAfter, size_t bitIdx)
{
  auto zExpBefore = zAngleBefore / M_PI - 0.5;
  auto  middleExp = yAngle / M_PI;
  std::string  middlePauli = "Rx";
  auto  zExpAfter = zAngleAfter / M_PI + 0.5;

  // Helper functions:
  const auto isNearZeroMod = [](double a, double period) -> bool {
    const auto halfPeriod = period / 2;
    const double TOL = 1e-8;
    return std::abs(fmod(a + halfPeriod, period) - halfPeriod) <  TOL;
  };

  const auto toQuarterTurns = [](double in_exp) -> int {
    return static_cast<int>(round(2 * in_exp)) % 4;
  };

  const auto isCliffordRotation = [&](double in_exp) -> bool {
    return isNearZeroMod(in_exp, 0.5);
  };

  const auto isQuarterTurn = [&](double in_exp) -> bool {
    return (isCliffordRotation(in_exp) && toQuarterTurns(in_exp) % 2 == 1);
  };

  const auto isHalfTurn = [&](double in_exp) -> bool {
    return (isCliffordRotation(in_exp) && toQuarterTurns(in_exp) == 2);
  };

  const auto isNoTurn = [&](double in_exp) -> bool {
    return (isCliffordRotation(in_exp) && toQuarterTurns(in_exp) == 0);
  };

  // Clean up angles
  if (isCliffordRotation(zExpBefore))
  {
    if ((isQuarterTurn(zExpBefore) || isQuarterTurn(zExpAfter)) != (isHalfTurn(middleExp) && isNoTurn(zExpBefore-zExpAfter)))
    {
      zExpBefore += 0.5;
      zExpAfter -= 0.5;
      middlePauli = "Ry";
    }
    if (isHalfTurn(zExpBefore) || isHalfTurn(zExpAfter))
    {
      zExpBefore -= 1;
      zExpAfter += 1;
      middleExp = -middleExp;
    }
  }
  if (isNoTurn(middleExp))
  {
    zExpBefore += zExpAfter;
    zExpAfter = 0;
  }
  else if (isHalfTurn(middleExp))
  {
    zExpAfter -= zExpBefore;
    zExpBefore = 0;
  }

  auto gateRegistry = xacc::getService<xacc::IRProvider>("quantum");
  auto composite = gateRegistry->createComposite("__TEMP__COMPOSITE__" + std::to_string(getTempId()));

  if (!isNoTurn(zExpBefore))
  {
    composite->addInstruction(gateRegistry->createInstruction("Rz", { bitIdx }, { zExpBefore * M_PI }));
  }
  if (!isNoTurn(middleExp))
  {
    composite->addInstruction(gateRegistry->createInstruction(middlePauli, { bitIdx }, { middleExp * M_PI }));
  }
  if (!isNoTurn(zExpAfter))
  {
    composite->addInstruction(gateRegistry->createInstruction("Rz", { bitIdx }, { zExpAfter * M_PI }));
  }

  return composite;
}

std::shared_ptr<xacc::CompositeInstruction> singleQubitGateGen(const Eigen::Matrix2cd& in_mat, size_t in_bitIdx)
{
  using GateMatrix = Eigen::Matrix2cd;
  auto gateRegistry = xacc::getService<xacc::IRProvider>("quantum");

  // Use Z-Y decomposition of Nielsen and Chuang (Theorem 4.1).
  // An arbitrary one qubit gate matrix can be written as
  // U = [ exp(j*(a-b/2-d/2))*cos(c/2), -exp(j*(a-b/2+d/2))*sin(c/2)
  //       exp(j*(a+b/2-d/2))*sin(c/2), exp(j*(a+b/2+d/2))*cos(c/2)]
  // where a,b,c,d are real numbers.
  const auto singleQubitGateDecompose = [](const Eigen::Matrix2cd& matrix) -> std::tuple<double, double, double, double> {
    if (allClose(matrix, GateMatrix::Identity()))
    {
      return std::make_tuple(0.0, 0.0, 0.0, 0.0);
    }
    const auto checkParams = [&matrix](double a, double bHalf, double cHalf, double dHalf) {
      GateMatrix U;
      U << std::exp(I*(a-bHalf-dHalf))*std::cos(cHalf),
          -std::exp(I*(a-bHalf+dHalf))*std::sin(cHalf),
          std::exp(I*(a+bHalf-dHalf))*std::sin(cHalf),
          std::exp(I*(a+bHalf+dHalf))*std::cos(cHalf);

      return allClose(U, matrix);
    };

    double a, bHalf, cHalf, dHalf;
    const double TOLERANCE = 1e-9;
    if (std::abs(matrix(0, 1)) < TOLERANCE)
    {
      auto two_a = fmod(std::arg(matrix(0, 0)*matrix(1, 1)), 2*M_PI);
      a = (std::abs(two_a) < TOLERANCE || std::abs(two_a) > 2*M_PI-TOLERANCE) ? 0 : two_a/2.0;
      auto dHalf = 0.0;
      auto b = std::arg(matrix(1, 1))-std::arg(matrix(0, 0));
      std::vector<double> possibleBhalf { fmod(b/2.0, 2 * M_PI), fmod(b/2.0 + M_PI, 2.0 * M_PI) };
      std::vector<double> possibleChalf { 0.0, M_PI };
      bool found = false;
      for (int i = 0; i < possibleBhalf.size(); ++i)
      {
        for (int j = 0; j < possibleChalf.size(); ++j)
        {
          bHalf = possibleBhalf[i];
          cHalf = possibleChalf[j];
          if (checkParams(a, bHalf, cHalf, dHalf))
          {
            found = true;
            break;
          }
        }
        if (found)
        {
          break;
        }
      }
      assert(found);
    }
    else if (std::abs(matrix(0, 0)) < TOLERANCE)
    {
      auto two_a = fmod(std::arg(-matrix(0, 1)*matrix(1, 0)), 2*M_PI);
      a = (std::abs(two_a) < TOLERANCE || std::abs(two_a) > 2*M_PI-TOLERANCE) ? 0 : two_a/2.0;
      dHalf = 0;
      auto b = std::arg(matrix(1, 0))-std::arg(matrix(0, 1)) + M_PI;
      std::vector<double> possibleBhalf { fmod(b/2., 2*M_PI), fmod(b/2.+M_PI, 2*M_PI) };
      std::vector<double> possibleChalf { M_PI/2., 3./2.*M_PI };
      bool found = false;
      for (int i = 0; i < possibleBhalf.size(); ++i)
      {
        for (int j = 0; j < possibleChalf.size(); ++j)
        {
          bHalf = possibleBhalf[i];
          cHalf = possibleChalf[j];
          if (checkParams(a, bHalf, cHalf, dHalf))
          {
            found = true;
            break;
          }
        }
        if (found)
        {
          break;
        }
      }
      assert(found);
    }
    else
    {
      auto two_a = fmod(std::arg(matrix(0, 0)*matrix(1, 1)), 2*M_PI);
      a = (std::abs(two_a) < TOLERANCE || std::abs(two_a) > 2*M_PI-TOLERANCE) ? 0 : two_a/2.0;
      auto two_d = 2.*std::arg(matrix(0, 1))-2.*std::arg(matrix(0, 0));
      std::vector<double> possibleDhalf { fmod(two_d/4., 2*M_PI),
                        fmod(two_d/4.+M_PI/2., 2*M_PI),
                        fmod(two_d/4.+M_PI, 2*M_PI),
                        fmod(two_d/4.+3./2.*M_PI, 2*M_PI) };
      auto two_b = 2.*std::arg(matrix(1, 0))-2.*std::arg(matrix(0, 0));
      std::vector<double> possibleBhalf { fmod(two_b/4., 2*M_PI),
                        fmod(two_b/4.+M_PI/2., 2*M_PI),
                        fmod(two_b/4.+M_PI, 2*M_PI),
                        fmod(two_b/4.+3./2.*M_PI, 2*M_PI) };
      auto tmp = std::acos(std::abs(matrix(1, 1)));
      std::vector<double> possibleChalf { fmod(tmp, 2*M_PI),
                        fmod(tmp+M_PI, 2*M_PI),
                        fmod(-1.*tmp, 2*M_PI),
                        fmod(-1.*tmp+M_PI, 2*M_PI) };
      bool found = false;
      for (int i = 0; i < possibleBhalf.size(); ++i)
      {
        for (int j = 0; j < possibleChalf.size(); ++j)
        {
          for (int k = 0; k < possibleDhalf.size(); ++k)
          {
            bHalf = possibleBhalf[i];
            cHalf = possibleChalf[j];
            dHalf = possibleDhalf[k];
            if (checkParams(a, bHalf, cHalf, dHalf))
            {
              found = true;
              break;
            }
          }
          if (found)
          {
            break;
          }
        }
        if (found)
        {
          break;
        }
      }
      assert(found);
    }

    // Final check:
    assert(checkParams(a, bHalf, cHalf, dHalf));
    return std::make_tuple(a, bHalf, cHalf, dHalf);
  };
  // Use Z-Y decomposition of Nielsen and Chuang (Theorem 4.1).
  // An arbitrary one qubit gate matrix can be writen as
  // U = [ exp(j*(a-b/2-d/2))*cos(c/2), -exp(j*(a-b/2+d/2))*sin(c/2)
  //       exp(j*(a+b/2-d/2))*sin(c/2), exp(j*(a+b/2+d/2))*cos(c/2)]
  // where a,b,c,d are real numbers.
  // Then U = exp(j*a) Rz(b) Ry(c) Rz(d).
  auto [a, bHalf, cHalf, dHalf] = singleQubitGateDecompose(in_mat);
  // Validate U = exp(j*a) Rz(b) Ry(c) Rz(d).
  const auto validate = [](const GateMatrix& in_mat, double a, double b, double c, double d) {
    GateMatrix Rz_b, Ry_c, Rz_d;
    Rz_b << std::exp(-I*b/2.0), 0, 0, std::exp(I*b/2.0);
    Rz_d << std::exp(-I*d/2.0), 0, 0, std::exp(I*d/2.0);
    Ry_c << std::cos(c/2), -std::sin(c/2), std::sin(c/2), std::cos(c/2);
    auto mat = std::exp(I*a)*Rz_b*Ry_c*Rz_d;
    return allClose(in_mat, mat);
  };
  // Validate the *raw* decomposition
  assert(validate(in_mat, a, 2*bHalf, 2*cHalf, 2*dHalf));

  // Simplify/optimize the sequence:
  auto composite = simplifySingleQubitSeq(2 * dHalf, 2 * cHalf, 2 * bHalf, in_bitIdx);

  // Validate the *simplified* sequence
  const auto validateSimplifiedSequence = [](const std::shared_ptr<xacc::CompositeInstruction>& in_composite, const GateMatrix& in_mat) {
    const auto Rx = [](double angle) {
      GateMatrix result;
      result << std::cos(angle/2.0), -I*std::sin(angle/2.0), -I*std::sin(angle/2.0), std::cos(angle/2.0);
      return result;
    };
    const auto Ry = [](double angle) {
      GateMatrix result;
      result << std::cos(angle/2), -std::sin(angle/2), std::sin(angle/2), std::cos(angle/2);
      return result;
    };
    const auto Rz = [](double angle) {
      GateMatrix result;
      result << std::exp(-I*angle/2.0), 0, 0, std::exp(I*angle/2.0);
      return result;
    };

    GateMatrix totalU = GateMatrix::Identity();
    for (size_t i = 0; i < in_composite->nInstructions(); ++i)
    {
      auto inst = in_composite->getInstruction(i);
      assert(inst->name() == "Rx" || inst->name() == "Ry" || inst->name() == "Rz");
      const auto angle = inst->getParameter(0).as<double>();
      if (inst->name() == "Rx")
      {
        totalU =  Rx(angle) * totalU;
      }
      if (inst->name() == "Ry")
      {
        totalU = Ry(angle) * totalU;
      }
      if (inst->name() == "Rz")
      {
        totalU = Rz(angle) * totalU;
      }
    }

    // Normalize the upto global phase:
    // Find index of the largest element:
    size_t colIdx = 0;
    size_t rowIdx = 0;
    double maxVal = std::abs(totalU(0,0));
    for (size_t i = 0; i < totalU.rows(); ++i)
    {
      for (size_t j = 0; j < totalU.cols(); ++j)
      {
        if (std::abs(totalU(i,j)) > maxVal)
        {
          maxVal = std::abs(totalU(i,j));
          colIdx = j;
          rowIdx = i;
        }
      }
    }

    const std::complex<double> globalFactor = in_mat(rowIdx, colIdx) / totalU(rowIdx, colIdx);
    totalU = globalFactor * totalU;
    return allClose(in_mat, totalU, 1e-6);
  };

  assert(validateSimplifiedSequence(composite, in_mat));
  return composite;
}
}

using namespace xacc;
using namespace xacc::quantum;

namespace xacc {
namespace circuits {
const std::vector<std::string> KAK::requiredKeys()
{
  return { "unitary" };
}

bool KAK::expand(const HeterogeneousMap& parameters)
{
  Eigen::Matrix4cd unitary;
  if (parameters.keyExists<Eigen::Matrix4cd>("unitary"))
  {
    unitary = parameters.get<Eigen::Matrix4cd>("unitary");
  }
  else if (parameters.keyExists<std::vector<std::complex<double>>>("unitary"))
  {
    auto matAsVec = parameters.get<std::vector<std::complex<double>>>("unitary");
    // Correct size: 4 x 4
    if (matAsVec.size() == 16)
    {
      for (int row = 0; row < 4; ++row)
      {
        for (int col = 0; col < 4; ++col)
        {
          // Expect row-by-row layout
          unitary(row, col) = matAsVec[4*row + col];
        }
      }
    }
  }

  if (!isUnitary(unitary))
  {
    xacc::error("Input matrix is not a 4x4 unitary matrix");
    return false;
  }

  // Vector of qubits:
  // Default is {0, 1}
  // This can be specified if needed.
  std::vector<size_t> bits {0, 1};
  if (parameters.keyExists<std::vector<int>>("qubits"))
  {
    auto qubitVec = parameters.get<std::vector<int>>("qubits");
    if (qubitVec.size() != 2)
    {
      xacc::error("Expected 2 qubits.");
      return false;
    }
    bits[0] = qubitVec[0];
    bits[1] = qubitVec[1];
  }

  auto result = kakDecomposition(unitary);
  if (!result.has_value())
  {
    return false;
  }

  auto composite = result->toGates(bits[0], bits[1]);
  addInstructions(composite->getInstructions());
  return true;
}

std::optional<KAK::KakDecomposition> KAK::kakDecomposition(const InputMatrix& in_matrix) const
{
  assert(isUnitary(in_matrix));
  Eigen::MatrixXcd mInMagicBasis = KAK_MAGIC_DAG() * in_matrix * KAK_MAGIC();
  auto [left, diag, right] = bidiagonalizeUnitary(mInMagicBasis);
  // Recover pieces.
  auto [a1, a0] = so4ToMagicSu2s(left.transpose());
  auto [b1, b0] = so4ToMagicSu2s(right.transpose());
  assert(isUnitary(a0));
  assert(isUnitary(a1));
  assert(isUnitary(b0));
  assert(isUnitary(b1));

  Eigen::Vector4cd angles;
  for (size_t i = 0; i < 4; ++i)
  {
    angles(i) = std::arg(diag[i]);
  }
  auto factors = KAK_GAMMA() * angles;
  KakDecomposition result;
  {
    result.g = std::exp(I * factors(0));
    result.a0 = a0;
    result.a1 = a1;
    result.b0 = b0;
    result.b1 = b1;
    result.x = factors(1).real();
    assert(std::abs(factors(1).imag()) < 1e-9);
    result.y = factors(2).real();
    assert(std::abs(factors(2).imag()) < 1e-9);
    result.z = factors(3).real();
    assert(std::abs(factors(3).imag()) < 1e-9);
  }

  const bool validateMatrix = allClose(result.toMat(), in_matrix);
  // Failed to validate
  if (!validateMatrix)
  {
    return std::nullopt;
  }

  auto canonicalizedInteraction = canonicalizeInteraction(result.x, result.y, result.z);

  // Combine the single-qubit blocks:
  result.b1 = canonicalizedInteraction.b1 * result.b1;
  result.b0 = canonicalizedInteraction.b0 * result.b0;
  result.a1 = result.a1 * canonicalizedInteraction.a1;
  result.a0 = result.a0 * canonicalizedInteraction.a0;
  result.g = result.g * canonicalizedInteraction.g;
  result.x = canonicalizedInteraction.x;
  result.y = canonicalizedInteraction.y;
  result.z = canonicalizedInteraction.z;

  assert(isCanonicalized(result.x, result.y, result.z));
  assert(allClose(result.toMat(), in_matrix));

  return result;
}

Eigen::MatrixXcd KAK::KakDecomposition::toMat() const
{
  auto before = Eigen::kroneckerProduct(b1, b0);
  auto after = Eigen::kroneckerProduct(a1, a0);
  Eigen::MatrixXcd unitary = interactionMatrixExp(x, y, z);
  auto total = g * after * unitary * before;
  return total;
}

std::shared_ptr<CompositeInstruction> KAK::KakDecomposition::toGates(size_t in_bit1, size_t in_bit2) const
{
  auto gateRegistry = xacc::getService<IRProvider>("quantum");
  const auto generateInteractionComposite = [&](size_t bit1, size_t bit2, double x, double y, double z) {
    const double TOL = 1e-8;
    // Full decomposition is required
    if (std::abs(z) >= TOL)
    {
      const double xAngle = M_PI * (x * -2 / M_PI + 0.5);
      const double yAngle = M_PI * (y * -2 / M_PI + 0.5);
      const double zAngle = M_PI * (z * -2 / M_PI + 0.5);
      auto composite = gateRegistry->createComposite("__TEMP__INTERACTION_COMPOSITE__" + std::to_string(getTempId()));

      composite->addInstruction(gateRegistry->createInstruction("H", { bit1 }));
      composite->addInstruction(gateRegistry->createInstruction("CZ", { bit2, bit1 }));
      composite->addInstruction(gateRegistry->createInstruction("H", { bit1 }));
      composite->addInstruction(gateRegistry->createInstruction("Rz", { bit1 }, { zAngle }));
      composite->addInstruction(gateRegistry->createInstruction("Rx", { bit1 }, { M_PI_2 }));
      composite->addInstruction(gateRegistry->createInstruction("H", { bit2 }));
      composite->addInstruction(gateRegistry->createInstruction("CZ", { bit1, bit2 }));
      composite->addInstruction(gateRegistry->createInstruction("H", { bit2 }));
      composite->addInstruction(gateRegistry->createInstruction("Ry", { bit1 }, { yAngle }));
      composite->addInstruction(gateRegistry->createInstruction("Rx", { bit2 }, { xAngle }));
      composite->addInstruction(gateRegistry->createInstruction("H", { bit1 }));
      composite->addInstruction(gateRegistry->createInstruction("CZ", { bit1, bit2 }));
      composite->addInstruction(gateRegistry->createInstruction("H", { bit1 }));
      composite->addInstruction(gateRegistry->createInstruction("Rx", { bit2 }, { -M_PI_2 }));

      const auto validateGateSequence = [&](const Eigen::Matrix4cd& in_target){
        const auto H = []() {
          GateMatrix result;
          result << 1.0/std::sqrt(2), 1.0/std::sqrt(2), 1.0/std::sqrt(2), -1.0/std::sqrt(2);
          return result;
        };
        const auto Rx = [](double angle) {
          GateMatrix result;
          result << std::cos(angle/2.0), -I*std::sin(angle/2.0), -I*std::sin(angle/2.0), std::cos(angle/2.0);
          return result;
        };
        const auto Ry = [](double angle) {
          GateMatrix result;
          result << std::cos(angle/2), -std::sin(angle/2), std::sin(angle/2), std::cos(angle/2);
          return result;
        };
        const auto Rz = [](double angle) {
          GateMatrix result;
          result << std::exp(-I*angle/2.0), 0, 0, std::exp(I*angle/2.0);
          return result;
        };
        const auto CZ = []() {
          Eigen::Matrix4cd cz;
          cz << 1, 0, 0, 0,
                0, 1, 0, 0,
                0, 0, 1, 0,
                0, 0, 0, -1;
          return cz;
        };

        Eigen::Matrix2cd IdMat = Eigen::Matrix2cd::Identity();
        Eigen::Matrix4cd totalU = Eigen::Matrix4cd::Identity();
        totalU *= Eigen::kroneckerProduct(IdMat, Rx(-M_PI_2));
        totalU *= Eigen::kroneckerProduct(H(), IdMat);
        totalU *= CZ();
        totalU *= Eigen::kroneckerProduct(H(), IdMat);
        totalU *= Eigen::kroneckerProduct(IdMat, Rx(xAngle));
        totalU *= Eigen::kroneckerProduct(Ry(yAngle), IdMat);
        totalU *= Eigen::kroneckerProduct(IdMat, H());
        totalU *= CZ();
        totalU *= Eigen::kroneckerProduct(IdMat, H());
        totalU *= Eigen::kroneckerProduct(Rx(M_PI_2), IdMat);
        totalU *= Eigen::kroneckerProduct(Rz(zAngle), IdMat);
        totalU *= Eigen::kroneckerProduct(H(), IdMat);
        totalU *= CZ();
        totalU *= Eigen::kroneckerProduct(H(), IdMat);
        // Find index of the largest element:
        size_t colIdx = 0;
        size_t rowIdx = 0;
        double maxVal = std::abs(totalU(0,0));
        for (size_t i = 0; i < totalU.rows(); ++i)
        {
          for (size_t j = 0; j < totalU.cols(); ++j)
          {
            if (std::abs(totalU(i,j)) > maxVal)
            {
              maxVal = std::abs(totalU(i,j));
              colIdx = j;
              rowIdx = i;
            }
          }
        }

        const std::complex<double> globalFactor = in_target(rowIdx, colIdx) / totalU(rowIdx, colIdx);
        totalU = globalFactor * totalU;
        return allClose(totalU, in_target);
      };

      assert(validateGateSequence(interactionMatrixExp(x, y, z)));
      return composite;
    }
    // ZZ interaction is near zero: only XX and YY
    else if (y >= TOL)
    {
      const double xAngle = -2 * x;
      const double yAngle = -2 * y;
      auto composite = gateRegistry->createComposite("__TEMP__INTERACTION_COMPOSITE__" + std::to_string(getTempId()));
      composite->addInstruction(gateRegistry->createInstruction("Rx", { bit2 }, { M_PI_2 }));
      composite->addInstruction(gateRegistry->createInstruction("H", { bit1 }));
      composite->addInstruction(gateRegistry->createInstruction("CZ", { bit2, bit1 }));
      composite->addInstruction(gateRegistry->createInstruction("H", { bit1 }));
      composite->addInstruction(gateRegistry->createInstruction("Ry", { bit1 }, { yAngle }));
      composite->addInstruction(gateRegistry->createInstruction("Rx", { bit2 }, { xAngle }));
      composite->addInstruction(gateRegistry->createInstruction("H", { bit1 }));
      composite->addInstruction(gateRegistry->createInstruction("CZ", { bit1, bit2 }));
      composite->addInstruction(gateRegistry->createInstruction("H", { bit1 }));
      composite->addInstruction(gateRegistry->createInstruction("Rx", { bit2 }, { -M_PI_2 }));

      const auto validateGateSequence = [&](const Eigen::Matrix4cd& in_target){
        const auto H = []() {
          GateMatrix result;
          result << 1.0/std::sqrt(2), 1.0/std::sqrt(2), 1.0/std::sqrt(2), -1.0/std::sqrt(2);
          return result;
        };
        const auto Rx = [](double angle) {
          GateMatrix result;
          result << std::cos(angle/2.0), -I*std::sin(angle/2.0), -I*std::sin(angle/2.0), std::cos(angle/2.0);
          return result;
        };
        const auto Ry = [](double angle) {
          GateMatrix result;
          result << std::cos(angle/2), -std::sin(angle/2), std::sin(angle/2), std::cos(angle/2);
          return result;
        };
        const auto Rz = [](double angle) {
          GateMatrix result;
          result << std::exp(-I*angle/2.0), 0, 0, std::exp(I*angle/2.0);
          return result;
        };
        const auto CZ = []() {
          Eigen::Matrix4cd cz;
          cz << 1, 0, 0, 0,
                0, 1, 0, 0,
                0, 0, 1, 0,
                0, 0, 0, -1;
          return cz;
        };

        Eigen::Matrix2cd IdMat = Eigen::Matrix2cd::Identity();
        Eigen::Matrix4cd totalU = Eigen::Matrix4cd::Identity();
        totalU *= Eigen::kroneckerProduct(IdMat, Rx(-M_PI_2));
        totalU *= Eigen::kroneckerProduct(H(), IdMat);
        totalU *= CZ();
        totalU *= Eigen::kroneckerProduct(H(), IdMat);
        totalU *= Eigen::kroneckerProduct(IdMat, Rx(xAngle));
        totalU *= Eigen::kroneckerProduct(Ry(yAngle), IdMat);
        totalU *= Eigen::kroneckerProduct(H(), IdMat);
        totalU *= CZ();
        totalU *= Eigen::kroneckerProduct(H(), IdMat);
        totalU *= Eigen::kroneckerProduct(IdMat, Rx(M_PI_2));

        // Find index of the largest element:
        size_t colIdx = 0;
        size_t rowIdx = 0;
        double maxVal = std::abs(totalU(0,0));
        for (size_t i = 0; i < totalU.rows(); ++i)
        {
          for (size_t j = 0; j < totalU.cols(); ++j)
          {
            if (std::abs(totalU(i,j)) > maxVal)
            {
              maxVal = std::abs(totalU(i,j));
              colIdx = j;
              rowIdx = i;
            }
          }
        }

        const std::complex<double> globalFactor = in_target(rowIdx, colIdx) / totalU(rowIdx, colIdx);
        totalU = globalFactor * totalU;
        return allClose(totalU, in_target);
      };

      assert(validateGateSequence(interactionMatrixExp(x, y, z)));
      return composite;
    }
    // only XX is significant
    else
    {
      const double xAngle = -2 * x;
      auto composite = gateRegistry->createComposite("__TEMP__INTERACTION_COMPOSITE__" + std::to_string(getTempId()));
      composite->addInstruction(gateRegistry->createInstruction("H", { bit1 }));
      composite->addInstruction(gateRegistry->createInstruction("CZ", { bit2, bit1 }));
      composite->addInstruction(gateRegistry->createInstruction("Rx", { bit2 }, { xAngle }));
      composite->addInstruction(gateRegistry->createInstruction("CZ", { bit1, bit2 }));
      composite->addInstruction(gateRegistry->createInstruction("H", { bit1 }));

      const auto validateGateSequence = [&](const Eigen::Matrix4cd& in_target){
        const auto H = []() {
          GateMatrix result;
          result << 1.0/std::sqrt(2), 1.0/std::sqrt(2), 1.0/std::sqrt(2), -1.0/std::sqrt(2);
          return result;
        };
        const auto Rx = [](double angle) {
          GateMatrix result;
          result << std::cos(angle/2.0), -I*std::sin(angle/2.0), -I*std::sin(angle/2.0), std::cos(angle/2.0);
          return result;
        };
        const auto Ry = [](double angle) {
          GateMatrix result;
          result << std::cos(angle/2), -std::sin(angle/2), std::sin(angle/2), std::cos(angle/2);
          return result;
        };
        const auto Rz = [](double angle) {
          GateMatrix result;
          result << std::exp(-I*angle/2.0), 0, 0, std::exp(I*angle/2.0);
          return result;
        };
        const auto CZ = []() {
          Eigen::Matrix4cd cz;
          cz << 1, 0, 0, 0,
                0, 1, 0, 0,
                0, 0, 1, 0,
                0, 0, 0, -1;
          return cz;
        };

        Eigen::Matrix2cd IdMat = Eigen::Matrix2cd::Identity();
        Eigen::Matrix4cd totalU = Eigen::Matrix4cd::Identity();

        totalU *= Eigen::kroneckerProduct(H(), IdMat);
        totalU *= CZ();
        totalU *= Eigen::kroneckerProduct(IdMat, Rx(xAngle));
        totalU *= CZ();
        totalU *= Eigen::kroneckerProduct(H(), IdMat);

        // Find index of the largest element:
        size_t colIdx = 0;
        size_t rowIdx = 0;
        double maxVal = std::abs(totalU(0,0));
        for (size_t i = 0; i < totalU.rows(); ++i)
        {
          for (size_t j = 0; j < totalU.cols(); ++j)
          {
            if (std::abs(totalU(i,j)) > maxVal)
            {
              maxVal = std::abs(totalU(i,j));
              colIdx = j;
              rowIdx = i;
            }
          }
        }

        const std::complex<double> globalFactor = in_target(rowIdx, colIdx) / totalU(rowIdx, colIdx);
        totalU = globalFactor * totalU;
        return allClose(totalU, in_target);
      };

      assert(validateGateSequence(interactionMatrixExp(x, y, z)));
      return composite;
    }
  };

  auto a0Comp = singleQubitGateGen(a0, in_bit2);
  auto a1Comp = singleQubitGateGen(a1, in_bit1);
  auto b0Comp = singleQubitGateGen(b0, in_bit2);
  auto b1Comp = singleQubitGateGen(b1, in_bit1);
  auto interactionComp = generateInteractionComposite(in_bit2, in_bit1, x, y, z);
  auto totalComposite = gateRegistry->createComposite("__TEMP__KAK_COMPOSITE__" + std::to_string(getTempId()));
  // U = g x (Gate A1 Gate A0) x exp(i(xXX + yYY + zZZ))x(Gate b1 Gate b0)
  // Before:
  totalComposite->addInstructions(b0Comp->getInstructions());
  totalComposite->addInstructions(b1Comp->getInstructions());
  // Interaction:
  totalComposite->addInstructions(interactionComp->getInstructions());
  // After:
  totalComposite->addInstructions(a0Comp->getInstructions());
  totalComposite->addInstructions(a1Comp->getInstructions());
  // Ignore global phase
  return totalComposite;
}

KAK::BidiagResult KAK::bidiagonalizeUnitary(const InputMatrix& in_matrix) const
{
  Eigen::Matrix4d realMat;
  Eigen::Matrix4d imagMat;
  for (int row = 0; row < in_matrix.rows(); ++row)
  {
    for (int col = 0; col < in_matrix.cols(); ++col)
    {
      realMat(row, col) = in_matrix(row, col).real();
      imagMat(row, col) = in_matrix(row, col).imag();
    }
  }
  // Assert A X B.T and A.T X B are hermitian
  assert(isHermitian(realMat * imagMat.transpose()));
  assert(isHermitian(realMat.transpose() * imagMat));

  auto [left, right] = bidiagonalizeRealMatrixPairWithSymmetricProducts(realMat, imagMat);

  // Convert to special orthogonal w/o breaking diagonalization.
  if (left.determinant() < 0)
  {
    for (int i = 0; i < left.cols(); ++i)
    {
      left(0, i) = -left(0, i);
    }
  }
  if (right.determinant() < 0)
  {
    for (int i = 0; i < right.rows(); ++i)
    {
      right(i, 0) = -right(i, 0);
    }
  }

  auto diag = left * in_matrix * right;
  // Validate:
  assert(isDiagonal(diag));

  std::vector<std::complex<double>> diagVec;
  for (int i = 0; i < diag.rows(); ++i)
  {
    diagVec.emplace_back(diag(i, i));
  }

  return std::make_tuple(left, diagVec, right);
}

std::tuple<std::complex<double>, KAK::GateMatrix, KAK::GateMatrix> KAK::kronFactor(const InputMatrix& in_matrix) const
{
  KAK::GateMatrix f1 = KAK::GateMatrix::Zero();
  KAK::GateMatrix f2 = KAK::GateMatrix::Zero();

  // Get row and column of the max element
  size_t a = 0;
  size_t b = 0;
  double maxVal = std::abs(in_matrix(a, b));
  for (int row = 0; row < in_matrix.rows(); ++row)
  {
    for (int col = 0; col < in_matrix.cols(); ++col)
    {
      if (std::abs(in_matrix(row, col)) > maxVal)
      {
        a = row;
        b = col;
        maxVal = std::abs(in_matrix(a, b));
      }
    }
  }

  // Extract sub-factors touching the reference cell.
  for (int i = 0; i < 2; ++i)
  {
    for (int j = 0; j < 2; ++j)
    {
      f1((a >> 1) ^ i, (b >> 1) ^ j) = in_matrix(a ^ (i << 1), b ^ (j << 1));
      f2((a & 1) ^ i, (b & 1) ^ j) = in_matrix(a ^ i, b ^ j);
    }
  }

  // Rescale factors to have unit determinants.
  f1 /= (std::sqrt(f1.determinant()));
  f2 /= (std::sqrt(f2.determinant()));

  //Determine global phase.
  std::complex<double> g = in_matrix(a, b) / (f1(a >> 1, b >> 1) * f2(a & 1, b & 1));
  if (g.real() < 0.0)
  {
    f1 *= -1;
    g = -g;
  }

  // Validate:
  Eigen::Matrix4cd testMat = g * Eigen::kroneckerProduct(f1, f2);
  assert(allClose(testMat, in_matrix));

  return std::make_tuple(g, f1, f2);
}

std::pair<KAK::GateMatrix, KAK::GateMatrix> KAK::so4ToMagicSu2s(const InputMatrix& in_matrix) const
{
  assert(isSpecialOrthogonal(in_matrix));
  auto matInMagicBasis = KAK_MAGIC() * in_matrix * KAK_MAGIC_DAG();
  auto [g, f1, f2] = kronFactor(matInMagicBasis);
  return std::make_pair(f1, f2);
}

Eigen::MatrixXd KAK::diagonalizeRealSymmetricMatrix(const Eigen::MatrixXd& in_mat) const
{
  assert(isHermitian(in_mat));
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(in_mat);
  Eigen::MatrixXd p = solver.eigenvectors();
  // Orthogonal basis (Hermitian/symmetric matrix)
  assert(isOrthogonal(p));
  // An orthogonal matrix P such that PT x matrix x P is diagonal.
  assert(isDiagonal(p.transpose() * in_mat * p));
  return p;
}

Eigen::MatrixXd KAK::diagonalizeRealSymmetricAndSortedDiagonalMatrices(const Eigen::MatrixXd& in_symMat, const Eigen::MatrixXd& in_diagMat) const
{
  assert(isDiagonal(in_diagMat));
  assert(isHermitian(in_symMat));
  const auto similarSingular = [&in_diagMat](int i, int j) {
    return std::abs(in_diagMat(i,i) - in_diagMat(j,j)) < 1e-5;
  };

  const auto ranges = contiguousGroups(in_diagMat.rows(), similarSingular);
  Eigen::MatrixXd p = Eigen::MatrixXd::Zero(in_symMat.rows(), in_symMat.cols());

  for (const auto& [start, end]: ranges)
  {
    const int blockSize = end - start;

    Eigen::MatrixXd block = Eigen::MatrixXd(blockSize, blockSize);
    for (int i = 0; i < blockSize; ++i)
    {
      for (int j = 0; j < blockSize; ++j)
      {
        block(i,j) = in_symMat(i + start, j + start);
      }
    }
    auto blockDiag = diagonalizeRealSymmetricMatrix(block);

    for (int i = 0; i < blockSize; ++i)
    {
      for (int j = 0; j < blockSize; ++j)
      {
        p(i + start, j + start) = blockDiag(i,j);
      }
    }
  }

  // P.T x symmetric_matrix x P is diagonal
  assert(isDiagonal(p.transpose() * in_symMat * p));
  // and P.T x diagonal_matrix x P = diagonal_matrix
  assert(allClose(p.transpose() * in_diagMat * p, in_diagMat));

  return p;
}

std::pair<Eigen::Matrix4d, Eigen::Matrix4d> KAK::bidiagonalizeRealMatrixPairWithSymmetricProducts(const Eigen::Matrix4d& in_mat1, const Eigen::Matrix4d& in_mat2) const
{
  const auto svd = [](const Eigen::MatrixXd& in_mat) -> std::tuple<Eigen::MatrixXd, Eigen::VectorXd, Eigen::MatrixXd> {
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(in_mat, Eigen::ComputeThinU | Eigen::ComputeThinV);
    return std::make_tuple(svd.matrixU(), svd.singularValues(), svd.matrixV().adjoint());
  };
  // Use SVD to bi-diagonalize the first matrix.
  auto [baseLeft, baseDiagVec, baseRight] = svd(in_mat1);

  Eigen::MatrixXd baseDiag = Eigen::MatrixXd::Zero(baseDiagVec.size(), baseDiagVec.size());
  for (int i = 0; i < baseDiagVec.size(); ++i)
  {
    baseDiag(i, i) = baseDiagVec(i);
  }

  // Determine where we switch between diagonalization-fixup strategies.
  const auto dim = baseDiag.rows();
  auto rank = dim;
  while (rank > 0 && std::abs(baseDiag(rank - 1, rank - 1) < 1e-5))
  {
    rank--;
  }
  Eigen::MatrixXd baseDiagTrim = Eigen::MatrixXd::Zero(rank, rank);
  for (int i = 0; i < rank; ++i)
  {
    for (int j = 0; j < rank; ++j)
    {
      baseDiagTrim(i, j) = baseDiag(i, j);
    }
  }

  // Try diagonalizing the second matrix with the same factors as the first.
  auto semiCorrected = baseLeft.transpose() * in_mat2 * baseRight.transpose();

  Eigen::MatrixXd overlap = Eigen::MatrixXd::Zero(rank, rank);
  for (int i = 0; i < rank; ++i)
  {
    for (int j = 0; j < rank; ++j)
    {
      overlap(i, j) = semiCorrected(i, j);
    }
  }

  auto overlapAdjust = diagonalizeRealSymmetricAndSortedDiagonalMatrices(overlap, baseDiagTrim);

  const auto extraSize = dim - rank;
  Eigen::MatrixXd extra(extraSize, extraSize);
  for (int i = 0; i < extraSize; ++i)
  {
    for (int j = 0; j < extraSize; ++j)
    {
      extra(i, j) = semiCorrected(i + rank, j + rank);
    }
  }

  static const auto emptySvdResult = std::make_tuple(Eigen::MatrixXd::Zero(0,0), Eigen::VectorXd::Zero(0), Eigen::MatrixXd::Zero(0,0));
  auto [extraLeftAdjust, extraDiag, extraRightAdjust] = (dim > rank) ? svd(extra): emptySvdResult;

  auto leftAdjust = blockDiag(overlapAdjust, extraLeftAdjust);
  auto rightAdjust = blockDiag(overlapAdjust.transpose(), extraRightAdjust);
  auto left = leftAdjust.transpose() * baseLeft.transpose();
  auto right = baseRight.transpose() * rightAdjust.transpose();
  // L x mat1 x R and L x mat2 x R are diagonal matrices.
  assert(isDiagonal(left * in_mat1 * right));
  assert(isDiagonal(left * in_mat2 * right));
  return std::make_pair(left, right);
}

KAK::KakDecomposition KAK::canonicalizeInteraction(double x, double y, double z) const
{
  // Accumulated global phase.
  std::complex<double> phase = 1.0;
  //Per-qubit left factors.
  std::vector<GateMatrix> left { GateMatrix::Identity(), GateMatrix::Identity() };
  // Per-qubit right factors.
  std::vector<GateMatrix> right { GateMatrix::Identity(), GateMatrix::Identity() };
  // Remaining XX/YY/ZZ interaction vector.
  std::vector<double> v { x, y, z };

  std::vector<GateMatrix> flippers {
    (GateMatrix() << 0, I, I, 0).finished(),
    (GateMatrix() << 0, 1, -1, 0).finished(),
    (GateMatrix() << I, 0, 0, -I).finished()
  };

  std::vector<GateMatrix> swappers {
    (GateMatrix() << I*M_SQRT1_2, M_SQRT1_2, -M_SQRT1_2, -I*M_SQRT1_2).finished(),
    (GateMatrix() << I*M_SQRT1_2, I*M_SQRT1_2, I*M_SQRT1_2, -I*M_SQRT1_2).finished(),
    (GateMatrix() << 0, I*M_SQRT1_2 + M_SQRT1_2, I*M_SQRT1_2 - M_SQRT1_2, 0).finished()
  };

  const auto shift = [&](int k, int step) {
    v[k] += step * M_PI_2;
    phase *= std::pow(I, step);
    const auto expFact = ((step % 4) + 4) % 4;
    const GateMatrix mat = flippers[k].array().pow(expFact);
    right[0] = mat * right[0];
    right[1] = mat * right[1];
  };

  const auto negate = [&](int k1, int k2) {
    v[k1] *= -1;
    v[k2] *= -1;
    phase *= -1;
    const auto& s = flippers[3 - k1 - k2];
    left[1] = left[1] * s;
    right[1] = s * right[1];
  };

  const auto swap = [&](int k1, int k2) {
    std::iter_swap(v.begin() + k1, v.begin() + k2);
    const auto& s = swappers[3 - k1 - k2];
    left[0] = left[0] * s;
    left[1] = left[1] * s;
    right[0] = s * right[0];
    right[1] = s * right[1];
  };

  const auto canonicalShift = [&](int k) {
    while (v[k] <= -M_PI_4)
    {
      shift(k, +1);
    }
    while (v[k] > M_PI_4)
    {
      shift(k, -1);
    }
  };

  const auto sort = [&](){
    if (std::abs(v[0]) < std::abs(v[1]))
    {
      swap(0, 1);
    }
    if (std::abs(v[1]) < std::abs(v[2]))
    {
      swap(1, 2);
    }
    if (std::abs(v[0]) < std::abs(v[1]))
    {
      swap(0, 1);
    }
  };

  canonicalShift(0);
  canonicalShift(1);
  canonicalShift(2);
  sort();

  if (v[0] < 0)
  {
    negate(0, 2);
  }
  if (v[1] < 0)
  {
    negate(1, 2);
  }
  canonicalShift(2);

  if ((v[0] > M_PI_4 - 1e-9) && (v[2] < 0))
  {
    shift(0, -1);
    negate(0, 2);
  }

  assert(isCanonicalized(v[0], v[1], v[2]));

  KakDecomposition result;
  {
    result.g = phase;
    result.a0 = left[1];
    result.a1 = left[0];
    result.b0 = right[1];
    result.b1 = right[0];
    result.x = v[0];
    result.y = v[1];
    result.z = v[2];
  }

  assert(allClose(result.toMat(), interactionMatrixExp(x, y, z)));
  return result;
}
bool ZYZ::expand(const xacc::HeterogeneousMap& runtimeOptions)
{
  Eigen::Matrix2cd unitary;
  if (runtimeOptions.keyExists<Eigen::Matrix2cd>("unitary"))
  {
    unitary = runtimeOptions.get<Eigen::Matrix2cd>("unitary");
  }
  else if (runtimeOptions.keyExists<std::vector<std::complex<double>>>("unitary"))
  {
    auto matAsVec = runtimeOptions.get<std::vector<std::complex<double>>>("unitary");
    // Correct size: 2 x 2
    if (matAsVec.size() == 4)
    {
      for (int row = 0; row < 2; ++row)
      {
        for (int col = 0; col < 2; ++col)
        {
          // Expect row-by-row layout
          unitary(row, col) = matAsVec[2*row + col];
        }
      }
    }
  }
  else
  {
    xacc::error("unitary matrix is required.");
    return false;
  }

  assert(isUnitary(unitary));
  auto decomposed = singleQubitGateGen(unitary, 0);
  addInstructions(decomposed->getInstructions());
  return true;
}
} // namespace circuits
} // namespace xacc


#ifndef CALIBRATOR_PROCESSES_GENERAL_HULLWHITEPROCESS_HPP
#define CALIBRATOR_PROCESSES_GENERAL_HULLWHITEPROCESS_HPP

#include <boost/bind.hpp>

#include <ql/stochasticprocess.hpp>
#include <ql/processes/forwardmeasureprocess.hpp>
#include <ql/math/integrals/simpsonintegral.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>

#include <calibrator/global.hpp>
#include <calibrator/models/integrableparameter.hpp>

namespace HJCALIBRATOR
{
	//! Time-dependent Hull-White process class
	/*! This class describes the time-dependent Hull-White process governed by
	\f[
	dr(t) = (\theta(t) - a(t)r(t)) dt + \sigma(t) dW_t.
	\f]

	Derivation Reference, Eq. numbering quoted from SSRN-id1514192

	\ingroup processes
	*/
	class GeneralizedHullWhiteProcess : public StochasticProcess1D
	{
	public :
		GeneralizedHullWhiteProcess( const Handle<YieldTermStructure>& h,
									 const IntegrableParameter& a, // must be an IntegrableParameter
									 const Parameter& sigma );

		//! \name StochasticProcess1D interface
		//@{
		Real x0() const override { return r0_; }
		Real drift( Time t, Rate r ) const override;
		Real diffusion( Time t, Rate r ) const override;
		Real variance( Time t0, Rate r0, Time dt ) const override;
		Real expectation( Time t0, Rate r0, Time dt ) const override;
		Real stdDeviation( Time t0, Rate r0, Time dt ) const override;
		//@}

		IntegrableParameter a() { return a_; }
		Parameter sigma() { return sigma_; }

		Real a( Time t ) { return a_( t ); }
		Real sigma( Time t ) { return sigma_( t ); }

		Real bondPrice( Time t, Time T, Rate r )const;

	private :
		Real A( Time t, Time T ) const;
		Real B( Time t, Time T ) const;

		Real theta( Time t ) const;
		Real E( Time t, Real multiplier = 1. ) const;
		Real alpha( Time t ) const;

		Real DVIntegrand( Time u, Time t ) const;
		Real VrIntegrand( Time t ) const;
		Real alphaIntegrand( Time u, Time t ) const;

		Handle<YieldTermStructure> termStructure_;
		IntegrableParameter a_;
		Parameter sigma_;
		Real r0_;

		SimpsonIntegral integrator_;
		boost::function<Real( Real )> Vrintegrand_; // variance integrand
		boost::function<Real( Real )> OneOverEintegrand_; // Eq. 31 integrand

	};

	//! Time-dependent Hull-White Forward-Rate process class
	/*
	Derivation Reference, Eq. numbering quoted from SSRN-id1514192

	\ingroup processes
	*/
	/*
	class GeneralizedHullwhiteForwardProcess : public ForwardMeasureProcess1D
	{
		GeneralizedHullwhiteForwardProcess( const Handle<YieldTermStructure>& h,
											const IntegrableParameter& a, // must be an IntegrableParameter
											const Parameter& sigma );

		//! \name StochasticProcess1D interface
		//@{
		Real x0() const override;
		Real drift( Time t, Rate r ) const override;
		Real diffusion( Time t, Rate r ) const override;
		Real variance( Time t0, Rate r0, Time dt ) const override;
		Real expectation( Time t0, Rate r0, Time dt ) const override;
		Real stdDeviation( Time t0, Rate r0, Time dt ) const override;
		//@}

	private :
		boost::shared_ptr<GeneralizedHullWhiteProcess> hwprocess_;
	};
	*/

	// inline definitions

	inline Real GeneralizedHullWhiteProcess::drift( Time t, Rate r ) const
	{
		return theta( t ) - a_( t ) * r;
	}

	inline Real GeneralizedHullWhiteProcess::diffusion( Time t, Rate r ) const
	{
		return sigma_( t );
	}

	inline Real GeneralizedHullWhiteProcess::variance( Time t0, Rate r0, Time dt ) const
	{
		Real Et = E( t0 + dt );

		return integrator_( Vrintegrand_, t0, t0 + dt ) / (Et * Et);
	}

	inline Real GeneralizedHullWhiteProcess::stdDeviation( Time t0, Rate r0, Time dt ) const
	{
		return sqrt( variance( t0, r0, dt ) );
	}

	inline Real GeneralizedHullWhiteProcess::expectation( Time t0, Rate r0, Time dt ) const
	{
		/* A part of eq.35 */
		Time s = t0;
		Time t = t0 + dt;

		Real RE = E( s ) / E( t );

		return RE * r0 + alpha( t ) - RE * alpha( s );
	}

	Real GeneralizedHullWhiteProcess::bondPrice( Time t, Time T, Rate r) const
	{
		return A( t, T ) * exp( -(B( t, T ) * r) );
	}

	Real GeneralizedHullWhiteProcess::A( Time t, Time T ) const
	{
		/* Modefication of the eq. 43 so that we have the general affine form P = Aexp(-Br)
		\f[
		A(t,T) = \frac{P(0,T)}{P(0,t)} + exp\left{B(t,T)f(0,t) - \frac{1}{2}B^2(t,T)V_r(0,t)\right}
		\f]
		*/
		Real discount_t = termStructure_->discount( t );
		Real discount_T = termStructure_->discount( T );
		Real forward = termStructure_->forwardRate( t, t, Continuous, NoFrequency );

		Real BtT = B( t, T );
		Real Vrt = variance( 0, 0, t );

		return (discount_T / discount_t) * exp( BtT*forward - 0.5 * BtT * BtT * Vrt );
	}

	Real GeneralizedHullWhiteProcess::B( Time t, Time T ) const
	{
		/* eq. 31
		\f[
		B(t,T) = E(t) \int_t^T \frac{du}{E(u)}
		\f]
		*/
		return E( t ) * integrator_( OneOverEintegrand_, t, T );
	}

	inline Real GeneralizedHullWhiteProcess::theta( Time t ) const
	{
		const Real dt = 0.000001; // Should it be variable?
		Real f = termStructure_->forwardRate( t, t, Continuous, NoFrequency );
		Real fup = termStructure_->forwardRate( t + dt, t + dt, Continuous, NoFrequency );
		Real f_prime = (fup - f) / dt;

		boost::function<Real( Real )> integrand;
		integrand = boost::bind( &GeneralizedHullWhiteProcess::DVIntegrand, this, _1, t );
		Real IntI = integrator_( integrand, 0, t );

		Real Et = E( t );
		Real at = a_( t );

		Real DV = (2. / Et) * IntI;
		Real D2V = 2.*variance( 0, 0, t ) - (2*at / Et) * IntI;

		/* eq. 39 */
		return f_prime + a_( t ) * f + 0.5 * (D2V + at * DV);
	}

	inline Real GeneralizedHullWhiteProcess::E( Time t, Real multiplier ) const
	{
		/* eq. 30
		\f[
		E(t) = exp(\int_0^t{a(u)du})
		\f]

		The multiplier is given to deal with the integrand of the eq. 31 (\f$ B(t,T) \f$)
		*/
		return exp( multiplier * a_.integral( 0, t ) );
	}

	inline Real GeneralizedHullWhiteProcess::DVIntegrand( Time u, Time t ) const
	{
		/* Integrand of \f$ \partial V(0,t) /\partial t \f$ in its derived form
		\f[
		\partial V(0,t) /\partial t = \frac{2}{E(t)\int_0^t \sigma(u,t)\sigma(u)E(u)du
		\f]

		Here, we takes \f$ I(u,t) = \sigma(u,t)\sigma(u)E(u) \f$
		so we have
		\f[
		\partial V(0,t) /\partial t = \frac{2}{E(t)\int_0^t I(u,t)du
		\partial^2 V(0,t) /\partial t^2 = -\frac{a(t)}{E(t)}\int_0^t I(u,t)du + 2*V_r(0,t)
		\f]

		where \f$ V_r(0,t \f$ is the variance of the short rate.
		\f]
		*/

		Real sigma_u = sigma_( u );

		return (sigma_u * B( u, t )) * sigma_u * E( u );
	}

	Real GeneralizedHullWhiteProcess::VrIntegrand( Time t ) const
	{
		/* Integrand of eq. 37
		\f[
		I(t) =  E^2(t)\sigam^2(t)
		\f]
		*/
		Real Et = E( t );
		Real sigmat = sigma_( t );

		return Et * Et * sigmat * sigmat;
	}

	Real GeneralizedHullWhiteProcess::alpha( Time t ) const
	{
		/* eq. 36
		\f[
		\alpha = f(0,t) + \frac{1}{E(t)}\int_0^t E(u)\sigma(u)B(u,t)du
		*/
		boost::function<Real( Real )> integrand;
		integrand = boost::bind( &GeneralizedHullWhiteProcess::alphaIntegrand, this, _1, t );
		Real IntI = integrator_( integrand, 0, t );
		Real forward = termStructure_->forwardRate( t, t, Continuous, NoFrequency );

		return forward + IntI / E( t );
	}

	Real GeneralizedHullWhiteProcess::alphaIntegrand( Time u, Time t ) const
	{
		Real sigma_t = sigma_( t );

		return E( t ) * sigma_t * sigma_t * B( u, t );
	}
}

#endif // !CALIBRATOR_PROCESSES_GENERAL_HULLWHITEPROCESS_HPP


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

int main () {
    using namespace boost::numeric::ublas;
    vector<double> u(3), v(3);
    for (unsigned i = 0; i < v.size (); ++ i) {
        u(i) = 3*i + 1;
        v(i) = i;
    }
    std::cout << u << std::endl << v << std::endl;
    double t = inner_prod(u, v);
    std::cout << t << std::endl;
    return 0;
}


#include <iostream>
#include <armadillo>
#include <cmath>

double calculate_anisotropy(const arma::mat &m) {

    const double iso = arma::mean(arma::eig_sym(m));
    double aniso = arma::accu(m % m);
    aniso = std::sqrt(std::abs(1.5*(aniso - (3.0*iso*iso))));

    return aniso;
}

int main() {

    arma::mat tensor(3, 3, arma::fill::zeros);

    tensor(0, 0) = 12;
    tensor(1, 1) = 12;
    tensor(2, 2) = -0;

    arma::vec principal_components;
    arma::mat orientation;

    arma::eig_sym(principal_components, orientation, tensor);

    principal_components.print("principal components (real)");
    orientation.print("orientation (real)");

    double isotropic = arma::mean(principal_components);
    std::cout << "isotropic  : " << isotropic << std::endl;
    double anisotropic = calculate_anisotropy(tensor);
    std::cout << "anisotropic: " << anisotropic << std::endl;

    return 0;
}