// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2013 Alec Jacobson <alecjacobson@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "bone_heat.h"
#include "EmbreeIntersector.h"
#include "bone_visible.h"
#include "../project_to_line_segment.h"
#include "../cotmatrix.h"
#include "../massmatrix.h"
#include "../mat_min.h"
#include <Eigen/Sparse>

bool igl::embree::bone_heat(
  const Eigen::MatrixXd & V,
  const Eigen::MatrixXi & F,
  const Eigen::MatrixXd & C,
  const Eigen::VectorXi & P,
  const Eigen::MatrixXi & BE,
  const Eigen::MatrixXi & CE,
  Eigen::MatrixXd & W)
{
  using namespace std;
  using namespace Eigen;
  assert(CE.rows() == 0 && "Cage edges not supported.");
  assert(C.cols() == V.cols() && "V and C should have same #cols");
  assert(BE.cols() == 2 && "BE should have #cols=2");
  assert(F.cols() == 3 && "F should contain triangles.");
  assert(V.cols() == 3 && "V should contain 3D positions.");

  const int n = V.rows();
  const int np = P.rows();
  const int nb = BE.rows();
  const int m = np + nb;

  // "double sided lighting"
  MatrixXi FF;
  FF.resize(F.rows()*2,F.cols());
  FF << F, F.rowwise().reverse();
  // Initialize intersector
  EmbreeIntersector ei;
  ei.init(V.cast<float>(),F.cast<int>());

  typedef Matrix<bool,Dynamic,1> VectorXb;
  typedef Matrix<bool,Dynamic,Dynamic> MatrixXb;
  MatrixXb vis_mask(n,m);
  // Distances
  MatrixXd D(n,m);
  // loop over points
  for(int j = 0;j<np;j++)
  {
    const Vector3d p = C.row(P(j));
    D.col(j) = (V.rowwise()-p.transpose()).rowwise().norm();
    VectorXb vj;
    bone_visible(V,F,ei,p,p,vj);
    vis_mask.col(j) = vj;
  }

  // loop over bones
  for(int j = 0;j<nb;j++)
  {
    const Vector3d s = C.row(BE(j,0));
    const Vector3d d = C.row(BE(j,1));
    VectorXd t,sqrD;
    project_to_line_segment(V,s,d,t,sqrD);
    D.col(np+j) = sqrD.array().sqrt();
    VectorXb vj;
    bone_visible(V,F,ei,s,d,vj);
    vis_mask.col(np+j) = vj;
  }

  if(CE.rows() > 0)
  {
    cerr<<"Error: Cage edges are not supported. Ignored."<<endl;
  }

  MatrixXd PP = MatrixXd::Zero(n,m);
  VectorXd min_D;
  VectorXd Hdiag = VectorXd::Zero(n);
  VectorXi J;
  mat_min(D,2,min_D,J);
  for(int i = 0;i<n;i++)
  {
    PP(i,J(i)) = 1;
    if(vis_mask(i,J(i)))
    {
      double hii = pow(min_D(i),-2.);
      Hdiag(i) = (hii>1e10?1e10:hii);
    }
  }
  SparseMatrix<double> Q,L,M;
  cotmatrix(V,F,L);
  massmatrix(V,F,MASSMATRIX_TYPE_DEFAULT,M);
  const auto & H = Hdiag.asDiagonal();
  Q = (-L+M*H);
  SimplicialLLT <SparseMatrix<double > > llt;
  llt.compute(Q);
  switch(llt.info())
  {
    case Eigen::Success:
      break;
    case Eigen::NumericalIssue:
      cerr<<"Error: Numerical issue."<<endl;
      return false;
    default:
      cerr<<"Error: Other."<<endl;
      return false;
  }

  const auto & rhs = M*H*PP;
  W = llt.solve(rhs);
  return true;
}


#include "average_case_error.hpp"

#include <boost/multiprecision/cpp_int.hpp>
#include <cudd_helpers.hpp>

using abo::util::NumberRepresentation;
using boost::multiprecision::cpp_dec_float_100;
using boost::multiprecision::uint256_t;

namespace abo::error_metrics {

cpp_dec_float_100 average_value(const std::vector<BDD>& f)
{

    uint256_t sum = 0;
    const uint256_t one = 1;

    int max_support_size = 0;
    for (const auto& bdd : f)
    {
        max_support_size = std::max(max_support_size, bdd.SupportSize());
    }

    for (unsigned int i = 0; i < f.size(); i++)
    {
        // TODO: CountMinterms returns a double, might loose precision
        double minterms = f[i].CountMinterm(max_support_size);
        sum += (one << i) * uint256_t(minterms);
    }

    return cpp_dec_float_100(sum) /
           cpp_dec_float_100(one << max_support_size);
}

cpp_dec_float_100 mean_squared_value(const std::vector<BDD>& f)
{

    uint256_t sum = 0;
    const uint256_t one = 1;

    int max_support_size = 0;
    for (const auto& bdd : f)
    {
        max_support_size = std::max(max_support_size, bdd.SupportSize());
    }

    for (unsigned int i = 0; i < f.size(); i++)
    {
        for (unsigned int b = i; b < f.size(); b++)
        {
            // cudd handles the case i = b efficiently
            BDD both = f[i] & f[b];
            double minterms = both.CountMinterm(max_support_size);
            // most terms (except for i == b), appear twice when factoring the square
            uint256_t doubling_factor = i == b ? 1 : 2;
            sum += (one << i) * (one << b) * uint256_t(minterms) *
                   doubling_factor;
        }
    }

    return cpp_dec_float_100(sum) /
           cpp_dec_float_100(one << max_support_size);
}

cpp_dec_float_100
average_case_error(const Cudd& mgr, const std::vector<BDD>& f,
                   const std::vector<BDD>& f_hat,
                   const util::NumberRepresentation num_rep)
{

    std::vector<BDD> absolute_difference =
        abo::util::bdd_absolute_difference(mgr, f, f_hat, num_rep);
    return average_value(absolute_difference);
}

cpp_dec_float_100
mean_squared_error(const Cudd& mgr, const std::vector<BDD>& f,
                   const std::vector<BDD>& f_hat,
                   const util::NumberRepresentation num_rep)
{

    std::vector<BDD> absolute_difference =
        abo::util::bdd_absolute_difference(mgr, f, f_hat, num_rep);
    return mean_squared_value(absolute_difference);
}

cpp_dec_float_100 average_case_error_add(const Cudd& mgr,
                                        const std::vector<BDD>& f,
                                        const std::vector<BDD>& f_hat,
                                        const NumberRepresentation num_rep)
{

    ADD diff = abo::util::absolute_difference_add(mgr, f, f_hat, num_rep);
    std::vector<std::pair<double, unsigned long>> terminal_values =
        abo::util::add_terminal_values(diff);

    uint256_t sum = 0;
    uint256_t path_sum = 0;
    for (auto p : terminal_values)
    {
        sum += uint256_t(p.first) * p.second;
        path_sum += p.second;
    }
    return cpp_dec_float_100(sum) /
           cpp_dec_float_100(path_sum);
}

cpp_dec_float_100 mean_squared_error_add(const Cudd& mgr,
                                        const std::vector<BDD>& f,
                                        const std::vector<BDD>& f_hat,
                                        const NumberRepresentation num_rep)
{

    ADD diff = abo::util::absolute_difference_add(mgr, f, f_hat, num_rep);
    std::vector<std::pair<double, unsigned long>> terminal_values =
        abo::util::add_terminal_values(diff);

    uint256_t sum = 0;
    uint256_t path_sum = 0;
    for (auto p : terminal_values)
    {
        uint256_t value(p.first);
        sum += value * value * p.second;
        path_sum += p.second;
    }
    return cpp_dec_float_100(sum) /
           cpp_dec_float_100(path_sum);
}

} // namespace abo::error_metrics


#define BOOST_TEST_MODULE graphing_test

#include <boost/test/included/unit_test.hpp>

#include "underground_modelling/graphing.h"

BOOST_AUTO_TEST_SUITE( graphing_tests )

BOOST_AUTO_TEST_CASE( graph ){

  using namespace gaus::underground_modelling;

  const arma::umat adjacency =
      {{0, 1, 1},
       {1, 0, 0},
       {1, 0, 0}};

  const Graph graph = make_graph(adjacency);

  BOOST_TEST(boost::num_edges(graph) == 4);
  BOOST_TEST(boost::num_vertices(graph) == 3);

}

BOOST_AUTO_TEST_CASE( cost ){

  using namespace gaus::underground_modelling;

  const arma::umat adjacency =
      {{0, 1},
       {1, 0}
      };

  const StationCoordinates s_coords =
      {{0, 1},
       {0, 0}
      };

  const auto cost = calculate_cost(adjacency, s_coords, {0.5, 0.2});

  BOOST_TEST(cost == 0.7);
}

BOOST_AUTO_TEST_CASE( terminal_stations ){

  using namespace gaus::underground_modelling;

  const arma::umat adjacency =
      {{0, 0},
       {0, 0}
      };

  const auto f_terminal = find_terminal_stations(adjacency);

  std::cout << "f_terminal " << f_terminal << std::endl;

  BOOST_TEST(f_terminal == 2);
}

BOOST_AUTO_TEST_CASE( connectivity ){
  using namespace gaus::underground_modelling;

  const arma::umat adjacency =
      {{0, 1, 0, 0},
       {1, 0, 0, 0},
       {0, 0, 0, 1},
       {0, 0, 1, 0},
      };

  const double connectivity = check_connectivity(adjacency);

  std::cout << "connectivity " << connectivity << std::endl;

  BOOST_TEST(connectivity == 2);
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright (c) 2007-2010 Tao Wang <dancefire@gmail.org>
 * See the file "LICENSE.txt" for usage and redistribution license requirements
 *
 *	$Id$
 */
#pragma once
#ifndef _OPENCLAS_K_SHORTEST_PATH_HPP_
#define _OPENCLAS_K_SHORTEST_PATH_HPP_

//	workaround for remove VC min(a,b) macro function definition,
//	which is make std::min() not working.
#if defined(_MSC_VER) && defined(min)
#undef min
#endif	//	min

#include "common.hpp"
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/dag_shortest_paths.hpp>
#include <list>
#include <vector>
#include <algorithm>

namespace openclas {

	using namespace boost;

	struct path_type {
		double weight;
		std::vector<size_t> nodelist;
		path_type()
			: weight(0), nodelist()
		{}
	};

	inline bool operator < (const path_type& left, const path_type& right)
	{
		return left.weight < right.weight;
	}

	//	Find all paths of given pair of node in a DAG. (DFS-like algorithm)
	template <class IncidenceGraph>
	void dag_all_paths(IncidenceGraph& g,
		typename graph_traits<IncidenceGraph>::vertex_descriptor begin,
		typename graph_traits<IncidenceGraph>::vertex_descriptor end,
		std::vector<path_type>& result_paths,
		path_type current_path = path_type())
	{
		function_requires<IncidenceGraphConcept<IncidenceGraph> >();

		if (num_vertices(g) == 0)
			return;	//	return if g is empty

		current_path.nodelist.push_back(begin);
		if (begin == end) {
			result_paths.push_back(current_path);
		} else {
			typename graph_traits<IncidenceGraph>::out_edge_iterator ei, ei_end;
			typename property_map<IncidenceGraph, edge_weight_t>::type
				w_map = get(edge_weight, g);
			double original_weight = current_path.weight;
			for (tie(ei, ei_end) = out_edges(begin, g); ei != ei_end; ++ei) {
				typename graph_traits<IncidenceGraph>::vertex_descriptor v = target(*ei, g);
				current_path.weight = original_weight + w_map[edge(begin, v, g).first];
				dag_all_paths(g, v, end, result_paths, current_path);
			}
		}
	}

	//	Find k-shortest-path in DAG.
	template <class IncidenceGraph>
	void dag_k_shortest_paths(IncidenceGraph& g,
		typename graph_traits<IncidenceGraph>::vertex_descriptor begin,
		typename graph_traits<IncidenceGraph>::vertex_descriptor end,
		std::vector<path_type>& result_paths,
		int k)
	{
		if (num_vertices(g) == 0)
			return;	//	return if g is empty

		if (k == 1) {
			path_type shortest_path;
			dag_shortest_path(g, begin, end, shortest_path);
			result_paths.push_back(shortest_path);
		}else{
			std::vector<path_type> candidate_paths;
			dag_all_paths(g, begin, end, candidate_paths);

			k = std::min(k, static_cast<int>(candidate_paths.size()));
			std::partial_sort(candidate_paths.begin(), candidate_paths.begin() + k, candidate_paths.end());
			result_paths.assign(candidate_paths.begin(), candidate_paths.begin() + k);
		}
	}

	//	Find shortest-path in DAG. O(n+m)
	template <class Graph>
	void dag_shortest_path(Graph& g,
		typename graph_traits<Graph>::vertex_descriptor begin,
		typename graph_traits<Graph>::vertex_descriptor end,
		path_type& result_path)
	{
		if (num_vertices(g) == 0)
			return;	//	return if g is empty

		typename property_map<Graph, vertex_distance_t>::type
			d_map = get(vertex_distance, g);
		typename property_map<Graph, vertex_predecessor_t>::type
			p_map = get(vertex_predecessor, g);

		if (begin == end) {
			result_path.nodelist.push_back(begin);
		} else {
			dag_shortest_paths(g, begin, distance_map(d_map).predecessor_map(p_map));

			if (end == p_map[end])	//	[begin] is not connected with [end]
				return;

			result_path.weight = d_map[end];
			result_path.nodelist.push_back(end);

			size_t current = end;
			while(current != begin)
			{
				current = p_map[current];
				result_path.nodelist.push_back(current);
			}
			std::reverse(result_path.nodelist.begin(), result_path.nodelist.end());
		}
	}
}

//	_OPENCLAS_K_SHORTEST_PATH_HPP_
#endif


/*
   Copyright (C) 2015-2021 by Synge Todo <wistaria@phys.s.u-tokyo.ac.jp>

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

     http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/

#include <cmath>
#include <gtest/gtest.h>
#include <boost/math/differentiation/autodiff.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include "ising/mp_wrapper.hpp"
#include "ising/free_energy/common.hpp"
#include "ising/free_energy/square.hpp"

using namespace boost::multiprecision;
using namespace ising::free_energy;

TEST(IsingFreeEnergy, SquareCount0) {
  typedef double real_t;
  unsigned Lx = 4;
  unsigned Ly = 4;
  auto Jx = convert<real_t>("1.5");
  auto Jy = convert<real_t>("2.5");
  auto t = convert<real_t>("2");
  auto vars = boost::math::differentiation::make_ftuple<real_t, 2, 2>(1 / t, 0);
  auto& beta = std::get<0>(vars);
  auto& h = std::get<1>(vars);
  auto f = square::finite_count(Lx, Ly, Jx, Jy, beta, h);
  EXPECT_DOUBLE_EQ(-4.087359662653047e+00, free_energy(f, beta, h));
  EXPECT_DOUBLE_EQ(-3.994108759068211e+00, energy(f, beta, h));
  EXPECT_DOUBLE_EQ(2.452622208849045e-02, specific_heat(f, beta, h));
  EXPECT_DOUBLE_EQ(1.597700713244840e+01, magnetization2(f, beta, h));
}


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 * All rights reserved.
 *
 * See LICENSE file in the root of the mrob library.
 *
 *
 * planeRegistration.hpp
 *
 *  Created on: Jan 28, 2019
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#ifndef PLANEREGISTRATION_HPP_
#define PLANEREGISTRATION_HPP_

#include <vector>
#include "mrob/SE3.hpp"
#include <Eigen/StdVector>
#include "mrob/plane.hpp"
#include "mrob/optimizer.hpp"

#include <unordered_map>
#include <memory>

namespace mrob{

/**
 * class PlaneRegistration introduced a class for the alignment of
 * planes.
 */
class PlaneRegistration: public Optimizer{

  public:
    // XXX is this mode used anymore? deprecated?
    enum TrajectoryMode{SEQUENCE=0, INTERPOLATION};
    // XXX Solve method is almost deprecated
    //enum SolveModeGrad{GRADIENT_DESCENT_NAIVE=0, GRADIENT_DESCENT_INCR, STEEPEST, HEAVYBALL, MOMENTUM, MOMENTUM_SEQ, BENGIOS_NAG, GRADIENT_DESCENT_BACKTRACKING, BFGS};
    enum SolveMode{INITIALIZE=0,
                   GRADIENT,
                   GRADIENT_BENGIOS_NAG,
                   GN_HESSIAN,
                   GN_CLAMPED_HESSIAN,
                   LM_SPHER,
                   LM_ELLIP};

  public:
    PlaneRegistration();
    //PlaneRegistration(uint_t numberPlanes , uint_t numberPoses);
    ~PlaneRegistration();

    // Function from the parent class Optimizer
    virtual matData_t calculate_error() override;
    virtual void calculate_gradient_hessian() override;
    virtual void update_state(const MatX1 &dx) override;
    virtual void bookkeep_state() override;
    virtual void update_state_from_bookkeep() override;

    // Specific methods
    void set_number_planes_and_poses(uint_t numPlanes, uint_t numPoses);
    uint_t get_number_planes() const {return numberPlanes_;};
    uint_t get_number_poses() const {return numberPoses_;};

    /**
     * solve() calculates the poses on trajectory such that the minimization objective
     * is met: J = sum (lamda_min_plane)
     */
    uint_t solve(SolveMode mode, bool singleIteration = false);
    /**
     * solve_interpolate() calculates the poses on trajectory such that the minimization objective
     * is met: J = sum (lamda_min_plane), and the trajectory is described as an interpolation from I to T_f
     */
    uint_t solve_interpolate_gradient(bool singleIteration = false);
    /**
     * solve_interpolate_hessian() calculates the poses on trajectory such that the minimization objective
     * is met: J = sum (lamda_min_plane), and the trajectory is described as an interpolation from I to T_f
     * using second order methods with Hessian. Very similar to solve_interpolate
     */
    uint_t solve_interpolate_hessian(bool singleIteration = false);
    /**
     * Initialization_solve give a first guess on all poses by using classical point-point
     * methods SVD-based to calculate an initial condition closer to the true solution
     */
    uint_t solve_initialize();
    /**
     * Solve quaternion plane uses a paramteric representation for each plane, a quaternion,
     * and optimizes both the plane parameters and the trajectory variables
     */
    uint_t solve_quaternion_plane();
    /**
     * reset_solution, resets the current calculated solution while maintainting all data (planes)
     * This function is intended for comparing different solvers without replicating data
     */
    void reset_solution();
    double get_current_error() const;
    /**
     * Get trajectory returns a smart pointer to the vector of transformations,
     * which is already shared by all Plane objects.
     * It serves for checking the solution and for modifying the initial conditions for optimization (if any).
     */
    //std::shared_ptr<std::vector<SE3>>& get_trajectory() {return trajectory_;};//if solved
    Mat4 get_trajectory(uint_t time);

    /**
     * add_plane adds a plane structure already initialized and filled with data
     */
    void add_plane(uint_t id, std::shared_ptr<Plane> &plane);
    std::shared_ptr<Plane> & get_plane(uint_t id);

    std::unordered_map<uint_t, std::shared_ptr<Plane>>& get_all_planes() {return planes_;};

    void set_alpha_parameter(double alpha) {alpha_ = alpha;};
    void set_beta_parameter(double beta) {beta_ = beta;};

    double calculate_poses_rmse(std::vector<SE3> & groundTruth) const;

    void print(bool plotPlanes = true) const;

    /**
     * print evaluate looks for degenerate cases, such as planes normal vectors,
     * Hessian rank, det of all normals, etc. Basically this function tries to answer
     * if the problem is ill-conditioned
     *
     * Returns: 0) current error
     *          1) number of iters,
     *          2) determinant
     *          3) number of negative eigenvalues
     *          4) conditioning number
     */
    std::vector<double> print_evaluate();

    /**
     * add point_cloud requires a complete set of points observed at a given time
     * stamp (XXX now only an integer) and fills in the registration structure.
     */
    void add_point_cloud_planes(uint_t time, std::vector<Mat31>& points, std::vector<uint_t>& point_ids);
    /**
     * get_point_cloud gets all raw point, according to the current time index
     * from trajectory. It does not distinguish between planes.
     */
    std::vector<Mat31> get_point_cloud(uint_t time);
    std::vector<Mat31> get_point_plane_ids(uint_t time);

  protected:
    // flag for detecting when is has been solved
    uint_t numberPlanes_, numberPoses_;
    uint_t isSolved_;
    PlaneRegistration::TrajectoryMode trajMode_;
    uint_t time_;
    std::unordered_map<uint_t, std::shared_ptr<Plane>> planes_;
    std::shared_ptr<std::vector<SE3>> trajectory_;
    SE3 bookept_trajectory_;//last pose is stored/bookept
    double tau_;//variable for weighting the number of poses in traj
    uint_t solveIters_;

    // 1st order parameters methods if used
    PlaneRegistration::SolveMode solveMode_;
    std::vector<Mat61> previousState_;
    double c1_, c2_;    //parameters for the Wolfe conditions DEPRECATED?
    double alpha_, beta_;

    //2nd order data (if used) TODO remove since they are defined in parent class
    Mat61 gradient__;
    Mat6 hessian__;

};

}// namespace
#endif /* PLANEREGISTRATION_HPP_ */


#include <boost/math/special_functions/cbrt.hpp>


#include <matrixlib/MatrixUtils.h>
#include <NTL/vec_GF2.h>

void random(NTL::mat_GF2 &L, int n1, int n2)
{
	L.SetDims(n1,n2);
	int hw;
	for (int i = 0; i < n1; i++)
	{
		do
	    	{
	    		hw = 0;
				for (int j = 0; j < n2; j++)
				{
					L[i][j] = NTL::random_GF2();
					if (L[i][j] == 1)
			            hw += 1;
				}
			} while (hw == 1);
	}
}
int initMatrixFromBit(NTL::mat_GF2 &M, long *data) {
    long i,j,n,m;
	for(i=0, n=M.NumRows(); i<n; i++){
		long t = *data;
		for(j=M.NumCols()-1; j>=0; j--){
			M.put(i,j,t%2);
			t >>= 1;
		}
		data++;
	}

	return 0;
}

int initVecFromBit(NTL::vec_GF2 &M, long data, int len) {
	M.SetLength(len);
    long i,j,n,m;
	for(i=len-1; i>=0; i--){
		M.put(i, data%2);
		data >>=1;
	}
	return 0;
}
int genRandomAffineAugmentedMatrix(NTL::mat_GF2 &X,  NTL::mat_GF2 &Y, int n) {
	NTL::mat_GF2 L;
	random(L, n, n);
	while(determinant(L)==0) {
		random(L, n, n);
	}
	NTL::mat_GF2 LINV = inv(L);
	NTL::vec_GF2 tran;
	// tran.SetLength(m);
	random(tran, n);
	NTL::mat_GF2 AugmentedMatrix;

	X.SetDims(n+1, n+1);
	int i,j;
	for (i=0; i<n; i++) {
		for ( j=0; j<n; j++) {
			X.put(i,j, L.get(i, j));
		}
	}

	for (j=0; j<n; j++) {
		X.put(n, j, 0);
	}
	X.put(n, n, 1);

	for (i=0; i<n; i++) {
		X.put(i, n, tran.get(i));
	}

	NTL::GF2 d;

	inv(d, Y, X);

	return 0;

}

int genRandomAffineMatrix(NTL::mat_GF2 &X,  NTL::mat_GF2 &Y, NTL::vec_GF2 &V, int n) {
	random(X, n, n);
	while(determinant(X)==0) {
		random(X, n, n);
	}
	NTL::GF2 d;
	inv(d, Y, X);
	random(V, n);
	return 0;
}

int genRandomAffineMatrix(NTL::mat_GF2 &x,  NTL::mat_GF2 &inv_x, NTL::vec_GF2 &v, NTL::vec_GF2 &inv_v, int n) {
	random(x, n, n);
	while(determinant(x)==0) {
		random(x, n, n);
	}
	NTL::GF2 d;
	inv(d, inv_x, x);
	random(v, n);
	mul(inv_v, inv_x, v);
	return 0;
}

int genRandomInvMatrix(NTL::mat_GF2 &x,  NTL::mat_GF2 &inv_x, int n) {
	random(x, n, n);
	while(determinant(x)==0) {
		random(x, n, n);
	}
	NTL::GF2 d;
	inv(d, inv_x, x);
	return 0;
}

uint32_t getDigitalFromVec(NTL::vec_GF2 &s) {
	uint32_t d = 0;
	int i;
	int m = s.length();
	for (i=0; i<m; i++) {
		d <<=1 ;
		d += ((s.get(i)==1)?1:0);
	}
	return d;
}

uint32_t get32FromVec(NTL::vec_GF2 &s) {
	uint32_t d = 0;
	int i;
	for (i=0; i<32; i++) {
		d <<=1 ;
		d += ((s.get(i)==1)?1:0);
	}
	return d;
}

uint8_t get8FromVec(const vector_transform_t &s) {
	uint8_t d = 0;
	int i;
	for (i=0; i<8; i++) {
		d <<=1 ;
		d += ((s.get(i)==1)?1:0);
	}
	return d;
}

int getVecFrom32(NTL::vec_GF2 &d,uint32_t s) {
	d.SetLength(32);
	int i;
	for (i=31; i>=0; i--) {
		d.put(i, s%2);
		s>>=1;
	}
	return 0;
}

int getAugmentedVecFrom32(NTL::vec_GF2 &d, uint32_t s) {
	d.SetLength(33);
	int i;
	for (i=31; i>=0; i--) {
		d.put(i, s%2);
		s>>=1;
	}
	d.put(32, 1);
	return 0;
}

int combineDiagMat(NTL::mat_GF2 &d, NTL::mat_GF2 &s1, NTL::mat_GF2 &s2) {
	long n1 = s1.NumCols();
	long n2 = s2.NumCols();
	d.SetDims(n1+n2, n1+n2);
	int i,j;
	for (i=0; i<n1; i++) {
		for (j=0; j<n1; j++) {
			d.put(i, j, s1.get(i,j));
		}
		for (j=0; j<n2; j++) {
			d.put(i, n1+j, 0);
		}
	}
	for (i=0; i<n2; i++) {
		for (j=0; j<n1; j++) {
			d.put(n1+i, j, 0);
		}
		for (j=0; j<n2; j++) {
			d.put(n1+i, n1+j, s2.get(i,j));
		}
	}
	return 0;
}

uint32_t applyAffineToU32(const affine_transform_t &aff, uint32_t data ) {
	NTL::vec_GF2 a,b;
	initVecFromBit(a, data, 32);
	// dumpVector(a);
	// dumpMatrix(aff.linearMap);
	b = aff.linearMap * a + aff.vectorTranslation;
	return get32FromVec(b);
}

uint8_t applyAffineToU8(const affine_transform_t &aff, uint8_t data) {
	NTL::vec_GF2 a,b;
	initVecFromBit(a, data, 8);
	// dumpVector(a);
	// dumpMatrix(aff.linearMap);
	b = aff.linearMap * a + aff.vectorTranslation;
	return (uint8_t)getDigitalFromVec(b);
}

uint8_t applyMatToU8(const NTL::mat_GF2 &mat, uint8_t data) {
	NTL::vec_GF2 a,b;
	initVecFromBit(a, data, 8);
	b = mat*a;
	return (uint8_t)getDigitalFromVec(b);
}

uint32_t applyMatToU32(const matrix_transform_t &mat, uint32_t data) {
	vector_transform_t a,b;
	initVecFromBit(a, data, 32);
	b = mat*a;
	return (uint32_t)getDigitalFromVec(b);
}

uint32_t addVecToU32(NTL::vec_GF2 &vec, uint32_t data) {
	NTL::vec_GF2 a,b;
	initVecFromBit(a, data, 32);
	b = vec+a;
	return (uint32_t)getDigitalFromVec(b);
}

uint8_t addVecToU8(NTL::vec_GF2 &vec, uint8_t data) {
	NTL::vec_GF2 a,b;
	initVecFromBit(a, data, 8);
	b = vec+a;
	return (uint8_t)getDigitalFromVec(b);
}

int genIndMatrix(NTL::mat_GF2 &mat, int size) {
	mat.SetDims(size, size);
	int i,j;
	for (i=0; i<size; i++) {
		for (j=0; j<size; j++) {
			mat.put(i, j, i==j);
		}
	}
	return 0;
}

int genZeroVec(NTL::vec_GF2 &vec, int size) {
	vec.SetLength(size);
	int i;
	for (i=0; i<size; i++) {
		vec.put(i, 0);
	}
	return 0;
}


#include <boost/math/differentiation/autodiff.hpp>
#include <iostream>

template <typename T>
T fourth_power(T const& x){
	T x4 = x*x;
	x4 *= x4;
	return x4;
}

template <typename T>
T func(T const& x, T const& y){
	return x*y;
}

int main(int argc, char **argv){
	using namespace boost::math::differentiation;

	constexpr unsigned Order = 5;

	auto const x = make_fvar<double, Order>(2.0);
	//auto const y = make_fvar<double, Order>(3.0);
	auto const f = fourth_power(x);

	for(unsigned int i=0; i<=Order; ++i)
		std::cout<< "f.derivative("<< i << ") = " << f.derivative(i) << std::endl;

	return 0;
}


#include "simulator.h"
#include <Eigen/Eigenvalues>
#include <future>
#include <iomanip>
#include <iostream>
#include <limits>
#include <set>

using namespace Simulator;

std::string Simulator::AlgorithmToStr(const Algorithms algorithm)
{
	switch (algorithm) {
	case Algorithms::Metropolis:
		return { "Metropolis method" };
	case Algorithms::Glauber:
		return { "Glauber dynamics" };
	case Algorithms::SCA:
		return { "Stochastic cellular automata" };
	case Algorithms::fcSCA:
		return { "Flip-Constrained Stochastic Cellular Automata" };
	case Algorithms::MA:
		return { "Momentum annealing" };
	case Algorithms::MMA:
		return { "Modified momentum annealing" };
	case Algorithms::HillClimbing:
		return { "Hill climbing" };
	default:
		return { "Warning: Unknown type." };
	}
}

// quadraticのキーのペア (i, j) は順番が i < j となっていなければならない。
IsingModel::IsingModel(const LinearBiases linear, const QuadraticBiases quadratic)
	: rand(std::make_unique<Rand>())
	, temperature(0.e0)
	, pinningParameter(0.e0)
	, flipTrialRate(0.e0)
	, algorithm(Algorithms::Metropolis)
{
	// spinsの添字と頂点の名前との対応表を作成。
	std::set<Node> nodes;
	for (auto iter = linear.begin(); iter != linear.end(); iter++)
		nodes.insert(iter->first);
	for (auto iter = quadratic.begin(); iter != quadratic.end(); iter++) {
		nodes.insert(iter->first.first);
		nodes.insert(iter->first.second);
	}
	std::size_t index = 0;
	for (const auto& key : nodes)
		nodeIndices[key] = index++;

	// Hamiltonianの定数と変数を初期化。
	auto maxNodes = nodeIndices.size();
	spins.setConstant(maxNodes, Spin::Up);
	previousSpins = spins;
	externalMagneticField.resize(maxNodes);
	for (const auto& node : nodeIndices) {
		auto iter = linear.find(node.first);
		if (iter != linear.end())
			externalMagneticField(node.second) = iter->second;
		else
			externalMagneticField(node.second) = 0.e0;
	}
	couplingCoefficients = Eigen::MatrixXd::Zero(maxNodes, maxNodes);
	for (const auto& row : nodeIndices) {
		for (const auto& column : nodeIndices) {
			if (row.first > column.first)
				continue;
			auto iter = quadratic.find(std::make_pair(row.first, column.first));
			if (iter != quadratic.end())
				couplingCoefficients(row.second, column.second) = couplingCoefficients(column.second, row.second) = iter->second;
			else
				couplingCoefficients(row.second, column.second) = couplingCoefficients(column.second, row.second) = 0.e0;
		}
	}
}

// 行列 (-J_{x, y})_{x, y} の最大固有値を計算する。
double IsingModel::CalcLargestEigenvalue() const
{
	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(-couplingCoefficients);
	return solver.eigenvalues().reverse()(0);
}

double IsingModel::GetEnergy() const
{
	// Remove double-counting duplicates by multiplying the sum by 1/2.
	return -spins.cast<double>().transpose() * (0.5e0 * couplingCoefficients * spins.cast<double>() + externalMagneticField);
}

double IsingModel::GetEnergyOnBipartiteGraph() const
{
	return -0.5e0 * spins.cast<double>().transpose() * couplingCoefficients * spins.cast<double>()
		- 0.5e0 * externalMagneticField.dot(spins.cast<double>() + previousSpins.cast<double>())
		+ 0.5e0 * pinningParameter * (spins.size() - spins.cast<double>().dot(previousSpins.cast<double>()));
}

void IsingModel::GiveSpins(const ConfigurationsType configurationType)
{
	switch (configurationType) {
	case ConfigurationsType::AllDown:
		spins.fill(Spin::Down);
		break;
	case ConfigurationsType::AllUp:
		spins.fill(Spin::Up);
		break;
	case ConfigurationsType::Uniform:
		for (auto i = 0; i < spins.size(); i++)
			spins(i) = rand->Bernoulli(0.5e0) ? Spin::Down : Spin::Up;
		break;
	default:
		break;
	}
	previousSpins = spins;
}

void IsingModel::Update()
{
	auto metropolisMethod = [this]() {
		unsigned int updatedNodeIndex = (*rand)(spins.size());
		double energyDifference = 2.e0 * static_cast<int>(spins(updatedNodeIndex)) * calcLocalMagneticField(updatedNodeIndex);
		if (energyDifference < 0.e0)
			spins(updatedNodeIndex) = flip(spins(updatedNodeIndex));
		else if (rand->Bernoulli(std::exp(-energyDifference / temperature)))
			spins(updatedNodeIndex) = flip(spins(updatedNodeIndex));
	};

	auto glauberDynamics = [this]() {
		unsigned int updatedNodeIndex = (*rand)(spins.size());
		if (rand->Bernoulli(1.e0 / (1.e0 + std::exp(-2.e0 * calcLocalMagneticField(updatedNodeIndex) / temperature))))
			spins(updatedNodeIndex) = Spin::Up;
		else
			spins(updatedNodeIndex) = Spin::Down;
	};

	auto stochasticCellularAutomata = [this]() {
		previousSpins = spins;
		spins = (
			calcLocalMagneticField(spins) + pinningParameter * spins.cast<double>()
			- temperature * Eigen::VectorXd::NullaryExpr(spins.size(), [this]() -> double { return rand->Logistic(); })
		).array().sign().cast<Spin>();  // 実質起こらないが、符号関数に渡しているため、スピンが0になる場合がある。
	};

	auto flipConstrainedStochasticCellularAutomata = [this]() {
		previousSpins = spins;
		//auto bernoulli =  Eigen::VectorXd::NullaryExpr(spins.size(), [this]() -> bool { return rand->Bernoulli(flipTrialRate) ; });  // = true w.p. flipTrialRate and = false w.p. 1 - flipTrialRate.
		spins = (
			calcLocalMagneticField(spins) + pinningParameter * spins.cast<double>()
			- temperature * Eigen::VectorXd::NullaryExpr(spins.size(), [this]() -> double { return rand->Logistic(); })
			+ Eigen::VectorXd::NullaryExpr(spins.size(), [this]() -> double {
				return rand->Bernoulli(flipTrialRate) ? 0.e0 : std::numeric_limits<double>::infinity();
			}).cwiseProduct(spins.cast<double>())
			//+ bernoulli.unaryExpr([](bool b) -> double { return b ? 0.e0 : std::numeric_limits<double>::infinity(); }).cwiseProduct(spins.cast<double>())
		).array().sign().cast<Spin>();  // 実質起こらないが、符号関数に渡しているため、スピンが0になる場合がある。
	};

	// 温度を下げなければ ``annealing'' ではないが、論文では区別していないので、ここでもこの名称を用いる。
	auto momentumAnnealing = [this]() {
		Configuration temp = (
			calcLocalMagneticField(spins) + pinningParameter * spins.cast<double>()
			- temperature * Eigen::VectorXd::NullaryExpr(spins.size(), [this]() -> double { return rand->Exponential(); }).cwiseProduct(previousSpins.cast<double>())
		).array().sign().cast<Spin>();  // 実質起こらないが、符号関数に渡しているため、スピンが0になる場合がある。
		previousSpins = spins;
		spins = temp;
	};

	auto modifiedMomentumAnnealing = [this]() {
		previousSpins = spins;
		spins = (
			calcLocalMagneticField(spins) + pinningParameter * spins.cast<double>()
			- temperature * Eigen::VectorXd::NullaryExpr(spins.size(), [this]() -> double { return rand->Exponential(); }).cwiseProduct(spins.cast<double>())
		).array().sign().cast<Spin>();  // 実質起こらないが、符号関数に渡しているため、スピンが0になる場合がある。
	};

	auto hillClimbing = [this]() {
		Configuration currentConfiguration = spins;
		while (true) {
			//double nextEval = std::numeric_limits<double>::infinity();
			double energyDifference = 0.e0;
			Configuration nextConfiguration = currentConfiguration;
			for (auto i = 0; i < spins.size(); i++) {
				double beforeEnergy = -1.e0 * static_cast<int>(currentConfiguration(i)) * calcLocalMagneticField(i);
				double afterEnergy = -1.e0 * static_cast<int>(flip(currentConfiguration(i))) * calcLocalMagneticField(i);
				if (energyDifference > afterEnergy - beforeEnergy) {
					energyDifference = afterEnergy - beforeEnergy;
					nextConfiguration(i) = flip(nextConfiguration(i));
				}
			}
			if (energyDifference >= 0.e0)
				break;
			currentConfiguration = nextConfiguration;
		}
		spins = currentConfiguration;
	};

	switch (algorithm) {
	case Algorithms::Metropolis:
		metropolisMethod();
		break;
	case Algorithms::Glauber:
		glauberDynamics();
		break;
	case Algorithms::SCA:
		stochasticCellularAutomata();
		break;
	case Algorithms::fcSCA:
		flipConstrainedStochasticCellularAutomata();
		break;
	case Algorithms::MA:
		momentumAnnealing();
		break;
	case Algorithms::MMA:
		modifiedMomentumAnnealing();
		break;
	case Algorithms::HillClimbing:
		hillClimbing();
		break;
	default:
		break;
	}
}

void IsingModel::Write() const
{
	std::cout << "Current spin configuration:" << std::endl;
	for (auto i = 0; i < spins.size(); i++)
		std::cout << std::setw(2) << static_cast<int>(spins(i));
	std::cout << "External magnetic field:" << std::endl;
	std::cout << externalMagneticField.transpose() << std::endl;
	std::cout << "Coupling coefficinets:" << std::endl;
	std::cout << couplingCoefficients << std::endl;
	std::cout << "Algorithm: " << AlgorithmToStr(algorithm) << std::endl;
	std::cout << "Temperature: " << temperature << std::endl;
	std::cout << "Pinning parameter: " << pinningParameter << std::endl;
	std::cout << "Flip trial rate: " << flipTrialRate << std::endl;
}


/* User code: This file will not be overwritten by TASTE. */

#include "printer.h"
#include <iostream>
#include <fstream>
#include "base_support/Base-samples-RigidBodyStateConvert.hpp"
#include "base_support/OpaqueConversion.hpp"
#include <Eigen/Core>
#include <vector>

void printer_startup()
{
    /* Write your initialization code here,
       but do not make any call to a required interface. */
}

// EIGEN HELPER METHODS
void to4d(Eigen::Matrix3d r, Eigen::Vector3d t, Eigen::Matrix4d & h){

  h<< r(0,0), r(0,1), r(0,2), t[0],
      r(1,0), r(1,1), r(1,2), t[1],
      r(2,0), r(2,1), r(2,2), t[2],
          0,      0,      0,    1;
}

void printer_PI_update_amp(const asn1SccBase_samples_RigidBodyState *IN_pose)
{
  base::Vector3d t;
  base::Quaterniond q;
  Eigen::Matrix3d r;

  static int i = 0;

  // extract orientation / translation
  asn1Scc_Vector3d_fromAsn1(t, IN_pose->position);
  asn1Scc_Quaterniond_fromAsn1(q, IN_pose->orientation);
  // convert quaternion to rotation mat
  r = q.normalized().toRotationMatrix();

  Eigen::Matrix4d p;

  to4d(r,t,p);

  std::ifstream f("marker.csv");
  bool good = f.good();

  std::ofstream myfile;
  if (good) myfile.open ("marker.csv", std::ios::app);
  else myfile.open ("marker.csv", std::ios::out);

  myfile << "\n" << std::floor(((p(0,3)) * 100000) + .5) / 100000
	 << ","  << std::floor(((p(1,3)) * 100000) + .5) / 100000
	 << ","  << std::floor(((p(2,3)) * 100000) + .5) / 100000;

  myfile.close();

  i++;

  std::cout << "got marker position:\n" << p << std::endl;
}

void printer_PI_update_arp(const asn1SccBase_samples_RigidBodyState *IN_pose)
{
  base::Vector3d t;
  base::Quaterniond q;
  Eigen::Matrix3d r;
  // extract orientation / translation
  asn1Scc_Vector3d_fromAsn1(t, IN_pose->position);
  asn1Scc_Quaterniond_fromAsn1(q, IN_pose->orientation);
  // convert quaternion to rotation mat
  r = q.normalized().toRotationMatrix();

  Eigen::Matrix4d p;

  to4d(r,t,p);

  std::ifstream f("marker.csv");
  bool good = f.good();

  std::ofstream myfile;
  if (good) myfile.open ("marker.csv", std::ios::app);
  else myfile.open ("marker.csv", std::ios::out);

  myfile << "\n" << std::floor(((p(0,3)) * 100000) + .5) / 100000
	 << ","  << std::floor(((p(1,3)) * 100000) + .5) / 100000
	 << ","  << std::floor(((p(2,3)) * 100000) + .5) / 100000;

  myfile.close();
  std::cout << "got robot position:\n" << p << std::endl;
}


#pragma once
#include <Eigen/Core>
#include <tuple>

//! The gradient of the shape function (on the reference element)
//!
//! We have three shape functions
//!
//! @param i integer between 0 and 2 (inclusive). Decides which shape function to return.
//! @param x x coordinate in the reference element.
//! @param y y coordinate in the reference element.
inline Eigen::Vector2d gradientLambda(const int i, double x, double y) {
	std::ignore = x;
	std::ignore = y;
	return Eigen::Vector2d(-1 + (i > 0) + (i == 1),
	                       -1 + (i > 0) + (i == 2));
}


#include <ostream>

#define NOMINMAX
#define BOOST_TEST_MAIN
#include <boost/test/included/unit_test.hpp>

#include "SimpleSparseMat.hpp"

#define FOR_MAT(W, H) for (size_t y = 0; y < H; ++y) for (size_t x = 0; x < W; ++x)

namespace ssmat
{
	template<typename T>
	inline std::ostream& operator<<(std::ostream& s, const ssmat::SparseEntry<T>& entry)
	{
		return s << "(" << entry.x << ", " << entry.y << " | " << entry.v << ")";
	}
}

namespace
{
	template<typename T, size_t W, size_t H>
	ssmat::SparseMat<T> FromArray(const T(&vs)[H][W])
	{
		std::vector<ssmat::SparseEntry<T>> entries;
		entries.reserve(W * H);
		for (ssmat::IndexT y = 0; y < H; ++y)
		{
			for (ssmat::IndexT x = 0; x < W; ++x)
			{
				const T val = vs[y][x];
				if (val != static_cast<T>(0))
				{
					entries.emplace_back(x, y, val);
				}
			}
		}
		return ssmat::SparseMat<T>(entries, ssmat::SparseFormat::CSR);
	}

	template<typename T, size_t W, size_t H>
	void ToArray(const ssmat::SparseMat<T>& sparseMat, T(&matOut)[H][W])
	{
		switch (sparseMat.getFormat())
		{
		case ssmat::SparseFormat::CSR:
			for (size_t y = 0; y < sparseMat.rowCount(); ++y)
			{
				for (size_t i = sparseMat.rowBegin(y); i < sparseMat.rowEnd(y); ++i)
				{
					matOut[y][sparseMat.getX(i)] = sparseMat.getV(i);
				}
			}
			break;
		case ssmat::SparseFormat::CSC:
			for (size_t y = 0; y < sparseMat.rowCount(); ++y)
			{
				for (size_t i = sparseMat.rowBegin(y); i < sparseMat.rowEnd(y); ++i)
				{
					matOut[sparseMat.getX(i)][y] = sparseMat.getV(i);
				}
			}
			break;
		default:
			break;
		}
	}

	template<typename T, size_t W_A, size_t H_A, size_t W_B>
	void MultipleMat(const T(&matA)[H_A][W_A], const T(&matB)[W_A][W_B], T(&matC)[H_A][W_B])
	{
		for (size_t i = 0; i < H_A; ++i)
		{
			for (size_t j = 0; j < W_B; ++j)
			{
				for (size_t k = 0; k < W_A; ++k)
				{
					matC[i][j] += matA[i][k] * matB[k][j];
				}
			}
		}
	}
}

BOOST_AUTO_TEST_SUITE(aa)

BOOST_AUTO_TEST_CASE(SparseMat_Init)
{
	constexpr int width = 4;
	constexpr int height = 3;
	const int matIn[height][width] = {
		{0, 1, 2, 1},
		{2, 3, 0, 5},
		{1, 0, 4, 0},
	};

	const auto sparseMat = FromArray(matIn);

	int matOut[height][width] = {};
	ToArray(sparseMat, matOut);

	FOR_MAT(width, height)
	{
		BOOST_CHECK_EQUAL(matIn[y][x], matOut[y][x]);
	}
}

BOOST_AUTO_TEST_CASE(SparseMat_Init2)
{
	auto data = ssmat::MakeEntries(
		{ 2,1,3,3,2,0,0,1 },
		{ 2,1,0,1,0,2,1,0 },
		{ 4,3,1,5,2,1,2,1 }
	);
	const auto entriesIn = SortEntries(data, ssmat::SparseFormat::CSR);

	const ssmat::SparseMat<int> sparseMat(entriesIn, ssmat::SparseFormat::CSR);
	const auto entriesOut = sparseMat.decompressEntries();

	BOOST_CHECK_EQUAL(entriesIn.size(), entriesOut.size());
	for (size_t i = 0; i < entriesIn.size(); ++i)
	{
		BOOST_CHECK_EQUAL(entriesIn[i], entriesOut[i]);
	}
}

BOOST_AUTO_TEST_CASE(SparseMat_Init3)
{
	auto data = ssmat::MakeEntries(
		{ 2,1,3,3,2,0,0,1 },
		{ 2,1,0,1,0,2,1,0 },
		{ 4,3,1,5,2,1,2,1 }
	);
	const auto entriesIn = SortEntries(data, ssmat::SparseFormat::CSC);

	const ssmat::SparseMat<int> sparseMat(entriesIn, ssmat::SparseFormat::CSC);
	const auto entriesOut = sparseMat.decompressEntries();

	BOOST_CHECK_EQUAL(entriesIn.size(), entriesOut.size());
	for (size_t i = 0; i < entriesIn.size(); ++i)
	{
		BOOST_CHECK_EQUAL(entriesIn[i], entriesOut[i]);
	}
}

BOOST_AUTO_TEST_CASE(SparseMat_Multiplication)
{
	constexpr int width = 4;
	constexpr int height = 3;
	const int matA[height][width] = {
		{0, 1, 2, 1},
		{2, 3, 0, 5},
		{1, 0, 4, 0},
	};
	const int matB[width][height] = {
		{0, 1, 2},
		{1, 4, 0},
		{0, 0, 1},
		{0, 3, 0},
	};
	int matC[height][height] = {};
	MultipleMat(matA, matB, matC);

	auto sparseMatA = FromArray(matA);
	auto sparseMatB = FromArray(matB);
	const auto sparseMatC = sparseMatA * sparseMatB;

	int matC_[height][height] = {};
	ToArray(sparseMatC, matC_);

	FOR_MAT(height, height)
	{
		BOOST_CHECK_EQUAL(matC[y][x], matC_[y][x]);
	}
}

BOOST_AUTO_TEST_CASE(SparseMat_Summation)
{
	constexpr int width = 4;
	constexpr int height = 3;
	const int matA[height][width] = {
		{0, 1, 2, 1},
		{2, 3, 0, 5},
		{1, 0, 4, 0},
	};
	const int matB[height][width] = {
		{3, 1, 2, 1},
		{0, 0, 1, 0},
		{1, 0, 3, 4},
	};

	const auto sparseMatA = FromArray(matA);
	const auto sparseMatB = FromArray(matB);
	const auto sparseMatC = sparseMatA + sparseMatB;

	int matC_[height][width] = {};
	ToArray(sparseMatC, matC_);

	FOR_MAT(width, height)
	{
		BOOST_CHECK_EQUAL(matA[y][x] + matB[y][x], matC_[y][x]);
	}
}

BOOST_AUTO_TEST_CASE(SparseMat_Insert)
{
	constexpr int width = 4;
	constexpr int height = 3;
	int matIn[height][width] = {
		{0, 0, 2, 1},
		{2, 0, 0, 5},
		{0, 0, 4, 0},
	};

	auto sparseMatA = FromArray(matIn);

	matIn[1][2] = 3;
	matIn[2][3] = 1;
	sparseMatA.insert(2, 1, 3);
	sparseMatA.insert(3, 2, 1);

	int matOut[height][width] = {};
	ToArray(sparseMatA, matOut);

	FOR_MAT(height, height)
	{
		BOOST_CHECK_EQUAL(matIn[y][x], matOut[y][x]);
	}
}

BOOST_AUTO_TEST_CASE(SparseMat_Append)
{
	constexpr int width = 4;
	constexpr int height = 3;
	int matIn[height][width] = {
		{1, 0, 0, 0},
		{0, 0, 1, 0},
		{0, 1, 0, 0},
	};

	auto sparseMatA = FromArray(matIn);
	matIn[1][3] = 3;
	matIn[0][3] = 1;
	matIn[2][0] = 1;
	std::vector<ssmat::SparseEntry<int>> additionalEntries;
	additionalEntries.emplace_back(3, 1, 3);
	additionalEntries.emplace_back(3, 0, 1);
	additionalEntries.emplace_back(0, 2, 1);
	sparseMatA.append(additionalEntries);

	int matOut[height][width] = {};
	ToArray(sparseMatA, matOut);

	FOR_MAT(height, height)
	{
		BOOST_CHECK_EQUAL(matIn[y][x], matOut[y][x]);
	}
}

BOOST_AUTO_TEST_CASE(SparseMat_Fill0)
{
	ssmat::SparseMat<int> sparseMat;
	sparseMat.fill(3, 7, 0);

	const auto& rowBeginIndices = sparseMat.getRowBeginIndices();
	const auto& xs = sparseMat.getXs();
	const auto& vs = sparseMat.getVs();

	BOOST_CHECK_EQUAL(rowBeginIndices.size(), 0);
	BOOST_CHECK_EQUAL(xs.size(), 0);
	BOOST_CHECK_EQUAL(vs.size(), 0);
}

BOOST_AUTO_TEST_CASE(SparseMat_Fill1)
{
	constexpr int width = 3;
	constexpr int height = 7;

	ssmat::SparseMat<int> sparseMat;
	sparseMat.fill(width, height, 2);
	sparseMat.fill(height, width, 1);

	int matOut[height][width] = {};
	ToArray(sparseMat, matOut);

	FOR_MAT(width, height)
	{
		BOOST_CHECK_EQUAL(matOut[y][x], 1);
	}
}

BOOST_AUTO_TEST_CASE(SparseMat_Transpose)
{
	constexpr int width = 4;
	constexpr int height = 3;
	const int matA[height][width] = {
		{0, 5, 0, 3},
		{2, 0, 0, 5},
		{1, 7, 0, 0},
	};
	const int matAt[width][height] = {
		{0, 2, 1},
		{5, 0, 7},
		{0, 0, 0},
		{3, 5, 0},
	};

	auto sparseMat = FromArray(matA);
	sparseMat.transpose();
	int matOut[width][height] = {};
	ToArray(sparseMat, matOut);

	FOR_MAT(height, width)
	{
		BOOST_CHECK_EQUAL(matOut[y][x], matAt[y][x]);
	}
}

BOOST_AUTO_TEST_SUITE_END()


#include "wx/wxprec.h"
#include "wx/wx.h"
#include "sample.xpm"
#include <Eigen/Dense>

using namespace Eigen;

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxAppHeat
/////////////////////////////////////////////////////////////////////////////////////////////////////

class wxAppHeat : public wxApp
{
public:
  virtual bool OnInit();
};

wxIMPLEMENT_APP(wxAppHeat);

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat
/////////////////////////////////////////////////////////////////////////////////////////////////////

class wxFrameHeat : public wxFrame
{
public:
  wxFrameHeat(const wxString& title);
  void OnQuit(wxCommandEvent& event);
  void OnAbout(wxCommandEvent& event);
  virtual void OnPaint(wxPaintEvent& event);
  wxColour to_color(float N);

  ~wxFrameHeat()
  {
    delete[] T_fvm;
    delete[] T_fdm_1;
    delete[] Tn_fdm_1;
    delete[] T_fdm_2;
    delete[] Tn_fdm_2;
  }

private:
  void init_constants();
  void solve_fvm();
  void solve_fdm_1();
  void solve_fdm_2();
  void draw_cell(wxDC* dc, float N, wxCoord x, wxCoord y, wxCoord width, wxCoord height);

  //temperature at the left hand side of the bar(deg C)
  float T_A;

  //temperature at the left hand side of the bar(deg C)
  float T_B;

  //thermal conductivity k, [W/mK]
  float k;

  //heat source per unit volume (W/m3)
  float S;

  /////////////////////////////////////////////////////////////////////////////////////////////////////
  //FVM
  /////////////////////////////////////////////////////////////////////////////////////////////////////

  //number of cells for FVM
  Index m_nbr_fvm;

  //temperature (solution)
  float* T_fvm;

  /////////////////////////////////////////////////////////////////////////////////////////////////////
  //FDM
  /////////////////////////////////////////////////////////////////////////////////////////////////////

  //number of cells for FVM
  int m_nbr_fdm;

  //temperature (solution)
  float* T_fdm_1;
  float* Tn_fdm_1;

  //temperature (solution)
  float* T_fdm_2;
  float* Tn_fdm_2;

private:
  wxDECLARE_EVENT_TABLE();
};

wxBEGIN_EVENT_TABLE(wxFrameHeat, wxFrame)
EVT_MENU(wxID_EXIT, wxFrameHeat::OnQuit)
EVT_MENU(wxID_ABOUT, wxFrameHeat::OnAbout)
EVT_PAINT(wxFrameHeat::OnPaint)
wxEND_EVENT_TABLE()

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxAppHeat::OnInit()
/////////////////////////////////////////////////////////////////////////////////////////////////////

bool wxAppHeat::OnInit()
{
  if (!wxApp::OnInit())
  {
    return false;
  }
  wxFrameHeat* frame = new wxFrameHeat("Heat");
  frame->Maximize(true);
  frame->Show(true);
  return true;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::wxFrameHeat
/////////////////////////////////////////////////////////////////////////////////////////////////////

wxFrameHeat::wxFrameHeat(const wxString& title)
  : wxFrame(NULL, wxID_ANY, title)
{
  SetIcon(wxICON(sample));
  wxMenu* menu_file = new wxMenu;
  menu_file->Append(wxID_EXIT, "E&xit\tAlt-X", "Quit this program");
  wxMenu* menu_help = new wxMenu;
  menu_help->Append(wxID_ABOUT, "&About\tF1", "Show about dialog");
  wxMenuBar* menu_bar = new wxMenuBar();
  menu_bar->Append(menu_file, "&File");
  menu_bar->Append(menu_help, "&Help");
  SetMenuBar(menu_bar);
  CreateStatusBar(2);
  SetStatusText("Ready");
  init_constants();
  solve_fvm();
  solve_fdm_1();
  solve_fdm_2();
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::OnQuit
/////////////////////////////////////////////////////////////////////////////////////////////////////

void wxFrameHeat::OnQuit(wxCommandEvent& WXUNUSED(event))
{
  Close(true);
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::OnAbout
/////////////////////////////////////////////////////////////////////////////////////////////////////

void wxFrameHeat::OnAbout(wxCommandEvent& WXUNUSED(event))
{
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::to_color
/////////////////////////////////////////////////////////////////////////////////////////////////////

wxColour wxFrameHeat::to_color(float N)
{
  wxColour C;
  if (N < 130) C = wxColour(187, 206, 255);
  else if (N < 160) C = wxColour(132, 166, 250);
  else if (N < 190) C = wxColour(66, 108, 218);
  else if (N < 200) C = wxColour(50, 72, 141);
  else C = wxColour(34, 47, 87);
  return C;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::draw_cell
/////////////////////////////////////////////////////////////////////////////////////////////////////

void wxFrameHeat::draw_cell(wxDC* dc, float N, wxCoord x, wxCoord y, wxCoord width, wxCoord height)
{
  dc->SetBrush(wxBrush(wxColour(to_color(N))));
  dc->DrawRectangle(x, y - height / 2, width, height);
  wxString str = wxString::Format(wxT("%.1f"), N);
  dc->DrawText(str, x + width / 2, y);
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::OnDraw
/////////////////////////////////////////////////////////////////////////////////////////////////////

void wxFrameHeat::OnPaint(wxPaintEvent&)
{
  wxPaintDC dc(this);
  wxFont font(wxFontInfo(8).FaceName("Courier"));
  dc.SetFont(font);
  wxCoord width = 100;
  wxCoord height = 50;
  wxCoord x = 0, y = height / 2;

  /////////////////////////////////////////////////////////////////////////////////////////////////////
  //FVM
  /////////////////////////////////////////////////////////////////////////////////////////////////////

  //boundary
  draw_cell(&dc, T_A, x, y, width, height);
  x += width;
  //interior
  for (int idx = 0; idx < m_nbr_fvm; idx++)
  {
    draw_cell(&dc, T_fvm[idx], x, y, width, height);
    x += width;
  }
  //boundary
  draw_cell(&dc, T_B, x, y, width, height);

  /////////////////////////////////////////////////////////////////////////////////////////////////////
  //FDM_1
  /////////////////////////////////////////////////////////////////////////////////////////////////////

  x = 0; y += 100;
  for (int idx = 0; idx < m_nbr_fdm; idx++)
  {
    draw_cell(&dc, T_fdm_1[idx], x, y, width, height);
    x += width;
  }

  /////////////////////////////////////////////////////////////////////////////////////////////////////
  //FDM_2
  /////////////////////////////////////////////////////////////////////////////////////////////////////

  x = 0; y += 100;
  for (int idx = 0; idx < m_nbr_fdm; idx++)
  {
    draw_cell(&dc, T_fdm_2[idx], x, y, width, height);
    x += width;
  }
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::init_constants
/////////////////////////////////////////////////////////////////////////////////////////////////////

void wxFrameHeat::init_constants()
{
  //temperature at the left hand side of the bar(deg C)
  T_A = 100;

  //temperature at the left hand side of the bar(deg C)
  T_B = 200;

  //thermal conductivity k, [W/mK]
  k = 100;

  //heat source per unit volume (W/m3)
  S = 1000.0f;;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::solve_fvm
//Finite Volume Method
/////////////////////////////////////////////////////////////////////////////////////////////////////

void wxFrameHeat::solve_fvm()
{
  //number of cells
  m_nbr_fvm = 5;

  ////////////////////////////////////////////////////////////////////////////////////////////////////
 //allocate solution vectors
 /////////////////////////////////////////////////////////////////////////////////////////////////////

 //FVM
  T_fvm = new float[m_nbr_fvm];

  /////////////////////////////////////////////////////////////////////////////////////////////////////
  //geometry
  /////////////////////////////////////////////////////////////////////////////////////////////////////

  //length of the bar(m)
  const float length = 5;

  //coordinates of the cell faces
  float* x_faces = new float[m_nbr_fvm + 1];
  x_faces[0] = 0;
  for (int idx = 1; idx < m_nbr_fvm + 1; idx++)
  {
    x_faces[idx] = x_faces[idx - 1] + length / m_nbr_fvm;
    wxLogDebug("x_faces [%d]=%f", idx, x_faces[idx]);
  }

  //coordinates of the cell centroids
  float* x_center = new float[m_nbr_fvm];
  for (int idx = 0; idx < m_nbr_fvm; idx++)
  {
    x_center[idx] = 0.5f * (x_faces[idx + 1] + x_faces[idx]);
    wxLogDebug("x_center [%d]=%f", idx, x_center[idx]);
  }

  //length of each cell
  float* cell_length = new float[m_nbr_fvm];
  for (int idx = 0; idx < m_nbr_fvm; idx++)
  {
    cell_length[idx] = x_faces[idx + 1] - x_faces[idx];
    wxLogDebug("cell_length [%d]=%f", idx, cell_length[idx]);
  }

  //distance between cell centroids
  float* dist_centroids = new float[m_nbr_fvm + 1];
  for (int idx = 0; idx < m_nbr_fvm - 1; idx++)
  {
    dist_centroids[idx] = x_center[idx + 1] - x_center[idx];
    wxLogDebug("dist_centroids [%d]=%f", idx, dist_centroids[idx]);
  }

  //for the boundary cell on the left, the distance is double the distance
  //from the cell centroid to the boundary face
  float dist_left = 2 * (x_center[0] - x_faces[0]);

  //for the boundary cell on the right, the distance is double the distance from
  //the cell centroid to the boundary cell face
  float dist_right = 2 * (x_faces[m_nbr_fvm] - x_center[m_nbr_fvm - 1]);

  for (int idx = 0; idx < m_nbr_fvm; idx++)
  {
    dist_centroids[idx + 1] = dist_centroids[idx];
  }
  dist_centroids[0] = dist_left;
  dist_centroids[m_nbr_fvm] = dist_right;
  for (int idx = 0; idx < m_nbr_fvm + 1; idx++)
  {
    wxLogDebug("dist_centroids [%d]=%f", idx, dist_centroids[idx]);
  }

  /////////////////////////////////////////////////////////////////////////////////////////////////////
  //constants
  /////////////////////////////////////////////////////////////////////////////////////////////////////

  //cross-sectional area A, [m2]
  const float Area = 0.1f;

  //cell volume
  float* cell_volume = new float[m_nbr_fvm];
  for (int idx = 0; idx < m_nbr_fvm; idx++)
  {
    cell_volume[idx] = cell_length[idx] * Area;
    wxLogDebug("cell_volume [%d]=%f", idx, cell_volume[idx]);
  }

  /////////////////////////////////////////////////////////////////////////////////////////////////////
  //create the matrices
  //zero initially
  /////////////////////////////////////////////////////////////////////////////////////////////////////

  MatrixXf A = MatrixXf::Zero(m_nbr_fvm, m_nbr_fvm);
  VectorXf B = VectorXf::Zero(m_nbr_fvm);

  for (Index n = 0; n < m_nbr_fvm; n++)
  {
    /////////////////////////////////////////////////////////////////////////////////////////////////////
    //left boundary cell
    /////////////////////////////////////////////////////////////////////////////////////////////////////

    if (n == 0)
    {
      //left coefficient aP, T at n
      float aP = k * Area / dist_centroids[n] + k * Area / dist_centroids[n] * 2.0;
      wxLogDebug("left A[%lld,%lld]=%f", n, n, aP);
      A(n, n) = aP;

      //right coefficient aR, T at n+1
      float aR = k * Area / dist_centroids[n];
      wxLogDebug("left A[%lld,%lld]=%f", n, n + 1, -1.0 * aR);
      A(n, n + 1) = -1.0 * aR;

      //source
      float source = S * cell_volume[n];
      //additional boundary source, temperature A
      source += T_A * Area * k / dist_centroids[n] * 2.0;
      wxLogDebug("B[%lld]=%f", n, source);
      B(n) = source;
    }

    /////////////////////////////////////////////////////////////////////////////////////////////////////
    //right boundary cell
    /////////////////////////////////////////////////////////////////////////////////////////////////////

    else if (n == m_nbr_fvm - 1)
    {
      //left coefficient aL, T at n-1
      float aL = k * Area / dist_centroids[n];;
      wxLogDebug("right A[%lld,%lld]=%f", n, n - 1, -1.0 * aL);
      A(n, n - 1) = -1.0 * aL;

      //right coefficient aP, T at n
      float aP = k * Area / dist_centroids[n] + k * Area / dist_centroids[n] * 2.0;
      wxLogDebug("right A[%lld,%lld]=%f", n, n, aP);
      A(n, n) = aP;

      //source
      float source = S * cell_volume[n];
      //additional boundary source, temperature B
      source += T_B * Area * k / dist_centroids[n] * 2.0;
      wxLogDebug("B[%lld]=%f", n, source);
      B(n) = source;
    }

    /////////////////////////////////////////////////////////////////////////////////////////////////////
    //interior cells
    /////////////////////////////////////////////////////////////////////////////////////////////////////

    else
    {
      //left coefficient aL, T at n-1
      float aL = k * Area / dist_centroids[n];
      wxLogDebug("A[%lld,%lld]=%f", n, n - 1, -1.0 * aL);
      A(n, n - 1) = -1.0 * aL;

      //right coefficient aR, T at n+1
      float aR = k * Area / dist_centroids[n];
      wxLogDebug("A[%lld,%lld]=%f", n, n + 1, -1.0 * aR);
      A(n, n + 1) = -1.0 * aR;

      //middle coefficient aP, T at n
      float aP = aR + aL;
      wxLogDebug("A[%lld,%lld]=%f", n, n, aP);
      A(n, n) = aP;

      //source
      float source = S * cell_volume[n];
      wxLogDebug("B[%lld]=%f", n, source);
      B(n) = source;
    }
  }

  //solve
  VectorXf T_vec = A.colPivHouseholderQr().solve(B);
  float* t = T_vec.data();
  for (int idx = 0; idx < m_nbr_fvm; idx++)
  {
    T_fvm[idx] = t[idx];
    wxLogDebug("%.1f", t[idx]);
  }

  delete[] x_faces;
  delete[] x_center;
  delete[] cell_length;
  delete[] dist_centroids;
  delete[] cell_volume;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::solve_fdm_1
//Finite Differences Method
//diffusion only
/////////////////////////////////////////////////////////////////////////////////////////////////////

void wxFrameHeat::solve_fdm_1()
{
  //number of cells
  m_nbr_fdm = 7;

  ////////////////////////////////////////////////////////////////////////////////////////////////////
 //allocate solution vectors
 /////////////////////////////////////////////////////////////////////////////////////////////////////

  //FDM
  T_fdm_1 = new float[m_nbr_fdm];
  Tn_fdm_1 = new float[m_nbr_fdm];

  //time step
  float dt = 0.005f;
  float dx = 1.0f;

  //initial and boundary values
  for (int i = 0; i < m_nbr_fdm; i++)
  {
    T_fdm_1[i] = 0;
  }
  T_fdm_1[0] = T_A;
  T_fdm_1[m_nbr_fdm - 1] = T_B;

  for (int i = 0; i < m_nbr_fdm; i++)
  {
    wxLogDebug("T[%d]=%.1f", i, T_fdm_1[i]);
  }

  //F must be <= 0.5
  float F = k * dt / (dx * dx);
  wxLogDebug("F=%.4f", F);
  for (int n = 0; n < 100; n++)
  {
    for (int i = 1; i < m_nbr_fdm - 1; i++)
    {
      Tn_fdm_1[i] = T_fdm_1[i] + F * (T_fdm_1[i - 1] - 2 * T_fdm_1[i] + T_fdm_1[i + 1]) + dt * S;
    }
    for (int i = 1; i < m_nbr_fdm - 1; i++)
    {
      T_fdm_1[i] = Tn_fdm_1[i];
    }
    for (int i = 0; i < m_nbr_fdm; i++)
    {
      wxLogDebug("T[%d]=%.1f", i, T_fdm_1[i]);
    }
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
//wxFrameHeat::solve_fdm_2
//Finite Differences Method
//advection only
/////////////////////////////////////////////////////////////////////////////////////////////////////

void wxFrameHeat::solve_fdm_2()
{
  //number of cells
  m_nbr_fdm = 7;

  ////////////////////////////////////////////////////////////////////////////////////////////////////
 //allocate solution vectors
 /////////////////////////////////////////////////////////////////////////////////////////////////////

  //FDM
  T_fdm_2 = new float[m_nbr_fdm];
  Tn_fdm_2 = new float[m_nbr_fdm];

  //time step
  float dt = 0.5f;
  float dx = 1.0f;

  //initial and boundary values
  for (int i = 0; i < m_nbr_fdm; i++)
  {
    T_fdm_2[i] = 0;
  }
  T_fdm_2[0] = T_A;
  T_fdm_2[m_nbr_fdm - 1] = T_B;

  for (int i = 0; i < m_nbr_fdm; i++)
  {
    wxLogDebug("T[%d]=%.1f", i, T_fdm_2[i]);
  }

  int iter = 500;
  float time = dt * (float)iter;
  float U = 1.0f; //velocity
  float C = U * dt / dx; //courant number <= 1
  wxLogDebug("time=%.1f F=%.4f", time, C);
  for (int n = 0; n < iter; n++)
  {
    for (int i = 1; i < m_nbr_fdm - 1; i++)
    {
      Tn_fdm_2[i] = T_fdm_2[i] - C * (T_fdm_2[i] - T_fdm_2[i - 1]);
    }
    for (int i = 1; i < m_nbr_fdm - 1; i++)
    {
      T_fdm_2[i] = Tn_fdm_2[i];
    }
    for (int i = 0; i < m_nbr_fdm; i++)
    {
      wxLogDebug("T[%d]=%.1f", i, T_fdm_2[i]);
    }
  }
}


/* Copyright (C) 2012-2020 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */

#include <NTL/ZZ.h>
#include <helib/polyEval.h>
#include <helib/EncryptedArray.h>
#include <helib/debugging.h>

#include "gtest/gtest.h"
#include "test_common.h"

namespace {
struct Parameters
{
  const long p; //   p is the plaintext base
  const long r; //   r is the lifting
  const long m; //   m is a specific cyclotomic ring
  const long d; //   d is the polynomial degree
  const long k; //   k is the baby-step parameter
  const long max_d;
  const long L;
  const bool isMonic;

  Parameters(long p,
             long r,
             long m,
             long d,
             long k,
             long max_d,
             long L,
             bool isMonic) :
      p(p), r(r), m(m), d(d), k(k), max_d(max_d), L(L), isMonic(isMonic){};

  friend std::ostream& operator<<(std::ostream& os, const Parameters& params)
  {
    return os << "{"
              << "p=" << params.p << ","
              << "r=" << params.r << ","
              << "m=" << params.m << ","
              << "d=" << params.d << ","
              << "k=" << params.k << ","
              << "max_d=" << params.max_d << ","
              << "L=" << params.L << ","
              << "isMonic=" << params.isMonic << "}";
  };
};

class GTestPolyEval : public ::testing::TestWithParam<Parameters>
{
protected:
  long p;
  long r;
  long d;
  long max_d;
  long L;
  bool isMonic;
  long m;
  long k;
  helib::Context context;
  long p2r;
  std::shared_ptr<helib::EncryptedArray> ea;
  helib::SecKey secretKey;
  const helib::PubKey& publicKey;

  GTestPolyEval() :
      p(GetParam().p),
      r(GetParam().r),
      d(GetParam().d),
      max_d(GetParam().max_d),
      L(GetParam().L),
      isMonic(GetParam().isMonic),
      m(GetParam().m),
      k(GetParam().k),
      context((helib::setDryRun(helib_test::dry), m), p, r),
      p2r(context.alMod.getPPowR()),
      ea(std::make_shared<helib::EncryptedArray>(
          (helib::buildModChain(context, L, /*c=*/3), context))),
      secretKey(context),
      publicKey((secretKey.GenSecKey(), secretKey))
      //  addSome1DMatrices(secretKey); // compute key-switching matrices
      {};

  virtual void SetUp() override
  {
#ifdef DEBUG_PRINTOUT
    helib::dbgEa = ea; // for debugging purposes
    helib::dbgKey = &secretKey;
#endif
    if (!helib_test::noPrint)
      std::cout << (helib::isDryRun() ? "* dry run, " : "* ") << "degree-" << d
                << ", m=" << m << ", L=" << L << ", p^r=" << p2r << std::endl;
  };

  virtual void TearDown() override { helib::cleanupGlobals(); }
};

TEST_P(GTestPolyEval, encryptedPolynomialsEvaluateAtEncryptedPointCorrectly)
{
  NTL::zz_pBak bak;
  bak.save();
  NTL::zz_p::init(p);
  NTL::zz_pXModulus phimX = NTL::conv<NTL::zz_pX>(ea->getPAlgebra().getPhimX());

  // Choose random plaintext polynomials
  NTL::zz_pX pX = NTL::random_zz_pX(deg(phimX) - 1);
  NTL::Vec<NTL::zz_pX> ppoly(NTL::INIT_SIZE, d);
  for (long i = 0; i < ppoly.length(); i++)
    random(ppoly[i], deg(phimX) - 1);

  // Evaluate the non-encrypted polynomial
  NTL::zz_pX pres =
      (ppoly.length() > 0) ? ppoly[ppoly.length() - 1] : NTL::zz_pX::zero();
  for (long i = ppoly.length() - 2; i >= 0; i--) {
    MulMod(pres, pres, pX, phimX);
    pres += ppoly[i];
  }

  // Encrypt the random polynomials
  helib::Ctxt cX(publicKey);
  NTL::Vec<helib::Ctxt> cpoly(NTL::INIT_SIZE, d, cX);

  secretKey.Encrypt(cX, NTL::conv<NTL::ZZX>(pX));

  for (long i = 0; i < ppoly.length(); i++)
    secretKey.Encrypt(cpoly[i], NTL::conv<NTL::ZZX>(ppoly[i]));

  // Evaluate the encrypted polynomial
  helib::polyEval(cX, cpoly, cX);

  // Compare the results
  NTL::ZZX ret;
  secretKey.Decrypt(ret, cX);
  NTL::zz_pX cres = NTL::conv<NTL::zz_pX>(ret);
  EXPECT_EQ(cres, pres) << "encrypted poly MISMATCH";
};

TEST_P(GTestPolyEval, evaluatePolynomialOnCiphertext)
{
  // evaluate at random points (at least one co-prime with p)
  std::vector<long> x;
  ea->random(x);
  while (NTL::GCD(x[0], p) != 1) {
    x[0] = NTL::RandomBnd(p2r);
  }
  helib::Ctxt inCtxt(publicKey), outCtxt(publicKey);
  ea->encrypt(inCtxt, publicKey, x);

  NTL::ZZX poly;
  for (long i = d; i >= 0; i--)
    SetCoeff(poly, i, NTL::RandomBnd(p2r)); // coefficients are random
  if (isMonic)
    SetCoeff(poly, d); // set top coefficient to 1

  // Evaluate poly on the ciphertext
  helib::polyEval(outCtxt, poly, inCtxt, k);

  // Check the result
  std::vector<long> y;
  ea->decrypt(outCtxt, secretKey, y);
  for (long i = 0; i < ea->size(); i++) {
    EXPECT_EQ(helib::polyEvalMod(poly, x[i], p2r), y[i])
        << "plaintext poly MISMATCH\n";
  }
};

std::vector<Parameters> getParameters()
{
  std::vector<Parameters> allParams;

  // SLOW
  const long p = 7;
  const long r = 2;
  long m = 0;
  const long k = 0;
  long d = 34;

  // FAST
  // const long p = 3;
  // const long r = 2;
  //      long m = 91;
  // const long k = 0;
  // long d = -1;

  const long max_d = (d <= 0) ? 35 : d;
  const long L = (7 + NTL::NextPowerOfTwo(max_d)) * 30;

  if (m < 2) {
    m = helib::FindM(
        /*secprm=*/80, L, /*c=*/3, p, 1, 0, m, !helib_test::noPrint);
  }

  // Test both monic and non-monic polynomials of this degree
  if (d >= 0) {
    allParams.emplace_back(Parameters{p, r, m, d, k, max_d, L, false});
    allParams.emplace_back(Parameters{p, r, m, d, k, max_d, L, true});
  } else {
    // Test degrees 1 to 3 and 25 through 35
    for (d = 1; d <= 3; d += 2)
      allParams.emplace_back(Parameters{p, r, m, d, k, max_d, L, true});
    for (d = 25; d <= 33; d += 2)
      allParams.emplace_back(Parameters{p, r, m, d, k, max_d, L, true});
  }
  return allParams;
};

INSTANTIATE_TEST_SUITE_P(manyDegrees,
                         GTestPolyEval,
                         ::testing::ValuesIn(getParameters()));

} // namespace


#pragma once

#include <Eigen/Dense>

namespace hacky_toolkit {

//! \brief A Plane (or hyperplane) is a susbspace of which its dimension is one
//! less than of its ambient space. The template argument of the dimension of
//! the plane equals that of its ambient space. I.e., a Plane<float, 2> is a
//! line in a 2d space. A plane is represented by a vector of Dim + 1 parameters
//! that can be used to obtain the general form of the plane equation:
//! a.dot((x, 1)) + d = 0
template <typename Scalar_, int Dim_>
class Plane {
  static_assert(Dim_ > 0, "DYNAMIC_DIMENSION_NOT_SUPPORTED");

 public:
  inline Plane() = default;

  template <typename Derived>
  inline Plane(Eigen::MatrixBase<Derived> const& p) : a_(p) {}

  template <typename Derived>
  inline Plane(Eigen::MatrixBase<Derived> const& normal, Scalar_ d) {
    a_.template head<Dim_>() = normal;
    a_(Dim_) = d;
  }

  //! \brief Creates a normalized plane using two points. The spatial dimension
  //! of the Plane must equal 2 when using this constructor. Input points can be
  //! of dimension 2 or 3.
  template <typename Derived0, typename Derived1>
  inline Plane(
      Eigen::MatrixBase<Derived0> const& x,
      Eigen::MatrixBase<Derived1> const& y) {
    static_assert(Dim_ == 2);
    static_assert(
        Derived0::SizeAtCompileTime == Dim_ ||
        Derived0::SizeAtCompileTime == Dim_ + 1);
    static_assert(Derived0::SizeAtCompileTime == Derived1::SizeAtCompileTime);
    if constexpr (Derived0::SizeAtCompileTime == Dim_) {
      a_ = x.homogeneous().cross(y.homogeneous());
    } else {
      a_ = x.cross(y);
    }
    Normalize();
  }

  //! \brief Creates a normalized plane using three points. The spatial
  //! dimension of the Plane must equal 3 when using this constructor. Input
  //! points can be of dimension 3 or 4.
  template <typename Derived0, typename Derived1, typename Derived2>
  inline Plane(
      Eigen::MatrixBase<Derived0> const& x,
      Eigen::MatrixBase<Derived1> const& y,
      Eigen::MatrixBase<Derived2> const& z) {
    static_assert(Dim_ == 3);
    static_assert(
        Derived0::SizeAtCompileTime == Dim_ ||
        Derived0::SizeAtCompileTime == Dim_ + 1);
    static_assert(
        Derived0::SizeAtCompileTime == Derived1::SizeAtCompileTime &&
        Derived1::SizeAtCompileTime == Derived2::SizeAtCompileTime);
    if constexpr (Derived0::SizeAtCompileTime == Dim_) {
      a_.template head<3>() = (y - x).cross(z - x);
      a_(Dim_) = -x.dot(a_.template head<3>());
    } else {
      auto xn = x.hnormalized().eval();
      a_.template head<3>() =
          (y.hnormalized() - xn).cross(z.hnormalized() - xn);
      a_(Dim_) = -xn.dot(a_.template head<3>());
    }
    Normalize();
  }

  template <typename Derived>
  inline Scalar_ SignedDistance(Eigen::MatrixBase<Derived> const& x) const {
    static_assert(
        Derived::SizeAtCompileTime == Dim_ ||
        Derived::SizeAtCompileTime == Dim_ + 1);
    if constexpr (Derived::SizeAtCompileTime == Dim_) {
      return a_.dot(x.homogeneous());
    } else {
      return a_.dot(x);
    }
  }

  template <typename Derived>
  inline Scalar_ Distance(Eigen::MatrixBase<Derived> const& x) const {
    return std::abs(SignedDistance(x));
  }

  template <typename Derived>
  inline bool Positive(Eigen::MatrixBase<Derived> const& x) const {
    return SignedDistance(x) > Scalar_(0.0);
  }

  template <typename Derived>
  inline bool Negative(Eigen::MatrixBase<Derived> const& x) const {
    return SignedDistance(x) < Scalar_(0.0);
  }

  //! \brief Tests is a point is considered to lie on the plane.
  //! \returns true if \p x is on the plane.
  template <typename Derived>
  inline bool Test(
      Eigen::MatrixBase<Derived> const& x,
      Scalar_ threshold = Eigen::NumTraits<Scalar_>::dummy_precision()) const {
    return Distance(x) < threshold;
  }

  //! \brief Normalizes the plane such that the length of the normal (the first
  //! Dim parameters of a()) equals 1.
  inline void Normalize() { a_ /= a_.template head<Dim_>().norm(); }

  //! \brief Point on the plane closest to the origin.
  inline Eigen::Matrix<Scalar_, Dim_, 1> Origin() const {
    return normal() * -d();
  }

  //! \brief Generates a random point on the unit hypersphere of the subspace
  //! defined by this plane at a distance of \p distance from the point returned
  //! by the Origin() method. The plane does not have to be normalized.
  //! \returns A random point on the plane.
  inline Eigen::Matrix<Scalar_, Dim_, 1> RandomPoint(
      Scalar_ distance = Scalar_(1.0)) const {
    // Perturb the normal with some noise.
    // What are the odds that the added noise equals 0?
    Eigen::Matrix<Scalar_, Dim_, 1> v =
        normal() + Eigen::Matrix<Scalar_, Dim_, 1>::Random();
    // Apply Gram-Schmidt process to get an orthogonal vector. Ie, subtract the
    // projection of v on the normal from v.
    return (v - normal() * normal().dot(v) / normal().squaredNorm())
                   .normalized() *
               distance +
           Origin();
  }

  //! \brief The orientation of the plane is defined by its normal. Not
  //! guaranteed to be normalized.
  //! \returns The plane normal.
  inline auto normal() const {
    // Eigen::VectorBlock<VectorType const, Dim_>
    return a_.template head<Dim_>();
  }

  //! \brief The negative of the distance from the origin to the plane in the
  //! direction of the normal. That is, the closest point on the plane to the
  //! origin equals:
  //! \code
  //! plane.normal() * -plane.d()
  //! \endcode
  //! \returns Negative distance from the origin to the plane.
  Scalar_ const& d() const { return a_(Dim_); }

  //! \brief The parameters of the plane. They describe the General form of the
  //! plane equation: a.dot((x, 1)) = 0
  auto const& a() const { return a_; }

  //! \brief Generates a Plane with a random normal at a distance somewhere in
  //! the range of [-max_distance:max_distance].
  static Plane Random(Scalar_ max_distance = Scalar_(1.0)) {
    return {
        Eigen::Matrix<Scalar_, Dim_, 1>::Random().normalized(),
        Eigen::Matrix<Scalar_, 1, 1>::Random()(0) * max_distance};
  }

 private:
  Eigen::Matrix<Scalar_, Dim_ + 1, 1> a_;
};

using Plane2d = Plane<double, 2>;
using Plane3d = Plane<double, 3>;

using Plane2f = Plane<float, 2>;
using Plane3f = Plane<float, 3>;

}  // namespace hacky_toolkit


#pragma once

#include <Eigen/Core>
#include <deal.II/base/index_set.h>
#include <cmath>

#include "base/numbers.hpp"
#include "enum/enum.hpp"
#include "quadrature/qmidpoint.hpp"
#include "quadrature/quad_handler1d.hpp"
#include "quadrature/tensor_product_quadrature.hpp"
#include "spectral/basis/spectral_elem_accessor.hpp"
#include "spectral/basis/toolbox/spectral_basis.hpp"

namespace boltzmann {
class Momentum
{
 private:
  typedef Eigen::Vector2d value_t;

 public:
  Momentum() {}

  template <typename SPECTRAL_BASIS>
  Momentum(const SPECTRAL_BASIS& spectral_basis)
  {
    init(spectral_basis);
  }

  template <typename SPECTRAL_BASIS>
  void init(const SPECTRAL_BASIS& spectral_basis);

  /**
   * @brief compute momentum,
   *
   * @param dst
   * @param src
   * @param count #physical dofs
   */
  template <typename NUMERIC_T>
  void compute(NUMERIC_T* dstx, NUMERIC_T* dsty, const NUMERIC_T* src, int count) const;

  template <typename VEC_IN, typename VEC_OUT, typename INDEXER>
  void compute(VEC_OUT& dstx,
               VEC_OUT& dsty,
               const VEC_IN& src,
               const dealii::IndexSet& relevant_dofs,
               const INDEXER& indexer) const;

  template <typename NUMERIC_T>
  value_t compute(const NUMERIC_T* src) const;

 private:
  typedef std::pair<unsigned int, value_t> entry_t;

 private:
  unsigned int n_velo_dofs;
  std::vector<entry_t> contributions;

 public:
  const std::vector<entry_t>& entries() const { return contributions; }
};

// ------------------------------------------------------------
template <typename SPECTRAL_BASIS>
void
Momentum::init(const SPECTRAL_BASIS& spectral_basis)
{
  n_velo_dofs = spectral_basis.n_dofs();

  typedef typename std::tuple_element<1, typename SPECTRAL_BASIS::elem_t::container_t>::type
      radial_elem_t;

  typedef typename std::tuple_element<0, typename SPECTRAL_BASIS::elem_t::container_t>::type
      angular_elem_t;

  auto& QR_handler = QuadHandler<MaxwellQuadrature>::GetInstance();
  typedef QuadAdaptor<MaxwellQuadrature> qr_adapt;

  unsigned int nqR = spectral::get_max_k(spectral_basis) + 2;
  Eigen::VectorXd pts_qR(nqR);
  Eigen::VectorXd wts_qR(nqR);

  qr_adapt::apply(pts_qR.data(), wts_qR.data(), QR_handler.get(nqR), 0.5);

  typedef typename SPECTRAL_BASIS::elem_t elem_t;
  for (auto it = spectral_basis.begin(); it != spectral_basis.end(); ++it) {
    typename elem_t::Acc::template get<radial_elem_t> getter;
    typename elem_t::Acc::template get<angular_elem_t> geta;

    const auto& rr = getter(*it);

    if (geta(*it).get_id().l != 1) continue;

    // make sure we are using the right basis
    const double w = rr.w();
    if (abs(0.5 - w) > 1e-10) throw std::runtime_error("Error: wrong weight in Momentum::init");

    value_t sum;
    sum[0] = 0.0;
    sum[1] = 0.0;
    if (geta(*it).get_id().t == TRIG::SIN) {
      // sin
      for (unsigned int q = 0; q < pts_qR.size(); ++q) {
        const double r = pts_qR[q];
        sum[TRIG::SIN] += rr.evaluate(r) * r * wts_qR[q];
      }
    } else if (geta(*it).get_id().t == TRIG::COS) {
      for (unsigned int q = 0; q < pts_qR.size(); ++q) {
        const double r = pts_qR[q];
        sum[TRIG::COS] += rr.evaluate(r) * r * wts_qR[q];
      }
    }

    const unsigned int i = it - spectral_basis.begin();
    contributions.push_back(std::make_pair(i, numbers::PI * sum));
  }
}

// // ------------------------------------------------------------
template <typename NUMERIC_T>
void
Momentum::compute(NUMERIC_T* dstx, NUMERIC_T* dsty, const NUMERIC_T* src, int count) const
{
#pragma omp parallel for
  for (int i = 0; i < count; ++i) {
    const NUMERIC_T* local_src = src + i * n_velo_dofs;
    value_t sum;
    sum[0] = 0;
    sum[1] = 0;
    for (unsigned int j = 0; j < contributions.size(); ++j) {
      sum += contributions[j].second * local_src[contributions[j].first];
    }
    dstx[i] = sum[0];
    dsty[i] = sum[1];
  }
}

// ------------------------------------------------------------
template <typename NUMERIC_T>
Momentum::value_t
Momentum::compute(const NUMERIC_T* src) const
{
  value_t sum;
  sum[0] = 0;
  sum[1] = 0;
  for (unsigned int j = 0; j < contributions.size(); ++j) {
    sum += contributions[j].second * src[contributions[j].first];
  }
  return sum;
}

// // ------------------------------------------------------------
template <typename VEC_IN, typename VEC_OUT, typename INDEXER>
void
Momentum::compute(VEC_OUT& dstx,
                  VEC_OUT& dsty,
                  const VEC_IN& src,
                  const dealii::IndexSet& relevant_dofs,
                  const INDEXER& indexer) const
{
#pragma omp parallel for
  for (unsigned int ix = 0; ix < relevant_dofs.size(); ++ix) {
    if (relevant_dofs.is_element(ix)) {
      value_t sum;
      sum[0] = 0;
      sum[1] = 0;
      for (unsigned int j = 0; j < contributions.size(); ++j) {
        unsigned int jx = contributions[j].first;
        sum += contributions[j].second * src[indexer.to_global(ix, jx)];
      }
      dstx[ix] = sum[0];
      dsty[ix] = sum[1];
    }
  }
}

}  // end namespace boltzmann


// File: recursator2.cpp

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/recursion/matrix_recursator.hpp>

int main(int, char**)
{
    using namespace mtl; using std::cout;

    typedef morton_dense<double, recursion::morton_z_mask>  matrix_type;
    matrix_type                                             A(10, 10);
    mat::hessian_setup(A, 3.0);
    mat::recursator<matrix_type>                          rec(A);

    // Create a recursator for the north_east quadrant of A
    mat::recursator<matrix_type>                          ne(north_east(rec));

    cout << "Test if recursator 'ne' refers to an empty matrix (shouldn't): " << is_empty(ne) << "\n";
    cout << "Test if north_east of 'ne' refers to an empty matrix (it should): " << is_empty(north_east(ne)) << "\n";

    cout << "Number of rows and columns of north_east quadrant is: " << num_rows(ne)
	 << " and " << num_cols(ne) << "\n";

    cout << "Test if 'ne' fills ils virtual quadrant (shouldn't): " << is_full(ne) << "\n";
    cout << "Test if north_west fills its virtual quadrant (it should): " << is_full(north_west(rec)) << "\n";

    return 0;
}


//! \file examples/Arrangement_on_surface_2/bgl_dual_adapter.cpp
// Adapting the dual of an arrangement to a BGL graph.

#include "arr_rational_nt.h"
#include <CGAL/Cartesian.h>
#include <CGAL/Arr_segment_traits_2.h>
#include <CGAL/Arr_extended_dcel.h>
#include <CGAL/Arrangement_2.h>
#include <CGAL/graph_traits_dual_arrangement_2.h>
#include <CGAL/Arr_face_index_map.h>

#include <climits>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/visitors.hpp>

#include "arr_print.h"

// A property map that reads/writes the information to/from the extended
// face.
template <typename Arrangement, class Type> class Extended_face_property_map {
public:
  typedef typename Arrangement::Face_handle       Face_handle;

  // Boost property type definitions.
  typedef boost::read_write_property_map_tag      category;
  typedef Type                                    value_type;
  typedef value_type&                             reference;
  typedef Face_handle                             key_type;

  // The get function is required by the property map concept.
  friend reference get(const Extended_face_property_map&, key_type key)
  { return key->data(); }

  // The put function is required by the property map concept.
  friend void put(const Extended_face_property_map&,
                  key_type key, value_type val)
  { key->set_data(val); }
};

typedef CGAL::Cartesian<Number_type>                         Kernel;
typedef CGAL::Arr_segment_traits_2<Kernel>                   Traits_2;
typedef CGAL::Arr_face_extended_dcel<Traits_2, unsigned int> Dcel;
typedef CGAL::Arrangement_2<Traits_2, Dcel>                  Ex_arrangement;
typedef CGAL::Dual<Ex_arrangement>                           Dual_arrangement;
typedef CGAL::Arr_face_index_map<Ex_arrangement>             Face_index_map;
typedef Extended_face_property_map<Ex_arrangement,unsigned int>
                                                             Face_property_map;
typedef Kernel::Point_2                                      Point_2;
typedef Kernel::Segment_2                                    Segment_2;

int main()
{
  // Construct an arrangement of seven intersecting line segments.
  Point_2 p1(1, 1), p2(1, 4), p3(2, 2), p4(3, 7), p5(4, 4), p6(7, 1), p7(9, 3);
  Ex_arrangement  arr;
  insert(arr, Segment_2(p1, p6));
  insert(arr, Segment_2(p1, p4));  insert(arr, Segment_2(p2, p6));
  insert(arr, Segment_2(p3, p7));  insert(arr, Segment_2(p3, p5));
  insert(arr, Segment_2(p6, p7));  insert(arr, Segment_2(p4, p7));

  // Create a mapping of the arrangement faces to indices.
  Face_index_map  index_map(arr);

  // Perform breadth-first search from the unbounded face, using the event
  // visitor to associate each arrangement face with its discover time.
  unsigned int    time = 0;
  boost::breadth_first_search(Dual_arrangement(arr), arr.unbounded_face(),
                              boost::vertex_index_map(index_map).visitor
                              (boost::make_bfs_visitor
                               (stamp_times(Face_property_map(), time,
                                            boost::on_discover_vertex()))));

  // Print the discover time of each arrangement face.
  Ex_arrangement::Face_iterator  fit;
  for (fit = arr.faces_begin(); fit != arr.faces_end(); ++fit) {
    std::cout << "Discover time " << fit->data() << " for ";
    if (fit != arr.unbounded_face()) {
      std::cout << "face ";
      print_ccb<Ex_arrangement>(fit->outer_ccb());
    }
    else std::cout << "the unbounded face." << std::endl;
  }
  return 0;
}


#define BOOST_TEST_MODULE MixedLatticeSolveTests

#include "DenseLattice.h"
#include "SparseLattice.h"
#include "MIAConfig.h"
#include "LibMIAUtil.h"

#include <Eigen/Dense>

#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

template<typename data_type>
void random_matrix(LibMIA::DenseLattice<data_type> & lat,double _prob,bool need_ranked=true, bool LSQR=false)
{
    boost::uniform_real<> uni_dist(0,1);
    boost::variate_generator<boost::random::mt19937&, boost::uniform_real<> > uni(LibMIA::LibMIA_gen(), uni_dist);
    boost::uniform_real<> uni_dist2(-10,10);
    boost::variate_generator<boost::random::mt19937&, boost::uniform_real<> > uni2(LibMIA::LibMIA_gen(), uni_dist2);
    lat.zeros();
    for(size_t k=0;k<(size_t)lat.depth();++k){
        auto _start=lat.data_begin()+k*lat.width()*lat.height();
        auto _end=lat.data_begin()+(k+1)*lat.width()*lat.height();
        bool flag=true;
        while(flag){
            for(auto it=_start;it<_end;++it){
                if(uni()<_prob){
                    *it=uni2();
                }
                else
                    *it=0;

            }
            if(need_ranked){
                if(LSQR){
                    auto _SVD=lat.tab_matrix(k).jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
                    if(!_SVD.nonzeroSingularValues()!=lat.width())
                        flag=false;
                }
                else{
                    auto _QR=lat.tab_matrix(k).colPivHouseholderQr();
                    if(_QR.rank()==lat.width())
                        flag=false;
                }
            }
            else
                flag=false;

        }
    }
}

template<typename data_type>
void solvework(size_t m1, size_t n1, size_t n2, size_t p){

    typedef LibMIA::DenseLattice<data_type> denseType;
    typedef LibMIA::SparseLattice<data_type> sparseType;

    denseType DenseLat1(m1,n1,p);
    denseType DenseLat1_lsqr(2*m1,n1,p);
    denseType DenseLat2(m1,n2,p);

    denseType DenseLat3;
    denseType DenseLat3_mixed;

    sparseType SparseLat1;
    sparseType SparseLat2;
    sparseType SparseLat1_lsqr;
    sparseType SparseLat3;

    random_matrix(DenseLat1,0.4);
    random_matrix(DenseLat1_lsqr,0.4,false,true);
    random_matrix(DenseLat2,0.4,false);

    SparseLat1=DenseLat1;
    SparseLat2=DenseLat2;
    SparseLat1_lsqr=DenseLat1_lsqr;

    DenseLat3=DenseLat1.solve(DenseLat2);
    DenseLat3_mixed=DenseLat1.solve(SparseLat2);

    BOOST_CHECK_MESSAGE(DenseLat3.fuzzy_equals(DenseLat3_mixed,test_precision<data_type>()),std::string("Full Dimension Solve Test 1 for ")+typeid(data_type).name());
    //now invert the sparse version instead

    DenseLat3_mixed=SparseLat1.solve(DenseLat2);

    BOOST_CHECK_MESSAGE(DenseLat3.fuzzy_equals(DenseLat3_mixed,test_precision<data_type>()),std::string("Full Dimension Solve Test 2 for ")+typeid(data_type).name());

    DenseLat2=denseType(2*m1,n2,p);
    random_matrix(DenseLat2,0.4,false);
    SparseLat2=DenseLat2;

    DenseLat3=DenseLat1_lsqr.solve(DenseLat2);
    DenseLat3_mixed=DenseLat1_lsqr.solve(SparseLat2);

    BOOST_CHECK_MESSAGE(DenseLat3.fuzzy_equals(DenseLat3_mixed,test_precision<data_type>()),std::string("LSQR Solve Test 1 for ")+typeid(data_type).name());

    DenseLat3_mixed=SparseLat1_lsqr.solve(DenseLat2);
    BOOST_CHECK_MESSAGE(DenseLat3.fuzzy_equals(DenseLat3_mixed,test_precision<data_type>()),std::string("LSQR Solve Test 2 for ")+typeid(data_type).name());

}

BOOST_AUTO_TEST_CASE( MixedLatticeSolveTests )
{

    //multwork<double>(3,3,3,3);
    //solvework<double>(5,5,5,5);
    //solvework<double>(20,20,10,20);
    solvework<float>(20,20,10,20);

}


// Copyright Nick Thompson, 2017
// 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_TEST_MODULE barycentric_rational

#include <cmath>
#include <random>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/type_index.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/math/interpolators/barycentric_rational.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

#ifdef BOOST_HAS_FLOAT128
#include <boost/multiprecision/float128.hpp>
#endif

using std::sqrt;
using std::abs;
using std::numeric_limits;
using boost::multiprecision::cpp_bin_float_50;

template<class Real>
void test_interpolation_condition()
{
    std::cout << "Testing interpolation condition for barycentric interpolation on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    std::mt19937 gen(4);
    boost::random::uniform_real_distribution<Real> dis(0.1f, 1);
    std::vector<Real> x(500);
    std::vector<Real> y(500);
    x[0] = dis(gen);
    y[0] = dis(gen);
    for (size_t i = 1; i < x.size(); ++i)
    {
        x[i] = x[i-1] + dis(gen);
        y[i] = dis(gen);
    }

    boost::math::barycentric_rational<Real> interpolator(x.data(), y.data(), y.size());

    for (size_t i = 0; i < x.size(); ++i)
    {
        Real z = interpolator(x[i]);
        BOOST_CHECK_CLOSE(z, y[i], 100*numeric_limits<Real>::epsilon());
    }
}

template<class Real>
void test_interpolation_condition_high_order()
{
    std::cout << "Testing interpolation condition in high order for barycentric interpolation on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    std::mt19937 gen(5);
    boost::random::uniform_real_distribution<Real> dis(0.1f, 1);
    std::vector<Real> x(500);
    std::vector<Real> y(500);
    x[0] = dis(gen);
    y[0] = dis(gen);
    for (size_t i = 1; i < x.size(); ++i)
    {
        x[i] = x[i-1] + dis(gen);
        y[i] = dis(gen);
    }

    // Order 5 approximation:
    boost::math::barycentric_rational<Real> interpolator(x.data(), y.data(), y.size(), 5);

    for (size_t i = 0; i < x.size(); ++i)
    {
        Real z = interpolator(x[i]);
        BOOST_CHECK_CLOSE(z, y[i], 100*numeric_limits<Real>::epsilon());
    }
}

template<class Real>
void test_constant()
{
    std::cout << "Testing that constants are interpolated correctly using barycentric interpolation on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";

    std::mt19937 gen(6);
    boost::random::uniform_real_distribution<Real> dis(0.1f, 1);
    std::vector<Real> x(500);
    std::vector<Real> y(500);
    Real constant = -8;
    x[0] = dis(gen);
    y[0] = constant;
    for (size_t i = 1; i < x.size(); ++i)
    {
        x[i] = x[i-1] + dis(gen);
        y[i] = y[0];
    }

    boost::math::barycentric_rational<Real> interpolator(x.data(), y.data(), y.size());

    for (size_t i = 0; i < x.size(); ++i)
    {
        // Don't evaluate the constant at x[i]; that's already tested in the interpolation condition test.
        Real t = x[i] + dis(gen);
        Real z = interpolator(t);
        BOOST_CHECK_CLOSE(z, constant, 100*sqrt(numeric_limits<Real>::epsilon()));
        BOOST_CHECK_SMALL(interpolator.prime(t), sqrt(numeric_limits<Real>::epsilon()));
    }
}

template<class Real>
void test_constant_high_order()
{
    std::cout << "Testing that constants are interpolated correctly in high order using barycentric interpolation on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";

    std::mt19937 gen(7);
    boost::random::uniform_real_distribution<Real> dis(0.1f, 1);
    std::vector<Real> x(500);
    std::vector<Real> y(500);
    Real constant = 5;
    x[0] = dis(gen);
    y[0] = constant;
    for (size_t i = 1; i < x.size(); ++i)
    {
        x[i] = x[i-1] + dis(gen);
        y[i] = y[0];
    }

    // Set interpolation order to 7:
    boost::math::barycentric_rational<Real> interpolator(x.data(), y.data(), y.size(), 7);

    for (size_t i = 0; i < x.size(); ++i)
    {
        Real t = x[i] + dis(gen);
        Real z = interpolator(t);
        BOOST_CHECK_CLOSE(z, constant, 1000*sqrt(numeric_limits<Real>::epsilon()));
        BOOST_CHECK_SMALL(interpolator.prime(t), 100*sqrt(numeric_limits<Real>::epsilon()));
    }
}

template<class Real>
void test_runge()
{
    std::cout << "Testing interpolation of Runge's 1/(1+25x^2) function using barycentric interpolation on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";

    std::mt19937 gen(8);
    boost::random::uniform_real_distribution<Real> dis(0.005f, 0.01f);
    std::vector<Real> x(500);
    std::vector<Real> y(500);
    x[0] = -2;
    y[0] = 1/(1+25*x[0]*x[0]);
    for (size_t i = 1; i < x.size(); ++i)
    {
        x[i] = x[i-1] + dis(gen);
        y[i] = 1/(1+25*x[i]*x[i]);
    }

    boost::math::barycentric_rational<Real> interpolator(x.data(), y.data(), y.size(), 5);

    for (size_t i = 0; i < x.size(); ++i)
    {
        Real t = x[i];
        Real z = interpolator(t);
        BOOST_CHECK_CLOSE(z, y[i], 0.03);
        Real z_prime = interpolator.prime(t);
        Real num = -50*t;
        Real denom = (1+25*t*t)*(1+25*t*t);
        if (abs(num/denom) > 0.00001)
        {
            BOOST_CHECK_CLOSE_FRACTION(z_prime, num/denom, 0.03);
        }
    }

    Real tol = 0.0001;
    for (size_t i = 0; i < x.size(); ++i)
    {
        Real t = x[i] + dis(gen);
        Real z = interpolator(t);
        BOOST_CHECK_CLOSE(z, 1/(1+25*t*t), tol);
        Real z_prime = interpolator.prime(t);
        Real num = -50*t;
        Real denom = (1+25*t*t)*(1+25*t*t);
        Real runge_prime = num/denom;

        if (abs(runge_prime) > 0 && abs(z_prime - runge_prime)/abs(runge_prime) > tol)
        {
            std::cout << "Error too high for t = " << t << " which is a distance " << t - x[i] << " from node " << i << "/" << x.size() << " associated with data (" << x[i] << ", " << y[i] << ")\n";
            BOOST_CHECK_CLOSE_FRACTION(z_prime, runge_prime, tol);
        }
    }
}

template<class Real>
void test_weights()
{
    std::cout << "Testing weights are calculated correctly using barycentric interpolation on type " << boost::typeindex::type_id<Real>().pretty_name() << "\n";

    std::mt19937 gen(9);
    boost::random::uniform_real_distribution<Real> dis(0.005, 0.01);
    std::vector<Real> x(500);
    std::vector<Real> y(500);
    x[0] = -2;
    y[0] = 1/(1+25*x[0]*x[0]);
    for (size_t i = 1; i < x.size(); ++i)
    {
        x[i] = x[i-1] + dis(gen);
        y[i] = 1/(1+25*x[i]*x[i]);
    }

    boost::math::detail::barycentric_rational_imp<Real> interpolator(x.data(), x.data() + x.size(), y.data(), 0);

    for (size_t i = 0; i < x.size(); ++i)
    {
        Real w = interpolator.weight(i);
        if (i % 2 == 0)
        {
            BOOST_CHECK_CLOSE(w, 1, 0.00001);
        }
        else
        {
            BOOST_CHECK_CLOSE(w, -1, 0.00001);
        }
    }

    // d = 1:
    interpolator = boost::math::detail::barycentric_rational_imp<Real>(x.data(), x.data() + x.size(), y.data(), 1);

    for (size_t i = 1; i < x.size() -1; ++i)
    {
        Real w = interpolator.weight(i);
        Real w_expect = 1/(x[i] - x[i - 1]) + 1/(x[i+1] - x[i]);
        if (i % 2 == 0)
        {
            BOOST_CHECK_CLOSE(w, -w_expect, 0.00001);
        }
        else
        {
            BOOST_CHECK_CLOSE(w, w_expect, 0.00001);
        }
    }

}

BOOST_AUTO_TEST_CASE(barycentric_rational)
{
    test_weights<double>();
    test_constant<float>();
    test_constant<double>();
    test_constant<long double>();
    test_constant<cpp_bin_float_50>();

    test_constant_high_order<float>();
    test_constant_high_order<double>();
    test_constant_high_order<long double>();
    test_constant_high_order<cpp_bin_float_50>();

    test_interpolation_condition<float>();
    test_interpolation_condition<double>();
    test_interpolation_condition<long double>();
    test_interpolation_condition<cpp_bin_float_50>();

    test_interpolation_condition_high_order<float>();
    test_interpolation_condition_high_order<double>();
    test_interpolation_condition_high_order<long double>();
    test_interpolation_condition_high_order<cpp_bin_float_50>();

    test_runge<double>();
    test_runge<long double>();
    test_runge<cpp_bin_float_50>();

#ifdef BOOST_HAS_FLOAT128
    test_interpolation_condition<boost::multiprecision::float128>();
        test_constant<boost::multiprecision::float128>();
        test_constant_high_order<boost::multiprecision::float128>();
        test_interpolation_condition_high_order<boost::multiprecision::float128>();
        test_runge<boost::multiprecision::float128>();
#endif

}