/*
 * Refs:
 * https://eigen.tuxfamily.org/dox/index.html
 * https://dritchie.github.io/csci2240/assignments/eigen_tutorial.pdf
 * */

#include <iostream>
#include <Eigen/Dense>

#define PRINT(x) std::cout << #x << ": " << std::endl << (x) << std::endl << std::endl
#define PRINT_SIZE(x) std::cout << #x << " is of size " << x.rows() << "x" << x.cols() << std::endl << std::endl
#define SECTION(x) std::cout << "======================== " << x << " =======================" << std::endl << std::endl

int main() {

  using namespace Eigen;

  SECTION("Initialisations");
  // Different ways to initialise matrix
  Matrix<double , Dynamic, Dynamic, 0, 6, 8> m0 = Matrix4d::Ones();
  // the last three are optional, so we don't have to worry about it most of the time.
  // isRowMajor=0: default is column major, MaxRowsAtCompileTime, MaxColsAtCompileTime
  PRINT(m0);
  MatrixXd m1(4, 4); // typedef Matrix<double, Dynamic, Dynamic> MatrixXd;
  m1 = Matrix4d::Zero();
  PRINT(m1);
  // fix size matrix
  Matrix4d m2 = Matrix4d::Random(); // [-1, 1]
  PRINT(m2);
  Vector4d m3(1.0, 2.0, 3.0, 4.0); // typedef Matrix<double, 4, 1> Vector3d;
  PRINT(m3);
  MatrixXd v = Vector4d::Ones(); // vectors just one dimensional matrices
  PRINT(v);
  VectorXd m4 = Vector4d::Constant(108.5); // column vector
  PRINT(m4);
  RowVectorXd m4r = Vector4d::Constant(108.5); // row vector
  PRINT(m4r);

  SECTION("Store orders");
  // store orders
  // https://eigen.tuxfamily.org/dox/group__TopicStorageOrders.html
  Matrix<double, 4, 4, RowMajor> m5;
  m5 << 1, 2, 3, 4,  5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16; // comma-initializer syntax
  PRINT(m5);
  std::cout << "In memory (row-major):" << std::endl;
  for (int i = 0; i < m5.size(); i++)
    std::cout << *(m5.data() + i) << "  ";
  std::cout << std::endl << std::endl;

  Matrix<double, 4, 4, ColMajor> m6;
  m6 << 1, 2, 3, 4,  5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16;
  PRINT(m6);
  std::cout << "In memory (column-major):" << std::endl;
  for (int i = 0; i < m6.size(); i++)
    std::cout << *(m6.data() + i) << "  ";
  std::cout << std::endl << std::endl;

  Matrix<double, 4, 4> m7; // default is Column major
  m7 << 1, 2, 3, 4,  5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16;
  PRINT(m7);
  std::cout << "In memory (column-major) is default:" << std::endl;
  for (int i = 0; i < m7.size(); i++)
    std::cout << *(m7.data() + i) << "  ";
  std::cout << std::endl << std::endl;

  // ref: https://eigen.tuxfamily.org/dox/group__TutorialAdvancedInitialization.html
  MatrixXd m8(4, 8);
  m8 << m6, m7;
  PRINT(m8);

  MatrixXd m9(8, 4);
  m9 << m6, m7;
  PRINT(m9);

  SECTION("Block Operation");
  // block operation https://eigen.tuxfamily.org/dox/group__TutorialBlockOperations.html
  MatrixXd m10(5, 9);
  m10.row(0) << RowVectorXd::Ones(9)*100;
  m10.block(1, 0, 4, 4) << m6;
  m10.block<4, 4>(1, 4) = m7*2;
  m10.col(8).tail(4) << VectorXd::Ones(4)*99; // col and row are special case of block
  PRINT(m10);
  // some other keywords:
  // topLeftCorner, bottomLeftCorner, topRightCorner, bottomRightCorner, topRows, bottomRows, leftCols, rightCols.
  PRINT(m5);
  PRINT(m5.leftCols(2));
  PRINT(m5.bottomRows<2>());
  m5.topLeftCorner(1,3) = m5.bottomRightCorner(3,1).transpose();
  PRINT(m5);
  VectorXd v0(6);
  v0 << 1, 2, 3, 4, 5, 6;
  PRINT(v0.head(3));
  PRINT(v0.tail<3>());
  v0.segment(1,4) *= 2;
  PRINT(v0);

  SECTION("Diagonal");
  // Coefficient accessors
  m0(2, 2) = 100;
  PRINT(m0);
  PRINT(m0.diagonal());
  MatrixXd m0d1 = m0.diagonal().asDiagonal();
  // https://eigen.tuxfamily.org/dox/classEigen_1_1MatrixBase.html#a14235b62c90f93fe910070b4743782d0
  PRINT(m0d1);
  DiagonalMatrix<double, Dynamic> m0d2;
  // http://eigen.tuxfamily.org/dox/classEigen_1_1DiagonalMatrix.html
  m0d2.diagonal() = m0.diagonal();
  PRINT(MatrixXd(m0d2));

  m1 = Matrix4d::Identity(); // overwrite matrix values
  PRINT(m1);

  SECTION("Vector operations");
  // Vector operations
  Vector3f v1, v2;
  v1 = Vector3f::Random();
  v2 = Vector3f::Random();
  PRINT(v1);
  PRINT(v2);
  PRINT(v1 * v2.transpose());
  PRINT(v1.transpose() * v2);
  PRINT(v1.dot(v2));
  PRINT(v1.cross(Vector3f(3., 4., 6.).normalized()));
  PRINT(v1.cross(v2));

  Vector4f v3 = v1.homogeneous();
  PRINT(v3);
  PRINT(v3.hnormalized());
  // element-wise similar to matrix
  PRINT(v1.array().sin());

  SECTION("Matrix Operations");
  // Matrix Operations
  PRINT(m0 + m1);
  PRINT(m0 * m1);
  PRINT(m1 += m1);
  PRINT(m1 *= 2);
  PRINT(m0 - m1 * 2.2);
  // Check if two matrices are the same
  PRINT(m0 * m1 == m0 - m1 * 2.2);
  PRINT(m1 == Matrix4d::Identity()*4);

  PRINT(m6.transpose()); // this doesn't modify m6
  // NEVER DO "a = a.transpose()" aliasing issue: https://eigen.tuxfamily.org/dox/group__TutorialMatrixArithmetic.html
  // but matrix operation is fine, no problem! a = a*a;
  // https://eigen.tuxfamily.org/dox/group__TopicAliasing.html
  m6.transposeInPlace(); // we can do this instead
  PRINT(m6);
  PRINT(m6.inverse()); // if not invertible then shows NaN

  SECTION("Element-Wise");
  // element-wise
  PRINT(m6.array().square());
  PRINT(m6.array() * m6.transpose().array());
  PRINT(((m0 * m1).array() == (m0 - m1 * 2.2).array()));

  SECTION("Basic arithmetic reduction operations");
  // Basic arithmetic reduction operations
  // https://eigen.tuxfamily.org/dox/group__TutorialMatrixArithmetic.html
  // https://eigen.tuxfamily.org/dox/group__TutorialReductionsVisitorsBroadcasting.html
  PRINT(m1);
  PRINT(m1.sum());
  PRINT(m1.prod());
  PRINT(m1.mean());
  PRINT(m1.minCoeff());
  PRINT(m1.maxCoeff());
  PRINT(m1.trace());
  // visitors
  MatrixXd::Index maxRow, maxCol;
  double maxOfm1 = m1.maxCoeff(&maxRow, &maxCol);
  std::cout << maxOfm1 << " is at position: (" << maxRow << "," << maxCol << ")" << std::endl << std::endl;
  std::ptrdiff_t i, j;
  double minOfm1 = m1.minCoeff(&i, &j);
  std::cout << minOfm1 << " is at position: (" << i << "," << j << ")" << std::endl << std::endl;
  // partial reductions
  PRINT(m1.colwise().maxCoeff());
  PRINT(m1.rowwise().maxCoeff());
  PRINT(m1.cwiseSqrt());
  PRINT(m1.cwiseSqrt().colwise().maxCoeff());

  SECTION("Norm");
  PRINT(m1.squaredNorm());
  PRINT(m1.norm());
  // lp-norm
  PRINT(m1.lpNorm<2>());
  PRINT(m1.lpNorm<1>());
  PRINT(m1.lpNorm<Infinity>());
  // Operator norm: https://en.wikipedia.org/wiki/Operator_norm
  // 1-norm(m1)
  PRINT(m1.cwiseAbs().colwise().sum().maxCoeff());
  PRINT(m1.colwise().lpNorm<1>().maxCoeff());
  // Infinity-norm(m1)
  PRINT(m1.cwiseAbs().rowwise().sum().maxCoeff());
  PRINT(m1.rowwise().lpNorm<1>().maxCoeff());

  PRINT(m6);
  MatrixXd::Index maxIndex;
  double maxNorm = m6.colwise().sum().maxCoeff(&maxIndex);
  std::cout << "Maximum sum at position " << maxIndex << std::endl
            << "The corresponding vector is: " << std::endl
            << m6.col(maxIndex) << std::endl
            << "And its sum is is: " << maxNorm << std::endl << std::endl;

  SECTION("Broadcasting");
  // https://eigen.tuxfamily.org/dox/group__TutorialReductionsVisitorsBroadcasting.html
  MatrixXd mb = MatrixXd::Zero(2, 4);
  VectorXd vb = Vector2d(0, 1);
  PRINT(mb);
  PRINT(vb);
  mb.colwise() += vb;
  PRINT(mb);
  mb.setZero();
  vb = Vector4d(0, 1, 2, 3);
  mb.rowwise() += vb.transpose();
  PRINT(mb);
  PRINT(vb);
  // find nearest neighbour
  mb.setRandom();
  vb.setRandom(2);
  PRINT(mb);
  PRINT(vb);
  MatrixXf::Index index;
  (mb.colwise() - vb).colwise().squaredNorm().minCoeff(&index);
  std::cout << "Nearest neighbour is column " << index << ":" << std::endl;
  std::cout << mb.col(index) << std::endl;

  SECTION("Resizing");
  // resizing
  // https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html
  VectorXd v4 = Vector4d::Ones()*100;
  PRINT(v4);
  v4.conservativeResize(6); // this leave the old value untouched
  PRINT(v4);
  v4.resize(8);
  PRINT(v4);

  PRINT_SIZE(m1);
  m1 = MatrixXd::Identity(6, 6); // copy and resize of matrix with dynamic size
  PRINT(m1);
  PRINT_SIZE(m1);

  // change type of matrix
  MatrixXf m11 = m1.cast<float>();
  PRINT(m11);

  SECTION("Temporary objects");
  // using comma-initializer as temporary objects
  // https://eigen.tuxfamily.org/dox/group__TutorialAdvancedInitialization.html
  MatrixXf mat = MatrixXf::Random(2, 3);
  PRINT(mat);
  mat = (MatrixXf(2,2) << 0, 1, 1, 0).finished() * mat;
  PRINT(mat);
  // https://eigen.tuxfamily.org/dox/structEigen_1_1CommaInitializer.html#a3cf9e2b8a227940f50103130b2d2859a

}


/**
 * \file boost/numeric/ublasx/operation/ql.hpp
 *
 * \brief The QL matrix decomposition.
 *
 * Given an \f$m\f$-by-\f$n\f$ matrix \f$A\f$, its QL-decomposition is a matrix
 * decomposition of the form:
 * \f[
 *   A=QL
 * \f]
 * where \f$L\f$ is an m-by-n lower trapezoidal (or, when \f$m \ge n\f$,
 * triangular) matrix and \f$Q\f$ is an m-by-m orthogonal (or unitary) matrix,
 * that is one satisfying:
 * \f[
 *  Q^{T}Q=I
 * \f]
  where \f$Q^{T}\f$ is the transpose of \f$Q\f$ and \f$I\f$ is the identity
 * matrix.
 *
 * For the special case of \f$m \ge n\f$, the factorization can be rewritten as:
 * \f[
 *  A=\begin{pmatrix}
 *     Q_1 & Q_2
 *     \end{pmatrix}
 *     \begin{pmatrix}
 *     L_1 \\
 *     L_2 \\
 *     \end{pmatrix}
 *   =\begin{pmatrix}
 *     Q_1 & Q_2
 *     \end{pmatrix}
 *     \begin{pmatrix}
 *     0 \\
 *     L_2 \\
 *     \end{pmatrix}
 *   = Q_2 L_2
 * \f]
 * where \f$Q_1\f$ is an m-by-(m-n) matrix, \f$Q_2\f$ is an m-by-n matrix,
 * \f$L_1\f$ is an (m-n)-by-n zero matrix, and \f$L_2\f$ is an n-by-n lower
 * triangular matrix.
 *
 * The QL factorization is particular useful for computing minimum-phase
 * filters.
 *
 * <hr/>
 *
 * Copyright (c) 2010, 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_QL_HPP
#define BOOST_NUMERIC_UBLASX_OPERATION_QL_HPP

#include <algorithm>
#include <boost/mpl/and.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/numeric/bindings/lapack/computational/geqlf.hpp>
#include <boost/numeric/bindings/lapack/computational/orgql.hpp>
#include <boost/numeric/bindings/lapack/computational/ormql.hpp>
#include <boost/numeric/bindings/lapack/computational/ungql.hpp>
#include <boost/numeric/bindings/ublas.hpp>
#include <boost/numeric/bindings/tag.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/ublas/detail/temporary.hpp>
#include <boost/numeric/ublas/expression_types.hpp>
#include <boost/numeric/ublas/fwd.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/traits.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_expression.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <boost/numeric/ublasx/operation/size.hpp>
#include <boost/numeric/ublasx/traits/layout_type.hpp>
#include <boost/type_traits/is_same.hpp>
#include <complex>
#include <cstddef>
#include <stdint.h>

namespace boost { namespace numeric { namespace ublasx {

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

namespace detail { namespace /*<unnamed>*/ {

struct ql_decomposition_impl_common;

/**
 * \brief Type-oriented operations for QL decomposition.
 *
 * \tparam IsComplex Logical parameter telling if the we are doing either a real
 *  or a complex QL decomposition.
 *
 * This class makes distinction between the real and the complex case.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <bool IsComplex>
struct ql_decomposition_impl;

/**
 * \brief Common operations for QL decomposition.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
struct ql_decomposition_impl_common
{
	/// Performan QL decomposition of the given input matrix \a A
	/// (row-major case).
	template <typename AMatrixT, typename TauVectorT>
		static void decompose(AMatrixT& A, TauVectorT& tau, row_major_tag)
	{
		matrix<typename matrix_traits<AMatrixT>::value_type, column_major> tmp_A(A);

		decompose(tmp_A, tau, column_major_tag());

		A = tmp_A;
	}

	/// Performan QL decomposition of the given input matrix \a A
	/// (column-major case).
	template <typename AMatrixT, typename TauVectorT>
		static void decompose(AMatrixT& A, TauVectorT& tau, column_major_tag)
	{
		typedef typename matrix_traits<AMatrixT>::size_type size_type;

		size_type m = num_rows(A);
		size_type n = num_columns(A);
		size_type k = ::std::min(m,n);

		if (size(tau) != k)
		{
			tau.resize(k, false);
		}

		::boost::numeric::bindings::lapack::geqlf(A, tau);
	}

	/// Extract the L matrix from a previously computing QL decomposition
	/// (row-major case).
	template <typename QLMatrixT, typename LMatrixT>
		static void extract_L(QLMatrixT const& QL, LMatrixT& L, bool full, row_major_tag)
	{
		matrix<typename matrix_traits<QLMatrixT>::value_type, column_major> tmp_QL(QL);
		matrix<typename matrix_traits<LMatrixT>::value_type, column_major> tmp_L(L);

		extract_L(tmp_QL, tmp_L, full, column_major_tag());

		L = tmp_L;
	}

	/**
	 * \brief Extract the L matrix from a previously computing QL decomposition
	 * (column-major case).
	 *
	 * Let QL be an m-by-n matrix, then the L matrix is built as:
	 * - If m >= n, the lower triangle of the submatrix QL(m-n+1:m,1:n)
	 *   contains the n-by-n lower triangular matrix L;
	 * - if m <= n, the elements on and below the (n-m)-th
	 *   superdiagonal contain the m-by-n lower trapezoidal matrix L.
	 * .
	 */
	template <typename QLMatrixT, typename LMatrixT>
		static void extract_L(QLMatrixT const& QL, LMatrixT& L, bool full, column_major_tag)
	{
		typedef typename matrix_traits<LMatrixT>::size_type size_type;
		typedef typename matrix_traits<LMatrixT>::value_type value_type;

		size_type m = num_rows(QL);
		size_type n = num_columns(QL);
		size_type nr = full ? m : ::std::min(m,n);

		if (num_rows(L) != nr && num_columns(L) != n)
		{
			L.resize(nr, n, false);
		}

		//::std::fill(L.data().begin(), L.data().end(), value_type/*zero*/());
		if (m >= n)
		{
			size_type k = m-n;
			size_type kr = k;

			// Set to zero the first m-n rows
			if (full)
			{
				subrange(L, 0, k, 0, n) = scalar_matrix<value_type>(k, n, value_type/*zero*/());
//				for (size_type row = 0; row < k; ++row)
//				{
//					for(size_type col = 0; col < n; ++col)
//					{
//						L(row,col) = value_type/*zero*/();
//					}
//				}
				kr = 0;
			}
			// the lower triangle of the submatrix QL(m-n+1:m,1:n) contains the
			// n-by-n lower triangular matrix L
			for (size_type row = k; row < m; ++row)
			{
				for (size_type col = 0; col < n; ++col)
				{
					if (col <= (row-k))
					{
						L(row-kr,col) = QL(row,col);
					}
					else
					{
						L(row-kr,col) = value_type/*zero*/();
					}
				}
			}
		}
		else
		{
			 // the elements on and below the (n-m)-th
			 // superdiagonal contain the m-by-n lower trapezoidal matrix L.
			size_type k = n-m;
			for (size_type row = 0; row < m; ++row)
			{
				for(size_type col = 0; col < n; ++col)
				{
					if (col <= (row+k))
					{
						L(row,col) = QL(row,col);
					}
					else
					{
						L(row,col) = value_type/*zero*/();
					}
				}
			}
		}
	}

	/**
	 * \brief Multiply the given \a C matrix by the \c Q matrix obtained from
	 *  the QL decomposition.
	 *
	 * \tparam QLMatrixT The type of the \a QL matrix.
	 * \tparam TAUMatrixT The type of the \a tau vector.
	 * \tparam CMatrixT The type of the \a C matrix.
	 *
	 * \param QL The matrix obtained by the QL decomposition such that the i-th
	 *  column contains the vector which defines the elementary reflector
	 *  \f$H(i)\f$, for \f$i = 1,2,\ldots,k\f$.
	 * \param tau The vector obtained by the QL decomposition containing the
	 *  scalar factors of the elementary reflectors \f$H(i)\f$, for
	 *  \f$i=1,2,\ldots,k\f$.
	 * \param left_Q A boolean value indicating which side of the product the
	 *  matrix \c Q will occupy. A \c true value indicates that \c Q is the left
	 *  operand, while a \c false value indicates that \c Q is the right
	 *  operand.
	 * \param trans_Q A boolean value indicating if the matrix \c Q is to be
	 *  transposed. A \c true value indicates that \c Q is to be transposed,
	 *  while a \c false value indicates that \c Q is to be taken as-is.
	 * \param orientation The matrix orientation fixed to row-major.
	 *
	 * Let \c Q be the matrix obtained from the QL decomposition represented
	 * by the \a QL matrix and the \a tau vector parameters.
	 * Then this function computes the following matrix product:
	 * \f{equation*}{
	 *   \begin{cases}
	 *   Q C, & \text{\texttt{left\_Q} = \emph{true} and \texttt{trans\_Q} = \emph{false}}, \\
	 *   Q^T C, & \text{\texttt{left\_Q} = \emph{true} and \texttt{trans\_Q} = \emph{true}}, \\
	 *   C Q, & \text{\texttt{left\_Q} = \emph{false} and \texttt{trans\_Q} = \emph{false}}, \\
	 *   C Q^T, & \text{\texttt{left\_Q} = \emph{false} and \texttt{trans\_Q} = \emph{true}}.
	 *  \end{cases}
	 * \f}
	 */
	template <typename QLMatrixT, typename TAUVectorT, typename CMatrixT>
		static void prod(QLMatrixT& QL, TAUVectorT const& tau, CMatrixT& C, bool left_Q, bool trans_Q, row_major_tag)
	{
		//NOTE: QL cannot be const since LAPACK::ORMQL modified it, restoring
		//      it at the end of the function.

		matrix<typename matrix_traits<QLMatrixT>::value_type, column_major> tmp_QL(QL);
		matrix<typename matrix_traits<CMatrixT>::value_type, column_major> tmp_C(C);

		prod(tmp_QL, tau, tmp_C, left_Q, trans_Q, column_major_tag());

		C = tmp_C;
	}

	/**
	 * \brief Multiply the given \a C matrix by the \c Q matrix obtained from
	 *  the QL decomposition.
	 *
	 * \tparam QLMatrixT The type of the \a QL matrix.
	 * \tparam TAUMatrixT The type of the \a tau vector.
	 * \tparam CMatrixT The type of the \a C matrix.
	 *
	 * \param QL The matrix obtained by the QL decomposition such that the i-th
	 *  column contains the vector which defines the elementary reflector
	 *  \f$H(i)\f$, for \f$i = 1,2,\ldots,k\f$.
	 * \param tau The vector obtained by the QL decomposition containing the
	 *  scalar factors of the elementary reflectors \f$H(i)\f$, for
	 *  \f$i=1,2,\ldots,k\f$.
	 * \param left_Q A boolean value indicating which side of the product the
	 *  matrix \c Q will occupy. A \c true value indicates that \c Q is the left
	 *  operand, while a \c false value indicates that \c Q is the right
	 *  operand.
	 * \param trans_Q A boolean value indicating if the matrix \c Q is to be
	 *  transposed. A \c true value indicates that \c Q is to be transposed,
	 *  while a \c false value indicates that \c Q is to be taken as-is.
	 * \param orientation The matrix orientation fixed to column-major.
	 *
	 * Let \c Q be the matrix obtained from the QL decomposition represented
	 * by the \a QL matrix and the \a tau vector parameters.
	 * Then this function computes the following matrix product:
	 * \f{equation*}{
	 *   \begin{cases}
	 *   Q C, & \text{\texttt{left\_Q} = \emph{true} and \texttt{trans\_Q} = \emph{false}}, \\
	 *   Q^T C, & \text{\texttt{left\_Q} = \emph{true} and \texttt{trans\_Q} = \emph{true}}, \\
	 *   C Q, & \text{\texttt{left\_Q} = \emph{false} and \texttt{trans\_Q} = \emph{false}}, \\
	 *   C Q^T, & \text{\texttt{left\_Q} = \emph{false} and \texttt{trans\_Q} = \emph{true}}.
	 *  \end{cases}
	 * \f}
	 */
	template <typename QLMatrixT, typename TAUVectorT, typename CMatrixT>
		static void prod(QLMatrixT& QL, TAUVectorT const& tau, CMatrixT& C, bool left_Q, bool trans_Q, column_major_tag /*orientation*/)
	{
		//NOTE: QL cannot be const since LAPACK::ORMQL modified it, restoring
		//      it at the end of the function.

//		typedef typename matrix_traits<QLMatrixT>::value_type value_type;
//		typedef typename matrix_traits<QLMatrixT>::size_type size_type;
//		typedef typename type_traits<value_type>::real_type real_type;
//
//		const ::fortran_int_t m = num_rows(C);
//		const ::fortran_int_t n = num_columns(C);
//		const ::fortran_int_t k = size(tau);
//		const ::fortran_int_t lda = num_rows(QL);
//		const ::fortran_int_t ldc = m;
//		real_type* work;
//		real_type opt_work_size;
//		::fortran_int_t lwork;
//		::std::ptrdiff_t info;

		if (left_Q)
		{
			if (trans_Q)
			{
//				//FIXME: actually (2010-08-13) bindinds::lapack::ormql has problems
//				info = ::boost::numeric::bindings::lapack::detail::ormql(
//					::boost::numeric::bindings::tag::left(),
//					::boost::numeric::bindings::tag::transpose(),
//					m,
//					n,
//					k,
//					QL.data().begin(),
//					lda,
//					tau.data().begin(),
//					C.data().begin(),
//					ldc,
//					&opt_work_size,
//					-1
//				);
//				lwork = static_cast< ::fortran_int_t >(opt_work_size);
//				work = new real_type[lwork];
//				info = ::boost::numeric::bindings::lapack::detail::ormql(
//					::boost::numeric::bindings::tag::left(),
//					::boost::numeric::bindings::tag::transpose(),
//					m,
//					n,
//					k,
//					QL.data().begin(),
//					lda,
//					tau.data().begin(),
//					C.data().begin(),
//					ldc,
//					work,
//					lwork
//				);
//				delete[] work;
				::boost::numeric::bindings::lapack::ormql(
					::boost::numeric::bindings::tag::left(),
					::boost::numeric::bindings::trans(QL),
					tau,
					C
				);
			}
			else
			{
//				//FIXME: actually (2010-08-13) bindinds::lapack::ormql has problems
//				info = ::boost::numeric::bindings::lapack::detail::ormql(
//					::boost::numeric::bindings::tag::left(),
//					::boost::numeric::bindings::tag::no_transpose(),
//					m,
//					n,
//					k,
//					QL.data().begin(),
//					lda,
//					tau.data().begin(),
//					C.data().begin(),
//					ldc,
//					&opt_work_size,
//					-1
//				);
//				lwork = static_cast< ::fortran_int_t >(opt_work_size);
//				work = new real_type[lwork];
//				info = ::boost::numeric::bindings::lapack::detail::ormql(
//					::boost::numeric::bindings::tag::left(),
//					::boost::numeric::bindings::tag::no_transpose(),
//					m,
//					n,
//					k,
//					QL.data().begin(),
//					lda,
//					tau.data().begin(),
//					C.data().begin(),
//					ldc,
//					work,
//					lwork
//				);
//				delete[] work;
				::boost::numeric::bindings::lapack::ormql(
					::boost::numeric::bindings::tag::left(),
					QL,
					tau,
					C
				);
			}
		}
		else
		{
			if (trans_Q)
			{
//				//FIXME: actually (2010-08-13) bindinds::lapack::ormql has problems
//				info = ::boost::numeric::bindings::lapack::detail::ormql(
//					::boost::numeric::bindings::tag::right(),
//					::boost::numeric::bindings::tag::transpose(),
//					m,
//					n,
//					k,
//					QL.data().begin(),
//					lda,
//					tau.data().begin(),
//					C.data().begin(),
//					ldc,
//					&opt_work_size,
//					-1
//				);
//				lwork = static_cast< ::fortran_int_t >(opt_work_size);
//				work = new real_type[lwork];
//				info = ::boost::numeric::bindings::lapack::detail::ormql(
//					::boost::numeric::bindings::tag::right(),
//					::boost::numeric::bindings::tag::transpose(),
//					m,
//					n,
//					k,
//					QL.data().begin(),
//					lda,
//					tau.data().begin(),
//					C.data().begin(),
//					ldc,
//					work,
//					lwork
//				);
//				delete[] work;
				::boost::numeric::bindings::lapack::ormql(
					::boost::numeric::bindings::tag::right(),
					::boost::numeric::bindings::trans(QL),
					tau,
					C
				);
			}
			else
			{
//				//FIXME: actually (2010-08-13) bindinds::lapack::ormql has problems
//				info = ::boost::numeric::bindings::lapack::detail::ormql(
//					::boost::numeric::bindings::tag::right(),
//					::boost::numeric::bindings::tag::no_transpose(),
//					m,
//					n,
//					k,
//					QL.data().begin(),
//					lda,
//					tau.data().begin(),
//					C.data().begin(),
//					ldc,
//					&opt_work_size,
//					-1
//				);
//				lwork = static_cast< ::fortran_int_t >(opt_work_size);
//				work = new real_type[lwork];
//				info = ::boost::numeric::bindings::lapack::detail::ormql(
//					::boost::numeric::bindings::tag::right(),
//					::boost::numeric::bindings::tag::no_transpose(),
//					m,
//					n,
//					k,
//					QL.data().begin(),
//					lda,
//					tau.data().begin(),
//					C.data().begin(),
//					ldc,
//					work,
//					lwork
//				);
//				delete[] work;
				::boost::numeric::bindings::lapack::ormql(
					::boost::numeric::bindings::tag::right(),
					QL,
					tau,
					C
				);
			}
		}
	}
};

/**
 * \brief QL decomposition operations for non-complex types.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <>
struct ql_decomposition_impl<false>: public ql_decomposition_impl_common
{
	/// Extract the Q matrix from a previously computing QL decomposition
	/// (row-major case).
	template <typename QLMatrixT, typename TauVectorT, typename QMatrixT>
		static void extract_Q(QLMatrixT const& QL, TauVectorT const& tau, QMatrixT& Q, bool full, row_major_tag)
	{
		matrix<typename matrix_traits<QLMatrixT>::value_type, column_major> tmp_QL(QL);
		matrix<typename matrix_traits<QMatrixT>::value_type, column_major> tmp_Q(Q);

		extract_Q(tmp_QL, tau, tmp_Q, full, column_major_tag());

		Q = tmp_Q;
	}

	/// Extract the Q matrix from a previously computing QL decomposition
	/// (column-major case).
	template <typename QLMatrixT, typename TauVectorT, typename QMatrixT>
		static void extract_Q(QLMatrixT const& QL, TauVectorT& tau, QMatrixT& Q, bool full, column_major_tag)
	{
		typedef typename matrix_traits<QMatrixT>::size_type size_type;
		typedef typename matrix_traits<QMatrixT>::value_type value_type;

		size_type m = num_rows(QL);
		size_type n = num_columns(QL);
		size_type nc = full ? m : ::std::min(m,n);

		if (num_rows(Q) != m || num_columns(Q) != nc)
		{
			Q.resize(m, nc, false);
		}

		if (m > n)
		{
			if (full)
			{
				subrange(Q, 0, m, 0, m-n) = scalar_matrix<value_type>(m, m-n, value_type/*zero*/());
				subrange(Q, 0, m, m-n, m) = QL;
			}
			else
			{
				Q = QL;
			}
		}
		else if (m < n)
		{
			subrange(Q, 0, m-1, 0, 1) = scalar_matrix<value_type>(m-1, 1, value_type/*zero*/());
			subrange(Q, m-1, m, 0, m) = scalar_matrix<value_type>(1, m, value_type/*zero*/());
			subrange(Q, 0, m-1, 1, m) = subrange(QL, 0, m-1, n-m+1, n);
		}
		else
		{
			Q = QL;
		}

		::boost::numeric::bindings::lapack::orgql(Q, tau);
/*
		// Compute Q without LAPACK
		//
		// The matrix Q is represented as a product of elementary reflectors
		//   Q = H(k) . . . H(2) H(1), where k = min(m,n).
		// Each H(i) has the form
		//   H(i) = I - tau * v * v'
		// where tau is a real scalar, and v is a real vector with
		// v(m-k+i+1:m) = 0 and v(m-k+i) = 1; v(1:m-k+i-1) is stored on exit in
		// A(1:m-k+i-1,n-k+i), and tau in TAU(i).

		size_type k = std::min(m, n);

		if (num_rows(Q) != m || num_columns(Q) != m)
		{
			Q.resize(m, m, false);
		}

		identity_matrix<value_type> I(m);

		Q = I;
		for (size_type i = k-1; (i+1) > 0; --i)
		{
			// Build v = [ A(1:m-k+i-1,n-k+i) 1 0 ... 0 ]

			vector<value_type> v(m, value_type());
			for (size_type j = 0; j < m-k+i; ++j)
			{
				v(j) = QL(j,i);
			}
			v(m-k+i) = value_type(1);

			matrix<value_type, column_major> H = I - tau(i)*outer_prod(v, v);
			Q = prod(Q, H);
		}
*/
	}
};

/**
 * \brief QL decomposition operations for complex types.
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <>
struct ql_decomposition_impl<true>: public ql_decomposition_impl_common
{
	/// Extract the Q matrix from a previously computing QL decomposition
	/// (row-major case).
	template <typename QLMatrixT, typename TauVectorT, typename QMatrixT>
		static void extract_Q(QLMatrixT const& QL, TauVectorT const& tau, QMatrixT& Q, bool full, row_major_tag)
	{
		matrix<typename matrix_traits<QLMatrixT>::value_type, column_major> tmp_QL(QL);
		matrix<typename matrix_traits<QMatrixT>::value_type, column_major> tmp_Q(Q);

		extract_Q(tmp_QL, tau, tmp_Q, full, column_major_tag());

		Q = tmp_Q;
	}

	/// Extract the Q matrix from a previously computing QL decomposition
	/// (column-major case).
	template <typename QLMatrixT, typename TauVectorT, typename QMatrixT>
		static void extract_Q(QLMatrixT const& QL, TauVectorT& tau, QMatrixT& Q, bool full, column_major_tag)
	{
		typedef typename matrix_traits<QMatrixT>::size_type size_type;
		typedef typename matrix_traits<QMatrixT>::value_type value_type;

		size_type m = num_rows(QL);
		size_type n = num_columns(QL);
		size_type nc = full ? m : std::min(m,n);

		if (num_rows(Q) != m || num_columns(Q) != nc)
		{
			Q.resize(m, nc, false);
		}

		if (m > n)
		{
			if (full)
			{
				subrange(Q, 0, m, 0, m-n) = scalar_matrix<value_type>(m, m-n, 0);
				subrange(Q, 0, m, m-n, m) = QL;
			}
			else
			{
				Q = QL;
			}
		}
		else if (m < n)
		{
			subrange(Q, 0, m-1, 0, 1) = scalar_matrix<value_type>(m-1, 1, 0);
			subrange(Q, m-1, m, 0, m) = scalar_matrix<value_type>(1, m, 0);
			subrange(Q, 0, m-1, 1, m) = subrange(QL, 0, m-1, n-m+1, n);
		}
		else
		{
			Q = QL;
		}

		::boost::numeric::bindings::lapack::ungql(Q, tau);
/*
		// Compute Q without LAPACK
		//
		// The matrix Q is represented as a product of elementary reflectors
		//   Q = H(k) . . . H(2) H(1), where k = min(m,n).
		// Each H(i) has the form
		//   H(i) = I - tau * v * v'
		// where tau is a real scalar, and v is a real vector with
		// v(m-k+i+1:m) = 0 and v(m-k+i) = 1; v(1:m-k+i-1) is stored on exit in
		// A(1:m-k+i-1,n-k+i), and tau in TAU(i).

		size_type k = std::min(m, n);

		if (num_rows(Q) != m || num_columns(Q) != m)
		{
			Q.resize(m, m, false);
		}

		identity_matrix<value_type> I(m);

		Q = I;
		for (size_type i = k-1; (i+1) > 0; --i)
		{
			// Build v = [ A(1:m-k+i-1,n-k+i) 1 0 ... 0 ]

			vector<value_type> v(m, value_type());
			for (size_type j = 0; j < m-k+i; ++j)
			{
				v(j) = QL(j,i);
			}
			v(m-k+i) = value_type(1);

			matrix<value_type, column_major> H = I - tau(i)*outer_prod(v, v);
			Q = prod(Q, H);
		}
*/
	}
};

/// Free function performing the QL decomposition of the given matrix expression \a A.
template<typename MatrixExprT, typename QMatrixT, typename LMatrixT, typename OrientationT>
void ql_decompose_impl(matrix_expression<MatrixExprT> const& A, QMatrixT& Q, LMatrixT& L, bool full, OrientationT orientation)
{
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;

	matrix<value_type, typename layout_type<MatrixExprT>::type> tmp_QL(A);
	vector<value_type> tmp_tau;

	ql_decomposition_impl<
			::boost::is_complex<value_type>::value
		>::template decompose(tmp_QL, tmp_tau, orientation);

	ql_decomposition_impl<
			::boost::is_complex<value_type>::value
		>::template extract_Q(tmp_QL, tmp_tau, Q, full, orientation);

	ql_decomposition_impl<
			::boost::is_complex<value_type>::value
		>::template extract_L(tmp_QL, L, full, orientation);
}

}} // Namespace detail::<unnamed>

/**
 * \brief QL decomposition.
 *
 * \tparam ValueT The type of the elements stored in the input matrices.
 *
 * \todo Currently, the type of the L matrix is a dense matrix.
 *  Can we use a better matrix structure?
 *
 * \author Marco Guazzone, marco.guazzone@gmail.com
 */
template <typename ValueT>
class ql_decomposition
{
	public: typedef ValueT value_type;
	private: typedef matrix<value_type, column_major> work_matrix_type;
	private: typedef vector<value_type> tau_vector_type;
	public: typedef work_matrix_type QL_matrix_type;
	public: typedef work_matrix_type Q_matrix_type;
	public: typedef work_matrix_type L_matrix_type;//TODO: Can I use a special matrix

	/// Default constructor.
	public: ql_decomposition()
	{
		// empty
	}

	/// Decompose the given matrix expression \a A.
	public: template <typename MatrixExprT>
		ql_decomposition(matrix_expression<MatrixExprT> const& A)
		: QL_(A)
	{
		decompose();
	}

	/// Decompose the given matrix expression \a A.
	public: template <typename MatrixExprT>
		void decompose(matrix_expression<MatrixExprT> const& A)
	{
		QL_ = A;

		decompose();
	}

	/**
	 * \brief Extract the \c Q matrix.
	 * \param full If \c false enables the economy-size mode whereby a
	 *  reduced (rectangular) Q matrix is returned instead of full (square) one.
	 * \return The \c Q matrix.
	 *
	 * The <em>economy-size</em> mode is useful when \f$m > n\f$ (where \f$m\f$
	 * and \f$n\f$ are the number of rows and columns of the decomposed matrix
	 * \f$A\f$).
	 * As a matter of fact, in this case, the QL factorization can be viewed as:
	 * \f[
	 *   A = QL = \begin{pmatrix} Q_1 & Q_2 \end{pmatrix} \begin{pmatrix} 0 \\ L \end{pmatrix} = Q_2 L
	 * \f]
	 * where \f$Q_2\f$ is an m-by-n matrix containing the n trailing columns of
	 * \f$Q\f$.
	 */
	public: Q_matrix_type Q(bool full = true) const
	{
		Q_matrix_type tmp_Q;

		detail::ql_decomposition_impl<
				::boost::is_complex<value_type>::value
			>::template extract_Q(QL_, tau_, tmp_Q, full, column_major_tag());

		return tmp_Q;
	}

	/**
	 * \brief Extract the \c L matrix.
	 * \param full If \c false enables the economy-size mode whereby a
	 *  reduced \f$\min(m,n)\f$-by\f$n\f$ \c L matrix is returned instead of the
	 *  full \f$m\f$-by-\f$n\f$ one.
	 * \return The \c L matrix.
	 *
	 * The <em>economy-size</em> mode is useful when \f$m > n\f$ (where \f$m\f$
	 * and \f$n\f$ are the number of rows and columns of the decomposed matrix
	 * \f$A\f$).
	 * As a matter of fact, in this case, the QL factorization can be viewed as:
	 * \f[
	 *   A = QL = \begin{pmatrix} Q_1 & Q_2 \end{pmatrix} \begin{pmatrix} 0 \\ L \end{pmatrix} = Q_2 L
	 * \f]
	 * where \f$Q_2\f$ is an m-by-n matrix containing the n trailing columns of
	 * \f$Q\f$.
	 */
	public: L_matrix_type L(bool full = true) const
	{
		L_matrix_type tmp_L;

		detail::ql_decomposition_impl<
				::boost::is_complex<value_type>::value
			>::template extract_L(QL_, tmp_L, full, column_major_tag());

		return tmp_L;
	}

	/// Perform the product \f$Q C\f$ and store the result in \a C.
	public: template <typename CMatrixT>
		void lprod_inplace(CMatrixT& C) const
	{
		typedef typename matrix_traits<CMatrixT>::orientation_category orientation_category;

		lprod_inplace(C, orientation_category());
	}

	/// Perform the product \f$C Q\f$ and store the result in \a C.
	public: template <typename CMatrixT>
		void rprod_inplace(CMatrixT& C) const
	{
		typedef typename matrix_traits<CMatrixT>::orientation_category orientation_category;

		rprod_inplace(C, orientation_category());
	}

	/// Perform the product \f$Q^T C\f$ and store the result in \a C.
	public: template <typename CMatrixT>
		void tlprod_inplace(CMatrixT& C) const
	{
		typedef typename matrix_traits<CMatrixT>::orientation_category orientation_category;

		tlprod_inplace(C, orientation_category());
	}

	/// Perform the product \f$C Q^T\f$ and store the result in \a C.
	public: template <typename CMatrixT>
		void trprod_inplace(CMatrixT& C) const
	{
		typedef typename matrix_traits<CMatrixT>::orientation_category orientation_category;

		trprod_inplace(C, orientation_category());
	}

	/// Perform the product \f$Q C\f$ and return the result.
	public: template <typename CMatrixExprT>
		typename matrix_temporary_traits<CMatrixExprT>::type lprod(matrix_expression<CMatrixExprT> const& C) const
	{
		typename matrix_temporary_traits<CMatrixExprT>::type tmp_C(C);

		lprod_inplace(tmp_C);

		return tmp_C;
	}

	/// Perform the product \f$C Q\f$ and return the result.
	public: template <typename CMatrixExprT>
		typename matrix_temporary_traits<CMatrixExprT>::type rprod(matrix_expression<CMatrixExprT> const& C) const
	{
		typename matrix_temporary_traits<CMatrixExprT>::type tmp_C(C);

		rprod_inplace(tmp_C);

		return tmp_C;
	}

	/// Perform the product \f$Q^T C\f$ and return the result.
	public: template <typename CMatrixExprT>
		typename matrix_temporary_traits<CMatrixExprT>::type tlprod(matrix_expression<CMatrixExprT> const& C) const
	{
		typename matrix_temporary_traits<CMatrixExprT>::type tmp_C(C);

		tlprod_inplace(tmp_C);

		return tmp_C;
	}

	/// Perform the product \f$C Q^T\f$ and return the result.
	public: template <typename CMatrixExprT>
		typename matrix_temporary_traits<CMatrixExprT>::type trprod(matrix_expression<CMatrixExprT> const& C) const
	{
		typename matrix_temporary_traits<CMatrixExprT>::type tmp_C(C);

		trprod_inplace(tmp_C);

		return tmp_C;
	}

	private: void decompose()
	{
		detail::ql_decomposition_impl<
				::boost::is_complex<value_type>::value
			>::template decompose(QL_, tau_, column_major_tag());
	}

	/// Perform the product \f$Q C\f$ and store the result in \a C (column-major
	/// case).
	private: template <typename CMatrixT>
		void lprod_inplace(CMatrixT& C, column_major_tag) const
	{
		detail::ql_decomposition_impl<
				::boost::is_complex<value_type>::value
			>::template prod(QL_, tau_, C, true, false, column_major_tag());
	}

	/// Perform the product \f$Q C\f$ and store the result in \a C (row-major
	/// case).
	private: template <typename CMatrixT>
		void lprod_inplace(CMatrixT& C, row_major_tag) const
	{
		work_matrix_type tmp_C(C);

		detail::ql_decomposition_impl<
				::boost::is_complex<value_type>::value
			>::template prod(QL_, tau_, tmp_C, true, false, column_major_tag());

		C = tmp_C;
	}

	/// Perform the product \f$C Q\f$ and store the result in \a C (column-major
	/// case).
	private: template <typename CMatrixT>
		void rprod_inplace(CMatrixT& C, column_major_tag) const
	{
		detail::ql_decomposition_impl<
				::boost::is_complex<value_type>::value
			>::template prod(QL_, tau_, C, false, false, column_major_tag());
	}

	/// Perform the product \f$C Q\f$ and store the result in \a C (row-major
	/// case).
	private: template <typename CMatrixT>
		void rprod_inplace(CMatrixT& C, row_major_tag) const
	{
		work_matrix_type tmp_C(C);

		detail::ql_decomposition_impl<
				::boost::is_complex<value_type>::value
			>::template prod(QL_, tau_, tmp_C, false, false, column_major_tag());

		C = tmp_C;
	}

	/// Perform the product \f$Q^T C\f$ and store the result in \a C
	/// (column-major case).
	private: template <typename CMatrixT>
		void tlprod_inplace(CMatrixT& C, column_major_tag) const
	{
		detail::ql_decomposition_impl<
				::boost::is_complex<value_type>::value
			>::template prod(QL_, tau_, C, true, true, column_major_tag());
	}

	/// Perform the product \f$Q^T C\f$ and store the result in \a C
	/// (row-major case).
	private: template <typename CMatrixT>
		void tlprod_inplace(CMatrixT& C, row_major_tag) const
	{
		work_matrix_type tmp_C(C);

		detail::ql_decomposition_impl<
				::boost::is_complex<value_type>::value
			>::template prod(QL_, tau_, tmp_C, true, true, column_major_tag());

		C = tmp_C;
	}

	/// Perform the product \f$C Q^T\f$ and store the result in \a C
	/// (column-major case).
	private: template <typename CMatrixT>
		void trprod_inplace(CMatrixT& C, column_major_tag) const
	{
		detail::ql_decomposition_impl<
				::boost::is_complex<value_type>::value
			>::template prod(QL_, tau_, C, false, true, column_major_tag());
	}

	/// Perform the product \f$C Q^T\f$ and store the result in \a C
	/// (row-major case).
	private: template <typename CMatrixT>
		void trprod_inplace(CMatrixT& C, row_major_tag) const
	{
		work_matrix_type tmp_C(C);

		detail::ql_decomposition_impl<
				::boost::is_complex<value_type>::value
			>::template prod(QL_, tau_, tmp_C, false, true, column_major_tag());

		C = tmp_C;
	}

	// NOTE: the 'mutable' keyword is needed in order to make 'const' the
	//       '?prod' methods ('lprod', 'tlprod', 'rprod', 'trprod').
	//       Indeed, these methods call the respective '?prod_inplace' methods
	//       which, in turns, call the LAPACK::ORMQL function which temporarily
	//       changes the QL matrix (and restores it before returning).
	private: mutable QL_matrix_type QL_;
	private: tau_vector_type tau_;
};

/// Free function performing the QL decomposition of the given matrix expression \a A.
template<typename MatrixExprT, typename OutMatrix1T, typename OutMatrix2T>
BOOST_UBLAS_INLINE
void ql_decompose(matrix_expression<MatrixExprT> const& A, OutMatrix1T& Q, OutMatrix2T& L, bool full = true)
{
	typedef typename matrix_traits<MatrixExprT>::orientation_category orientation_category1;
	typedef typename matrix_traits<OutMatrix1T>::orientation_category orientation_category2;
	typedef typename matrix_traits<OutMatrix2T>::orientation_category orientation_category3;

	// precondition: same orientation category
	BOOST_MPL_ASSERT(
		(::boost::mpl::and_<
			::boost::is_same<orientation_category1,orientation_category2>,
			::boost::is_same<orientation_category1,orientation_category3>
		>)
	);

	detail::ql_decompose_impl(A, Q, L, full, orientation_category1());
}

/// Free function performing the QL decomposition of the given matrix expression \a A.
template<typename MatrixExprT>
BOOST_UBLAS_INLINE
ql_decomposition<typename matrix_traits<MatrixExprT>::value_type> ql_decompose(matrix_expression<MatrixExprT> const& A)
{
	typedef typename matrix_traits<MatrixExprT>::value_type value_type;

	return ql_decomposition<value_type>(A);
}

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

#endif // BOOST_NUMERIC_UBLASX_OPERATION_QL_HPP


#pragma once

#include <vector>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <cmath>
#include "SAMSON.hpp"
#include "ADNConstants.hpp"

namespace ublas = boost::numeric::ublas;

namespace ADNVectorMath {
  using namespace ublas;

  ublas::vector<double> CreateBoostVector(std::vector<double> vec);
  std::vector<double> CreateStdVector(ublas::vector<double> vec);
  ublas::matrix<double> CreateBoostMatrix(std::vector<std::vector<double>> vecovec);
  ublas::vector<double> CalculateCM(ublas::matrix<double> positions);
  ublas::vector<double> CalculateCM(ublas::matrix<double> weightedPositions, double totalMass);
  ublas::vector<double> CrossProduct(ublas::vector<double> v, ublas::vector<double> w);
  ublas::vector<double> DirectionVector(ublas::vector<double> p, ublas::vector<double> q);
  double DegToRad(double degree);
  ublas::matrix<double> MakeRotationMatrix(ublas::vector<double> dir, double angle);
  ublas::matrix<double> SkewMatrix(ublas::vector<double> v);
  ublas::vector<double> InitializeVector(size_t size);  // deprecated, use constructor
  ublas::matrix<double> InitializeMatrix(size_t sz_r, size_t sz_c);  // deprecated, use constructor
  ublas::matrix<double> InitializeMatrix(size_t sz);  // deprecated, use constructor
  ublas::matrix<double> Translate(ublas::matrix<double> input, ublas::vector<double> t_vector);
  ublas::matrix<double> Rotate(ublas::matrix<double> input, ublas::matrix<double> rot_matrix);  // deprecated, use ApplyTransformation
  ublas::matrix<double> CenterSystem(ublas::matrix<double> input);
  void AddRowToMatrix(ublas::matrix<double> &input, ublas::vector<double> r);
  ublas::vector<double> CalculatePlane(ublas::matrix<double> mat);
  ublas::matrix<double> FindOrthogonalSubspace(ublas::vector<double> z);
  ublas::matrix<double> InvertMatrix(const ublas::matrix<double>& input);
  double Determinant(ublas::matrix<double> mat);
  bool IsNearlyZero(double n, double tol = 0.000000001);
  double CalculateVectorNorm(ublas::vector<double> v);
  /*!
    * Applies the transformation given by t_mat to a set of points
    * \param the transformation matrix
    * \param a matrix holding coordinates of points
    * \return a matrix with the coordinates after the transformation
  */
  ublas::matrix<double> ApplyTransformation(ublas::matrix<double> t_mat, ublas::matrix<double> points);

  ublas::vector<double> Spherical2Cartesian(ublas::vector<double> spher);

  // SAMSON types operations
  SBVector3 SBCrossProduct(SBVector3 v, SBVector3 w);
  double SBInnerProduct(SBVector3 v, SBVector3 w);

  //! Calculation of parameters of dna nanotubes
  SBQuantity::length CalculateNanotubeRadius(int numDs);
  int CalculateNanotubeDoubleStrands(SBQuantity::length radius);

  //! Bezier curves
  //! Calculates the length of a quadratic Bezier curve
  SBQuantity::length LengthQuadraticBezier(SBPosition3 P0, SBPosition3 P1, SBPosition3 P2);
  SBPosition3 QuadraticBezierPoint(SBPosition3 P0, SBPosition3 P1, SBPosition3 P2, double t);
  SBVector3 DerivativeQuadraticBezier(SBPosition3 P0, SBPosition3 P1, SBPosition3 P2, double t);
};


#include "ssd_ros_tracking/ssd_ekf.h"
#include <Eigen/Dense>
#include <pcl/point_types.h>

// association param
#define PRE_SIZE 5

// dynamic detector param
#define MIN_VELOCITY 0.3
#define MAX_VELOCITY 2

using namespace std;
using namespace Eigen;

MatrixXf EKF::move(MatrixXf x, MatrixXf u, float dt)
{
	/* 動作予測
	 * x : 状態
	 * u : 制御
	 * dt: 前ステップとの時間差
	 */
	MatrixXf A(3,1);
	MatrixXf I = MatrixXf::Identity(3,3);
	MatrixXf B(3,2);

	float theta = x.coeffRef(2,0) + u.coeffRef(1,0)*dt/2;

	B << dt*cos(theta),  0,
		 dt*sin(theta),  0,
		             0, dt;

	A = I*x + B*u;

	return A;
}

MatrixXf EKF::jacobF(MatrixXf x, MatrixXf u, float dt)
{
	/* ヤコビ行列
	 * x : 状態
	 * u : 制御
	 * dt: 前ステップとの時間差
	 */
	MatrixXf F(3,3);
	float b     = dt*u.coeffRef(0,0);
	float theta = x.coeffRef(2,0);

	F << 1, 0, -b*sin(theta),
		 0, 1,  b*cos(theta),
		 0, 0,             1;

	return F;
}

MatrixXf EKF::jacobG(MatrixXf x, MatrixXf u, float dt)
{
	/* ヤコビ行列
	 * x : 状態
	 * u : 制御
	 * dt: 前ステップとの時間差
	 */
	MatrixXf G(3,3);
	float b     = u.coeffRef(0,0)*dt;
	float theta = u.coeffRef(1,0)*dt/2 + x.coeffRef(2,0);

	G << 1, 0, -b*sin(theta),
		 0, 1,  b*cos(theta),
		 0, 0,             1;
	return G;
}

MatrixXf EKF::jacobV(MatrixXf x, MatrixXf u, float dt)
{
	/* ヤコビ行列
	 * x : 状態
	 * u : 制御
	 * dt: 前ステップとの時間差
	 */
	MatrixXf V(3,2);
	float theta = u.coeffRef(1,0)*dt/2 + x.coeffRef(2,0);
	float v     = u.coeffRef(0,0);

	V << dt*cos(theta), (-v*dt*dt)*sin(theta)/2,
		 dt*sin(theta),  (v*dt*dt)*cos(theta)/2,
		             0,                      dt;

	return V;
}

MatrixXf EKF::jacobM(MatrixXf u, double s_input[])
{
	/* ヤコビ行列
	 * u : 制御
	 * s_input: 制御系の計測誤差パラメータ
	 */
	MatrixXf M(2,2);
	float v  = u.coeffRef(0,0);
	float w  = u.coeffRef(1,0);
	float a1 = (float)s_input[0];
	float a2 = (float)s_input[1];
	float a3 = (float)s_input[2];
	float a4 = (float)s_input[3];

	M << a1*v*v + a2*w*w,               0,
		               0, a3*v*v + a4*w*w;

	return M;
}

MatrixXf EKF::jacobH(MatrixXf x)
{
	/* ヤコビ行列
	 * x : 状態
	 */
	MatrixXf H(3,3);

	H<< 1, 0, 0,
	    0, 1, 0,
	    0, 0, 1;

	 return H;
}

void initCluster( clusterInfo& cluster,
				  PointI& init_pt)
{
	// float init_r      = 0.01; // 観測誤差
	float init_p      = 0.001;  //初期位置の分散値
	float init_theta = 0;
	// cluster.x << init_pt.x, init_pt.y, 0.0, 0.0;
	cluster.x << init_pt.x, init_pt.y, init_theta;
	cluster.P << init_p,    0.0,   0.0,
		 		    0.0, init_p,   0.0,
		 		    0.0,    0.0, init_p;
	// cluster.R << init_r,    0.0,
	// 			    0.0, init_r;
	cluster.pre_vel.x       = 0;
	cluster.pre_vel.y       = 0;
	cluster.u               = Eigen::VectorXf::Zero(2);
	cluster.track_num       = 1;
	cluster.confidence      = 1;
	cluster.width           = init_pt.normal_x;
	cluster.length          = init_pt.normal_y;
	cluster.height          = init_pt.normal_z;
	cluster.count           = 0;
	cluster.pre_position.resize(PRE_SIZE);
	cluster.pre_position[0] = init_pt;
	cluster.label = 0; // static: 0  dynamic: 1
	cluster.update_comp_flag = true;
	cluster.init_flag = true;

	cluster.velocity = 0;
}

void Prediction( clusterInfo& cluster,
				 double dt,
				 double s_input[])
{
	EKF ekf;
	/* u   : (v, w)の転置行列　v:並進速度, w:角速度
	 * x   : (x, y, θ)の転置行列
	 * dt	   : 前ステップからの経過時間
	 * s_input : 動作モデルのノイズパラメータ
	 */

	MatrixXf Gt = MatrixXf::Zero(3,3);
	MatrixXf Vt = MatrixXf::Zero(3,2);
	MatrixXf Mt = MatrixXf::Zero(2,2);

	// Gt = ekf.jacobG(cluster.x, cluster.u, dt, pitch);
	Gt = ekf.jacobF(cluster.x, cluster.u, dt);
	Vt = ekf.jacobV(cluster.x, cluster.u, dt);
	Mt = ekf.jacobM(cluster.u, s_input);

	cluster.x = ekf.move(cluster.x, cluster.u, dt);
	cluster.P = Gt*cluster.P*Gt.transpose() + Vt*Mt*Vt.transpose();

	// cluster.label = 1;
}

void MeasurementUpdate(	clusterInfo& cluster,
						PointI& obj_centroid,
						double dt,
						double s_measurement[])
{
	/* x	: 状態(x, y, yaw)の転置行列
	 * u	: 制御(v, w)の転置行列
	 * s_measurement: 観測ノイズ
	 * sigma: 推定誤差
	 */

	EKF ekf;

	MatrixXf Z = MatrixXf::Zero(3,1);	// 観測 (x,y,θ)
	MatrixXf Q = MatrixXf::Zero(3,3);
	MatrixXf H = MatrixXf::Zero(3,3);
	MatrixXf y = MatrixXf::Zero(3,1);
	MatrixXf S = MatrixXf::Zero(3,3);
	MatrixXf K = MatrixXf::Zero(3,3);
	MatrixXf I = MatrixXf::Identity(3,3);

	Z.coeffRef(0,0) = obj_centroid.x;
	Z.coeffRef(1,0) = obj_centroid.y;
	Z.coeffRef(2,0) = atan2((obj_centroid.y-cluster.x[1]), (obj_centroid.x-cluster.x[0]));

	// cluster.u[0] = sqrt(pow((obj_centroid.y-cluster.x[1]), 2) + pow((obj_centroid.x-cluster.x[0]), 2))/dt;
	cluster.u[0] = sqrt(pow((obj_centroid.y-cluster.x[1]), 2) + pow((obj_centroid.x-cluster.x[0]), 2))/dt;

	if(cluster.init_flag){
	// cluster.u[1] = atan2((obj_centroid.y-cluster.x[1]), (obj_centroid.x-cluster.x[0]))/dt;
	cluster.u[1] = 0;
	cluster.init_flag = false;
	}

	// else cluster.u[1] = (atan2((obj_centroid.y-cluster.x[1]), (obj_centroid.x-cluster.x[0])) - cluster.x[2])/dt;
	else cluster.u[1] = (Z.coeffRef(2,0) - cluster.x[2])/dt;

	Q.coeffRef(0,0) = (float)s_measurement[0];
	Q.coeffRef(1,1) = (float)s_measurement[1];
	Q.coeffRef(2,2) = (float)s_measurement[2];

	H = ekf.jacobH(cluster.x);
	y = Z - H*cluster.x;
	S = H*cluster.P*H.transpose() + Q;
	K = cluster.P*H.transpose()*S.inverse();

	cluster.x = cluster.x + K*y;
	cluster.P = (I - K*H)*cluster.P;

	// 今まで座標をPRE_SIZEの数だけ格納
	for (int i=0; i<(PRE_SIZE-1); i++) cluster.pre_position[i+1] = cluster.pre_position[i];
	cluster.pre_position[0].x = cluster.x(0);
	cluster.pre_position[0].y = cluster.x(1);
	cluster.pre_position[0].z = cluster.x(2);

	if(cluster.pre_position[PRE_SIZE-1].x != 0.0){
		cluster.velocity = sqrt(pow((cluster.pre_position[0].x-cluster.pre_position[PRE_SIZE-1].x), 2.0) + pow((cluster.pre_position[0].y-cluster.pre_position[PRE_SIZE-1].y), 2.0));
	}

	if(cluster.velocity > 0.1 && cluster.velocity < 1) cluster.label = 1;

	// cout<<"pre_position = "<<cluster.pre_position.size()<<endl;
		// for (int j=0; j<cluster.pre_position.size(); j++){
		// 	// cout<<"label = "<<clusters[i].label<<endl;
        //
			// if(cluster.confidence > 0 && cluster.label == 1){ // label 0:static 1:dynamic
		// 		cout<<" confidence = "<<cluster.confidence<<" pre_pos["<<j<<"] x = "<<cluster.pre_position[j].x<<" y = "<<cluster.pre_position[j].y<<endl;
		// 	}
		// }
}


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

using namespace boost;

template < typename Graph, typename ParentMap >
struct edge_writer
{
  edge_writer(const Graph & g, const ParentMap & p)
  : m_g(g), m_parent(p)
  {
  }

  template < typename Edge >
    void operator() (std::ostream & out, const Edge & e) const
  {
    out << "[label=\"" << get(edge_weight, m_g, e) << "\"";
    typename graph_traits < Graph >::vertex_descriptor
      u = source(e, m_g), v = target(e, m_g);
    if (m_parent[v] == u)
        out << ", color=\"black\"";
    else
        out << ", color=\"grey\"";
      out << "]";
  }
  const Graph & m_g;
  ParentMap m_parent;
};
template < typename Graph, typename Parent >
edge_writer < Graph, Parent >
make_edge_writer(const Graph & g, const Parent & p)
{
  return edge_writer < Graph, Parent > (g, p);
}

int
main()
{
  enum { u, v, x, y, z, N };
  char name[] = { 'u', 'v', 'x', 'y', 'z' };
  typedef std::pair < int, int >E;
  const int n_edges = 10;
  E edge_array[] = { E(u, y), E(u, x), E(u, v), E(v, u),
      E(x, y), E(x, v), E(y, v), E(y, z), E(z, u), E(z,x) };
  int weight[n_edges] = { -4, 8, 5, -2, 9, -3, 7, 2, 6, 7 };

  typedef adjacency_list < vecS, vecS, directedS,
    no_property, property < edge_weight_t, int > > Graph;
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  // VC++ can't handle the iterator constructor
  Graph g(N);
  for (std::size_t j = 0; j < n_edges; ++j)
    add_edge(edge_array[j].first, edge_array[j].second, g);
#else
  Graph g(edge_array, edge_array + n_edges, N);
#endif
  graph_traits < Graph >::edge_iterator ei, ei_end;
  property_map<Graph, edge_weight_t>::type weight_pmap = get(edge_weight, g);
  int i = 0;
  for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei, ++i)
    weight_pmap[*ei] = weight[i];

  std::vector<int> distance(N, std::numeric_limits < short >::max());
  std::vector<std::size_t> parent(N);
  for (i = 0; i < N; ++i)
    parent[i] = i;
  distance[z] = 0;

#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  bool r = bellman_ford_shortest_paths
    (g, int(N), weight_pmap, &parent[0], &distance[0],
     closed_plus<int>(), std::less<int>(), default_bellman_visitor());
#else
  bool r = bellman_ford_shortest_paths
    (g, int (N), weight_map(weight_pmap).distance_map(&distance[0]).
     predecessor_map(&parent[0]));
#endif

  if (r)
    for (i = 0; i < N; ++i)
      std::cout << name[i] << ": " << std::setw(3) << distance[i]
        << " " << name[parent[i]] << std::endl;
  else
    std::cout << "negative cycle" << std::endl;

  std::ofstream dot_file("figs/bellman-eg.dot");
  dot_file << "digraph D {\n"
    << "  rankdir=LR\n"
    << "  size=\"5,3\"\n"
    << "  ratio=\"fill\"\n"
    << "  edge[style=\"bold\"]\n" << "  node[shape=\"circle\"]\n";

  {
    for (tie(ei, ei_end) = edges(g); ei != ei_end; ++ei) {
      graph_traits < Graph >::edge_descriptor e = *ei;
      graph_traits < Graph >::vertex_descriptor
        u = source(e, g), v = target(e, g);
      // VC++ doesn't like the 3-argument get function, so here
      // we workaround by using 2-nested get()'s.
      dot_file << name[u] << " -> " << name[v]
        << "[label=\"" << get(get(edge_weight, g), e) << "\"";
      if (parent[v] == u)
        dot_file << ", color=\"black\"";
      else
        dot_file << ", color=\"grey\"";
      dot_file << "]";
    }
  }
  dot_file << "}";
  return EXIT_SUCCESS;
}


#include <NTL/ZZ_pEXFactoring.h>
#include <NTL/FacVec.h>
#include <NTL/fileio.h>
#include <NTL/new.h>

NTL_START_IMPL

static
void IterPower(ZZ_pE& c, const ZZ_pE& a, long n)
{
   ZZ_pE res;

   long i;

   res = a;

   for (i = 0; i < n; i++)
      power(res, res, ZZ_p::modulus());

   c = res;
}

void SquareFreeDecomp(vec_pair_ZZ_pEX_long& u, const ZZ_pEX& ff)
{
   ZZ_pEX f = ff;

   if (!IsOne(LeadCoeff(f)))
      LogicError("SquareFreeDecomp: bad args");

   ZZ_pEX r, t, v, tmp1;
   long m, j, finished, done;

   u.SetLength(0);

   if (deg(f) == 0)
      return;

   m = 1;
   finished = 0;

   do {
      j = 1;
      diff(tmp1, f);
      GCD(r, f, tmp1);
      div(t, f, r);

      if (deg(t) > 0) {
         done = 0;
         do {
            GCD(v, r, t);
            div(tmp1, t, v);
            if (deg(tmp1) > 0) append(u, cons(tmp1, j*m));
            if (deg(v) > 0) {
               div(r, r, v);
               t = v;
               j++;
            }
            else
               done = 1;
         } while (!done);
         if (deg(r) == 0) finished = 1;
      }

      if (!finished) {
         /* r is a p-th power */

         long k, d;
         long p = to_long(ZZ_p::modulus());

         d = deg(r)/p;
         f.rep.SetLength(d+1);
         for (k = 0; k <= d; k++)
            IterPower(f.rep[k], r.rep[k*p], ZZ_pE::degree()-1);
         m = m*p;
      }
   } while (!finished);
}

static
void AbsTraceMap(ZZ_pEX& h, const ZZ_pEX& a, const ZZ_pEXModulus& F)
{
   ZZ_pEX res, tmp;

   long k = NumBits(ZZ_pE::cardinality())-1;

   res = a;
   tmp = a;

   long i;
   for (i = 0; i < k-1; i++) {
      SqrMod(tmp, tmp, F);
      add(res, res, tmp);
   }

   h = res;
}

void FrobeniusMap(ZZ_pEX& h, const ZZ_pEXModulus& F)
{
   PowerXMod(h, ZZ_pE::cardinality(), F);
}

static
void RecFindRoots(vec_ZZ_pE& x, const ZZ_pEX& f)
{
   if (deg(f) == 0) return;

   if (deg(f) == 1) {
      long k = x.length();
      x.SetLength(k+1);
      negate(x[k], ConstTerm(f));
      return;
   }

   ZZ_pEX h;

   ZZ_pEX r;

   {
      ZZ_pEXModulus F;
      build(F, f);

      do {
         random(r, deg(F));
         if (IsOdd(ZZ_pE::cardinality())) {
            PowerMod(h, r, RightShift(ZZ_pE::cardinality(), 1), F);
            sub(h, h, 1);
         }
         else {
            AbsTraceMap(h, r, F);
         }
         GCD(h, h, f);
      } while (deg(h) <= 0 || deg(h) == deg(f));
   }

   RecFindRoots(x, h);
   div(h, f, h);
   RecFindRoots(x, h);
}

void FindRoots(vec_ZZ_pE& x, const ZZ_pEX& ff)
{
   ZZ_pEX f = ff;

   if (!IsOne(LeadCoeff(f)))
      LogicError("FindRoots: bad args");

   x.SetMaxLength(deg(f));
   x.SetLength(0);
   RecFindRoots(x, f);
}

void split(ZZ_pEX& f1, ZZ_pEX& g1, ZZ_pEX& f2, ZZ_pEX& g2,
           const ZZ_pEX& f, const ZZ_pEX& g,
           const vec_ZZ_pE& roots, long lo, long mid)
{
   long r = mid-lo+1;

   ZZ_pEXModulus F;
   build(F, f);

   vec_ZZ_pE lroots(INIT_SIZE, r);
   long i;

   for (i = 0; i < r; i++)
      lroots[i] = roots[lo+i];

   ZZ_pEX h, a, d;
   BuildFromRoots(h, lroots);
   CompMod(a, h, g, F);

   GCD(f1, a, f);

   div(f2, f, f1);

   rem(g1, g, f1);
   rem(g2, g, f2);
}

void RecFindFactors(vec_ZZ_pEX& factors, const ZZ_pEX& f, const ZZ_pEX& g,
                    const vec_ZZ_pE& roots, long lo, long hi)
{
   long r = hi-lo+1;

   if (r == 0) return;

   if (r == 1) {
      append(factors, f);
      return;
   }

   ZZ_pEX f1, g1, f2, g2;

   long mid = (lo+hi)/2;

   split(f1, g1, f2, g2, f, g, roots, lo, mid);

   RecFindFactors(factors, f1, g1, roots, lo, mid);
   RecFindFactors(factors, f2, g2, roots, mid+1, hi);
}

void FindFactors(vec_ZZ_pEX& factors, const ZZ_pEX& f, const ZZ_pEX& g,
                 const vec_ZZ_pE& roots)
{
   long r = roots.length();

   factors.SetMaxLength(r);
   factors.SetLength(0);

   RecFindFactors(factors, f, g, roots, 0, r-1);
}

void IterFindFactors(vec_ZZ_pEX& factors, const ZZ_pEX& f,
                     const ZZ_pEX& g, const vec_ZZ_pE& roots)
{
   long r = roots.length();
   long i;
   ZZ_pEX h;

   factors.SetLength(r);

   for (i = 0; i < r; i++) {
      sub(h, g, roots[i]);
      GCD(factors[i], f, h);
   }
}

void TraceMap(ZZ_pEX& w, const ZZ_pEX& a, long d, const ZZ_pEXModulus& F,
              const ZZ_pEX& b)

{
   if (d < 0) LogicError("TraceMap: bad args");

   ZZ_pEX y, z, t;

   z = b;
   y = a;
   clear(w);

   while (d) {
      if (d == 1) {
         if (IsZero(w))
            w = y;
         else {
            CompMod(w, w, z, F);
            add(w, w, y);
         }
      }
      else if ((d & 1) == 0) {
         Comp2Mod(z, t, z, y, z, F);
         add(y, t, y);
      }
      else if (IsZero(w)) {
         w = y;
         Comp2Mod(z, t, z, y, z, F);
         add(y, t, y);
      }
      else {
         Comp3Mod(z, t, w, z, y, w, z, F);
         add(w, w, y);
         add(y, t, y);
      }

      d = d >> 1;
   }
}

void PowerCompose(ZZ_pEX& y, const ZZ_pEX& h, long q, const ZZ_pEXModulus& F)
{
   if (q < 0) LogicError("PowerCompose: bad args");

   ZZ_pEX z(INIT_SIZE, F.n);
   long sw;

   z = h;
   SetX(y);

   while (q) {
      sw = 0;

      if (q > 1) sw = 2;
      if (q & 1) {
         if (IsX(y))
            y = z;
         else
            sw = sw | 1;
      }

      switch (sw) {
      case 0:
         break;

      case 1:
         CompMod(y, y, z, F);
         break;

      case 2:
         CompMod(z, z, z, F);
         break;

      case 3:
         Comp2Mod(y, z, y, z, z, F);
         break;
      }

      q = q >> 1;
   }
}

long ProbIrredTest(const ZZ_pEX& f, long iter)
{
   long n = deg(f);

   if (n <= 0) return 0;
   if (n == 1) return 1;

   ZZ_pEXModulus F;

   build(F, f);

   ZZ_pEX b, r, s;

   FrobeniusMap(b, F);

   long all_zero = 1;

   long i;

   for (i = 0; i < iter; i++) {
      random(r, n);
      TraceMap(s, r, n, F, b);

      all_zero = all_zero && IsZero(s);

      if (deg(s) > 0) return 0;
   }

   if (!all_zero || (n & 1)) return 1;

   PowerCompose(s, b, n/2, F);
   return !IsX(s);
}

NTL_THREAD_LOCAL long ZZ_pEX_BlockingFactor = 10;

void RootEDF(vec_ZZ_pEX& factors, const ZZ_pEX& f, long verbose)
{
   vec_ZZ_pE roots;
   double t;

   if (verbose) { cerr << "finding roots..."; t = GetTime(); }
   FindRoots(roots, f);
   if (verbose) { cerr << (GetTime()-t) << "\n"; }

   long r = roots.length();
   factors.SetLength(r);
   for (long j = 0; j < r; j++) {
      SetX(factors[j]);
      sub(factors[j], factors[j], roots[j]);
   }
}

void EDFSplit(vec_ZZ_pEX& v, const ZZ_pEX& f, const ZZ_pEX& b, long d)
{
   ZZ_pEX a, g, h;
   ZZ_pEXModulus F;
   vec_ZZ_pE roots;

   build(F, f);
   long n = F.n;
   long r = n/d;
   random(a, n);
   TraceMap(g, a, d, F, b);
   MinPolyMod(h, g, F, r);
   FindRoots(roots, h);
   FindFactors(v, f, g, roots);
}

void RecEDF(vec_ZZ_pEX& factors, const ZZ_pEX& f, const ZZ_pEX& b, long d,
            long verbose)
{
   vec_ZZ_pEX v;
   long i;
   ZZ_pEX bb;

   if (verbose) cerr << "+";

   EDFSplit(v, f, b, d);
   for (i = 0; i < v.length(); i++) {
      if (deg(v[i]) == d) {
         append(factors, v[i]);
      }
      else {
         ZZ_pEX bb;
         rem(bb, b, v[i]);
         RecEDF(factors, v[i], bb, d, verbose);
      }
   }
}

void EDF(vec_ZZ_pEX& factors, const ZZ_pEX& ff, const ZZ_pEX& bb,
         long d, long verbose)

{
   ZZ_pEX f = ff;
   ZZ_pEX b = bb;

   if (!IsOne(LeadCoeff(f)))
      LogicError("EDF: bad args");

   long n = deg(f);
   long r = n/d;

   if (r == 0) {
      factors.SetLength(0);
      return;
   }

   if (r == 1) {
      factors.SetLength(1);
      factors[0] = f;
      return;
   }

   if (d == 1) {
      RootEDF(factors, f, verbose);
      return;
   }

   double t;
   if (verbose) {
      cerr << "computing EDF(" << d << "," << r << ")...";
      t = GetTime();
   }

   factors.SetLength(0);

   RecEDF(factors, f, b, d, verbose);

   if (verbose) cerr << (GetTime()-t) << "\n";
}

void SFCanZass(vec_ZZ_pEX& factors, const ZZ_pEX& ff, long verbose)
{
   ZZ_pEX f = ff;

   if (!IsOne(LeadCoeff(f)))
      LogicError("SFCanZass: bad args");

   if (deg(f) == 0) {
      factors.SetLength(0);
      return;
   }

   if (deg(f) == 1) {
      factors.SetLength(1);
      factors[0] = f;
      return;
   }

   factors.SetLength(0);

   double t;

   ZZ_pEXModulus F;
   build(F, f);

   ZZ_pEX h;

   if (verbose) { cerr << "computing X^p..."; t = GetTime(); }
   FrobeniusMap(h, F);
   if (verbose) { cerr << (GetTime()-t) << "\n"; }

   vec_pair_ZZ_pEX_long u;
   if (verbose) { cerr << "computing DDF..."; t = GetTime(); }
   NewDDF(u, f, h, verbose);
   if (verbose) {
      t = GetTime()-t;
      cerr << "DDF time: " << t << "\n";
   }

   ZZ_pEX hh;
   vec_ZZ_pEX v;

   long i;
   for (i = 0; i < u.length(); i++) {
      const ZZ_pEX& g = u[i].a;
      long d = u[i].b;
      long r = deg(g)/d;

      if (r == 1) {
         // g is already irreducible

         append(factors, g);
      }
      else {
         // must perform EDF

         if (d == 1) {
            // root finding
            RootEDF(v, g, verbose);
            append(factors, v);
         }
         else {
            // general case
            rem(hh, h, g);
            EDF(v, g, hh, d, verbose);
            append(factors, v);
         }
      }
   }
}

void CanZass(vec_pair_ZZ_pEX_long& factors, const ZZ_pEX& f, long verbose)
{
   if (!IsOne(LeadCoeff(f)))
      LogicError("CanZass: bad args");

   double t;
   vec_pair_ZZ_pEX_long sfd;
   vec_ZZ_pEX x;

   if (verbose) { cerr << "square-free decomposition..."; t = GetTime(); }
   SquareFreeDecomp(sfd, f);
   if (verbose) cerr << (GetTime()-t) << "\n";

   factors.SetLength(0);

   long i, j;

   for (i = 0; i < sfd.length(); i++) {
      if (verbose) {
         cerr << "factoring multiplicity " << sfd[i].b
              << ", deg = " << deg(sfd[i].a) << "\n";
      }

      SFCanZass(x, sfd[i].a, verbose);

      for (j = 0; j < x.length(); j++)
         append(factors, cons(x[j], sfd[i].b));
   }
}

void mul(ZZ_pEX& f, const vec_pair_ZZ_pEX_long& v)
{
   long i, j, n;

   n = 0;
   for (i = 0; i < v.length(); i++)
      n += v[i].b*deg(v[i].a);

   ZZ_pEX g(INIT_SIZE, n+1);

   set(g);
   for (i = 0; i < v.length(); i++)
      for (j = 0; j < v[i].b; j++) {
         mul(g, g, v[i].a);
      }

   f = g;
}

long BaseCase(const ZZ_pEX& h, long q, long a, const ZZ_pEXModulus& F)
{
   long b, e;
   ZZ_pEX lh(INIT_SIZE, F.n);

   lh = h;
   b = 1;
   e = 0;
   while (e < a-1 && !IsX(lh)) {
      e++;
      b *= q;
      PowerCompose(lh, lh, q, F);
   }

   if (!IsX(lh)) b *= q;

   return b;
}

void TandemPowerCompose(ZZ_pEX& y1, ZZ_pEX& y2, const ZZ_pEX& h,
                        long q1, long q2, const ZZ_pEXModulus& F)
{
   ZZ_pEX z(INIT_SIZE, F.n);
   long sw;

   z = h;
   SetX(y1);
   SetX(y2);

   while (q1 || q2) {
      sw = 0;

      if (q1 > 1 || q2 > 1) sw = 4;

      if (q1 & 1) {
         if (IsX(y1))
            y1 = z;
         else
            sw = sw | 2;
      }

      if (q2 & 1) {
         if (IsX(y2))
            y2 = z;
         else
            sw = sw | 1;
      }

      switch (sw) {
      case 0:
         break;

      case 1:
         CompMod(y2, y2, z, F);
         break;

      case 2:
         CompMod(y1, y1, z, F);
         break;

      case 3:
         Comp2Mod(y1, y2, y1, y2, z, F);
         break;

      case 4:
         CompMod(z, z, z, F);
         break;

      case 5:
         Comp2Mod(z, y2, z, y2, z, F);
         break;

      case 6:
         Comp2Mod(z, y1, z, y1, z, F);
         break;

      case 7:
         Comp3Mod(z, y1, y2, z, y1, y2, z, F);
         break;
      }

      q1 = q1 >> 1;
      q2 = q2 >> 1;
   }
}

long RecComputeDegree(long u, const ZZ_pEX& h, const ZZ_pEXModulus& F,
                      FacVec& fvec)
{
   if (IsX(h)) return 1;

   if (fvec[u].link == -1) return BaseCase(h, fvec[u].q, fvec[u].a, F);

   ZZ_pEX h1, h2;
   long q1, q2, r1, r2;

   q1 = fvec[fvec[u].link].val;
   q2 = fvec[fvec[u].link+1].val;

   TandemPowerCompose(h1, h2, h, q1, q2, F);
   r1 = RecComputeDegree(fvec[u].link, h2, F, fvec);
   r2 = RecComputeDegree(fvec[u].link+1, h1, F, fvec);
   return r1*r2;
}

long RecComputeDegree(const ZZ_pEX& h, const ZZ_pEXModulus& F)
   // f = F.f is assumed to be an "equal degree" polynomial
   // h = X^p mod f
   // the common degree of the irreducible factors of f is computed
{
   if (F.n == 1 || IsX(h))
      return 1;

   FacVec fvec;

   FactorInt(fvec, F.n);

   return RecComputeDegree(fvec.length()-1, h, F, fvec);
}

void FindRoot(ZZ_pE& root, const ZZ_pEX& ff)
// finds a root of ff.
// assumes that ff is monic and splits into distinct linear factors

{
   ZZ_pEXModulus F;
   ZZ_pEX h, h1, f;
   ZZ_pEX r;

   f = ff;

   if (!IsOne(LeadCoeff(f)))
      LogicError("FindRoot: bad args");

   if (deg(f) == 0)
      LogicError("FindRoot: bad args");

   while (deg(f) > 1) {
      build(F, f);
      random(r, deg(F));
      if (IsOdd(ZZ_pE::cardinality())) {
         PowerMod(h, r, RightShift(ZZ_pE::cardinality(), 1), F);
         sub(h, h, 1);
      }
      else {
         AbsTraceMap(h, r, F);
      }
      GCD(h, h, f);
      if (deg(h) > 0 && deg(h) < deg(f)) {
         if (deg(h) > deg(f)/2)
            div(f, f, h);
         else
            f = h;
      }
   }

   negate(root, ConstTerm(f));
}

static
long power(long a, long e)
{
   long i, res;

   res = 1;
   for (i = 1; i <= e; i++)
      res = res * a;

   return res;
}

static
long IrredBaseCase(const ZZ_pEX& h, long q, long a, const ZZ_pEXModulus& F)
{
   long e;
   ZZ_pEX X, s, d;

   e = power(q, a-1);
   PowerCompose(s, h, e, F);
   SetX(X);
   sub(s, s, X);
   GCD(d, F.f, s);
   return IsOne(d);
}

static
long RecIrredTest(long u, const ZZ_pEX& h, const ZZ_pEXModulus& F,
                 const FacVec& fvec)
{
   long  q1, q2;
   ZZ_pEX h1, h2;

   if (IsX(h)) return 0;

   if (fvec[u].link == -1) {
      return IrredBaseCase(h, fvec[u].q, fvec[u].a, F);
   }

   q1 = fvec[fvec[u].link].val;
   q2 = fvec[fvec[u].link+1].val;

   TandemPowerCompose(h1, h2, h, q1, q2, F);
   return RecIrredTest(fvec[u].link, h2, F, fvec)
          && RecIrredTest(fvec[u].link+1, h1, F, fvec);
}

long DetIrredTest(const ZZ_pEX& f)
{
   if (deg(f) <= 0) return 0;
   if (deg(f) == 1) return 1;

   ZZ_pEXModulus F;

   build(F, f);

   ZZ_pEX h;

   FrobeniusMap(h, F);

   ZZ_pEX s;
   PowerCompose(s, h, F.n, F);
   if (!IsX(s)) return 0;

   FacVec fvec;

   FactorInt(fvec, F.n);

   return RecIrredTest(fvec.length()-1, h, F, fvec);
}

long IterIrredTest(const ZZ_pEX& f)
{
   if (deg(f) <= 0) return 0;
   if (deg(f) == 1) return 1;

   ZZ_pEXModulus F;

   build(F, f);

   ZZ_pEX h;

   FrobeniusMap(h, F);

   long CompTableSize = 2*SqrRoot(deg(f));

   ZZ_pEXArgument H;

   build(H, h, F, CompTableSize);

   long i, d, limit, limit_sqr;
   ZZ_pEX g, X, t, prod;

   SetX(X);

   i = 0;
   g = h;
   d = 1;
   limit = 2;
   limit_sqr = limit*limit;

   set(prod);

   while (2*d <= deg(f)) {
      sub(t, g, X);
      MulMod(prod, prod, t, F);
      i++;
      if (i == limit_sqr) {
         GCD(t, f, prod);
         if (!IsOne(t)) return 0;

         set(prod);
         limit++;
         limit_sqr = limit*limit;
         i = 0;
      }

      d = d + 1;
      if (2*d <= deg(f)) {
         CompMod(g, g, H, F);
      }
   }

   if (i > 0) {
      GCD(t, f, prod);
      if (!IsOne(t)) return 0;
   }

   return 1;
}

static
void MulByXPlusY(vec_ZZ_pEX& h, const ZZ_pEX& f, const ZZ_pEX& g)
// h represents the bivariate polynomial h[0] + h[1]*Y + ... + h[n-1]*Y^k,
// where the h[i]'s are polynomials in X, each of degree < deg(f),
// and k < deg(g).
// h is replaced by the bivariate polynomial h*(X+Y) (mod f(X), g(Y)).

{
   long n = deg(g);
   long k = h.length()-1;

   if (k < 0) return;

   if (k < n-1) {
      h.SetLength(k+2);
      h[k+1] = h[k];
      for (long i = k; i >= 1; i--) {
         MulByXMod(h[i], h[i], f);
         add(h[i], h[i], h[i-1]);
      }
      MulByXMod(h[0], h[0], f);
   }
   else {
      ZZ_pEX b, t;

      b = h[n-1];
      for (long i = n-1; i >= 1; i--) {
         mul(t, b, g.rep[i]);
         MulByXMod(h[i], h[i], f);
         add(h[i], h[i], h[i-1]);
         sub(h[i], h[i], t);
      }
      mul(t, b, g.rep[0]);
      MulByXMod(h[0], h[0], f);
      sub(h[0], h[0], t);
   }

   // normalize

   k = h.length()-1;
   while (k >= 0 && IsZero(h[k])) k--;
   h.SetLength(k+1);
}

static
void IrredCombine(ZZ_pEX& x, const ZZ_pEX& f, const ZZ_pEX& g)
{
   if (deg(f) < deg(g)) {
      IrredCombine(x, g, f);
      return;
   }

   // deg(f) >= deg(g)...not necessary, but maybe a little more
   //                    time & space efficient

   long df = deg(f);
   long dg = deg(g);
   long m = df*dg;

   vec_ZZ_pEX h(INIT_SIZE, dg);

   long i;
   for (i = 0; i < dg; i++) h[i].SetMaxLength(df);

   h.SetLength(1);
   set(h[0]);

   vec_ZZ_pE a;

   a.SetLength(2*m);

   for (i = 0; i < 2*m; i++) {
      a[i] = ConstTerm(h[0]);
      if (i < 2*m-1)
         MulByXPlusY(h, f, g);
   }

   MinPolySeq(x, a, m);
}

static
void BuildPrimePowerIrred(ZZ_pEX& f, long q, long e)
{
   long n = power(q, e);

   do {
      random(f, n);
      SetCoeff(f, n);
   } while (!IterIrredTest(f));
}

static
void RecBuildIrred(ZZ_pEX& f, long u, const FacVec& fvec)
{
   if (fvec[u].link == -1)
      BuildPrimePowerIrred(f, fvec[u].q, fvec[u].a);
   else {
      ZZ_pEX g, h;
      RecBuildIrred(g, fvec[u].link, fvec);
      RecBuildIrred(h, fvec[u].link+1, fvec);
      IrredCombine(f, g, h);
   }
}

void BuildIrred(ZZ_pEX& f, long n)
{
   if (n <= 0)
      LogicError("BuildIrred: n must be positive");

   if (NTL_OVERFLOW(n, 1, 0)) ResourceError("overflow in BuildIrred");

   if (n == 1) {
      SetX(f);
      return;
   }

   FacVec fvec;

   FactorInt(fvec, n);

   RecBuildIrred(f, fvec.length()-1, fvec);
}

#if 0
void BuildIrred(ZZ_pEX& f, long n)
{
   if (n <= 0)
      LogicError("BuildIrred: n must be positive");

   if (n == 1) {
      SetX(f);
      return;
   }

   ZZ_pEX g;

   do {
      random(g, n);
      SetCoeff(g, n);
   } while (!IterIrredTest(g));

   f = g;

}
#endif

void BuildRandomIrred(ZZ_pEX& f, const ZZ_pEX& g)
{
   ZZ_pEXModulus G;
   ZZ_pEX h, ff;

   build(G, g);
   do {
      random(h, deg(g));
      IrredPolyMod(ff, h, G);
   } while (deg(ff) < deg(g));

   f = ff;
}

/************* NEW DDF ****************/

NTL_THREAD_LOCAL long ZZ_pEX_GCDTableSize = 4;
NTL_THREAD_LOCAL double ZZ_pEXFileThresh = NTL_FILE_THRESH;
NTL_THREAD_LOCAL static vec_ZZ_pEX *BabyStepFile=0;
NTL_THREAD_LOCAL static vec_ZZ_pEX *GiantStepFile=0;
NTL_THREAD_LOCAL static long use_files;

static
double CalcTableSize(long n, long k)
{
   double sz = ZZ_p::storage();
   sz = sz*ZZ_pE::degree();
   sz = sz + NTL_VECTOR_HEADER_SIZE + sizeof(vec_ZZ_p);
   sz = sz*n;
   sz = sz + NTL_VECTOR_HEADER_SIZE + sizeof(vec_ZZ_pE);
   sz = sz * k;
   sz = sz/1024;
   return sz;
}

static
void GenerateBabySteps(ZZ_pEX& h1, const ZZ_pEX& f, const ZZ_pEX& h, long k,
                       FileList& flist, long verbose)

{
   double t;

   if (verbose) { cerr << "generating baby steps..."; t = GetTime(); }

   ZZ_pEXModulus F;
   build(F, f);

   ZZ_pEXArgument H;

#if 0
   double n2 = sqrt(double(F.n));
   double n4 = sqrt(n2);
   double n34 = n2*n4;
   long sz = long(ceil(n34/sqrt(sqrt(2.0))));
#else
   long sz = 2*SqrRoot(F.n);
#endif

   build(H, h, F, sz);

   h1 = h;

   long i;

   if (!use_files) {
      (*BabyStepFile).SetLength(k-1);
   }

   for (i = 1; i <= k-1; i++) {
      if (use_files) {
         ofstream s;
         OpenWrite(s, FileName("baby", i), flist);
         s << h1 << "\n";
         CloseWrite(s);
      }
      else
         (*BabyStepFile)(i) = h1;

      CompMod(h1, h1, H, F);
      if (verbose) cerr << "+";
   }

   if (verbose)
      cerr << (GetTime()-t) << "\n";

}

static
void GenerateGiantSteps(const ZZ_pEX& f, const ZZ_pEX& h, long l,
                        FileList& flist, long verbose)
{

   double t;

   if (verbose) { cerr << "generating giant steps..."; t = GetTime(); }

   ZZ_pEXModulus F;
   build(F, f);

   ZZ_pEXArgument H;

#if 0
   double n2 = sqrt(double(F.n));
   double n4 = sqrt(n2);
   double n34 = n2*n4;
   long sz = long(ceil(n34/sqrt(sqrt(2.0))));
#else
   long sz = 2*SqrRoot(F.n);
#endif

   build(H, h, F, sz);

   ZZ_pEX h1;

   h1 = h;

   long i;

   if (!use_files) {
      (*GiantStepFile).SetLength(l);
   }

   for (i = 1; i <= l-1; i++) {
      if (use_files) {
         ofstream s;
         OpenWrite(s, FileName("giant", i), flist);
         s << h1 << "\n";
         CloseWrite(s);
      }
      else
        (*GiantStepFile)(i) = h1;

      CompMod(h1, h1, H, F);
      if (verbose) cerr << "+";
   }

   if (use_files) {
      ofstream s;
      OpenWrite(s, FileName("giant", i), flist);
      s << h1 << "\n";
      CloseWrite(s);
   }
   else
      (*GiantStepFile)(i) = h1;

   if (verbose)
      cerr << (GetTime()-t) << "\n";

}

static
void NewAddFactor(vec_pair_ZZ_pEX_long& u, const ZZ_pEX& g, long m, long verbose)
{
   long len = u.length();

   u.SetLength(len+1);
   u[len].a = g;
   u[len].b = m;

   if (verbose) {
      cerr << "split " << m << " " << deg(g) << "\n";
   }
}

static
void NewProcessTable(vec_pair_ZZ_pEX_long& u, ZZ_pEX& f, const ZZ_pEXModulus& F,
                     vec_ZZ_pEX& buf, long size, long StartInterval,
                     long IntervalLength, long verbose)

{
   if (size == 0) return;

   ZZ_pEX& g = buf[size-1];

   long i;

   for (i = 0; i < size-1; i++)
      MulMod(g, g, buf[i], F);

   GCD(g, f, g);

   if (deg(g) == 0) return;

   div(f, f, g);

   long d = (StartInterval-1)*IntervalLength + 1;
   i = 0;
   long interval = StartInterval;

   while (i < size-1 && 2*d <= deg(g)) {
      GCD(buf[i], buf[i], g);
      if (deg(buf[i]) > 0) {
         NewAddFactor(u, buf[i], interval, verbose);
         div(g, g, buf[i]);
      }

      i++;
      interval++;
      d += IntervalLength;
   }

   if (deg(g) > 0) {
      if (i == size-1)
         NewAddFactor(u, g, interval, verbose);
      else
         NewAddFactor(u, g, (deg(g)+IntervalLength-1)/IntervalLength, verbose);
   }
}

static
void FetchGiantStep(ZZ_pEX& g, long gs, const ZZ_pEXModulus& F)
{
   if (use_files) {
      ifstream s;
      OpenRead(s, FileName("giant", gs));
      NTL_INPUT_CHECK_ERR(s >> g);
   }
   else
      g = (*GiantStepFile)(gs);

   rem(g, g, F);
}

static
void FetchBabySteps(vec_ZZ_pEX& v, long k)
{
   v.SetLength(k);

   SetX(v[0]);

   long i;
   for (i = 1; i <= k-1; i++) {
      if (use_files) {
         ifstream s;
         OpenRead(s, FileName("baby", i));
         NTL_INPUT_CHECK_ERR(s >> v[i]);
      }
      else
         v[i] = (*BabyStepFile)(i);
   }
}

static
void GiantRefine(vec_pair_ZZ_pEX_long& u, const ZZ_pEX& ff, long k, long l,
                 long verbose)

{
   double t;

   if (verbose) {
      cerr << "giant refine...";
      t = GetTime();
   }

   u.SetLength(0);

   vec_ZZ_pEX BabyStep;

   FetchBabySteps(BabyStep, k);

   vec_ZZ_pEX buf(INIT_SIZE, ZZ_pEX_GCDTableSize);

   ZZ_pEX f;
   f = ff;

   ZZ_pEXModulus F;
   build(F, f);

   ZZ_pEX g;
   ZZ_pEX h;

   long size = 0;

   long first_gs;

   long d = 1;

   while (2*d <= deg(f)) {

      long old_n = deg(f);

      long gs = (d+k-1)/k;
      long bs = gs*k - d;

      if (bs == k-1) {
         size++;
         if (size == 1) first_gs = gs;
         FetchGiantStep(g, gs, F);
         sub(buf[size-1], g, BabyStep[bs]);
      }
      else {
         sub(h, g, BabyStep[bs]);
         MulMod(buf[size-1], buf[size-1], h, F);
      }

      if (verbose && bs == 0) cerr << "+";

      if (size == ZZ_pEX_GCDTableSize && bs == 0) {
         NewProcessTable(u, f, F, buf, size, first_gs, k, verbose);
         if (verbose) cerr << "*";
         size = 0;
      }

      d++;

      if (2*d <= deg(f) && deg(f) < old_n) {
         build(F, f);

         long i;
         for (i = 1; i <= k-1; i++)
            rem(BabyStep[i], BabyStep[i], F);
      }
   }

   if (size > 0) {
      NewProcessTable(u, f, F, buf, size, first_gs, k, verbose);
      if (verbose) cerr << "*";
   }

   if (deg(f) > 0)
      NewAddFactor(u, f, 0, verbose);

   if (verbose) {
      t = GetTime()-t;
      cerr << "giant refine time: " << t << "\n";
   }
}

static
void IntervalRefine(vec_pair_ZZ_pEX_long& factors, const ZZ_pEX& ff,
                    long k, long gs, const vec_ZZ_pEX& BabyStep, long verbose)

{
   vec_ZZ_pEX buf(INIT_SIZE, ZZ_pEX_GCDTableSize);

   ZZ_pEX f;
   f = ff;

   ZZ_pEXModulus F;
   build(F, f);

   ZZ_pEX g;

   FetchGiantStep(g, gs, F);

   long size = 0;

   long first_d;

   long d = (gs-1)*k + 1;
   long bs = k-1;

   while (bs >= 0 && 2*d <= deg(f)) {

      long old_n = deg(f);

      if (size == 0) first_d = d;
      rem(buf[size], BabyStep[bs], F);
      sub(buf[size], buf[size], g);
      size++;

      if (size == ZZ_pEX_GCDTableSize) {
         NewProcessTable(factors, f, F, buf, size, first_d, 1, verbose);
         size = 0;
      }

      d++;
      bs--;

      if (bs >= 0 && 2*d <= deg(f) && deg(f) < old_n) {
         build(F, f);
         rem(g, g, F);
      }
   }

   NewProcessTable(factors, f, F, buf, size, first_d, 1, verbose);

   if (deg(f) > 0)
      NewAddFactor(factors, f, deg(f), verbose);
}

static
void BabyRefine(vec_pair_ZZ_pEX_long& factors, const vec_pair_ZZ_pEX_long& u,
                long k, long l, long verbose)

{
   double t;

   if (verbose) {
      cerr << "baby refine...";
      t = GetTime();
   }

   factors.SetLength(0);

   vec_ZZ_pEX BabyStep;

   long i;
   for (i = 0; i < u.length(); i++) {
      const ZZ_pEX& g = u[i].a;
      long gs = u[i].b;

      if (gs == 0 || 2*((gs-1)*k+1) > deg(g))
         NewAddFactor(factors, g, deg(g), verbose);
      else {
         if (BabyStep.length() == 0)
            FetchBabySteps(BabyStep, k);
         IntervalRefine(factors, g, k, gs, BabyStep, verbose);
      }
   }

   if (verbose) {
      t = GetTime()-t;
      cerr << "baby refine time: " << t << "\n";
   }
}

void NewDDF(vec_pair_ZZ_pEX_long& factors,
            const ZZ_pEX& f,
            const ZZ_pEX& h,
            long verbose)

{
   if (!IsOne(LeadCoeff(f)))
      LogicError("NewDDF: bad args");

   if (deg(f) == 0) {
      factors.SetLength(0);
      return;
   }

   if (deg(f) == 1) {
      factors.SetLength(0);
      append(factors, cons(f, 1L));
      return;
   }

   long B = deg(f)/2;
   long k = SqrRoot(B);
   long l = (B+k-1)/k;

   ZZ_pEX h1;

   if (CalcTableSize(deg(f), k + l - 1) > ZZ_pEXFileThresh)
      use_files = 1;
   else
      use_files = 0;

   FileList flist;

   vec_ZZ_pEX local_BabyStepFile;
   vec_ZZ_pEX local_GiantStepFile;

   BabyStepFile = &local_BabyStepFile;
   GiantStepFile = &local_GiantStepFile;

   GenerateBabySteps(h1, f, h, k, flist, verbose);

   GenerateGiantSteps(f, h1, l, flist, verbose);

   vec_pair_ZZ_pEX_long u;
   GiantRefine(u, f, k, l, verbose);
   BabyRefine(factors, u, k, l, verbose);
}

long IterComputeDegree(const ZZ_pEX& h, const ZZ_pEXModulus& F)
{
   long n = deg(F);

   if (n == 1 || IsX(h)) return 1;

   long B = n/2;
   long k = SqrRoot(B);
   long l = (B+k-1)/k;

   ZZ_pEXArgument H;

#if 0
   double n2 = sqrt(double(n));
   double n4 = sqrt(n2);
   double n34 = n2*n4;
   long sz = long(ceil(n34/sqrt(sqrt(2.0))));
#else
   long sz = 2*SqrRoot(F.n);
#endif

   build(H, h, F, sz);

   ZZ_pEX h1;
   h1 = h;

   vec_ZZ_pEX baby;
   baby.SetLength(k);

   SetX(baby[0]);

   long i;

   for (i = 1; i <= k-1; i++) {
      baby[i] = h1;
      CompMod(h1, h1, H, F);
      if (IsX(h1)) return i+1;
   }

   build(H, h1, F, sz);

   long j;

   for (j = 2; j <= l; j++) {
      CompMod(h1, h1, H, F);

      for (i = k-1; i >= 0; i--) {
         if (h1 == baby[i])
            return j*k-i;
      }
   }

   return n;
}

NTL_END_IMPL


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

namespace filter_bay
{
/*!
Model of a gaussian measurement.
The equation ist:
\f[
  y_k = H x_k + v_k
\f]
Where \f$x_k\f$ is the state and \f$v_k\f$ the measurement noise at step k.
The measurement noise is assumed to be a constant white noise.

See https://en.wikipedia.org/wiki/Kalman_filter#Update for further explanation.
*/
template <size_t state_size, size_t observation_size>
struct GaussianObservationModel
{
  /*! \f$x_k\f$ */
  using State = typename Eigen::Matrix<double, state_size, 1>;
  /*! \f$P_k\f$ */
  using StateCovariance = typename Eigen::Matrix<double, state_size, state_size>;
  /*! \f$y_k\f$ */
  using Observation = typename Eigen::Matrix<double, observation_size, 1>;
  /*! \f$H\f$ */
  using ObservationMatrix = typename Eigen::Matrix<double, observation_size, state_size>;
  /*! \f$R\f$ */
  using NoiseCovariance = typename Eigen::Matrix<double, observation_size, observation_size>;
  /*! \f$K\f$ */
  using KalmanGain = typename Eigen::Matrix<double, state_size, observation_size>;

  /*! Observatin matrix of the state */
  ObservationMatrix H;
  /*! Measurement covariance matrix */
  NoiseCovariance R;

  GaussianObservationModel() {}

  GaussianObservationModel(ObservationMatrix h, NoiseCovariance r)
      : H(std::move(h)), R(std::move(r)) {}

  /*!
  Calculates the expected measurement for the given state.
  \param x current state
  */
  Observation calculate_measurement(const State &x) const
  {
    return H * x;
  }

  /*!
  Calculates the kalman gain via the observation model.
  \param P the current covariance of the state (serves as confidence)
  */
  KalmanGain calculate_gain(const StateCovariance &P) const
  {
    // Calculate P * H^T only once
    Eigen::Matrix<double, state_size, observation_size> PH_T = P * H.transpose();
    Eigen::Matrix<double, observation_size, observation_size> S = R + H * PH_T;
    return PH_T * S.inverse();
  }

  /*!
  Calculates the update of the state with the given observation model.
  \param x current (predicted) state
  \param y current measurement
  \param K kalman gain for the (predicted) state covariance
  */
  State update_state(const State &x, const Observation &y, KalmanGain &K) const
  {
    Observation residual = y - calculate_measurement(x);
    return x + K * residual;
  }

  StateCovariance update_covariance(const StateCovariance &P,
                                    const KalmanGain &K) const
  {
    // P = (I-KH)P(I-KH)^T + KRK^T
    // This is more numerically stable and works for non-optimal K vs the equation
    // P = (I - KH) P usually seen in the literature.
    // https://github.com/rlabbe/filterpy/blob/master/filterpy/kalman/kalman_filter.py
    StateCovariance I = StateCovariance::Identity();
    auto I_KH = I - K * H;
    return I_KH * P * I_KH.transpose() + K * R * K.transpose();
  }
};
} // namespace filter_bay


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

#include <experimental/filesystem>
#include <iostream>
#include <string>

namespace fs = std::experimental::filesystem;

template <std::size_t... Idx, typename T, typename R>
bool read_row_help(std::index_sequence<Idx...>, T& row, R& r) {
  return r.read_row(std::get<Idx>(row)...);
}

template <std::size_t... Idx, typename T>
void fill_values(std::index_sequence<Idx...>,
                 T& row,
                 std::vector<double>& data) {
  data.insert(data.end(), {std::get<Idx>(row)...});
}

int main(int argc, char** argv) {
  if (argc > 1) {
    auto file_path = fs::path(argv[1]);
    if (fs::exists(file_path)) {
      const uint32_t columns_num = 5;
      io::CSVReader<columns_num> csv_reader(file_path);

      std::vector<std::string> categorical_column;
      std::vector<double> values;
      using RowType = std::tuple<double, double, double, double, std::string>;
      RowType row;

      uint32_t rows_num = 0;
      try {
        bool done = false;
        while (!done) {
          done = !read_row_help(
              std::make_index_sequence<std::tuple_size<RowType>::value>{}, row,
              csv_reader);
          if (!done) {
            categorical_column.push_back(std::get<4>(row));
            fill_values(std::make_index_sequence<columns_num - 1>{}, row,
                        values);
            ++rows_num;
          }
        }
      } catch (const io::error::no_digit& err) {
        // ignore bad formated samples
        std::cerr << err.what() << std::endl;
      }

      auto x_data = Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic,
                                             Eigen::Dynamic, Eigen::RowMajor>>(
          values.data(), rows_num, columns_num - 1);

      std::cout << x_data << std::endl;

      // Feature-scaling(Normalization):
      // Standardization - zero mean + 1 std
      Eigen::Array<double, 1, Eigen::Dynamic> std_dev =
          ((x_data.rowwise() - x_data.colwise().mean())
               .array()
               .square()
               .colwise()
               .sum() /
           (x_data.rows() - 1))
              .sqrt();

      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> x_data_std =
          (x_data.rowwise() - x_data.colwise().mean()).array().rowwise() /
          std_dev;

      std::cout << x_data_std << std::endl;

      // Min-Max normalization
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> x_data_min_max =
          (x_data.rowwise() - x_data.colwise().minCoeff()).array().rowwise() /
          (x_data.colwise().maxCoeff() - x_data.colwise().minCoeff()).array();

      std::cout << x_data_min_max << std::endl;

      // Average normalization
      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> x_data_avg =
          (x_data.rowwise() - x_data.colwise().mean()).array().rowwise() /
          (x_data.colwise().maxCoeff() - x_data.colwise().minCoeff()).array();

      std::cout << x_data_avg << std::endl;

    } else {
      std::cout << "File path is incorrect " << file_path << "\n";
    }
  } else {
    std::cout << "Please provide a path to a dataset file\n";
  }

  return 0;
}


#include <iostream>
#include <cmath>
#include <Eigen/Dense>
#include "LCPSolve.h"

using namespace Eigen;
using namespace std;

// Forward declarations.
bool checkCompatability(const MatrixXd &M, const int &dim);
bool checkTrivialSol(const VectorXd &q);
MatrixXd constructTableau(const MatrixXd &M, const VectorXd &q);
void pivot(MatrixXd &tableau, const int &row, const int &col);
int initializeTableau(MatrixXd &tableau);
VectorXd basicVars(const MatrixXd &tableau);
bool checkSol(const MatrixXd &tableau);
bool checkRayTermination(const MatrixXd &tableu, const int &pivotCol);
int minRatioTest(const MatrixXd &tableau, const int &pivotCol);
MatrixXd extractSolution(const MatrixXd &tableau);

// Solve LCP by Lemke's Method.
LCP LCPSolve(MatrixXd M, VectorXd q) {
    const int dim = q.size();

    // Initialize LCP data structure.
    LCP solution {};
    solution.M = M;
    solution.q = q;

    // Check that inputs are compatable.
    bool compatable = checkCompatability(M, dim);
    if (!compatable) {
        solution.exitCond = 2;
        return solution;
    }

    // Check for trivial solution.
    bool trivial = checkTrivialSol(q);
    if (trivial) {
        solution.z = VectorXd::Zero(dim);
        solution.w = q;
        solution.exitCond = 0;
        return solution;
    }

    // Construct & initialize tableau.
    MatrixXd tableau = constructTableau(M, q);
    VectorXd index = VectorXd::LinSpaced(dim, 0, dim-1);

    int pivotCol = initializeTableau(tableau);
    int pivotRow = pivotCol - dim;
    index(pivotRow) = 2*dim;

    // Now, the tableau is initialized with a feasible basis. Pivot until there is a feasible basis w/o z0.
    const int maxIter {pow(2, dim)};
    int iter {0};
    bool solFound = false;
    while ((!solFound) && (iter < maxIter)) {
        // Check for ray termination.
        bool rayTermination = checkRayTermination(tableau, pivotCol);
        if (rayTermination) {
            solFound = true;
        } else {
            // Minimum ratio test to determine the pivot row (blocked/dropped variable).
            pivotRow = minRatioTest(tableau, pivotCol);
            pivot(tableau, pivotRow, pivotCol);
            int drop = index(pivotRow);
            index(pivotRow) = pivotCol;

            // Find next entering variable (pivotCol). Dropped variable is pivotRow.
            if (drop > dim-1) {
                pivotCol = drop - dim;
            } else {
                pivotCol = drop + dim;
            }

            // Check for solution.
            solFound = checkSol(tableau);
            iter++;
        }
    }

    // Return solution.
    if (solFound) {
        MatrixXd sols = extractSolution(tableau);
        solution.z = sols.col(0);
        solution.w = sols.col(1);
        solution.exitCond = 0;
    } else {
        MatrixXd sols = extractSolution(tableau);
        solution.z = sols.col(0);
        solution.w = sols.col(1);
        solution.exitCond = 3;
    }

    return solution;
}

// Check that the inputs M and q are compatable.
bool checkCompatability(const MatrixXd &M, const int &dim) {
    const int rows = M.rows();
    const int cols = M.cols();

    if (rows != cols)
        return false;
    else if (rows != dim)
        return false;
    else
        return true;
}

// Check for trivial solution where q is positive.
bool checkTrivialSol(const VectorXd &q) {
    const int dim = q.size();
    for (int i = 0; i < dim; i++)
        if (q[i] <= 0)
            return false;
    return true;
}

// Construct Lemke tableau with auxilillary variable.
MatrixXd constructTableau(const MatrixXd &M, const VectorXd &q) {
    const int dim = q.size();
    MatrixXd tableau = MatrixXd::Zero(dim, 2*dim + 2); // Initialize with zeros.
    VectorXd auxVar = -VectorXd::Ones(dim); // Auxillary variable z_0 = {-1, -1, ... -1}.
    tableau.topLeftCorner(dim, dim) = MatrixXd::Identity(dim, dim); // Enter identity matrix on left side (w_1, w_2, etc).
    tableau.middleCols(dim, dim) = -M; // Enter I-M (z_1, z_2, etc).
    tableau.col(2*dim) = auxVar; // Enter auxiliary variable.
    tableau.col(2*dim + 1) = q; // Enter q.
    return tableau;
}

// Pivot function.
void pivot(MatrixXd &tableau, const int &row, const int &col) {
    const int dim = tableau.rows();
    double pivotElement = tableau(row, col);
    VectorXd newPivotRow = (1/pivotElement)*tableau.row(row);
    tableau.row(row) = newPivotRow;

    VectorXd newNonPivotRow {};
    for (int i = 0; i < dim; i++) {
        if (i != row) {
            newNonPivotRow = tableau.row(i) - tableau(i, col)*tableau.row(row);
            tableau.row(i) = newNonPivotRow;
        }
    }
}

// Initialize tableau.
int initializeTableau(MatrixXd &tableau) {
    const int dim = tableau.rows();
    const int cols = 2*dim +2;

    // Pivot row of min element of q w.r.t. aux column.
    int minRow {0};
    double minVal = tableau.coeff(minRow, cols-1);
    for (int i = 1; i < dim; i++) {
        if (tableau.coeff(i, cols-1) < minVal) {
            minRow = i;
            minVal = tableau(minRow, cols-1);
        }
    }

    pivot(tableau, minRow, cols-2);

    // Return the next entering variable, which is the complement to the non-basic variable.
    int newPivotCol = minRow + dim;
    return newPivotCol;
}

// Return which variables are basic.
VectorXd basicVars(const MatrixXd &tableau) {
    int cols = tableau.cols();
    double varNorm {};
    VectorXd isBasic = VectorXd::Zero(cols - 1);

    double err {1e-8};
    for (int i = 0; i < cols - 1; i++) {
        varNorm = tableau.col(i).norm();
        if (abs(1 - varNorm) < err) {
            isBasic[i] = true;
        } else {
            isBasic[i] = false;
        }
    }

    return isBasic;
}

// Check for solution.
bool checkSol(const MatrixXd &tableau) {

    // Determine basic variables.
    VectorXd isBasic = basicVars(tableau);

    // Size information and initialize q & z0.
    bool solFound {false};
    int dim = tableau.rows();
    int cols = 2*dim + 2;
    VectorXd q = tableau.col(cols-1);
    VectorXd z0 = tableau.col(cols-2);

    // No solution if any element of q is negative.
    for (int i = 0; i <= dim-1; i++) {
        if (q[i] < 0) {
            return solFound;
        }
    }

    // No solution if auxillary variable z0 is basic.
    if (isBasic[cols-2] == 1) {
        return solFound;
    }

    // If q is positive and z0 is non-basic, solution has been found.
    solFound = true;
    return solFound;
}

// Check for secondary ray termination.
bool checkRayTermination(const MatrixXd &tableu, const int &pivotCol) {
    VectorXd column = tableu.col(pivotCol);
    int negTestSum {0};
    for (int i {0}; i < column.size(); i++) {
        if (column(i) <= 0) {
            negTestSum++;
        }
    }

    if (negTestSum == column.size()) {
        return true;
    } else {
        return false;
    }
}

// Minimum ratio test to determine pivot row.
int minRatioTest(const MatrixXd &tableau, const int &pivotCol) {
    const int dim = tableau.rows();

    VectorXd ratioTest = VectorXd::Zero(dim);
    for (int i = 0; i < dim; i++) {
        ratioTest[i] = tableau.coeff(i, 2*dim + 1) / tableau.coeff(i, pivotCol);
        if (ratioTest[i] < 0) {
            ratioTest[i] = 1e10;
        }
    }

    int pivotRow {0};
    double minRatio = ratioTest[0];
    for (int i = 1; i < dim; i++) {
        if (ratioTest[i] < minRatio) {
            minRatio = ratioTest[i];
            pivotRow = i;
        }
    }

    return pivotRow;
}

// Extract solution from tableau.
MatrixXd extractSolution(const MatrixXd &tableau) {
    const int dim = tableau.rows();
    MatrixXd A = MatrixXd::Zero(dim, 2*dim);
    VectorXd z = VectorXd::Zero(dim);
    VectorXd w = VectorXd::Zero(dim);
    VectorXd isBasic = basicVars(tableau);

    for (int i = 0; i < 2*dim; i++)
        if (isBasic[i] == 1)
            A.col(i) = tableau.col(i);

    for (int i = 0; i < 2*dim; i++) {
        if (i < dim) {
            w[i] = A.col(i).dot(tableau.col(2*dim + 1));
        } else {
            z[i-dim] = A.col(i).dot(tableau.col(2*dim + 1));
        }
    }

    MatrixXd sols = MatrixXd::Zero(dim, 2);
    sols.col(0) = z;
    sols.col(1) = w;
    return sols;
}