// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG, www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also tools/license/license.mtl.txt in the distribution.

#include <iostream>
#include <complex>

#include <boost/type_traits.hpp>

#define MTL_VERBOSE_TEST                                   //  To print out everything

// #include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/operations.hpp>
#include <boost/numeric/mtl/io/test_ostream.hpp>

// using namespace std;

using namespace mtl::io;

template <typename Vector>
struct is_float_vec
  : boost::is_floating_point<typename mtl::Collection<Vector>::value_type>
{};

template <typename Vector>
struct is_int_vec
  : boost::is_integral<typename mtl::Collection<Vector>::value_type>
{};

static int errors = 0, tests = 0;

void expect(bool cond, std::string msg)
{
    if (!cond) {
        std::cerr << "ERROR: " << msg << std::endl;
        ++errors;
    }
    ++tests;
}

template <typename Vector>
void float_only_test(Vector&, const char*, boost::integral_constant<bool, false>)
{
}

template <typename Vector>
void float_only_test(Vector& u, const char* name, boost::integral_constant<bool, true>)
{
    Vector v(5);

    for (int i = 0; i < 5; ++i)
        u[i] = i / 4.0;
    tout << "Non-complex-" << name << ": u = " << u << "\n";

    // Inverse trigonometric functions
    v = acos(u);
    tout << "acos(u) = " << v << "\n";
    expect(std::abs(v[0] - M_PI/2.0) < 0.0001, "acos(0) should be pi/2.");

    v = asin(u);
    tout << "asin(u) = " << v << "\n";
    expect(std::abs(v[4] - M_PI/2.0) < 0.0001, "asin(1) should be pi/2.");

    v = atan(u);
    tout << "atan(u) = " << v << "\n";
    expect(std::abs(v[4] - M_PI/4.0) < 0.0001, "atan(1) should be pi/4.");

# ifdef MTL_WITH_MATH_ELEVEN
    for (int i = 0; i < 5; ++i)
        u[i] = 1. + i / 2.0;
    tout << "u = " << u << "\n";
    v = acosh(u);
    tout << "acosh(u) = " << v << "\n";
    expect(std::abs(v[2] - 1.31696)  < 0.0001, "acosh(2) should be about 1.31696.");

    v = asinh(u);
    tout << "asinh(u) = " << v << "\n";
    expect(std::abs(v[2] - 1.44363)  < 0.0001, "asinh(2) should be about 1.44363.");

    for (int i = 0; i < 5; ++i)
        u[i] = -0.8 + i * 0.4;
    tout << "u = " << u << "\n";
    v = atanh(u);
    tout << "atanh(u) = " << v << "\n";
    expect(std::abs(v[4] - 1.098612289)  < 0.0001, "atanh(0.8) should be about 1.098612289.");
# endif

    // Rounding operations
    for (int i = 0; i < 5; ++i)
        u[i] = -1. + 0.4 * i;
    tout << "u = " << u << "\n";

    v = ceil(u);
    tout << "ceil(u) = " << v << "\n";
    expect(v[1] == 0.0, "ceil(-0.6) should be 0.");
    expect(v[4] == 1.0, "ceil(0.6) should be 1.");

    v = floor(u);
    tout << "floor(u) = " << v << "\n";
    expect(v[1] == -1.0, "floor(-0.6) should be -1.");
    expect(v[4] == 0.0, "floor(0.6) should be 0.");

# ifdef MTL_WITH_MATH_ELEVEN
    v = round(u);
    tout << "round(u) = " << v << "\n";
    expect(v[1] == -1.0, "round(-0.6) should be -1.");
    expect(v[4] == 1.0, "round(0.6) should be 1.");

    v = trunc(u);
    tout << "trunc(u) = " << v << "\n";
    expect(v[1] == 0.0, "trunc(-0.6) should be 0.");
    expect(v[4] == 0.0, "trunc(0.6) should be 0.");

# endif

}

template <typename Vector>
void non_int_test(Vector&, const char*, boost::integral_constant<bool, true>)
{
}

template <typename Vector>
void non_int_test(Vector& u, const char* name, boost::integral_constant<bool, false>)
{
    Vector v(5);
    // Trigonometric functions
    for (int i = 0; i < 5; ++i)
        u[i] = i * M_PI / 8.0;
    tout << name << ": u = " << u << "\n";

    v = acos(u);
    tout << "cos(u) = " << v << "\n";
    expect(std::abs(v[2] - 0.667457) < 0.0001, "cos(pi/4) should be 0.667457.");

    v = sin(u);
    tout << "sin(u) = " << v << "\n";
    expect(std::abs(v[4] - 1.0) < 0.0001, "asin(pi/2) should be 1.");

    v = tan(u);
    tout << "tan(u) = " << v << "\n";
    expect(std::abs(v[2] - 1) < 0.0001, "tan(pi/4) should be 1.");

    v = cosh(u);
    tout << "acosh(u) = " << v << "\n";
    expect(std::abs(v[4] - 2.50918)  < 0.0001, "acosh(pi/2) should be about 2.50918.");

    v = sinh(u);
    tout << "sinh(u) = " << v << "\n";
    expect(std::abs(v[4] - 2.3013)  < 0.001, "sinh(pi/2) should be about 2.3013.");

    v = tanh(u);
    tout << "tanh(u) = " << v << "\n";
    expect(std::abs(v[4] - 0.917152)  < 0.0001, "tanh(pi/2) should be about 0.917152.");

    iota(u, 1);
    tout << name << ": u = " << u << "\n";

    v = log(u);
    tout << "log(u) = " << v << "\n";
    expect(std::abs(v[4] - 1.60944)  < 0.0001, "log(5) should be about 1.60944.");

# ifdef MTL_WITH_MATH_ELEVEN
    v = log10(u);
    tout << "log10(u) = " << v << "\n";
    expect(std::abs(v[4] - 0.69897)  < 0.0001, "log10(5) should be about 0.69897.");
# endif

}

template <typename Vector>
void int_float_test(Vector& u, const char* name, boost::integral_constant<bool, true>)
{
    iota(u, -2);
    Vector v(5);
    tout << name << ": u = " << u << "\n";

    // Test that integer values should invariant regarding rounding operations
    v = ceil(u);
    tout << "ceil(u) = " << v << "\n";
    expect(v[1] == -1.0, "ceil(-2) should be -1.");
    expect(v[4] == 2.0, "ceil(2) should be 2.");

    v = floor(u);
    tout << "floor(u) = " << v << "\n";
    expect(v[1] == -1.0, "floor(-2) should be -1.");
    expect(v[4] == 2.0, "floor(2) should be 2.");

# ifdef MTL_WITH_MATH_ELEVEN
    v = round(u);
    tout << "round(u) = " << v << "\n";
    expect(v[1] == -1.0, "round(-2) should be -1.");
    expect(v[4] == 2.0, "round(2) should be 2.");

    v = trunc(u);
    tout << "trunc(u) = " << v << "\n";
    expect(v[1] == -1.0, "trunc(-2) should be -1.");
    expect(v[4] == 2.0, "trunc(2) should be 2.");

    iota(u, 1);

    v = log2(u);
    tout << "log2(u) = " << v << "\n";
    typename Vector::value_type log2_ref = is_int_vec<Vector>::value  ? 2.0 : 2.32193;
    expect(std::abs(v[4] - log2_ref)  < 0.0001, "log2(5) wrong."); // not elegant

# endif

}

template <typename Vector>
void int_float_test(Vector&, const char*, boost::integral_constant<bool, false>)
{
}

template <typename Vector>
void all_test(Vector& u, const char* name)
{
    iota(u, -2);
    Vector v(5);

    tout << name << ": u = " << u << "\n";
    v = abs(u);
    tout << "abs(u) = " << v << "\n";
    expect(v[0] == 2.0, "abs(-2) should be 2");
    expect(v[4] == 2.0, "abs(2) should be 2");
}

template <typename Vector>
void test(const char* name)
{
    Vector u(5);
    all_test(u, name);
    int_float_test(u, name, boost::integral_constant<bool, is_int_vec<Vector>::value || is_float_vec<Vector>::value>());
    non_int_test(u, name, is_int_vec<Vector>());
    float_only_test(u, name, is_float_vec<Vector>());

    tout << "\n\n";
}

int main(int, char**)
{
    using mtl::vec::parameters;
    using namespace mtl;

    tout << "Testing vector operations\n\n";

    test<dense_vector<int> >("test int");
    test<dense_vector<float> >("test float");
    test<dense_vector<double> >("test double");
# ifdef MTL_WITH_MATH_ELEVEN
    // acos is not defined for complex in C++98
    test<dense_vector<std::complex<double> > >("test complex<double>");
# endif
    test<dense_vector<float, parameters<row_major> > >("test float in row vector");

    std::cout << errors << " encountered in " << tests << ".\n";
    return errors == 0 ? 0 : 1;
}


#ifndef CPPID_GM3D_MAT3_HPP
#define CPPID_GM3D_MAT3_HPP

#include <boost/qvm/all.hpp>

#include "mat4.hpp"

namespace cppid::gm3d {

template<typename T>
struct mat3 {
  T m[9];

  constexpr mat3() noexcept = default;

  explicit constexpr mat3(T s) noexcept
  : mat3{s, 0, 0, 0, s, 0, 0, 0, s}
  {
  }

  constexpr mat3(
   T m00, T m01, T m02, T m10, T m11, T m12, T m20, T m21, T m22) noexcept
  : m{m00, m01, m02, m10, m11, m12, m20, m21, m22}
  {
  }

  template<typename U,
           std::enable_if_t<std::is_constructible_v<T, U> &&
                            !std::is_convertible_v<U, T>>* = nullptr>
  explicit constexpr mat3(const mat4<U>& m4) noexcept
  : mat3(m4.m[0],
         m4.m[1],
         m4.m[2],
         m4.m[4],
         m4.m[5],
         m4.m[6],
         m4.m[8],
         m4.m[9],
         m4.m[10])
  {
  }

  template<typename U,
           std::enable_if_t<std::is_constructible_v<T, U> &&
                            std::is_convertible_v<U, T>>* = nullptr>
  mat3(const mat4<U>& m4) noexcept
  : mat3(m4.m[0],
         m4.m[1],
         m4.m[2],
         m4.m[4],
         m4.m[5],
         m4.m[6],
         m4.m[8],
         m4.m[9],
         m4.m[10])
  {
  }

  template<typename U,
           std::enable_if_t<std::is_constructible_v<T, U> &&
                            !std::is_convertible_v<U, T>>* = nullptr>
  explicit constexpr mat3(const mat3<U>& m3) noexcept
  : mat3(m3.m[0],
         m3.m[1],
         m3.m[2],
         m3.m[3],
         m3.m[4],
         m3.m[5],
         m3.m[6],
         m3.m[7],
         m3.m[8])
  {
  }

  template<typename U,
           std::enable_if_t<std::is_constructible_v<T, U> &&
                            std::is_convertible_v<U, T>>* = nullptr>
  constexpr mat3(const mat3<U>& m3) noexcept
  : mat3(m3.m[0],
         m3.m[1],
         m3.m[2],
         m3.m[3],
         m3.m[4],
         m3.m[5],
         m3.m[6],
         m3.m[7],
         m3.m[8])
  {
  }

  constexpr void m00(T val) noexcept
  {
    m[0] = val;
  }

  constexpr void m01(T val) noexcept
  {
    m[1] = val;
  }

  constexpr void m02(T val) noexcept
  {
    m[2] = val;
  }

  constexpr void m10(T val) noexcept
  {
    m[3] = val;
  }

  constexpr void m11(T val) noexcept
  {
    m[4] = val;
  }

  constexpr void m12(T val) noexcept
  {
    m[5] = val;
  }

  constexpr void m20(T val) noexcept
  {
    m[6] = val;
  }

  constexpr void m21(T val) noexcept
  {
    m[7] = val;
  }

  constexpr void m22(T val) noexcept
  {
    m[8] = val;
  }

  constexpr auto m00() const noexcept -> T
  {
    return m[0];
  }

  constexpr auto m01() const noexcept -> T
  {
    return m[1];
  }

  constexpr auto m02() const noexcept -> T
  {
    return m[2];
  }

  constexpr auto m10() const noexcept -> T
  {
    return m[3];
  }

  constexpr auto m11() const noexcept -> T
  {
    return m[4];
  }

  constexpr auto m12() const noexcept -> T
  {
    return m[5];
  }

  constexpr auto m20() const noexcept -> T
  {
    return m[6];
  }

  constexpr auto m21() const noexcept -> T
  {
    return m[7];
  }

  constexpr auto m22() const noexcept -> T
  {
    return m[8];
  }
};

} // namespace cppid::gm3d

namespace boost::qvm {

template<class T>
struct mat_traits<cppid::gm3d::mat3<T>> {
  using scalar_type = T;
  static int const rows = 3;
  static int const cols = 3;

  template<int Row, int Col>
  static constexpr auto read_element(cppid::gm3d::mat3<scalar_type> const& x)
   -> scalar_type
  {
    return x.m[Row * 3 + Col];
  }

  template<int Row, int Col>
  static constexpr auto write_element(cppid::gm3d::mat3<scalar_type>& x)
   -> scalar_type&
  {
    return x.m[Row * 3 + Col];
  }

  static constexpr auto
  read_element_idx(int row, int col, cppid::gm3d::mat3<scalar_type> const& x)
   -> scalar_type
  {
    return x.m[row * 3 + col];
  }

  static constexpr auto
  write_element_idx(int row, int col, cppid::gm3d::mat3<scalar_type>& x)
   -> scalar_type&
  {
    return x.m[row * 3 + col];
  }
};
} // namespace boost::qvm

#endif


#include "meshview/util.hpp"

#include <fstream>
#include <iostream>
#include <Eigen/Geometry>

#include "meshview/common.hpp"
#include "meshview/internal/assert.hpp"

namespace meshview {
namespace util {

Matrix4f persp(float xscale, float yscale, float z_near, float z_far) {
    Matrix4f m;
    m << xscale, 0.f, 0.f, 0.f, 0.f, yscale, 0.f, 0.f, 0.f, 0.f,
        -(z_far + z_near) / (z_far - z_near),
        -2.f * z_near * z_far / (z_far - z_near), 0.f, 0.f, -1.f, 0.f;
    // 0.f, 0.f, z_far / (z_near - z_far), -(z_far * z_near) / (z_far - z_near),
    // 0.f, 0.f, -1.f, 0.f;
    return m;
}

Matrix4f look_at(const Eigen::Ref<const Vector3f>& pos,
                 const Eigen::Ref<const Vector3f>& fw,
                 const Eigen::Ref<const Vector3f>& up) {
    Matrix4f m;
    m.topLeftCorner<1, 3>() = fw.cross(up).transpose();
    m.block<1, 3>(1, 0) = up.transpose();
    m.block<1, 3>(2, 0) = -fw.transpose();
    m.bottomLeftCorner<1, 3>().setZero();
    m(3, 3) = 1.f;
    m.topRightCorner<3, 1>() = -m.topLeftCorner<3, 3>() * pos;
    return m;
}

void estimate_normals(const Eigen::Ref<const Points>& verts,
                      const Eigen::Ref<const Triangles>& faces,
                      Eigen::Ref<Points> out) {
    if (faces.rows() == 0) {
        // Defer to 'no-element-buffer' version
        estimate_normals(verts, out);
        return;
    }
    Vector face_cnt(verts.rows());
    face_cnt.setZero();

    out.setZero();

    Eigen::RowVector3f face_normal;
    for (int i = 0; i < faces.rows(); ++i) {
        face_normal =
            (verts.row(faces(i, 1)) - verts.row(faces(i, 0)))
                .cross(verts.row(faces(i, 2)) - verts.row(faces(i, 1)))
                .normalized();
        for (int j = 0; j < 3; ++j) {
            out.row(faces(i, j)) += face_normal;
            face_cnt[faces(i, j)] += 1.f;
        }
    }
    out.array().colwise() /= face_cnt.array();
}

void estimate_normals(const Eigen::Ref<const Points>& verts,
                      Eigen::Ref<Points> out) {
    Vector face_cnt(verts.rows());
    face_cnt.setZero();
    out.setZero();
    Vector3f face_normal;
    for (int i = 0; i < verts.rows(); i += 3) {
        face_cnt.segment<3>(i).array() += 1.f;
        out.middleRows<3>(i).rowwise() +=
            (verts.row(i + 1) - verts.row(i))
                .cross(verts.row(i + 2) - verts.row(i + 1))
                .normalized();
    }
    out.array().colwise() /= face_cnt.array();
}

Eigen::Matrix<Index, Eigen::Dynamic, 1> make_uv_to_vert_map(
    size_t num_uv_verts, const Eigen::Ref<const Triangles>& tri_faces,
    const Eigen::Ref<const Triangles>& uv_tri_faces) {
    _MESHVIEW_ASSERT_EQ(tri_faces.rows(), uv_tri_faces.rows());
    Eigen::Matrix<Index, Eigen::Dynamic, 1> result(num_uv_verts);
    result.setConstant(-1);
    for (size_t i = 0; i < uv_tri_faces.rows(); ++i) {
        for (size_t j = 0; j < 3; ++j) {
            result[uv_tri_faces(i, j)] = tri_faces(i, j);
        }
    }
    // Each uv vertex must be matched to some vertex
    for (size_t i = 0; i < num_uv_verts; ++i) {
        _MESHVIEW_ASSERT_NE(result[i], (Index)-1);
    }
    return result;
}

}  // namespace util
}  // namespace meshview


/**
 * @file   testGradientDescentOptimizer.cpp
 * @brief
 * @author ydjian
 * @date   Jun 11, 2012
 */

#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Pose2.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/tuple/tuple.hpp>

using namespace std;
using namespace gtsam;

boost::tuple<NonlinearFactorGraph, Values> generateProblem() {

  // 1. Create graph container and add factors to it
  NonlinearFactorGraph graph ;

  // 2a. Add Gaussian prior
  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
  SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
  graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));

  // 2b. Add odometry factors
  SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
  graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0   ), odometryNoise));
  graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
  graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
  graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise));

  // 2c. Add pose constraint
  SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
  graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty));

  // 3. Create the data structure to hold the initialEstimate estinmate to the solution
  Values initialEstimate;
  Pose2 x1(0.5, 0.0, 0.2   ); initialEstimate.insert(1, x1);
  Pose2 x2(2.3, 0.1,-0.2   ); initialEstimate.insert(2, x2);
  Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insert(3, x3);
  Pose2 x4(4.0, 2.0, M_PI  ); initialEstimate.insert(4, x4);
  Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insert(5, x5);

  return boost::tie(graph, initialEstimate);
}

/* ************************************************************************* */
TEST(optimize, GradientDescentOptimizer) {

  NonlinearFactorGraph graph;
  Values initialEstimate;

  boost::tie(graph, initialEstimate) = generateProblem();
  // cout << "initial error = " << graph.error(initialEstimate) << endl ;

  // Single Step Optimization using Levenberg-Marquardt
  NonlinearOptimizerParams param;
  param.maxIterations = 500;    /* requires a larger number of iterations to converge */
  param.verbosity = NonlinearOptimizerParams::SILENT;

  NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param);
  Values result = optimizer.optimize();
//  cout << "gd1 solver final error = " << graph.error(result) << endl;

  /* the optimality of the solution is not comparable to the */
  DOUBLES_EQUAL(0.0, graph.error(result), 1e-2);

  CHECK(1);
}

/* ************************************************************************* */
TEST(optimize, ConjugateGradientOptimizer) {

  NonlinearFactorGraph graph;
  Values initialEstimate;

  boost::tie(graph, initialEstimate) = generateProblem();
//  cout << "initial error = " << graph.error(initialEstimate) << endl ;

  // Single Step Optimization using Levenberg-Marquardt
  NonlinearOptimizerParams param;
  param.maxIterations = 500;    /* requires a larger number of iterations to converge */
  param.verbosity = NonlinearOptimizerParams::SILENT;

  NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param);
  Values result = optimizer.optimize();
//  cout << "cg final error = " << graph.error(result) << endl;

  /* the optimality of the solution is not comparable to the */
  DOUBLES_EQUAL(0.0, graph.error(result), 1e-2);
}

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


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// This file is part of the Boost Graph Library
//
// You should have received a copy of the License Agreement for the
// Boost Graph Library along with the software; see the file LICENSE.
// If not, contact Office of Research, Indiana University,
// Bloomington, IN 47405.
//
// Permission to modify the code and to distribute the code is
// granted, provided the text of this NOTICE is retained, a notice if
// the code was modified is included with the above COPYRIGHT NOTICE
// and with the COPYRIGHT NOTICE in the LICENSE file, and that the
// LICENSE file is distributed with the modified code.
//
// LICENSOR MAKES NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED.
// By way of example, but not limitation, Licensor MAKES NO
// REPRESENTATIONS OR WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY
// PARTICULAR PURPOSE OR THAT THE USE OF THE LICENSED SOFTWARE COMPONENTS
// OR DOCUMENTATION WILL NOT INFRINGE ANY PATENTS, COPYRIGHTS, TRADEMARKS
// OR OTHER RIGHTS.
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>

int
main()
{
  using namespace boost;
  typedef adjacency_list < vecS, vecS, undirectedS,
    property<vertex_distance_t, int>, property < edge_weight_t, int > > Graph;
  typedef std::pair < int, int >E;
  const int num_nodes = 5;
  E edges[] = { E(0, 2), E(1, 3), E(1, 4), E(2, 1), E(2, 3),
    E(3, 4), E(4, 0)
  };
  int weights[] = { 1, 1, 2, 7, 3, 1, 1 };
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  Graph g(num_nodes);
  property_map<Graph, edge_weight_t>::type weightmap = get(edge_weight, g);
  for (std::size_t j = 0; j < sizeof(edges) / sizeof(E); ++j) {
    graph_traits<Graph>::edge_descriptor e; bool inserted;
    tie(e, inserted) = add_edge(edges[j].first, edges[j].second, g);
    weightmap[e] = weights[j];
  }
#else
  Graph g(edges, edges + sizeof(edges) / sizeof(E), weights, num_nodes);
  property_map<Graph, edge_weight_t>::type weightmap = get(edge_weight, g);
#endif
  std::vector < graph_traits < Graph >::vertex_descriptor >
    p(num_vertices(g));

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  property_map<Graph, vertex_distance_t>::type distance = get(vertex_distance, g);
  property_map<Graph, vertex_index_t>::type indexmap = get(vertex_index, g);
  prim_minimum_spanning_tree
    (g, *vertices(g).first, &p[0], distance, weightmap, indexmap,
     default_dijkstra_visitor());
#else
  prim_minimum_spanning_tree(g, &p[0]);
#endif

  for (std::size_t i = 0; i != p.size(); ++i)
    if (p[i] != i)
      std::cout << "parent[" << i << "] = " << p[i] << std::endl;
    else
      std::cout << "parent[" << i << "] = no parent" << std::endl;

  return EXIT_SUCCESS;
}


/**
 * Copyright 2020, Massachusetts Institute of Technology,
 * Cambridge, MA 02139
 * All Rights Reserved
 * Authors: Jingnan Shi, et al. (see THANKS for the full author list)
 * See LICENSE for the license information
 */

#include <array>
#include <iostream>
#include <fstream>
#include <random>
#include <unordered_map>
#include <algorithm>
#include <chrono>

#include "gtest/gtest.h"
#include "gmock/gmock.h"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "teaser/certification.h"
#include "test_utils.h"

/**
 * Acceptable numerical error threshold for all certification tests
 */
const float ACCEPTABLE_ERROR = 1e-7;

/**
 * Test fixture for loading data case by case
 */
class DRSCertifierTest : public ::testing::Test {
protected:
  /**
   * Inputs for the DRSCertifier
   *
   * Note that some may be intermediate outputs from other functions.
   */
  struct Inputs {
    teaser::DRSCertifier::Params params;
    Eigen::Matrix<float, 3, Eigen::Dynamic> v1;
    Eigen::Matrix<float, 3, Eigen::Dynamic> v2;
    Eigen::Matrix3f R_est;
    Eigen::Quaternion<float> q_est;
    Eigen::Matrix<float, 1, Eigen::Dynamic> theta_est;
    Eigen::MatrixXf W;
    Eigen::MatrixXf M_affine;
    float mu;
  };

  /**
   * Expected outputs for methods under the DRSCertifier
   */
  struct ExpectedOutputs {
    Eigen::Matrix4f omega;
    Eigen::MatrixXf block_diag_omega;
    Eigen::MatrixXf Q_cost;
    Eigen::MatrixXf lambda_guess;
    Eigen::MatrixXf A_inv;
    Eigen::MatrixXf W_dual;
    float suboptimality_1st_iter;
    teaser::CertificationResult certification_result;
  };

  struct CaseData {
    Inputs inputs;
    ExpectedOutputs expected_outputs;
  };

  /**
   * Helper function to load parameters
   * @param file_path
   * @param cbar2
   * @param noise_bound
   */
  void loadScalarParameters(std::string file_path, teaser::DRSCertifier::Params* params) {
    // Open the file
    std::ifstream file;
    file.open(file_path);
    if (!file) {
      std::cerr << "Unable to open file: " << file_path << "." << std::endl;
      exit(1);
    }
    std::string line;
    std::string delimiter = ":";
    while (std::getline(file, line)) {

      size_t delim_idx = line.find(delimiter, 0);
      std::string param = line.substr(0, delim_idx);
      std::string value = line.substr(delim_idx + 2, line.length());

      if (param == "cbar2") {
        params->cbar2 = std::stod(value);
      } else if (param == "noise_bound") {
        params->noise_bound = std::stod(value);
      } else if (param == "max_iterations") {
        params->max_iterations = std::stod(value);
      }
    }
  }

  /**
   * Helper function to compare two certification results
   * @param actual_result
   * @param expected_result
   */
  void compareCertificationResult(teaser::CertificationResult actual_result,
                                  teaser::CertificationResult expected_result) {

    EXPECT_EQ(actual_result.suboptimality_traj.size(), expected_result.suboptimality_traj.size())
        << "Mismatch in suboptimality trajectory sizes.";

    auto act_itr = actual_result.suboptimality_traj.begin();
    auto act_end_itr = actual_result.suboptimality_traj.end();
    auto exp_itr = expected_result.suboptimality_traj.begin();
    auto exp_end_itr = expected_result.suboptimality_traj.end();
    while (act_itr != act_end_itr && exp_itr != exp_end_itr &&
           std::abs(*act_itr - *exp_itr) < ACCEPTABLE_ERROR) {
      ++act_itr, ++exp_itr;
    }
    EXPECT_TRUE(act_itr == act_end_itr && exp_itr == exp_end_itr)
        << "Mismatch between actual and expected suboptimality trajectory values.";

    ASSERT_TRUE(std::abs(actual_result.best_suboptimality - expected_result.best_suboptimality) <
                ACCEPTABLE_ERROR)
        << "Incorrect best optimality gap.";
  }

  /**
   * Helper function to set up small certification test instances
   */
  void setupSmallInstances() {
    std::string root_dir = "./data/certification_small_instances/";
    auto cases = teaser::test::readSubdirs(root_dir);
    for (const auto& c : cases) {
      std::string case_dir = root_dir + c;

      CaseData data;

      // Inputs:
      // scalar parameters
      loadScalarParameters(case_dir + "/parameters.txt", &(data.inputs.params));

      // v1: 3-by-N matrix
      // These are the TIMs
      std::ifstream v1_source_file(case_dir + "/v1.csv");
      data.inputs.v1 =
          teaser::test::readFileToEigenMatrix<float, 3, Eigen::Dynamic>(v1_source_file);

      // v2: 3-by-N matrix
      // These are the TIMs
      std::ifstream v2_source_file(case_dir + "/v2.csv");
      data.inputs.v2 =
          teaser::test::readFileToEigenMatrix<float, 3, Eigen::Dynamic>(v2_source_file);

      // q_est: estimated quaternion
      std::ifstream q_source_file(case_dir + "/q_est.csv");
      auto q_mat = teaser::test::readFileToEigenFixedMatrix<float, 4, 1>(q_source_file);
      Eigen::Quaternion<float> q(q_mat(3), q_mat(0), q_mat(1), q_mat(2));
      data.inputs.q_est = q;

      // R_est: estimated quaternion
      std::ifstream R_est_source_file(case_dir + "/R_est.csv");
      data.inputs.R_est = teaser::test::readFileToEigenFixedMatrix<float, 3, 3>(R_est_source_file);

      // theta_est: binary outlier vector
      std::ifstream theta_source_file(case_dir + "/theta_est.csv");
      data.inputs.theta_est =
          teaser::test::readFileToEigenMatrix<float, 1, Eigen::Dynamic>(theta_source_file);

      // W: inputs for optimal dual projection
      std::ifstream W_source_file(case_dir + "/W_1st_iter.csv");
      data.inputs.W = teaser::test::readFileToEigenMatrix<float, Eigen::Dynamic, Eigen::Dynamic>(
          W_source_file);

      // M_affine: for calculating suboptimality
      std::ifstream M_affine_source_file(case_dir + "/M_affine_1st_iter.csv");
      data.inputs.M_affine =
          teaser::test::readFileToEigenMatrix<float, Eigen::Dynamic, Eigen::Dynamic>(
              M_affine_source_file);

      // mu: for calculating suboptimality
      std::ifstream mu_source_file(case_dir + "/mu.csv");
      data.inputs.mu = teaser::test::readFileToEigenFixedMatrix<float, 1, 1>(mu_source_file)(0);

      // Outputs:
      // omega: omega1 matrix
      std::ifstream omega_source_file(case_dir + "/omega.csv");
      data.expected_outputs.omega =
          teaser::test::readFileToEigenFixedMatrix<float, 4, 4>(omega_source_file);

      // block_diag_omega: block diagonal omega matrix
      std::ifstream block_diag_omega_source_file(case_dir + "/block_diag_omega.csv");
      data.expected_outputs.block_diag_omega =
          teaser::test::readFileToEigenMatrix<float, Eigen::Dynamic, Eigen::Dynamic>(
              block_diag_omega_source_file);

      // Q_cost: Q_cost matrix
      std::ifstream q_cost_source_file(case_dir + "/Q_cost.csv");
      data.expected_outputs.Q_cost =
          teaser::test::readFileToEigenMatrix<float, Eigen::Dynamic, Eigen::Dynamic>(
              q_cost_source_file);

      // lambda guess: initial guess
      std::ifstream lambda_guess_source_file(case_dir + "/lambda_bar_init.csv");
      data.expected_outputs.lambda_guess =
          teaser::test::readFileToEigenMatrix<float, Eigen::Dynamic, Eigen::Dynamic>(
              lambda_guess_source_file);

      // A_inv: inverse map from getLinearProjection
      std::ifstream A_inv_source_file(case_dir + "/A_inv.csv");
      data.expected_outputs.A_inv =
          teaser::test::readFileToEigenMatrix<float, Eigen::Dynamic, Eigen::Dynamic>(
              A_inv_source_file);

      // W_dual: output from optimal dual projection
      std::ifstream W_dual_source_file(case_dir + "/W_dual_1st_iter.csv");
      data.expected_outputs.W_dual =
          teaser::test::readFileToEigenMatrix<float, Eigen::Dynamic, Eigen::Dynamic>(
              W_dual_source_file);

      // suboptimality: calculated suboptimality after 1st iteration
      std::ifstream suboptimality_source_file(case_dir + "/suboptimality_1st_iter.csv");
      data.expected_outputs.suboptimality_1st_iter =
          teaser::test::readFileToEigenFixedMatrix<float, 1, 1>(suboptimality_source_file)(0);

      // certification_result: a struct holding certification results. Specifically:
      // suboptimality_trajy: suboptimality gaps throughout all the iterations
      // best_suboptimality: smallest suboptimality gap
      std::ifstream suboptimality_traj_source_file(case_dir + "/suboptimality_traj.csv");
      Eigen::RowVectorXf suboptimality_traj_mat =
          teaser::test::readFileToEigenMatrix<float, 1, Eigen::Dynamic>(
              suboptimality_traj_source_file);
      for (size_t i = 0; i < suboptimality_traj_mat.cols(); ++i) {
        data.expected_outputs.certification_result.suboptimality_traj.push_back(
            suboptimality_traj_mat(i));
      }
      data.expected_outputs.certification_result.best_suboptimality =
          suboptimality_traj_mat.minCoeff();
      data.expected_outputs.certification_result.is_optimal = true;

      small_instances_params_[c] = data;
    }
  }

  /**
   * Helper function to load parameters for large problem instances
   *
   * Large instances are only used for testing the main certification function
   */
  void setupLargeInstances() {
    std::string root_dir = "./data/certification_large_instances/";
    auto cases = teaser::test::readSubdirs(root_dir);
    for (const auto& c : cases) {
      std::string case_dir = root_dir + c;

      CaseData data;

      // Inputs:
      // scalar parameters
      loadScalarParameters(case_dir + "/parameters.txt", &(data.inputs.params));

      // v1: 3-by-N matrix
      // These are the TIMs
      std::ifstream v1_source_file(case_dir + "/v1.csv");
      data.inputs.v1 =
          teaser::test::readFileToEigenMatrix<float, 3, Eigen::Dynamic>(v1_source_file);

      // v2: 3-by-N matrix
      // These are the TIMs
      std::ifstream v2_source_file(case_dir + "/v2.csv");
      data.inputs.v2 =
          teaser::test::readFileToEigenMatrix<float, 3, Eigen::Dynamic>(v2_source_file);

      // q_est: estimated quaternion
      std::ifstream q_source_file(case_dir + "/q_est.csv");
      auto q_mat = teaser::test::readFileToEigenFixedMatrix<float, 4, 1>(q_source_file);
      Eigen::Quaternion<float> q(q_mat(3), q_mat(0), q_mat(1), q_mat(2));
      data.inputs.q_est = q;

      // R_est: estimated quaternion
      std::ifstream R_est_source_file(case_dir + "/R_est.csv");
      data.inputs.R_est = teaser::test::readFileToEigenFixedMatrix<float, 3, 3>(R_est_source_file);

      // theta_est: binary outlier vector
      std::ifstream theta_source_file(case_dir + "/theta_est.csv");
      data.inputs.theta_est =
          teaser::test::readFileToEigenMatrix<float, 1, Eigen::Dynamic>(theta_source_file);

      // suboptimality: calculated suboptimality after 1st iteration
      std::ifstream suboptimality_source_file(case_dir + "/suboptimality_1st_iter.csv");
      data.expected_outputs.suboptimality_1st_iter =
          teaser::test::readFileToEigenFixedMatrix<float, 1, 1>(suboptimality_source_file)(0);

      // certification_result: a struct holding certification results. Specifically:
      // suboptimality_trajy: suboptimality gaps throughout all the iterations
      // best_suboptimality: smallest suboptimality gap
      std::ifstream suboptimality_traj_source_file(case_dir + "/suboptimality_traj.csv");
      Eigen::RowVectorXf suboptimality_traj_mat =
          teaser::test::readFileToEigenMatrix<float, 1, Eigen::Dynamic>(
              suboptimality_traj_source_file);
      for (size_t i = 0; i < suboptimality_traj_mat.cols(); ++i) {
        data.expected_outputs.certification_result.suboptimality_traj.push_back(
            suboptimality_traj_mat(i));
      }
      data.expected_outputs.certification_result.best_suboptimality =
          suboptimality_traj_mat.minCoeff();
      data.expected_outputs.certification_result.is_optimal = true;

      large_instances_params_[c] = data;
    }
  }

  void SetUp() override {
    // load case parameters for small instances
    setupSmallInstances();

    // load case parameters for large instances
    setupLargeInstances();
  }

  /**
   * Helper function to run a provided function over a dictionary of case data
   * @tparam Functor
   * @param functor
   */
  template <typename Functor>
  void testThroughCases(std::string test_case_name, Functor functor,
                        const std::map<std::string, CaseData>& case_params) {
    // get all case names
    std::string ptr_str = "Timing info for test case: " + test_case_name;
    std::string div_str(ptr_str.size(), '=');
    std::cout << ptr_str << std::endl;
    std::cout << div_str << std::endl;
    for (auto const& kv : case_params) {
      const auto& case_name = kv.first;
      std::chrono::steady_clock clock;
      auto t1 = clock.now();
      functor(kv.second);
      auto t2 = clock.now();
      std::chrono::duration<float, std::milli> diff = t2 - t1;
      std::cout << "\nN=" << kv.second.inputs.v1.cols() << " | Test took "
                << static_cast<float>(diff.count()) / 1000.0 << "seconds." << std::endl;
    }
    std::cout << div_str << std::endl;
  }

  // parameters per case
  std::map<std::string, CaseData> small_instances_params_;
  std::map<std::string, CaseData> large_instances_params_;
};

TEST_F(DRSCertifierTest, GetOmega1) {
  auto test_run = [](CaseData case_data) {
    const auto& expected_output = case_data.expected_outputs.omega;

    // construct the certifier
    teaser::DRSCertifier certifier(case_data.inputs.params);

    // perform the computation
    Eigen::Matrix4f actual_output = certifier.getOmega1(case_data.inputs.q_est);
    ASSERT_TRUE(actual_output.isApprox(expected_output))
        << "Actual output: " << actual_output << "Expected output: " << expected_output;
  };

  testThroughCases(test_info_->name(), test_run, small_instances_params_);
}

TEST_F(DRSCertifierTest, GetBlockDiagOmega) {
  auto test_run = [](CaseData case_data) {
    // Case 1: N=10
    const auto& expected_output = case_data.expected_outputs.block_diag_omega;

    // construct the certifier
    teaser::DRSCertifier certifier(case_data.inputs.params);

    // perform the computation
    Eigen::MatrixXf actual_output;
    int Npm = (case_data.inputs.v1.cols() + 1) * 4;
    certifier.getBlockDiagOmega(Npm, case_data.inputs.q_est, &actual_output);
    ASSERT_TRUE(actual_output.isApprox(expected_output))
        << "Actual output: " << actual_output << "Expected output: " << expected_output;
  };

  testThroughCases(test_info_->name(), test_run, small_instances_params_);
}

TEST_F(DRSCertifierTest, GetQCost) {
  auto test_run = [](CaseData case_data) {
    const auto& expected_output = case_data.expected_outputs.Q_cost;

    // construct the certifier
    teaser::DRSCertifier certifier(case_data.inputs.params);

    // perform the computation
    Eigen::MatrixXf actual_output;
    certifier.getQCost(case_data.inputs.v1, case_data.inputs.v2, &actual_output);
    ASSERT_TRUE(actual_output.isApprox(expected_output))
        << "Actual output: " << actual_output << "Expected output: " << expected_output;
  };

  testThroughCases(test_info_->name(), test_run, small_instances_params_);
}

TEST_F(DRSCertifierTest, GetLambdaGuess) {
  auto test_run = [](CaseData case_data) {
    const auto& expected_output = case_data.expected_outputs.lambda_guess;

    // construct the certifier
    teaser::DRSCertifier certifier(case_data.inputs.params);

    teaser::SparseMatrix actual_output;
    certifier.getLambdaGuess(case_data.inputs.R_est, case_data.inputs.theta_est,
                             case_data.inputs.v1, case_data.inputs.v2, &actual_output);

    ASSERT_TRUE(actual_output.isApprox(expected_output))
        << "Actual output: " << actual_output << "Expected output: " << expected_output;
  };

  testThroughCases(test_info_->name(), test_run, small_instances_params_);
}

TEST_F(DRSCertifierTest, GetLinearProjection) {
  auto test_run = [](CaseData case_data) {
    const auto& expected_output = case_data.expected_outputs.A_inv;

    // construct the certifier
    teaser::DRSCertifier certifier(case_data.inputs.params);

    Eigen::Matrix<float, 1, Eigen::Dynamic> theta_prepended(1,
                                                             case_data.inputs.theta_est.cols() + 1);
    theta_prepended << 1, case_data.inputs.theta_est;
    ASSERT_TRUE(theta_prepended.rows() == 1);
    ASSERT_TRUE(theta_prepended.cols() > 0);

    teaser::SparseMatrix actual_output;
    certifier.getLinearProjection(theta_prepended, &actual_output);

    ASSERT_TRUE(actual_output.isApprox(expected_output))
        << "Actual output: " << actual_output << "Expected output: " << expected_output;
  };

  testThroughCases(test_info_->name(), test_run, small_instances_params_);
}

TEST_F(DRSCertifierTest, GetOptimalDualProjection) {
  auto test_run = [](CaseData case_data) {
    // prepare parameters
    // theta prepended
    Eigen::Matrix<float, 1, Eigen::Dynamic> theta_prepended(1,
                                                             case_data.inputs.theta_est.cols() + 1);
    theta_prepended << 1, case_data.inputs.theta_est;
    ASSERT_TRUE(theta_prepended.rows() == 1);
    ASSERT_TRUE(theta_prepended.cols() > 0);

    // A_inv
    teaser::SparseMatrix A_inv_sparse = case_data.expected_outputs.A_inv.sparseView();

    // construct the certifier
    teaser::DRSCertifier certifier(case_data.inputs.params);

    Eigen::MatrixXf actual_output;
    certifier.getOptimalDualProjection(case_data.inputs.W, theta_prepended, A_inv_sparse,
                                       &actual_output);

    const auto& expected_output = case_data.expected_outputs.W_dual;
    for (size_t col = 0; col < expected_output.cols(); ++col) {
      for (size_t row = 0; row < expected_output.rows(); ++row) {
        if (std::abs(actual_output(row, col) - expected_output(row, col)) > ACCEPTABLE_ERROR) {
          std::cout << "Row: " << row << " Col: " << col << " Value: " << actual_output(row, col)
                    << " Expected: " << expected_output(row, col) << std::endl;
        }
      }
    }
    ASSERT_TRUE(actual_output.isApprox(expected_output))
        << "Actual output: " << actual_output << "Expected output: " << expected_output;
  };

  testThroughCases(test_info_->name(), test_run, small_instances_params_);
}

TEST_F(DRSCertifierTest, ComputeSubOptimalityGap) {
  auto test_run = [](CaseData case_data) {
    // construct the certifier
    teaser::DRSCertifier certifier(case_data.inputs.params);

    float actual_output = certifier.computeSubOptimalityGap(
        case_data.inputs.M_affine, case_data.inputs.mu, case_data.inputs.v1.cols());
    const auto& expected_output = case_data.expected_outputs.suboptimality_1st_iter;

    ASSERT_TRUE(std::abs(actual_output - expected_output) < ACCEPTABLE_ERROR);
  };

  testThroughCases(test_info_->name(), test_run, small_instances_params_);
}

TEST_F(DRSCertifierTest, Certify) {
  auto test_run = [&](CaseData case_data) {
    // construct the certifier
    teaser::DRSCertifier certifier(case_data.inputs.params);

    auto actual_output = certifier.certify(case_data.inputs.R_est, case_data.inputs.v1,
                                           case_data.inputs.v2, case_data.inputs.theta_est);

    compareCertificationResult(actual_output, case_data.expected_outputs.certification_result);
  };

  testThroughCases(test_info_->name(), test_run, small_instances_params_);
}

TEST_F(DRSCertifierTest, LargeInstance) {
  auto test_run = [&](CaseData case_data) {
    // construct the certifier
    teaser::DRSCertifier certifier(case_data.inputs.params);

    auto actual_output = certifier.certify(case_data.inputs.R_est, case_data.inputs.v1,
                                           case_data.inputs.v2, case_data.inputs.theta_est);

    compareCertificationResult(actual_output, case_data.expected_outputs.certification_result);
  };

  testThroughCases(test_info_->name(), test_run, large_instances_params_);
}

TEST_F(DRSCertifierTest, Random100Points) {
  // generate 3 random large problem instances
  std::map<std::string, CaseData> random_instances_params;
  int num_tests = 5;
  int N = 100;

  for (size_t i = 0; i < num_tests; ++i) {
    std::string case_name = "Random100-" + std::to_string(i) + "-" + std::to_string(N);

    CaseData data;

    // Inputs:
    // scalar parameters
    data.inputs.params.cbar2 = 1;
    data.inputs.params.noise_bound = 0.01;

    // generate random vectors and transformations
    data.inputs.v1 = Eigen::Matrix<float, 3, Eigen::Dynamic>::Random(3, N);
    data.inputs.q_est = Eigen::Quaternion<float>::UnitRandom();
    data.inputs.R_est = data.inputs.q_est.toRotationMatrix();
    data.inputs.theta_est = Eigen::Matrix<float, 1, Eigen::Dynamic>::Ones(1, N);

    // calculate vectors after transformation
    // noise bounded by 0.01
    Eigen::Matrix<float, 3, Eigen::Dynamic> noise =
        (Eigen::Matrix<float, 3, Eigen::Dynamic>::Random(3, N).array() + 1) / 200.0;
    data.inputs.v2 = data.inputs.R_est * data.inputs.v1;

    // outliers
    float outlier_ratio = 0.1;
    int outlier_start_idx = (int)(N * (1 - outlier_ratio));
    for (size_t i = outlier_start_idx; i < N; ++i) {
      data.inputs.v2.col(i) = Eigen::Matrix<float, 3, Eigen::Dynamic>::Random(3, 1) * 5 +
                              Eigen::Matrix<float, 3, Eigen::Dynamic>::Ones(3, 1) * 5;
      data.inputs.theta_est(0, i) = -1;
    }

    // expected outputs
    data.expected_outputs.certification_result.is_optimal = true;
    data.expected_outputs.certification_result.best_suboptimality = 1e-5; // a very small value

    random_instances_params[case_name] = data;
  }

  auto test_run = [](CaseData case_data) {
    // construct the certifier
    teaser::DRSCertifier certifier(case_data.inputs.params);

    auto actual_output = certifier.certify(case_data.inputs.R_est, case_data.inputs.v1,
                                           case_data.inputs.v2, case_data.inputs.theta_est);
    const auto& expected_output = case_data.expected_outputs.certification_result;

    ASSERT_TRUE(expected_output.is_optimal == actual_output.is_optimal);
    ASSERT_TRUE(expected_output.best_suboptimality >= actual_output.best_suboptimality);
  };

  testThroughCases(test_info_->name(), test_run, random_instances_params);
}

TEST_F(DRSCertifierTest, RandomLargeInstsances) {
  // generate 3 random large problem instances
  std::map<std::string, CaseData> random_instances_params;
  std::array<float, 3> problem_sizes = {200, 300, 400};

  for (size_t i = 0; i < problem_sizes.size(); ++i) {

    int N = problem_sizes.at(i);
    std::string case_name = "Random-" + std::to_string(N);

    CaseData data;

    // Inputs:
    // scalar parameters
    data.inputs.params.cbar2 = 1;
    data.inputs.params.noise_bound = 0.01;

    // generate random vectors and transformations
    data.inputs.v1 = Eigen::Matrix<float, 3, Eigen::Dynamic>::Random(3, N);
    data.inputs.q_est = Eigen::Quaternion<float>::UnitRandom();
    data.inputs.R_est = data.inputs.q_est.toRotationMatrix();
    data.inputs.theta_est = Eigen::Matrix<float, 1, Eigen::Dynamic>::Ones(1, N);

    // calculate vectors after transformation
    // noise bounded by 0.01
    Eigen::Matrix<float, 3, Eigen::Dynamic> noise =
        (Eigen::Matrix<float, 3, Eigen::Dynamic>::Random(3, N).array() + 1) / 200.0;
    data.inputs.v2 = data.inputs.R_est * data.inputs.v1;

    // outliers
    float outlier_ratio = 0.1;
    int outlier_start_idx = (int)(N * (1 - outlier_ratio));
    for (size_t i = outlier_start_idx; i < N; ++i) {
      data.inputs.v2.col(i) = Eigen::Matrix<float, 3, Eigen::Dynamic>::Random(3, 1) * 5 +
                              Eigen::Matrix<float, 3, Eigen::Dynamic>::Ones(3, 1) * 5;
      data.inputs.theta_est(0, i) = -1;
    }

    // expected outputs
    data.expected_outputs.certification_result.is_optimal = true;
    data.expected_outputs.certification_result.best_suboptimality = 1e-5; // a very small value

    random_instances_params[case_name] = data;
  }

  auto test_run = [](CaseData case_data) {
    // construct the certifier
    teaser::DRSCertifier certifier(case_data.inputs.params);

    auto actual_output = certifier.certify(case_data.inputs.R_est, case_data.inputs.v1,
                                           case_data.inputs.v2, case_data.inputs.theta_est);
    const auto& expected_output = case_data.expected_outputs.certification_result;

    ASSERT_TRUE(expected_output.is_optimal == actual_output.is_optimal);
    ASSERT_TRUE(expected_output.best_suboptimality >= actual_output.best_suboptimality);
  };

  testThroughCases(test_info_->name(), test_run, random_instances_params);
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE LibmeshEntityIteratorTester
#include <boost/test/included/unit_test.hpp>

#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <vector>

#include <LibmeshAdjacencies.hpp>
#include <LibmeshEntity.hpp>
#include <LibmeshEntityExtraData.hpp>
#include <LibmeshEntityIterator.hpp>

#include <Teuchos_Array.hpp>
#include <Teuchos_ArrayRCP.hpp>
#include <Teuchos_CommHelpers.hpp>
#include <Teuchos_DefaultComm.hpp>
#include <Teuchos_DefaultMpiComm.hpp>
#include <Teuchos_OpaqueWrapper.hpp>
#include <Teuchos_RCP.hpp>
#include <Teuchos_Tuple.hpp>
#include <Teuchos_TypeTraits.hpp>

#include <libmesh/cell_hex8.h>
#include <libmesh/libmesh.h>
#include <libmesh/mesh.h>
#include <libmesh/node.h>
#include <libmesh/parallel.h>
#include <libmesh/point.h>

//---------------------------------------------------------------------------//
// MPI Setup
//---------------------------------------------------------------------------//

template <class Ordinal>
Teuchos::RCP<const Teuchos::Comm<Ordinal>> getDefaultComm()
{
#ifdef HAVE_MPI
    return Teuchos::DefaultComm<Ordinal>::getComm();
#else
    return Teuchos::rcp( new Teuchos::SerialComm<Ordinal>() );
#endif
}

//---------------------------------------------------------------------------//
// Hex-8 test.
BOOST_AUTO_TEST_CASE( checkLibmeshEntityIterator )
{
	const std::string argv_string = "unit_test --keep-cout";
	const char *argv_char = argv_string.c_str();
	Teuchos::GlobalMPISession mpiSession(
			&boost::unit_test::framework::master_test_suite().argc,
			&boost::unit_test::framework::master_test_suite().argv);
	auto comm = Teuchos::DefaultComm<int>::getComm();
	auto mpi_comm = Teuchos::rcp_dynamic_cast<const Teuchos::MpiComm<int>>(
			comm);
	auto opaque_comm = mpi_comm->getRawMpiComm();
	auto raw_comm = (*opaque_comm)();

    // Create the mesh.
    int space_dim = 3;
    libMesh::LibMeshInit libmesh_init( 1, &argv_char, raw_comm );
    BOOST_VERIFY( libMesh::initialized() );
    BOOST_VERIFY( (int)libmesh_init.comm().rank() == comm->getRank() );
    Teuchos::RCP<libMesh::Mesh> mesh =
        Teuchos::rcp( new libMesh::Mesh( libmesh_init.comm(), space_dim ) );

    // Create the nodes.
    int rank = comm->getRank();
    Teuchos::Array<libMesh::Node *> nodes( 8 );
    double node_coords[3];
    node_coords[0] = 0.0;
    node_coords[1] = 0.0;
    node_coords[2] = 0.0;
    nodes[0] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 0,
        rank );

    node_coords[0] = 1.0;
    node_coords[1] = 0.0;
    node_coords[2] = 0.0;
    nodes[1] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 1,
        rank );

    node_coords[0] = 1.0;
    node_coords[1] = 1.0;
    node_coords[2] = 0.0;
    nodes[2] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 2,
        rank );

    node_coords[0] = 0.0;
    node_coords[1] = 1.0;
    node_coords[2] = 0.0;
    nodes[3] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 3,
        rank );

    node_coords[0] = 0.0;
    node_coords[1] = 0.0;
    node_coords[2] = 1.0;
    nodes[4] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 4,
        rank );

    node_coords[0] = 1.0;
    node_coords[1] = 0.0;
    node_coords[2] = 1.0;
    nodes[5] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 5,
        rank );

    node_coords[0] = 1.0;
    node_coords[1] = 1.0;
    node_coords[2] = 1.0;
    nodes[6] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 6,
        rank );

    node_coords[0] = 0.0;
    node_coords[1] = 1.0;
    node_coords[2] = 1.0;
    nodes[7] = mesh->add_point(
        libMesh::Point( node_coords[0], node_coords[1], node_coords[2] ), 7,
        rank );

    // Make a hex-8.
    libMesh::Elem *hex_elem = mesh->add_elem( new libMesh::Hex8 );
    hex_elem->processor_id() = rank;
    hex_elem->set_id() = 2 * rank;
    for ( int i = 0; i < 8; ++i )
        hex_elem->set_node( i ) = nodes[i];

    // Make 2 subdomains and put the hex-8 in the first subdomain.
    int subdomain_1_id = 1;
    int subdomain_2_id = 2;
    std::set<libMesh::subdomain_id_type> subdomain_ids;
    subdomain_ids.insert( subdomain_1_id );
    subdomain_ids.insert( subdomain_2_id );
    hex_elem->subdomain_id() = subdomain_1_id;

    // Make 2 boundaries and add the first elem side to one and first node to
    // the second.
    int boundary_1_id = 1;
    int boundary_2_id = 2;
    mesh->get_boundary_info().add_side( hex_elem, 0, boundary_1_id );
    mesh->get_boundary_info().add_node( nodes[0], boundary_2_id );

    // Check libmesh validity.
    mesh->libmesh_assert_valid_parallel_ids();

    // Make an adjacency data structure.
    LibmeshAdapter::LibmeshAdjacencies adjacencies( mesh );

    // Make an iterator for the hex.
    LibmeshAdapter::ElemPredicateFunction all_pred =
        [=]( LibmeshAdapter::LibmeshEntity<libMesh::Elem> ) { return true; };
    auto entity_iterator =
    		LibmeshAdapter::LibmeshEntityIterator<
            libMesh::Mesh::const_element_iterator>(
            mesh->elements_begin(), mesh->elements_begin(),
            mesh->elements_end(), mesh.ptr(),
            Teuchos::ptrFromRef( adjacencies ), all_pred );

    // Test the entity iterator.
    unsigned int num_hex = 1;
    BOOST_VERIFY( entity_iterator.size() == num_hex );
    BOOST_VERIFY( entity_iterator == entity_iterator.begin() );
    BOOST_VERIFY( entity_iterator != entity_iterator.end() );

    // Test the first entity under the iterator with a pointer dereference.
    BOOST_VERIFY( hex_elem->id() == entity_iterator->id() );
    BOOST_VERIFY( comm->getRank() == entity_iterator->ownerRank() );
    BOOST_VERIFY( space_dim == entity_iterator->topologicalDimension() );
    BOOST_VERIFY( space_dim == entity_iterator->physicalDimension() );

    BOOST_VERIFY( entity_iterator->inBlock( subdomain_1_id ) );
    BOOST_VERIFY( !entity_iterator->inBlock( subdomain_2_id ) );
    BOOST_VERIFY( entity_iterator->onBoundary( boundary_1_id ) );
    BOOST_VERIFY( !entity_iterator->onBoundary( boundary_2_id ) );

    auto extra_data_1 =
        entity_iterator->extraData();
    BOOST_VERIFY( hex_elem ==
                   Teuchos::rcp_dynamic_cast<
				   LibmeshAdapter::LibmeshEntityExtraData<libMesh::Elem>>(
                       extra_data_1 )
                       ->d_libmesh_geom.getRawPtr() );

    Teuchos::Tuple<double, 6> hex_bounds_1;
    entity_iterator->boundingBox( hex_bounds_1 );
    BOOST_VERIFY( 0.0 == hex_bounds_1[0] );
    BOOST_VERIFY( 0.0 == hex_bounds_1[1] );
    BOOST_VERIFY( 0.0 == hex_bounds_1[2] );
    BOOST_VERIFY( 1.0 == hex_bounds_1[3] );
    BOOST_VERIFY( 1.0 == hex_bounds_1[4] );
    BOOST_VERIFY( 1.0 == hex_bounds_1[5] );

    // Test the end of the iterator.
    entity_iterator++;
    BOOST_VERIFY( entity_iterator != entity_iterator.begin() );
    BOOST_VERIFY( entity_iterator == entity_iterator.end() );

    // Make an iterator with a subdomain 1 predicate.
//    DataTransferKit::BlockPredicate subdomain_1_pred(
//        Teuchos::Array<int>( 1, subdomain_1_id ) );
//    auto subdomain_1_iterator =
//    		LibmeshAdapter::LibmeshEntityIterator<
//            libMesh::Mesh::const_element_iterator>(
//            mesh->elements_begin(), mesh->elements_begin(),
//            mesh->elements_end(), mesh.ptr(),
//            Teuchos::ptrFromRef( adjacencies ),
//            subdomain_1_pred.getFunction() );
//    BOOST_VERIFY( subdomain_1_iterator.size() == num_hex );
//
//    // Make an iterator with a subdomain 2 predicate.
//    DataTransferKit::BlockPredicate subdomain_2_pred(
//        Teuchos::Array<int>( 2, subdomain_2_id ) );
//    auto subdomain_2_iterator =
//    		LibmeshAdapter::LibmeshEntityIterator<
//            libMesh::Mesh::const_element_iterator>(
//            mesh->elements_begin(), mesh->elements_begin(),
//            mesh->elements_end(), mesh.ptr(),
//            Teuchos::ptrFromRef( adjacencies ),
//            subdomain_2_pred.getFunction() );
//    BOOST_VERIFY( subdomain_2_iterator.size() == 0 );
//
//    // Make a boundary iterator for the elems.
//    DataTransferKit::BoundaryPredicate boundary_1_elem_pred(
//        Teuchos::Array<int>( 1, subdomain_1_id ) );
//    auto elem_boundary_it_1 =
//    		LibmeshAdapter::LibmeshEntityIterator<
//            libMesh::Mesh::const_element_iterator>(
//            mesh->elements_begin(), mesh->elements_begin(),
//            mesh->elements_end(), mesh.ptr(),
//            Teuchos::ptrFromRef( adjacencies ),
//            boundary_1_elem_pred.getFunction() );
//    BOOST_VERIFY( 1 == elem_boundary_it_1.size() );
//
//    // Make a boundary iterator for the elems.
//    DataTransferKit::BoundaryPredicate boundary_2_elem_pred(
//        Teuchos::Array<int>( 1, subdomain_2_id ) );
//    auto elem_boundary_it_2 =
//        LibmeshAdapter::LibmeshEntityIterator<
//            libMesh::Mesh::const_element_iterator>(
//            mesh->elements_begin(), mesh->elements_begin(),
//            mesh->elements_end(), mesh.ptr(),
//            Teuchos::ptrFromRef( adjacencies ),
//            boundary_2_elem_pred.getFunction() );
//    BOOST_VERIFY( 0 == elem_boundary_it_2.size() );

    // Make an iterator for the nodes.
    LibmeshAdapter::NodePredicateFunction all_nodes = [&](LibmeshAdapter::LibmeshEntity<libMesh::Node>) {
    	return true;
    };
    auto node_iterator =
    		LibmeshAdapter::LibmeshEntityIterator<
            libMesh::Mesh::const_node_iterator>(
            mesh->nodes_begin(), mesh->nodes_begin(), mesh->nodes_end(),
            mesh.ptr(), Teuchos::ptrFromRef( adjacencies ), all_nodes );
    BOOST_VERIFY( 8 == node_iterator.size() );

    // Make a boundary iterator for the nodes.
//    DataTransferKit::BoundaryPredicate boundary_1_node_pred(
//        Teuchos::Array<int>( 1, subdomain_1_id ) );
//    auto node_boundary_it_1 =
//    		LibmeshAdapter::LibmeshEntityIterator<
//            libMesh::Mesh::const_node_iterator>(
//            mesh->nodes_begin(), mesh->nodes_begin(), mesh->nodes_end(),
//            mesh.ptr(), Teuchos::ptrFromRef( adjacencies ),
//            boundary_1_node_pred.getFunction() );
//    BOOST_VERIFY( 0 == node_boundary_it_1.size() );
//
//    // Make a boundary iterator for the nodes.
//    DataTransferKit::BoundaryPredicate boundary_2_node_pred(
//        Teuchos::Array<int>( 1, subdomain_2_id ) );
//    auto node_boundary_it_2 =
//    		LibmeshAdapter::LibmeshEntityIterator<
//            libMesh::Mesh::const_node_iterator>(
//            mesh->nodes_begin(), mesh->nodes_begin(), mesh->nodes_end(),
//            mesh.ptr(), Teuchos::ptrFromRef( adjacencies ),
//            boundary_2_node_pred.getFunction() );
//    BOOST_VERIFY( 1 == node_boundary_it_2.size() );
}

//---------------------------------------------------------------------------//
// end tstLibmeshEntityIterator.cpp
//---------------------------------------------------------------------------//


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <iostream>
#include <math.h>
#include <Eigen/Dense>
#include <float.h>
#include "fcm.h"

FCM::FCM(double m, double epsilon){
    m_epsilon = epsilon;
    m_m = m;
    m_membership = nullptr;
    m_data = nullptr;
    m_cluster_center = nullptr;
    m_num_clusters = 0;
    m_num_dimensions = 0;
}

FCM::~FCM(){
    if(m_data!=nullptr){
        delete m_data;
        m_data = nullptr;
    }

    if(m_membership!=nullptr){
        delete m_membership;
        m_membership = nullptr;
    }

    if(m_cluster_center!=nullptr){
        delete m_cluster_center;
        m_cluster_center = nullptr;
    }
}

double FCM::update_membership(){
    /*
     *
    */
    long k, i;
    double new_uik;
    double max_diff = 0.0, diff;

    if(m_data==nullptr || m_data->rows()==0){
        throw std::logic_error("ERROR: data should not be empty when updating the membership");
    }

    if(m_membership==nullptr || m_membership->rows() == 0 || m_membership->rows() != m_data->rows()){
        //cout << "init the membership";
        this->init_membership();
    }
    if(m_num_clusters==0){
        throw std::logic_error("ERROR: the number of clusters should be set");
    }

//    cout <<"mdata rows: "<< m_data->rows()<<endl;
//    cout << "mdata cols: "<<m_data->cols()<<endl;

    for (i = 0; i < m_num_clusters; i++) {
        for (k = 0; k < m_data->rows(); k++) {
            //cout << "point: " << k << " and cluster" << i <<endl;
            //cout << "\nwill ask for the new new_uik"<< endl;
            new_uik = this->compute_membership_point(i, k);
            //cout << new_uik << endl;
            diff = new_uik - (*m_membership)(k,i); // We need the membership inversed which is more natural for us
            if (diff > max_diff){
                max_diff = diff;
            }
            (*m_membership)(k,i) = new_uik;
        }
    }
    return max_diff;
}

void FCM::compute_centers(){
    long i, j, k;
    double numerator, denominator;
    MatrixXf t;
    t.resize(m_data->rows(), m_num_clusters);
    if(m_data == nullptr || m_data->rows() == 0){
        throw std::logic_error("ERROR: number of rows is zero");
        return;
    }
    for (i = 0; i < m_data->rows(); i++) { // compute (u^m) for each cluster for each point
        for (j = 0; j < m_num_clusters; j++) {
            t(i,j) = pow((*m_membership)(i,j), m_m);
        }
    }
    for (j = 0; j < m_num_clusters; j++) { // loop for each cluster
        for (k = 0; k < m_num_dimensions; k++) { // for each dimension
            numerator = 0.0;
            denominator = 0.0;
            for (i = 0; i < m_data->rows(); i++) {
                numerator += t(i,j) * (*m_data)(i,k);
                denominator += t(i,j);
            }
            (*m_cluster_center)(j,k) = numerator / denominator;
        }
    }
}

double FCM::get_dist(long i, long k){
  /*
   * distance which is denoted in the paper as d
   * k is the data point
   * i is the cluster center point
  */
  //cout<<"get_dist: point: "<<k<<" and cluster "<<i<<endl;
  long j;
  double sqsum = 0.0;
  if(m_num_clusters==0){
      throw std::logic_error("ERROR: number of clusters should not be zero\n");
  }
  if(m_num_dimensions==0){
      throw std::logic_error("ERROR: number of dimensions should not be zero\n");
  }
  for (j = 0; j < m_num_dimensions; j++) {
      sqsum += pow( ((*m_data)(k,j) - (*m_cluster_center)(i,j)) ,2) ;
  }
  return sqrt(sqsum);
}

double FCM::compute_membership_point(long i, long k){
    /*
     * i the cluster
     * k is the data point
    */
    //cout << __func__ <<"  num of cluster: "<<m_num_clusters<<endl;
    long j;
    double t, seg=0.0;
    double exp = 2 / (m_m - 1);
    double dik, djk;
    if(m_num_clusters==0){
        throw std::logic_error("ERROR: number of clusters should not be zero\n");
    }
    for (j = 0; j < m_num_clusters; j++) {
      dik = this->get_dist(i, k);
      djk = this->get_dist(j,k);
      if(djk==0){
          djk = DBL_MIN;
      }
      t = dik / djk;
      t = pow(t, exp);
      //cout << "cluster: " << i << "data: " << k << " - " << "t: "<<t<<endl;
      seg += t;
    }
    //cout << "seg: "<<seg << " u: "<<(1.0/seg)<<endl;
    return 1.0 / seg;
}

void FCM::set_data(MatrixXf *data){
    if(m_data!=nullptr){
        delete m_data;
    }
    if(data->rows()==0){
        throw std::logic_error("ERROR: seting empty data");
    }
    m_data = data;
    m_num_dimensions = m_data->cols();
}

void FCM::set_membership(MatrixXf *membership){
    if(m_data==0){
        throw std::logic_error("ERROR: the data should present before setting up the membership");
    }
    if(m_num_clusters==0){
        if(membership->cols() == 0){
            throw std::logic_error("ERROR: the number of clusters is 0 and the membership matrix is empty");
        }
        else{
            this->set_num_clusters(membership->cols());
        }
    }
    if(m_membership!=nullptr){
        delete m_membership;
    }
    m_membership = membership;
    if(m_membership->rows()==0){
        m_membership->resize(m_data->rows(), m_num_clusters);
    }
}

void FCM::init_membership(){
    long i, j;
    double mem;
    if(m_num_clusters == 0){
        throw std::logic_error("ERROR: the number of clusters is 0");
    }
    if(m_data==nullptr){
        throw std::logic_error("ERROR: the data should present before setting up the membership");
    }
    if(m_membership!=nullptr){
        delete m_membership;
    }
    m_membership = new MatrixXf;
    m_membership->resize(m_data->rows(), m_num_clusters);
    mem = 1.0 / m_num_clusters;
    for(j=0;j<m_num_clusters;j++){
        for(i=0;i<m_data->rows();i++){
            (*m_membership)(i,j) = mem;
        }
    }
}

void FCM::set_num_clusters(long num_clusters){
    m_num_clusters = num_clusters;
    if(m_cluster_center){
        delete m_cluster_center;
    }
    m_cluster_center = new MatrixXf;
    m_cluster_center->resize(m_num_clusters, m_num_dimensions);
}

MatrixXf * FCM::get_data(){
    return m_data;
}

MatrixXf * FCM::get_membership(){
    return m_membership;
}

MatrixXf * FCM::get_cluster_center(){
    return m_cluster_center;
}


#pragma once
#include <Eigen/Core>
#include <vector>
#include <memory>
#include <array>
#include "mtao/types.h"
#include <sstream>
#include <numeric>

namespace mtao { namespace geometry {
    template <typename T, int D>
        class KDTree;
    template <typename T, int D, int Axis>
        //class KDNode: public KDNodeBase<KDNode<T,D,Axis>> {
        class KDNode {
            public:
                using Vec = mtao::Vector<T,D>;
                using NodeType = KDNode<T,D,Axis>;
                constexpr static int ChildAxis = (Axis+1)%D;
                constexpr static int ParentAxis = (Axis-1+D)%D;
                using ChildNodeType = KDNode<T,D,ChildAxis>;
                using ParentNodeType = KDNode<T,D,ParentAxis>;
                //balanced construciton iterator, used for one constructor
                using BCIt = mtao::vector<size_t>::iterator;

                ~KDNode() = default;
                KDNode() = delete;
                KDNode(const KDNode&) = delete;
                KDNode(KDNode&&) = delete;
                KDNode& operator=(const KDNode&) = delete;
                KDNode& operator=(KDNode&&) = delete;
                KDNode(const KDTree<T,D>& tree, KDNode&& o): m_tree(tree), m_index(o.m_index) {
                    if(o.left()) {
                        left() = std::make_unique<ChildNodeType>(tree,std::move(*o.left()));
                    }
                    if(o.right()) {
                        right() = std::make_unique<ChildNodeType>(tree,std::move(*o.right()));
                    }
                }

                KDNode(const KDTree<T,D>& tree, int index): m_tree(tree), m_index(index) {}
                KDNode(const KDTree<T,D>& tree, const BCIt& begin, const BCIt& end): m_tree(tree) {
                    assert(std::distance(begin,end) > 0);

                    std::sort(begin,end,[&](size_t a, size_t b) {
                            return point(a)(Axis) < point(b)(Axis);
                            });
                    BCIt mid = begin + std::distance(begin,end)/2;
                    BCIt right_start = mid+1;
                    m_index = *mid;
                    if(std::distance(begin,mid) > 0) {
                        left() = std::make_unique<ChildNodeType>(m_tree,begin,mid);
                    }
                    if(std::distance(right_start,end) > 0) {
                        right() = std::make_unique<ChildNodeType>(m_tree,right_start,end);
                    }

                }
                auto&& point() const {
                    return point(m_index);
                }
                auto&& point(size_t idx) const {return m_tree.point(idx);}
                size_t index()const {return m_index;}

                template <typename Derived>
                    bool operator<(const Eigen::MatrixBase<Derived>& p) const {
                        //positive -> right
                        //negative -> left
                        return axis_dist(p) < 0;
                    }
                template <typename Derived>
                    T axis_dist(const Eigen::MatrixBase<Derived>& p) const {
                        return p(Axis) - point()(Axis);
                    }
                template <typename Derived>
                    size_t axis_choice(const Eigen::MatrixBase<Derived>& p) const {
                        return (*this < p)?0:1;
                    }

                template <typename Derived>
                    auto&& pick_child(const Eigen::MatrixBase<Derived>& p) {
                        return m_children[axis_choice(p)];
                    }
                template <typename Derived>
                    auto&& pick_child(const Eigen::MatrixBase<Derived>& p) const {
                        return m_children[axis_choice(p)];
                    }

                void insert(size_t idx) {
                    auto&& c = pick_child(point(idx));
                    if(c) {
                        c->insert(idx);
                    } else {
                        c = std::make_unique<ChildNodeType>(m_tree,idx);
                    }
                }

                template <typename Derived>
                   void nearest(const Eigen::MatrixBase<Derived>& p, size_t& nearest_index, T& nearest_dist) const {

                        T dist = (point() - p).norm();
                        if(dist < nearest_dist) {
                            nearest_index = index();
                            nearest_dist = dist;
                        }

                        auto comp_child = [&](const std::unique_ptr<ChildNodeType>& c) {
                            if(c) {
                                c->nearest(p,nearest_index,nearest_dist);
                            }
                        };
                        T ad = axis_dist(p);
                        bool in_nearband = nearest_dist >= std::abs(ad);

                        //[  node     point  child]
                        //[  node     child  point]
                        //[  point    node   child]
                        //axis nearest_dist is negative and considering going down right child
                        if(ad >= 0 || in_nearband) {
                            comp_child(right());
                        }
                        //[  point  child     node]
                        //[  child  point     node]
                        //[  child    node   point]
                        //axis nearest_dist is positive and considering going down left child
                        if(ad < 0 || in_nearband) {
                            comp_child(left());
                        }

                    }

                std::unique_ptr<ChildNodeType>& left() { return m_children[0]; }
                const std::unique_ptr<ChildNodeType>& left() const { return m_children[0]; }

                std::unique_ptr<ChildNodeType>& right() { return m_children[1]; }
                const std::unique_ptr<ChildNodeType>& right() const { return m_children[1]; }

                int max_depth() const {
                    int depth = 0;
                    if(left()) {
                        depth = std::max(left()->max_depth()+1,depth);
                    }
                    if(right()) {
                        depth = std::max(right()->max_depth()+1,depth);
                    }
                    return depth;
                }

                operator std::string() const {
                    std::stringstream ss;
                    ss << m_index << "[";
                    if(left()) {
                        ss << std::string(*left());
                    } else {
                        ss << ".";
                    }
                    ss << " | ";
                    if(right()) {
                        ss << std::string(*right());
                    } else {
                        ss << ".";
                    }
                    ss << "]";

                    return ss.str();
                }
            private:
                const KDTree<T,D>& m_tree;
                size_t m_index;
                std::array<std::unique_ptr<ChildNodeType>,2> m_children;
        };

    template <typename T, int D>
        class KDTree {
            public:
                using Vec = mtao::Vector<T,D>;
                constexpr static int Axis = 0;
                using NodeType = KDNode<T,D,0>;
                const mtao::vector<Vec>& points()const {return m_points;}
                auto&& point(size_t idx) const {return m_points[idx];}
                size_t size() const { return points().size(); }

                void rebalance() {
                    std::vector<size_t> P(m_points.size());
                    std::iota(P.begin(),P.end(),0);
                    m_node = std::make_unique<NodeType>(*this,P.begin(),P.end());
                }
                KDTree() = default;
                KDTree(const KDTree&) = delete;
                KDTree(KDTree&&) = delete;
                ~KDTree() = default;
                KDTree& operator=(const KDTree&) = delete;
                KDTree& operator=(KDTree&& o) {
                    m_points = std::move(o.m_points);
                    if(o.m_node) {
                        m_node = std::make_unique<NodeType>(*this,std::move(*o.m_node));
                    }
                    return *this;
                }
                void reserve(size_t size) { m_points.reserve(size); }
                KDTree(const mtao::vector<Vec>& points): m_points(points) {
                    rebalance();
                }
                size_t insert(const Vec& p) {
                    size_t newidx = m_points.size();
                    m_points.push_back(p);
                    if(!m_node) {
                        m_node = std::make_unique<NodeType>(*this, newidx);
                    } else {
                        m_node->insert(newidx);
                    }
                    return newidx;
                }
                size_t pruning_insertion(const Vec& p, T eps = T(1e-5)) {

                    if(m_node) {
                        auto [ni,d] = nearest(p);
                        if(eps == 0) {
                            if(point(ni) == p) {
                                return ni;
                            }
                        }
                        else if(d < eps) {
                            return ni;
                        }
                    }
                    return insert(p);
                }
                template <typename Derived>
                    std::tuple<size_t,T> nearest(const typename Eigen::MatrixBase<Derived>& p) const {
                        size_t idx = 0;
                        T d = std::numeric_limits<T>::max();
                        assert(m_node);

                        m_node->nearest(p,idx,d);
                        return {idx,d};
                    }
                template <typename Derived>
                    size_t nearest_index(const typename Eigen::MatrixBase<Derived>& p) const {
                        return std::get<0>(nearest(p));
                    }
                template <typename Derived>
                    size_t nearest_distance(const typename Eigen::MatrixBase<Derived>& p) const {
                        return std::get<1>(nearest(p));
                    }
                template <typename Derived>
                    const Vec& nearest_point(const typename Eigen::MatrixBase<Derived>& p) const { return point(nearest_index(p)); }

                operator std::string() const { return *m_node; }

                int max_depth() const {
                    if(m_node) {
                        return m_node->max_depth() + 1;
                    }
                    return 0;
                }

            private:
                std::unique_ptr<NodeType> m_node;
                mtao::vector<Vec> m_points;

        };

}}


/**
 * @file RtModel.hpp
 * @author Alexander Herzog
 * @author Brahayam Ponton (brahayam.ponton@tuebingen.mpg.de)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-10-06
 */

#pragma once

#include <boost/thread.hpp>
#include <solver/interface/Exprs.hpp>
#include <solver/rt_optimizer/RtHQPCost.hpp>
#include <solver/rt_optimizer/RtHQPSolver.hpp>

namespace rt_solver {

  template<int Max_Ineq_Rows, int Max_Eq_Rows, int Num_OptVars>
  class RtModel
  {
    public:
      RtModel(YAML::Node params);
      virtual ~RtModel(){}

      bool solve();
      void initialize();
      virtual void update(){}

      // helper functions
      template <typename VecDer>
      double inequalitySlack(Eigen::MatrixBase<VecDer>& dist);
      template <typename Mat>
      void reduceColumns(Eigen::EigenBase<Mat>& mat) const;
      template<int Max_Rows, typename Mat_Type, typename Vec_Type>
      void appendRowsOfRank(int rank_to_add, const typename rt_solver::RtVector<Max_Rows>::i& ranks,
        const Eigen::MatrixBase<Mat_Type>& subst_mat, const Eigen::MatrixBase<Vec_Type>& subst_vec, bool append_to_equalities, int starting_column);

      // getter and setter methods
      typename rt_solver::RtVector<Max_Eq_Rows>::d& nextEqCostVec() { return next_eq_cost_vec_; }
      typename rt_solver::RtVector<Max_Ineq_Rows>::d& nextIneqCostVec() { return next_ineq_cost_vec_; }
      typename rt_solver::RtMatrix<Max_Eq_Rows, Num_OptVars>::d& nextEqCostMat() { return next_eq_cost_mat_; }
      typename rt_solver::RtMatrix<Max_Ineq_Rows, Num_OptVars>::d& nextIneqCostMat() { return next_ineq_cost_mat_; }

      const typename rt_solver::RtVector<Max_Eq_Rows>::d& nextEqCostVec() const { return next_eq_cost_vec_; }
      const typename rt_solver::RtVector<Max_Ineq_Rows>::d& nextIneqCostVec() const { return next_ineq_cost_vec_; }
      const typename rt_solver::RtMatrix<Max_Eq_Rows, Num_OptVars>::d& nextEqCostMat() const { return next_eq_cost_mat_; }
      const typename rt_solver::RtMatrix<Max_Ineq_Rows, Num_OptVars>::d& nextIneqCostMat() const { return next_ineq_cost_mat_; }

      std::vector<rt_solver::RtHQPCost*>& subCostComposers() { return sub_cost_composers_; }

      // methods to use easier interface to formulate problems
      void clean();
      solver::Var getVar(const int var_id);
      bool checkIfHigherRanksLeft(const int rank);
      void checkSlacknessOfHierarchy(const int rank);
      void addCostsToHierarchy(const int rank_to_add);
      void addLinConstr(const solver::LinExpr& lhs, const std::string sense, const solver::LinExpr& rhs, const int rank);
      void print();

      std::vector<int>& hierarchiesWithoutSlacks() { return hierarchies_without_slacks_; }
      const std::vector<int>& hierarchiesWithoutSlacks() const { return hierarchies_without_slacks_; }

    public:
      double n_solved_ranks_;
      bool is_solution_valid_;
      int num_variables_optimized_;
      std::vector<double> qp_dur_, svd_wait_;
      Eigen::Matrix<int, Num_OptVars, 1> full_to_opt_variable_index_;
      mutable rt_solver::RtHQPSolver<Max_Ineq_Rows, Num_OptVars, Max_Eq_Rows> hqp_solver_;

    private:
      std::vector<solver::Var> vars_;
      std::vector<int> hierarchies_without_slacks_;
      std::vector<std::tuple<int, solver::LinExpr>> leqcons_, lineqcons_;

      std::vector<rt_solver::RtHQPCost*> sub_cost_composers_;
      typename rt_solver::RtVector<Max_Eq_Rows>::d next_eq_cost_vec_;
      typename rt_solver::RtVector<Max_Ineq_Rows>::d next_ineq_cost_vec_;
      typename rt_solver::RtMatrix<Max_Eq_Rows, Num_OptVars>::d next_eq_cost_mat_;
      typename rt_solver::RtMatrix<Max_Ineq_Rows, Num_OptVars>::d next_ineq_cost_mat_;

  };

}


/**
 * @file newproblem.cc
 * @brief NPDE homework NewProblem code
 * @author Oliver Rietmann, Erick Schulz
 * @date 01.01.2020
 * @copyright Developed at ETH Zurich
 */

#include "newproblem.h"

#include <Eigen/Core>

namespace NewProblem {

/* SAM_LISTING_BEGIN_1 */
Eigen::VectorXd dummyFunction(double x, int n) {
#if SOLUTION
  // Appears only in mastersolution
  return Eigen::Vector2d::Constant(1.0);
#else
  // Appears only in mysolution and templates
  return Eigen::Vector2d::Zero();
#endif
}
/* SAM_LISTING_END_1 */

}  // namespace NewProblem


#include "ViscositySolver.h"

#include <iostream>

#include <Eigen/Sparse>

#include "ComputeWeights.h"
#include "LevelSet.h"

namespace FluidSim2D::SimTools
{

using SolveReal = double;
using Vector = Eigen::VectorXd;

void ViscositySolver(float dt,
						const LevelSet& surface,
						VectorGrid<float>& velocity,
						const LevelSet& solidSurface,
						const VectorGrid<float>& solidVelocity,
						const ScalarGrid<float>& viscosity)
{
	// For efficiency sake, this should only take in velocity on a staggered grid
	// that matches the center sampled surface and collision
	assert(surface.isGridMatched(solidSurface));
	assert(surface.isGridMatched(viscosity));
	assert(velocity.isGridMatched(solidVelocity));

	for (int axis : {0, 1})
	{
		Vec2i faceSize = velocity.size(axis);
		--faceSize[axis];

		assert(faceSize == surface.size());
	}

	int samples = 3;

	ScalarGrid<float> centerAreas = computeSupersampledAreas(surface, ScalarGridSettings::SampleType::CENTER, 3);
	ScalarGrid<float> nodeAreas = computeSupersampledAreas(surface, ScalarGridSettings::SampleType::NODE, 3);
	VectorGrid<float> faceAreas = computeSupersampledFaceAreas(surface, 3);

	SolveReal discreteScalar = dt / sqr(surface.dx());

	// Pre-scale all the control volumes with coefficients to reduce
	// redundant operations when building the linear system.

	tbb::parallel_for(tbb::blocked_range<int>(0, centerAreas.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
		{
			for (int cellIndex = range.begin(); cellIndex != range.end(); ++cellIndex)
			{
				Vec2i cell = centerAreas.unflatten(cellIndex);

				if (centerAreas(cell) > 0)
					centerAreas(cell) *= 2. * discreteScalar * viscosity(cell);
			}
		});

	tbb::parallel_for(tbb::blocked_range<int>(0, nodeAreas.voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
		{
			for (int nodeIndex = range.begin(); nodeIndex != range.end(); ++nodeIndex)
			{
				Vec2i node = nodeAreas.unflatten(nodeIndex);

				if (nodeAreas(node) > 0)
					nodeAreas(node) *= discreteScalar * viscosity.biLerp(nodeAreas.indexToWorld(Vec2f(node)));
			}
		});

	enum class MaterialLabels { SOLID_FACE, LIQUID_FACE, AIR_FACE };

	VectorGrid<MaterialLabels> materialFaceLabels(surface.xform(), surface.size(), MaterialLabels::AIR_FACE, VectorGridSettings::SampleType::STAGGERED);

	// Set material labels for each grid face. We assume faces along the simulation boundary
	// are solid.

	for (int faceAxis : {0, 1})
	{
		tbb::parallel_for(tbb::blocked_range<int>(0, materialFaceLabels.grid(faceAxis).voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
			{
				for (int faceIndex = range.begin(); faceIndex != range.end(); ++faceIndex)
				{
					Vec2i face = materialFaceLabels.grid(faceAxis).unflatten(faceIndex);

					if (face[faceAxis] == 0 || face[faceAxis] == materialFaceLabels.size(faceAxis)[faceAxis] - 1)
						continue;

					bool isFaceInSolve = false;

					for (int direction : {0, 1})
					{
						Vec2i cell = faceToCell(face, faceAxis, direction);
						if (centerAreas(cell) > 0) isFaceInSolve = true;
					}

					if (!isFaceInSolve)
					{
						for (int direction : {0, 1})
						{
							Vec2i node = faceToNode(face, faceAxis, direction);
							if (nodeAreas(node) > 0) isFaceInSolve = true;
						}
					}

					if (isFaceInSolve)
					{
						if (solidSurface.biLerp(materialFaceLabels.indexToWorld(Vec2f(face), faceAxis)) <= 0.)
							materialFaceLabels(face, faceAxis) = MaterialLabels::SOLID_FACE;
						else
							materialFaceLabels(face, faceAxis) = MaterialLabels::LIQUID_FACE;
					}
				}
			});
	}

	int liquidDOFCount = 0;

	constexpr int UNLABELLED_CELL = -1;

	VectorGrid<int> liquidFaceIndices(surface.xform(), surface.size(), UNLABELLED_CELL, VectorGridSettings::SampleType::STAGGERED);

	for (int axis : {0, 1})
	{
		forEachVoxelRange(Vec2i(0), materialFaceLabels.size(axis), [&](const Vec2i& face)
			{
				if (materialFaceLabels(face, axis) == MaterialLabels::LIQUID_FACE)
					liquidFaceIndices(face, axis) = liquidDOFCount++;
			});
	}

	std::vector<Eigen::Triplet<SolveReal>> sparseElements;
	Vector initialGuessVector = Vector::Zero(liquidDOFCount);
	Vector rhsVector = Vector::Zero(liquidDOFCount);

	{
		tbb::enumerable_thread_specific<std::vector<Eigen::Triplet<SolveReal>>> parallelSparseElements;

		for (int faceAxis : {0, 1})
		{
			tbb::parallel_for(tbb::blocked_range<int>(0, materialFaceLabels.grid(faceAxis).voxelCount(), materialFaceLabels.grid(faceAxis).voxelCount() + 1/*tbbLightGrainSize*/), [&](const tbb::blocked_range<int>& range)
				{
					auto& localSparseElements = parallelSparseElements.local();

					for (int faceIndex = range.begin(); faceIndex != range.end(); ++faceIndex)
					{
						Vec2i face = materialFaceLabels.grid(faceAxis).unflatten(faceIndex);

						int liquidFaceIndex = liquidFaceIndices(face, faceAxis);

						if (liquidFaceIndex >= 0)
						{
							assert(materialFaceLabels(face, faceAxis) == MaterialLabels::LIQUID_FACE);

							// Use old velocity as an initial guess since we're solving for a new
							// velocity field with viscous forces applied to the old velocity field.
							initialGuessVector(liquidFaceIndex) = velocity(face, faceAxis);

							// Build RHS with volume weights
							SolveReal localFaceArea = faceAreas(face, faceAxis);

							rhsVector(liquidFaceIndex) = localFaceArea * velocity(face, faceAxis);

							// Add volume weight to diagonal
							SolveReal diagonal = localFaceArea;

							// Build cell centered stress terms
							for (int divergenceDirection : {0, 1})
							{
								Vec2i cell = faceToCell(face, faceAxis, divergenceDirection);

								assert(cell[faceAxis] >= 0 && cell[faceAxis] < centerAreas.size()[faceAxis]);

								if (centerAreas(cell) > 0)
								{
									SolveReal divergenceSign = (divergenceDirection == 0) ? -1 : 1;

									for (int gradientDirection : {0, 1})
									{
										Vec2i adjacentFace = cellToFace(cell, faceAxis, gradientDirection);

										SolveReal gradientSign = (gradientDirection == 0) ? -1. : 1.;

										SolveReal coefficient = divergenceSign * gradientSign * centerAreas(cell);

										int adjacentFaceIndex = liquidFaceIndices(adjacentFace, faceAxis);
										if (adjacentFaceIndex >= 0)
										{
											if (adjacentFaceIndex == liquidFaceIndex)
												diagonal -= coefficient;
											else
												localSparseElements.emplace_back(liquidFaceIndex, adjacentFaceIndex, -coefficient);
										}
										else if (materialFaceLabels(adjacentFace, faceAxis) == MaterialLabels::SOLID_FACE)
											rhsVector(liquidFaceIndex) += coefficient * solidVelocity(adjacentFace, faceAxis);
										else
											assert(materialFaceLabels(adjacentFace, faceAxis) == MaterialLabels::AIR_FACE);
									}
								}
							}

							// Build node stresses.
							for (int divergenceDirection : {0, 1})
							{
								Vec2i node = faceToNode(face, faceAxis, divergenceDirection);

								if (nodeAreas(node) > 0)
								{
									SolveReal divergenceSign = (divergenceDirection == 0) ? -1. : 1.;

									for (int gradientAxis : {0, 1})
									{
										int gradientFaceAxis = (gradientAxis + 1) % 2;
										for (int gradientDirection : {0, 1})
										{
											SolveReal gradientSign = gradientDirection == 0 ? -1. : 1.;

											Vec2i localGradientFace = nodeToFace(node, gradientAxis, gradientDirection);

											int gradientFaceIndex = liquidFaceIndices(localGradientFace, gradientFaceAxis);

											SolveReal coefficient = divergenceSign * gradientSign * nodeAreas(node);

											if (gradientFaceIndex >= 0)
											{
												if (gradientFaceIndex == liquidFaceIndex)
													diagonal -= coefficient;
												else
													localSparseElements.emplace_back(liquidFaceIndex, gradientFaceIndex, -coefficient);
											}
											else if (materialFaceLabels(localGradientFace, gradientFaceAxis) == MaterialLabels::SOLID_FACE)
												rhsVector(liquidFaceIndex) += coefficient * solidVelocity(localGradientFace, gradientFaceAxis);
											else assert(materialFaceLabels(localGradientFace, gradientFaceAxis) == MaterialLabels::AIR_FACE);
										}
									}
								}
							}

							assert(diagonal > 0);
							localSparseElements.emplace_back(liquidFaceIndex, liquidFaceIndex, diagonal);
						}
						else assert(materialFaceLabels(face, faceAxis) != MaterialLabels::LIQUID_FACE);
					}
				});
		}

		mergeLocalThreadVectors(sparseElements, parallelSparseElements);
	}

	Eigen::SparseMatrix<SolveReal> sparseMatrix(liquidDOFCount, liquidDOFCount);
	sparseMatrix.setFromTriplets(sparseElements.begin(), sparseElements.end());

	Eigen::ConjugateGradient<Eigen::SparseMatrix<SolveReal>, Eigen::Upper | Eigen::Lower> solver;
	solver.compute(sparseMatrix);
	solver.setTolerance(1E-3);

	if (solver.info() != Eigen::Success)
	{
		std::cout << "   Solver failed to build" << std::endl;
		return;
	}

	Vector solutionVector = solver.solveWithGuess(rhsVector, initialGuessVector);

	if (solver.info() != Eigen::Success)
	{
		std::cout << "   Solver failed to converge" << std::endl;
		return;
	}
	else
	{
		std::cout << "    Solver iterations:     " << solver.iterations() << std::endl;
		std::cout << "    Solver error: " << solver.error() << std::endl;
	}

	for (int faceAxis : {0, 1})
	{
		tbb::parallel_for(tbb::blocked_range<int>(0, materialFaceLabels.grid(faceAxis).voxelCount(), tbbLightGrainSize), [&](const tbb::blocked_range<int>& range)
			{
				for (int faceIndex = range.begin(); faceIndex != range.end(); ++faceIndex)
				{
					Vec2i face = materialFaceLabels.grid(faceAxis).unflatten(faceIndex);

					int liquidFaceIndex = liquidFaceIndices(face, faceAxis);
					if (liquidFaceIndex >= 0)
					{
						assert(materialFaceLabels(face, faceAxis) == MaterialLabels::LIQUID_FACE);
						velocity(face, faceAxis) = solutionVector(liquidFaceIndex);
					}
				}
			});
	}
}

}


#include "FermionToSpinTransformation.hpp"
#include <iostream>
#include <unordered_map>

#include "DiagonalizeTask.hpp"
#include <Eigen/Sparse>
#include <iomanip>

namespace xacc {
namespace vqe {

VQETaskResult DiagonalizeTask::execute(
		Eigen::VectorXd parameters) {
	int nQubits = std::stoi(xacc::getOption("n-qubits"));

	auto hamiltonianInstruction = program->getPauliOperator();

	std::shared_ptr<DiagonalizeBackend> backend;
	if (xacc::optionExists("diagonalize-backend")) {
		auto str = xacc::getOption("diagonalize-backend");
		backend = xacc::getService<
				DiagonalizeBackend>(str);
	} else {
		backend = xacc::getService<
				DiagonalizeBackend>("diagonalize-eigen");
	}

    double energy = 0.0;
    if (xacc::optionExists("print-ground-state")) {
        auto pair = backend->diagonalizeWithGroundState(program);
        energy = pair.first;
        std::stringstream s;
        s << "Ground State = (" << -1.0*pair.second.transpose() << ")";
        xacc::info(s.str());
    } else {
	    energy = backend->diagonalize(program);
    }

	VQETaskResult result;
	result.angles = parameters;
	result.energy = energy;
	return result;
}

double EigenDiagonalizeBackend::diagonalize(PauliOperator& hamiltonian) {
    auto nQubits = hamiltonian.nQubits();
	auto fermionTransformation =
			xacc::optionExists("fermion-transformation") ?
					xacc::getOption("fermion-transformation") : "";
	std::complex<double> gsReal;

	Eigen::VectorXd eigenvalues;
	if (xacc::optionExists("diag-number-symmetry") &&
			xacc::optionExists("n-electrons")) {
		int nElectrons = std::stoi(xacc::getOption("n-electrons"));

		// Generate all n-qubit bitstrings with n-electron
		// bits set
		std::string initBitString = "";
		for (int i = 0; i < nQubits - nElectrons; i++)
			initBitString += "0";
		for (int i = 0; i < nElectrons; i++)
			initBitString += "1";

		// Create occupation basis bit strings
		std::vector<std::string> bitStrings;
		do {
			std::cout << initBitString << "\n";
			bitStrings.push_back(initBitString);
		} while (std::next_permutation(initBitString.begin(),
				initBitString.end()));

		// Transform bit strings from occupation basis
		// to bravyi kitaev basis if needed
		if (fermionTransformation == "bk") {

			// Build up BK transformation matrix
			Eigen::MatrixXi B(1,1); B(0,0) = 1;
			while (true) {

				Eigen::MatrixXi oldB = B;
				Eigen::MatrixXi ones = Eigen::MatrixXi::Ones(1,oldB.cols());
				B.resize(oldB.rows()*2, oldB.cols()*2);
				B.setZero();

				// Set 2nd quadrant
				B.block(0,0,oldB.rows(), oldB.cols()) = oldB;

				// Set 4th quadrant
				B.block(oldB.rows(), oldB.cols(), oldB.rows(), oldB.cols()) = oldB;

				// Set the ones in the first quadrant
				B.block(0, B.cols()-oldB.cols(), ones.rows(), ones.cols()) = ones;

				if (B.rows() == nQubits) {
					break;
				} else if (B.rows() > nQubits) {
					std::cout << "Getting Subblock:\n" << B << "\n\n";
					Eigen::MatrixXi subB = B.block(B.rows()-nQubits, B.cols()-nQubits, nQubits, nQubits);
//					subB.block(nQubits-1, 0, 1, nQubits) = Eigen::MatrixXi::Ones(1,nQubits);
					B = subB;
					break;
				}
			}

			std::cout << "SeeleyBK=\n" << B << "\n\n";

			int end = nQubits-1;
			for (int start = 0; ; start++) {
				B.row(start).swap(B.row(end));
				end--;
				if (start == end) break;
			}

			end = nQubits -1;
			for (int start = 0; ; start++) {
				B.col(start).swap(B.col(end));
				end--;
				if (start == end) break;
			}

			std::cout << "TranterBK:\n" << B << "\n";

			std::vector<std::string> newBitStrings;
			for (auto& bs : bitStrings) {
				Eigen::VectorXi x(nQubits);
				for (int i = 0; i < bs.length(); i++) {
					x(i) = bs[i] == '1' ? 1 : 0;
				}

				Eigen::VectorXi y = B * x;
				for (int i = 0; i < y.size(); i++) y(i) %= 2;

				std::string newbitstring = "";
				for (int i = 0; i < y.size(); i++) {
					newbitstring += y(i) == 0 ? '0' : '1';
				}
				newBitStrings.push_back(newbitstring);
			}

			bitStrings.clear();
			bitStrings = newBitStrings;
		}

		std::map<std::string, std::uint64_t> bitsToIdx;
		std::map<std::uint64_t, std::string> idxToBits;

		std::uint64_t counter = 0;
		for (auto& bs : bitStrings) {
			bitsToIdx.insert( { bs, counter });
			idxToBits.insert( { counter, bs });
			counter++;
		}
		int nBitStrings = counter;

		xacc::info("Considering Hamiltonian subspace spanned by "
				+ std::to_string(nBitStrings) + " eigenstates with "
				+ std::to_string(nElectrons) + " occupations");

		Eigen::MatrixXcd mat(nBitStrings, nBitStrings);
		mat.setZero();
		for (std::uint64_t i = 0; i < nBitStrings; i++) {
			auto bitStr = idxToBits[i];
			auto braResults = hamiltonian.computeActionOnKet(bitStr);

			for (auto& r : braResults) {
				if (bitStr == r.first) {
					mat(i, i) += r.second;
				}
			}

			for (auto& result : braResults) {
				std::uint64_t k = bitsToIdx[result.first];
				if (i != k)
					mat(i, k) += result.second;
			}

		}
		Eigen::SelfAdjointEigenSolver<Eigen::MatrixXcd> es(mat);

		eigenvalues = es.eigenvalues();
	} else {
		std::size_t dim = 1;
		std::size_t two = 2;
		for (int i = 0; i < nQubits; i++)
			dim *= two;

		auto getBitStrForIdx = [&](std::uint64_t i) {
			std::stringstream s;
			for (int k = nQubits - 1; k >= 0; k--) s << ((i >> k) & 1);
			return s.str();
		};
		Eigen::MatrixXcd A(dim, dim);
		A.setZero();
		for (std::uint64_t myRow = 0; myRow < dim; myRow++) {
			auto rowBitStr = getBitStrForIdx(myRow);
			auto results = hamiltonian.computeActionOnBra(rowBitStr);
			for (auto& result : results) {
				std::uint64_t k = std::stol(result.first, nullptr, 2);
				A(myRow, k) += result.second;
			}
		}

		es.compute(A);
		eigenvalues = es.eigenvalues();
	}

	gsReal = eigenvalues(0);
	std::stringstream ss;
	ss << std::setprecision(12) << gsReal;
	xacc::info("Ground State Energy of Hamiltonian = " + ss.str());
	return std::real(gsReal);
}

double EigenDiagonalizeBackend::diagonalize(
		std::shared_ptr<VQEProgram> prog) {
	auto hamiltonian = prog->getPauliOperator();
    return diagonalize(hamiltonian);
}

std::pair<double, Eigen::VectorXcd> EigenDiagonalizeBackend::diagonalizeWithGroundState(std::shared_ptr<VQEProgram> prog) {
    auto ground = diagonalize(prog);
    return {ground, es.eigenvectors().col(0)};
}
}
}


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

#define BOOST_MATH_MAX_SERIES_ITERATION_POLICY 10000000

#define BOOST_MATH_USE_MPFR
#include "mp_t.hpp"
#include <boost/math/special_functions/hypergeometric_1f1.hpp>
#include <boost/math/special_functions/hypergeometric_pfq.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/lexical_cast.hpp>
#include <fstream>
#include <map>
#include <boost/math/tools/test_data.hpp>
#include <boost/random.hpp>

#include <boost/multiprecision/mpfr.hpp>

using namespace boost::math::tools;
using namespace boost::math;
using namespace std;
using namespace boost::multiprecision;

struct hypergeometric_1f1_gen
{
   mp_t operator()(mp_t a1, mp_t a2, mp_t z)
   {
      mp_t result;
      try {
         result = (mp_t)log(abs(boost::math::hypergeometric_pFq_precision({ mpfr_float(a1) }, { mpfr_float(a2) }, mpfr_float(z), 70, 25.0)));
         std::cout << a1 << " " << a2 << " " << z << " " << result << std::endl;
      }
      catch (...)
      {
         throw std::domain_error("");
      }
      if (fabs(result) > (std::numeric_limits<double>::max)())
      {
         std::cout << "Rejecting over large value\n";
         throw std::domain_error("");
      }
      if (fabs(result) < 1/1024.0)
      {
         std::cout << "Rejecting over small value\n";
         throw std::domain_error("");
      }
      return result;
   }
};

int main(int, char* [])
{
   parameter_info<mp_t> arg1, arg2, arg3;
   test_data<mp_t> data;

   std::cout << "Welcome.\n"
      "This program will generate spot tests for 1F1 (Yeh!!):\n";

   std::string line;
   //bool cont;

   std::vector<mp_t> v;
   random_ns::mt19937 rnd;
   random_ns::uniform_real_distribution<float> ur_a(0, 1);

   mp_t p = ur_a(rnd);
   p *= 1e6;
   v.push_back(p);
   v.push_back(-p);
   p = ur_a(rnd);
   p *= 1e5;
   v.push_back(p);
   v.push_back(-p);
   p = ur_a(rnd);
   p *= 1e4;
   v.push_back(p);
   v.push_back(-p);
   p = ur_a(rnd);
   p *= 1e3;
   v.push_back(p);
   v.push_back(-p);
   p = ur_a(rnd);
   p *= 1e2;
   v.push_back(p);
   v.push_back(-p);
   p = ur_a(rnd);
   p *= 1e-5;
   v.push_back(p);
   v.push_back(-p);
   p = ur_a(rnd);
   p *= 1e-12;
   v.push_back(p);
   v.push_back(-p);
   p = ur_a(rnd);
   p *= 1e-30;
   v.push_back(p);
   v.push_back(-p);

   for (unsigned i = 0; i < v.size(); ++i)
   {
      for (unsigned j = 0; j < v.size(); ++j)
      {
         for (unsigned k = 0; k < v.size(); ++k)
         {
            std::cout << i << " " << j << " " << k << std::endl;
            std::cout << v[i] << " " << (v[j] * 3) / 2 << " " << (v[k] * 5) / 4 << std::endl;
            arg1 = make_single_param(v[i]);
            arg2 = make_single_param(mp_t((v[j] * 3) / 2));
            arg3 = make_single_param(mp_t((v[k] * 5) / 4));
            data.insert(hypergeometric_1f1_gen(), arg1, arg2, arg3);
         }
      }
   }

   std::cout << "Enter name of test data file [default=hypergeometric_1f1.ipp]";
   std::getline(std::cin, line);
   boost::algorithm::trim(line);
   if(line == "")
      line = "hypergeometric_1f1.ipp";
   std::ofstream ofs(line.c_str());
   ofs << std::scientific << std::setprecision(40);
   write_code(ofs, data, line.c_str());

   return 0;
}