/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2001 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Authors: Wolfgang Bangerth, Ralf Hartmann, University of Heidelberg, 2001
 */

// 以下第一个include文件现在可能已经众所周知，不需要进一步解释。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/convergence_table.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/manifold_lib.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/fe/fe_values.h>

// 这个包含文件是新的。即使我们在本教程中不求解PDE，我们也要使用FE_Nothing类提供的自由度为零的假有限元。

#include <deal.II/fe/fe_nothing.h>

// 下面的头文件也是新的：在其中，我们声明了MappingQ类，我们将使用该类来处理任意阶的多项式映射。

#include <deal.II/fe/mapping_q.h>

// 这又是一个C++的文件。

#include <iostream>
#include <fstream>
#include <cmath>

// 最后一步和以前的程序一样。

namespace Step10
{
  using namespace dealii;

// 现在，由于我们要计算 $\pi$ 的值，我们必须与一些东西进行比较。这些是 $\pi$ 的前几个数字，我们事先定义好，以便以后使用。由于我们想计算两个数字的差值，而这两个数字是相当精确的，计算出的 $\pi$ 的近似值的精度在一个双数变量可以容纳的数字范围内，所以我们宁可将参考值声明为 long double ，并给它增加一些数字。

  const long double pi = 3.141592653589793238462643L;

// 然后，第一个任务将是生成一些输出。由于这个程序非常小，我们在其中没有采用面向对象的技术，也没有声明类（当然，我们使用了库的面向对象的功能）。相反，我们只是将功能打包成独立的函数。我们使这些函数成为空间维数的模板，以符合使用deal.II时的通常做法，尽管我们只对两个空间维数使用这些函数，当试图对任何其他空间维数使用时，会出现异常。

// 这些函数中的第一个只是生成一个圆的三角形（hyperball），并输出 $Q_p$ 的不同值的单元的映射。然后，我们细化一次网格，再做一次。

  template <int dim>
  void gnuplot_output()
  {
    std::cout << "Output of grids into gnuplot files:" << std::endl
              << "===================================" << std::endl;
//因此，
//首先生成一个圆的粗略三角剖分，并将一个合适的边界描述与之关联。默认情况下， GridGenerator::hyper_ball 将SphericalManifold附加到边界上（内部使用FlatManifold），所以我们简单地调用该函数并继续前进。

    Triangulation<dim> triangulation;
    GridGenerator::hyper_ball(triangulation);

// 然后在当前网格上交替生成 $Q_1$ 、 $Q_2$ 和 $Q_3$ 映射的输出，以及（在循环体的末端）对网格进行一次全局细化。

    for (unsigned int refinement = 0; refinement < 2; ++refinement)
      {
        std::cout << "Refinement level: " << refinement << std::endl;

        std::string filename_base = "ball_" + std::to_string(refinement);

        for (unsigned int degree = 1; degree < 4; ++degree)
          {
            std::cout << "Degree = " << degree << std::endl;

// 为此，首先建立一个描述映射的对象。这是用MappingQ类来完成的，该类在构造函数中采用了它应使用的多项式程度作为参数。

            const MappingQ<dim> mapping(degree);

// 顺便提一下，对于一个片状线性映射，你可以给MappingQ的构造函数一个 1 的值，但也有一个MappingQ1类可以达到同样的效果。历史上，它以比MappingQ更简单的方式做了很多事情，但今天只是后者的一个包装。然而，如果你没有明确指定另一个映射，它仍然是库中许多地方隐含使用的类。

// 为了真正用这个映射写出现在的网格，我们设置了一个对象，我们将用它来输出。我们将生成Gnuplot输出，它由一组描述映射的三角图的线条组成。默认情况下，三角剖分的每个面只画一条线，但由于我们想明确地看到映射的效果，所以我们想更详细地了解这些面。这可以通过传递给输出对象一个包含一些标志的结构来实现。在目前的情况下，由于Gnuplot只能画直线，我们在面孔上输出了一些额外的点，这样每个面孔就由30条小线来画，而不是只有一条。这足以让我们看到一条弯曲的线，而不是一组直线的印象。

            GridOut               grid_out;
            GridOutFlags::Gnuplot gnuplot_flags(false, 60);
            grid_out.set_flags(gnuplot_flags);

// 最后，生成一个文件名和一个用于输出的文件。

            std::string filename =
              filename_base + "_mapping_q_" + std::to_string(degree) + ".dat";
            std::ofstream gnuplot_file(filename);

// 然后把三角图写到这个文件里。该函数的最后一个参数是一个指向映射对象的指针。这个参数有一个默认值，如果没有给出值，就会取一个简单的MappingQ1对象，我们在上面简单介绍过。这样就会在输出中产生一个真实边界的片状线性近似。

            grid_out.write_gnuplot(triangulation, gnuplot_file, &mapping);
          }
        std::cout << std::endl;

// 在循环结束时，对网格进行全局细化。

        triangulation.refine_global();
      }
  }

// 现在我们进行代码的主要部分，即 $\pi$ 的近似。圆的面积当然是由 $\pi r^2$ 给出的，所以有一个半径为1的圆，面积代表的只是被搜索的数字。面积的数值计算是通过在整个计算域中积分值为1的常数函数来进行的，即通过计算面积 $\int_K 1 dx=\int_{\hat K} 1
// \ \textrm{det}\ J(\hat x) d\hat x \approx \sum_i \textrm{det}
// \ J(\hat x_i)w(\hat x_i)$ ，其中总和延伸到三角形中所有活动单元上的所有正交点， $w(x_i)$ 是正交点的重量 $x_i$ 。每个单元上的积分都是通过数字正交来逼近的，因此我们唯一需要的额外成分是建立一个FEValues对象，提供每个单元的相应`JxW`值。注意`JxW`是指Jacobian determinant
// times weight的缩写；因为在数字正交中，两个因子总是出现在相同的地方，所以我们只提供合并的数量，而不是两个单独的数量）。我们注意到，在这里我们不会在其最初的目的中使用FEValues对象，即用于计算特定正交点上的特定有限元的基函数值。相反，我们只用它来获得正交点的 "JxW"，而不考虑我们将给FEValues对象的构造者的（假）有限元。给予FEValues对象的实际有限元根本不使用，所以我们可以给任何。

  template <int dim>
  void compute_pi_by_area()
  {
    std::cout << "Computation of Pi by the area:" << std::endl
              << "==============================" << std::endl;

// 对于所有单元的数字正交，我们采用足够高的正交规则。我们选择8阶的QGauss（4点），以确保数字正交引起的误差比由于边界近似的阶数，即所采用的映射的阶数（最大6）要高。请注意，积分，雅各布行列式，不是一个多项式函数（相反，它是一个有理函数），所以我们不使用高斯正交来获得积分的精确值，就像在有限元计算中经常做的那样，但也可以使用任何类似阶数的正交公式来代替。

    const QGauss<dim> quadrature(4);

// 现在开始在多项式映射度=1...4的基础上进行循环。

    for (unsigned int degree = 1; degree < 5; ++degree)
      {
        std::cout << "Degree = " << degree << std::endl;

// 首先生成三角形、边界和映射对象，正如已经看到的那样。

        Triangulation<dim> triangulation;
        GridGenerator::hyper_ball(triangulation);

        const MappingQ<dim> mapping(degree);

// 我们现在创建一个有限元。与其他的例子程序不同，我们实际上不需要用形状函数做任何计算；我们只需要FEValues对象的`JxW`值。因此，我们使用特殊的有限元类FE_Nothing，它的每个单元的自由度正好为零（顾名思义，每个单元的局部基础为空集）。FE_Nothing的一个比较典型的用法见  step-46  。

        const FE_Nothing<dim> fe;

// 同样地，我们需要创建一个DoFHandler对象。我们实际上并没有使用它，但是它将为我们提供`active_cell_iterators'，这是重新初始化三角形的每个单元上的FEValues对象所需要的。

        DoFHandler<dim> dof_handler(triangulation);

// 现在我们设置FEValues对象，向构造函数提供Mapping、假有限元和正交对象，以及要求只在正交点提供`JxW`值的更新标志。这告诉FEValues对象在调用 reinit 函数时不需要计算其他数量，从而节省计算时间。

// 与之前的例子程序相比，FEValues对象的构造最重要的区别是，我们传递了一个映射对象作为第一个参数，它将被用于计算从单元到实数单元的映射。在以前的例子中，这个参数被省略了，结果是隐含地使用了MappingQ1类型的对象。

        FEValues<dim> fe_values(mapping, fe, quadrature, update_JxW_values);

// 我们使用一个ConvergenceTable类的对象来存储所有重要的数据，如 $\pi$ 的近似值和与 $\pi$ 的真实值相比的误差。我们还将使用ConvergenceTable类提供的函数来计算 $\pi$ 的近似值的收敛率。

        ConvergenceTable table;

// 现在我们在三角形的几个细化步骤上循环。

        for (unsigned int refinement = 0; refinement < 6;
             ++refinement, triangulation.refine_global(1))
          {

// 在这个循环中，我们首先将当前三角形的活动单元的数量添加到表格中。这个函数会自动创建一个上标为 "cells "的表格列，以防这个列之前没有被创建。

            table.add_value("cells", triangulation.n_active_cells());

// 然后我们为虚拟有限元分配自由度。严格来说，在我们的特殊情况下，我们不需要这个函数的调用，但我们调用它是为了让DoFHandler高兴 -- 否则它将在下面的 FEValues::reinit 函数中抛出一个断言。

            dof_handler.distribute_dofs(fe);

// 我们将变量面积定义为 "长双"，就像我们之前为 "pi "变量所做的那样。

            long double area = 0;

// 现在我们循环所有的单元格，重新初始化每个单元格的FEValues对象，并将该单元格的所有`JxW`值加到`area`上......

            for (const auto &cell : dof_handler.active_cell_iterators())
              {
                fe_values.reinit(cell);
                for (unsigned int i = 0; i < fe_values.n_quadrature_points; ++i)
                  area += static_cast<long double>(fe_values.JxW(i));
              }

// ...并将得到的区域值和错误存储在表中。我们需要静态转换为双数，因为没有实现add_value(string, long double)函数。请注意，这也涉及到第二个调用，因为 std 命名空间中的 fabs 函数在其参数类型上是重载的，所以存在一个获取并返回 long double 的版本，而全局命名空间中只有一个这样的函数被声明（获取并返回一个双数）。

            table.add_value("eval.pi", static_cast<double>(area));
            table.add_value("error", static_cast<double>(std::fabs(area - pi)));
          }

// 我们想计算`error`列的收敛率。因此我们需要在调用`evaluate_all_convergence_rates`之前，将其他列从收敛率评估中省略。

        table.omit_column_from_convergence_rate_evaluation("cells");
        table.omit_column_from_convergence_rate_evaluation("eval.pi");
        table.evaluate_all_convergence_rates(
                                    ConvergenceTable::reduction_rate_log2);

// 最后我们设置一些量的输出精度和科学模式...

        table.set_precision("eval.pi", 16);
        table.set_scientific("error", true);

// ...并将整个表格写到  std::cout.  。
        table.write_text(std::cout);

        std::cout << std::endl;
      }
  }

// 下面的第二个函数也是计算 $\pi$ 的近似值，但这次是通过域的周长 $2\pi r$ 而不是面积。这个函数只是前一个函数的一个变体。因此，我们主要是给出不同之处的文件。

  template <int dim>
  void compute_pi_by_perimeter()
  {
    std::cout << "Computation of Pi by the perimeter:" << std::endl
              << "===================================" << std::endl;

// 我们采取同样的正交顺序，但这次是`dim-1`维正交，因为我们将在（边界）线上而不是在单元上积分。

    const QGauss<dim - 1> quadrature(4);

// 我们在所有度数上循环，创建三角形、边界、映射、假有限元和DoFHandler对象，如之前所见。

    for (unsigned int degree = 1; degree < 5; ++degree)
      {
        std::cout << "Degree = " << degree << std::endl;
        Triangulation<dim> triangulation;
        GridGenerator::hyper_ball(triangulation);

        const MappingQ<dim>   mapping(degree);
        const FE_Nothing<dim> fe;

        DoFHandler<dim> dof_handler(triangulation);

// 然后我们创建一个FEFaceValues对象，而不是像前一个函数中的FEValues对象。同样，我们传递一个映射作为第一个参数。

        FEFaceValues<dim> fe_face_values(mapping,
                                         fe,
                                         quadrature,
                                         update_JxW_values);
        ConvergenceTable  table;

        for (unsigned int refinement = 0; refinement < 6;
             ++refinement, triangulation.refine_global(1))
          {
            table.add_value("cells", triangulation.n_active_cells());

            dof_handler.distribute_dofs(fe);

// 现在我们在所有单元和每个单元的所有面上运行。只有边界面上的`JxW`值的贡献被添加到长双变量`周长`中。

            long double perimeter = 0;
            for (const auto &cell : dof_handler.active_cell_iterators())
              for (const auto &face : cell->face_iterators())
                if (face->at_boundary())
                  {

// 我们用单元格迭代器和面的编号重新启动FEFaceValues对象。

                    fe_face_values.reinit(cell, face);
                    for (unsigned int i = 0;
                         i < fe_face_values.n_quadrature_points;
                         ++i)
                      perimeter +=
                        static_cast<long double>(fe_face_values.JxW(i));
                  }

// 然后将评估后的数值存储在表中...

            table.add_value("eval.pi", static_cast<double>(perimeter / 2.0L));
            table.add_value(
              "error", static_cast<double>(std::fabs(perimeter / 2.0L - pi)));
          }

// ......然后像前一个函数那样结束这个函数。

        table.omit_column_from_convergence_rate_evaluation("cells");
        table.omit_column_from_convergence_rate_evaluation("eval.pi");
        table.evaluate_all_convergence_rates(
          ConvergenceTable::reduction_rate_log2);

        table.set_precision("eval.pi", 16);
        table.set_scientific("error", true);

        table.write_text(std::cout);

        std::cout << std::endl;
      }
  }
} // namespace Step10

// 下面的主函数只是按照上述函数的出现顺序来调用它们。除此以外，它看起来就像以前的教程程序的主函数一样。

int main()
{
  try
    {
      std::cout.precision(16);

      const unsigned int dim = 2;

      Step10::gnuplot_output<dim>();

      Step10::compute_pi_by_area<dim>();
      Step10::compute_pi_by_perimeter<dim>();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


/*
 * testSudoku.cpp
 * @brief develop code for Sudoku CSP solver
 * @date Jan 29, 2012
 * @author Frank Dellaert
 */

#include <gtsam_unstable/discrete/CSP.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
using boost::assign::insert;
#include <iostream>
#include <sstream>
#include <stdarg.h>

using namespace std;
using namespace gtsam;

#define PRINT false

class Sudoku: public CSP {

	/// sudoku size
	size_t n_;

	/// discrete keys
	typedef std::pair<size_t, size_t> IJ;
	std::map<IJ, DiscreteKey> dkeys_;

public:

	/// return DiscreteKey for cell(i,j)
	const DiscreteKey& dkey(size_t i, size_t j) const {
		return dkeys_.at(IJ(i, j));
	}

	/// return Index for cell(i,j)
	Index key(size_t i, size_t j) const {
		return dkey(i, j).first;
	}

	/// Constructor
	Sudoku(size_t n, ...) :
			n_(n) {
		// Create variables, ordering, and unary constraints
		va_list ap;
		va_start(ap, n);
		Index k=0;
		for (size_t i = 0; i < n; ++i) {
			for (size_t j = 0; j < n; ++j, ++k) {
				// create the key
				IJ ij(i, j);
				dkeys_[ij] = DiscreteKey(k, n);
				// get the unary constraint, if any
				int value = va_arg(ap, int);
				// cout << value << " ";
				if (value != 0) addSingleValue(dkeys_[ij], value - 1);
			}
			//cout << endl;
		}
		va_end(ap);

		// add row constraints
		for (size_t i = 0; i < n; i++) {
			DiscreteKeys dkeys;
			for (size_t j = 0; j < n; j++)
				dkeys += dkey(i, j);
			addAllDiff(dkeys);
		}

		// add col constraints
		for (size_t j = 0; j < n; j++) {
			DiscreteKeys dkeys;
			for (size_t i = 0; i < n; i++)
				dkeys += dkey(i, j);
			addAllDiff(dkeys);
		}

		// add box constraints
		size_t N = (size_t)sqrt(double(n)), i0 = 0;
		for (size_t I = 0; I < N; I++) {
			size_t j0 = 0;
			for (size_t J = 0; J < N; J++) {
				// Box I,J
				DiscreteKeys dkeys;
				for (size_t i = i0; i < i0 + N; i++)
					for (size_t j = j0; j < j0 + N; j++)
						dkeys += dkey(i, j);
				addAllDiff(dkeys);
				j0 += N;
			}
			i0 += N;
		}
	}

	/// Print readable form of assignment
	void printAssignment(DiscreteFactor::sharedValues assignment) const {
		for (size_t i = 0; i < n_; i++) {
			for (size_t j = 0; j < n_; j++) {
				Index k = key(i, j);
				cout << 1 + assignment->at(k) << " ";
			}
			cout << endl;
		}
	}

	/// solve and print solution
	void printSolution() {
		DiscreteFactor::sharedValues MPE = optimalAssignment();
		printAssignment(MPE);
	}

};

/* ************************************************************************* */
TEST_UNSAFE( Sudoku, small)
{
	Sudoku csp(4,
			1,0, 0,4,
			0,0, 0,0,

			4,0, 2,0,
			0,1, 0,0);

	// Do BP
	csp.runArcConsistency(4,10,PRINT);

	// optimize and check
	CSP::sharedValues solution = csp.optimalAssignment();
	CSP::Values expected;
	insert(expected)
	(csp.key(0,0), 0)(csp.key(0,1), 1)(csp.key(0,2), 2)(csp.key(0,3), 3)
	(csp.key(1,0), 2)(csp.key(1,1), 3)(csp.key(1,2), 0)(csp.key(1,3), 1)
	(csp.key(2,0), 3)(csp.key(2,1), 2)(csp.key(2,2), 1)(csp.key(2,3), 0)
	(csp.key(3,0), 1)(csp.key(3,1), 0)(csp.key(3,2), 3)(csp.key(3,3), 2);
	EXPECT(assert_equal(expected,*solution));
	//csp.printAssignment(solution);
}

/* ************************************************************************* */
TEST_UNSAFE( Sudoku, easy)
{
	Sudoku sudoku(9,
			0,0,5, 0,9,0, 0,0,1,
			0,0,0, 0,0,2, 0,7,3,
			7,6,0, 0,0,8, 2,0,0,

			0,1,2, 0,0,9, 0,0,4,
			0,0,0, 2,0,3, 0,0,0,
			3,0,0, 1,0,0, 9,6,0,

			0,0,1, 9,0,0, 0,5,8,
			9,7,0, 5,0,0, 0,0,0,
			5,0,0, 0,3,0, 7,0,0);

	// Do BP
	sudoku.runArcConsistency(4,10,PRINT);

	// sudoku.printSolution(); // don't do it
}

/* ************************************************************************* */
TEST_UNSAFE( Sudoku, extreme)
{
	Sudoku sudoku(9,
			0,0,9, 7,4,8, 0,0,0,
			7,0,0, 0,0,0, 0,0,0,
			0,2,0, 1,0,9, 0,0,0,

			0,0,7, 0,0,0, 2,4,0,
			0,6,4, 0,1,0, 5,9,0,
			0,9,8, 0,0,0, 3,0,0,

			0,0,0, 8,0,3, 0,2,0,
			0,0,0, 0,0,0, 0,0,6,
			0,0,0, 2,7,5, 9,0,0);

	// Do BP
	sudoku.runArcConsistency(9,10,PRINT);

#ifdef METIS
	VariableIndex index(sudoku);
	index.print("index");
	ofstream os("/Users/dellaert/src/hmetis-1.5-osx-i686/extreme-dual.txt");
  index.outputMetisFormat(os);
#endif

  //sudoku.printSolution(); // don't do it
}

/* ************************************************************************* */
TEST_UNSAFE( Sudoku, AJC_3star_Feb8_2012)
{
	Sudoku sudoku(9,
			9,5,0, 0,0,6, 0,0,0,
			0,8,4, 0,7,0, 0,0,0,
			6,2,0, 5,0,0, 4,0,0,

			0,0,0, 2,9,0, 6,0,0,
			0,9,0, 0,0,0, 0,2,0,
			0,0,2, 0,6,3, 0,0,0,

			0,0,9, 0,0,7, 0,6,8,
			0,0,0, 0,3,0, 2,9,0,
			0,0,0, 1,0,0, 0,3,7);

	// Do BP
	sudoku.runArcConsistency(9,10,PRINT);

	//sudoku.printSolution(); // don't do it
}

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


// Copyright 2004 The Trustees of Indiana University.

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_KAMADA_KAWAI_SPRING_LAYOUT_HPP
#define BOOST_GRAPH_KAMADA_KAWAI_SPRING_LAYOUT_HPP

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/type_traits/is_convertible.hpp>
#include <utility>
#include <iterator>
#include <vector>
#include <boost/limits.hpp>
#include <cmath>

namespace boost {
  namespace detail { namespace graph {
    /**
     * Denotes an edge or display area side length used to scale a
     * Kamada-Kawai drawing.
     */
    template<bool Edge, typename T>
    struct edge_or_side
    {
      explicit edge_or_side(T value) : value(value) {}

      T value;
    };

    /**
     * Compute the edge length from an edge length. This is trivial.
     */
    template<typename Graph, typename DistanceMap, typename IndexMap,
             typename T>
    T compute_edge_length(const Graph&, DistanceMap, IndexMap,
                          edge_or_side<true, T> length)
    { return length.value; }

    /**
     * Compute the edge length based on the display area side
       length. We do this by dividing the side length by the largest
       shortest distance between any two vertices in the graph.
     */
    template<typename Graph, typename DistanceMap, typename IndexMap,
             typename T>
    T
    compute_edge_length(const Graph& g, DistanceMap distance, IndexMap index,
                        edge_or_side<false, T> length)
    {
      T result(0);

      typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;

      for (vertex_iterator ui = vertices(g).first, end = vertices(g).second;
           ui != end; ++ui) {
        vertex_iterator vi = ui;
        for (++vi; vi != end; ++vi) {
          T dij = distance[get(index, *ui)][get(index, *vi)];
          if (dij > result) result = dij;
        }
      }
      return length.value / result;
    }

    /**
     * Implementation of the Kamada-Kawai spring layout algorithm.
     */
    template<typename Graph, typename PositionMap, typename WeightMap,
             typename EdgeOrSideLength, typename Done,
             typename VertexIndexMap, typename DistanceMatrix,
             typename SpringStrengthMatrix, typename PartialDerivativeMap>
    struct kamada_kawai_spring_layout_impl
    {
      typedef typename property_traits<WeightMap>::value_type weight_type;
      typedef std::pair<weight_type, weight_type> deriv_type;
      typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
      typedef typename graph_traits<Graph>::vertex_descriptor
        vertex_descriptor;

      kamada_kawai_spring_layout_impl(
        const Graph& g,
        PositionMap position,
        WeightMap weight,
        EdgeOrSideLength edge_or_side_length,
        Done done,
        weight_type spring_constant,
        VertexIndexMap index,
        DistanceMatrix distance,
        SpringStrengthMatrix spring_strength,
        PartialDerivativeMap partial_derivatives)
        : g(g), position(position), weight(weight),
          edge_or_side_length(edge_or_side_length), done(done),
          spring_constant(spring_constant), index(index), distance(distance),
          spring_strength(spring_strength),
          partial_derivatives(partial_derivatives) {}

      // Compute contribution of vertex i to the first partial
      // derivatives (dE/dx_m, dE/dy_m) (for vertex m)
      deriv_type
      compute_partial_derivative(vertex_descriptor m, vertex_descriptor i)
      {
#ifndef BOOST_NO_STDC_NAMESPACE
        using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE

        deriv_type result(0, 0);
        if (i != m) {
          weight_type x_diff = position[m].x - position[i].x;
          weight_type y_diff = position[m].y - position[i].y;
          weight_type dist = sqrt(x_diff * x_diff + y_diff * y_diff);
          result.first = spring_strength[get(index, m)][get(index, i)]
            * (x_diff - distance[get(index, m)][get(index, i)]*x_diff/dist);
          result.second = spring_strength[get(index, m)][get(index, i)]
            * (y_diff - distance[get(index, m)][get(index, i)]*y_diff/dist);
        }

        return result;
      }

      // Compute partial derivatives dE/dx_m and dE/dy_m
      deriv_type
      compute_partial_derivatives(vertex_descriptor m)
      {
#ifndef BOOST_NO_STDC_NAMESPACE
        using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE

        deriv_type result(0, 0);

        // TBD: looks like an accumulate to me
        std::pair<vertex_iterator, vertex_iterator> verts = vertices(g);
        for (/* no init */; verts.first != verts.second; ++verts.first) {
          vertex_descriptor i = *verts.first;
          deriv_type deriv = compute_partial_derivative(m, i);
          result.first += deriv.first;
          result.second += deriv.second;
        }

        return result;
      }

      // The actual Kamada-Kawai spring layout algorithm implementation
      bool run()
      {
#ifndef BOOST_NO_STDC_NAMESPACE
        using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE

        // Compute d_{ij} and place it in the distance matrix
        if (!johnson_all_pairs_shortest_paths(g, distance, index, weight,
                                              weight_type(0)))
          return false;

        // Compute L based on side length (if needed), or retrieve L
        weight_type edge_length =
          detail::graph::compute_edge_length(g, distance, index,
                                             edge_or_side_length);

        // Compute l_{ij} and k_{ij}
        const weight_type K = spring_constant;
        vertex_iterator ui, end = vertices(g).second;
        for (ui = vertices(g).first; ui != end; ++ui) {
          vertex_iterator vi = ui;
          for (++vi; vi != end; ++vi) {
            weight_type dij = distance[get(index, *ui)][get(index, *vi)];
            if (dij == (std::numeric_limits<weight_type>::max)())
              return false;
            distance[get(index, *ui)][get(index, *vi)] = edge_length * dij;
            distance[get(index, *vi)][get(index, *ui)] = edge_length * dij;
            spring_strength[get(index, *ui)][get(index, *vi)] = K/(dij*dij);
            spring_strength[get(index, *vi)][get(index, *ui)] = K/(dij*dij);
          }
        }

        // Compute Delta_i and find max
        vertex_descriptor p = *vertices(g).first;
        weight_type delta_p(0);

        for (ui = vertices(g).first; ui != end; ++ui) {
          deriv_type deriv = compute_partial_derivatives(*ui);
          put(partial_derivatives, *ui, deriv);

          weight_type delta =
            sqrt(deriv.first*deriv.first + deriv.second*deriv.second);

          if (delta > delta_p) {
            p = *ui;
            delta_p = delta;
          }
        }

        while (!done(delta_p, p, g, true)) {
          // The contribution p makes to the partial derivatives of
          // each vertex. Computing this (at O(n) cost) allows us to
          // update the delta_i values in O(n) time instead of O(n^2)
          // time.
          std::vector<deriv_type> p_partials(num_vertices(g));
          for (ui = vertices(g).first; ui != end; ++ui) {
            vertex_descriptor i = *ui;
            p_partials[get(index, i)] = compute_partial_derivative(i, p);
          }

          do {
            // Compute the 4 elements of the Jacobian
            weight_type dE_dx_dx = 0, dE_dx_dy = 0, dE_dy_dx = 0, dE_dy_dy = 0;
            for (ui = vertices(g).first; ui != end; ++ui) {
              vertex_descriptor i = *ui;
              if (i != p) {
                weight_type x_diff = position[p].x - position[i].x;
                weight_type y_diff = position[p].y - position[i].y;
                weight_type dist = sqrt(x_diff * x_diff + y_diff * y_diff);
                weight_type dist_cubed = dist * dist * dist;
                weight_type k_mi = spring_strength[get(index,p)][get(index,i)];
                weight_type l_mi = distance[get(index, p)][get(index, i)];
                dE_dx_dx += k_mi * (1 - (l_mi * y_diff * y_diff)/dist_cubed);
                dE_dx_dy += k_mi * l_mi * x_diff * y_diff / dist_cubed;
                dE_dy_dx += k_mi * l_mi * x_diff * y_diff / dist_cubed;
                dE_dy_dy += k_mi * (1 - (l_mi * x_diff * x_diff)/dist_cubed);
              }
            }

            // Solve for delta_x and delta_y
            weight_type dE_dx = get(partial_derivatives, p).first;
            weight_type dE_dy = get(partial_derivatives, p).second;

            weight_type delta_x =
              (dE_dx_dy * dE_dy - dE_dy_dy * dE_dx)
              / (dE_dx_dx * dE_dy_dy - dE_dx_dy * dE_dy_dx);

            weight_type delta_y =
              (dE_dx_dx * dE_dy - dE_dy_dx * dE_dx)
              / (dE_dy_dx * dE_dx_dy - dE_dx_dx * dE_dy_dy);

            // Move p by (delta_x, delta_y)
            position[p].x += delta_x;
            position[p].y += delta_y;

            // Recompute partial derivatives and delta_p
            deriv_type deriv = compute_partial_derivatives(p);
            put(partial_derivatives, p, deriv);

            delta_p =
              sqrt(deriv.first*deriv.first + deriv.second*deriv.second);
          } while (!done(delta_p, p, g, false));

          // Select new p by updating each partial derivative and delta
          vertex_descriptor old_p = p;
          for (ui = vertices(g).first; ui != end; ++ui) {
            deriv_type old_deriv_p = p_partials[get(index, *ui)];
            deriv_type old_p_partial =
              compute_partial_derivative(*ui, old_p);
            deriv_type deriv = get(partial_derivatives, *ui);

            deriv.first += old_p_partial.first - old_deriv_p.first;
            deriv.second += old_p_partial.second - old_deriv_p.second;

            put(partial_derivatives, *ui, deriv);
            weight_type delta =
              sqrt(deriv.first*deriv.first + deriv.second*deriv.second);

            if (delta > delta_p) {
              p = *ui;
              delta_p = delta;
            }
          }
        }

        return true;
      }

      const Graph& g;
      PositionMap position;
      WeightMap weight;
      EdgeOrSideLength edge_or_side_length;
      Done done;
      weight_type spring_constant;
      VertexIndexMap index;
      DistanceMatrix distance;
      SpringStrengthMatrix spring_strength;
      PartialDerivativeMap partial_derivatives;
    };
  } } // end namespace detail::graph

  /// States that the given quantity is an edge length.
  template<typename T>
  inline detail::graph::edge_or_side<true, T>
  edge_length(T x)
  { return detail::graph::edge_or_side<true, T>(x); }

  /// States that the given quantity is a display area side length.
  template<typename T>
  inline detail::graph::edge_or_side<false, T>
  side_length(T x)
  { return detail::graph::edge_or_side<false, T>(x); }

  /**
   * \brief Determines when to terminate layout of a particular graph based
   * on a given relative tolerance.
   */
  template<typename T = double>
  struct layout_tolerance
  {
    layout_tolerance(const T& tolerance = T(0.001))
      : tolerance(tolerance), last_energy((std::numeric_limits<T>::max)()),
        last_local_energy((std::numeric_limits<T>::max)()) { }

    template<typename Graph>
    bool
    operator()(T delta_p,
               typename boost::graph_traits<Graph>::vertex_descriptor p,
               const Graph& g,
               bool global)
    {
      if (global) {
        if (last_energy == (std::numeric_limits<T>::max)()) {
          last_energy = delta_p;
          return false;
        }

        T diff = last_energy - delta_p;
        if (diff < T(0)) diff = -diff;
        bool done = (delta_p == T(0) || diff / last_energy < tolerance);
        last_energy = delta_p;
        return done;
      } else {
        if (last_local_energy == (std::numeric_limits<T>::max)()) {
          last_local_energy = delta_p;
          return delta_p == T(0);
        }

        T diff = last_local_energy - delta_p;
        bool done = (delta_p == T(0) || (diff / last_local_energy) < tolerance);
        last_local_energy = delta_p;
        return done;
      }
    }

  private:
    T tolerance;
    T last_energy;
    T last_local_energy;
  };

  /** \brief Kamada-Kawai spring layout for undirected graphs.
   *
   * This algorithm performs graph layout (in two dimensions) for
   * connected, undirected graphs. It operates by relating the layout
   * of graphs to a dynamic spring system and minimizing the energy
   * within that system. The strength of a spring between two vertices
   * is inversely proportional to the square of the shortest distance
   * (in graph terms) between those two vertices. Essentially,
   * vertices that are closer in the graph-theoretic sense (i.e., by
   * following edges) will have stronger springs and will therefore be
   * placed closer together.
   *
   * Prior to invoking this algorithm, it is recommended that the
   * vertices be placed along the vertices of a regular n-sided
   * polygon.
   *
   * \param g (IN) must be a model of Vertex List Graph, Edge List
   * Graph, and Incidence Graph and must be undirected.
   *
   * \param position (OUT) must be a model of Lvalue Property Map,
   * where the value type is a class containing fields @c x and @c y
   * that will be set to the @c x and @c y coordinates of each vertex.
   *
   * \param weight (IN) must be a model of Readable Property Map,
   * which provides the weight of each edge in the graph @p g.
   *
   * \param edge_or_side_length (IN) provides either the unit length
   * @c e of an edge in the layout or the length of a side @c s of the
   * display area, and must be either @c boost::edge_length(e) or @c
   * boost::side_length(s), respectively.
   *
   * \param done (IN) is a 4-argument function object that is passed
   * the current value of delta_p (i.e., the energy of vertex @p p),
   * the vertex @p p, the graph @p g, and a boolean flag indicating
   * whether @p delta_p is the maximum energy in the system (when @c
   * true) or the energy of the vertex being moved. Defaults to @c
   * layout_tolerance instantiated over the value type of the weight
   * map.
   *
   * \param spring_constant (IN) is the constant multiplied by each
   * spring's strength. Larger values create systems with more energy
   * that can take longer to stabilize; smaller values create systems
   * with less energy that stabilize quickly but do not necessarily
   * result in pleasing layouts. The default value is 1.
   *
   * \param index (IN) is a mapping from vertices to index values
   * between 0 and @c num_vertices(g). The default is @c
   * get(vertex_index,g).
   *
   * \param distance (UTIL/OUT) will be used to store the distance
   * from every vertex to every other vertex, which is computed in the
   * first stages of the algorithm. This value's type must be a model
   * of BasicMatrix with value type equal to the value type of the
   * weight map. The default is a a vector of vectors.
   *
   * \param spring_strength (UTIL/OUT) will be used to store the
   * strength of the spring between every pair of vertices. This
   * value's type must be a model of BasicMatrix with value type equal
   * to the value type of the weight map. The default is a a vector of
   * vectors.
   *
   * \param partial_derivatives (UTIL) will be used to store the
   * partial derivates of each vertex with respect to the @c x and @c
   * y coordinates. This must be a Read/Write Property Map whose value
   * type is a pair with both types equivalent to the value type of
   * the weight map. The default is an iterator property map.
   *
   * \returns @c true if layout was successful or @c false if a
   * negative weight cycle was detected.
   */
  template<typename Graph, typename PositionMap, typename WeightMap,
           typename T, bool EdgeOrSideLength, typename Done,
           typename VertexIndexMap, typename DistanceMatrix,
           typename SpringStrengthMatrix, typename PartialDerivativeMap>
  bool
  kamada_kawai_spring_layout(
    const Graph& g,
    PositionMap position,
    WeightMap weight,
    detail::graph::edge_or_side<EdgeOrSideLength, T> edge_or_side_length,
    Done done,
    typename property_traits<WeightMap>::value_type spring_constant,
    VertexIndexMap index,
    DistanceMatrix distance,
    SpringStrengthMatrix spring_strength,
    PartialDerivativeMap partial_derivatives)
  {
    BOOST_STATIC_ASSERT((is_convertible<
                           typename graph_traits<Graph>::directed_category*,
                           undirected_tag*
                         >::value));

    detail::graph::kamada_kawai_spring_layout_impl<
      Graph, PositionMap, WeightMap,
      detail::graph::edge_or_side<EdgeOrSideLength, T>, Done, VertexIndexMap,
      DistanceMatrix, SpringStrengthMatrix, PartialDerivativeMap>
      alg(g, position, weight, edge_or_side_length, done, spring_constant,
          index, distance, spring_strength, partial_derivatives);
    return alg.run();
  }

  /**
   * \overload
   */
  template<typename Graph, typename PositionMap, typename WeightMap,
           typename T, bool EdgeOrSideLength, typename Done,
           typename VertexIndexMap>
  bool
  kamada_kawai_spring_layout(
    const Graph& g,
    PositionMap position,
    WeightMap weight,
    detail::graph::edge_or_side<EdgeOrSideLength, T> edge_or_side_length,
    Done done,
    typename property_traits<WeightMap>::value_type spring_constant,
    VertexIndexMap index)
  {
    typedef typename property_traits<WeightMap>::value_type weight_type;

    typename graph_traits<Graph>::vertices_size_type n = num_vertices(g);
    typedef std::vector<weight_type> weight_vec;

    std::vector<weight_vec> distance(n, weight_vec(n));
    std::vector<weight_vec> spring_strength(n, weight_vec(n));
    std::vector<std::pair<weight_type, weight_type> > partial_derivatives(n);

    return
      kamada_kawai_spring_layout(
        g, position, weight, edge_or_side_length, done, spring_constant, index,
        distance.begin(),
        spring_strength.begin(),
        make_iterator_property_map(partial_derivatives.begin(), index,
                                   std::pair<weight_type, weight_type>()));
  }

  /**
   * \overload
   */
  template<typename Graph, typename PositionMap, typename WeightMap,
           typename T, bool EdgeOrSideLength, typename Done>
  bool
  kamada_kawai_spring_layout(
    const Graph& g,
    PositionMap position,
    WeightMap weight,
    detail::graph::edge_or_side<EdgeOrSideLength, T> edge_or_side_length,
    Done done,
    typename property_traits<WeightMap>::value_type spring_constant)
  {
    return kamada_kawai_spring_layout(g, position, weight, edge_or_side_length,
                                      done, spring_constant,
                                      get(vertex_index, g));
  }

  /**
   * \overload
   */
  template<typename Graph, typename PositionMap, typename WeightMap,
           typename T, bool EdgeOrSideLength, typename Done>
  bool
  kamada_kawai_spring_layout(
    const Graph& g,
    PositionMap position,
    WeightMap weight,
    detail::graph::edge_or_side<EdgeOrSideLength, T> edge_or_side_length,
    Done done)
  {
    typedef typename property_traits<WeightMap>::value_type weight_type;
    return kamada_kawai_spring_layout(g, position, weight, edge_or_side_length,
                                      done, weight_type(1));
  }

  /**
   * \overload
   */
  template<typename Graph, typename PositionMap, typename WeightMap,
           typename T, bool EdgeOrSideLength>
  bool
  kamada_kawai_spring_layout(
    const Graph& g,
    PositionMap position,
    WeightMap weight,
    detail::graph::edge_or_side<EdgeOrSideLength, T> edge_or_side_length)
  {
    typedef typename property_traits<WeightMap>::value_type weight_type;
    return kamada_kawai_spring_layout(g, position, weight, edge_or_side_length,
                                      layout_tolerance<weight_type>(),
                                      weight_type(1.0),
                                      get(vertex_index, g));
  }
} // end namespace boost

#endif // BOOST_GRAPH_KAMADA_KAWAI_SPRING_LAYOUT_HPP


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

#include <boost/assign/list_of.hpp>
#include <boost/lexical_cast.hpp>
using boost::assign::list_of;
using boost::assign::map_list_of;

#include <evaluation/util/CoordinateDescentParameterOptimiser.h>
using namespace evaluation;

#include <tvgutil/numbers/NumberSequenceGenerator.h>
using namespace tvgutil;

//#################### HELPER FUNCTIONS ####################

float sum_squares_cost_fn(const ParamSet& params)
{
  float cost = 0.0f;
  for(std::map<std::string,std::string>::const_iterator it = params.begin(), iend = params.end(); it != iend; ++it)
  {
    float value = boost::lexical_cast<float>(it->second);
    cost += value * value;
  }
  return cost;
}

//#################### TESTS ####################

BOOST_AUTO_TEST_SUITE(test_CoordinateDescentParameterOptimiser)

BOOST_AUTO_TEST_CASE(optimise_for_parameters_test)
{
  // Set up the optimiser.
  const unsigned int seed = 12345;
  const size_t epochCount = 10;
  CoordinateDescentParameterOptimiser optimiser(sum_squares_cost_fn, epochCount, seed);
  optimiser.add_param("Foo", NumberSequenceGenerator::generate_stepped<float>(-5.5f, 1.5f, 5.0f))
           .add_param("Bar", NumberSequenceGenerator::generate_stepped<float>(-1000.0f, 1.0f, 5.0f))
           .add_param("Boo", list_of<float>(-10.0f)(-5.0f)(-2.0f)(0.0f)(5.0f)(15.0f))
           .add_param("Dum", list_of<float>(0.0f));

  // Use the optimiser to choose a set of parameters.
  float cost;
  ParamSet params = optimiser.optimise_for_parameters(&cost);

  // Check that the chosen parameters are as expected.
  ParamSet expectedParams = map_list_of("Foo",boost::lexical_cast<std::string>(0.5f))
                                       ("Bar",boost::lexical_cast<std::string>(0.0f))
                                       ("Boo",boost::lexical_cast<std::string>(0.0f))
                                       ("Dum",boost::lexical_cast<std::string>(0.0f));

  BOOST_CHECK_EQUAL(ParamSetUtil::param_set_to_string(params), ParamSetUtil::param_set_to_string(expectedParams));

  // Check that the cost of the chosen parameters is as expected.
  const float expectedCost = 0.25f;
  const float TOL = 1e-5f;
  BOOST_CHECK_CLOSE(cost, expectedCost, TOL);
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/test/unit_test.hpp>
#include "algorithms/math/parallel_lines.hpp"

BOOST_AUTO_TEST_SUITE(CheckIfParallelLines)

BOOST_AUTO_TEST_CASE(dots)
{
    Point dot;
    Line first(dot, dot);
    Line second(dot, dot);

    BOOST_CHECK(AreLinesParallel(first, second));
}

BOOST_AUTO_TEST_CASE(line_and_dot)
{
    Point dot;
    Line first(dot, dot);
    Point x(1, 1), y(2, 2);
    Line second(x, y);

    BOOST_CHECK(false == AreLinesParallel(first, second));
}

BOOST_AUTO_TEST_CASE(same_lines)
{
    Point x(1, 1), y(2, 2);
    Line first(x, y);
    Line second(x, y);

    BOOST_CHECK(AreLinesParallel(first, second));
}

BOOST_AUTO_TEST_CASE(parallel)
{
    Point x1(-12, 0), y1(0, 8);
    Line first(x1, y1);

    Point x2(0, -12), y2(18, 0);
    Line second(x2, y2);

    BOOST_CHECK(AreLinesParallel(first, second));
}

BOOST_AUTO_TEST_CASE(not_parallel)
{
    Point x1(-12, 0), y1(0, 8);
    Line first(x1, y1);

    Point x2(0, -12), y2(18, 5);
    Line second(x2, y2);

    BOOST_CHECK(false == AreLinesParallel(first, second));
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE "test_gocontact_potential"

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

#include <mjolnir/forcefield/local/GoContactPotential.hpp>

BOOST_AUTO_TEST_CASE(GoContact_double)
{
    using real_type = double;
    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;

    const real_type e  = 1.0;
    const real_type r0 = 5.0;

    mjolnir::GoContactPotential<real_type> Go(e, r0);

    const real_type x_min = 0.8 * r0;
    const real_type x_max = 5.0 * r0;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + i * dx;
        const real_type pot1 = Go.potential(x + h);
        const real_type pot2 = Go.potential(x - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = Go.derivative(x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(h));
    }
}

BOOST_AUTO_TEST_CASE(GoContact_float)
{
    using real_type = double;
    constexpr std::size_t N = 100;
    constexpr real_type   h = 1e-3;

    const real_type e  = 1.0;
    const real_type r0 = 5.0;

    mjolnir::GoContactPotential<real_type> Go(e, r0);

    const real_type x_min = 0.8 * r0;
    const real_type x_max = 5.0 * r0;
    const real_type dx = (x_max - x_min) / N;

    for(std::size_t i=0; i<N; ++i)
    {
        const real_type x    = x_min + i * dx;
        const real_type pot1 = Go.potential(x + h);
        const real_type pot2 = Go.potential(x - h);
        const real_type dpot = (pot1 - pot2) / (2 * h);
        const real_type deri = Go.derivative(x);

        BOOST_TEST(dpot == deri, boost::test_tools::tolerance(h));
    }
}


/**
 * \file boost/numeric/ublasx/operation/log10.hpp
 *
 * \brief Apply the \c std::log10 function to each element of a vector or
 *  matrix expression.
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */

#ifndef BOOST_NUMERIC_UBLASX_OPERATION_LOG10_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_LOG10_HPP

#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublasx/expression/matrix_unary_functor.hpp>
#include <boost/numeric/ublasx/expression/vector_unary_functor.hpp>
#include <cmath>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

namespace detail {

template <typename VectorExprT>
struct vector_log10_functor_traits
{
	typedef VectorExprT input_expression_type;
	typedef typename vector_traits<input_expression_type>::value_type signature_argument_type;
	typedef signature_argument_type signature_result_type;
	typedef vector_unary_functor_traits<
				input_expression_type,
				signature_result_type (signature_argument_type)
			> unary_functor_expression_type;
	typedef typename unary_functor_expression_type::result_type result_type;
	typedef typename unary_functor_expression_type::expression_type expression_type;
};

template <typename MatrixExprT>
struct matrix_log10_functor_traits
{
	typedef MatrixExprT input_expression_type;
	typedef typename matrix_traits<input_expression_type>::value_type signature_argument_type;
	typedef signature_argument_type signature_result_type;
	typedef matrix_unary_functor_traits<
				input_expression_type,
				signature_result_type (signature_argument_type)
			> unary_functor_expression_type;
	typedef typename unary_functor_expression_type::result_type result_type;
	typedef typename unary_functor_expression_type::expression_type expression_type;
};

namespace /*<unnamed>*/ {

/// Auxiliary function used to replace ::std::log2 when that is not available.
template <typename T>
BOOST_UBLAS_INLINE
T log10(T x)
{
	return ::std::log10(x);
}

} // Namespace <unnamed>

} // Namespace detail

/**
 * \brief Applies the \c std::log10 function to each element of a given vector
 *  expression.
 *
 * \tparam VectorExprT The type of the input vector expression.
 *
 * \param ve The input vector expression.
 * \return A vector expression representing the application of \c std::log10 to
 *  each element of \a ve.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename VectorExprT>
BOOST_UBLAS_INLINE
typename detail::vector_log10_functor_traits<VectorExprT>::result_type log10(vector_expression<VectorExprT> const& ve)
{
	typedef typename detail::vector_log10_functor_traits<VectorExprT>::expression_type expression_type;
	typedef typename detail::vector_log10_functor_traits<VectorExprT>::signature_result_type signature_result_type;

	return expression_type(ve(), detail::log10<signature_result_type>);
}

/**
 * \brief Applies the \c std::log10 function to each element of a given matrix
 *  expression.
 *
 * \tparam MatrixExprT The type of the input matrix expression.
 *
 * \param me The input matrix expression.
 * \return A matrix expression representing the application of \c std::log10 to
 *  each element of \a me.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename MatrixExprT>
BOOST_UBLAS_INLINE
typename detail::matrix_log10_functor_traits<MatrixExprT>::result_type log10(matrix_expression<MatrixExprT> const& me)
{
	typedef typename detail::matrix_log10_functor_traits<MatrixExprT>::expression_type expression_type;
	typedef typename detail::matrix_log10_functor_traits<MatrixExprT>::signature_result_type signature_result_type;

	return expression_type(me(), detail::log10<signature_result_type>);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_LOG10_HPP


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/math/find_polynomial_roots_jenkins_traub.h"

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <glog/logging.h>

#include <cmath>
#include <complex>
#include <limits>
#include <vector>

#include "theia/math/polynomial.h"
#include "theia/math/util.h"

namespace theia {

using Eigen::MatrixXd;
using Eigen::Vector3d;
using Eigen::VectorXd;
using Eigen::Vector3cd;
using Eigen::VectorXcd;

namespace {
// Machine precision constants.
static const double mult_eps = std::numeric_limits<double>::epsilon();
static const double sum_eps = std::numeric_limits<double>::epsilon();
static const double kAbsoluteTolerance = 1e-14;
static const double kRelativeTolerance = 1e-10;

enum class ConvergenceType {
  NO_CONVERGENCE = 0,
  LINEAR_CONVERGENCE = 1,
  QUADRATIC_CONVERGENCE = 2
};

// Perform division by a linear term of the form (z - x) and evaluate P at x.
void SyntheticDivisionAndEvaluate(const VectorXd& polynomial,
                                  const double x,
                                  VectorXd* quotient,
                                  double* eval) {
  quotient->setZero(polynomial.size() - 1);
  (*quotient)(0) = polynomial(0);
  for (int i = 1; i < polynomial.size() - 1; i++) {
    (*quotient)(i) = polynomial(i) + (*quotient)(i - 1) * x;
  }

  const VectorXd::ReverseReturnType& creverse_quotient = quotient->reverse();
  *eval = polynomial.reverse()(0) + creverse_quotient(0) * x;
}

// Perform division of a polynomial by a quadratic factor. The quadratic divisor
// should have leading 1s.
void QuadraticSyntheticDivision(const VectorXd& polynomial,
                                const VectorXd& quadratic_divisor,
                                VectorXd* quotient,
                                VectorXd* remainder) {
  CHECK_EQ(quadratic_divisor.size(), 3);
  CHECK_GE(polynomial.size(), 3);

  quotient->setZero(polynomial.size() - 2);
  remainder->setZero(2);

  (*quotient)(0) = polynomial(0);
  // If the quotient is a constant then polynomial is degree 2 and the math is
  // simple.
  if (quotient->size() == 1) {
    *remainder =
        polynomial.tail<2>() - polynomial(0) * quadratic_divisor.tail<2>();
    return;
  }

  (*quotient)(1) = polynomial(1) - polynomial(0) * quadratic_divisor(1);
  for (int i = 2; i < polynomial.size() - 2; i++) {
    (*quotient)(i) = polynomial(i) - (*quotient)(i - 2) * quadratic_divisor(2) -
        (*quotient)(i - 1) * quadratic_divisor(1);
  }

  const VectorXd::ReverseReturnType &creverse_quotient = quotient->reverse();
  (*remainder)(0) = polynomial.reverse()(1) -
                    quadratic_divisor(1) * creverse_quotient(0) -
                    quadratic_divisor(2) * creverse_quotient(1);
  (*remainder)(1) =
      polynomial.reverse()(0) - quadratic_divisor(2) * creverse_quotient(0);
}

// Determines whether the iteration has converged by examining the three most
// recent values for convergence.
template<typename T>
bool HasConverged(const T& sequence) {
  const bool convergence_condition_1 =
      std::abs(sequence(1) - sequence(0)) < std::abs(sequence(0)) / 2.0;
  const bool convergence_condition_2 =
      std::abs(sequence(2) - sequence(1)) < std::abs(sequence(1)) / 2.0;

  // If the sequence has converged then return true.
  return convergence_condition_1 && convergence_condition_2;
}

// Determines if the root has converged by measuring the relative and absolute
// change in the root value. This stopping criterion is a simple measurement
// that proves to work well. It is referred to as "Ward's method" in the
// following reference:
//
// Nikolajsen, Jorgen L. "New stopping criteria for iterative root finding."
// Royal Society open science (2014)
template <typename T>
bool HasRootConverged(const std::vector<T>& roots) {
  static const double kRootMagnitudeTolerance = 1e-8;

  if (roots.size() != 3) {
    return false;
  }

  const double e_i = std::abs(roots[2] - roots[1]);
  const double e_i_minus_1 = std::abs(roots[1] - roots[0]);
  const double mag_root = std::abs(roots[1]);
  if (e_i <= e_i_minus_1) {
    if (mag_root < kRootMagnitudeTolerance) {
      return e_i < kAbsoluteTolerance;
    } else {
      return e_i / mag_root <= kRelativeTolerance;
    }
  }

  return false;
}

// Implementation closely follows the three-stage algorithm for finding roots of
// polynomials with real coefficients as outlined in: "A Three-Stage Algorithm
// for Real Polynomaials Using Quadratic Iteration" by Jenkins and Traub, SIAM
// 1970. Please note that this variant is different than the complex-coefficient
// version, and is estimated to be up to 4 times faster.
class JenkinsTraubSolver {
 public:
  JenkinsTraubSolver(const VectorXd& coeffs,
                     VectorXd* real_roots,
                     VectorXd* complex_roots)
      : polynomial_(coeffs),
        real_roots_(real_roots),
        complex_roots_(complex_roots),
        num_solved_roots_(0) {}

  // Extracts the roots using the Jenkins Traub method.
  bool ExtractRoots();

 private:
  // Removes any zero roots and divides polynomial by z.
  void RemoveZeroRoots();

  // Computes the magnitude of the roots to provide and initial search radius
  // for the iterative solver.
  double ComputeRootRadius();

  // Computes the zero-shift applied to the K-Polynomial.
  void ComputeZeroShiftKPolynomial();

  // Stage 1 of the Jenkins-Traub method. This stage is not technically
  // necessary, but helps separate roots that are close to zero.
  void ApplyZeroShiftToKPolynomial(const int num_iterations);

  // Computes and returns the update of sigma(z) based on the current
  // K-polynomial.
  //
  // NOTE: This function is used by the fixed shift iterations (which hold sigma
  // constant) so sigma is *not* modified internally by this function. If you
  // want to change sigma, simply call
  //    sigma = ComputeNextSigma();
  VectorXd ComputeNextSigma();

  // Updates the K-polynomial based on the current value of sigma for the fixed
  // or variable shift stage.
  void UpdateKPolynomialWithQuadraticShift(
      const VectorXd& polynomial_quotient,
      const VectorXd& k_polynomial_quotient);

  // Apply fixed-shift iterations to the K-polynomial to separate the
  // roots. Based on the convergence of the K-polynomial, we apply a
  // variable-shift linear or quadratic iteration to determine a real root or
  // complex conjugate pair of roots respectively.
  ConvergenceType ApplyFixedShiftToKPolynomial(const std::complex<double>& root,
                                               const int max_iterations);

  // Applies one of the variable shifts to the K-Polynomial. Returns true upon
  // successful convergence to a good root, and false otherwise.
  bool ApplyVariableShiftToKPolynomial(
      const ConvergenceType& fixed_shift_convergence,
      const std::complex<double>& root);

  // Applies a quadratic shift to the K-polynomial to determine a pair of roots
  // that are complex conjugates. Return true if a root was successfully found.
  bool ApplyQuadraticShiftToKPolynomial(const std::complex<double>& root,
                                        const int max_iterations);

  // Applies a linear shift to the K-polynomial to determine a single real root.
  // Return true if a root was successfully found.
  bool ApplyLinearShiftToKPolynomial(const std::complex<double>& root,
                                     const int max_iterations);

  // These methods determine whether the root finding has converged based on the
  // machine roundoff error expected in evaluating the polynomials at the root.
  bool HasQuadraticSequenceConverged(const VectorXd& quotient,
                                     const std::complex<double>& root);
  bool HasLinearSequenceConverged(const VectorXd& quotient,
                                  const double root,
                                  const double p_at_root);

  // Adds the root to the output variables.
  void AddRootToOutput(const double real, const double imag);

  // Solves polynomials of degree <= 2.
  bool SolveClosedFormPolynomial();

  // Helper variables to manage the polynomials as they are being manipulated
  // and deflated.
  VectorXd polynomial_;
  VectorXd k_polynomial_;
  // Sigma is the quadratic factor the divides the K-polynomial.
  Vector3d sigma_;

  // Let us define a, b, c, and d such that:
  //   P(z) = Q_P * sigma(z) + b * (z + u) + a
  //   K(z) = Q_K * sigma(z) + d * (z + u ) + c
  //
  // where Q_P and Q_K are the quotients from polynomial division of
  // sigma(z). Note that this means for a given a root s of sigma:
  //
  //   P(s)      = a - b * s_conj
  //   P(s_conj) = a - b * s
  //   K(s)      = c - d * s_conj
  //   K(s_conj) = c - d * s
  double a_, b_, c_, d_;

  // Output reference variables.
  VectorXd* real_roots_;
  VectorXd* complex_roots_;
  int num_solved_roots_;

  // Keeps track of whether the linear and quadratic shifts have been attempted
  // yet so that we do not attempt the same shift twice.
  bool attempted_linear_shift_;
  bool attempted_quadratic_shift_;

  // Number of zero-shift iterations to perform.
  static const int kNumZeroShiftIterations = 20;

  // The number of fixed shift iterations is computed as
  //   # roots found * this multiplier.
  static const int kFixedShiftIterationMultiplier = 20;

  // If the fixed shift iterations fail to converge, we restart this many times
  // before considering the solve attempt as a failure.
  static const int kMaxFixedShiftRestarts = 20;

  // The maximum number of linear shift iterations to perform before considering
  // the shift as a failure.
  static const int kMaxLinearShiftIterations = 20;

  // The maximum number of quadratic shift iterations to perform before
  // considering the shift as a failure.
  static const int kMaxQuadraticShiftIterations = 20;

  // When quadratic shift iterations are stalling, we attempt a few fixed shift
  // iterations to help convergence.
  static const int kInnerFixedShiftIterations = 5;

  // During quadratic iterations, the real values of the root pairs should be
  // nearly equal since the root pairs are complex conjugates. This tolerance
  // measures how much the real values may diverge before consider the quadratic
  // shift to be failed.
  const double kRootPairTolerance = 0.01;
};

bool JenkinsTraubSolver::ExtractRoots() {
  if (polynomial_.size() == 0) {
    LOG(ERROR) << "Invalid polynomial of size 0 passed to "
                  "FindPolynomialRootsJenkinsTraub";
    return false;
  }

  // Remove any leading zeros of the polynomial.
  polynomial_ = RemoveLeadingZeros(polynomial_);

  const int degree = static_cast<int>(polynomial_.size()) - 1;

  // Allocate the output roots.
  if (real_roots_ != NULL) {
    real_roots_->setZero(degree);
  }
  if (complex_roots_ != NULL) {
    complex_roots_->setZero(degree);
  }

  // Normalize the polynomial.
  polynomial_ /= polynomial_(0);

  // Remove any zero roots.
  RemoveZeroRoots();

  // Choose the initial starting value for the root-finding on the complex
  // plane.
  double phi = DegToRad(49.0);

  // Iterate until the polynomial has been completely deflated.
  for (int i = 0; i < degree; i++) {
    // Compute the root radius.
    const double root_radius = ComputeRootRadius();

    // Solve in closed form if the polynomial is small enough.
    if (polynomial_.size() <= 3) {
      break;
    }

    // Stage 1: Apply zero-shifts to the K-polynomial to separate the small
    // zeros of the polynomial.
    ApplyZeroShiftToKPolynomial(kNumZeroShiftIterations);

    // Stage 2: Apply fixed shift iterations to the K-polynomial to separate the
    // roots further.
    std::complex<double> root;
    ConvergenceType convergence = ConvergenceType::NO_CONVERGENCE;
    for (int j = 0; j < kMaxFixedShiftRestarts; j++) {
      root = root_radius * std::complex<double>(std::cos(phi), std::sin(phi));
      convergence = ApplyFixedShiftToKPolynomial(
          root, kFixedShiftIterationMultiplier * (i + 1));

      if (convergence != ConvergenceType::NO_CONVERGENCE) {
        break;
      }

      // Rotate the initial root value on the complex plane and try again.
      phi += DegToRad(94.0);
    }

    // Stage 3: Find the root(s) with variable shift iterations on the
    // K-polynomial. If this stage was not successful then we return a failure.
    if (!ApplyVariableShiftToKPolynomial(convergence, root)) {
      return false;
    }
  }
  return SolveClosedFormPolynomial();
}

// Stage 1: Generate K-polynomials with no shifts (i.e. zero-shifts).
void JenkinsTraubSolver::ApplyZeroShiftToKPolynomial(
    const int num_iterations) {
  // K0 is the first order derivative of polynomial.
  k_polynomial_ = DifferentiatePolynomial(polynomial_) / polynomial_.size();
  for (int i = 1; i < num_iterations; i++) {
    ComputeZeroShiftKPolynomial();
  }
}

ConvergenceType JenkinsTraubSolver::ApplyFixedShiftToKPolynomial(
    const std::complex<double>& root, const int max_iterations) {
  // Compute the fixed-shift quadratic:
  // sigma(z) = (x - m - n * i) * (x - m + n * i) = x^2 - 2 * m + m^2 + n^2.
  sigma_(0) = 1.0;
  sigma_(1) = -2.0 * root.real();
  sigma_(2) = root.real() * root.real() + root.imag() * root.imag();

  // Compute the quotient and remainder for divinding P by the quadratic
  // divisor. Since this iteration involves a fixed-shift sigma these may be
  // computed once prior to any iterations.
  VectorXd polynomial_quotient, polynomial_remainder;
  QuadraticSyntheticDivision(
      polynomial_, sigma_, &polynomial_quotient, &polynomial_remainder);

  // Compute a and b from the above equations.
  b_ = polynomial_remainder(0);
  a_ = polynomial_remainder(1) - b_ * sigma_(1);

  // Precompute P(s) for later using the equation above.
  const std::complex<double> p_at_root = a_ - b_ * std::conj(root);

  // These two containers hold values that we test for convergence such that the
  // zero index is the convergence value from 2 iterations ago, the first
  // index is from one iteration ago, and the second index is the current value.
  Vector3cd t_lambda = Vector3cd::Zero();
  Vector3d sigma_lambda = Vector3d::Zero();
  VectorXd k_polynomial_quotient, k_polynomial_remainder;
  for (int i = 0; i < max_iterations; i++) {
    k_polynomial_ /= k_polynomial_(0);

    // Divide the shifted polynomial by the quadratic polynomial.
    QuadraticSyntheticDivision(
        k_polynomial_, sigma_, &k_polynomial_quotient, &k_polynomial_remainder);
    d_ = k_polynomial_remainder(0);
    c_ = k_polynomial_remainder(1) - d_ * sigma_(1);

    // Test for convergence.
    const VectorXd variable_shift_sigma = ComputeNextSigma();
    const std::complex<double> k_at_root = c_ - d_ * std::conj(root);

    t_lambda.head<2>() = t_lambda.tail<2>().eval();
    sigma_lambda.head<2>() = sigma_lambda.tail<2>().eval();
    t_lambda(2) = root - p_at_root / k_at_root;
    sigma_lambda(2) = variable_shift_sigma(2);

    // Return with the convergence code if the sequence has converged.
    if (HasConverged(sigma_lambda)) {
      return ConvergenceType::QUADRATIC_CONVERGENCE;
    } else if (HasConverged(t_lambda)) {
      return ConvergenceType::LINEAR_CONVERGENCE;
    }

    // Compute K_next using the formula above.
    UpdateKPolynomialWithQuadraticShift(polynomial_quotient,
                                        k_polynomial_quotient);
  }
  return ConvergenceType::NO_CONVERGENCE;
}

bool JenkinsTraubSolver::ApplyVariableShiftToKPolynomial(
    const ConvergenceType& fixed_shift_convergence,
    const std::complex<double>& root) {
  attempted_linear_shift_ = false;
  attempted_quadratic_shift_ = false;
  if (fixed_shift_convergence == ConvergenceType::LINEAR_CONVERGENCE) {
    return ApplyLinearShiftToKPolynomial(root, kMaxLinearShiftIterations);
  } else if (fixed_shift_convergence ==
             ConvergenceType::QUADRATIC_CONVERGENCE) {
    return ApplyQuadraticShiftToKPolynomial(root, kMaxQuadraticShiftIterations);
  }
  return false;
}

// Generate K-polynomials with variable-shifts. During variable shifts, the
// quadratic shift is computed as:
//                | K0(s1)  K0(s2)  z^2 |
//                | K1(s1)  K1(s2)    z |
//                | K2(s1)  K2(s2)    1 |
//    sigma(z) = __________________________
//                  | K1(s1)  K2(s1) |
//                  | K2(s1)  K2(s2) |
// Where K0, K1, and K2 are successive zero-shifts of the K-polynomial.
//
// The K-polynomial shifts are otherwise exactly the same as Stage 2 after
// accounting for a variable-shift sigma.
bool JenkinsTraubSolver::ApplyQuadraticShiftToKPolynomial(
    const std::complex<double>& root, const int max_iterations) {
  // Only proceed if we have not already tried a quadratic shift.
  if (attempted_quadratic_shift_) {
    return false;
  }

  const double kTinyRelativeStep = 0.01;

  // Compute the fixed-shift quadratic:
  // sigma(z) = (x - m - n * i) * (x - m + n * i) = x^2 - 2 * m + m^2 + n^2.
  sigma_(0) = 1.0;
  sigma_(1) = -2.0 * root.real();
  sigma_(2) = root.real() * root.real() + root.imag() * root.imag();

  // These two containers hold values that we test for convergence such that the
  // zero index is the convergence value from 2 iterations ago, the first
  // index is from one iteration ago, and the second index is the current value.
  VectorXd polynomial_quotient, polynomial_remainder, k_polynomial_quotient,
      k_polynomial_remainder;
  double poly_at_root(0), prev_poly_at_root(0), prev_v(0);
  bool tried_fixed_shifts = false;

  // These containers maintain a history of the predicted roots. The convergence
  // of the algorithm is determined by the convergence of the root value.
  std::vector<std::complex<double> > roots1, roots2;
  roots1.push_back(root);
  roots2.push_back(std::conj(root));
  for (int i = 0; i < max_iterations; i++) {
    // Terminate if the root evaluation is within our tolerance. This will
    // return false if we do not have enough samples.
    if (HasRootConverged(roots1) && HasRootConverged(roots2)) {
      AddRootToOutput(roots1[1].real(), roots1[1].imag());
      AddRootToOutput(roots2[1].real(), roots2[1].imag());
      polynomial_ = polynomial_quotient;
      return true;
    }

    QuadraticSyntheticDivision(
        polynomial_, sigma_, &polynomial_quotient, &polynomial_remainder);

    // Compute a and b from the above equations.
    b_ = polynomial_remainder(0);
    a_ = polynomial_remainder(1) - b_ * sigma_(1);

    // Solve for the roots of the quadratic factor sigma.
    std::complex<double> roots[2];
    VectorXd real, imag;
    FindQuadraticPolynomialRoots(sigma_, &real, &imag);
    roots[0] = std::complex<double>(real(0), imag(0));
    roots[1] = std::complex<double>(real(1), imag(1));

    // Check that the roots are close. If not, then try a linear shift.
    if (std::abs(std::abs(roots[0].real()) - std::abs(roots[1].real())) >
        kRootPairTolerance * std::abs(roots[1].real())) {
      return ApplyLinearShiftToKPolynomial(root, kMaxLinearShiftIterations);
    }

    // If the iteration is stalling at a root pair then apply a few fixed shift
    // iterations to help convergence.
    poly_at_root =
        std::abs(a_ - roots[0].real() * b_) + std::abs(roots[0].imag() * b_);
    const double rel_step = std::abs((sigma_(2) - prev_v) / sigma_(2));
    if (!tried_fixed_shifts && rel_step < kTinyRelativeStep &&
        prev_poly_at_root > poly_at_root) {
      tried_fixed_shifts = true;
      ApplyFixedShiftToKPolynomial(roots[0], kInnerFixedShiftIterations);
    }

    // Divide the shifted polynomial by the quadratic polynomial.
    QuadraticSyntheticDivision(
        k_polynomial_, sigma_, &k_polynomial_quotient, &k_polynomial_remainder);
    d_ = k_polynomial_remainder(0);
    c_ = k_polynomial_remainder(1) - d_ * sigma_(1);

    prev_v = sigma_(2);
    sigma_ = ComputeNextSigma();

    // Compute K_next using the formula above.
    UpdateKPolynomialWithQuadraticShift(polynomial_quotient,
                                        k_polynomial_quotient);
    k_polynomial_ /= k_polynomial_(0);
    prev_poly_at_root = poly_at_root;

    // Save the roots for convergence testing.
    roots1.push_back(roots[0]);
    roots2.push_back(roots[1]);
    if (roots1.size() > 3) {
      roots1.erase(roots1.begin());
      roots2.erase(roots2.begin());
    }
  }

  attempted_quadratic_shift_ = true;
  return ApplyLinearShiftToKPolynomial(root, kMaxLinearShiftIterations);
}

// Generate K-Polynomials with variable-shifts that are linear. The shift is
// computed as:
//   K_next(z) = 1 / (z - s) * (K(z) - K(s) / P(s) * P(z))
//   s_next = s - P(s) / K_next(s)
bool JenkinsTraubSolver::ApplyLinearShiftToKPolynomial(
    const std::complex<double>& root, const int max_iterations) {
  if (attempted_linear_shift_) {
    return false;
  }

  // Compute an initial guess for the root.
  double real_root = (root -
                      EvaluatePolynomial(polynomial_, root) /
                      EvaluatePolynomial(k_polynomial_, root)).real();

  VectorXd deflated_polynomial, deflated_k_polynomial;
  double polynomial_at_root(0), k_polynomial_at_root(0);

  // This container maintains a history of the predicted roots. The convergence
  // of the algorithm is determined by the convergence of the root value.
  std::vector<double> roots;
  roots.push_back(real_root);
  for (int i = 0; i < max_iterations; i++) {
    // Terminate if the root evaluation is within our tolerance. This will
    // return false if we do not have enough samples.
    if (HasRootConverged(roots)) {
      AddRootToOutput(roots[1], 0);
      polynomial_ = deflated_polynomial;
      return true;
    }

    const double prev_polynomial_at_root = polynomial_at_root;
    SyntheticDivisionAndEvaluate(
        polynomial_, real_root, &deflated_polynomial, &polynomial_at_root);

    // If the root is exactly the root then end early. Otherwise, the k
    // polynomial will be filled with inf or nans.
    if (std::abs(polynomial_at_root) <= kAbsoluteTolerance) {
      AddRootToOutput(roots[0], 0);
      polynomial_ = deflated_polynomial;
      return true;
    }

    // Update the K-Polynomial.
    SyntheticDivisionAndEvaluate(k_polynomial_, real_root,
                                 &deflated_k_polynomial, &k_polynomial_at_root);
    k_polynomial_ = AddPolynomials(
        deflated_k_polynomial,
        -k_polynomial_at_root / polynomial_at_root * deflated_polynomial);
    k_polynomial_ /= k_polynomial_(0);

    // Compute the update for the root estimation.
    k_polynomial_at_root = EvaluatePolynomial(k_polynomial_, real_root);
    const double delta_root = polynomial_at_root / k_polynomial_at_root;
    real_root -= delta_root;

    // Save the root so that convergence can be measured. Only the 3 most
    // recently root values are needed.
    roots.push_back(real_root);
    if (roots.size() > 3) {
      roots.erase(roots.begin());
    }

    // If the linear iterations appear to be stalling then we may have found a
    // double real root of the form (z - x^2). Attempt a quadratic variable
    // shift from the current estimate of the root.
    if (i >= 2 &&
        std::abs(delta_root) < 0.001 * std::abs(real_root) &&
        std::abs(prev_polynomial_at_root) < std::abs(polynomial_at_root)) {
      const std::complex<double> new_root(real_root, 0);
      return ApplyQuadraticShiftToKPolynomial(new_root,
                                              kMaxQuadraticShiftIterations);
    }
  }

  attempted_linear_shift_ = true;
  return ApplyQuadraticShiftToKPolynomial(root, kMaxQuadraticShiftIterations);
}

bool JenkinsTraubSolver::HasQuadraticSequenceConverged(
    const VectorXd& quotient, const std::complex<double>& root) {
  const double z = std::sqrt(std::abs(sigma_(2)));
  const double t = -root.real() * b_;

  double e = 2.0 * std::abs(quotient(0));
  for (int i = 1; i < quotient.size(); i++) {
    e = e * z + std::abs(quotient(i));
  }
  e = e * z + std::abs(a_ + t);
  e *= 5.0 * mult_eps + 4.0 * sum_eps;
  e = e -
      (5.0 * mult_eps + 2.0 * sum_eps) * (std::abs(a_ + t) + std::abs(b_) * z);
  e = e + 2.0 * sum_eps * std::abs(t);
  return std::abs(a_ - b_ * root) < e;
}

bool JenkinsTraubSolver::HasLinearSequenceConverged(const VectorXd& quotient,
                                                    const double root,
                                                    const double p_at_root) {
  double e = std::abs(quotient(0)) * mult_eps / (sum_eps + mult_eps);
  const double abs_root = std::abs(root);
  for (int i = 0; i < quotient.size(); i++) {
    e = e * abs_root + std::abs(quotient(i));
  }
  const double machine_precision =
      (sum_eps + mult_eps) * e - mult_eps * std::abs(p_at_root);
  return std::abs(p_at_root) < machine_precision;
}

void JenkinsTraubSolver::AddRootToOutput(const double real, const double imag) {
  if (real_roots_ != NULL) {
    (*real_roots_)(num_solved_roots_) = real;
  }
  if (complex_roots_ != NULL) {
    (*complex_roots_)(num_solved_roots_) = imag;
  }
  ++num_solved_roots_;
}

void JenkinsTraubSolver::RemoveZeroRoots() {
  int num_zero_roots = 0;

  const VectorXd::ReverseReturnType& creverse_polynomial =
      polynomial_.reverse();
  while (creverse_polynomial(num_zero_roots) == 0) {
    ++num_zero_roots;
  }

  // The output roots have 0 as the default value so there is no need to
  // explicitly add the zero roots.
  polynomial_ = polynomial_.head(polynomial_.size() - num_zero_roots).eval();
}

bool JenkinsTraubSolver::SolveClosedFormPolynomial() {
  const int degree = static_cast<int>(polynomial_.size()) - 1;

  // Is the polynomial constant?
  if (degree == 0) {
    LOG(WARNING) << "Trying to extract roots from a constant "
                 << "polynomial in FindPolynomialRoots";
    // We return true with no roots, not false, as if the polynomial is constant
    // it is correct that there are no roots. It is not the case that they were
    // there, but that we have failed to extract them.
    return true;
  }

  // Linear
  if (degree == 1) {
    AddRootToOutput(-polynomial_(1) / polynomial_(0), 0);
    return true;
  }

  // Quadratic
  if (degree == 2) {
    VectorXd real, imaginary;
    FindQuadraticPolynomialRoots(polynomial_, &real, &imaginary);
    AddRootToOutput(real(0), imaginary(0));
    AddRootToOutput(real(1), imaginary(1));
    return true;
  }

  return false;
}

// Computes a lower bound on the radius of the roots of polynomial by examining
// the Cauchy sequence:
//
//    z^n + |a_1| * z^{n - 1} + ... + |a_{n-1}| * z - |a_n|
//
// The unique positive zero of this polynomial is an approximate lower bound of
// the radius of zeros of the original polynomial.
double JenkinsTraubSolver::ComputeRootRadius() {
  static const double kEpsilon = 1e-2;
  static const int kMaxIterations = 100;

  VectorXd poly = polynomial_;
  // Take the absolute value of all coefficients.
  poly = poly.array().abs();
  // Negate the last coefficient.
  poly(poly.size() - 1) *= -1.0;

  // Find the unique positive zero using Newton-Raphson iterations.
  double x0 = 1.0;
  return FindRootIterativeNewton(poly, x0, kEpsilon, kMaxIterations);
}

// The k polynomial with a zero-shift is
//  (K(x) - K(0) / P(0) * P(x)) / x.
//
// This is equivalent to:
//    K(x) - K(0)      K(0)     P(x) - P(0)
//    ___________   -  ____  *  ___________
//         x           P(0)          x
//
// Note that removing the constant term and dividing by x is equivalent to
// shifting the polynomial to one degree lower in our representation.
void JenkinsTraubSolver::ComputeZeroShiftKPolynomial() {
  // Evaluating the polynomial at zero is equivalent to the constant term
  // (i.e. the last coefficient).
  const double polynomial_at_zero = polynomial_(polynomial_.size() - 1);
  const double k_at_zero = k_polynomial_(k_polynomial_.size() - 1);

  k_polynomial_ = AddPolynomials(k_polynomial_.head(k_polynomial_.size() - 1),
                                 -k_at_zero / polynomial_at_zero *
                                 polynomial_.head(polynomial_.size() - 1));
}

// The iterations are computed with the following equation:
//              a^2 + u * a * b + v * b^2
//   K_next =  ___________________________ * Q_K
//                    b * c - a * d
//
//                      a * c + u * a * d + v * b * d
//             +  (z - _______________________________) * Q_P + b.
//                              b * c - a * d
//
// This is done using *only* realy arithmetic so it can be done very fast!
void JenkinsTraubSolver::UpdateKPolynomialWithQuadraticShift(
    const VectorXd& polynomial_quotient,
    const VectorXd& k_polynomial_quotient) {
  const double coefficient_q_k =
      (a_ * a_ + sigma_(1) * a_ * b_ + sigma_(2) * b_ * b_) /
      (b_ * c_ - a_ * d_);
  VectorXd linear_polynomial(2);
  linear_polynomial(0) = 1.0;
  linear_polynomial(1) =
      -(a_ * c_ + sigma_(1) * a_ * d_ + sigma_(2) * b_ * d_) /
      (b_ * c_ - a_ * d_);
  k_polynomial_ = AddPolynomials(
      coefficient_q_k * k_polynomial_quotient,
      MultiplyPolynomials(linear_polynomial, polynomial_quotient));

  k_polynomial_(k_polynomial_.size() - 1) += b_;
}

// Using a bit of algebra, the update of sigma(z) can be computed from the
// previous value along with a, b, c, and d defined above. The details of this
// simplification can be found in "Three Stage Variable-Shift Iterations for the
// Solution of Polynomial Equations With a Posteriori Error Bounds for the
// Zeros" by M.A. Jenkins, Doctoral Thesis, Stanford Univeristy, 1969.
//
// NOTE: we assume the leading term of quadratic_sigma is 1.0.
VectorXd JenkinsTraubSolver::ComputeNextSigma() {
  const double u = sigma_(1);
  const double v = sigma_(2);
  const VectorXd::ReverseReturnType& creverse_k_polynomial =
      k_polynomial_.reverse();
  const VectorXd::ReverseReturnType& creverse_polynomial =
      polynomial_.reverse();

  const double b1 = -creverse_k_polynomial(0) / creverse_polynomial(0);
  const double b2 = -(creverse_k_polynomial(1) + b1 * creverse_polynomial(1)) /
                    creverse_polynomial(0);

  const double a1 = b_* c_ - a_ * d_;
  const double a2 = a_ * c_ + u * a_ * d_ + v * b_* d_;
  const double c2 = b1 * a2;
  const double c3 = b1 * b1 * (a_ * a_ + u * a_ * b_ + v * b_ * b_);
  const double c4 = v * b2 * a1 - c2 - c3;
  const double c1 = c_ * c_ + u * c_ * d_ + v * d_ * d_ +
                    b1 * (a_ * c_ + u * b_ * c_ + v * b_ * d_) - c4;
  const double delta_u = -(u * (c2 + c3) + v * (b1 * a1 + b2 * a2)) / c1;
  const double delta_v = v * c4 / c1;

  // Update u and v in the quadratic sigma.
  VectorXd new_quadratic_sigma(3);
  new_quadratic_sigma(0) = 1.0;
  new_quadratic_sigma(1) = u + delta_u;
  new_quadratic_sigma(2) = v + delta_v;
  return new_quadratic_sigma;
}

}  // namespace

bool FindPolynomialRootsJenkinsTraub(const VectorXd& polynomial,
                                     VectorXd* real_roots,
                                     VectorXd* complex_roots) {
  JenkinsTraubSolver solver(polynomial, real_roots, complex_roots);
  return solver.ExtractRoots();
}

}  // namespace theia


//Link to Boost
 #define BOOST_TEST_DYN_LINK

//VERY IMPORTANT - include this last
#include <boost/test/unit_test.hpp>

#include <cmath>
#include <boost/math/special_functions/gamma.hpp>
#include "test.h"
#include "../common.h"
#include "../CftData.h"

BOOST_FIXTURE_TEST_SUITE(CftData_suite, SimpleTestFixture, * utf::label("CftData"))

BOOST_DATA_TEST_CASE(CftData_construct_test, CreateCfgConfigTestData(TCNumber), cfg)
{
    CftData data(cfg);
    BOOST_TEST(data.D == cfg.D);
    BOOST_TEST(data.MaxPrimaryId() == Sum(cfg.OperatorNumbers));
    BOOST_TEST(data.MaxScalarId() == cfg.OperatorNumbers[0]);

    int stressTensorId = data.StressTensorId;
    BOOST_TEST(data.GetPrimaryInfo(stressTensorId).Spin == 2);
    BOOST_TEST(data.GetPrimaryInfo(stressTensorId).Dim == cfg.D);

    float_type Pi = acos(-1.0);
    for (int i = 1; i <= data.MaxScalarId(); i++) {
        MY_FLOAT_EQUAL(data.GetOpeCoefficient(i, i, 0), 1.0, tol);

        float_type prefactor = - data.D * data.GetPrimaryDim(i) / (data.D - 1);
        float_type sd = 2 * pow(Pi, data.D/2.0) / boost::math::tgamma<float_type>(data.D/2.0);
        MY_FLOAT_EQUAL(data.GetOpeCoefficient(i, i, data.StressTensorId), prefactor / sd, tol);

        for (int j = i + 1; j <= data.MaxScalarId(); j++) {
            BOOST_TEST(data.GetOpeCoefficient(i, j, 0) == 0.0);
            BOOST_TEST(data.GetOpeCoefficient(i, j, data.StressTensorId) == 0.0);
        }
    }
}

BOOST_DATA_TEST_CASE(CftData_SaveLoad_test, CreateCfgConfigTestData(TCNumber) ^ bdata::xrange(TCNumber), cfg, index)
{
    CftData data(cfg);

    string file = "CftData_SaveLoad_test" + ToString(index) + ".txt";
    data.Save(file);

    CftData data2;
    data2.LoadFromFile(file);

    BOOST_TEST(data2.D == data.D);
    BOOST_TEST(data2.StressTensorId == data.StressTensorId);
    BOOST_TEST(data2.MaxPrimaryId() == data.MaxPrimaryId());
    BOOST_TEST(data2.MaxScalarId() == data.MaxScalarId());

    for (int i = 0; i <= data.MaxPrimaryId(); i++) {
        BOOST_TEST(data2.GetPrimarySpin(i) == data.GetPrimarySpin(i));
        MY_FLOAT_EQUAL(data2.GetPrimaryDim(i), data.GetPrimaryDim(i), tol);
    }

    for (int i = 1; i <= data.MaxScalarId(); i++) {
        for (int j = i; j <= data.MaxScalarId(); j++) {
            MY_FLOAT_EQUAL(data2.GetOpeCoefficient(i, j, 0), data.GetOpeCoefficient(i, j, 0), tol);
        }
    }

    for (int i = 1; i <= data.MaxScalarId(); i++) {
        for (int j = i; j <= data.MaxScalarId(); j++) {
            for (int k = j; k <= data.MaxPrimaryId(); k++) {
                BOOST_TEST_INFO("i=" << i << ", j=" << j << ", k=" << k);
                MY_FLOAT_EQUAL(data2.GetOpeCoefficient(i, j, k), data.GetOpeCoefficient(i, j, k), tol);
            }
        }
    }

}

BOOST_DATA_TEST_CASE(CftData_PrimaryNumber_test, CreateCfgConfigTestData(TCNumber), cfg)
{
    CftData data(cfg);
    for (int i = 0; i < cfg.OperatorNumbers.size(); i++) {
        BOOST_TEST(data.PrimaryNumber(i) == cfg.OperatorNumbers[i]);
    }
}

BOOST_DATA_TEST_CASE(CftData_GetSetPrimaryInfo_test, CreateCfgConfigTestData(TCNumber), cfg)
{
    CftData data(cfg);
    int id = cfg.OperatorNumbers[0] + randomint(1, cfg.OperatorNumbers[1]);
    double dim = random(2.0, 4.0);

    data.SetPrimaryDim(id, dim);
    PrimaryInfo info = data.GetPrimaryInfo(id);

    BOOST_TEST(info.Id == id);
    BOOST_TEST(info.Spin == 1);
    MY_FLOAT_EQUAL(info.Dim, dim, tol);
}

BOOST_DATA_TEST_CASE(CftData_GetSetOpeCoefficient_test, CreateCfgConfigTestData(TCNumber), cfg)
{
    CftData data(cfg);
    int id1 = randomint(0, data.MaxPrimaryId());
    int id2 = randomint(0, data.MaxPrimaryId());
    int id3 = randomint(0, data.MaxPrimaryId());
    float_type expect = random(-2.0, 2.0);

    data.SetOpeCoefficient(id1, id2, id3, expect);
    float_type actual = data.GetOpeCoefficient(id2, id3, id1);
    MY_FLOAT_EQUAL(actual, expect, tol);
}

// test suite end
BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_IROUND_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_IROUND_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing iround capabilities

    Computes the integer conversion of the round of its parameter.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    as_integer_t<T> r = iround(x);
    @endcode

    is similar to:

    @code
    as_integer_t<T> r = toints(round(x));
    @endcode

  **/
  const boost::dispatch::functor<tag::iround_> iround = {};
} }
#endif

#include <boost/simd/function/scalar/iround.hpp>
#include <boost/simd/function/simd/iround.hpp>

#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testBayesTree.cpp
 * @brief   Unit tests for Bayes Tree
 * @author  Frank Dellaert
 * @author  Michael Kaess
 * @author  Viorela Ila
 */

#include <boost/assign/std/list.hpp> // for operator +=
#include <boost/assign/std/vector.hpp>
#include <boost/assign/list_of.hpp>
using namespace boost::assign;

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>

#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/inference/SymbolicSequentialSolver.h>

using namespace std;
using namespace gtsam;

///* ************************************************************************* */
//// SLAM example from RSS sqrtSAM paper
static const Index _x3_=0, _x2_=1;
//static const Index _x1_=2, _l2_=3, _l1_=4; // unused
//IndexConditional::shared_ptr
//		x3(new IndexConditional(_x3_)),
//		x2(new IndexConditional(_x2_,_x3_)),
//		x1(new IndexConditional(_x1_,_x2_,_x3_)),
//		l1(new IndexConditional(_l1_,_x1_,_x2_)),
//		l2(new IndexConditional(_l2_,_x1_,_x3_));
//
//// Bayes Tree for sqrtSAM example
//SymbolicBayesTree createSlamSymbolicBayesTree(){
//	// Create using insert
////	Ordering slamOrdering; slamOrdering += _x3_, _x2_, _x1_, _l2_, _l1_;
//	SymbolicBayesTree bayesTree_slam;
//	bayesTree_slam.insert(x3);
//	bayesTree_slam.insert(x2);
//	bayesTree_slam.insert(x1);
//	bayesTree_slam.insert(l2);
//	bayesTree_slam.insert(l1);
//	return bayesTree_slam;
//}

/* ************************************************************************* */
// Conditionals for ASIA example from the tutorial with A and D evidence
static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5;
static IndexConditional::shared_ptr
	B(new IndexConditional(_B_)),
	L(new IndexConditional(_L_, _B_)),
	E(new IndexConditional(_E_, _L_, _B_)),
	S(new IndexConditional(_S_, _L_, _B_)),
	T(new IndexConditional(_T_, _E_, _L_)),
	X(new IndexConditional(_X_, _E_));

// Cliques
static IndexConditional::shared_ptr
  ELB(IndexConditional::FromKeys(cref_list_of<3>(_E_)(_L_)(_B_), 3));

// Bayes Tree for Asia example
static SymbolicBayesTree createAsiaSymbolicBayesTree() {
	SymbolicBayesTree bayesTree;
//	Ordering asiaOrdering; asiaOrdering += _X_, _T_, _S_, _E_, _L_, _B_;
	SymbolicBayesTree::insert(bayesTree, B);
	SymbolicBayesTree::insert(bayesTree, L);
	SymbolicBayesTree::insert(bayesTree, E);
	SymbolicBayesTree::insert(bayesTree, S);
	SymbolicBayesTree::insert(bayesTree, T);
	SymbolicBayesTree::insert(bayesTree, X);
	return bayesTree;
}

/* ************************************************************************* */
TEST( BayesTree, constructor )
{
	// Create using insert
	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();

	// Check Size
	LONGS_EQUAL(4,bayesTree.size());

	// Check root
	boost::shared_ptr<IndexConditional> actual_root = bayesTree.root()->conditional();
	CHECK(assert_equal(*ELB,*actual_root));

	// Create from symbolic Bayes chain in which we want to discover cliques
	BayesNet<IndexConditional> ASIA;
	ASIA.push_back(X);
	ASIA.push_back(T);
	ASIA.push_back(S);
	ASIA.push_back(E);
	ASIA.push_back(L);
	ASIA.push_back(B);
	SymbolicBayesTree bayesTree2(ASIA);

	// Check whether the same
	CHECK(assert_equal(bayesTree,bayesTree2));

	// CHECK findParentClique, should *not depend on order of parents*
//	Ordering ordering; ordering += _X_, _T_, _S_, _E_, _L_, _B_;
//	IndexTable<Symbol> index(ordering);

	list<Index> parents1; parents1 += _E_, _L_;
	CHECK(assert_equal(_E_, bayesTree.findParentClique(parents1)));

	list<Index> parents2; parents2 += _L_, _E_;
	CHECK(assert_equal(_E_, bayesTree.findParentClique(parents2)));

	list<Index> parents3; parents3 += _L_, _B_;
	CHECK(assert_equal(_L_, bayesTree.findParentClique(parents3)));
}

/* ************************************************************************* */
TEST(BayesTree, clear)
{
//	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
//	bayesTree.clear();
//
//	SymbolicBayesTree expected;
//
//	// Check whether cleared BayesTree is equal to a new BayesTree
//	CHECK(assert_equal(expected, bayesTree));
}

/* ************************************************************************* *
Bayes Tree for testing conversion to a forest of orphans needed for incremental.
       A,B
   C|A    E|B
   D|C    F|E
   */
/* ************************************************************************* */
TEST( BayesTree, removePath )
{
  const Index _A_=5, _B_=4, _C_=3, _D_=2, _E_=1, _F_=0;
	IndexConditional::shared_ptr
			A(new IndexConditional(_A_)),
			B(new IndexConditional(_B_, _A_)),
			C(new IndexConditional(_C_, _A_)),
			D(new IndexConditional(_D_, _C_)),
			E(new IndexConditional(_E_, _B_)),
			F(new IndexConditional(_F_, _E_));
	SymbolicBayesTree bayesTree;
//	Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_;
	SymbolicBayesTree::insert(bayesTree, A);
	SymbolicBayesTree::insert(bayesTree, B);
	SymbolicBayesTree::insert(bayesTree, C);
	SymbolicBayesTree::insert(bayesTree, D);
	SymbolicBayesTree::insert(bayesTree, E);
	SymbolicBayesTree::insert(bayesTree, F);

	// remove C, expected outcome: factor graph with ABC,
	// Bayes Tree now contains two orphan trees: D|C and E|B,F|E
	SymbolicFactorGraph expected;
	expected.push_factor(_B_,_A_);
//	expected.push_factor(_A_);
	expected.push_factor(_C_,_A_);
	SymbolicBayesTree::Cliques expectedOrphans;
  expectedOrphans += bayesTree[_D_], bayesTree[_E_];

  BayesNet<IndexConditional> bn;
	SymbolicBayesTree::Cliques orphans;
	bayesTree.removePath(bayesTree[_C_], bn, orphans);
	SymbolicFactorGraph factors(bn);
  CHECK(assert_equal((SymbolicFactorGraph)expected, factors));
  CHECK(assert_equal(expectedOrphans, orphans));

  // remove E: factor graph with EB; E|B removed from second orphan tree
	SymbolicFactorGraph expected2;
  expected2.push_factor(_E_,_B_);
  SymbolicBayesTree::Cliques expectedOrphans2;
  expectedOrphans2 += bayesTree[_F_];

  BayesNet<IndexConditional> bn2;
	SymbolicBayesTree::Cliques orphans2;
  bayesTree.removePath(bayesTree[_E_], bn2, orphans2);
  SymbolicFactorGraph factors2(bn2);
  CHECK(assert_equal((SymbolicFactorGraph)expected2, factors2));
  CHECK(assert_equal(expectedOrphans2, orphans2));
}

/* ************************************************************************* */
TEST( BayesTree, removePath2 )
{
	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();

	// Call remove-path with clique B
	BayesNet<IndexConditional> bn;
	SymbolicBayesTree::Cliques orphans;
  bayesTree.removePath(bayesTree[_B_], bn, orphans);
	SymbolicFactorGraph factors(bn);

	// Check expected outcome
	SymbolicFactorGraph expected;
	expected.push_factor(_E_,_L_,_B_);
//	expected.push_factor(_L_,_B_);
//	expected.push_factor(_B_);
  CHECK(assert_equal(expected, factors));
	SymbolicBayesTree::Cliques expectedOrphans;
  expectedOrphans += bayesTree[_S_], bayesTree[_T_], bayesTree[_X_];
  CHECK(assert_equal(expectedOrphans, orphans));
}

/* ************************************************************************* */
TEST( BayesTree, removePath3 )
{
	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();

	// Call remove-path with clique S
	BayesNet<IndexConditional> bn;
	SymbolicBayesTree::Cliques orphans;
  bayesTree.removePath(bayesTree[_S_], bn, orphans);
	SymbolicFactorGraph factors(bn);

	// Check expected outcome
	SymbolicFactorGraph expected;
	expected.push_factor(_E_,_L_,_B_);
//	expected.push_factor(_L_,_B_);
//	expected.push_factor(_B_);
	expected.push_factor(_S_,_L_,_B_);
  CHECK(assert_equal(expected, factors));
	SymbolicBayesTree::Cliques expectedOrphans;
  expectedOrphans += bayesTree[_T_], bayesTree[_X_];
  CHECK(assert_equal(expectedOrphans, orphans));
}

void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, SymbolicBayesTree::Cliques& cliques)	{
	// Check if subtree exists
	if (subtree) {
		cliques.push_back(subtree);
		// Recursive call over all child cliques
		BOOST_FOREACH(SymbolicBayesTree::sharedClique& childClique, subtree->children()) {
			getAllCliques(childClique,cliques);
		}
	}
}

/* ************************************************************************* */
TEST( BayesTree, shortcutCheck )
{
  const Index _A_=6, _B_=5, _C_=4, _D_=3, _E_=2, _F_=1, _G_=0;
	IndexConditional::shared_ptr
			A(new IndexConditional(_A_)),
			B(new IndexConditional(_B_, _A_)),
			C(new IndexConditional(_C_, _A_)),
			D(new IndexConditional(_D_, _C_)),
			E(new IndexConditional(_E_, _B_)),
			F(new IndexConditional(_F_, _E_)),
			G(new IndexConditional(_G_, _F_));
	SymbolicBayesTree bayesTree;
//	Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_;
	SymbolicBayesTree::insert(bayesTree, A);
	SymbolicBayesTree::insert(bayesTree, B);
	SymbolicBayesTree::insert(bayesTree, C);
	SymbolicBayesTree::insert(bayesTree, D);
	SymbolicBayesTree::insert(bayesTree, E);
	SymbolicBayesTree::insert(bayesTree, F);
	SymbolicBayesTree::insert(bayesTree, G);

	//bayesTree.print("BayesTree");
	//bayesTree.saveGraph("BT1.dot");

	SymbolicBayesTree::sharedClique rootClique= bayesTree.root();
	//rootClique->printTree();
	SymbolicBayesTree::Cliques allCliques;
	getAllCliques(rootClique,allCliques);

	BayesNet<IndexConditional> bn;
	BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) {
		//clique->print("Clique#");
		bn = clique->shortcut(rootClique, &EliminateSymbolic);
		//bn.print("Shortcut:\n");
		//cout << endl;
	}

	// Check if all the cached shortcuts are cleared
	rootClique->deleteCachedShorcuts();
	BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) {
		bool notCleared = clique->cachedShortcut();
		CHECK( notCleared == false);
	}

//	BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) {
//		clique->print("Clique#");
//		if(clique->cachedShortcut()){
//			bn = clique->cachedShortcut().get();
//			bn.print("Shortcut:\n");
//		}
//		else
//			cout << "Not Initialized" << endl;
//		cout << endl;
//	}
}

/* ************************************************************************* */
TEST( BayesTree, removeTop )
{
	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();

	// create a new factor to be inserted
	boost::shared_ptr<IndexFactor> newFactor(new IndexFactor(_S_,_B_));

	// Remove the contaminated part of the Bayes tree
	BayesNet<IndexConditional> bn;
	SymbolicBayesTree::Cliques orphans;
	list<Index> keys; keys += _B_,_S_;
	bayesTree.removeTop(keys, bn, orphans);
	SymbolicFactorGraph factors(bn);

	// Check expected outcome
	SymbolicFactorGraph expected;
	expected.push_factor(_E_,_L_,_B_);
//	expected.push_factor(_L_,_B_);
//	expected.push_factor(_B_);
	expected.push_factor(_S_,_L_,_B_);
  CHECK(assert_equal(expected, factors));
	SymbolicBayesTree::Cliques expectedOrphans;
  expectedOrphans += bayesTree[_T_], bayesTree[_X_];
  CHECK(assert_equal(expectedOrphans, orphans));

  // Try removeTop again with a factor that should not change a thing
	boost::shared_ptr<IndexFactor> newFactor2(new IndexFactor(_B_));
	BayesNet<IndexConditional> bn2;
	SymbolicBayesTree::Cliques orphans2;
	keys.clear(); keys += _B_;
	bayesTree.removeTop(keys, bn2, orphans2);
	SymbolicFactorGraph factors2(bn2);
	SymbolicFactorGraph expected2;
  CHECK(assert_equal(expected2, factors2));
	SymbolicBayesTree::Cliques expectedOrphans2;
  CHECK(assert_equal(expectedOrphans2, orphans2));
}

/* ************************************************************************* */
TEST( BayesTree, removeTop2 )
{
	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();

	// create two factors to be inserted
	SymbolicFactorGraph newFactors;
	newFactors.push_factor(_B_);
	newFactors.push_factor(_S_);

	// Remove the contaminated part of the Bayes tree
	BayesNet<IndexConditional> bn;
	SymbolicBayesTree::Cliques orphans;
  list<Index> keys; keys += _B_,_S_;
	bayesTree.removeTop(keys, bn, orphans);
	SymbolicFactorGraph factors(bn);

	// Check expected outcome
	SymbolicFactorGraph expected;
	expected.push_factor(_E_,_L_,_B_);
//	expected.push_factor(_L_,_B_);
//	expected.push_factor(_B_);
	expected.push_factor(_S_,_L_,_B_);
  CHECK(assert_equal(expected, factors));
	SymbolicBayesTree::Cliques expectedOrphans;
  expectedOrphans += bayesTree[_T_], bayesTree[_X_];
	CHECK(assert_equal(expectedOrphans, orphans));
}

/* ************************************************************************* */
TEST( BayesTree, removeTop3 )
{
  const Index _x4_=5, _l5_=6;
	// simple test case that failed after COLAMD was fixed/activated
	IndexConditional::shared_ptr
	X(new IndexConditional(_l5_)),
	A(new IndexConditional(_x4_, _l5_)),
	B(new IndexConditional(_x2_, _x4_)),
	C(new IndexConditional(_x3_, _x2_));

//	Ordering newOrdering;
//	newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_;
	SymbolicBayesTree bayesTree;
	SymbolicBayesTree::insert(bayesTree, X);
	SymbolicBayesTree::insert(bayesTree, A);
	SymbolicBayesTree::insert(bayesTree, B);
	SymbolicBayesTree::insert(bayesTree, C);

	// remove all
	list<Index> keys;
	keys += _l5_, _x2_, _x3_, _x4_;
	BayesNet<IndexConditional> bn;
	SymbolicBayesTree::Cliques orphans;
	bayesTree.removeTop(keys, bn, orphans);
	SymbolicFactorGraph factors(bn);

	CHECK(orphans.size() == 0);
}
///* ************************************************************************* */
///**
// *  x2 - x3 - x4 - x5
// *   |  /       \   |
// *  x1 /				 \ x6
// */
//TEST( BayesTree, insert )
//{
//	// construct bayes tree by split the graph along the separator x3 - x4
//	const Index _x1_=0, _x2_=1, _x6_=2, _x5_=3, _x3_=4, _x4_=5;
//	SymbolicFactorGraph fg1, fg2, fg3;
//	fg1.push_factor(_x3_, _x4_);
//	fg2.push_factor(_x1_, _x2_);
//	fg2.push_factor(_x2_, _x3_);
//	fg2.push_factor(_x1_, _x3_);
//	fg3.push_factor(_x5_, _x4_);
//	fg3.push_factor(_x6_, _x5_);
//	fg3.push_factor(_x6_, _x4_);
//
////	Ordering ordering1; ordering1 += _x3_, _x4_;
////	Ordering ordering2; ordering2 += _x1_, _x2_;
////	Ordering ordering3; ordering3 += _x6_, _x5_;
//
//	BayesNet<IndexConditional> bn1, bn2, bn3;
//	bn1 = *SymbolicSequentialSolver::EliminateUntil(fg1, _x4_+1);
//	bn2 = *SymbolicSequentialSolver::EliminateUntil(fg2, _x2_+1);
//	bn3 = *SymbolicSequentialSolver::EliminateUntil(fg3, _x5_+1);
//
//	// insert child cliques
//	SymbolicBayesTree actual;
//	list<SymbolicBayesTree::sharedClique> children;
//	SymbolicBayesTree::sharedClique r1 = actual.insert(bn2, children);
//	SymbolicBayesTree::sharedClique r2 = actual.insert(bn3, children);
//
//	// insert root clique
//	children.push_back(r1);
//	children.push_back(r2);
//	actual.insert(bn1, children, true);
//
//	// traditional way
//	SymbolicFactorGraph fg;
//	fg.push_factor(_x3_, _x4_);
//	fg.push_factor(_x1_, _x2_);
//	fg.push_factor(_x2_, _x3_);
//	fg.push_factor(_x1_, _x3_);
//  fg.push_factor(_x5_, _x4_);
//  fg.push_factor(_x6_, _x5_);
//  fg.push_factor(_x6_, _x4_);
//
////	Ordering ordering;  ordering += _x1_, _x2_, _x6_, _x5_, _x3_, _x4_;
//	BayesNet<IndexConditional> bn(*SymbolicSequentialSolver(fg).eliminate());
//	SymbolicBayesTree expected(bn);
//	CHECK(assert_equal(expected, actual));
//
//}
/* ************************************************************************* */

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