//  (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;

}


#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;
    }
}


// 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;
}


/*
 *  (C) Copyright Nick Thompson 2018.
 *  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_INTEGER_MOD_INVERSE_HPP
#define BOOST_INTEGER_MOD_INVERSE_HPP
#include <stdexcept>
#include <boost/throw_exception.hpp>
#include <boost/integer/extended_euclidean.hpp>

namespace boost { namespace integer {

// From "The Joy of Factoring", Algorithm 2.7.
// Here's some others names I've found for this function:
// PowerMod[a, -1, m] (Mathematica)
// mpz_invert (gmplib)
// modinv (some dude on stackoverflow)
// Would mod_inverse be sometimes mistaken as the modular *additive* inverse?
// In any case, I think this is the best name we can get for this function without agonizing.
template<class Z>
Z mod_inverse(Z a, Z modulus)
{
    if (modulus < Z(2))
    {
        BOOST_THROW_EXCEPTION(std::domain_error("mod_inverse: modulus must be > 1"));
    }
    // make sure a < modulus:
    a = a % modulus;
    if (a == Z(0))
    {
        // a doesn't have a modular multiplicative inverse:
        return Z(0);
    }
    boost::integer::euclidean_result_t<Z> u = boost::integer::extended_euclidean(a, modulus);
    if (u.gcd > Z(1))
    {
        return Z(0);
    }
    // x might not be in the range 0 < x < m, let's fix that:
    while (u.x <= Z(0))
    {
        u.x += modulus;
    }
    // While indeed this is an inexpensive and comforting check,
    // the multiplication overflows and hence makes the check itself buggy.
    //BOOST_ASSERT(u.x*a % modulus == 1);
    return u.x;
}

}}
#endif


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

#include <iostream>
#include <fstream>
#include <utility>
#include <cmath>
#include <experimental/random>

#include <cstdlib>
#include <Eigen/unsupported/Eigen/FFT>

using namespace Eigen;

#include "kshape.h"

typedef Matrix<long double,Dynamic,Dynamic> MatrixXld;
typedef Matrix< std::complex< long double >, Dynamic, Dynamic > MatrixXcld;
typedef Matrix< long double, Dynamic, 1 > VectorXld;
typedef Matrix< std::complex< long double >, Dynamic, 1 > VectorXcld;

VectorXld coefWiseDivision(VectorXld x, VectorXld y){
    VectorXld ans(x.size());
    for (int i=0;i<x.size();++i){
          ans(i) = x(i)/y(i);
    }
    return ans;
}

VectorXld circshift(VectorXld vec,int shift)
{
  if (shift==0)
	{
		return vec;
	}

	int n = vec.size();
	VectorXld y(n);
  y.setZero();

	if (shift > 0) // shift right
	{
		y.head(shift) = vec.tail(shift);
		y.tail(n - shift) = vec.head(n - shift);
	}

  if(shift<0) // shift left
	{
		y.head(n + shift) = vec.tail(n + shift);
		y.tail(abs(shift)) = vec.head(abs(shift));

	}

	return y;
}

VectorXld shiftWithZeors(VectorXld vec, int shift){
  int n = vec.size();
  VectorXld newvec(n);
  newvec.setZero();
  if (shift==0)
	{
    newvec.array() = vec.array();
		return newvec;
	}

  if (std::abs(shift)>n){
    return newvec;
  }

	 if (shift > 0) // shift right
	{
		newvec.tail(n - shift) = vec.head(n - shift);
    //vec.head(shift).setZero();
	}

  if(shift<0) // shift left
	{
		newvec.head(n + shift) = vec.tail(n + shift);
		//vec.tail(abs(shift)).setZero();
	}

	return newvec;
}

VectorXld NCC(VectorXld x, VectorXld y){
    long double normed =  x.norm()*y.norm();
    if (normed == 0){
        normed =  1000000000000.0 ;
    }

    int x_len = x.size();

    int fft_size = 1 << (int)ceil(log2((2*x_len - 1)));

    FFT<long double> fft;
    VectorXcld x_out;

    VectorXld transformed_x(fft_size);
    transformed_x.setZero();

    transformed_x.head(x_len) = x;
    fft.fwd(x_out, transformed_x);

    VectorXcld y_out;
    VectorXld transformed_y(fft_size);
    transformed_y.setZero();
    transformed_y.head(y.size()) = y;
    fft.fwd(y_out, transformed_y);

    VectorXld cc_out;
    VectorXcld inter =  x_out.cwiseProduct( y_out.conjugate() );
    fft.inv(cc_out, inter);

    VectorXld cc(2*x_len - 1);
    cc.setZero();
    cc.head(x_len-1) = cc_out.tail(x_len-1);
    cc.tail(x_len) = cc_out.head(x_len);

    return cc.real() / normed;
}

std::vector<MatrixXld> NCC3D(MatrixXld x, MatrixXld y){
    VectorXld x_norm = x.rowwise().norm();
    VectorXld y_norm = y.rowwise().norm();
    MatrixXld den(x_norm.size(),y_norm.size());
    for (int i = 0;i<x_norm.size();++i){
          den.row(i) = x_norm(i)*(y_norm);
    }
    den = den.unaryExpr([](long double v) { return v==0.0 ? 1000000000.0 : v; });
    int x_len = x.cols();
    int y_len = y.cols();

    int fft_size = 1 << (int)ceil(log2((2*x_len - 1)));

    FFT<long double> fft;

    MatrixXcld x_out;
    MatrixXcld y_out;

    std::vector<MatrixXld> cc(y.rows());

    for (int k=0;k<y.rows();++k){

      VectorXld y_in(fft_size);
      y_in.setZero();
      VectorXcld fft_y_out;

      y_in.head(y_len) = y.row(k);
      fft.fwd(fft_y_out, y_in);

      MatrixXld cc_row(x.rows(), 2*x_len-1);
      for (int j=0;j<x.rows();++j){
          VectorXld x_in(fft_size);
          x_in.setZero();
          VectorXcld fft_x_out;

          x_in.head(x_len) = x.row(j);
          fft.fwd(fft_x_out, x_in);
          VectorXld inverted_product;
          VectorXcld t1 = fft_x_out.cwiseProduct(fft_y_out.conjugate());
          fft.inv(inverted_product, t1);

          cc_row.row(j).head(x_len-1) = inverted_product.tail(x_len-1);
          cc_row.row(j).tail(x_len) = inverted_product.head(x_len);
          cc_row.row(j) = cc_row.row(j).array() / den.coeff(j,k);
      }
      cc[k]=cc_row;
    }

    return cc;
}

std::pair<long double,VectorXld> SBD (VectorXld x, VectorXld y){
      VectorXld ncc = NCC(x, y);
      int idx;
      long double dist = 1.0 - ncc.maxCoeff(&idx);

      VectorXld yshift = shiftWithZeors(y, (idx+1)-std::max(x.size(),y.size()));

     return std::make_pair(dist, yshift);
}

MatrixXld z_norm(MatrixXld a, int axis = 0,int ddof=0){
      VectorXld means;
      VectorXld sstd;
      if (axis == 0){
        means = a.colwise().mean();
        int n = means.size();

        VectorXld sstd = VectorXld(n);
        for (int i=0;i<n;++i){
          sstd(i)=((a.col(i).array()-means(i)).pow(2).colwise().sum().sum()/(a.rows()-ddof));
        }

        sstd = sstd.array().pow(0.5);

        MatrixXld temp = (a.rowwise()-means.transpose());
        for (int i=0;i<n;i++){

          temp.col(i) = (temp.col(i)/(sstd(i)));
        }

        return temp;
      }
      else {
        means = a.rowwise().mean();
        int n = means.size();
        VectorXld sstd = VectorXld(n);
        for (int i=0;i<n;++i){
          sstd(i)=((a.row(i).array()-means(i)).pow(2).rowwise().sum().sum()/(a.cols()-ddof));
        }

        sstd = sstd.array().pow(0.5);

        MatrixXld temp = (a.colwise()-means);
        for (int i=0;i<n;i++){
          temp.row(i) = (temp.row(i)/(sstd(i)));
        }

        return temp;
      }
}

VectorXld extractShape(MatrixXld cluster, VectorXld cur_center){
    if (cluster.rows()==1){
      return cluster.row(0);
    }

      for (int i=0;i<cluster.rows();++i){
          if (cur_center.sum()!=0.0){
              cluster.row(i) = SBD(cur_center,cluster.row(i)).second;
          }
    }
    MatrixXld y = z_norm(cluster, 1,1);
    MatrixXld s = y.transpose()*y;

    int columns = cluster.cols();

    MatrixXld p(columns, columns);
    p.array() = 1.0/(long double)columns;

    p = MatrixXld::Identity(columns, columns) - p;

    MatrixXld m = (p.transpose()*s)*p;
    SelfAdjointEigenSolver<MatrixXld> eigensolver(m);
    MatrixXcld eigenVectors = eigensolver.eigenvectors();
    int cols = eigenVectors.cols();
    VectorXcld eigenvalues = eigensolver.eigenvalues();

    VectorXld centroid = eigenVectors.col(cols-1).real();

    long double dist1 = (cluster.row(0)-centroid.transpose()).array().pow(2).sum();
    long double dist2 = (cluster.row(0)+centroid.transpose()).array().pow(2).sum();

    if (dist1>=dist2){
      centroid.array() *= -1.0;
    }

    return z_norm(centroid,0,1);

  }

std::pair<std::vector<int>, MatrixXld> kshape(MatrixXld x, int k, int runs_to_average_over){
    int m = x.rows();
    int n = x.cols();

    std::vector<int> vidx(m);
    std::vector<int> bestidx(m);

    MatrixXld bestcentroids(k,n);
    bestcentroids.setZero();

    int number_in_center[k];
    long double bestdist=0.0;

    for (int run=0;run<runs_to_average_over;++run){
          MatrixXld centroids(k,n);
          centroids.setZero();

          bool changed;
          long double dist;
          memset(number_in_center,0,sizeof(number_in_center));

          for (int i=0;i<m;i++){
            int rint = std::experimental::randint(0, k-1);
            vidx[i] = rint;
            number_in_center[rint] += 1;
          }

          for (int iter=0;iter<100;++iter){
            std::cout << "Commence Iteration " << iter;
            for (int j=0;j<k;++j){
                int center_j_size = number_in_center[j];
                MatrixXld center_j = MatrixXld(center_j_size, n);
                int row = 0;
                for (int l=0;l<m;++l){
                    if (vidx[l]==j){
                        center_j.row(row) = x.row(l);
                        ++row;
                    }
                }

                if (row==0){
                  centroids.row(j).array() = 0;
                }
                else {
                  centroids.row(j) = extractShape(center_j, centroids.row(j)).transpose();
                }

            }

            std::vector<MatrixXld> x_corr = NCC3D(x, centroids);

            MatrixXld distances(m, k);

            for (int i=0;i<k;++i){
              distances.col(i) = 1 - x_corr[i].rowwise().maxCoeff().array();
            }
            memset(number_in_center,0,sizeof(number_in_center));
            changed = false;
            dist = 0.0;
            for (int i=0;i<x.rows();++i){
              int tempi;
              dist+=std::abs(distances.row(i).minCoeff(&tempi));
              if (vidx[i]!=tempi){
                vidx[i]=tempi;
                changed = true;
              }
              number_in_center[tempi]++;
            }

            std::cout << "\r";
            if (!changed){
              if (dist>bestdist){
                bestdist=dist;
                bestidx=vidx;
                bestcentroids = centroids;
              }
              break;
            }
          }

          if (changed){
            if (dist>bestdist){
              bestdist=dist;
              bestidx=vidx;
              bestcentroids = centroids;
            }
          }
    }

    return std::make_pair(bestidx, bestcentroids);
}

MatrixXld read_csv(std::string filename, int rows, int cols){
      std::ifstream file(filename);
      MatrixXld mat(rows,cols);

      std::string line;
      long double val; int row=0;

      while(std::getline(file, line))
      {
          std::stringstream ss(line);
          // Keep track of the current column index
          int col = 0;
          // Extract each integer
          while(ss >> val){
              // Add the current integer to the 'colIdx' column's values vector
              mat(row,col) = val;
              // If the next token is a comma, ignore it and move on
              if(ss.peek() == ',') ss.ignore();
              // Increment the column index
              col++;
          }
        row++;
      }

      file.close();

      return z_norm(mat,1,1);
}

void write_csv(std::string filename, MatrixXld mat){
      std::ofstream file(filename);

      for(int i = 0; i < mat.rows(); ++i)
      {
          for(int j = 0; j < mat.cols(); ++j)
          {
              file << mat(i,j);
              if(j != mat.cols() - 1) file << ",";
          }
          file << "\n";
      }
      file.close();

}

void write_csv(std::string filename, std::vector<int> v){
      std::ofstream file(filename);

      for(int j = 0; j < v.size(); ++j){
            file << v[j];
            if(j != v.size() - 1) file << ",";
      }
      file << "\n";
      file.close();
}

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

  if (argc < 6){
    std::cerr << "Need to speficy a csv, and its number of rows and columns, as well as K and a number of runs" << std::endl;
    return 1;
  }

  std::string csv_filename = std::string(argv[1]);
  int rows = atoi(argv[2]), cols = atoi(argv[3]), k = atoi(argv[4]), runs=atoi(argv[5]);

  MatrixXld mat = read_csv(csv_filename, rows, cols);
  mat = z_norm(mat, 1,1);
  std::pair<std::vector<int>, MatrixXld> result = kshape(mat, k, runs);
  write_csv("out_centroids.csv", result.second);
  write_csv("out_indices.csv", result.first);
  std::vector<int> cluster_counts(k);
  for (auto i: result.first) cluster_counts[i]++;
  std::cout << std::endl;
  for (int i=0;i<k;++i){
    std::cout << i << ": " << cluster_counts[i] << std::endl;
  }

  return 0;
}