#include <vector>
#include <list>
#include <string>
#include <iostream>
#include <iomanip>
#include <sstream>
#include <algorithm>
#include <iterator>

#include <boost/cstdlib.hpp>

double distance(const std::vector<std::size_t> &lhs, const std::vector<std::size_t> &rhs, const std::vector<std::vector<double>> &distances)
{
  double sum_d = 0.0;
  std::size_t n = 0;
  for(std::size_t x : lhs) {
    for(std::size_t y : rhs) {
      //std::cerr << "distances[" << x << "][" << y << "]=" << distances.at(x).at(y) << std::endl;
      sum_d += distances.at(x).at(y);
      ++n;
    }
  }
  //std::cerr << "sum_d=" << sum_d << std::endl;
  return sum_d / n;
}

int main()
{
  std::cerr.precision(3);
  //std::cerr << std::fixed;

  std::size_t n;
  std::cin >> n;
  //std::cerr << "n=" << n << std::endl;

  std::vector<std::vector<double>> distances(n, std::vector<double>(n, 0.0));
  std::string line;
  std::getline(std::cin, line);
  std::size_t l = 0;
  while(true) {
    std::getline(std::cin, line);
    if(!std::cin) {
      break;
    }

    std::istringstream linestream(line);
    for(std::size_t i = 0; i < n; ++i) {
      double x;
      linestream >> x;
      distances.at(l).at(i) = x;
    }
    ++l;
  }

  //std::cerr << "distances=[" << std::endl;
  //for(const std::vector<double> &row : distances) {
    //std::cerr << "  [ ";
    //std::copy(row.begin(), row.end(), std::ostream_iterator<double>(std::cerr, " "));
    //std::cerr << "]" << std::endl;
  //}
  //std::cerr << "]" << std::endl;

  std::list<std::vector<std::size_t>> clusters;
  typedef std::list<std::vector<std::size_t>>::iterator clusters_iterator;
  for(std::size_t i = 0; i < n; ++i) {
    clusters.push_back(std::vector<std::size_t>(1, i));
  }

  //for(const auto &cluster: clusters) {
    //std::cerr << "cluster=[ ";
    //std::copy(cluster.begin(), cluster.end(), std::ostream_iterator<std::size_t>(std::cerr, " "));
    //std::cerr << "]" << std::endl;
  //}

  while(clusters.size() > 1) {
    const clusters_iterator c_end = clusters.end();
    double min_d = std::numeric_limits<double>::max(); // minimum distance
    std::pair<clusters_iterator, clusters_iterator> min_c = std::make_pair(c_end, c_end); // indices of two clusters with minimum distance
    for(clusters_iterator c_i = clusters.begin(); c_i != c_end; ++c_i) {
      for(clusters_iterator c_j = c_i; c_j != c_end; ++c_j) {
        if(c_i == c_j) {
          continue;
        }
        double d = distance(*c_i, *c_j, distances);
        if(d <= min_d) {
          min_d = d;
          min_c = std::make_pair(c_i, c_j);
        }
      }
    }
    //std::cerr << "min_d=" << min_d << std::endl;

    //std::cerr << "min_c.first=[ ";
    //std::copy(min_c.first->begin(), min_c.first->end(), std::ostream_iterator<std::size_t>(std::cerr, " "));
    //std::cerr << "]" << std::endl;
    //std::cerr << "min_c.second=[ ";
    //std::copy(min_c.second->begin(), min_c.second->end(), std::ostream_iterator<std::size_t>(std::cerr, " "));
    //std::cerr << "]" << std::endl;

    std::copy(min_c.second->begin(), min_c.second->end(), std::back_inserter(*min_c.first));
    //std::sort(min_c.first->begin(), min_c.first->end());

    //std::cerr << "min_c.first=[ ";
    //std::copy(min_c.first->begin(), min_c.first->end(), std::ostream_iterator<std::size_t>(std::cerr, " "));
    //std::cerr << "]" << std::endl;
    //std::cerr << "min_c.second=[ ";
    //std::copy(min_c.second->begin(), min_c.second->end(), std::ostream_iterator<std::size_t>(std::cerr, " "));
    //std::cerr << "]" << std::endl;

    clusters.erase(min_c.second);

    //for(const auto &cluster: clusters) {
      //std::cerr << "cluster=[ ";
      //std::copy(cluster.begin(), cluster.end(), std::ostream_iterator<std::size_t>(std::cerr, " "));
      //std::cerr << "]" << std::endl;
    //}
    for(std::size_t i: *min_c.first) {
      std::cout << (i + 1) << " ";
    }
    std::cout << std::endl;
    //std::cout << std::endl;
  }

  return boost::exit_success;
}

// vim: set ts=2 sw=2 et:


#pragma once

#include "perceive/foundation.hpp"
#include "perceive/utils/sdbm-hash.hpp"
#include <Eigen/Core>
#include <cmath>

namespace perceive
{
// --------------------------------------------------------------------- Vector2
#pragma pack(push, 1)
template<typename T> class Vector2T
{
 public:
   using value_type = T;

   T x, y;

   Vector2T()
       : x(T(0.0))
       , y(T(0.0))
   {}
   Vector2T(T x_, T y_)
       : x(x_)
       , y(y_)
   {}
   Vector2T(const float p[2])
   {
      x = p[0];
      y = p[1];
   }
   Vector2T(const double p[2])
   {
      x = p[0];
      y = p[1];
   }

   Vector2T& operator=(const Vector2T& v) = default;

   Vector2T& operator=(const Eigen::Vector2f& v)
   {
      for(int i = 0; i < 2; ++i) this->operator[](i) = v(i);
      return *this;
   }

   static Vector2T nan() { return Vector2T(T(NAN), T(NAN)); }
   static Vector2T range()
   {
      auto mmax = std::numeric_limits<T>::max();
      auto mmin = std::numeric_limits<T>::lowest();
      return Vector2T(mmax, mmin);
   }

   void union_value(T v)
   {
      if(v < this->x) this->x = v;
      if(v > this->y) this->y = v;
   }

   unsigned size() const { return 2; }

   Vector2T& normalise(T epsilon = 1e-9)
   {
      // Don't normalize if we don't have to
      T mag2 = x * x + y * y;
      if(std::fabs(mag2 - T(1.0)) > epsilon) {
         T mag_inv = T(1.0) / std::sqrt(mag2);
         x *= mag_inv;
         y *= mag_inv;
      }
      return *this;
   }
   Vector2T normalised(T epsilon = T(1e-9)) const
   {
      Vector2T res = *this;
      res.normalise(epsilon);
      return res;
   }
   Vector2T& normalize(T epsilon = T(1e-9)) { return normalise(epsilon); }
   Vector2T normalized(T epsilon = T(1e-9)) const
   {
      return normalised(epsilon);
   }
   T quadrance() const { return x * x + y * y; }
   T norm() const { return T(std::sqrt(quadrance())); }
   T dot(const Vector2T& rhs) const { return x * rhs.x + y * rhs.y; }
   T perp_dot(const Vector2T& rhs) const { return x * rhs.y - y * rhs.x; }
   T quadrance(const Vector2T& rhs) const
   {
      return (x - rhs.x) * (x - rhs.x) + (y - rhs.y) * (y - rhs.y);
   }
   T distance(const Vector2T& rhs) const { return (*this - rhs).norm(); }

   // As polar co-ordinates
   T& mag() { return x; }
   const T& mag() const { return x; }
   T& theta() { return y; }
   const T& theta() const { return y; }

   // As format
   T& width() { return x; }
   const T& width() const { return x; }
   T& height() { return y; }
   const T& height() const { return y; }

   Vector2T& set_to(const T& a, const T& b)
   {
      x = a;
      y = b;
      return *this;
   }
   Vector2T& set_to(T a[2])
   {
      set_to(a[0], a[1]);
      return *this;
   }

   T* copy_to(T a[2]) const
   {
      a[0] = x;
      a[1] = y;
      return a;
   }

   T* ptr()
   {
#ifdef DEBUG_BUILD
      assert(&x == reinterpret_cast<const T*>(this) + 0);
      assert(&y == reinterpret_cast<const T*>(this) + 1);
#endif
      return &x;
   }
   const T* ptr() const { return const_cast<Vector2T<T>*>(this)->ptr(); }

   T& operator[](int idx)
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 2);
#endif
      return ptr()[idx];
   }
   const T& operator[](int idx) const
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 2);
#endif
      return ptr()[idx];
   }

   T& operator()(int idx)
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 2);
#endif
      return ptr()[idx];
   }

   const T& operator()(int idx) const
   {
#ifdef DEBUG_BUILD
      assert(idx >= 0 && idx < 2);
#endif
      return ptr()[idx];
   }

   Vector2T flip_x() const { return Vector2T(-x, y); }
   Vector2T flip_y() const { return Vector2T(x, -y); }
   Vector2T clockwise_90() const { return Vector2T(y, -x); }
   Vector2T counter_clockwise_90() const { return Vector2T(-y, x); }
   Vector2T rotate(T theta) const
   {
      auto sin_t = std::sin(theta);
      auto cos_t = std::cos(theta);
      return Vector2T<T>(x * cos_t - y * sin_t, x * sin_t + y * cos_t);
   }

   Vector2T round() const { return Vector2T(std::round(x), std::round(y)); }

   Vector2T& operator*=(T scalar)
   {
      x *= scalar;
      y *= scalar;
      return *this;
   }

   Vector2T& operator/=(T scalar)
   {
      x /= scalar;
      y /= scalar;
      return *this;
   }
   Vector2T operator*(T scalar) const
   {
      Vector2T res(*this);
      res *= scalar;
      return res;
   }

   Vector2T operator/(T scalar) const
   {
      Vector2T res(*this);
      res /= scalar;
      return res;
   }

   Vector2T& operator+=(const Vector2T& rhs)
   {
      x += rhs.x;
      y += rhs.y;
      return *this;
   }

   Vector2T& operator-=(const Vector2T& rhs)
   {
      x -= rhs.x;
      y -= rhs.y;
      return *this;
   }

   Vector2T operator+(const Vector2T& rhs) const
   {
      Vector2T res(*this);
      res += rhs;
      return res;
   }
   Vector2T operator-(const Vector2T& rhs) const
   {
      Vector2T res(*this);
      res -= rhs;
      return res;
   }

   Vector2T operator-() const { return Vector2T<T>(-x, -y); }

   bool operator==(const Vector2T& rhs) const
   {
      return x == rhs.x && y == rhs.y;
   }
   bool operator!=(const Vector2T& rhs) const { return !(*this == rhs); }

   bool operator<(const Vector2T& rhs) const
   {
      return x == rhs.x ? y < rhs.y : x < rhs.x;
   }
   bool operator<=(const Vector2T& rhs) const
   {
      return x == rhs.x ? y <= rhs.y : x <= rhs.x;
   }
   bool operator>(const Vector2T& rhs) const { return !(*this <= rhs); }
   bool operator>=(const Vector2T& rhs) const { return !(*this < rhs); }

   bool has_nan() const { return x != x || y != y; }
   bool is_nan() const { return std::isnan(x) || std::isnan(y); }
   bool is_finite() const { return std::isfinite(x) && std::isfinite(y); }
   bool is_unit_vector(T epsilon = 1e-9) const
   {
      return is_finite() && fabs(quadrance() - 1.0) < epsilon;
   }

   inline friend bool isfinite(const Vector2T& o) noexcept
   {
      return o.is_finite();
   }

   std::string to_string(const char* fmt = nullptr) const
   {
      if(fmt == nullptr) {
         if constexpr(std::is_floating_point<T>::value) {
            fmt = "[{:7.5f}, {:7.5f}]";
         } else {
            fmt = "[{}, {}]";
         }
      }
      return format(fmt, x, y);
   }
   std::string to_str() const { return format("[{}, {}]", x, y); }

   void print(const char* msg = NULL, bool newline = true) const
   {
      printf("%s%s%s%s",
             msg,
             (msg == NULL ? "" : " "),
             to_string().c_str(),
             (newline ? "\n" : ""));
      fflush(stdout);
   }

   size_t hash() const { return sdbm_hash(ptr(), sizeof(T) * size()); }

   friend std::string str(const Vector2T<T>& o) noexcept
   {
      return o.to_string();
   }
};
#pragma pack(pop)

// Scalar multiplication
template<typename T> Vector2T<T> operator*(float a, const Vector2T<T>& v)
{
   return v * T(a);
}
template<typename T> Vector2T<T> operator/(float a, const Vector2T<T>& v)
{
   return v / T(a);
}
template<typename T> Vector2T<T> operator*(double a, const Vector2T<T>& v)
{
   return v * T(a);
}
template<typename T> Vector2T<T> operator/(double a, const Vector2T<T>& v)
{
   return v / T(a);
}

// String shim
template<typename T> std::string str(const Vector2T<T>& v)
{
   return v.to_string();
}
template<typename T>
std::ostream& operator<<(std::ostream& out, const Vector2T<T>& v)
{
   out << v.to_string();
   return out;
}

template<typename T> inline T perp_dot(T x1, T y1, T x2, T y2)
{
   return x1 * y2 - x2 * y1;
}

} // namespace perceive


//============================================================================
// Name        : autodiff.cpp
// Author      :
// Version     :
// Copyright   : Your copyright notice
// Description : Hello World in C++, Ansi-style
//============================================================================

#include <iostream>
#include <sstream>
#include <numeric>
#include <boost/foreach.hpp>
#include "autodiff.h"
#include "Stack.h"
#include "Tape.h"
#include "BinaryOPNode.h"
#include "UaryOPNode.h"

using namespace std;

namespace AutoDiff
{

#if FORWARD_ENABLED

unsigned int num_var = 0;

void hess_forward(Node* root, unsigned int nvar, double** hess_mat)
{
	assert(nvar == num_var);
	unsigned int len = (nvar+3)*nvar/2;
	root->hess_forward(len,hess_mat);
}

#endif

PNode* create_param_node(double value){
	return new PNode(value);
}
VNode* create_var_node(double v)
{
	return new VNode(v);
}
OPNode* create_binary_op_node(OPCODE code, Node* left, Node* right)
{
	return BinaryOPNode::createBinaryOpNode(code,left,right);
}
OPNode* create_uary_op_node(OPCODE code, Node* left)
{
	return UaryOPNode::createUnaryOpNode(code,left);
}
double eval_function(Node* root)
{
	assert(SD->size()==0);
	assert(SV->size()==0);
	root->eval_function();
	assert(SV->size()==1);
	double val = SV->pop_back();
	return val;
}

double grad_reverse(Node* root,vector<Node*>& vnodes, vector<double>& grad)
{
	grad.clear();
	BOOST_FOREACH(Node* node, vnodes)
	{
		assert(node->getType()==VNode_Type);
		static_cast<VNode*>(node)->adj = NaN_Double;
	}

	assert(SD->size()==0);
	root->grad_reverse_0();
	assert(SV->size()==1);
	root->grad_reverse_1_init_adj();
	root->grad_reverse_1();
	assert(SD->size()==0);
	double val = SV->pop_back();
	assert(SV->size()==0);
	//int i=0;
	BOOST_FOREACH(Node* node, vnodes)
	{
		assert(node->getType()==VNode_Type);
		grad.push_back(static_cast<VNode*>(node)->adj);
		static_cast<VNode*>(node)->adj = NaN_Double;
	}
	assert(grad.size()==vnodes.size());
	//all nodes are VNode and adj == NaN_Double -- this reset adj for this expression tree by root
	return val;
}

double grad_reverse(Node* root, vector<Node*>& vnodes, col_compress_matrix_row& rgrad)
{
	BOOST_FOREACH(Node* node, vnodes)
	{
		assert(node->getType()==VNode_Type);
		static_cast<VNode*>(node)->adj = NaN_Double;
	}
	assert(SD->size()==0);
	root->grad_reverse_0();
	assert(SV->size()==1);
	root->grad_reverse_1_init_adj();
	root->grad_reverse_1();
	assert(SD->size()==0);
	double val = SV->pop_back();
	assert(SV->size()==0);
	unsigned int i =0;
	BOOST_FOREACH(Node* node, vnodes)
	{
		assert((node)->getType()==VNode_Type);
		double diff = static_cast<VNode*>(node)->adj;
		if(!isnan(diff)){
			rgrad(i) = diff;
			static_cast<VNode*>(node)->adj = NaN_Double;
		}
		i++;
	}
	//all nodes are VNode and adj == NaN_Double -- this reset adj for this expression tree by root
	assert(i==vnodes.size());
	return val;
}

double hess_reverse(Node* root,vector<Node*>& vnodes,vector<double>& dhess)
{
	TT->clear();
	II->clear();
	assert(TT->empty());
	assert(II->empty());
	assert(TT->index==0);
	assert(II->index==0);
	dhess.clear();

//	for(vector<Node*>::iterator it=nodes.begin();it!=nodes.end();it++)
//	{
//		assert((*it)->getType()==VNode_Type);
//		(*it)->index = 0;
//	} //this work complete in hess-reverse_0_init_index

	assert(root->n_in_arcs == 0);
	root->hess_reverse_0_init_n_in_arcs();
	assert(root->n_in_arcs == 1);
	root->hess_reverse_0();
	double val = NaN_Double;
	root->hess_reverse_get_x(TT->index,val);
//	cout<<TT->toString();
//	cout<<endl;
//	cout<<II->toString();
//	cout<<"======================================= hess_reverse_0"<<endl;
	root->hess_reverse_1_init_x_bar(TT->index);
	assert(root->n_in_arcs == 1);
	root->hess_reverse_1(TT->index);
	assert(root->n_in_arcs == 0);
	assert(II->index==0);
//	cout<<TT->toString();
//	cout<<endl;
//	cout<<II->toString();
//	cout<<"======================================= hess_reverse_1"<<endl;

	for(vector<Node*>::iterator it=vnodes.begin();it!=vnodes.end();it++)
	{
		assert((*it)->getType()==VNode_Type);
		dhess.push_back(TT->get((*it)->index-1));
	}

	TT->clear();
	II->clear();
	root->hess_reverse_1_clear_index();
	return val;
}

double hess_reverse(Node* root,vector<Node*>& vnodes,col_compress_matrix_col& chess)
{
	TT->clear();
	II->clear();
	assert(TT->empty());
	assert(II->empty());
	assert(TT->index==0);
	assert(II->index==0);

//	for(vector<Node*>::iterator it=nodes.begin();it!=nodes.end();it++)
//	{
//		assert((*it)->getType()==VNode_Type);
//		(*it)->index = 0;
//	} //this work complete in hess-reverse_0_init_index

	assert(root->n_in_arcs == 0);
	//reset node index and n_in_arcs - for the Tape location
	root->hess_reverse_0_init_n_in_arcs();
	assert(root->n_in_arcs == 1);
	root->hess_reverse_0();
	double val = NaN_Double;
	root->hess_reverse_get_x(TT->index,val);
//	cout<<TT->toString();
//	cout<<endl;
//	cout<<II->toString();
//	cout<<"======================================= hess_reverse_0"<<endl;
	root->hess_reverse_1_init_x_bar(TT->index);
	assert(root->n_in_arcs == 1);
	root->hess_reverse_1(TT->index);
	assert(root->n_in_arcs == 0);
	assert(II->index==0);
//	cout<<TT->toString();
//	cout<<endl;
//	cout<<II->toString();
//	cout<<"======================================= hess_reverse_1"<<endl;

	unsigned int i =0;
	BOOST_FOREACH(Node* node, vnodes)
	{
		assert(node->getType() == VNode_Type);
		//node->index = 0 means this VNode is not in the tree
		if(node->index!=0)
		{
			double hess = TT->get(node->index -1);
			if(!isnan(hess))
			{
				chess(i) = chess(i) + hess;
			}
		}
		i++;
	}
	assert(i==vnodes.size());
	root->hess_reverse_1_clear_index();
	TT->clear();
	II->clear();
	return val;
}

unsigned int nzGrad(Node* root)
{
	unsigned int nzgrad,total = 0;
	boost::unordered_set<Node*> nodes;
	root->collect_vnodes(nodes,total);
	nzgrad = nodes.size();
	return nzgrad;
}

/*
 * number of non-zero gradient in constraint tree root that also belong to vSet
 */
unsigned int nzGrad(Node* root, boost::unordered_set<Node*>& vSet)
{
	unsigned int nzgrad=0, total=0;
	boost::unordered_set<Node*> vnodes;
	root->collect_vnodes(vnodes,total);
	//cout<<"nzGrad - vnodes size["<<vnodes.size()<<"] -- total node["<<total<<"]"<<endl;
	for(boost::unordered_set<Node*>::iterator it=vnodes.begin();it!=vnodes.end();it++)
	{
		Node* n = *it;
		if(vSet.find(n) != vSet.end())
		{
			nzgrad++;
		}
	}
	return nzgrad;
}

void nonlinearEdges(Node* root, EdgeSet& edges)
{
	root->nonlinearEdges(edges);
}

unsigned int nzHess(EdgeSet& eSet,boost::unordered_set<Node*>& set1, boost::unordered_set<Node*>& set2)
{
	list<Edge>::iterator i = eSet.edges.begin();
	for(;i!=eSet.edges.end();)
	{
		Edge e =*i;
		Node* a = e.a;
		Node* b = e.b;
		if((set1.find(a)!=set1.end() && set2.find(b)!=set2.end())
			||
			(set1.find(b)!=set1.end() && set2.find(a)!=set2.end()))
		{
			//e is connected between set1 and set2
			i++;
		}
		else
		{
			i = eSet.edges.erase(i);
		}
	}
	unsigned int diag=eSet.numSelfEdges();
	unsigned int nzHess = (eSet.size())*2 - diag;
	return nzHess;
}

unsigned int nzHess(EdgeSet& edges)
{
	unsigned int diag=edges.numSelfEdges();
	unsigned int nzHess = (edges.size())*2 - diag;
	return nzHess;
}

unsigned int numTotalNodes(Node* root)
{
	unsigned int total = 0;
	boost::unordered_set<Node*> nodes;
	root->collect_vnodes(nodes,total);
	return total;
}

string tree_expr(Node* root)
{
	ostringstream oss;
	oss<<"visiting tree == "<<endl;
	int level = 0;
	root->inorder_visit(level,oss);
	return oss.str();
}

void print_tree(Node* root)
{
	cout<<"visiting tree == "<<endl;
	int level = 0;
	root->inorder_visit(level,cout);
}

void autodiff_setup()
{
	Stack::diff = new Stack();
	Stack::vals = new Stack();
	Tape<unsigned int>::indexTape = new Tape<unsigned int>();
	Tape<double>::valueTape = new Tape<double>();
}

void autodiff_cleanup()
{
	delete Stack::diff;
	delete Stack::vals;
	delete Tape<unsigned int>::indexTape;
	delete Tape<double>::valueTape;
}

} //AutoDiff namespace end


/*
   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.
*/

// Critical temperature of hexagonal lattice Ising model

#include <iomanip>
#include <iostream>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include "ising/mp_wrapper.hpp"
#include "options.hpp"
#include "hexagonal.hpp"

template<class T>
void calc(const std::string& Ja_in, const std::string& Jb_in, const std::string& Jc_in) {
  typedef T real_t;
  real_t Ja = convert<real_t>(Ja_in);
  real_t Jb = convert<real_t>(Jb_in);
  real_t Jc = convert<real_t>(Jc_in);
  auto tc = ising::tc::hexagonal(Ja, Jb, Jc);
  std::cout << std::scientific << std::setprecision(std::numeric_limits<real_t>::digits10)
            << "# lattice: hexagonal\n"
            << "# precision: " << std::numeric_limits<real_t>::digits10 << std::endl
            << "# Ja Jb Jc Tc 1/Tc" << std::endl
            << Ja << ' ' << Jb << ' ' << Jc << ' ' << tc << ' ' << (1 / tc) << std::endl;
}

int main(int argc, char **argv) {
  namespace mp = boost::multiprecision;
  options3 opt(argc, argv);
  if (!opt.valid) return 127;
  if (opt.prec <= std::numeric_limits<float>::digits10) {
    calc<float>(opt.Ja, opt.Jb, opt.Jc);
  } else if (opt.prec <= std::numeric_limits<double>::digits10) {
    calc<double>(opt.Ja, opt.Jb, opt.Jc);
  } else if (opt.prec <= std::numeric_limits<mp_wrapper<mp::cpp_dec_float_50>>::digits10) {
    calc<mp_wrapper<mp::cpp_dec_float_50>>(opt.Ja, opt.Jb, opt.Jc);
  } else if (opt.prec <= std::numeric_limits<mp_wrapper<mp::cpp_dec_float_100>>::digits10) {
    calc<mp_wrapper<mp::cpp_dec_float_100>>(opt.Ja, opt.Jb, opt.Jc);
  } else {
    std::cerr << "Error: Required precision is too high\n"; return 127;
  }
}


//
// $Id: Parabola.cpp 2051 2010-06-15 18:39:13Z chambm $
//
//
// Original author: Darren Kessner <darren@proteowizard.org>
//
// Copyright 2006 Louis Warschaw Prostate Cancer Center
//   Cedars Sinai Medical Center, Los Angeles, California  90048
//
// 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.
//

#define PWIZ_SOURCE

#include "Parabola.hpp"
#include "pwiz/utility/misc/Std.hpp"

#ifndef NDEBUG
#define NDEBUG
#endif // NDEBUG
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
namespace ublas = boost::numeric::ublas;

#if BOOST_UBLAS_TYPE_CHECK
goo; // need -DNDEBUG in makefile!
#endif

namespace pwiz {
namespace math {

PWIZ_API_DECL Parabola::Parabola(double a, double b, double c)
:   a_(3)
{
    a_[0] = a;
    a_[1] = b;
    a_[2] = c;
}

PWIZ_API_DECL Parabola::Parabola(vector<double> a)
:   a_(a)
{
    if (a_.size() != 3)
        throw logic_error("[Parabola::Parabola()] 3 coefficients required.");
}

namespace {

void solve(ublas::matrix<double>& A, ublas::vector<double>& a)
{
    ublas::permutation_matrix<size_t> pm(3);
    int singular = lu_factorize(A, pm);
    if (singular)
        throw runtime_error("[Parabola.cpp::solve()] Matrix is singular.");

    lu_substitute(A, pm, a); // may cause assertion without NDEBUG defined for ublas, probably due to roundoff errors
}

void fitExact(const vector< pair<double,double> >& samples, vector<double>& coefficients)
{
    if (samples.size() != 3)
        throw logic_error("[Parabola.cpp::fitExact()] Exactly 3 samples required.\n");

    // fit parabola to the 3 samples (xi,yi):
    //
    //   ( x1^2  x1  1 )( a[0] )   ( y1 )
    //   ( x2^2  x2  1 )( a[1] ) = ( y2 )
    //   ( x3^2  x3  1 )( a[2] )   ( y3 )

    ublas::matrix<double> A(3,3);
    ublas::vector<double> a(3);

    for (int i=0; i<3; i++)
    {
        double x = samples[i].first;
        double y = samples[i].second;
        A(i,0) = x*x;
        A(i,1) = x;
        A(i,2) = 1;
        a(i) = y;
    }

    solve(A, a);
    copy(a.begin(), a.end(), coefficients.begin());
}

void fitWeightedLeastSquares(const vector< pair<double,double> >& samples,
                             const vector<double>& weights,
                             vector<double>& coefficients)
{
    if (samples.size() != weights.size())
        throw logic_error("[Parabola.cpp::fitWeightedLeastSquares] Wrong weight count.");

    // given samples {(xi,yi)} and weights {wi}
    // minimize e(a) = sum[wi(p(a,xi)-yi)^2],
    // where p(a,xi) = a[0](xi)^2 + a[1](xi) + a[0]c
    //
    // de/da == 0 =>
    //   ( sum_wx4  sum_wx3  sum_wx2 )( a[0] )   ( sum_wyx2 )
    //   ( sum_wx3  sum_wx2  sum_wx1 )( a[1] ) = ( sum_wyx1 )
    //   ( sum_wx2  sum_wx1  sum_wx0 )( a[2] )   ( sum_wyx0 )
    //
    // where:
    //   sum_wxn means sum[wi*(xi)^n]
    //   sum_wyxn means sum[wi*yi*(xi)^n]

    double sum_wx4 = 0;
    double sum_wx3 = 0;
    double sum_wx2 = 0;
    double sum_wx1 = 0;
    double sum_wx0 = 0;
    double sum_wyx2 = 0;
    double sum_wyx1 = 0;
    double sum_wyx0 = 0;

    for (unsigned int i=0; i<samples.size(); i++)
    {
        double x = samples[i].first;
        double y = samples[i].second;
        double w = weights[i];

        sum_wx4 += w*pow(x,4);
        sum_wx3 += w*pow(x,3);
        sum_wx2 += w*x*x;
        sum_wx1 += w*x;
        sum_wx0 += w;
        sum_wyx2 += w*y*x*x;
        sum_wyx1 += w*y*x;
        sum_wyx0 += w*y;
    }

    ublas::matrix<double> A(3,3);
    ublas::vector<double> a(3);

    A(0,0) = sum_wx4;
    A(1,0) = A(0,1) = sum_wx3;
    A(2,0) = A(1,1) = A(0,2) = sum_wx2;
    A(1,2) = A(2,1) = sum_wx1;
    A(2,2) = sum_wx0;

    a(0) = sum_wyx2;
    a(1) = sum_wyx1;
    a(2) = sum_wyx0;

    solve(A, a);
    copy(a.begin(), a.end(), coefficients.begin());
}

} // namespace

PWIZ_API_DECL Parabola::Parabola(const vector< pair<double,double> >& samples)
:   a_(3)
{
    if (samples.size() < 3)
        throw logic_error("[Parabola::Parabola()] At least 3 samples required.");

    if (samples.size() == 3)
        fitExact(samples, a_);
    else
        fitWeightedLeastSquares(samples, vector<double>(samples.size(),1), a_);
}

// construct by weighted least squares
PWIZ_API_DECL
Parabola::Parabola(const std::vector< std::pair<double,double> >& samples,
                   const std::vector<double>& weights)
:   a_(3)
{
    fitWeightedLeastSquares(samples, weights, a_);
}

PWIZ_API_DECL ostream& operator<<(ostream& os, const Parabola& p)
{
    vector<double> a = p.coefficients();
    os << "[Parabola (" << a[0] << ", " << a[1] << ", " << a[2] << ")]";
    return os;
}

} // namespace math
} // namespace pwiz


#include "../Galerkin.hpp"
#include <iostream>

using namespace Galerkin;
using namespace Elements;
using namespace Rationals;
using namespace Polynomials;

#include <Eigen/Cholesky>
#include <Eigen/Core>

constexpr std::array<double, 5> scalings = {0.5, 1, 0.5, 0.25, 0.25};

constexpr std::array<double, 5> offsets = { 0.5, 2, 3.5, 4.25, 4.75 };

constexpr std::array<C1IntervalElement<double>, 5> elements = {
    C1IntervalElement(scalings[0], offsets[0]), C1IntervalElement(scalings[1], offsets[1]),
    C1IntervalElement(scalings[2], offsets[2]), C1IntervalElement(scalings[3], offsets[3]),
    C1IntervalElement(scalings[4], offsets[4])};

constexpr std::array<std::array<int, 4>, 5> dof_numbers = {
    std::array{0, 1, 2, 3}, std::array{3, 2, 4, 5}, std::array{5, 4, 6, 7}, std::array{7, 6, 8, 9},
    std::array{9, 8, 10, 11}};

constexpr std::size_t num_dofs = 12;

constexpr auto el_stiffness(const C1IntervalElement<double> &el)
{
    auto form = [&](auto f, auto g) { return el.template partial<0>(f) * el.template partial<0>(g); };

    return el.form_matrix(form);
}

Eigen::MatrixXd stiffness_matrix()
{
    Eigen::MatrixXd K = Eigen::MatrixXd::Zero(num_dofs, num_dofs);
    for (int i = 0; i < 5; ++i)
    {
        auto Ke = el_stiffness(elements[i]);
        for (int j = 0; j < 4; ++j)
        {
            for (int k = 0; k < 4; ++k)
            {
                K(dof_numbers[i][j], dof_numbers[i][k]) += Ke(j, k);
            }
        }
    }
    return K;
}

static_assert(elements[0].coordinate_map()(std::tuple(-1))[0] == 0);
static_assert(elements[0].coordinate_map()(std::tuple(1))[0] == 1);

constexpr auto u = make_poly(
    std::tuple(rational<3, 8>, rational<-47, 12>, rational<97, 8>, -rational<115, 12>),
    PowersList<Powers<4>, Powers<3>, Powers<2>, Powers<1>>{});

constexpr auto f = u.template partial<0>().template partial<0>();

Eigen::VectorXd forcing_vector()
{
    Eigen::VectorXd F = Eigen::VectorXd::Zero(num_dofs);
    for (int i = 0; i < 5; ++i)
    {
        const auto &el = elements[i];
        for (int j = 0; j < 4; ++j)
        {
            auto rule = el.coordinate_map().template quadrature_rule<6>();
            double detj = el.coordinate_map().detJ()();
            auto g = [&](auto xi) {
                return detj * f(el.coordinate_map()(std::tuple(xi))) * el.basis()[j](xi);
            };
            F[dof_numbers[i][j]] -= Quadrature::integrate(g, rule);
        }
    }

    return F;
}

void print_polynomial(const Polynomial<double, Powers<0>, Powers<1>, Powers<2>, Powers<3>> &p)
{
    printf("%f * x^3 + %f * x^2 + %f * x + %f", p.coeffs()[3], p.coeffs()[2], p.coeffs()[1], p.coeffs()[0]);
}

void eliminate_dirichlet_bc(Eigen::MatrixXd &K, Eigen::VectorXd &F, int which)
{
    for (int i = 0; i < K.rows(); ++i)
    {
        K(i, which) = 0;
        K(which, i) = 0;
    }
    K(which, which) = 1.0;
    F(which) = 0.0;
}

int main()
{
    auto K = stiffness_matrix();
    auto F = forcing_vector();
    eliminate_dirichlet_bc(K, F, 1);
    eliminate_dirichlet_bc(K, F, num_dofs - 2);
    std::cout << "K = " << K << '\n';
    std::cout << "F = " << F << '\n';
    Eigen::VectorXd U = K.ldlt().solve(F);

    for (int i = 0; i < 5; ++i)
    {
        const auto &el = elements[i];
        printf("Solution on element %d: ", i);
        auto p = 0.0 * el.basis()[0];
        for (int j = 0; j < 4; ++j)
        {
            p += el.basis()[j] * U[dof_numbers[i][j]];
        }
        print_polynomial(p);
        printf("\n");
    }

    return 0;
}


#include "cmath"
#include "softmax.h"
#include <ctime>
#include <iostream>
#include <vector>
#include <map>
#include <algorithm>
#include <fstream>

#include <Eigen/Dense>

using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::VectorXi;
using Eigen::MatrixXi;
using Eigen::ArrayXXd;

namespace lrprox {

  SOFTMAX::SOFTMAX(int num_dims, int num_classes)
    : num_dims_(num_dims), num_classes_(num_classes) {
    initWeight_();
  }

  double SOFTMAX::cost(const MatrixXd &X, const MatrixXi &Y) {
    // X: n * num_dims, y: n * num_classes
    // minimize negative log prob
    ArrayXXd term1 = (X * weight_).array();
    // do not take average
    return ((logsumexp(term1.matrix()).replicate(1, term1.cols()).array() - term1) * Y.cast<double>().array()).sum();
  }

  MatrixXd SOFTMAX::grad(const MatrixXd &X, const MatrixXi &Y) {
    MatrixXd term1 = (X * weight_).array().exp().matrix();
    ArrayXXd P = term1.array() / term1.rowwise().sum().replicate(1, term1.cols()).array();

    return X.transpose() * (P - Y.cast<double>().array()).matrix();
//    return weight_;
  }
//
//  VectorXi predict(const MatrixXd &X);
//
  const MatrixXd& SOFTMAX::getWeight() {
    return weight_;
  }
  void SOFTMAX::updateWeight(const std::vector<double>& weight) {
    for (int i = 0; i < weight_.cols(); i++) {
      weight_.col(i) = VectorXd::Map(&weight[i*weight_.rows()], weight_.rows());
    }
  }
  void SOFTMAX::updateWeight(const MatrixXd& weight) {
    weight_ = weight;
  }

  void SOFTMAX::outputWeight(std::vector<double>& weight) {
    weight.resize(weight_.rows() * weight_.cols());
    for (int i = 0; i < weight_.cols(); i++) {
       VectorXd::Map(&weight[i*weight_.rows()], weight_.rows()) = weight_.col(i);
    }
  }

  MatrixXi SOFTMAX::onehot_encoder(const VectorXi &y) {
    std::vector<int> labels;
    std::map<int, int> label_to_idx;
    for (int i = 0; i < y.size(); i++) {
      if (label_to_idx.count(y(i)) == 0) {
        label_to_idx[y(i)] = 1;
        labels.push_back(y(i));
      }
    }
    std::sort(labels.begin(), labels.end());
    for (int i = 0; i < labels.size(); i++) {
      label_to_idx[labels[i]] = i;
    }
    MatrixXi Y = MatrixXi::Zero(y.size(), labels.size());
    for (int i = 0; i < y.size(); i++) {
      Y(i, label_to_idx[y(i)]) = 1;
    }
    return Y;
  }

//
//  bool saveModel(std::string &filename);

  void SOFTMAX::initWeight_() {
//    weight_ = MatrixXd::Random(num_dims_, num_classes_);
    weight_ = MatrixXd::Ones(num_dims_, num_classes_);
  }

  MatrixXd SOFTMAX::logsumexp(const MatrixXd &X) {
    MatrixXd max_X = X.rowwise().maxCoeff();
    return max_X + (X - max_X.replicate(1, X.cols())).array().exp().matrix().rowwise().sum().array().log().matrix();
  }

//bool LR::SaveModel(std::string& filename) {
//  std::ofstream fout(filename.c_str());
//  fout << num_feature_dim_ << std::endl;
//  for (int i = 0; i < num_feature_dim_; ++i) {
//    fout << weight_[i] << ' ';
//  }
//  fout << std::endl;
//  fout.close();
//  return true;
//}

//std::string LR::DebugInfo() {
//  std::ostringstream out;
//  for (size_t i = 0; i < weight_.size(); ++i) {
//    out << weight_[i] << " ";
//  }
//  return out.str();
//}

//  float LR::Sigmoid_(std::vector<float> feature) {
//    float z = 0;
//    for (size_t j = 0; j < weight_.size(); ++j) {
//      z += weight_[j] * feature[j];
//    }
//    return 1. / (1. + exp(-z));
//  }

} // namespace lrprox


#include "stdafx.h"
#include "CppUnitTest.h"
#include <vector>
#include <algorithm>
#include <list>
#include "../../../iterators.hpp"
#include "../../../math/root.hpp"
#include "../../../math/axis.hpp"
#include "../../../math/multiaxis.hpp"
#include "../../../math/combinatorics.hpp"
#include <boost/math/distributions/normal.hpp>
using namespace Microsoft::VisualStudio::CppUnitTestFramework;

namespace hmLib {
	TEST_CLASS(test_math_root) {
	public:
		TEST_METHOD(root_finding_toms748) {
		auto f = [](double x) {return (x - 0.3)*(x - 0.4549)*(x - 0.8991); };

		std::vector<double> Ans;
		hmLib::math::stable_root_toms748(f, 0.0, 1.0, 0.001, 1e-5, hmLib::back_inserter(Ans));

		Assert::AreEqual(1u, Ans.size(), L"Ans Num Error");
//		Assert::AreEqual(0.3, Ans.at(0), 1e-5, L"Ans Num Error");
		Assert::AreEqual(0.4549, Ans.at(0), 1e-5, L"Ans Num Error");
//		Assert::AreEqual(0.8991, Ans.at(2), 1e-5, L"Ans Num Error");
		}
		TEST_METHOD(root_finding_bisect) {
		auto f = [](double x) {return (x - 0.3)*(x - 0.4549)*(x - 0.8991); };

		std::vector<double> Ans;
		hmLib::math::bisect_root_stepper<double> Stepper(0.001, 1e-5);
		hmLib::math::stable_root(Stepper, f, 0.0, 1.0, hmLib::back_inserter(Ans));

		Assert::AreEqual(1u, Ans.size(), L"Ans Num Error");
//		Assert::AreEqual(0.3, Ans.at(0), 1e-5, L"Ans Num Error");
		Assert::AreEqual(0.4549, Ans.at(0), 1e-5, L"Ans Num Error");
//		Assert::AreEqual(0.8991, Ans.at(2), 1e-5, L"Ans Num Error");
		}
	};
	TEST_CLASS(test_math_axis) {
		TEST_METHOD(make_range_axis_with_borders) {
			auto Axis1 = make_range_axis(0.0, 10.0, 11);
			Assert::AreEqual(11u, Axis1.size());
			Assert::AreEqual( 0.0, Axis1.lower());
			Assert::AreEqual(10.0, Axis1.upper());

			auto Axis2 = make_range_axis(0.0, 10.0, 11, math::range_axis_option::none);
			Assert::AreEqual(11u, Axis2.size());
			Assert::AreEqual(0.0, Axis2.lower());
			Assert::AreEqual(10.0, Axis2.upper());

			auto Axis3 = make_range_axis(0.0, 10.0, 9, math::range_axis_option::exclude_boundary);
			Assert::AreEqual(9u, Axis3.size());
			Assert::AreEqual(1.0, Axis3.lower());
			Assert::AreEqual(9.0, Axis3.upper());

			auto Axis4 = make_range_axis(0.0, 10.0, 10, math::range_axis_option::exclude_lower_boundary);
			Assert::AreEqual(10u, Axis4.size());
			Assert::AreEqual(1.0, Axis4.lower());
			Assert::AreEqual(10.0, Axis4.upper());

			auto Axis5 = make_range_axis(0.0, 10.0, 10, math::range_axis_option::exclude_upper_boundary);
			Assert::AreEqual(10u, Axis5.size());
			Assert::AreEqual(0.0, Axis5.lower());
			Assert::AreEqual(9.0, Axis5.upper());
		}
		TEST_METHOD(axis_grid_index) {
			{
				auto Axis = make_range_axis(0.0, 1.0, 11);
				Assert::AreEqual(0.1, Axis.interval());
				for(unsigned int i = 0; i<Axis.size(); ++i) {
					Assert::AreEqual<int>(i, Axis.index(0.1*i-0.04));
					Assert::AreEqual<int>(i, Axis.index(0.1*i+0.04));
				}
			}
			{
				auto Axis = make_range_axis(0.0, 1.0, 11, math::grid_adjuster<math::grid_policy::floor_grid_tag,-8>());
				Assert::AreEqual(0.1, Axis.interval());
				for(unsigned int i = 0; i<Axis.size(); ++i) {
					Assert::AreEqual<int>(i, Axis.index(0.1*i+0.04));
					Assert::AreEqual<int>(i, Axis.index(0.1*i+0.08));
				}
			}
			{
				auto Axis = make_range_axis(0.0, 1.0, 11, math::ceil_grid_adjuster<-8>());
				Assert::AreEqual(0.1, Axis.interval());
				for(unsigned int i = 0; i<Axis.size(); ++i) {
					Assert::AreEqual<int>(i, Axis.index(0.1*i-0.04));
					Assert::AreEqual<int>(i, Axis.index(0.1*i-0.08));
				}
			}
		}
		TEST_METHOD(axis_flort_index) {
			{
				auto Axis = make_range_axis(0.0, 1.0, 11);

				Assert::AreEqual(0.0, Axis.float_index(0.00), 1e-5);
				Assert::AreEqual(0.2, Axis.float_index(0.02), 1e-5);
				Assert::AreEqual(4.6, Axis.float_index(0.46), 1e-5);
				Assert::AreEqual(8.3, Axis.float_index(0.83), 1e-5);
			}
		}
		TEST_METHOD(axis_grid_round_range) {
			auto Axis = make_range_axis(0.0, 1.0, 11);

			Assert::AreEqual(0.1, Axis.interval());

			{
				auto Ans = Axis.weighted_index(0.30-0.04, 0.30+0.04);
				Assert::AreEqual(1u, Ans.size());
				Assert::AreEqual(0.8, Ans.volume(),1e-5);
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(1.0, Ans.at(0).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(1.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}

			{
				auto Ans = Axis.weighted_index(0.30-0.04, 0.30+0.06);
				Assert::AreEqual(2u, Ans.size());
				Assert::AreEqual(1.0, Ans.volume(),1e-5);
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(0.9 , Ans.at(0).second, 1e-5);
				Assert::AreEqual(4, Ans.at(1).first);
				Assert::AreEqual(0.1, Ans.at(1).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(0.9, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(4, (*Itr).first);
				Assert::AreEqual(0.1, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}

			{
				auto Ans = Axis.weighted_index(0.30-0.04, 0.30+0.26);
				Assert::AreEqual(4u, Ans.size());
				Assert::AreEqual(3.0, Ans.volume(), 1e-5);
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(0.9/3.0, Ans.at(0).second, 1e-5);
				Assert::AreEqual(4, Ans.at(1).first);
				Assert::AreEqual(1.0/3.0, Ans.at(1).second, 1e-5);
				Assert::AreEqual(5, Ans.at(2).first);
				Assert::AreEqual(1.0/3.0, Ans.at(2).second, 1e-5);
				Assert::AreEqual(6, Ans.at(3).first);
				Assert::AreEqual(0.1/3.0, Ans.at(3).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(0.9/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(4, (*Itr).first);
				Assert::AreEqual(1.0/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(5, (*Itr).first);
				Assert::AreEqual(1.0/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(6, (*Itr).first);
				Assert::AreEqual(0.1/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}
		}
		TEST_METHOD(axis_grid_floor_range) {
			auto Axis = make_range_axis(0.0, 1.0, 11,math::floor_grid_adjuster<-8>());

			Assert::AreEqual(0.1, Axis.interval());

			{
				auto Ans = Axis.weighted_index(0.30+0.04, 0.30+0.08);
				Assert::AreEqual(1u, Ans.size());
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(1.0, Ans.at(0).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(1.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}

			{
				auto Ans = Axis.weighted_index(0.30+0.01, 0.30+0.11);
				Assert::AreEqual(2u, Ans.size());
				Assert::AreEqual(1.0, Ans.volume(), 1e-5);
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(0.9, Ans.at(0).second, 1e-5);
				Assert::AreEqual(4, Ans.at(1).first);
				Assert::AreEqual(0.1, Ans.at(1).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(0.9, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(4, (*Itr).first);
				Assert::AreEqual(0.1, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}

			{
				auto Ans = Axis.weighted_index(0.30+0.01, 0.30+0.31);
				Assert::AreEqual(4u, Ans.size());
				Assert::AreEqual(3.0, Ans.volume(), 1e-5);
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(0.9/3.0, Ans.at(0).second, 1e-5);
				Assert::AreEqual(4, Ans.at(1).first);
				Assert::AreEqual(1.0/3.0, Ans.at(1).second, 1e-5);
				Assert::AreEqual(5, Ans.at(2).first);
				Assert::AreEqual(1.0/3.0, Ans.at(2).second, 1e-5);
				Assert::AreEqual(6, Ans.at(3).first);
				Assert::AreEqual(0.1/3.0, Ans.at(3).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(0.9/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(4, (*Itr).first);
				Assert::AreEqual(1.0/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(5, (*Itr).first);
				Assert::AreEqual(1.0/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(6, (*Itr).first);
				Assert::AreEqual(0.1/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}
		}
		TEST_METHOD(axis_grid_ceil_range) {
			auto Axis = make_range_axis(0.0, 1.0, 11, math::ceil_grid_adjuster<-8>());

			Assert::AreEqual(0.1, Axis.interval());

			{
				auto Ans = Axis.weighted_index(0.20+0.04, 0.20+0.08);
				Assert::AreEqual(1u, Ans.size());
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(1.0, Ans.at(0).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(1.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}

			{
				auto Ans = Axis.weighted_index(0.20+0.01, 0.20+0.11);
				Assert::AreEqual(2u, Ans.size());
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(0.9, Ans.at(0).second, 1e-5);
				Assert::AreEqual(4, Ans.at(1).first);
				Assert::AreEqual(0.1, Ans.at(1).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(0.9, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(4, (*Itr).first);
				Assert::AreEqual(0.1, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}

			{
				auto Ans = Axis.weighted_index(0.20+0.01, 0.20+0.31);
				Assert::AreEqual(4u, Ans.size());
				Assert::AreEqual(3.0, Ans.volume(), 1e-5);
				Assert::AreEqual(3, Ans.at(0).first);
				Assert::AreEqual(0.9/3.0, Ans.at(0).second, 1e-5);
				Assert::AreEqual(4, Ans.at(1).first);
				Assert::AreEqual(1.0/3.0, Ans.at(1).second, 1e-5);
				Assert::AreEqual(5, Ans.at(2).first);
				Assert::AreEqual(1.0/3.0, Ans.at(2).second, 1e-5);
				Assert::AreEqual(6, Ans.at(3).first);
				Assert::AreEqual(0.1/3.0, Ans.at(3).second, 1e-5);

				auto Itr = Ans.begin();
				auto End = Ans.end();
				Assert::IsFalse(Itr==End);
				Assert::AreEqual(3, (*Itr).first);
				Assert::AreEqual(0.9/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(4, (*Itr).first);
				Assert::AreEqual(1.0/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(5, (*Itr).first);
				Assert::AreEqual(1.0/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::AreEqual(6, (*Itr).first);
				Assert::AreEqual(0.1/3.0, (*Itr).second, 1e-5);
				++Itr;
				Assert::IsTrue(Itr==End);
			}
		}
		TEST_METHOD(axis_mapping_floor_floor) {
			auto Axis1 = make_range_axis(0.2, 0.89, 4, math::floor_grid_adjuster<-8>());	//[0.2, 0.43, 0.66, 0.89]
			auto Axis2 = make_range_axis(0.02, 0.74, 7, math::floor_grid_adjuster<-8>());	//[0.02, 0.14, 0.26, 0.38, 0.50, 0.62, 0.74]

			auto Mapper = hmLib::map_axis(Axis1, Axis2);
			//lower & upper
			Assert::AreEqual(0, Mapper.lower());
			Assert::AreEqual(1, Mapper.upper());

			//inside
			Assert::IsTrue(Mapper.inside(0));
			Assert::IsTrue(Mapper.inside(1));
			Assert::IsFalse(Mapper.inside(2));
			Assert::IsFalse(Mapper.inside(3));

			auto WI = Mapper.weighted_index(0);
			Assert::AreEqual(3u, WI.size());
			Assert::AreEqual(1, WI.at(0).first);
			Assert::AreEqual((0.26-0.20)/0.23, WI.at(0).second, 1e-5);
			Assert::AreEqual(2, WI.at(1).first);
			Assert::AreEqual((0.38-0.26)/0.23, WI.at(1).second, 1e-5);
			Assert::AreEqual(3, WI.at(2).first);
			Assert::AreEqual((0.43-0.38)/0.23, WI.at(2).second, 1e-5);
		}
		TEST_METHOD(axis_mapping_round_round) {
			auto Axis1 = make_range_axis(0.2+0.23/2, 0.89+0.23/2, 4, math::round_grid_adjuster<-8>());	//[0.2, 0.43, 0.66, 0.89]
			auto Axis2 = make_range_axis(0.02+0.12/2, 0.74+0.12/2, 7, math::round_grid_adjuster<-8>());	//[0.02, 0.14, 0.26, 0.38, 0.50, 0.62, 0.74]

			auto Mapper = hmLib::map_axis(Axis1, Axis2);
			//lower & upper
			Assert::AreEqual(0, Mapper.lower());
			Assert::AreEqual(1, Mapper.upper());

			//inside
			Assert::IsTrue(Mapper.inside(0));
			Assert::IsTrue(Mapper.inside(1));
			Assert::IsFalse(Mapper.inside(2));
			Assert::IsFalse(Mapper.inside(3));

			auto WI = Mapper.weighted_index(0);
			Assert::AreEqual(3u, WI.size());
			Assert::AreEqual(1, WI.at(0).first);
			Assert::AreEqual((0.26-0.20)/0.23, WI.at(0).second, 1e-5);
			Assert::AreEqual(2, WI.at(1).first);
			Assert::AreEqual((0.38-0.26)/0.23, WI.at(1).second, 1e-5);
			Assert::AreEqual(3, WI.at(2).first);
			Assert::AreEqual((0.43-0.38)/0.23, WI.at(2).second, 1e-5);
		}
		TEST_METHOD(axis_mapping_ceil_round) {
			auto Axis1 = make_range_axis(0.2+0.23, 0.89+0.23, 4, math::ceil_grid_adjuster<-8>());	//[0.2, 0.43, 0.66, 0.89]
			auto Axis2 = make_range_axis(0.02+0.12/2, 0.74+0.12/2, 7, math::round_grid_adjuster<-8>());	//[0.02, 0.14, 0.26, 0.38, 0.50, 0.62, 0.74]

			auto Mapper = hmLib::map_axis(Axis1, Axis2);
			//lower & upper
			Assert::AreEqual(0, Mapper.lower());
			Assert::AreEqual(1, Mapper.upper());

			//inside
			Assert::IsTrue(Mapper.inside(0));
			Assert::IsTrue(Mapper.inside(1));
			Assert::IsFalse(Mapper.inside(2));
			Assert::IsFalse(Mapper.inside(3));

			auto WI = Mapper.weighted_index(0);
			Assert::AreEqual(3u, WI.size());
			Assert::AreEqual(1, WI.at(0).first);
			Assert::AreEqual((0.26-0.20)/0.23, WI.at(0).second, 1e-5);
			Assert::AreEqual(2, WI.at(1).first);
			Assert::AreEqual((0.38-0.26)/0.23, WI.at(1).second, 1e-5);
			Assert::AreEqual(3, WI.at(2).first);
			Assert::AreEqual((0.43-0.38)/0.23, WI.at(2).second, 1e-5);
		}
		TEST_METHOD(axis_mapping_round_ceil) {
			auto Axis1 = make_range_axis(0.2+0.23/2, 0.89+0.23/2, 4, math::round_grid_adjuster<-8>());	//[0.2, 0.43, 0.66, 0.89]
			auto Axis2 = make_range_axis(0.02+0.12, 0.74+0.12, 7, math::ceil_grid_adjuster<-8>());	//[0.02, 0.14, 0.26, 0.38, 0.50, 0.62, 0.74]

			auto Mapper = hmLib::map_axis(Axis1, Axis2);
			//lower & upper
			Assert::AreEqual(0, Mapper.lower());
			Assert::AreEqual(1, Mapper.upper());

			//inside
			Assert::IsTrue(Mapper.inside(0));
			Assert::IsTrue(Mapper.inside(1));
			Assert::IsFalse(Mapper.inside(2));
			Assert::IsFalse(Mapper.inside(3));

			auto WI = Mapper.weighted_index(0);
			Assert::AreEqual(3u, WI.size());
			Assert::AreEqual(1, WI.at(0).first);
			Assert::AreEqual((0.26-0.20)/0.23, WI.at(0).second, 1e-5);
			Assert::AreEqual(2, WI.at(1).first);
			Assert::AreEqual((0.38-0.26)/0.23, WI.at(1).second, 1e-5);
			Assert::AreEqual(3, WI.at(2).first);
			Assert::AreEqual((0.43-0.38)/0.23, WI.at(2).second, 1e-5);
		}
		TEST_METHOD(axis_mapping_norm) {
			auto Axis1 = hmLib::make_range_axis(1.0, 2.0, 101);
			auto Axis2 = hmLib::make_range_axis(0.5, 2.5, 101);

			auto Dist = boost::math::normal_distribution<double>(1.5, 0.1);

			std::vector<double> Vec1(Axis1.size(), 0.);

			for(unsigned int i = 0; i<Axis1.size(); ++i) {
				Vec1[i] = boost::math::pdf(Dist, Axis1[i])*Axis1.interval();
			}
			auto Mapper = Axis1.map_to(Axis2);

			std::vector<double> Vec2(Axis2.size(), 0.);
			for(unsigned int i = 0; i<Vec1.size(); ++i) {
				auto wi = Mapper.weighted_index(i);
				for(auto p:wi) {
					Vec2[p.first] += p.second*Vec1[i];
				}
			}

			auto Sum1 = std::accumulate(Vec1.begin(), Vec1.end(), 0.0);
			auto Sum2 = std::accumulate(Vec2.begin(), Vec2.end(), 0.0);

			Assert::AreEqual(1.0, Sum1, 1e-3);
			Assert::AreEqual(Sum1, Sum2, 1e-4);
		}
	};
	TEST_CLASS(test_math_multiaxis) {
		TEST_METHOD(test_map) {
			lattice_axis<double, 2> Axes1;
			Axes1.axis(0).assign(4.95, 5.05, 101);
			Axes1.axis(1).assign(0.80, 0.90, 101);

			lattice_axis<double, 2> Axes2;
			Axes2.axis(0).assign(4.90, 5.10, 101);
			Axes2.axis(1).assign(0.80, 0.90, 101);

			auto Dist1 = boost::math::normal_distribution<double>(5.00, 0.01);
			auto Dist2 = boost::math::normal_distribution<double>(0.85, 0.01);
			lattices::indexer<2> Indexer(lattices::extent_type<2>{101, 101});
			std::vector<double> Vec1(101*101, 0.0);
			for(unsigned int i = 0; i<Axes1.axis(0).size(); ++i) {
				for(unsigned int j = 0; j<Axes1.axis(1).size(); ++j) {
					Vec1[Indexer.index(lattices::point_type<2>{static_cast<int>(i), static_cast<int>(j)})] = boost::math::pdf(Dist1, Axes1.axis(0)[i])*Axes1.axis(0).interval()
						* boost::math::pdf(Dist2, Axes1.axis(1)[j])*Axes1.axis(1).interval();
				}
			}

			auto Mapper = Axes1.map_to(Axes2);
			std::vector<double> Vec2(Vec1.size(), 0.);
			for(unsigned int i = 0; i<Axes1.axis(0).size(); ++i) {
				for(unsigned int j = 0; j<Axes1.axis(1).size(); ++j) {
					auto wi = Mapper.weighted_point(lattices::point_type<2>{static_cast<int>(i), static_cast<int>(j)});
					for(auto p:wi) {
						Vec2[Indexer.index(p.first)] += p.second*Vec1[Indexer.index(lattices::point_type<2>{static_cast<int>(i), static_cast<int>(j)})];
					}
				}
			}

			auto Sum1 = std::accumulate(Vec1.begin(), Vec1.end(), 0.0);
			auto Sum2 = std::accumulate(Vec2.begin(), Vec2.end(), 0.0);

			Assert::AreEqual(1.0, Sum1, 1e-3);
			Assert::AreEqual(Sum1, Sum2, 1e-4);

		}
	};
	TEST_CLASS(test_math_combi) {
		TEST_METHOD(test_combination) {
			unsigned int N = 5;
			unsigned int R = 3;
			combination_indexer<> Indexer(N, R);
			Assert::AreEqual<unsigned long long>(10, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_combination_at) {
			unsigned int N = 5;
			unsigned int R = 3;
			combination_indexer<> Indexer(N, R);
			Assert::AreEqual<unsigned long long>(10, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(std::size_t i = 0; i<Indexer.size(); ++i) {
					Assert::IsTrue(Indexer[i]<N);
					Val *= 10;
					Val += Indexer.at(i);
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_combination_excp1_at) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,4,6 };
			combination_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(10, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(std::size_t i = 0; i<Indexer.size(); ++i) {
					Assert::IsTrue(Indexer[i]<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), Indexer[i])==Excp.end());
					Val *= 10;
					Val += Indexer.at(i);
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_combination_excp1) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,4,6 };
			combination_indexer<> Indexer(N, R,Excp.begin(),Excp.end());
			Assert::AreEqual<unsigned long long>(10, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_combination_excp2) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,3,4,6,3,4,0,0,0,6 };
			combination_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(10, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_combination_excp3) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 5,6,7,8,9 };
			combination_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(10, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multicombination) {
			unsigned int N = 5;
			unsigned int R = 3;
			multicombination_indexer<> Indexer(N, R);
			Assert::AreEqual<unsigned long long>(nHr(5,3), Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multicombination_at) {
			unsigned int N = 5;
			unsigned int R = 3;
			multicombination_indexer<> Indexer(N, R);
			Assert::AreEqual<unsigned long long>(nHr(5,3), Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(std::size_t i = 0; i<Indexer.size(); ++i) {
					Assert::IsTrue(Indexer[i]<N);
					Val *= 10;
					Val += Indexer.at(i);
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multicombination_excp1_at) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,4,6 };
			multicombination_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(nHr(5, 3), Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(std::size_t i = 0; i<Indexer.size(); ++i) {
					Assert::IsTrue(Indexer[i]<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), Indexer[i])==Excp.end());
					Val *= 10;
					Val += Indexer.at(i);
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multicombination_excp) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,4,6 };
			multicombination_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(nHr(N-Excp.size(),R), Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multicombination_excp2) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,3,4,6,3,4,0,0,0,6 };
			multicombination_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(nHr(5,3), Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multicombination_excp3) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 5,6,7,8,9 };
			multicombination_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(nHr(5,3), Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_permutation) {
			unsigned int N = 5;
			unsigned int R = 3;
			permutation_indexer<> Indexer(N, R);
			Assert::AreEqual<unsigned long long>(60, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_permutation_at) {
			unsigned int N = 5;
			unsigned int R = 3;
			permutation_indexer<> Indexer(N, R);
			Assert::AreEqual<unsigned long long>(60, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(std::size_t i = 0; i<Indexer.size(); ++i) {
					Assert::IsTrue(Indexer[i]<N);
					Val *= 10;
					Val += Indexer.at(i);
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_permutation_excp1_at) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,4,6 };
			permutation_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(60, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(std::size_t i = 0; i<Indexer.size(); ++i) {
					Assert::IsTrue(Indexer[i]<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), Indexer[i])==Excp.end());
					Val *= 10;
					Val += Indexer.at(i);
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_permutation_excp1) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,4,6 };
			permutation_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(60, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_permutation_excp2) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,3,4,6,3,4,0,0,0,6 };
			permutation_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(60, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_permutation_excp3) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 5,6,7,8,9 };
			permutation_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(60, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multipermutation) {
			unsigned int N = 5;
			unsigned int R = 3;
			multipermutation_indexer<> Indexer(N, R);
			Assert::AreEqual<unsigned long long>(125, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multipermutation_at) {
			unsigned int N = 5;
			unsigned int R = 3;
			multipermutation_indexer<> Indexer(N, R);
			Assert::AreEqual<unsigned long long>(125, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(std::size_t i = 0; i<Indexer.size(); ++i) {
					Assert::IsTrue(Indexer[i]<N);
					Val *= 10;
					Val += Indexer.at(i);
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multipermutation_excp1_at) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,4,6 };
			multipermutation_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(125, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(std::size_t i = 0; i<Indexer.size(); ++i) {
					Assert::IsTrue(Indexer[i]<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), Indexer[i])==Excp.end());
					Val *= 10;
					Val += Indexer.at(i);
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multipermutation_excp) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,4,6 };
			multipermutation_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(125, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multipermutation_excp2) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 0,3,3,4,6,3,4,0,0,0,6 };
			multipermutation_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(125, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}
		TEST_METHOD(test_multipermutation_excp3) {
			unsigned int N = 9;
			unsigned int R = 3;
			std::vector<unsigned int> Excp{ 5,6,7,8,9 };
			multipermutation_indexer<> Indexer(N, R, Excp.begin(), Excp.end());
			Assert::AreEqual<unsigned long long>(125, Indexer.total_casenum());

			std::vector<unsigned long long> Vec;
			for(; Indexer.valid(); Indexer.next()) {
				unsigned long long Val = 0;
				for(auto Itr = Indexer.begin(); Itr!=Indexer.end(); ++Itr) {
					Assert::IsTrue(*Itr<N);
					Assert::IsTrue(std::find(Excp.begin(), Excp.end(), *Itr)==Excp.end());
					Val *= 10;
					Val += *Itr;
				}
				Vec.push_back(Val);
			}
			Assert::AreEqual<unsigned long long>(Indexer.total_casenum(), Vec.size());
			Assert::IsTrue(std::unique(Vec.begin(), Vec.end())==Vec.end());
		}

	};
}


/*++
    BDcpp -- Simple Bjontegaard Delta metric implementation for C++.

    MIT License

    Copyright (c) 2022 Tim Bruylants

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

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

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

#include "bdcpp.h"

#include <Eigen/Dense>
#include <cmath>

namespace bdcpp
{
namespace details
{
// Fit a polynomial of given order on the curve (minimizing squared distance).
std::vector<value_type> polyFit(const curve_data_type& curve, const size_t order)
{
    const size_t numCoefficients = order + 1;
    const size_t nCount = curve.size();

    Eigen::MatrixX<value_type> X(nCount, numCoefficients);
    Eigen::MatrixX<value_type> Y(nCount, 1);

    // fill X and Y matrices (X is a Vandermonde matrix)
    for (size_t row = 0; row < nCount; ++row)
    {
        Y(row, 0) = curve[row].second;
        value_type v = (value_type)1;
        for (size_t col = 0; col < numCoefficients; ++col)
        {
            X(row, col) = v;
            v *= curve[row].first;
        }
    }

    // Solve for the polynomial coefficients (one column) and return.
    const Eigen::VectorX<value_type> coefficients = X.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Y);
    return std::vector<value_type>(coefficients.data(), coefficients.data() + numCoefficients);
}

// Calculates Y(x), where the polynomial coefficients are given.
value_type polyVal(const std::vector<value_type>& coefficients, const value_type x)
{
    assert(!coefficients.empty());
    size_t c = coefficients.size();
    value_type r = coefficients[--c];
    while (c != 0)
    {
        r *= x;
        r += coefficients[--c];
    }
    return r;
}

std::vector<value_type> polyIntegrate(const std::vector<value_type>& coefficients, const value_type constant = 0)
{
    const size_t numCoefficients = coefficients.size();
    std::vector<value_type> ic(numCoefficients + 1);
    ic[0] = constant;
    for (size_t c = 0; c < numCoefficients; ++c)
    {
        ic[c + 1] = coefficients[c] / (c + 1);
    }
    return ic;
}

// The main Bjontegaard calculation to get the area surface difference between two curves.
value_type bdDiff(const curve_data_type& curveA, const curve_data_type& curveB, const int polyOrder)
{
    assert(polyOrder >= 3);
    // Take lowest and highest X values (assumes sorted curves and relevant range overlap).
    const auto lowX = std::max(curveA.front().first, curveB.front().first);
    const auto highX = std::min(curveA.back().first, curveB.back().first);

    // Fit curves as polynomials and integrate them.
    const auto iCoefficientsA = polyIntegrate(polyFit(curveA, (size_t)polyOrder));
    const auto iCoefficientsB = polyIntegrate(polyFit(curveB, (size_t)polyOrder));

    // Calculate the definite integrals.
    const auto intA = polyVal(iCoefficientsA, highX) - polyVal(iCoefficientsA, lowX);
    const auto intB = polyVal(iCoefficientsB, highX) - polyVal(iCoefficientsB, lowX);

    // Return the BD diff (as the area over range).
    return (intB - intA) / (highX - lowX);
}

// Prepare curve points for BD calculations.
template<bool TRANSPOSE>
curve_data_type prepareCurve(const curve_data_type& curve)
{
    assert(curve.size() >= 4);
    auto newCurve(curve);
    std::sort(newCurve.begin(), newCurve.end(), [](const curve_data_point_type& vA, const curve_data_point_type& vB)
        {
            return vA.first < vB.first;
        });
    std::for_each(newCurve.begin(), newCurve.end(), [](curve_data_point_type& v)
        {
            v.first = std::log(v.first);
            if constexpr (TRANSPOSE)
            {
                std::swap(v.first, v.second);
            }
        });
    return newCurve;
}
}

// Calculate the BD-SNR for the two given curves (returns a distortion improvement in dB).
value_type bdsnr(const curve_data_type& curveA, const curve_data_type& curveB, const int polyOrder)
{
    return details::bdDiff(details::prepareCurve<false>(curveA), details::prepareCurve<false>(curveB), polyOrder);
}

// Calculate the BD-BR for the two given curves (returns a rate improvement in %).
value_type bdbr(const curve_data_type& curveA, const curve_data_type& curveB, const int polyOrder)
{
    return (std::exp(details::bdDiff(details::prepareCurve<true>(curveA), details::prepareCurve<true>(curveB), polyOrder)) - 1) * 100;
}
}


/*
 * main.cpp
 *
 *  Created on: 3 Aug 2018
 *      Author: scsjd
 */

#include <chrono>
#include <iostream>
#include <random>
#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>
#include <NTL/vec_ZZ.h>
#include <NTL/vec_ZZ_p.h>
#include "DETEncrypter.h"
#include "DETDecrypter.h"
#include "SSEEncrypter.h"
#include "SSEDecrypter.h"
#include "GACDEncrypter.h"
#include "GACDDecrypter.h"
#include "PolyACDEncrypter.h"
#include "PolyACDDecrypter.h"
#include "HE1Encrypter.h"
#include "HE1Decrypter.h"
#include "HE1NEncrypter.h"
#include "HE1NDecrypter.h"
#include "HE2Encrypter.h"
#include "HE2Decrypter.h"
#include "HE2NEncrypter.h"
#include "HE2NDecrypter.h"
#include "HE2Ciphertext.h"
#include "PolyCiphertext.h"
#include "SSECiphertext.h"

NTL::ZZX createRandomPoly(std::mt19937& rng, int mu, int n) {
	NTL::ZZX p;
	//Generate random int for degree of poly
	unsigned int rnd = (rng() % n) + 1;
	//Generate random coefficients for poly
	p.SetLength(rnd+1);
	for (unsigned int i = 0; i < rnd + 1; i++) {
		NTL::ZZ tmp;
		RandomBits(tmp, mu);
		p[i] = tmp;
	}
	return p;
}

int main(){
	std::cout << "======= SSE Tests =======" << std::endl;

	std::string test = "this is a test";

	std::cout << "DET consistency test: ";
	DETEncrypter DETenc;
	std::string DETsecrets = DETenc.writeSecretsToJSON();
	std::string DETencrypted = DETenc.encryptToHex(test);

	DETDecrypter DETdec;
	DETdec.readSecretsFromJSON(DETsecrets);
	std::string DETdecrypted = DETdec.decryptFromHex(DETencrypted);
	if(test==DETdecrypted) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "DET equality test: ";
	std::string encrypted1 = DETenc.encryptToHex(test);
	std::string encrypted2 = DETenc.encryptToHex(test);
	if(encrypted1==encrypted2) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "SSE consistency test: ";
	SSEEncrypter SSEenc;
	std::string SSEsecrets = SSEenc.writeSecretsToJSON();
	std::string SSEencrypted = SSEenc.encrypt(test);

	SSEDecrypter SSEdec;
	SSEdec.readSecretsFromJSON(SSEsecrets);
	std::string SSEdecrypted = SSEdec.decrypt(SSEencrypted);
	if(test==SSEdecrypted) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "SSE match test 1: ";
	std::string searchKey = SSEenc.createHexEncodedSearchKey(test);
	SSECiphertext ssec(SSEencrypted);
	if(ssec.match(searchKey)) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "SSE match test 2: ";
	std::string test2 = "The quick brown fox jumped over the lazy dog";
	std::string SSEencrypted2 = SSEenc.encrypt(test2);
	std::string searchKey2 = SSEenc.createHexEncodedSearchKey(test2);
	SSECiphertext ssec2(SSEencrypted2);
	if(ssec2.match(searchKey2)) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "======= OPE Tests =======" << std::endl;
	//Data for GACD test
	int bits = 32;
	NTL::ZZ ope_m1 = NTL::RandomBits_ZZ(bits);
	NTL::ZZ ope_m2 = NTL::RandomBits_ZZ(bits);

	std::cout << "GACD consistency test: ";
	GACDEncrypter GACDenc(GACDEncrypter::getMinimumKeyLength(bits));
	std::string GACDsecrets = GACDenc.writeSecretsToJSON();
	NTL::ZZ GACDencrypted1 = GACDenc.encrypt(ope_m1);

	GACDDecrypter GACDdec;
	GACDdec.readSecretsFromJSON(GACDsecrets);
	NTL::ZZ GACDdecrypted = GACDdec.decrypt(GACDencrypted1);
	if(ope_m1==GACDdecrypted) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "GACD order-preserving test: ";
	NTL::ZZ GACDencrypted2 = GACDenc.encrypt(ope_m2);
	if(ope_m1 < ope_m2){
		if(GACDencrypted1<GACDencrypted2) std::cout << "PASSED" << std::endl;
		else std::cout << "FAILED" << std::endl;
	}
	else{
		if (ope_m1>ope_m2){
			if(GACDencrypted1>GACDencrypted2) std::cout << "PASSED" << std::endl;
			else std::cout << "FAILED" << std::endl;
		}
		else{
			//random order
			std::cout << "PASSED" << std::endl;
		}
	}

	//Data for PolyACD tests
	int lambda = 80;
	int mu = 32;
	int cdegree = 10;
	int pdegree = 5;
	std::mt19937 rng;
	unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
	rng.seed(seed);
	NTL::ZZX ope_p1 = createRandomPoly(rng,mu,pdegree);
	PolyCiphertext polyp1(ope_p1);
	NTL::ZZX ope_p2 = createRandomPoly(rng,mu,pdegree);
	PolyCiphertext polyp2(ope_p2);

	std::cout << "PolyACD consistency test: ";
	PolyACDEncrypter PolyACDenc(lambda,mu,cdegree);
	std::string PolyACDsecrets = PolyACDenc.writeSecretsToJSON();
	NTL::ZZX PolyACDencrypted1 = PolyACDenc.encrypt(ope_p1);

	PolyACDDecrypter PolyACDdec;
	PolyACDdec.readSecretsFromJSON(PolyACDsecrets);
	NTL::ZZX PolyACDdecrypted = PolyACDdec.decrypt(PolyACDencrypted1);
	if(ope_p1==PolyACDdecrypted) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "PolyACD order-preserving test: ";
	NTL::ZZX PolyACDencrypted2 = PolyACDenc.encrypt(ope_p2);
	PolyCiphertext polyc1(PolyACDencrypted1);
	PolyCiphertext polyc2(PolyACDencrypted2);
	if(polyp1 < polyp2){
		if(polyc1<polyc2) std::cout << "PASSED" << std::endl;
		else std::cout << "FAILED" << std::endl;
	}
	else{
		if (polyp1>polyp2){
			if(polyc1>polyc2) std::cout << "PASSED" << std::endl;
			else std::cout << "FAILED" << std::endl;
		}
		else{
			//random order
			std::cout << "PASSED" << std::endl;
		}
	}

	std::cout << "======= Integer HE Tests =======" << std::endl;
	//Data for HEx tests
	int n = 1000;
	int d = 2;
	int rho1=8;
	int rho2=32;
	int rhoprime=32;

	NTL::ZZ he_rho1_m1 = NTL::RandomBits_ZZ(rho1);
	NTL::ZZ he_rho1_m2 = NTL::RandomBits_ZZ(rho1);
	NTL::ZZ he_rho2_m1 = NTL::RandomBits_ZZ(rho2);
	NTL::ZZ he_rho2_m2 = NTL::RandomBits_ZZ(rho2);
	NTL::ZZ rho1_sum = he_rho1_m1+he_rho1_m2;
	NTL::ZZ rho1_product = he_rho1_m1*he_rho1_m2;
	NTL::ZZ rho2_sum = he_rho2_m1+he_rho2_m2;
	NTL::ZZ rho2_product = he_rho2_m1*he_rho2_m2;

	std::cout << "HE1 consistency test: ";
	HE1Encrypter he1enc(n,d,rho2);
	he1enc.init();
	//NTL::ZZ he1key = he1enc.getKey();
	std::string he1secrets = he1enc.writeSecretsToJSON();
	NTL::ZZ_p he1_c1 = he1enc.encrypt(he_rho2_m1);

	HE1Decrypter he1dec;
	//he1dec.setKey(he1key);
	he1dec.readSecretsFromJSON(he1secrets);
	he1dec.init();
	NTL::ZZ he1_m1 = he1dec.decrypt(he1_c1);
	if(he1_m1==he_rho2_m1) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "HE1 homomorphic tests:" << std::endl;
	NTL::ZZ_p he1_c2 = he1enc.encrypt(he_rho2_m2);
	NTL::ZZ_p he1_sum = he1_c1+he1_c2;
	NTL::ZZ_p he1_product = he1_c1*he1_c2;
	NTL::ZZ he1_sum_dec = he1dec.decrypt(he1_sum);
	NTL::ZZ he1_product_dec = he1dec.decrypt(he1_product);
	std::cout << "\t Sum: ";
	if(he1_sum_dec==rho2_sum) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;
	std::cout << "\t Product: ";
	if(he1_product_dec==rho2_product) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "HE1N consistency test: ";
	HE1NEncrypter he1nenc(n,d,rho1,rhoprime);
	he1nenc.init();
	//NTL::vec_ZZ he1nkey = he1nenc.getKey();
	std::string he1nsecrets = he1nenc.writeSecretsToJSON();
	NTL::ZZ_p he1n_c1 = he1nenc.encrypt(he_rho1_m1);

	HE1NDecrypter he1ndec;
	//he1ndec.setKey(he1nkey);
	he1ndec.readSecretsFromJSON(he1nsecrets);
	NTL::ZZ he1n_m1 = he1ndec.decrypt(he1n_c1);
	if(he1n_m1==he_rho1_m1) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "HE1N homomorphic tests:" << std::endl;
	NTL::ZZ_p he1n_c2 = he1nenc.encrypt(he_rho1_m2);
	NTL::ZZ_p he1n_sum = he1n_c1+he1n_c2;
	NTL::ZZ_p he1n_product = he1n_c1*he1n_c2;
	NTL::ZZ he1n_sum_dec = he1ndec.decrypt(he1n_sum);
	NTL::ZZ he1n_product_dec = he1ndec.decrypt(he1n_product);
	std::cout << "\t Sum: ";
	if(he1n_sum_dec==rho1_sum) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;
	std::cout << "\t Product: ";
	if(he1n_product_dec==rho1_product) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "HE2 consistency test: ";
	HE2Encrypter he2enc(n,d,rho2);
	he2enc.init();
	std::string he2secrets = he2enc.writeSecretsToJSON();
	std::string he2params = he2enc.writeParametersToJSON();
	NTL::vec_ZZ_p he2_c1 = he2enc.encrypt(he_rho2_m1);

	HE2Decrypter he2dec;
	he2dec.readSecretsFromJSON(he2secrets);
	NTL::ZZ he2_m1 = he2dec.decrypt(he2_c1);
	if(he2_m1==he_rho2_m1) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "HE2 homomorphic tests:" << std::endl;
	NTL::vec_ZZ_p he2_c2 = he2enc.encrypt(he_rho2_m2);
	HE2Ciphertext::setParameters(he2params);
	HE2Ciphertext he2c1(he2_c1);
	HE2Ciphertext he2c2(he2_c2);
	NTL::vec_ZZ_p he2_sum = (he2c1+he2c2).get_ciphertext();
	NTL::vec_ZZ_p he2_product = (he2c1*he2c2).get_ciphertext();
	NTL::ZZ he2_sum_dec = he2dec.decrypt(he2_sum);
	NTL::ZZ he2_product_dec = he2dec.decrypt(he2_product);
	std::cout << "\t Sum: ";
	if(he2_sum_dec==rho2_sum) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;
	std::cout << "\t Product: ";
	if(he2_product_dec==rho2_product) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "HE2N consistency test: ";
	HE2NEncrypter he2nenc(n,d,rho1,rhoprime);
	he2nenc.init();
	std::string he2nsecrets = he2nenc.writeSecretsToJSON();
	std::string he2nparams = he2nenc.writeParametersToJSON();
	NTL::vec_ZZ_p he2n_c1 = he2nenc.encrypt(he_rho1_m1);

	HE2NDecrypter he2ndec;
	he2ndec.readSecretsFromJSON(he2nsecrets);
	NTL::ZZ he2n_m1 = he2ndec.decrypt(he2n_c1);
	if(he2n_m1==he_rho1_m1) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;

	std::cout << "HE2N homomorphic tests:" << std::endl;
	NTL::vec_ZZ_p he2n_c2 = he2nenc.encrypt(he_rho1_m2);
	//Fails in setParameters when initialising R
	HE2Ciphertext::setParameters(he2nparams);
	HE2Ciphertext he2nc1(he2n_c1);
	HE2Ciphertext he2nc2(he2n_c2);
	HE2Ciphertext he2ncsum = he2nc1+he2nc2;
	HE2Ciphertext he2ncproduct = he2nc1*he2nc2;
	NTL::vec_ZZ_p he2n_sum = he2ncsum.get_ciphertext();
	NTL::vec_ZZ_p he2n_product = (he2nc1*he2nc2).get_ciphertext();
	NTL::ZZ he2n_sum_dec = he2ndec.decrypt(he2n_sum);
	NTL::ZZ he2n_product_dec = he2ndec.decrypt(he2n_product);
	std::cout << "\t Sum: ";
	if(he2n_sum_dec==rho1_sum) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;
	std::cout << "\t Product: ";
	if(he2n_product_dec==rho1_product) std::cout << "PASSED" << std::endl;
	else std::cout << "FAILED" << std::endl;
}


#include <opencv2/opencv.hpp>
#include <sophus/se3.hpp>
#include <boost/format.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;

typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;

// Camera intrinsics
double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;

// baseline
double baseline = 0.573;
// paths
string left_file = "../left.png";
string disparity_file = "../disparity.png";
boost::format fmt_others("../%06d.png");    // other files

// useful typedefs
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
typedef Eigen::Matrix<double, 2, 6> Matrix26d;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

// bilinear interpolation
inline float get(const cv::Mat &img, float x, float y) {
    // boundary check
    if (x < 0) x = 0;
    if (y < 0) y = 0;
    if (x >= img.cols) x = img.cols - 1;
    if (y >= img.rows) y = img.rows - 1;
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    return float(
        (1 - xx) * (1 - yy) * data[0] +
        xx * (1 - yy) * data[1] +
        (1 - xx) * yy * data[img.step] +
        xx * yy * data[img.step + 1]
    );
}

Eigen::Vector3d get_3D_point_from_depth(const Eigen::Vector2d& p, double depth)
{
    return Eigen::Vector3d(depth * (p.x() - cx) / fx,
                           depth * (p.y() - cy) / fy,
                           depth);
}

using namespace Sophus;
// Local parameterization needed to handle SE3 from Sophus (from Sophus/test/ceres/)
class LocalParameterizationSE3 : public ceres::LocalParameterization {
 public:
  virtual ~LocalParameterizationSE3() {}

  // SE3 plus operation for Ceres
  //
  //  T * exp(x)
  //
  virtual bool Plus(double const* T_raw, double const* delta_raw,
                    double* T_plus_delta_raw) const {
    Eigen::Map<SE3d const> const T(T_raw);
    Eigen::Map<Vector6d const> const delta(delta_raw);
    Eigen::Map<SE3d> T_plus_delta(T_plus_delta_raw);
    T_plus_delta = T * SE3d::exp(delta); ///// check if good left or righ multiply ??????????????????
    // std::cout << (SE3d::exp(delta) * T).matrix() << "\n";
    // std::cout << (T * SE3d::exp(delta)).matrix() << "\n\n";
    return true;
  }

  // Jacobian of SE3 plus operation for Ceres
  //
  // Dx T * exp(x)  with  x=0
  //
  virtual bool ComputeJacobian(double const* T_raw,
                               double* jacobian_raw) const {
    Eigen::Map<SE3d const> T(T_raw);
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> jacobian(
        jacobian_raw);
    jacobian = T.Dx_this_mul_exp_x_at_0();
    return true;
  }

  virtual int GlobalSize() const { return SE3d::num_parameters; }

  virtual int LocalSize() const { return SE3d::DoF; }
};

struct PhotometricError: public ceres::SizedCostFunction<1, 7>
{
    PhotometricError(const cv::Mat& img1, const cv::Mat& img2, const Eigen::Vector2d& p1, const Eigen::Vector3d& P1, const Eigen::Matrix3d& K)
    : _img1(img1), _img2(img2), _p1(p1), _P1(P1), _K(K) {}

    virtual bool Evaluate(double const* const *params,
                          double *residuals,
                          double **jacobians) const {
        const Eigen::Map<const Sophus::SE3d> Rt(params[0]);

        Eigen::Vector3d P2 = Rt * _P1;
        Eigen::Vector3d p2 = _K * P2;
        p2 /= p2.z();

        double v1 = get(_img1, _p1.x(), _p1.y());
        double v2 = get(_img2, p2.x(), p2.y());
        double err = v1 - v2;
        residuals[0] = err;

        if (!jacobians) return true;
        if (!jacobians[0]) return true;

        double fx = _K(0, 0);
        double fy = _K(1, 1);
        double cx = _K(0, 2);
        double cy = _K(1, 2);
        double X2 = std::pow(P2.x(), 2);
        double Y2 = std::pow(P2.y(), 2);
        double Z2 = std::pow(P2.z(), 2);

        int xx = 0, yy = 0;
        double dx = 0.5 * (get(_img2, p2.x() + xx + 1, p2.y() + yy) - get(_img2, p2.x() + xx - 1, p2.y() + yy));
        double dy = 0.5 * (get(_img2, p2.x() + xx, p2.y() + yy + 1) - get(_img2, p2.x() + xx, p2.y() + yy - 1));
        Eigen::Vector2d dIdu(dx, dy);
        Eigen::Matrix<double, 2, 6> dudRt;
        dudRt << fx/P2.z(), 0.0, -fx*P2.x() / Z2,-fx * P2.x() * P2.y() / Z2, fx + fx * X2 / Z2, -fx * P2.y() / P2.z(),
                 0.0, fy / P2.z(), -fy*P2.y()/Z2, -fy-fy * Y2 / Z2, fy * P2.x() * P2.y() / Z2, fy * P2.x() / P2.z();
        Eigen::Matrix<double, 6, 1> J = -(dIdu.transpose() * dudRt).transpose();
        jacobians[0][0] = J(0, 0);
        jacobians[0][1] = J(1, 0);
        jacobians[0][2] = J(2, 0);
        jacobians[0][3] = J(3, 0);
        jacobians[0][4] = J(4, 0);
        jacobians[0][5] = J(5, 0);
        jacobians[0][6] = 0.0;

        return true;
    }

    private:
        cv::Mat _img1, _img2;
        Eigen::Vector2d _p1;
        Eigen::Vector3d _P1;
        Eigen::Matrix3d _K;
};

/**
 * pose estimation using direct method
 * @param img1
 * @param img2
 * @param px_ref
 * @param depth_ref
 * @param T21
 */
void DirectPoseEstimationSingleLayer(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    const Eigen::Matrix3d& K,
    Sophus::SE3d &Rt // points from cam1 reference frame to cam2
)
{
    int nb_iters = 10;
    int half_w_size = 2;
    double prev_cost = 0.0;

    ceres::Problem problem;

    problem.AddParameterBlock(Rt.data(), 7, new LocalParameterizationSE3());
std::cout << "AFTER add parameters bloxk" << std::endl;
    for (int i = 0; i < px_ref.size(); ++i)
    {
        const auto& p1 = px_ref[i];
        Eigen::Vector3d P1 = get_3D_point_from_depth(p1, depth_ref[i]);
        problem.AddResidualBlock(
            new PhotometricError(img1, img2, p1, P1, K),
            nullptr,
            Rt.data()
        );
    }
std::cout << "AFTER ad dresiduals block" << std::endl;

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve(options, &problem, &summary);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double >>(t2 - t1);
    cout << "optimization with ceres costs time: " << time_used.count() << " seconds." << endl;

    std::cout << "translation: " << Rt.translation().transpose() << "\n";
    std::cout << "rotation: " << Rt.so3().unit_quaternion().toRotationMatrix() << "\n";

}

void DirectPoseEstimationPyramidal(
    const cv::Mat &img1,
    const cv::Mat &img2,
    const VecVector2d &px_ref,
    const vector<double> depth_ref,
    const Eigen::Matrix3d& K,
    Sophus::SE3d &Rt)
{
    int nb_levels = 1;
    double factor = 2.0;
    double scale = 1.0 / std::pow(factor, nb_levels-1);

    for (int l = 0; l < nb_levels; ++l)
    {
        cv::Mat img1_r, img2_r;
        cv::resize(img1, img1_r, cv::Size(), scale, scale);
        cv::resize(img2, img2_r, cv::Size(), scale, scale);

        Eigen::Matrix3d K_r = K;
        K_r(0, 0) *= scale;
        K_r(1, 1) *= scale;
        K_r(0, 2) *= scale;
        K_r(1, 2) *= scale;
        auto p_r = px_ref;
        for (auto& p : p_r)
        {
            p *= scale;
        }

        DirectPoseEstimationSingleLayer(img1_r, img2_r, p_r, depth_ref, K_r, Rt);

        scale *= factor;
    }
}

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

    cv::Mat left_img = cv::imread(left_file, 0);
    cv::Mat disparity_img = cv::imread(disparity_file, 0);

    // let's randomly pick pixels in the first image and generate some 3d points in the first image's frame
    cv::RNG rng(1994);
    int nPoints = 2000;
    int boarder = 20;
    VecVector2d pixels_ref;
    vector<double> depth_ref;

    // generate pixels in ref and load depth data
    for (int i = 0; i < nPoints; i++) {
        int x = rng.uniform(boarder, left_img.cols - boarder);  // don't pick pixels close to boarder
        int y = rng.uniform(boarder, left_img.rows - boarder);  // don't pick pixels close to boarder
        int disparity = disparity_img.at<uchar>(y, x);
        double depth = fx * baseline / disparity; // you know this is disparity to depth
        depth_ref.push_back(depth);
        pixels_ref.push_back(Eigen::Vector2d(x, y));
    }

    // estimates 01~05.png's pose using this information
    Sophus::SE3d Rt;
    Eigen::Matrix3d K;
    K << fx, 0.0, cx,
         0.0, fy, cy,
         0.0, 0.0, 1.0;

    for (int i = 1; i < 2; i++) {  // 1~10
        cv::Mat img = cv::imread((fmt_others % i).str(), 0);
        // try single layer by uncomment this line
        DirectPoseEstimationSingleLayer(left_img, img, pixels_ref, depth_ref, K, Rt);
        // DirectPoseEstimationPyramidal(left_img, img, pixels_ref, depth_ref, K, Rt);

        // plot the projected pixels here
        cv::Mat img2_show;
        cv::cvtColor(img, img2_show, CV_GRAY2BGR);
        std::vector<Eigen::Vector2d> projections(pixels_ref.size());
        for (int i = 0; i < pixels_ref.size(); ++i)
        {
            Eigen::Vector3d P_ref = get_3D_point_from_depth(pixels_ref[i], depth_ref[i]);
            Eigen::Vector3d uv = K * (Rt * P_ref);
            projections[i] = uv.hnormalized();
        }

        for (size_t i = 0; i < pixels_ref.size(); ++i) {
            auto p_ref = pixels_ref[i];
            auto p_cur = projections[i];
            if (p_cur[0] > 0 && p_cur[1] > 0) {
                cv::circle(img2_show, cv::Point2f(p_cur[0], p_cur[1]), 2, cv::Scalar(0, 250, 0), 2);
                cv::line(img2_show, cv::Point2f(p_ref[0], p_ref[1]), cv::Point2f(p_cur[0], p_cur[1]),
                        cv::Scalar(0, 250, 0));
            }
        }
        cv::imshow("current", img2_show);
        cv::waitKey();
        // cv::imwrite("img_"+std::to_string(i) + ".png", img2_show);

    }
    return 0;
}