//          Copyright W.P. McNeill 2010.
// Distributed under 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)

// This program uses the A-star search algorithm in the Boost Graph Library to
// solve a maze.  It is an example of how to apply Boost Graph Library
// algorithms to implicit graphs.
//
// This program generates a random maze and then tries to find the shortest
// path from the lower left-hand corner to the upper right-hand corner.  Mazes
// are represented by two-dimensional grids where a cell in the grid may
// contain a barrier.  You may move up, down, right, or left to any adjacent
// cell that does not contain a barrier.
//
// Once a maze solution has been attempted, the maze is printed.  If a
// solution was found it will be shown in the maze printout and its length
// will be returned.  Note that not all mazes have solutions.
//
// The default maze size is 20x10, though different dimensions may be
// specified on the command line.

#include <boost/graph/astar_search.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/grid_graph.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>
#include <ctime>
#include <iostream>

boost::mt19937 random_generator;

// Distance traveled in the maze
typedef double distance;

#define GRID_RANK 2
typedef boost::grid_graph<GRID_RANK> grid;
typedef boost::graph_traits<grid>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<grid>::vertices_size_type vertices_size_type;

// A hash function for vertices.
struct vertex_hash:std::unary_function<vertex_descriptor, std::size_t> {
  std::size_t operator()(vertex_descriptor const& u) const {
    std::size_t seed = 0;
    boost::hash_combine(seed, u[0]);
    boost::hash_combine(seed, u[1]);
    return seed;
  }
};

typedef boost::unordered_set<vertex_descriptor, vertex_hash> vertex_set;
typedef boost::vertex_subset_complement_filter<grid, vertex_set>::type
        filtered_grid;

// A searchable maze
//
// The maze is grid of locations which can either be empty or contain a
// barrier.  You can move to an adjacent location in the grid by going up,
// down, left and right.  Moving onto a barrier is not allowed.  The maze can
// be solved by finding a path from the lower-left-hand corner to the
// upper-right-hand corner.  If no open path exists between these two
// locations, the maze is unsolvable.
//
// The maze is implemented as a filtered grid graph where locations are
// vertices.  Barrier vertices are filtered out of the graph.
//
// A-star search is used to find a path through the maze. Each edge has a
// weight of one, so the total path length is equal to the number of edges
// traversed.
class maze {
public:
  friend std::ostream& operator<<(std::ostream&, const maze&);
  friend maze random_maze(std::size_t, std::size_t);

  maze():m_grid(create_grid(0, 0)),m_barrier_grid(create_barrier_grid()) {};
  maze(std::size_t x, std::size_t y):m_grid(create_grid(x, y)),
       m_barrier_grid(create_barrier_grid()) {};

  // The length of the maze along the specified dimension.
  vertices_size_type length(std::size_t d) const {return m_grid.length(d);}

  bool has_barrier(vertex_descriptor u) const {
    return m_barriers.find(u) != m_barriers.end();
  }

  // Try to find a path from the lower-left-hand corner source (0,0) to the
  // upper-right-hand corner goal (x-1, y-1).
  vertex_descriptor source() const {return vertex(0, m_grid);}
  vertex_descriptor goal() const {
    return vertex(num_vertices(m_grid)-1, m_grid);
  }

  bool solve();
  bool solved() const {return !m_solution.empty();}
  bool solution_contains(vertex_descriptor u) const {
    return m_solution.find(u) != m_solution.end();
  }

private:
  // Create the underlying rank-2 grid with the specified dimensions.
  grid create_grid(std::size_t x, std::size_t y) {
    boost::array<std::size_t, GRID_RANK> lengths = { {x, y} };
    return grid(lengths);
  }

  // Filter the barrier vertices out of the underlying grid.
  filtered_grid create_barrier_grid() {
    return boost::make_vertex_subset_complement_filter(m_grid, m_barriers);
  }

  // The grid underlying the maze
  grid m_grid;
  // The underlying maze grid with barrier vertices filtered out
  filtered_grid m_barrier_grid;
  // The barriers in the maze
  vertex_set m_barriers;
  // The vertices on a solution path through the maze
  vertex_set m_solution;
  // The length of the solution path
  distance m_solution_length;
};

// Euclidean heuristic for a grid
//
// This calculates the Euclidean distance between a vertex and a goal
// vertex.
class euclidean_heuristic:
      public boost::astar_heuristic<filtered_grid, double>
{
public:
  euclidean_heuristic(vertex_descriptor goal):m_goal(goal) {};

  double operator()(vertex_descriptor v) {
    return sqrt(pow(m_goal[0] - v[0], 2) + pow(m_goal[1] - v[1], 2));
  }

private:
  vertex_descriptor m_goal;
};

// Exception thrown when the goal vertex is found
struct found_goal {};

// Visitor that terminates when we find the goal vertex
struct astar_goal_visitor:public boost::default_astar_visitor {
  astar_goal_visitor(vertex_descriptor goal):m_goal(goal) {};

  void examine_vertex(vertex_descriptor u, const filtered_grid&) {
    if (u == m_goal)
      throw found_goal();
  }

private:
  vertex_descriptor m_goal;
};

// Solve the maze using A-star search.  Return true if a solution was found.
bool maze::solve() {
  boost::static_property_map<distance> weight(1);
  // The predecessor map is a vertex-to-vertex mapping.
  typedef boost::unordered_map<vertex_descriptor,
                               vertex_descriptor,
                               vertex_hash> pred_map;
  pred_map predecessor;
  boost::associative_property_map<pred_map> pred_pmap(predecessor);
  // The distance map is a vertex-to-distance mapping.
  typedef boost::unordered_map<vertex_descriptor,
                               distance,
                               vertex_hash> dist_map;
  dist_map distance;
  boost::associative_property_map<dist_map> dist_pmap(distance);

  vertex_descriptor s = source();
  vertex_descriptor g = goal();
  euclidean_heuristic heuristic(g);
  astar_goal_visitor visitor(g);

  try {
    astar_search(m_barrier_grid, s, heuristic,
                 boost::weight_map(weight).
                 predecessor_map(pred_pmap).
                 distance_map(dist_pmap).
                 visitor(visitor) );
  } catch(found_goal fg) {
    // Walk backwards from the goal through the predecessor chain adding
    // vertices to the solution path.
    for (vertex_descriptor u = g; u != s; u = predecessor[u])
      m_solution.insert(u);
    m_solution.insert(s);
    m_solution_length = distance[g];
    return true;
  }

  return false;
}

#define BARRIER "#"
// Print the maze as an ASCII map.
std::ostream& operator<<(std::ostream& output, const maze& m) {
  // Header
  for (vertices_size_type i = 0; i < m.length(0)+2; i++)
    output << BARRIER;
  output << std::endl;
  // Body
  for (int y = m.length(1)-1; y >= 0; y--) {
    // Enumerate rows in reverse order and columns in regular order so that
    // (0,0) appears in the lower left-hand corner.  This requires that y be
    // int and not the unsigned vertices_size_type because the loop exit
    // condition is y==-1.
    for (vertices_size_type x = 0; x < m.length(0); x++) {
      // Put a barrier on the left-hand side.
      if (x == 0)
        output << BARRIER;
      // Put the character representing this point in the maze grid.
      vertex_descriptor u = {{x, y}};
      if (m.solution_contains(u))
        output << ".";
      else if (m.has_barrier(u))
        output << BARRIER;
      else
        output << " ";
      // Put a barrier on the right-hand side.
      if (x == m.length(0)-1)
        output << BARRIER;
    }
    // Put a newline after every row except the last one.
    output << std::endl;
  }
  // Footer
  for (vertices_size_type i = 0; i < m.length(0)+2; i++)
    output << BARRIER;
  if (m.solved())
    output << std::endl << "Solution length " << m.m_solution_length;
  return output;
}

// Return a random integer in the interval [a, b].
std::size_t random_int(std::size_t a, std::size_t b) {
  if (b < a)
    b = a;
  boost::uniform_int<> dist(a, b);
  boost::variate_generator<boost::mt19937&, boost::uniform_int<> >
  generate(random_generator, dist);
  return generate();
}

// Generate a maze with a random assignment of barriers.
maze random_maze(std::size_t x, std::size_t y) {
  maze m(x, y);
  vertices_size_type n = num_vertices(m.m_grid);
  vertex_descriptor s = m.source();
  vertex_descriptor g = m.goal();
  // One quarter of the cells in the maze should be barriers.
  int barriers = n/4;
  while (barriers > 0) {
    // Choose horizontal or vertical direction.
    std::size_t direction = random_int(0, 1);
    // Walls range up to one quarter the dimension length in this direction.
    vertices_size_type wall = random_int(1, m.length(direction)/4);
    // Create the wall while decrementing the total barrier count.
    vertex_descriptor u = vertex(random_int(0, n-1), m.m_grid);
    while (wall) {
      // Start and goal spaces should never be barriers.
      if (u != s && u != g) {
        wall--;
        if (!m.has_barrier(u)) {
          m.m_barriers.insert(u);
          barriers--;
        }
      }
      vertex_descriptor v = m.m_grid.next(u, direction);
      // Stop creating this wall if we reached the maze's edge.
      if (u == v)
        break;
      u = v;
    }
  }
  return m;
}

int main (int argc, char const *argv[]) {
  // The default maze size is 20x10.  A different size may be specified on
  // the command line.
  std::size_t x = 20;
  std::size_t y = 10;

  if (argc == 3) {
    x = boost::lexical_cast<std::size_t>(argv[1]);
    y = boost::lexical_cast<std::size_t>(argv[2]);
  }

  random_generator.seed(std::time(0));
  maze m = random_maze(x, y);

  if (m.solve())
    std::cout << "Solved the maze." << std::endl;
  else
    std::cout << "The maze is not solvable." << std::endl;
  std::cout << m << std::endl;
  return 0;
}


#include <eve/function/heuman_lambda.hpp>
#include <boost/math/special_functions/heuman_lambda.hpp>
#include <eve/wide.hpp>
#include <iostream>
#include <eve/constant/pio_2.hpp>
#include <iostream>

using wide_ft = eve::wide<float, eve::fixed<4>>;

int main()
{
  wide_ft k    = {1.0e-3f, 0.1f, 0.75f, 0.5f};
  wide_ft phi0 = {0.2f, 1.0e-5f, 0.5f, 0.0f};
  wide_ft phi1 = phi0+eve::pio_2(as(phi0));

  std::cout << "---- simd" << '\n'
             << "<- phi0                   = " << phi0 << '\n'
             << "<- phi1                   = " << phi1 << '\n'
             << "<- k                      = " << k << '\n'
             << "-> heuman_lambda(phi0, k) = " << eve::heuman_lambda(phi0, k) << '\n'
             << "-> heuman_lambda(phi1, k) = " << eve::heuman_lambda(phi1, k) << '\n'       ;

  float kf = 0.1f;
  float phif = 1.1f;

  std::cout << "---- scalar" << '\n'
            << "<- xf                      = " << kf << '\n'
            << "<- phif                    = " << phif<< '\n'
            << "-> heuman_lambda(phif, kf) = " << eve::heuman_lambda(phif, kf) << '\n';

  return 0;
}


#ifndef BOOST_METAPARSE_GETTING_STARTED_5_2_2_HPP
#define BOOST_METAPARSE_GETTING_STARTED_5_2_2_HPP

// Automatically generated header file

// Definitions before section 5.2.1
#include "5_2_1.hpp"

// Definitions of section 5.2.1
#include <boost/mpl/fold.hpp>

using vector_of_numbers =
 boost::mpl::vector<
   boost::mpl::int_<2>,
   boost::mpl::int_<5>,
   boost::mpl::int_<6>
 >;

template <class Vector>
 struct sum_vector :
    boost::mpl::fold<
      Vector,
      boost::mpl::int_<0>,
      boost::mpl::lambda<
        boost::mpl::plus<boost::mpl::_1, boost::mpl::_2>
      >::type
    >
  {};

// query:
//    sum_vector<vector_of_numbers>::type

template <class Sum, class Item>
   struct sum_items :
     boost::mpl::plus<
       Sum,
       typename boost::mpl::at_c<Item, 1>::type
     >
 {};

// query:
//   sum_items<
//      mpl_::integral_c<int, 1>,
//      boost::mpl::vector<mpl_::char_<'+'>, mpl_::integral_c<int, 2>>
//    >::type

// query:
//    boost::mpl::at_c<temp_result, 1>::type

// query:
//   boost::mpl::fold<
//      boost::mpl::at_c<temp_result, 1>::type, /* The vector to summarise */
//      boost::mpl::int_<0>, /* The value to start the sum from */
//      boost::mpl::quote2<sum_items> /* The function to call in each iteration */
//    >::type

#endif


#include "Camera2D.h"
#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <GL/glut.h>
Camera2D::
Camera2D()
	:mCx(0.0),mCy(0.5),mdx(2.0)
{

}

void
Camera2D::
Apply()
{
	GLint w = glutGet(GLUT_WINDOW_WIDTH);
	GLint h = glutGet(GLUT_WINDOW_HEIGHT);
	double aspect_ratio_inv = (double)h/(double)w;
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glOrtho(mCx - mdx*0.5,mCx + mdx*0.5,
			mCy - mdx*aspect_ratio_inv*0.5,mCy + mdx*aspect_ratio_inv*0.5,-1,1);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
}
void
Camera2D::
Pan(int x,int y,int prev_x,int prev_y)
{
	float delta = (float)prev_y - (float)y;
	delta*=0.01;

	mdx += delta;
	if(mdx<0.1)
		mdx=0.1;
	else if(mdx>100)
		mdx = 100.0;

}
void
Camera2D::
Translate(int x,int y,int prev_x,int prev_y)
{
	Eigen::Vector2d delta = GetWorldPosition(prev_x,prev_y)-GetWorldPosition(x,y);

	mCx += delta[0];
	mCy += delta[1];
}
Eigen::Vector2d
Camera2D::
GetWorldPosition(int x,int y)
{
	GLint w = glutGet(GLUT_WINDOW_WIDTH);
	GLint h = glutGet(GLUT_WINDOW_HEIGHT);

	Eigen::Vector2d screen_pos((double)x/(double)w-0.5,(1.0-(double)y/(double)h)-0.5);
	Eigen::Vector2d pos = mdx*screen_pos+Eigen::Vector2d(mCx,mCy);

	return pos;
}


#ifndef __DOUBLE_PENDULUM_H_
#define __DOUBLE_PENDULUM_H_

#include <SFML/Graphics.hpp>
#include <armadillo>

class DoublePendulum {

    private:

        // Representation of the pendulums
        sf::RectangleShape pend1;
        sf::RectangleShape pend2;

        // Representation of the pivots
        sf::CircleShape pivot1;
        sf::CircleShape pivot2;

        double gravity;

        // Pivot vector coordinates
        arma::vec P1;
        arma::vec P2;

        // Center of mass vector coordinates
        arma::vec X1;
        arma::vec X2;

        // Vector from pivot to center of mass
        arma::vec R1;
        arma::vec R2;
        // Respective lengths
        double R1Length;
        double R2Length;

        // Angle at pivot between R and the downward vertical position [rad]
        double theta1;
        double theta2;

        // Vector from pivot 1 to pivot 2
        arma::vec L1;
        // Respective length
        double L1Length;

        // Angle from R1 to L1 [rad]
        double phi;

        // Mass of the pendulums [kg]
        double m1;
        double m2;

        // Rotation interia about the center of mass of the pendulums
        double I1;
        double I2;

    public:

        // Default constructor
        DoublePendulum();

        // Draw figures
        void draw(sf::RenderWindow *window);

};

#endif // __DOUBLE_PENDULUM_H_


#include <gtest/gtest.h>
#include <boost/math/distributions.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <stan/math/prim/mat.hpp>
#include <test/unit/math/prim/mat/prob/vector_rng_test_helper.hpp>
#include <limits>
#include <vector>

class UniformTestRig : public VectorRealRNGTestRig {
 public:
  UniformTestRig(std::vector<double> good_p1, std::vector<int> good_p1_int,
                 std::vector<double> bad_p1, std::vector<int> bad_p1_int,
                 std::vector<double> good_p2, std::vector<int> good_p2_int,
                 std::vector<double> bad_p2, std::vector<int> bad_p2_int)
      : VectorRealRNGTestRig(10000, 10, good_p1, good_p1_int, bad_p1,
                             bad_p1_int, good_p2, good_p2_int, bad_p2,
                             bad_p2_int) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& alpha, const T2& beta, const T3& unused,
                        T_rng& rng) const {
    return stan::math::uniform_rng(alpha, beta, rng);
  }

  std::vector<double> generate_quantiles(double alpha, double beta,
                                         double unused) const {
    std::vector<double> quantiles;
    double K = stan::math::round(2 * std::pow(N_, 0.4));
    boost::math::uniform_distribution<> dist(alpha, beta);

    for (int i = 1; i < K; ++i) {
      double frac = i / K;
      quantiles.push_back(quantile(dist, frac));
    }
    quantiles.push_back(std::numeric_limits<double>::max());

    return quantiles;
  }
};

TEST(ProbDistributionsUniform, errorCheck) {
  // beta must be greater than alpha, so we have to be careful about how the
  // tests are initialized here (the VectorRNGTestRig class assumes no order)

  check_dist_throws_all_types(
      UniformTestRig({-2.5, -1.7, -0.1, 0.0, 0.5}, {-3, -2, -1, 0, 1}, {}, {},
                     {1.1, 2.2, 3.8}, {2, 3, 6}, {}, {}));

  check_dist_throws_all_types(
      UniformTestRig({-7.5, -6.7, -5.1, -4.0}, {-7, -6, -5, -4}, {}, {},
                     {-3.0, -2.2, 0.0, 1.0}, {-3, -2, 0, 1, 2}, {}, {}));
}

TEST(ProbDistributionsUniform, distributionTest) {
  check_quantiles_real_real(UniformTestRig({-1.7, -0.1, 0.0, 0.5},
                                           {-2, -1, 0, 1}, {}, {}, {1.1, 3.8},
                                           {2, 6}, {}, {}));

  check_quantiles_real_real(UniformTestRig({-7.5, -5.1, -4.0}, {-7, -6, -4}, {},
                                           {}, {-2.2, 0.0, 1.0}, {-3, 0, 1, 2},
                                           {}, {}));
}


/**
Copyright (c) 2016 Theodore Gast, Chuyuan Fu, Chenfanfu Jiang, Joseph Teran

Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
of the Software, and to permit persons to whom the Software is furnished to do
so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

If the code is used in an article, the following paper shall be cited:
@techreport{qrsvd:2016,
  title={Implicit-shifted Symmetric QR Singular Value Decomposition of 3x3 Matrices},
  author={Gast, Theodore and Fu, Chuyuan and Jiang, Chenfanfu and Teran, Joseph},
  year={2016},
  institution={University of California Los Angeles}
}

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include <iostream>
#include <cmath>
#include "Tools.h"
#include "ImplicitQRSVD.h"
#include <Eigen/Jacobi>
#include <math.h>

template <class T>
void testAccuracy(const std::vector<Eigen::Matrix<T, 3, 3> >& AA,
    const std::vector<Eigen::Matrix<T, 3, 3> >& UU,
    const std::vector<Eigen::Matrix<T, 3, 1> >& SS,
    const std::vector<Eigen::Matrix<T, 3, 3> >& VV)
{
    T max_UUt_error = 0, max_VVt_error = 0, max_detU_error = 0, max_detV_error = 0, max_reconstruction_error = 0;
    T ave_UUt_error = 0, ave_VVt_error = 0, ave_detU_error = 0, ave_detV_error = 0, ave_reconstruction_error = 0;
    for (size_t i = 0; i < AA.size(); i++) {
        Eigen::Matrix<T, 3, 3> M = AA[i];
        Eigen::Matrix<T, 3, 1> S = SS[i];
        Eigen::Matrix<T, 3, 3> U = UU[i];
        Eigen::Matrix<T, 3, 3> V = VV[i];
        T error;
        error = (U * U.transpose() - Eigen::Matrix<T, 3, 3>::Identity()).array().abs().maxCoeff();
        max_UUt_error = (error > max_UUt_error) ? error : max_UUt_error;
        ave_UUt_error += fabs(error);
        error = (V * V.transpose() - Eigen::Matrix<T, 3, 3>::Identity()).array().abs().maxCoeff();
        max_VVt_error = (error > max_VVt_error) ? error : max_VVt_error;
        ave_VVt_error += fabs(error);
        error = fabs(fabs(U.determinant()) - (T)1);
        max_detU_error = (error > max_detU_error) ? error : max_detU_error;
        ave_detU_error += fabs(error);
        error = fabs(fabs(V.determinant()) - (T)1);
        max_detV_error = (error > max_detV_error) ? error : max_detV_error;
        ave_detV_error += fabs(error);
        error = (U * S.asDiagonal() * V.transpose() - M).array().abs().maxCoeff();
        max_reconstruction_error = (error > max_reconstruction_error) ? error : max_reconstruction_error;
        ave_reconstruction_error += fabs(error);
    }
    ave_UUt_error /= (T)(AA.size());
    ave_VVt_error /= (T)(AA.size());
    ave_detU_error /= (T)(AA.size());
    ave_detV_error /= (T)(AA.size());
    ave_reconstruction_error /= (T)(AA.size());
    std::cout << std::setprecision(10) << " UUt max error: " << max_UUt_error
              << " VVt max error: " << max_VVt_error
              << " detU max error:" << max_detU_error
              << " detV max error:" << max_detV_error
              << " recons max error:" << max_reconstruction_error << std::endl;
    std::cout << std::setprecision(10) << " UUt ave error: " << ave_UUt_error
              << " VVt ave error: " << ave_VVt_error
              << " detU ave error:" << ave_detU_error
              << " detV ave error:" << ave_detV_error
              << " recons ave error:" << ave_reconstruction_error << std::endl;
}

template <class T>
void runImplicitQRSVD(const int repeat, const std::vector<Eigen::Matrix<T, 3, 3> >& tests, const bool accuracy_test)
{
    using namespace JIXIE;
    std::vector<Eigen::Matrix<T, 3, 3> > UU, VV;
    std::vector<Eigen::Matrix<T, 3, 1> > SS;
    JIXIE::Timer timer;
    timer.start();
    double total_time = 0;
    for (int test_iter = 0; test_iter < repeat; test_iter++) {
        timer.click();
        for (size_t i = 0; i < tests.size(); i++) {
            Eigen::Matrix<T, 3, 3> M = tests[i];
            Eigen::Matrix<T, 3, 1> S;
            Eigen::Matrix<T, 3, 3> U;
            Eigen::Matrix<T, 3, 3> V;
            singularValueDecomposition(M, U, S, V);
            if (accuracy_test && test_iter == 0) {
                UU.push_back(U);
                SS.push_back(S);
                VV.push_back(V);
            }
        }
        double this_time = timer.click();
        total_time += this_time;
        std::cout << std::setprecision(10) << "impQR time: " << this_time << std::endl;
    }
    std::cout << std::setprecision(10) << "impQR Average time: " << total_time / (double)(repeat) << std::endl;
    if (accuracy_test)
        testAccuracy(tests, UU, SS, VV);
}

template <class T>
void addRandomCases(std::vector<Eigen::Matrix<T, 3, 3> >& tests, const T random_range, const int N)
{
    using namespace JIXIE;
    int old_count = tests.size();
    std::cout << std::setprecision(10) << "Adding random test cases with range " << -random_range << " to " << random_range << std::endl;
    RandomNumber<T> random_gen(123);
    for (int t = 0; t < N; t++) {
        Eigen::Matrix<T, 3, 3> Z;
        random_gen.fill(Z, -random_range, random_range);
        tests.push_back(Z);
    }
    std::cout << std::setprecision(10) << tests.size() - old_count << " cases added." << std::endl;
    std::cout << std::setprecision(10) << "Total test cases: " << tests.size() << std::endl;
}

template <class T>
void addIntegerCases(std::vector<Eigen::Matrix<T, 3, 3> >& tests, const int int_range)
{
    using namespace JIXIE;
    int old_count = tests.size();
    std::cout << std::setprecision(10) << "Adding integer test cases with range " << -int_range << " to " << int_range << std::endl;
    Eigen::Matrix<T, 3, 3> Z;
    Z.fill(-int_range);
    typename Eigen::Matrix<T, 3, 3>::Index i = 0;
    tests.push_back(Z);
    while (i < Eigen::Matrix<T, 3, 3>::SizeAtCompileTime) {
        if (Z(i) < int_range) {
            Z(i)++;
            tests.push_back(Z);
            i = 0;
        }
        else {
            Z(i) = -int_range;
            i++;
        }
    }
    std::cout << std::setprecision(10) << tests.size() - old_count << " cases added." << std::endl;
    std::cout << std::setprecision(10) << "Total test cases: " << tests.size() << std::endl;
}

template <class T>
void addPerturbationFromIdentityCases(std::vector<Eigen::Matrix<T, 3, 3> >& tests, const int num_perturbations, const T perturb)
{
    using namespace JIXIE;
    int old_count = tests.size();
    std::vector<Eigen::Matrix<T, 3, 3> > tests_tmp;
    Eigen::Matrix<T, 3, 3> Z = Eigen::Matrix<T, 3, 3>::Identity();
    tests_tmp.push_back(Z);
    std::cout << std::setprecision(10) << "Adding perturbed identity test cases with perturbation " << perturb << std::endl;
    RandomNumber<T> random_gen(123);
    size_t special_cases = tests_tmp.size();
    for (size_t t = 0; t < special_cases; t++) {
        for (int i = 0; i < num_perturbations; i++) {
            random_gen.fill(Z, -perturb, perturb);
            tests.push_back(tests_tmp[t] + Z);
        }
    }
    std::cout << std::setprecision(10) << tests.size() - old_count << " cases added." << std::endl;
    std::cout << std::setprecision(10) << "Total test cases: " << tests.size() << std::endl;
}

template <class T>
void addPerturbationCases(std::vector<Eigen::Matrix<T, 3, 3> >& tests, const int int_range, const int num_perturbations, const T perturb)
{
    using namespace JIXIE;
    int old_count = tests.size();
    std::vector<Eigen::Matrix<T, 3, 3> > tests_tmp;
    Eigen::Matrix<T, 3, 3> Z;
    Z.fill(-int_range);
    typename Eigen::Matrix<T, 3, 3>::Index i = 0;
    tests_tmp.push_back(Z);
    while (i < Eigen::Matrix<T, 3, 3>::SizeAtCompileTime) {
        if (Z(i) < int_range) {
            Z(i)++;
            tests_tmp.push_back(Z);
            i = 0;
        }
        else {
            Z(i) = -int_range;
            i++;
        }
    }
    std::cout << std::setprecision(10) << "Adding perturbed integer test cases with perturbation " << perturb << " and range " << -int_range << " to " << int_range << std::endl;
    RandomNumber<T> random_gen(123);
    size_t special_cases = tests_tmp.size();
    for (size_t t = 0; t < special_cases; t++) {
        for (int i = 0; i < num_perturbations; i++) {
            random_gen.fill(Z, -perturb, perturb);
            tests.push_back(tests_tmp[t] + Z);
        }
    }
    std::cout << std::setprecision(10) << tests.size() - old_count << " cases added." << std::endl;
    std::cout << std::setprecision(10) << "Total test cases: " << tests.size() << std::endl;
}

void runBenchmark()
{
    using namespace JIXIE;
    using std::fabs;

    bool run_qr;

    bool test_float;
    bool test_double;
    bool accuracy_test;
    bool normalize_matrix;
    int number_of_repeated_experiments;
    bool test_random;
    int random_range;
    int number_of_random_cases;
    bool test_integer;
    int integer_range;
    bool test_perturbation;
    int perturbation_count;
    float float_perturbation;
    double double_perturbation;
    bool test_perturbation_from_identity;
    int perturbation_from_identity_count;
    float float_perturbation_identity;
    double double_perturbation_identity;
    std::string title;

    // Finalized options
    run_qr = true;

    test_float = true;
    test_double = true;
    normalize_matrix = false;
    int number_of_repeated_experiments_for_timing = 2;

    for (int test_number = 1; test_number <= 10; test_number++) {

        if (test_number == 1) {
            title = "random timing test";
            number_of_repeated_experiments = number_of_repeated_experiments_for_timing;
            accuracy_test = false;
            test_random = true, random_range = 3, number_of_random_cases = 1024 * 1024; // random test
            test_integer = false; // integer test
            integer_range = 2; // this variable is used by both integer test and perturbed integer test
            test_perturbation = false, integer_range = 3, perturbation_count = 4, float_perturbation = (float)256 * std::numeric_limits<float>::epsilon(), double_perturbation = (double)256 * std::numeric_limits<double>::epsilon(); // perturbed integer test
            test_perturbation_from_identity = false, perturbation_from_identity_count = 1024 * 1024, float_perturbation_identity = 1e-3, double_perturbation_identity = 1e-3; // perturbed itentity test
        }
        if (test_number == 2) {
            title = "integer timing test";
            number_of_repeated_experiments = number_of_repeated_experiments_for_timing;
            accuracy_test = false;
            test_random = false, random_range = 3, number_of_random_cases = 1024 * 1024; // random test
            test_integer = true; // integer test
            integer_range = 2; // this variable is used by both integer test and perturbed integer test
            test_perturbation = false, perturbation_count = 4, float_perturbation = (float)256 * std::numeric_limits<float>::epsilon(), double_perturbation = (double)256 * std::numeric_limits<double>::epsilon(); // perturbed integer test
            test_perturbation_from_identity = false, perturbation_from_identity_count = 1024 * 1024, float_perturbation_identity = 1e-3, double_perturbation_identity = 1e-3; // perturbed itentity test
        }
        if (test_number == 3) {
            title = "integer-perturbation timing test: 256 eps";
            number_of_repeated_experiments = number_of_repeated_experiments_for_timing;
            accuracy_test = false;
            test_random = false, random_range = 3, number_of_random_cases = 1024 * 1024; // random test
            test_integer = false; // integer test
            integer_range = 2; // this variable is used by both integer test and perturbed integer test
            test_perturbation = true, perturbation_count = 4, float_perturbation = (float)256 * std::numeric_limits<float>::epsilon(), double_perturbation = (double)256 * std::numeric_limits<double>::epsilon(); // perturbed integer test
            test_perturbation_from_identity = false, perturbation_from_identity_count = 1024 * 1024, float_perturbation_identity = 1e-3, double_perturbation_identity = 1e-3; // perturbed itentity test
        }
        if (test_number == 4) {
            title = "identity-perturbation timing test: 1e-3";
            number_of_repeated_experiments = number_of_repeated_experiments_for_timing;
            accuracy_test = false;
            test_random = false, random_range = 3, number_of_random_cases = 1024 * 1024; // random test
            test_integer = false; // integer test
            integer_range = 2; // this variable is used by both integer test and perturbed integer test
            test_perturbation = false, perturbation_count = 4, float_perturbation = (float)256 * std::numeric_limits<float>::epsilon(), double_perturbation = (double)256 * std::numeric_limits<double>::epsilon(); // perturbed integer test
            test_perturbation_from_identity = true, perturbation_from_identity_count = 1024 * 1024, float_perturbation_identity = 1e-3, double_perturbation_identity = 1e-3; // perturbed itentity test
        }
        if (test_number == 5) {
            title = "identity-perturbation timing test: 256 eps";
            number_of_repeated_experiments = number_of_repeated_experiments_for_timing;
            accuracy_test = false;
            test_random = false, random_range = 3, number_of_random_cases = 1024 * 1024; // random test
            test_integer = false; // integer test
            integer_range = 2; // this variable is used by both integer test and perturbed integer test
            test_perturbation = false, perturbation_count = 4, float_perturbation = (float)256 * std::numeric_limits<float>::epsilon(), double_perturbation = (double)256 * std::numeric_limits<double>::epsilon(); // perturbed integer test
            test_perturbation_from_identity = true, perturbation_from_identity_count = 1024 * 1024, float_perturbation_identity = (float)256 * std::numeric_limits<float>::epsilon(), double_perturbation_identity = (double)256 * std::numeric_limits<double>::epsilon(); // perturbed itentity test
        }

        if (test_number == 6) {
            title = "random accuracy test";
            number_of_repeated_experiments = 1;
            accuracy_test = true;
            test_random = true, random_range = 3, number_of_random_cases = 1024 * 1024; // random test
            test_integer = false; // integer test
            integer_range = 2; // this variable is used by both integer test and perturbed integer test
            test_perturbation = false, integer_range = 3, perturbation_count = 4, float_perturbation = (float)256 * std::numeric_limits<float>::epsilon(), double_perturbation = (double)256 * std::numeric_limits<double>::epsilon(); // perturbed integer test
            test_perturbation_from_identity = false, perturbation_from_identity_count = 1024 * 1024, float_perturbation_identity = 1e-3, double_perturbation_identity = 1e-3; // perturbed itentity test
        }
        if (test_number == 7) {
            title = "integer accuracy test";
            number_of_repeated_experiments = 1;
            accuracy_test = true;
            test_random = false, random_range = 3, number_of_random_cases = 1024 * 1024; // random test
            test_integer = true; // integer test
            integer_range = 2; // this variable is used by both integer test and perturbed integer test
            test_perturbation = false, perturbation_count = 4, float_perturbation = (float)256 * std::numeric_limits<float>::epsilon(), double_perturbation = (double)256 * std::numeric_limits<double>::epsilon(); // perturbed integer test
            test_perturbation_from_identity = false, perturbation_from_identity_count = 1024 * 1024, float_perturbation_identity = 1e-3, double_perturbation_identity = 1e-3; // perturbed itentity test
        }
        if (test_number == 8) {
            title = "integer-perturbation accuracy test: 256 eps";
            number_of_repeated_experiments = 1;
            accuracy_test = true;
            test_random = false, random_range = 3, number_of_random_cases = 1024 * 1024; // random test
            test_integer = false; // integer test
            integer_range = 2; // this variable is used by both integer test and perturbed integer test
            test_perturbation = true, perturbation_count = 4, float_perturbation = (float)256 * std::numeric_limits<float>::epsilon(), double_perturbation = (double)256 * std::numeric_limits<double>::epsilon(); // perturbed integer test
            test_perturbation_from_identity = false, perturbation_from_identity_count = 1024 * 1024, float_perturbation_identity = 1e-3, double_perturbation_identity = 1e-3; // perturbed itentity test
        }
        if (test_number == 9) {
            title = "identity-perturbation accuracy test: 1e-3";
            number_of_repeated_experiments = 1;
            accuracy_test = true;
            test_random = false, random_range = 3, number_of_random_cases = 1024 * 1024; // random test
            test_integer = false; // integer test
            integer_range = 2; // this variable is used by both integer test and perturbed integer test
            test_perturbation = false, perturbation_count = 4, float_perturbation = (float)256 * std::numeric_limits<float>::epsilon(), double_perturbation = (double)256 * std::numeric_limits<double>::epsilon(); // perturbed integer test
            test_perturbation_from_identity = true, perturbation_from_identity_count = 1024 * 1024, float_perturbation_identity = 1e-3, double_perturbation_identity = 1e-3; // perturbed itentity test
        }
        if (test_number == 10) {
            title = "identity-perturbation accuracy test: 256 eps";
            number_of_repeated_experiments = 1;
            accuracy_test = true;
            test_random = false, random_range = 3, number_of_random_cases = 1024 * 1024; // random test
            test_integer = false; // integer test
            integer_range = 2; // this variable is used by both integer test and perturbed integer test
            test_perturbation = false, perturbation_count = 4, float_perturbation = (float)256 * std::numeric_limits<float>::epsilon(), double_perturbation = (double)256 * std::numeric_limits<double>::epsilon(); // perturbed integer test
            test_perturbation_from_identity = true, perturbation_from_identity_count = 1024 * 1024, float_perturbation_identity = (float)256 * std::numeric_limits<float>::epsilon(), double_perturbation_identity = (double)256 * std::numeric_limits<double>::epsilon(); // perturbed itentity test
        }

        std::cout << " \n========== RUNNING BENCHMARK TEST == " << title << "=======" << std::endl;
        std::cout << " run_qr " << run_qr << std::endl;
        std::cout << " test_float " << test_float << std::endl;
        std::cout << " test_double " << test_double << std::endl;
        std::cout << " accuracy_test " << accuracy_test << std::endl;
        std::cout << " normalize_matrix " << normalize_matrix << std::endl;
        std::cout << " number_of_repeated_experiments " << number_of_repeated_experiments << std::endl;
        std::cout << " test_random " << test_random << std::endl;
        std::cout << " random_range " << random_range << std::endl;
        std::cout << " number_of_random_cases " << number_of_random_cases << std::endl;
        std::cout << " test_integer " << test_integer << std::endl;
        std::cout << " integer_range " << integer_range << std::endl;
        std::cout << " test_perturbation " << test_perturbation << std::endl;
        std::cout << " perturbation_count " << perturbation_count << std::endl;
        std::cout << " float_perturbation " << float_perturbation << std::endl;
        std::cout << " double_perturbation " << double_perturbation << std::endl;
        std::cout << " test_perturbation_from_identity " << test_perturbation_from_identity << std::endl;
        std::cout << " perturbation_from_identity_count " << perturbation_from_identity_count << std::endl;
        std::cout << " float_perturbation_identity " << float_perturbation_identity << std::endl;
        std::cout << " double_perturbation_identity " << double_perturbation_identity << std::endl;

        std::cout << std::setprecision(10) << "\n--- float test ---\n" << std::endl;
        if (test_float) {
            std::vector<Eigen::Matrix<float, 3, 3> > tests;
            if (test_integer)
                addIntegerCases(tests, integer_range);
            if (test_perturbation)
                addPerturbationCases(tests, integer_range, perturbation_count, float_perturbation);
            if (test_perturbation_from_identity)
                addPerturbationFromIdentityCases(tests, perturbation_from_identity_count, float_perturbation_identity);
            if (test_random)
                addRandomCases(tests, (float)random_range, number_of_random_cases);
            if (normalize_matrix) {
                for (size_t i = 0; i < tests.size(); i++) {
                    float norm = tests[i].norm();
                    if (norm > (float)8 * std::numeric_limits<float>::epsilon()) {
                        tests[i] /= norm;
                    }
                }
            }
            std::cout << std::setprecision(10) << "\n-----------" << std::endl;
            if (run_qr)
                runImplicitQRSVD(number_of_repeated_experiments, tests, accuracy_test);

        }

        std::cout << std::setprecision(10) << "\n--- double test ---\n" << std::endl;
        if (test_double) {
            std::vector<Eigen::Matrix<double, 3, 3> > tests;
            if (test_integer)
                addIntegerCases(tests, integer_range);
            if (test_perturbation)
                addPerturbationCases(tests, integer_range, perturbation_count, double_perturbation);
            if (test_perturbation_from_identity)
                addPerturbationFromIdentityCases(tests, perturbation_from_identity_count, double_perturbation_identity);
            if (test_random)
                addRandomCases(tests, (double)random_range, number_of_random_cases);
            if (normalize_matrix) {
                for (size_t i = 0; i < tests.size(); i++) {
                    double norm = tests[i].norm();
                    if (norm > (double)8 * std::numeric_limits<double>::epsilon()) {
                        tests[i] /= norm;
                    }
                }
            }
            std::cout << std::setprecision(10) << "\n-----------" << std::endl;
            if (run_qr)
                runImplicitQRSVD(number_of_repeated_experiments, tests, accuracy_test);

        }
    }
}

void calc_V(const Eigen::Matrix2f& C, Eigen::Matrix2f& V) {

	Eigen::Matrix2f Sigma;
	float tau, t_2, c_2, s_2;

	tau = (C(1, 1) - C(0, 0)) / (2 * C(1, 0));
	if (tau>0) {
		t_2 = tau - sqrt(1 + tau*tau);
	}
	else {
		t_2 = tau + sqrt(1 + tau*tau);
	}
	c_2 = 1 / sqrt(1 + t_2*t_2);
	s_2 = t_2*c_2;
	V << c_2, -s_2, s_2, c_2;
}

void signconvention(Matrix2f& U, Matrix2f& V, Vector2f& sigma) {
	Vector2f tempvec;
	float tempf;
	if (sigma(0)<0 && sigma(1)<0) {
		U = -U;
		sigma = -sigma;
	}
	if (sigma(0)<0) {
		tempvec = U.col(0);
		U.col(0) << U.col(1);
		U.col(1) = tempvec;
		tempf = sigma(0);
		sigma(0) = sigma(1);
		sigma(1) = tempf;
		tempvec = V.col(0);
		V.col(0) << V.col(1);
		V.col(1) = tempvec;

	}
	else if (sigma(0) >= 0 && sigma(1) >= 0) {
		if (sigma(1) > sigma(0)) {
			tempvec = U.col(0);
			U.col(0) << U.col(1);
			U.col(1) = tempvec;
			tempf = sigma(0);
			sigma(0) = sigma(1);
			sigma(1) = tempf;
			tempvec = V.col(0);
			V.col(0) << V.col(1);
			V.col(1) = tempvec;
		}
	}

}

void My_SVD(const Eigen::Matrix2f& F,Eigen::Matrix2f& U,Eigen::Matrix2f& sigma,Eigen::Matrix2f& V){
//
//Compute the SVD of input F with sign conventions discussed in class and in assignment
//
//input: F
//output: U,sigma,V with F=U*sigma*V.transpose() and U*U.transpose()=V*V.transpose()=I;

	Eigen::Matrix2f C, A, Sigma;
	Eigen::Vector2f b, v, sigma_vec;
	Eigen::JacobiRotation<float> G;
	float a, c_U, s_U;

	C = F.transpose()*F;
	calc_V(C, V);

	A = F*V;

	G.makeGivens(A(0, 0), A(1, 0));
	v << 1, 0;
	v.applyOnTheLeft(0, 1, G);
	c_U = v(0);
	s_U = -v(1);
	U << c_U, s_U, -s_U, c_U;
	Sigma = A;
	Sigma.applyOnTheLeft(0, 1, G.adjoint());

	sigma_vec(0) = Sigma(0, 0);
	sigma_vec(1) = Sigma(1, 1);

	signconvention(U, V, sigma_vec);

	sigma << sigma_vec(0), 0,
		0, sigma_vec(1);

}

void My_Polar(const Eigen::Matrix3f& F,Eigen::Matrix3f& R,Eigen::Matrix3f& S){
  //
  //Compute the polar decomposition of input F (with det(R)=1)
  //
  //input: F
  //output: R,s with F=R*S and R*R.transpose()=I and S=S.transpose()

	Eigen::JacobiRotation<float> G;
	float tol = 1e-6, max_it = 1e4, S_diff[] = { 100,100,100 }, S_diff_max;
	int it = 0, i_rot = 0, i, j, i_max;

	R = MatrixXf::Identity(3, 3);
	S = F;

	for (i_max = 0; i_max < 3; i_max++) {
		if (i_max == 0)
			S_diff_max = S_diff[0];
		else if (S_diff[i_max] > S_diff[i_max - 1])
			S_diff_max = S_diff[i_max];
	}

	while (it<max_it && S_diff_max > tol) {

		for (i_rot = 0; i_rot < 3; i_rot++) {
			if (i_rot == 0) {
				i = 1;
				j = 2;
			}
			else if (i_rot == 1) {
				i = 0;
				j = 2;
			}
			else if (i_rot == 2) {
				i = 0;
				j = 1;
			}
			G.makeGivens(S(i, i) + S(j, j), S(i, j) - S(j, i));
			R.applyOnTheRight(i, j, G.adjoint());
			S.applyOnTheLeft(i, j, G);

		}

		it++;
		S_diff[0] = std::abs(S(1, 2) - S(2, 1));
		S_diff[1] = std::abs(S(0, 2) - S(2, 0));
		S_diff[2] = std::abs(S(0, 1) - S(1, 0));
		for (i_max = 0; i_max < 3; i_max++) {
			if (i_max == 0)
				S_diff_max = S_diff[0];
			else if (S_diff[i_max] > S_diff[i_max - 1])
				S_diff_max = S_diff[i_max];
		}

	}

}

// void Algorithm_2_Test(){
//
//   Eigen::Matrix2f F,C,U,V;
//   F<<1,2,3,4;
//   C=F*F.transpose();
//   Eigen::Vector2f s2;
//   JIXIE::Jacobi(C,s2,V);
//
// }

int main()
{
  bool run_benchmark = false;
  if (run_benchmark) runBenchmark();
}


// The MIT License (MIT)

// Copyright (c) 2018 li chunpeng, Xidian university

// Permission is hereby granted, free of charge, to any person obtaining sum copy of
// this software and associated documentation files (the "Software"), to deal in
// the Software without restriction, including without limitation the rights to
// use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
// the Software, and to permit persons to whom the Software is furnished to do so,
// subject to the following conditions:

// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
// FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
// IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

#ifndef _URANUS_UN_CONSTRAINED_HPP_
#define _URANUS_UN_CONSTRAINED_HPP_

// 常见无约束优化算法
// 直接方法
// 间接方法

#include <iostream>
#include <Eigen/Dense>
#include "Matrix.hpp"

using namespace std;

template<int Dim> struct problem
{
	uranus::SquareMatrix<Dim> matirx_Jacobian_;  // 一阶求导矩阵

	uranus::Vector<Dim>& gradient(uranus::Vector<Dim>& var_x)
	{
		var_x = matirx_Jacobian_ * var_x;
		return var_x;
	}
};

template<int Dim> double myval(uranus::Vector<Dim> s_k)
{
	double sum = 0;
	for(int i=0;i<Dim;++i)
	{
		sum = sum + pow(s_k(i),2);
	}
	return sqrt(sum);
}
/**
 * @brief 使用秩为2的模拟牛顿法, BFGS
 * @param 求解的问题f
 * @param 初始点 var_x,是一个向量，可包含多维
 * @param 终止条件 delta, 默认 0.001
 * @param 是否打印中间结果
 * @return 求解得到的最优点
 */
template<int Dim>
uranus::Vector<Dim> BFGS(problem<Dim> f,
						 uranus::Vector<Dim> var_x,
						 const double delta = 0.001,
						 bool visual = false)
{
	uranus::Vector<Dim> s_k;
	uranus::Vector<Dim> y_k;
	uranus::SquareMatrix<Dim> H_k;
	uranus::SquareMatrix<Dim> E_k1;
	uranus::SquareMatrix<Dim> E_k2;

	// init 单位矩阵
	for (int i = 0;i < Dim; ++i)
		for (int j = 0;j < Dim; ++j)
			if ( i == j) H_k(i,j) = 1;
			else H_k(i,j) = 0;

	while(myval<Dim>(s_k) < delta)
	{
		s_k = -H_k * f.gradient(var_x);           // s_k = var_x2 - var_x1
		y_k = f.gradient(var_x);
		var_x = var_x - H_k * f.gradient(var_x);
		y_k = f.gradient(var_x) - y_k;            // y_k = \delta f_{k} - \delta f_{k-1}

		E_k1  = (s_k * s_k.transpose() - H_k * y_k * s_k.transpose() - s_k * y_k.transpose() * H_k)
		      / (s_k.transpose() * y_k);
		E_k2 = s_k * s_k.transpose();

		double k21 = y_k.transpose() * H_k * y_k;
		double k22 = pow((s_k.transpose() * y_k),2);

		H_k = H_k + E_k1 + E_k2 * k21/k22;    // update H_k

		if(visual)
		{
			cout << "s_k:\n" << s_k << "\n"
				 << "y_k:\n" << y_k << "\n"
				 << "H_k:\n" << H_k << "\n";
 		}
	}
	return var_x; // result
}
/**
 * @brief 使用BFGS的外点法
 * @param 初始点 var_x,是一个向量，可包含多维
 * @param 终止条件 delta, 默认 0.001
 * @param 是否打印中间结果
 * @return 求解得到的最优点
 */
template<int Dim>
void External_point_method(uranus::Vector<Dim> var_x,
						   const double delta = 0.001,
						   const double C = 10
						   bool visual = false)
{
	double M_k = 0.1;                          // init M_k > 0
	do{
		M_k = C * M_k;                         // update M_k
		f.Jacobian_Matrix(1,1) = 2 + M_k;      // update J mat

		var_x = BFGS(f, var_x, delta, visual); // min f
	}
	while(M_k * pow(var_x(1)-1), 2) > delta);

}

#endif


#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>
#include <set>

using namespace std;
using boost::multiprecision::cpp_int;

int main(int argc, char *argv[]) {
	// Sets only hold unique integers, making it the perfect container.
	set<cpp_int> numbers;
	for(cpp_int a = 2; a < 101; a++) {
		for(int b = 2; b < 101; b++) {
			numbers.emplace(boost::multiprecision::pow(a, b));
		}
	}
	cout << numbers.size() << endl;
	return 0;
}


// Copyright Louis Dionne 2015
// Distributed under the Boost Software License, Version 1.0.

#include <boost/hana/assert.hpp>
#include <boost/hana/functional/fix.hpp>
#include <boost/hana/functional/partial.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/lazy.hpp>
#include <boost/hana/tuple.hpp>
using namespace boost::hana;

namespace disambiguate {
// sample(sorting-now)
template <typename Xs, typename Pred>
auto sort(Xs xs, Pred pred) {
  return eval_if(length(xs) < size_t<2>,
    lazy(xs),
    lazy([=](auto xs) {
      auto pivot = head(xs);
      auto parts = partition(tail(xs), partial(pred, pivot));
      return concat(
        append(sort(second(parts), pred), pivot),
        sort(first(parts), pivot)
      );
    })(xs)
  );
}
// end-sample
}

int main() {
  using disambiguate::sort;
  BOOST_HANA_CONSTANT_CHECK(
    sort(make_tuple(), less) == make_tuple()
  );

  BOOST_HANA_CONSTANT_CHECK(
    sort(make_tuple(int_<1>), less) == make_tuple(int_<1>)
  );

  BOOST_HANA_CONSTANT_CHECK(
    sort(make_tuple(int_<2>, int_<1>), less) == make_tuple(int_<1>, int_<2>)
  );

  BOOST_HANA_CONSTANT_CHECK(
    sort(make_tuple(int_<3>, int_<2>, int_<1>), less) ==
      make_tuple(int_<1>, int_<2>, int_<3>)
  );

  BOOST_HANA_CONSTANT_CHECK(
    sort(make_tuple(int_<4>, int_<3>, int_<2>, int_<1>), less) ==
      make_tuple(int_<1>, int_<2>, int_<3>, int_<4>)
  );
}


#include <Eigen/Dense>
#include <gtest/gtest.h>

#include "solvers/least_squares.h"

namespace lf
{
namespace
{
class LeastSquaresFixture
  : public LeastSquares, public testing::Test
{
protected:
  LeastSquaresFixture(): LeastSquares() {};
};

TEST_F(LeastSquaresFixture, LeastSquares) {
  // Initialization
  Eigen::ArrayXf noise_x{ 2 };
  noise_x << 0, 1;
  Eigen::ArrayXf noise_y{ 2 };
  noise_y << 0, 1;

  Line line = Fit(noise_x, noise_y);
  ASSERT_FLOAT_EQ(line.m, 1.0f);
  ASSERT_FLOAT_EQ(line.b, 0.0f);
}
} // namespace
} // namespace lf


#include <iostream>
#include <armadillo>

using namespace std;
using namespace arma;

int main()
{
    int nx=400;
    int nz=600;
    arma::Mat<float> model_ori(nz,nx,fill::zeros);

    arma::Mat<float> model_sur(nz,nx,fill::zeros);

    arma::Col<float> surface(nx,fill::zeros);

    model_ori.load("./model_nz600_nx400.dat",raw_binary);
    model_ori.reshape(nz,nx);

    for (int ix=0; ix<nx ;ix++)
    {
        int flag = 0;

        for(int iz=1; iz<nz; iz++)
        {
            if(flag==0 &&  model_ori(iz,ix)>5*model_ori(iz-1,ix))
            {
                surface(ix)=iz;
                flag = 1;
            }
        }
    }

    float mean = arma::median(surface);

    for(int ix=0; ix<nx ;ix++)
    {
        for(int iz=0; iz<nz;iz++)
        {
            if (iz<surface(ix))
            {
                model_sur(iz,ix)=model_ori(iz,ix);
            }
            else
            {
                //01 : every point
                //model_sur(iz,ix)=model_ori(surface(ix),ix);

                //02 : mean velocity
                //model_sur(iz,ix)=mean;

                //03 : some trick
                if(model_ori(iz,ix)>1000)
                {
                    model_sur(iz,ix) = 1000;
                }
                else
                {
                    model_sur(iz,ix) = model_ori(iz,ix);
                }

            }
        }
    }

    model_sur.save("model_topo.dat",raw_binary);

    return 0;
}


/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

// includes
// std
#include <iostream>

// boost
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Eigen_QLD
#include <boost/math/constants/constants.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

// Eigen
#include <Eigen/Dense>

// EigenQP
#include <eigen-qld/QLD.h>

struct QP1
{
  QP1()
  {
    nrvar = 6;
    nreq = 3;
    nrineq = 2;

    Q.resize(nrvar, nrvar);
    Aeq.resize(nreq, nrvar);
    Aineq.resize(nrineq, nrvar);
    A.resize(nreq + nrineq, nrvar);

    C.resize(nrvar);
    Beq.resize(nreq);
    Bineq.resize(nrineq);
    B.resize(nreq + nrineq);
    XL.resize(nrvar);
    XU.resize(nrvar);
    X.resize(nrvar);

    Aeq << 1., -1., 1., 0., 3., 1., -1., 0., -3., -4., 5., 6., 2., 5., 3., 0., 1., 0.;
    Beq << 1., 2., 3.;

    Aineq << 0., 1., 0., 1., 2., -1., -1., 0., 2., 1., 1., 0.;
    Bineq << -1., 2.5;

    A.topRows(nreq) = Aeq;
    A.bottomRows(nrineq) = -Aineq;

    B.head(nreq) = -Beq;
    B.tail(nrineq) = Bineq;

    // with  x between ci and cs:
    XL << -1000., -10000., 0., -1000., -1000., -1000.;
    XU << 10000., 100., 1.5, 100., 100., 1000.;

    // and minimize 0.5*x'*Q*x + p'*x with
    C << 1., 2., 3., 4., 5., 6.;
    Q.setIdentity();

    X << 1.7975426, -0.3381487, 0.1633880, -4.9884023, 0.6054943, -3.1155623;
  }

  int nrvar, nreq, nrineq;
  Eigen::MatrixXd Q, Aeq, Aineq, A;
  Eigen::VectorXd C, Beq, Bineq, B, XL, XU, X;
};

void ineqWithXBounds(Eigen::MatrixXd & Aineq,
                     Eigen::VectorXd & Bineq,
                     const Eigen::VectorXd & XL,
                     const Eigen::VectorXd & XU)
{
  double inf = std::numeric_limits<double>::infinity();

  std::vector<std::pair<int, double>> lbounds, ubounds;

  for(int i = 0; i < XL.rows(); ++i)
  {
    if(XL[i] != -inf) lbounds.emplace_back(i, XL[i]);
    if(XU[i] != inf) ubounds.emplace_back(i, XU[i]);
  }

  long int nrconstr = Bineq.rows() + static_cast<long int>(lbounds.size()) + static_cast<long int>(ubounds.size());

  Eigen::MatrixXd A(Eigen::MatrixXd::Zero(nrconstr, Aineq.cols()));
  Eigen::VectorXd B(Eigen::VectorXd::Zero(nrconstr));

  A.block(0, 0, Aineq.rows(), Aineq.cols()) = Aineq;
  B.segment(0, Bineq.rows()) = Bineq;

  int start = static_cast<int>(Aineq.rows());

  for(int i = 0; i < static_cast<int>(lbounds.size()); ++i)
  {
    const auto & b = lbounds[i];
    A(start, b.first) = -1.;
    B(start) = -b.second;
    ++start;
  }

  for(int i = 0; i < static_cast<int>(ubounds.size()); ++i)
  {
    const auto & b = ubounds[i];
    A(start, b.first) = 1.;
    B(start) = b.second;
    ++start;
  }

  Aineq = A;
  Bineq = B;
}

BOOST_AUTO_TEST_CASE(QLD)
{
  QP1 qp1;

  Eigen::QLD qld(qp1.nrvar, qp1.nreq, qp1.nrineq);

  qld.solve(qp1.Q, qp1.C, qp1.Aeq, qp1.Beq, qp1.Aineq, qp1.Bineq, qp1.XL, qp1.XU);
  BOOST_CHECK_SMALL((qld.result() - qp1.X).norm(), 1e-6);

  Eigen::QLDDirect qldd(qp1.nrvar, qp1.nreq, qp1.nrineq);

  qldd.solve(qp1.Q, qp1.C, qp1.A, qp1.B, qp1.XL, qp1.XU, 3);
  BOOST_CHECK_SMALL((qld.result() - qp1.X).norm(), 1e-6);
}

BOOST_AUTO_TEST_CASE(QLDSize)
{
  QP1 qp1;

  Eigen::QLD qld(qp1.nrvar, qp1.nreq + 10, qp1.nrineq + 22);

  qld.solve(qp1.Q, qp1.C, qp1.Aeq, qp1.Beq, qp1.Aineq, qp1.Bineq, qp1.XL, qp1.XU);

  BOOST_CHECK_SMALL((qld.result() - qp1.X).norm(), 1e-6);
}

BOOST_AUTO_TEST_CASE(IneqWithXBounds)
{
  QP1 qp1;

  ineqWithXBounds(qp1.Aineq, qp1.Bineq, qp1.XL, qp1.XU);

  double inf = std::numeric_limits<double>::infinity();
  for(int i = 0; i < qp1.nrvar; ++i)
  {
    qp1.XL[i] = -inf;
    qp1.XU[i] = inf;
  }

  int nrineq = static_cast<int>(qp1.Aineq.rows());
  Eigen::QLD qld(qp1.nrvar, qp1.nreq, nrineq);

  qld.solve(qp1.Q, qp1.C, qp1.Aeq, qp1.Beq, qp1.Aineq, qp1.Bineq, qp1.XL, qp1.XU);

  BOOST_CHECK_SMALL((qld.result() - qp1.X).norm(), 1e-6);
}


// Copyright (c) 2018 James Pritts
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

// Modified for HomLib to accept Eigen matrices by Marcus Valtonen Örnhag

#include <Eigen/Dense>
#include "gj.hpp"

namespace HomLib {
    void gj(Eigen::MatrixXd *M) {
        int rcnt = M->rows();
        int ccnt = M->cols();

        M->transposeInPlace();
        double* A = M->data();

        double tol = 1e-15;
        int r = 0;      // row
        int c = 0;      // col
        int k;
        int l;
        int dstofs;
        int srcofs;
        int ofs = 0;
        int pofs = 0;
        double b;

        // gj
        ofs = 0;
        pofs = 0;
        while (r < rcnt && c < ccnt) {
            // find pivot
            double apivot = 0;
            double pivot = 0;
            int pivot_r = -1;

            pofs = ofs;
            for (k = r; k < rcnt; k++) {
                // pivot selection criteria here !
                if (fabs(*(A+pofs)) > apivot) {
                    pivot = *(A+pofs);
                    apivot = fabs(pivot);
                    pivot_r = k;
                }
                pofs += ccnt;
            }

            if (apivot < tol) {
                // empty col - shift to next col (or jump)
                c++;
                ofs++;

            } else {
                // process rows

                // exchange pivot and selected rows
                // + divide row
                if (pivot_r == r) {
                    srcofs = ofs;
                    for (l = c; l < ccnt; l++) {
                        *(A+srcofs) = *(A+srcofs)/pivot;
                        srcofs++;
                    }

                } else {
                    srcofs = ofs;
                    dstofs = ccnt*pivot_r+c;
                    for (l = c; l < ccnt; l++) {
                        b = *(A+srcofs);
                        *(A+srcofs) = *(A+dstofs)/pivot;
                        *(A+dstofs) = b;

                        srcofs++;
                        dstofs++;
                    }
                }

                // zero bottom
                pofs = ofs + ccnt;
                for (k = r + 1; k < rcnt; k++) {
                        // nonzero row
                        b = *(A+pofs);
                        dstofs = pofs + 1;
                        srcofs = ofs + 1;
                        for (l = c + 1; l < ccnt; l++) {
                            *(A+dstofs) = (*(A+dstofs) - *(A+srcofs) * b);
                            dstofs++;
                            srcofs++;
                        }
                        *(A+pofs) = 0;

                    pofs += ccnt;
                }

                // zero top
                pofs = c;
                for (k = 0; k < r; k++) {
                        // nonzero row
                        b = *(A+pofs);
                        dstofs = pofs + 1;
                        srcofs = ofs + 1;
                        for (l = c + 1; l < ccnt; l++) {
                            *(A+dstofs) = (*(A+dstofs) - *(A+srcofs) * b);
                            dstofs++;
                            srcofs++;
                        }
                        *(A+pofs) = 0;

                    pofs += ccnt;
                }

                r++;
                c++;
                ofs += ccnt + 1;
            }
        }

        (*M) << Eigen::Map<Eigen::MatrixXd>(A, ccnt, rcnt);
        M->transposeInPlace();
    }
}  // namespace HomLib


// 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 (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>
#include <boost/timer.hpp>

template <typename Matrix>
void setup(Matrix& A)
{
    const int n= num_rows(A);
    A= 1.0;
    mtl::mat::inserter<Matrix, mtl::update_plus<double> > ins(A);

    for (int i= 0; i < 30 * n; i++) {
	int r= rand()%n, c= rand()%n;
	ins[r][c] << -1;
	ins[r][r] << 1;
    }
}

int main(int argc, char* argv[])
{
    // For a more realistic example set sz to 1000 or larger
    int size = 3;
    if (argc > 1) size = atoi(argv[1]);
    int N = size * size;

    typedef mtl::compressed2D<double>  matrix_type;
    typedef mtl::dense_vector<double>  vector_type;
    mtl::compressed2D<double>          A(N, N);
    setup(A);

    boost::timer                       fac_timer;
    itl::pc::ilu_0<matrix_type>        P(A);
    std::cout << "Factorization took " << fac_timer.elapsed() << "s\n";

    mtl::dense_vector<double> x(N, 3.0), x2(N), Px(N), x3(N), x4(N), x5(N);

    matrix_type L(P.get_L()), U(P.get_U());

    x2= strict_upper(U) * x;
    for (int i= 0; i < N; i++)
	x2[i]+= 1. / U[i][i] * x[i];

    Px= L * x2 + x2;

    x4= unit_lower_trisolve(L, Px);
    if (two_norm(vector_type(x4 - x2)) > 0.01) throw "Error in unit_lower_trisolve.";

    x5= inverse_upper_trisolve(U, x4);
    if (two_norm(vector_type(x5 - x)) > 0.01) throw "Error in inverse_upper_trisolve.";

    boost::timer                       solve_timer;
    x3= solve(P, Px);
    std::cout << "Solving took " << solve_timer.elapsed() << "s\n";

    if (two_norm(vector_type(x3 - x)) > 0.01) throw "Error in solve.";

    return 0;
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of
// the License, or (at your option) any later version.
//
// Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

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

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

  typedef typename MatrixType::Scalar Scalar;
  typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime> SquareMatrixType;
  typedef Matrix<Scalar, MatrixType::ColsAtCompileTime, 1> VectorType;

  MatrixType a = MatrixType::Random(rows,cols);
  QR<MatrixType> qrOfA(a);
  VERIFY_IS_APPROX(a, qrOfA.matrixQ() * qrOfA.matrixR());
  VERIFY_IS_NOT_APPROX(a+MatrixType::Identity(rows, cols), qrOfA.matrixQ() * qrOfA.matrixR());

  #if 0 // eigenvalues module not yet ready
  SquareMatrixType b = a.adjoint() * a;

  // check tridiagonalization
  Tridiagonalization<SquareMatrixType> tridiag(b);
  VERIFY_IS_APPROX(b, tridiag.matrixQ() * tridiag.matrixT() * tridiag.matrixQ().adjoint());

  // check hessenberg decomposition
  HessenbergDecomposition<SquareMatrixType> hess(b);
  VERIFY_IS_APPROX(b, hess.matrixQ() * hess.matrixH() * hess.matrixQ().adjoint());
  VERIFY_IS_APPROX(tridiag.matrixT(), hess.matrixH());
  b = SquareMatrixType::Random(cols,cols);
  hess.compute(b);
  VERIFY_IS_APPROX(b, hess.matrixQ() * hess.matrixH() * hess.matrixQ().adjoint());
  #endif
}

void test_eigen2_qr()
{
  for(int i = 0; i < 1; i++) {
    CALL_SUBTEST_1( qr(Matrix2f()) );
    CALL_SUBTEST_2( qr(Matrix4d()) );
    CALL_SUBTEST_3( qr(MatrixXf(12,8)) );
    CALL_SUBTEST_4( qr(MatrixXcd(5,5)) );
    CALL_SUBTEST_4( qr(MatrixXcd(7,3)) );
  }

#ifdef EIGEN_TEST_PART_5
  // small isFullRank test
  {
    Matrix3d mat;
    mat << 1, 45, 1, 2, 2, 2, 1, 2, 3;
    VERIFY(mat.qr().isFullRank());
    mat << 1, 1, 1, 2, 2, 2, 1, 2, 3;
    //always returns true in eigen2support
    //VERIFY(!mat.qr().isFullRank());
  }

#endif
}


#pragma once
#include <cstdint>
#include <cassert>
#include <algorithm>
#include <map>
#include <boost/dynamic_bitset.hpp>
//#include "BitOperations.h"
#include "../Basis/AbstractBasis2D.hpp"

template<typename UINT>
class TIJ1J2_2D
{
private:
	const edlib::AbstractBasis2D<UINT>& basis_;
	double J1_;
	double J2_;
	int sign_;

public:
	TIJ1J2_2D(const edlib::AbstractBasis2D<UINT>& basis, double J1, double J2,
			bool signRule = false)
		: basis_(basis), J1_(J1), J2_(J2)
	{
		if(signRule)
			sign_ = -1;
		else
			sign_ = 1;
	}

	std::map<int,double> getCol(UINT n) const
	{
		uint32_t Lx = basis_.getLx();
		uint32_t Ly = basis_.getLy();

		UINT a = basis_.getNthRep(n);
		const boost::dynamic_bitset<> bs(Lx*Ly, a);

		std::map<int, double> m;
		for(uint32_t nx = 0; nx < Lx; nx++)
		{
			for(uint32_t ny = 0; ny < Ly; ny++)
			{
				auto i = basis_.toIdx(nx, ny);
				//Nearest neighbor x
				{
				auto j = basis_.toIdx((nx+1)%Lx, ny);
				int zz = (1-2*bs[i])*(1-2*bs[j]);

				m[n] += J1_*zz;

				UINT s = a;
				s ^= basis_.mask({i,j});

				int bidx;
				double coeff;

				std::tie(bidx, coeff) = basis_.hamiltonianCoeff(s, n);

				if(bidx >= 0)
					m[bidx] += J1_*(1-zz)*coeff*sign_;
				}
				//Nearest neighbor y
				{
				auto j = basis_.toIdx(nx, (ny+1)%Ly);
				int zz = (1-2*bs[i])*(1-2*bs[j]);

				m[n] += J1_*zz;

				UINT s = a;
				s ^= basis_.mask({i,j});

				int bidx;
				double coeff;

				std::tie(bidx, coeff) = basis_.hamiltonianCoeff(s, n);

				if(bidx >= 0)
					m[bidx] += J1_*(1-zz)*coeff*sign_;
				}
				//Next-nearest neighbor right up
				{
				auto j = basis_.toIdx((nx+1)%Lx, (ny+1)%Ly);
				int zz = (1-2*bs[i])*(1-2*bs[j]);

				m[n] += J2_*zz;

				UINT s = a;
				s ^= basis_.mask({i,j});

				int bidx;
				double coeff;

				std::tie(bidx, coeff) = basis_.hamiltonianCoeff(s, n);

				if(bidx >= 0)
					m[bidx] += J2_*(1-zz)*coeff;
				}
				//Next-nearest neighbor right down
				{
				auto j = basis_.toIdx((nx+1)%Lx, (ny-1+Ly)%Ly);
				int zz = (1-2*bs[i])*(1-2*bs[j]);

				m[n] += J2_*zz;

				UINT s = a;
				s ^= basis_.mask({i,j});

				int bidx;
				double coeff;

				std::tie(bidx, coeff) = basis_.hamiltonianCoeff(s, n);

				if(bidx >= 0)
					m[bidx] += J2_*(1-zz)*coeff;
				}
			}
		}
		return m;
	}
};