#pragma once

#include <boost/optional.hpp>

#include <random>
#include <algorithm>
#include <vector>
#include <list>

namespace roerei
{

class partition
{
private:
	partition() = delete;

public:
	static std::vector<size_t> generate_bare(size_t n, size_t parts, boost::optional<uint64_t> seed = boost::none)
	{
		std::vector<size_t> result(n);

		auto it = result.begin();
		size_t remaining = n;
		for(size_t parts_left = parts; parts_left > 0; parts_left--)
		{
			size_t size = remaining / parts_left;
			if(remaining % parts_left > 0)
				size++;

			remaining -= size;
			it = std::fill_n(it, size, parts - parts_left);
		}

		assert(it == result.end());

		if(!seed)
		{
			std::random_device rd;
			seed = rd();
		}

		std::mt19937 g(*seed);
		std::shuffle(result.begin(), result.end(), g);

		return result;
	}
};

}


#include <cstdlib>
#include <iostream>
#include <complex>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <Eigen/Core>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/eigen/vector.hpp>
#include <boost/numeric/bindings/blas/level1.hpp>
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

typedef double real;
typedef std::complex<real> complex;

int main(int argc, char *argv[]) {
  {
    typedef ublas::vector<real> vector;
    typedef ublas::matrix<real> matrix;
    typedef vector::size_type size_type;
    rand_normal<real>::reset();
    size_type n=8;
    vector v1(n), v2(n);
    for (size_type i=0; i<n; ++i)
      v1(i)=rand_normal<real>::get();
    for (size_type i=0; i<n; ++i)
      v2(i)=rand_normal<real>::get();
    matrix M1(n, n), M2(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i) {
    	M1(i, j)=0;
    	M2(i, j)=0;
      }
    ublas::matrix_column<matrix> mc(M1, 2);
    ublas::matrix_row<matrix> mr(M2, 3);
    mc=v1;
    mr=v2;
    std::cout << "ublas using vectors    : dot(v1, v2) = " << ublas::inner_prod(v1, v2) << '\n'
	      << "blas using vectors     : dot(v1, v2) = " << blas::dot(v1, v2) << '\n'
	      << "blas using cols & rows : dot(v1, v2) = " << blas::dot(mc, mr) << '\n'
	      << "blas using vectors     : dotu(v1, v2) = " << blas::dotu(v1, v2) << '\n'
	      << "blas using cols & rows : dotu(v1, v2) = " << blas::dotu(mc, mr) << '\n';
  }
  {
    typedef ublas::vector<complex> vector;
    typedef ublas::matrix<complex> matrix;
    typedef vector::size_type size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    vector v1(n), v2(n);
    for (size_type i=0; i<n; ++i)
      v1(i)=rand_normal<complex>::get();
    for (size_type i=0; i<n; ++i)
      v2(i)=rand_normal<complex>::get();
    matrix M1(n, n), M2(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i) {
    	M1(i, j)=0;
    	M2(i, j)=0;
      }
    ublas::matrix_column<matrix> mc(M1, 2);
    ublas::matrix_row<matrix> mr(M2, 3);
    mc=v1;
    mr=v2;
    std::cout << "ublas using vectors    : dot(v1, v2) = " << ublas::inner_prod(v1, v2) << '\n'
	      << "blas using vectors     : dot(v1, v2) = " << blas::dot(v1, v2) << '\n'
	      << "blas using cols & rows : dot(v1, v2) = " << blas::dot(mc, mr) << '\n'
	      << "blas using vectors     : dotu(v1, v2) = " << blas::dotu(v1, v2) << '\n'
	      << "blas using cols & rows : dotu(v1, v2) = " << blas::dotu(mc, mr) << '\n';
  }
  {
    typedef Eigen::Matrix<real, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<real>::reset();
    size_type n=8;
    vector v1(n), v2(n);
    for (size_type i=0; i<n; ++i)
      v1(i)=rand_normal<real>::get();
    for (size_type i=0; i<n; ++i)
      v2(i)=rand_normal<real>::get();
    matrix M1(n, n), M2(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i) {
    	M1(i, j)=0;
    	M2(i, j)=0;
      }
    auto mc=M1.col(2);
    auto mr=M2.row(3);
    mc=v1;
    mr=v2;
    std::cout << "eigen using vectors    : dot(v1, v2) = " << v1.transpose()*v2 << '\n'
	      << "blas using vectors     : dot(v1, v2) = " << blas::dot(v1, v2) << '\n'
	      << "blas using cols & rows : dot(v1, v2) = " << blas::dot(mc, mr) << '\n'
	      << "blas using vectors     : dotu(v1, v2) = " << blas::dotu(v1, v2) << '\n'
	      << "blas using cols & rows : dotu(v1, v2) = " << blas::dotu(mc, mr) << '\n';
  }
  {
    typedef Eigen::Matrix<complex, Eigen::Dynamic, 1> vector;
    typedef Eigen::Matrix<complex, Eigen::Dynamic, Eigen::Dynamic> matrix;
    typedef int size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    vector v1(n), v2(n);
    for (size_type i=0; i<n; ++i)
      v1(i)=rand_normal<complex>::get();
    for (size_type i=0; i<n; ++i)
      v2(i)=rand_normal<complex>::get();
    matrix M1(n, n), M2(n, n);
    for (size_type j=0; j<n; ++j)
      for (size_type i=0; i<n; ++i) {
    	M1(i, j)=0;
    	M2(i, j)=0;
      }
    auto mc=M1.col(2);
    auto mr=M2.row(3);
    mc=v1;
    mr=v2;
    std::cout << "eigen using vectors    : dot(v1, v2) = " << v1.transpose()*v2 << '\n'
	      << "blas using vectors     : dot(v1, v2) = " << blas::dot(v1, v2) << '\n'
	      << "blas using cols & rows : dot(v1, v2) = " << blas::dot(mc, mr) << '\n'
	      << "blas using vectors     : dotu(v1, v2) = " << blas::dotu(v1, v2) << '\n'
	      << "blas using cols & rows : dotu(v1, v2) = " << blas::dotu(mc, mr) << '\n';
  }
  return EXIT_SUCCESS;
}


#define BOOST_TEST_MODULE "test_expand"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include <periortree/boundary_condition.hpp>
#include <periortree/expand.hpp>
#include <test/point_type.hpp>
#include <test/aabb_type.hpp>

BOOST_AUTO_TEST_CASE(test_centroid_unlimited)
{
    {
        const perior::unlimited_boundary<perior::test::xyz> boundary;

        const perior::test::xyz  l1(0., 0., 0.);
        const perior::test::xyz  u1(5., 5., 5.);
        const perior::test::aabb box1(l1, u1);

        const perior::test::xyz  l2(2., 2., 2.);
        const perior::test::xyz  u2(7., 7., 7.);
        const perior::test::aabb box2(l2, u2);

        perior::test::aabb ex(box1);
        perior::expand(ex, box2, boundary);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.x, 0.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.y, 0.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.z, 0.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.x, 7.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.y, 7.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.z, 7.0, 1e-12);
    }

    {
        const perior::unlimited_boundary<perior::test::xyz> boundary;

        const perior::test::xyz  l1(0., 0., 0.);
        const perior::test::xyz  u1(5., 5., 5.);
        const perior::test::aabb box1(l1, u1);

        const perior::test::xyz  l2(-1., 2., -1.);
        const perior::test::xyz  u2( 7., 3.,  5.);
        const perior::test::aabb box2(l2, u2);

        perior::test::aabb ex(box1);
        perior::expand(ex, box2, boundary);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.x, -1.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.y,  0.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.z, -1.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.x,  7.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.y,  5.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.z,  5.0, 1e-12);
    }
}

BOOST_AUTO_TEST_CASE(test_centroid_cubic_periodic)
{
    const perior::test::xyz lw(0., 0., 0.);
    const perior::test::xyz up(10., 10., 10.);
    const perior::cubic_periodic_boundary<perior::test::xyz> boundary(lw, up);
    /* inside the boundary */{
        const perior::test::xyz  l1(0., 0., 0.);
        const perior::test::xyz  u1(5., 5., 5.);
        const perior::test::aabb box1(l1, u1);

        const perior::test::xyz  l2(2., 2., 2.);
        const perior::test::xyz  u2(7., 7., 7.);
        const perior::test::aabb box2(l2, u2);

        perior::test::aabb ex(box1);
        perior::expand(ex, box2, boundary);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.x, 0.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.y, 0.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.z, 0.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.x, 7.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.y, 7.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.z, 7.0, 1e-12);
    }
    /* box includes entry */{
        const perior::test::xyz  l1(0., 0., 0.);
        const perior::test::xyz  u1(5., 5., 5.);
        const perior::test::aabb box1(l1, u1);

        const perior::test::xyz  l2(2., 2., 2.);
        const perior::test::xyz  u2(3., 3., 3.);
        const perior::test::aabb box2(l2, u2);

        perior::test::aabb ex(box1);
        perior::expand(ex, box2, boundary);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.x, 0.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.y, 0.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.z, 0.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.x, 5.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.y, 5.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.z, 5.0, 1e-12);
    }
    /* entry includes box */{
        const perior::test::xyz  l1(2., 2., 2.);
        const perior::test::xyz  u1(3., 3., 3.);
        const perior::test::aabb box1(l1, u1);

        const perior::test::xyz  l2(0., 0., 0.);
        const perior::test::xyz  u2(5., 5., 5.);
        const perior::test::aabb box2(l2, u2);

        perior::test::aabb ex(box1);
        perior::expand(ex, box2, boundary);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.x, 0.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.y, 0.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.z, 0.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.x, 5.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.y, 5.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.z, 5.0, 1e-12);
    }
    /* the nearest one is a superposed one */{
        const perior::test::xyz  l1(0., 0., 0.);
        const perior::test::xyz  u1(1., 1., 1.);
        const perior::test::aabb box1(l1, u1);

        const perior::test::xyz  l2( 9.,  9.,  9.);
        const perior::test::xyz  u2(10., 10., 10.);
        const perior::test::aabb box2(l2, u2);

        perior::test::aabb ex(box1);
        perior::expand(ex, box2, boundary);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.x, 9.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.y, 9.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.z, 9.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.x, 1.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.y, 1.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.z, 1.0, 1e-12);
    }
    /* the nearest one is a superposed one in only 1D */{
        const perior::test::xyz  l1( 0., 0., 0.);
        const perior::test::xyz  u1( 1., 1., 1.);
        const perior::test::aabb box1(l1, u1);

        const perior::test::xyz  l2( 9., 0., 0.);
        const perior::test::xyz  u2(10., 1., 1.);
        const perior::test::aabb box2(l2, u2);

        perior::test::aabb ex(box1);
        perior::expand(ex, box2, boundary);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.x, 9.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.y, 0.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.lower.z, 0.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.x, 1.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.y, 1.0, 1e-12);
        BOOST_CHECK_CLOSE_FRACTION(ex.upper.z, 1.0, 1e-12);
    }
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. 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_

//
// Compare arithmetic results using fixed_int to GMP results.
//

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#ifdef TEST_GMP
#include <boost/multiprecision/gmp.hpp>
#endif
#ifdef TEST_MPFR
#include <boost/multiprecision/mpfr.hpp>
#endif
#include <boost/multiprecision/cpp_dec_float.hpp>
#include "test.hpp"

template <class Number, class BigNumber>
void test()
{
   using namespace boost::multiprecision;
   typedef Number test_type;

   test_type a = 1;
   a /= 3;
   test_type b = -a;

   BigNumber r;
   BOOST_CHECK_EQUAL(add(r, a, a), BigNumber(a) + BigNumber(a));
   BOOST_CHECK_EQUAL(subtract(r, a, b), BigNumber(a) - BigNumber(b));
   BOOST_CHECK_EQUAL(subtract(r, b, a), BigNumber(b) - BigNumber(a));
   BOOST_CHECK_EQUAL(multiply(r, a, a), BigNumber(a) * BigNumber(a));
}

int main()
{
   using namespace boost::multiprecision;

   test<cpp_dec_float_50, cpp_dec_float_100>();

#ifdef TEST_GMP
   test<mpf_float_50, mpf_float_100>();
#endif
#ifdef TEST_MPFR
   test<mpfr_float_50, mpfr_float_100>();
#endif

   return boost::report_errors();
}


#include <boost/test/unit_test.hpp>
#include "functions/multiplication.hh"
#include "functions/full_function_defs.hh"
#include "functions/variables.hh"
#include "functions/polynomial.hh"
#include "functions/addition.hh"
#include "functions/operators.hh"
#include "functions/streaming.hh"

BOOST_AUTO_TEST_CASE(multiplication_test) {
  using namespace manifolds;
  Multiplication<decltype(x), decltype(z)> a(x, z);

  BOOST_CHECK_EQUAL(a(1, 2, 3, 4), 3);
  BOOST_CHECK_EQUAL(a(1, 2, 4, 8, 16), 4);

  auto f = Cos()(x * x) * y;
  BOOST_CHECK_EQUAL(f(2, 3), std::cos(4.0) * 3);
  auto f2 = -2_c * x * Sin()(x * x) * y;
  BOOST_CHECK_EQUAL((-2_c)(1), -2);
  BOOST_CHECK_EQUAL((-2_c * x * y)(2, 1), -4);
  BOOST_CHECK_EQUAL((Sin()(x * x))(2), std::sin(4));
  BOOST_CHECK_EQUAL((-2_c * Sin())(2), -2 * std::sin(2));
  BOOST_CHECK_EQUAL(f2(2, 1), -4 * std::sin(4));
}


// Copyright Yamaha 2021
// MIT License
// https://github.com/yamaha-bps/cbr_control/blob/master/LICENSE

#ifndef NL_PROBLEM_SEGWAY_HPP_
#define NL_PROBLEM_SEGWAY_HPP_

#include <Eigen/Dense>

#include "segway_dynamics.hpp"

struct SegwayProblem
{
  constexpr static std::size_t nx = 7;
  constexpr static std::size_t nu = 2;
  constexpr static bool xl_always_feasible = false;

  using state_t = Eigen::Matrix<double, nx, 1>;
  using deriv_t = state_t;
  using input_t = Eigen::Matrix<double, nu, 1>;
  using Q_t = Eigen::Matrix<double, nx, nx>;
  using R_t = Eigen::Matrix<double, nu, nu>;

  // constants x y psi vel r theta thetaDot
  const Q_t Q = (state_t() << 10., 0.001, 0.0, 1., 0.001, 1.0, 0.1).finished().asDiagonal();
  const R_t R = Eigen::Vector2d(1, 0.1).asDiagonal();
  const Q_t QT = Q;

  template<typename T1, typename T2>
  auto get_f(const Eigen::MatrixBase<T1> & x, const Eigen::MatrixBase<T2> & u) const
  {
    static_assert(
      T1::RowsAtCompileTime == nx &&
      T1::ColsAtCompileTime == 1,
      "First argument must be an nx*1 Eigen Matrix");

    static_assert(
      T2::RowsAtCompileTime == nu &&
      T2::ColsAtCompileTime == 1,
      "Second argument must be an nu*1 Eigen Matrix");

    return segway_dynamics<T1, T2>(x, u);
  }

  void get_state_lb(double, Eigen::Ref<state_t> state_lb) const
  {
    state_lb <<
      -100.,
      -100.,
      -6.2832,
      -10.,
      -31.416,
      -6.2832,
      -31.416;
  }

  void get_state_ub(double, Eigen::Ref<state_t> state_ub) const
  {
    state_ub <<
      100.,
      100.,
      6.2832,
      10.,
      31.416,
      6.2832,
      31.416;
  }

  void get_input_lb(double, Eigen::Ref<input_t> input_lb) const
  {
    input_lb << -15, -15;
  }

  void get_input_ub(double, Eigen::Ref<input_t> input_ub) const
  {
    input_ub << 15, 15;
  }

  const Q_t & get_Q(double) const {return Q;}
  const R_t & get_R(double) const {return R;}
  const Q_t & get_QT() const {return QT;}

};

#endif  // NL_PROBLEM_SEGWAY_HPP_


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

BOOST_AUTO_TEST_SUITE(TestEvaluateReversePolishNotation)

BOOST_AUTO_TEST_CASE(no_tokens)
{
    const std::vector<std::string> tokens;
    const int expected = 0;
    BOOST_CHECK(expected == Algo::Math::ReversePolishNotation::evaluate(tokens));
}

BOOST_AUTO_TEST_CASE(addition_of_two_values)
{
    const std::vector<std::string> tokens = {"5", "890", "+"};
    const int expected = 895;
    BOOST_CHECK(expected == Algo::Math::ReversePolishNotation::evaluate(tokens));
}

BOOST_AUTO_TEST_CASE(subtraction_of_two_values)
{
    const std::vector<std::string> tokens = {"-1", "-7", "-"};
    const int expected = 6;
    BOOST_CHECK(expected == Algo::Math::ReversePolishNotation::evaluate(tokens));
}

BOOST_AUTO_TEST_CASE(multiplication_of_two_values)
{
    const std::vector<std::string> tokens = {"-1", "10", "*"};
    const int expected = -10;
    BOOST_CHECK(expected == Algo::Math::ReversePolishNotation::evaluate(tokens));
}

BOOST_AUTO_TEST_CASE(division_of_two_values)
{
    {
        const std::vector<std::string> tokens = {"1", "1", "/"};
        const int expected = 1;
        BOOST_CHECK(expected == Algo::Math::ReversePolishNotation::evaluate(tokens));
    }

    {
        const std::vector<std::string> tokens = {"7", "3", "/"};
        const int expected = 2;
        BOOST_CHECK(expected == Algo::Math::ReversePolishNotation::evaluate(tokens));
    }

    {
        const std::vector<std::string> tokens = {"1", "3", "/"};
        const int expected = 0;
        BOOST_CHECK(expected == Algo::Math::ReversePolishNotation::evaluate(tokens));
    }

    {
        const std::vector<std::string> tokens = {"0", "5", "/"};
        const int expected = 0;
        BOOST_CHECK(expected == Algo::Math::ReversePolishNotation::evaluate(tokens));
    }

    {
        const std::vector<std::string> tokens = {"12", "0", "/"};
        const int expected = 0;
        BOOST_CHECK(expected == Algo::Math::ReversePolishNotation::evaluate(tokens));
    }
}

BOOST_AUTO_TEST_CASE(long_valid_expession)
{
    {
        const std::vector<std::string> tokens = {"2", "1", "+", "3", "*"};
        const int expected = 9;
        BOOST_CHECK(expected == Algo::Math::ReversePolishNotation::evaluate(tokens));
    }

    {
        const std::vector<std::string> tokens = {"4", "13", "5", "/", "+"};
        const int expected = 6;
        BOOST_CHECK(expected == Algo::Math::ReversePolishNotation::evaluate(tokens));
    }

    {
        const std::vector<std::string> tokens =
            {"10", "6", "9", "3", "+", "-11", "*", "/", "*", "17", "+", "5", "+"};
        const int expected = 22;
        BOOST_CHECK(expected == Algo::Math::ReversePolishNotation::evaluate(tokens));
    }
}

BOOST_AUTO_TEST_SUITE_END()


/* Author: Wolfgang Bangerth, Texas A&M University, 2005, 2006 */

/*    $Id: step-20.cc 28518 2013-02-21 20:26:17Z bangerth $       */
/*                                                                */
/*    Copyright (C) 2005-2008, 2010-2013 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// @sect3{Include files}

// Since this program is only an adaptation of step-4, there is not much new
// stuff in terms of header files. In deal.II, we usually list include files
// in the order base-lac-grid-dofs-fe-numerics, followed by C++ standard
// include files:
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/function.h>
#include <deal.II/lac/block_vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/block_sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
// For our Schur complement solver, we need two new objects. One is a matrix
// object which acts as the inverse of a matrix by calling an iterative
// solver.
#include <deal.II/lac/iterative_inverse.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_renumbering.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_system.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>

#include <fstream>
#include <iostream>

// This is the only significant new header, namely the one in which the
// Raviart-Thomas finite element is declared:
#include <deal.II/fe/fe_raviart_thomas.h>

// Finally, as a bonus in this program, we will use a tensorial
// coefficient. Since it may have a spatial dependence, we consider it a
// tensor-valued function. The following include file provides the
// TensorFunction class that offers such functionality:
#include <deal.II/base/tensor_function.h>

// The last step is as in all previous programs:
namespace Step20
{
  using namespace dealii;

  // @sect3{The MixedLaplaceProblem class template}

  // Again, since this is an adaptation of step-6, the main class is almost
  // the same as the one in that tutorial program. In terms of member
  // functions, the main differences are that the constructor takes the degree
  // of the Raviart-Thomas element as an argument (and that there is a
  // corresponding member variable to store this value) and the addition of
  // the compute_error function in which, no surprise, we will
  // compute the difference between the exact and the numerical solution to
  // determine convergence of our computations:
  template <int dim>
  class MixedLaplaceProblem
  {
  public:
    MixedLaplaceProblem (const unsigned int degree);
    void run ();

  private:
    void make_grid_and_dofs ();
    void assemble_system ();
    void solve ();
    void compute_errors () const;
    void output_results () const;

    const unsigned int   degree;

    Triangulation<dim>   triangulation;
    FESystem<dim>        fe;
    DoFHandler<dim>      dof_handler;

    // The second difference is that the sparsity pattern, the system matrix,
    // and solution and right hand side vectors are now blocked. What this
    // means and what one can do with such objects is explained in the
    // introduction to this program as well as further down below when we
    // explain the linear solvers and preconditioners for this problem:
    BlockSparsityPattern      sparsity_pattern;
    BlockSparseMatrix<double> system_matrix;

    BlockVector<double>       solution;
    BlockVector<double>       system_rhs;
  };

  // @sect3{Right hand side, boundary values, and exact solution}

  // Our next task is to define the right hand side of our problem (i.e., the
  // scalar right hand side for the pressure in the original Laplace
  // equation), boundary values for the pressure, as well as a function that
  // describes both the pressure and the velocity of the exact solution for
  // later computations of the error. Note that these functions have one, one,
  // and dim+1 components, respectively, and that we pass the
  // number of components down to the Function@<dim@> base
  // class. For the exact solution, we only declare the function that actually
  // returns the entire solution vector (i.e. all components of it) at
  // once. Here are the respective declarations:
  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    RightHandSide () : Function<dim>(1) {}

    virtual double value (const Point<dim>   &p,
                          const unsigned int  component = 0) const;
  };

  template <int dim>
  class PressureBoundaryValues : public Function<dim>
  {
  public:
    PressureBoundaryValues () : Function<dim>(1) {}

    virtual double value (const Point<dim>   &p,
                          const unsigned int  component = 0) const;
  };

  template <int dim>
  class ExactSolution : public Function<dim>
  {
  public:
    ExactSolution () : Function<dim>(dim+1) {}

    virtual void vector_value (const Point<dim> &p,
                               Vector<double>   &value) const;
  };

  // And then we also have to define these respective functions, of
  // course. Given our discussion in the introduction of how the solution
  // should look like, the following computations should be straightforward:
  template <int dim>
  double RightHandSide<dim>::value (const Point<dim>  & /*p*/,
                                    const unsigned int /*component*/) const
  {
    return 0;
  }

  template <int dim>
  double PressureBoundaryValues<dim>::value (const Point<dim> &p,
                                             const unsigned int /*component*/) const
  {
    const double alpha = 0.3;
    const double beta = 1;
    return -(alpha*p[0]*p[1]*p[1]/2 + beta*p[0] - alpha*p[0]*p[0]*p[0]/6);
  }

  template <int dim>
  void
  ExactSolution<dim>::vector_value (const Point<dim> &p,
                                    Vector<double>   &values) const
  {
    Assert (values.size() == dim+1,
            ExcDimensionMismatch (values.size(), dim+1));

    const double alpha = 0.3;
    const double beta = 1;

    values(0) = alpha*p[1]*p[1]/2 + beta - alpha*p[0]*p[0]/2;
    values(1) = alpha*p[0]*p[1];
    values(2) = -(alpha*p[0]*p[1]*p[1]/2 + beta*p[0] - alpha*p[0]*p[0]*p[0]/6);
  }

  // @sect3{The inverse permeability tensor}

  // In addition to the other equation data, we also want to use a
  // permeability tensor, or better -- because this is all that appears in the
  // weak form -- the inverse of the permeability tensor,
  // KInverse. For the purpose of verifying the exactness of the
  // solution and determining convergence orders, this tensor is more in the
  // way than helpful. We will therefore simply set it to the identity matrix.
  //
  // However, a spatially varying permeability tensor is indispensable in
  // real-life porous media flow simulations, and we would like to use the
  // opportunity to demonstrate the technique to use tensor valued functions.
  //
  // Possibly unsurprising, deal.II also has a base class not only for scalar
  // and generally vector-valued functions (the Function base
  // class) but also for functions that return tensors of fixed dimension and
  // rank, the TensorFunction template. Here, the function under
  // consideration returns a dim-by-dim matrix, i.e. a tensor of rank 2 and
  // dimension dim. We then choose the template arguments of the
  // base class appropriately.
  //
  // The interface that the TensorFunction class provides is
  // essentially equivalent to the Function class. In particular,
  // there exists a value_list function that takes a list of
  // points at which to evaluate the function, and returns the values of the
  // function in the second argument, a list of tensors:
  template <int dim>
  class KInverse : public TensorFunction<2,dim>
  {
  public:
    KInverse () : TensorFunction<2,dim>() {}

    virtual void value_list (const std::vector<Point<dim> > &points,
                             std::vector<Tensor<2,dim> >    &values) const;
  };

  // The implementation is less interesting. As in previous examples, we add a
  // check to the beginning of the class to make sure that the sizes of input
  // and output parameters are the same (see step-5 for a discussion of this
  // technique). Then we loop over all evaluation points, and for each one
  // first clear the output tensor and then set all its diagonal elements to
  // one (i.e. fill the tensor with the identity matrix):
  template <int dim>
  void
  KInverse<dim>::value_list (const std::vector<Point<dim> > &points,
                             std::vector<Tensor<2,dim> >    &values) const
  {
    Assert (points.size() == values.size(),
            ExcDimensionMismatch (points.size(), values.size()));

    for (unsigned int p=0; p<points.size(); ++p)
      {
        values[p].clear ();

        for (unsigned int d=0; d<dim; ++d)
          values[p][d][d] = 1.;
      }
  }

  // @sect3{MixedLaplaceProblem class implementation}

  // @sect4{MixedLaplaceProblem::MixedLaplaceProblem}

  // In the constructor of this class, we first store the value that was
  // passed in concerning the degree of the finite elements we shall use (a
  // degree of zero, for example, means to use RT(0) and DG(0)), and then
  // construct the vector valued element belonging to the space $X_h$ described
  // in the introduction. The rest of the constructor is as in the early
  // tutorial programs.
  //
  // The only thing worth describing here is the constructor call of the
  // fe variable. The FESystem class to which this
  // variable belongs has a number of different constructors that all refer to
  // binding simpler elements together into one larger element. In the present
  // case, we want to couple a single RT(degree) element with a single
  // DQ(degree) element. The constructor to FESystem that does
  // this requires us to specity first the first base element (the
  // FE_RaviartThomas object of given degree) and then the number
  // of copies for this base element, and then similarly the kind and number
  // of FE_DGQ elements. Note that the Raviart Thomas element
  // already has dim vector components, so that the coupled
  // element will have dim+1 vector components, the first
  // dim of which correspond to the velocity variable whereas the
  // last one corresponds to the pressure.
  //
  // It is also worth comparing the way we constructed this element from its
  // base elements, with the way we have done so in step-8: there, we have
  // built it as fe (FE_Q@<dim@>(1), dim), i.e. we have simply
  // used dim copies of the FE_Q(1) element, one
  // copy for the displacement in each coordinate direction.
  template <int dim>
  MixedLaplaceProblem<dim>::MixedLaplaceProblem (const unsigned int degree)
    :
    degree (degree),
    fe (FE_RaviartThomas<dim>(degree), 1,
        FE_DGQ<dim>(degree), 1),
    dof_handler (triangulation)
  {}

  // @sect4{MixedLaplaceProblem::make_grid_and_dofs}

  // This next function starts out with well-known functions calls that create
  // and refine a mesh, and then associate degrees of freedom with it:
  template <int dim>
  void MixedLaplaceProblem<dim>::make_grid_and_dofs ()
  {
    GridGenerator::hyper_cube (triangulation, -1, 1);
    triangulation.refine_global (3);

    dof_handler.distribute_dofs (fe);

    // However, then things become different. As mentioned in the
    // introduction, we want to subdivide the matrix into blocks corresponding
    // to the two different kinds of variables, velocity and pressure. To this
    // end, we first have to make sure that the indices corresponding to
    // velocities and pressures are not intermingled: First all velocity
    // degrees of freedom, then all pressure DoFs. This way, the global matrix
    // separates nicely into a $2 \times 2$ system. To achieve this, we have to
    // renumber degrees of freedom base on their vector component, an
    // operation that conveniently is already implemented:
    DoFRenumbering::component_wise (dof_handler);

    // The next thing is that we want to figure out the sizes of these blocks,
    // so that we can allocate an appropriate amount of space. To this end, we
    // call the DoFTools::count_dofs_per_component function that
    // counts how many shape functions are non-zero for a particular vector
    // component. We have dim+1 vector components, and we have to
    // use the knowledge that for Raviart-Thomas elements all shape functions
    // are nonzero in all components. In other words, the number of velocity
    // shape functions equals the number of overall shape functions that are
    // nonzero in the zeroth vector component. On the other hand, the number
    // of pressure variables equals the number of shape functions that are
    // nonzero in the dim-th component. Let us compute these numbers and then
    // create some nice output with that:
    std::vector<unsigned int> dofs_per_component (dim+1);
    DoFTools::count_dofs_per_component (dof_handler, dofs_per_component);
    const unsigned int n_u = dofs_per_component[0],
                       n_p = dofs_per_component[dim];

    std::cout << "Number of active cells: "
              << triangulation.n_active_cells()
              << std::endl
              << "Total number of cells: "
              << triangulation.n_cells()
              << std::endl
              << "Number of degrees of freedom: "
              << dof_handler.n_dofs()
              << " (" << n_u << '+' << n_p << ')'
              << std::endl;

    // The next task is to allocate a sparsity pattern for the matrix that we
    // will create. The way this works is that we first obtain a guess for the
    // maximal number of nonzero entries per row (this could be done more
    // efficiently in this case, but we only want to solve relatively small
    // problems for which this is not so important). In the second step, we
    // allocate a $2 \times 2$ block pattern and then reinitialize each of the blocks
    // to its correct size using the n_u and n_p
    // variables defined above that hold the number of velocity and pressure
    // variables. In this second step, we only operate on the individual
    // blocks of the system. In the third step, we therefore have to instruct
    // the overlying block system to update its knowledge about the sizes of
    // the blocks it manages; this happens with the
    // sparsity_pattern.collect_sizes() call:
    const unsigned int
    n_couplings = dof_handler.max_couplings_between_dofs();

    sparsity_pattern.reinit (2,2);
    sparsity_pattern.block(0,0).reinit (n_u, n_u, n_couplings);
    sparsity_pattern.block(1,0).reinit (n_p, n_u, n_couplings);
    sparsity_pattern.block(0,1).reinit (n_u, n_p, n_couplings);
    sparsity_pattern.block(1,1).reinit (n_p, n_p, n_couplings);
    sparsity_pattern.collect_sizes();

    // Now that the sparsity pattern and its blocks have the correct sizes, we
    // actually need to construct the content of this pattern, and as usual
    // compress it, before we also initialize a block matrix with this block
    // sparsity pattern:
    DoFTools::make_sparsity_pattern (dof_handler, sparsity_pattern);
    sparsity_pattern.compress();

    system_matrix.reinit (sparsity_pattern);

    // Then we have to resize the solution and right hand side vectors in
    // exactly the same way:
    solution.reinit (2);
    solution.block(0).reinit (n_u);
    solution.block(1).reinit (n_p);
    solution.collect_sizes ();

    system_rhs.reinit (2);
    system_rhs.block(0).reinit (n_u);
    system_rhs.block(1).reinit (n_p);
    system_rhs.collect_sizes ();
  }

  // @sect4{MixedLaplaceProblem::assemble_system}

  // Similarly, the function that assembles the linear system has mostly been
  // discussed already in the introduction to this example. At its top, what
  // happens are all the usual steps, with the addition that we do not only
  // allocate quadrature and FEValues objects for the cell terms,
  // but also for face terms. After that, we define the usual abbreviations
  // for variables, and the allocate space for the local matrix and right hand
  // side contributions, and the array that holds the global numbers of the
  // degrees of freedom local to the present cell.
  template <int dim>
  void MixedLaplaceProblem<dim>::assemble_system ()
  {
    QGauss<dim>   quadrature_formula(degree+2);
    QGauss<dim-1> face_quadrature_formula(degree+2);

    FEValues<dim> fe_values (fe, quadrature_formula,
                             update_values    | update_gradients |
                             update_quadrature_points  | update_JxW_values);
    FEFaceValues<dim> fe_face_values (fe, face_quadrature_formula,
                                      update_values    | update_normal_vectors |
                                      update_quadrature_points  | update_JxW_values);

    const unsigned int   dofs_per_cell   = fe.dofs_per_cell;
    const unsigned int   n_q_points      = quadrature_formula.size();
    const unsigned int   n_face_q_points = face_quadrature_formula.size();

    FullMatrix<double>   local_matrix (dofs_per_cell, dofs_per_cell);
    Vector<double>       local_rhs (dofs_per_cell);

    std::vector<unsigned int> local_dof_indices (dofs_per_cell);

    // The next step is to declare objects that represent the source term,
    // pressure boundary value, and coefficient in the equation. In addition
    // to these objects that represent continuous functions, we also need
    // arrays to hold their values at the quadrature points of individual
    // cells (or faces, for the boundary values). Note that in the case of the
    // coefficient, the array has to be one of matrices.
    const RightHandSide<dim>          right_hand_side;
    const PressureBoundaryValues<dim> pressure_boundary_values;
    const KInverse<dim>               k_inverse;

    std::vector<double> rhs_values (n_q_points);
    std::vector<double> boundary_values (n_face_q_points);
    std::vector<Tensor<2,dim> > k_inverse_values (n_q_points);

    // Finally, we need a couple of extractors that we will use to get at the
    // velocity and pressure components of vector-valued shape
    // functions. Their function and use is described in detail in the @ref
    // vector_valued report. Essentially, we will use them as subscripts on
    // the FEValues objects below: the FEValues object describes all vector
    // components of shape functions, while after subscription, it will only
    // refer to the velocities (a set of dim components starting
    // at component zero) or the pressure (a scalar component located at
    // position dim):
    const FEValuesExtractors::Vector velocities (0);
    const FEValuesExtractors::Scalar pressure (dim);

    // With all this in place, we can go on with the loop over all cells. The
    // body of this loop has been discussed in the introduction, and will not
    // be commented any further here:
    typename DoFHandler<dim>::active_cell_iterator
    cell = dof_handler.begin_active(),
    endc = dof_handler.end();
    for (; cell!=endc; ++cell)
      {
        fe_values.reinit (cell);
        local_matrix = 0;
        local_rhs = 0;

        right_hand_side.value_list (fe_values.get_quadrature_points(),
                                    rhs_values);
        k_inverse.value_list (fe_values.get_quadrature_points(),
                              k_inverse_values);

        for (unsigned int q=0; q<n_q_points; ++q)
          for (unsigned int i=0; i<dofs_per_cell; ++i)
            {
              const Tensor<1,dim> phi_i_u     = fe_values[velocities].value (i, q);
              const double        div_phi_i_u = fe_values[velocities].divergence (i, q);
              const double        phi_i_p     = fe_values[pressure].value (i, q);

              for (unsigned int j=0; j<dofs_per_cell; ++j)
                {
                  const Tensor<1,dim> phi_j_u     = fe_values[velocities].value (j, q);
                  const double        div_phi_j_u = fe_values[velocities].divergence (j, q);
                  const double        phi_j_p     = fe_values[pressure].value (j, q);

                  local_matrix(i,j) += (phi_i_u * k_inverse_values[q] * phi_j_u
                                        - div_phi_i_u * phi_j_p
                                        - phi_i_p * div_phi_j_u)
                                       * fe_values.JxW(q);
                }

              local_rhs(i) += -phi_i_p *
                              rhs_values[q] *
                              fe_values.JxW(q);
            }

        for (unsigned int face_no=0;
             face_no<GeometryInfo<dim>::faces_per_cell;
             ++face_no)
          if (cell->at_boundary(face_no))
            {
              fe_face_values.reinit (cell, face_no);

              pressure_boundary_values
              .value_list (fe_face_values.get_quadrature_points(),
                           boundary_values);

              for (unsigned int q=0; q<n_face_q_points; ++q)
                for (unsigned int i=0; i<dofs_per_cell; ++i)
                  local_rhs(i) += -(fe_face_values[velocities].value (i, q) *
                                    fe_face_values.normal_vector(q) *
                                    boundary_values[q] *
                                    fe_face_values.JxW(q));
            }

        // The final step in the loop over all cells is to transfer local
        // contributions into the global matrix and right hand side
        // vector. Note that we use exactly the same interface as in previous
        // examples, although we now use block matrices and vectors instead of
        // the regular ones. In other words, to the outside world, block
        // objects have the same interface as matrices and vectors, but they
        // additionally allow to access individual blocks.
        cell->get_dof_indices (local_dof_indices);
        for (unsigned int i=0; i<dofs_per_cell; ++i)
          for (unsigned int j=0; j<dofs_per_cell; ++j)
            system_matrix.add (local_dof_indices[i],
                               local_dof_indices[j],
                               local_matrix(i,j));
        for (unsigned int i=0; i<dofs_per_cell; ++i)
          system_rhs(local_dof_indices[i]) += local_rhs(i);
      }
  }

  // @sect3{Linear solvers and preconditioners}

  // The linear solvers and preconditioners we use in this example have been
  // discussed in significant detail already in the introduction. We will
  // therefore not discuss the rationale for these classes here any more, but
  // rather only comment on implementational aspects.

  // @sect4{The SchurComplement class template}

  // The next class is the Schur complement class. Its rationale has also been
  // discussed in length in the introduction. The only things we would like to
  // note is that the class, too, is derived from the Subscriptor
  // class and that as mentioned above it stores pointers to the entire block
  // matrix and the inverse of the mass matrix block using
  // SmartPointer objects.
  //
  // The vmult function requires two temporary vectors that we do
  // not want to re-allocate and free every time we call this function. Since
  // here, we have full control over the use of these vectors (unlike above,
  // where a class called by the vmult function required these
  // vectors, not the vmult function itself), we allocate them
  // directly, rather than going through the VectorMemory
  // mechanism. However, again, these member variables do not carry any state
  // between successive calls to the member functions of this class (i.e., we
  // never care what values they were set to the last time a member function
  // was called), we mark these vectors as mutable.
  //
  // The rest of the (short) implementation of this class is straightforward
  // if you know the order of matrix-vector multiplications performed by the
  // vmult function:
  class SchurComplement : public Subscriptor
  {
  public:
    SchurComplement (const BlockSparseMatrix<double> &A,
                     const IterativeInverse<Vector<double> > &Minv);

    void vmult (Vector<double>       &dst,
                const Vector<double> &src) const;

  private:
    const SmartPointer<const BlockSparseMatrix<double> > system_matrix;
    const SmartPointer<const IterativeInverse<Vector<double> > > m_inverse;

    mutable Vector<double> tmp1, tmp2;
  };

  SchurComplement::SchurComplement (const BlockSparseMatrix<double> &A,
                                    const IterativeInverse<Vector<double> > &Minv)
    :
    system_matrix (&A),
    m_inverse (&Minv),
    tmp1 (A.block(0,0).m()),
    tmp2 (A.block(0,0).m())
  {}

  void SchurComplement::vmult (Vector<double>       &dst,
                               const Vector<double> &src) const
  {
    system_matrix->block(0,1).vmult (tmp1, src);
    m_inverse->vmult (tmp2, tmp1);
    system_matrix->block(1,0).vmult (dst, tmp2);
  }

  // @sect4{The ApproximateSchurComplement class template}

  // The third component of our solver and preconditioner system is the class
  // that approximates the Schur complement so we can form a an InverseIterate
  // object that approximates the inverse of the Schur complement. It follows
  // the same pattern as the Schur complement class, with the only exception
  // that we do not multiply with the inverse mass matrix in
  // vmult, but rather just do a single Jacobi
  // step. Consequently, the class also does not have to store a pointer to an
  // inverse mass matrix object.
  //
  // We will later use this class as a template argument to the
  // IterativeInverse class which will in turn want to use it as a
  // template argument for the PointerMatrix class. The latter class
  // has a function that requires us to also write a function that
  // provides the product with the transpose of the matrix this object
  // represents. As a consequence, in the code below, we also
  // implement a <tt>Tvmult</tt> function here that represents the
  // product of the transpose matrix with a vector. It is easy to see
  // how this needs to be implemented here: since the matrix is
  // symmetric, we can as well call vmult wherever the
  // product with the transpose matrix is required. (Note, however,
  // that even though we implement this function here, there will in
  // fact not be any need for it as long as we use SolverCG as the
  // solver since that solver does not ever call the function that
  // provides this operation.)
  class ApproximateSchurComplement : public Subscriptor
  {
  public:
    ApproximateSchurComplement (const BlockSparseMatrix<double> &A);

    void vmult (Vector<double>       &dst,
                const Vector<double> &src) const;
    void Tvmult (Vector<double>       &dst,
                 const Vector<double> &src) const;

  private:
    const SmartPointer<const BlockSparseMatrix<double> > system_matrix;

    mutable Vector<double> tmp1, tmp2;
  };

  ApproximateSchurComplement::ApproximateSchurComplement (const BlockSparseMatrix<double> &A)
    :
    system_matrix (&A),
    tmp1 (A.block(0,0).m()),
    tmp2 (A.block(0,0).m())
  {}

  void ApproximateSchurComplement::vmult (Vector<double>       &dst,
                                          const Vector<double> &src) const
  {
    system_matrix->block(0,1).vmult (tmp1, src);
    system_matrix->block(0,0).precondition_Jacobi (tmp2, tmp1);
    system_matrix->block(1,0).vmult (dst, tmp2);
  }

  void ApproximateSchurComplement::Tvmult (Vector<double>       &dst,
                                           const Vector<double> &src) const
  {
    vmult (dst, src);
  }

  // @sect4{MixedLaplace::solve}

  // After all these preparations, we can finally write the function that
  // actually solves the linear problem. We will go through the two parts it
  // has that each solve one of the two equations, the first one for the
  // pressure (component 1 of the solution), then the velocities (component 0
  // of the solution). Both parts need an object representing the inverse mass
  // matrix and an auxiliary vector, and we therefore declare these objects at
  // the beginning of this function.
  template <int dim>
  void MixedLaplaceProblem<dim>::solve ()
  {
    PreconditionIdentity identity;
    IterativeInverse<Vector<double> > m_inverse;
    m_inverse.initialize(system_matrix.block(0,0), identity);
    m_inverse.solver.select("cg");
    static ReductionControl inner_control(1000, 0., 1.e-13);
    m_inverse.solver.set_control(inner_control);

    Vector<double> tmp (solution.block(0).size());

    // Now on to the first equation. The right hand side of it is $B^TM^{-1}F-G$,
    // which is what we compute in the first few lines. We then declare the
    // objects representing the Schur complement, its approximation, and the
    // inverse of the approximation. Finally, we declare a solver object and
    // hand off all these matrices and vectors to it to compute block 1 (the
    // pressure) of the solution:
    {
      Vector<double> schur_rhs (solution.block(1).size());

      m_inverse.vmult (tmp, system_rhs.block(0));
      system_matrix.block(1,0).vmult (schur_rhs, tmp);
      schur_rhs -= system_rhs.block(1);

      SchurComplement
      schur_complement (system_matrix, m_inverse);

      ApproximateSchurComplement
      approximate_schur_complement (system_matrix);

      IterativeInverse<Vector<double> >
      preconditioner;
      preconditioner.initialize(approximate_schur_complement, identity);
      preconditioner.solver.select("cg");
      preconditioner.solver.set_control(inner_control);

      SolverControl solver_control (solution.block(1).size(),
                                    1e-12*schur_rhs.l2_norm());
      SolverCG<>    cg (solver_control);

      cg.solve (schur_complement, solution.block(1), schur_rhs,
                preconditioner);

      std::cout << solver_control.last_step()
                << " CG Schur complement iterations to obtain convergence."
                << std::endl;
    }

    // After we have the pressure, we can compute the velocity. The equation
    // reads $MU=-BP+F$, and we solve it by first computing the right hand
    // side, and then multiplying it with the object that represents the
    // inverse of the mass matrix:
    {
      system_matrix.block(0,1).vmult (tmp, solution.block(1));
      tmp *= -1;
      tmp += system_rhs.block(0);

      m_inverse.vmult (solution.block(0), tmp);
    }
  }

  // @sect3{MixedLaplaceProblem class implementation (continued)}

  // @sect4{MixedLaplace::compute_errors}

  // After we have dealt with the linear solver and preconditioners, we
  // continue with the implementation of our main class. In particular, the
  // next task is to compute the errors in our numerical solution, in both the
  // pressures as well as velocities.
  //
  // To compute errors in the solution, we have already introduced the
  // VectorTools::integrate_difference function in step-7 and
  // step-11. However, there we only dealt with scalar solutions, whereas here
  // we have a vector-valued solution with components that even denote
  // different quantities and may have different orders of convergence (this
  // isn't the case here, by choice of the used finite elements, but is
  // frequently the case in mixed finite element applications). What we
  // therefore have to do is to `mask' the components that we are interested
  // in. This is easily done: the
  // VectorTools::integrate_difference function takes as its last
  // argument a pointer to a weight function (the parameter defaults to the
  // null pointer, meaning unit weights). What we simply have to do is to pass
  // a function object that equals one in the components we are interested in,
  // and zero in the other ones. For example, to compute the pressure error,
  // we should pass a function that represents the constant vector with a unit
  // value in component dim, whereas for the velocity the
  // constant vector should be one in the first dim components,
  // and zero in the location of the pressure.
  //
  // In deal.II, the ComponentSelectFunction does exactly this:
  // it wants to know how many vector components the function it is to
  // represent should have (in our case this would be dim+1, for
  // the joint velocity-pressure space) and which individual or range of
  // components should be equal to one. We therefore define two such masks at
  // the beginning of the function, following by an object representing the
  // exact solution and a vector in which we will store the cellwise errors as
  // computed by integrate_difference:
  template <int dim>
  void MixedLaplaceProblem<dim>::compute_errors () const
  {
    const ComponentSelectFunction<dim>
    pressure_mask (dim, dim+1);
    const ComponentSelectFunction<dim>
    velocity_mask(std::make_pair(0, dim), dim+1);

    ExactSolution<dim> exact_solution;
    Vector<double> cellwise_errors (triangulation.n_active_cells());

    // As already discussed in step-7, we have to realize that it is
    // impossible to integrate the errors exactly. All we can do is
    // approximate this integral using quadrature. This actually presents a
    // slight twist here: if we naively chose an object of type
    // QGauss@<dim@>(degree+1) as one may be inclined to do (this
    // is what we used for integrating the linear system), one realizes that
    // the error is very small and does not follow the expected convergence
    // curves at all. What is happening is that for the mixed finite elements
    // used here, the Gauss points happen to be superconvergence points in
    // which the pointwise error is much smaller (and converges with higher
    // order) than anywhere else. These are therefore not particularly good
    // points for ingration. To avoid this problem, we simply use a
    // trapezoidal rule and iterate it degree+2 times in each
    // coordinate direction (again as explained in step-7):
    QTrapez<1>     q_trapez;
    QIterated<dim> quadrature (q_trapez, degree+2);

    // With this, we can then let the library compute the errors and output
    // them to the screen:
    VectorTools::integrate_difference (dof_handler, solution, exact_solution,
                                       cellwise_errors, quadrature,
                                       VectorTools::L2_norm,
                                       &pressure_mask);
    const double p_l2_error = cellwise_errors.l2_norm();

    VectorTools::integrate_difference (dof_handler, solution, exact_solution,
                                       cellwise_errors, quadrature,
                                       VectorTools::L2_norm,
                                       &velocity_mask);
    const double u_l2_error = cellwise_errors.l2_norm();

    std::cout << "Errors: ||e_p||_L2 = " << p_l2_error
              << ",   ||e_u||_L2 = " << u_l2_error
              << std::endl;
  }

  // @sect4{MixedLaplace::output_results}

  // The last interesting function is the one in which we generate graphical
  // output. Everything here looks obvious and familiar. Note how we construct
  // unique names for all the solution variables at the beginning, like we did
  // in step-8 and other programs later on. The only thing worth mentioning is
  // that for higher order elements, in seems inappropriate to only show a
  // single bilinear quadrilateral per cell in the graphical output. We
  // therefore generate patches of size (degree+1)x(degree+1) to capture the
  // full information content of the solution. See the step-7 tutorial program
  // for more information on this.
  //
  // Note that we output the dim+1 components of the solution
  // vector as a collection of individual scalars here. Most visualization
  // programs will then only offer to visualize them individually, rather than
  // allowing us to plot the flow field as a vector field. However, as
  // explained in the corresponding function of step-22 or the @ref VVOutput
  // "Generating graphical output" section of the @ref vector_valued module,
  // instructing the DataOut class to identify components of the FESystem
  // object as elements of a dim-dimensional vector is not
  // actually very difficult and will then allow us to show results as vector
  // plots. We skip this here for simplicity and refer to the links above for
  // more information.
  template <int dim>
  void MixedLaplaceProblem<dim>::output_results () const
  {
    std::vector<std::string> solution_names;
    switch (dim)
      {
      case 2:
        solution_names.push_back ("u");
        solution_names.push_back ("v");
        solution_names.push_back ("p");
        break;

      case 3:
        solution_names.push_back ("u");
        solution_names.push_back ("v");
        solution_names.push_back ("w");
        solution_names.push_back ("p");
        break;

      default:
        Assert (false, ExcNotImplemented());
      }

    DataOut<dim> data_out;

    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (solution, solution_names);

    data_out.build_patches (degree+1);

    std::ofstream output ("solution.gmv");
    data_out.write_gmv (output);
  }

  // @sect4{MixedLaplace::run}

  // This is the final function of our main class. It's only job is to call
  // the other functions in their natural order:
  template <int dim>
  void MixedLaplaceProblem<dim>::run ()
  {
    make_grid_and_dofs();
    assemble_system ();
    solve ();
    compute_errors ();
    output_results ();
  }
}

// @sect3{The main function}

// The main function we stole from step-6 instead of step-4. It is almost
// equal to the one in step-6 (apart from the changed class names, of course),
// the only exception is that we pass the degree of the finite element space
// to the constructor of the mixed laplace problem (here, we use zero-th order
// elements).
int main ()
{
  try
    {
      using namespace dealii;
      using namespace Step20;

      deallog.depth_console (0);

      MixedLaplaceProblem<2> mixed_laplace_problem(0);
      mixed_laplace_problem.run ();
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

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

  return 0;
}


#ifndef __fovis_internal_utils_hpp__
#define __fovis_internal_utils_hpp__

#include <stdio.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <emmintrin.h>

#include "options.hpp"

namespace fovis
{

#define FOVIS_IS_ALIGNED16(x) (((uintptr_t)(x) & 0xf) == 0)

static inline int
round_up_to_multiple(int x, int a)
{
  int rem = x % a;
  if(rem)
    return x + (a - rem);
  return x;
}

static inline Eigen::Vector3d
_quat_to_roll_pitch_yaw(const Eigen::Quaterniond&q)
{
  double roll_a = 2 * (q.w() * q.x() + q.y() * q.z());
  double roll_b = 1 - 2 * (q.x() * q.x() + q.y() * q.y());
  double roll = atan2(roll_a, roll_b);

  double pitch_sin = 2 * (q.w() * q.y() - q.z() * q.x());
  double pitch = asin(pitch_sin);

  double yaw_a = 2 * (q.w() * q.z() + q.x() * q.y());
  double yaw_b = 1 - 2 * (q.y() * q.y() + q.z() * q.z());
  double yaw = atan2(yaw_a, yaw_b);
  return Eigen::Vector3d(roll, pitch, yaw);
}

static inline Eigen::Quaterniond
_rpy_to_quat(const Eigen::Vector3d rpy)
{
  double roll = rpy(0), pitch = rpy(1), yaw = rpy(2);

  double halfroll = roll / 2;
  double halfpitch = pitch / 2;
  double halfyaw = yaw / 2;

  double sin_r2 = sin(halfroll);
  double sin_p2 = sin(halfpitch);
  double sin_y2 = sin(halfyaw);

  double cos_r2 = cos(halfroll);
  double cos_p2 = cos(halfpitch);
  double cos_y2 = cos(halfyaw);

  Eigen::Quaterniond q;
  q.w() = cos_r2 * cos_p2 * cos_y2 + sin_r2 * sin_p2 * sin_y2;
  q.x() = sin_r2 * cos_p2 * cos_y2 - cos_r2 * sin_p2 * sin_y2;
  q.y() = cos_r2 * sin_p2 * cos_y2 + sin_r2 * cos_p2 * sin_y2;
  q.z() = cos_r2 * cos_p2 * sin_y2 - sin_r2 * sin_p2 * cos_y2;
  return q;
}

static inline void
print_isometry(const Eigen::Isometry3d & iso)
{
  const Eigen::Vector3d & t = iso.translation();
  Eigen::Vector3d rpy = _quat_to_roll_pitch_yaw(Eigen::Quaterniond(iso.rotation()))*180.0/M_PI;
  fprintf(stderr, "trans:(% 6.3f % 6.3f % 6.3f) rot:(% 6.3f % 6.3f % 6.3f)",t(0),t(1),t(2),rpy(0),rpy(1),rpy(2));
  //    dbg("rot:(% 6.3f % 6.3f % 6.3f)",rpy(0),rpy(1),rpy(2));
}

bool optionsGetInt(const VisualOdometryOptions& options, std::string name,
        int* result);

bool optionsGetBool(const VisualOdometryOptions& options, std::string name,
        bool* result);

int optionsGetIntOrFromDefault(const VisualOdometryOptions& options,
        std::string name, const VisualOdometryOptions& defaults);

bool optionsGetBoolOrFromDefault(const VisualOdometryOptions& options,
        std::string name, const VisualOdometryOptions& defaults);

bool optionsGetDouble(const VisualOdometryOptions& options, std::string name,
        double* result);

double optionsGetDoubleOrFromDefault(const VisualOdometryOptions& options,
        std::string name, const VisualOdometryOptions& defaults);

}

#endif


#include <stan/math/prim/scal.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <vector>

TEST(ProbDistributionsHypergeometric, error_check) {
  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::math::hypergeometric_rng(10, 10, 15, rng));

  EXPECT_THROW(stan::math::hypergeometric_rng(30, 10, 15, rng),
               std::domain_error);
  EXPECT_THROW(stan::math::hypergeometric_rng(-30, 10, 15, rng),
               std::domain_error);
  EXPECT_THROW(stan::math::hypergeometric_rng(30, -10, 15, rng),
               std::domain_error);
  EXPECT_THROW(stan::math::hypergeometric_rng(30, 10, -15, rng),
               std::domain_error);
}

TEST(ProbDistributionsHypergeometric, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  int N = 10000;
  int num_draws = 10;
  int K = num_draws;
  boost::math::hypergeometric_distribution<>dist (15, num_draws, 25);
  boost::math::chi_squared mydist(K-1);

  std::vector<int> loc(K - 1);
  for(int i = 1; i < K; i++)
    loc[i - 1] = i - 1;

  int count = 0;
  std::vector<int> bin(K);
  std::vector<double> expect(K);
  for(int i = 0 ; i < K; i++) {
    bin[i] = 0;
    expect[i] = N * pdf(dist, i);
  }

  while (count < N) {
    int a = stan::math::hypergeometric_rng(num_draws, 10, 15, rng);
    int i = 0;
    while (i < K-1 && a > loc[i])
      ++i;
    ++bin[i];
    count++;
   }

  double chi = 0;

  for(int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file standard_gaussian_mapping.hpp
 * \date May 2014
 * \author Manuel Wuthrich (manuel.wuthrich@gmail.com)
 * \author Jan Issac (jan.issac@gmail.com)
 */

#pragma once

#include <Eigen/Dense>

#include <type_traits>

#include <fl/util/traits.hpp>
#include <fl/util/scalar_matrix.hpp>
#include <fl/distribution/interface/sampling.hpp>
#include <fl/distribution/standard_gaussian.hpp>

namespace fl
{

/**
 * \ingroup distribution_interfaces
 *
 * \brief Represents the interface which provides a mapping
 *        from a standard normal variate onto the underlying distribution which
 *        implements this interface
 *
 * \tparam Variate              The Distribution variate type. This is the type
 *                              which is being returned by the mapping or
 *                              sampling, respectively.
 * \tparam StdVariateDimension  Dimension of the source variate type which is
 *                              the standard normal variate
 *                              \f$x_{SNV}\sim{\cal N}(0, I)\f$
 */
template <typename Variate, int StdVariateDimension>
class StandardGaussianMapping
    : public Sampling<Variate>
{
public:
    typedef Eigen::Matrix<Real, StdVariateDimension, 1> StandardVariate;

    /**
     * StandardGaussianMapping constructor. It initializes the mapper
     *
     * \param snv_dimension     Dimension of the standard normal variate
     */
    explicit StandardGaussianMapping(
            int snv_dimension = DimensionOf<StandardVariate>())
        : standard_gaussian_(snv_dimension)
    { }

    /**
     * \brief Overridable default destructor
     */
    virtual ~StandardGaussianMapping() noexcept { }

    /**
     * \brief Mapps a standard normal variate onto a sample of the underlying
     *        distribution which implements this mapper
     *
     * \param sample  SNV sample which will be mapped onto a variate sampe
     *
     * \return A variate according to the underlying distribution
     */
    virtual Variate map_standard_normal(const StandardVariate& sample) const = 0;

    /**
     * \return A variate according to the underlying distribution
     */
    virtual Variate sample() const
    {
        return map_standard_normal(standard_gaussian_.sample());
    }

    /**
     * \return Dimension of the standard normal variate used for mapping
     */
    virtual int standard_variate_dimension() const
    {
        return standard_gaussian_.dimension();
    }

    /**
     * \brief Sets the dimension of the standard normal variate
     *
     * \param snv_dimension The new dimension of the SNV
     */
    virtual void standard_variate_dimension(int snv_dimension)
    {
        standard_gaussian_.dimension(snv_dimension);
    }

protected:
    /**
     * \brief SNV generator
     */
    mutable StandardGaussian<StandardVariate> standard_gaussian_;
};

/**
 * \ingroup distribution_interfaces
 *
 * \brief Represents the interface which provides a mapping
 *        from a scalar standard normal variate onto the underlying distribution
 *        which implements this interface.
 *
 * \tparam Variate          The Distribution variate type. This is the type
 *                          which is being returned by the mapping or sampling,
 *                          respectively.
 */
template <typename Variate>
class StandardGaussianMapping<Variate, 1>
    : public Sampling<Variate>
{
public:
    typedef ScalarMatrix StandardVariate;

    /// \todo fix this (unused argument)
    explicit StandardGaussianMapping(int snv_dimension = 1)
    { }

    /**
     * \brief Overridable default destructor
     */
    virtual ~StandardGaussianMapping() noexcept { }

    /**
     * \brief Mapps a one dimensional standard normal variate onto a sample of
     *        the underlying distribution which implements this mapper
     *
     * \param sample SNV sample which will be mapped onto a variate sampe
     *
     * \return A variate according to the underlying distribution
     */
    virtual Variate map_standard_normal(const StandardVariate& sample) const = 0;

    /**
     * \return A variate according to the underlying distribution
     */
    virtual Variate sample() const
    {
        return map_standard_normal(standard_gaussian_.sample());
    }

    /**
     * \return Dimension of the standard normal variate used for mapping
     */
    virtual int standard_variate_dimension() const
    {
        return 1;
    }

    /// \todo fix this (unused argument)
    virtual void standard_variate_dimension(int snv_dimension)
    {
    }

protected:
    /**
     * \brief One dimensional SNV generator
     */
    mutable StandardGaussian<Real> standard_gaussian_;
};

}


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

  @copyright 2016 NumScale SAS

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

/*!
  @ingroup group-constant
  @defgroup constant-Nbdigits Nbdigits (function template)

  Generates the number of mantissa bits of a floating point number,
  and the total number of bits for integral types.

  @headerref{<boost/simd/constant/nbdigits.hpp>}

  @par Description

  1.  @code
      template<typename T> T Nbdigits();
      @endcode

  2.  @code
      template<typename T> T Nbdigits( boost::simd::as_<T> const& target );
      @endcode

  Generates a value of type @c T containing the Nbdigits constant.

  @par Parameters

  | Name                | Description                                                         |
  |--------------------:|:--------------------------------------------------------------------|
  | **target**          | a [placeholder](@ref type-as) value encapsulating the constant type |

  @par Return Value
  A value of type @c as_integer_t<T> that evaluates to

  | Type                | double                        | float         | Integral        |
  |--------------------:|:------------------------------|---------------|-----------------|
  | value               |   53                          |      24       | sizeof(T)*8     |

   @par Requirements
  - **T** models Value
**/

#include <boost/simd/constant/scalar/nbdigits.hpp>
#include <boost/simd/constant/simd/nbdigits.hpp>

#endif


#include <ancse/dg_rate_of_change.hpp>

#include <Eigen/Dense>
#include <ancse/config.hpp>
#include <ancse/polynomial_basis.hpp>
#include <ancse/dg_handler.hpp>
#include <ancse/numerical_flux.hpp>
#include <fmt/format.h>

/// DG numerical flux term
template <class NumericalFlux>
void DGRateOfChange<NumericalFlux>
:: eval_numerical_flux (Eigen::MatrixXd &dudt,
                        const Eigen::MatrixXd &u0) const
{
    // implement the loop for DG numerical flux term.
    const int n_cells= grid.n_cells;
    const int n_ghost= grid.n_ghost;

    const int n_vars= model->get_nvars();

    const double dx= grid.dx;
    Eigen::VectorXd fL= Eigen::VectorXd::Zero(n_vars), fR= Eigen::VectorXd::Zero(n_vars);
    Eigen::VectorXd uL, uR;

    for (int i= n_ghost - 1; i < n_cells - n_ghost; ++i)
    {
        uL= u0.col(i);
        uR= u0.col(i + 1);

        fL= fR;
        fR= numerical_flux(uL, uR);

        dudt.col(i)= (fL - fR) / dx;
    }
}

/// DG volume integral term
template <class NumericalFlux>
void DGRateOfChange<NumericalFlux>
:: eval_volume_integral(Eigen::MatrixXd &dudt,
                        const Eigen::MatrixXd &u0) const
{
    // implement the loop for DG volume integral.
}

#define REGISTER_NUMERICAL_FLUX(token, FluxType, flux)          \
    if (config["flux"] == (token)) {                            \
        return std::make_shared< DGRateOfChange<FluxType> >(    \
            grid, model, flux, poly_basis, dg_handler);                     \
    }

std::shared_ptr<RateOfChange> make_dg_rate_of_change(
    const nlohmann::json &config,
    const Grid &grid,
    const std::shared_ptr<Model> &model,
    const PolynomialBasis &poly_basis,
    const DGHandler &dg_handler,
    const std::shared_ptr<SimulationTime> &simulation_time)
{
    // Register the other numerical fluxes.

    REGISTER_NUMERICAL_FLUX("central_flux", CentralFlux, CentralFlux(model))
    REGISTER_NUMERICAL_FLUX("rusanov", Rusanov, Rusanov(model))
    REGISTER_NUMERICAL_FLUX("lax_friedrichs",
                            LaxFriedrichs,
                            LaxFriedrichs(grid, model, simulation_time))
    REGISTER_NUMERICAL_FLUX("roe", Roe, Roe(model))
    REGISTER_NUMERICAL_FLUX("hll", HLL, HLL(model))
    REGISTER_NUMERICAL_FLUX("hllc", HLLc, HLLc(model))

    throw std::runtime_error(
        fmt::format("Unknown numerical flux. {}",
                    std::string(config["flux"])));
}

#undef REGISTER_NUMERICAL_FLUX


/*
 * RWHEC_Nov2015_main.hpp
 *
 *  Created on: Nov 16, 2015
 *      Author: atabb
 */

#ifndef TABB_AHMADYOUSEF_RWHEC_MAY2018_MAIN_HPP_
#define TABB_AHMADYOUSEF_RWHEC_MAY2018_MAIN_HPP_

#include "ceres/ceres.h"
#include "glog/logging.h"
#include "Calibration2.hpp"
#include <iostream>
#include "CostFunctions.hpp"
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

using namespace Eigen;

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;

using namespace std;

int RobotWorldHandEyeCalibration(double square_mm_height, double square_mm_width,
		int chess_h, int chess_w, string source_dir, string write_dir,  bool do_camcali, bool do_rwhec, bool do_reconstruction, bool verbose);

void WriteCaliFile(CaliObjectOpenCV2* CO, std::ofstream& out);

void WriteCaliFile(CaliObjectOpenCV2* CO, vector<MatrixXd>& As, vector<Matrix4d>& Bs, Matrix4d& X, Matrix4d& Z, std::ofstream& out);
//void WriteCaliFile(CaliObjectOpenCV2* CO, vector<MatrixXd>& As, vector<MatrixXd>& Bs, Matrix& X, Matrix& Z, std::ofstream& out);

//void ReadRobotFileDensoArm(string write_dir, vector<Matrix4d>& Bs);

void ReadRobotFileRobotCaliTxt(string filename, vector<Matrix4d>& Bs);

double AssessErrorWhole(vector<MatrixXd>& As, vector<Matrix4d>& Bs, Matrix4d& X, Matrix4d& Z);

double AssessRotationError(vector<MatrixXd>& As, vector<Matrix4d>& Bs, Matrix4d& X, Matrix4d& Z);

double AssessRotationErrorAxisAngle(vector<MatrixXd>& As, vector<Matrix4d>& Bs, Matrix4d& X, Matrix4d& Z);

double AssessTranslationErrorDenominator(vector<Matrix4d>& As, vector<Matrix4d>& Bs, Matrix4d& X, Matrix4d& Z);

//double AssessTranslationError(vector<MatrixXd>& As_temp, vector<Matrix4d>& Bs_temp, Matrix4d& X_temp, Matrix4d& Z_temp);
double AssessTranslationError(vector<MatrixXd>& As, vector<Matrix4d>& Bs, Matrix4d& X, Matrix4d& Z);

double CalculateReprojectionError(CaliObjectOpenCV2* CO, vector<MatrixXd>& As, vector<Matrix4d>& Bs,
			Matrix4d& X, Matrix4d& Z, std::ofstream& out, string directory, int cam_number);

void WritePatterns(double* pattern_points, int chess_h, int chess_w, int index_number, string outfile);

string FindValueOfFieldInFile(string filename, string fieldTag, bool seperator);

void EnsureDirHasTrailingBackslash(string& write_directory);

Matrix4d ConvertToMM(Matrix4d XM){
	double unit = 1000;
  XM(0,3) = unit*XM(0,3);
  XM(1,3) = unit*XM(1,3);
  XM(2,3) = unit*XM(2,3);
  return XM;
}

Matrix4d ConvertFromMM(Matrix4d XM){
	double unit = 1000;
  XM(0,3) = XM(0,3)/unit;
  XM(1,3) = XM(1,3)/unit;
  XM(2,3) = XM(2,3)/unit;
  return XM;
}

double ConvertToMM(double XM){
	double unit = 1000;
  return unit*XM;
}

double ConvertFromMM(double XM){
	double unit = 1000;
  return XM/unit;
}

#endif /* TABB_AHMADYOUSEF_RWHEC_MAY2018_MAIN_HPP_ */


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2014 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include "noarbsabr.hpp"
#include "utilities.hpp"

#include <boost/assign/list_of.hpp>

#include <ql/termstructures/volatility/sabrsmilesection.hpp>
#include <ql/experimental/volatility/noarbsabrsmilesection.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;

namespace {
void checkD0(const Real sigmaI, const Real beta, const Real rho, const Real nu,
             const Real tau, const unsigned int absorptions) {

    Real forward = 0.03; // does not matter in the end
    Real alpha = sigmaI / std::pow(forward, beta - 1.0);

    detail::D0Interpolator d(forward, tau, alpha, beta, nu, rho);

    if (std::fabs(d() * detail::NoArbSabrModel::nsim - (Real)absorptions) > 0.1)
        BOOST_ERROR("failed to reproduce number of absorptions at sigmaI="
                    << sigmaI << ", beta=" << beta << ", rho=" << rho << ", nu="
                    << nu << " tau=" << tau << ": D0Interpolator says "
                    << d() * detail::NoArbSabrModel::nsim
                    << " while the reference value is " << absorptions);

    return;
}
}

void NoArbSabrTest::testAbsorptionMatrix() {

    BOOST_TEST_MESSAGE("Testing noarb-sabr absorption matrix ...");

    // check some points explicitly against the external file's contents

    // sigmaI, beta, rho, nu, tau, absorptions
    checkD0(1,0.01,0.75,0.1,0.25,60342); // upper left corner
    checkD0(0.8,0.01,0.75,0.1,0.25,12148);
    checkD0(0.05,0.01,0.75,0.1,0.25,0);
    checkD0(1,0.01,0.75,0.1,10.0,1890509);
    checkD0(0.8,0.01,0.75,0.1,10.0,1740233);
    checkD0(0.05,0.01,0.75,0.1,10.0,0);
    checkD0(1,0.01,0.75,0.1,30.0,2174176);
    checkD0(0.8,0.01,0.75,0.1,30.0,2090672);
    checkD0(0.05,0.01,0.75,0.1,30.0,31);
    checkD0(0.35,0.10,-0.75,0.1,0.25,0);
    checkD0(0.35,0.10,-0.75,0.1,14.75,1087841);
    checkD0(0.35,0.10,-0.75,0.1,30.0,1406569);
    checkD0(0.24,0.90,0.50,0.8,1.25,27);
    checkD0(0.24,0.90,0.50,0.8,25.75,167541);
    checkD0(0.05,0.90,-0.75,0.8,2.0,17);
    checkD0(0.05,0.90,-0.75,0.8,30.0,42100); // lower right corner

    // check the entire matrix against a hash value produced on the original
    // quantlib file noarbsabrabsprobs.cpp to make sure that this has
    // not been changed

    try {
        NoArbSabrModel::checkAbsorptionMatrix();
    } catch(QuantLib::Error) {
        BOOST_ERROR("failed to verify the hash value of the absorption matrix");
    }

}

void NoArbSabrTest::testConsistencyWithHagan() {

    BOOST_TEST_MESSAGE("Testing consistency of noarb-sabr with Hagan et al (2002)");

    // parameters taken from Doust's paper, figure 3

    Real tau = 1.0;
    Real beta = 0.5;
    Real alpha = 0.026;
    Real rho = -0.1;
    Real nu = 0.4;
    Real f = 0.0488;

    SabrSmileSection sabr(tau,f,boost::assign::list_of(alpha)(beta)(nu)(rho));
    NoArbSabrSmileSection noarbsabr(tau,f,boost::assign::list_of(alpha)(beta)(nu)(rho));

    Real absProb=noarbsabr.model()->absorptionProbability();
    if( absProb > 1E-10 || absProb < 0.0 )
        BOOST_ERROR("absorption probability should be close to zero, but is " << absProb);

    Real strike = 0.0001;
    while (strike < 0.15) {
        // test vanilla prices
        Real sabrPrice = sabr.optionPrice(strike);
        Real noarbsabrPrice = noarbsabr.optionPrice(strike);
        if (std::fabs(sabrPrice - noarbsabrPrice) > 1e-5)
            BOOST_ERROR("incosistent Hagan price ("
                        << sabrPrice << ") and noarb-sabr price ("
                        << noarbsabrPrice << ") at strike " << strike);
        // test digitals
        Real sabrDigital = sabr.digitalOptionPrice(strike);
        Real noarbsabrDigital = noarbsabr.digitalOptionPrice(strike);
        if (std::fabs(sabrDigital - noarbsabrDigital) > 1e-3)
            BOOST_ERROR("incosistent Hagan digital ("
                        << sabrDigital << ") and noarb-sabr digital ("
                        << noarbsabrDigital << ") at strike " << strike);
        // test density
        Real sabrDensity = sabr.density(strike);
        Real noarbsabrDensity = noarbsabr.density(strike);
        if (std::fabs(sabrDensity - noarbsabrDensity) > 1e-0)
            BOOST_ERROR("incosistent Hagan density ("
                        << sabrDensity << ") and noarb-sabr density ("
                        << noarbsabrDensity << ") at strike " << strike);
        strike += 0.0001;
    }

}

test_suite* NoArbSabrTest::suite() {
    test_suite* suite = BOOST_TEST_SUITE("NoArbSabrModel tests");
    suite->add(QUANTLIB_TEST_CASE(&NoArbSabrTest::testAbsorptionMatrix));
    suite->add(QUANTLIB_TEST_CASE(&NoArbSabrTest::testConsistencyWithHagan));
    return suite;
}


#include <vector>
#include <iostream>

#include <glm/glm.hpp>
using namespace glm;

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseQR>

#include "Types.h"
#include "Dynamic.h"

namespace{
    using namespace BalloonFEM;
    /* push mat3 coefficients into SpMat,
     * T(target_off + i, source_off + j, H[j][i])
     * pay attention that H is column major
     */
    void pushMat3(std::vector<T> &coeff, int target_off, int source_off, Mat3 H)
    {
        for(int i = 0; i < 3; i++)
            for(int j = 0; j < 3; j++)
			if (H[j][i] != 0)
                coeff.push_back( T(target_off + i, source_off + j, H[j][i]) );
    }

    /* push mat3 coefficients into SpMat,
     * T(target_off, source_off + i, H[i])
     */
    void pushVec3(std::vector<T> &coeff, int target_off, int source_off, Vec3 H)
    {
        for(int i = 0; i < 3; i++)
            coeff.push_back( T(target_off, source_off + i, H[i]) );
    }

    int sgn(double val){
        return ((double(0) < val) - (val < double(0)));
    }
}

namespace BalloonFEM
{
    SpMat Engine::bendingForceAndGradient(ObjState &state, Vvec3 &f_sum)
    {
        /* compute theta */
        Vvec3 &pos = state.world_space_pos;

        SpVec dphi = SpVec::Zero(m_tetra->num_hindges);
        SpMat ddphi(m_tetra->num_hindges, m_tetra->num_hindges);

        ///////////////////////////////////////////////////////////////////////
        /* compute hindge angle theta and phi, dphi, ddphi */
        int offset = 0;
        for(MIter f = m_tetra->films.begin(); f != m_tetra->films.end(); f++)
        {
			std::vector<Piece> &pieces = f->pieces;
            for(EIter h = f->hindges.begin(); h != f->hindges.end(); h++)
            {
                /* normal of piece_info[0] */
                iVec3 &id0 = pieces[h->piece_info[0].x].v_id;
                Vec3 n0 = cross( pos[id0[0]] - pos[id0[2]], pos[id0[1]] - pos[id0[2]] );
                n0 /= length(n0);

                /* normal of piece_info[1] */
                iVec3 &id1 = pieces[h->piece_info[1].x].v_id;
                Vec3 n1 = cross( pos[id1[0]] - pos[id1[2]], pos[id1[1]] - pos[id1[2]] );
                n1 /= length(n1);

                /* edge direction , is the positive direction of piece_info[0]*/
				int i = h->piece_info[0].y;
				int j = (i + 1) % 3, k = (i + 2) % 3;
                Vec3 e = pos[id0[k]] -  pos[id0[j]];
                e /= length(e);

				/* signed theta is defined as positive when n0 n1 point away from each other */
                /* energy is 2*sin(x/2)^2 */
				double tmp = max(min(dot(n0, n1), 1.0), -1.0);
                double theta = acos(tmp) * sgn(dot(e, cross(n0, n1)));
                dphi(offset) = m_bend_model->dphi(theta, h->theta);
                ddphi.coeffRef(offset, offset) = m_bend_model->ddphi(theta, h->theta);
                offset ++;
            }
        }

        //////////////////////////////////////////////////////////////////////
        /* compute hessian matrix and theta gradient */
		printf("compute bending force and hessian matrix \n");
        std::vector<T> theta_coeff;
        theta_coeff.clear();
		theta_coeff.reserve( 2 * 9 * m_tetra->num_hindges );

        std::vector<T> hessian_coeff;
        hessian_coeff.clear();
        hessian_coeff.reserve( 9 * 9 * m_tetra->num_pieces );

        /* compute detrivation of hindge angle theta */
        offset = 0;     /* since there may be more than one film obj */
        for(MIter f = m_tetra->films.begin(); f != m_tetra->films.end(); f++)
        {
            for(PIter p = f->pieces.begin(); p != f->pieces.end(); p++)
            {
                iVec3 &v_id = p->v_id;
                /* edge dire , length , norm and area*/
                Vec3 e[3] = {
                    pos[v_id[2]] - pos[v_id[1]],
                    pos[v_id[0]] - pos[v_id[2]],
                    pos[v_id[1]] - pos[v_id[0]],
                };

                Vec3 norm = cross( e[0], e[1] );
                double A2 = length(norm);
                norm /= A2;

				Vec3 l = Vec3(length(e[0]), length(e[1]), length(e[2]));
				for (int i = 0; i < 3; i++)
					e[i] /= l[i];

                /* height of vertex */
                Vec3 h_inv = l / A2;
				Mat3 w = outerProduct(h_inv, h_inv);

                /* cosine of vertex */

                double cos[3] = {
                    - dot(e[1], e[2]),
                    - dot(e[2], e[0]),
                    - dot(e[0], e[1]),
                };

                /* edge normal m */
                Vec3 m[3] = {
                    cross(e[0], norm),
                    cross(e[1], norm),
                    cross(e[2], norm),
                };

                /* intermediate var Mi */
                Mat3 M[3] = {
                    outerProduct(norm, m[0]),
					outerProduct(norm, m[1]),
					outerProduct(norm, m[2]),
                };

                /* intermediate var Ni */
                Mat3 N[3] = {
                    M[0] / (l.x * l.x), M[1] / (l.y * l.y), M[2] / (l.z * l.z)
                };

                /* intermediate var ci */
                double c[3] = {0, 0, 0};
                Mat3 R[3] = {Mat3(0), Mat3(0), Mat3(0)};
                for(int i = 0; i < 3; i++)
                    if (p->hindge_id[i] != -1)
                    {
						c[i] = dphi(p->hindge_id[i] + offset);
                        R[i] = c[i]*N[i];
                    }

                /* intermediate var di */
                double d[3] = {0, 0, 0};
                for(int i = 0; i < 3; i++)
                {
                    int j = (i+1) % 3, k = (i+2) % 3;
                    d[i] = c[j] * cos[k] + c[k] * cos[j] - c[i];
                }

                /* for each triangle, compute contribution to Heissen */
                for(int i = 0; i < 3; i++)
                {
                    int j = (i+1) % 3, k = (i+2) % 3;

                    /* j = i */
                    Mat3 H = w[i][i] * d[i] * (M[i] + transpose(M[i])) - R[j] - R[k];

                    pushMat3(hessian_coeff, 3 * v_id[i], 3 * v_id[i], H);

                    /* j = i + 1 */
                    H = w[i][j] * (d[i] * transpose(M[j]) + d[j] * M[i]) + R[k];

                    pushMat3(hessian_coeff, 3 * v_id[i], 3 * v_id[j], H);

                    /* symmetric j = i + 2 */
                    pushMat3(hessian_coeff, 3 * v_id[j], 3 * v_id[i], transpose(H));
                }

                /* for each vertex if its correspond edge is hindge
                 * compute gradient of theta
                 */
                for(int i = 0; i < 3; i++)
                {
                    if (p->hindge_id[i] != -1)
                    {
                        int h_id = p->hindge_id[i] + offset;

                        int j = (i+1) % 3;
                        int k = (i+2) % 3;

                        Vec3 tmp;

                        /* \dev_{x0} theta = - n / h0 */
                        tmp = - norm * h_inv[i];
                        pushVec3( theta_coeff, h_id, 3 * v_id[i], tmp);

                        /* \dev_{x1} theta = n * cos2 / h1 */
                        tmp = norm * cos[k] * h_inv[j];
                        pushVec3( theta_coeff, h_id, 3 * v_id[j], tmp);

                        /* \dev_{x1} theta = n * cos1 / h2 */
                        tmp = norm * cos[j] * h_inv[k];
                        pushVec3( theta_coeff, h_id, 3 * v_id[k], tmp);
                    }
                }

            }
            offset += f->hindges.size();
        }

        /* compute bending force */
        SpMat theta_grad(m_tetra->num_hindges, 3 * m_tetra->num_vertex);
        theta_grad.setFromTriplets(theta_coeff.begin(), theta_coeff.end());

        SpVec bendforce = - dphi.transpose() * theta_grad;

        for(size_t i = 0; i < f_sum.size(); i++)
        {
            f_sum[i] += Vec3(
                    bendforce(3*i    ),
                    bendforce(3*i + 1),
                    bendforce(3*i + 2));
        }

        SpMat H(3 * m_tetra->num_vertex, 3 * m_tetra->num_vertex);

        H.setFromTriplets(hessian_coeff.begin(), hessian_coeff.end());

		H +=  theta_grad.transpose() * ddphi * theta_grad;

        return H;
    }
}


#include <string>

#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_ros/transform_listener.h>

#include <Eigen/Geometry>
#include <Eigen/SVD>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "relational_position_publisher");
  ros::NodeHandle n {};

  int target_number {1};
  std::string root_name {"openni_coordinater"};
  std::string to_name {"left_foot"};
  std::string from_name {"torso"};
  {
    ros::NodeHandle pn {"~"};
    pn.getParam("target_number", target_number);
    pn.getParam("root", root_name);
    pn.getParam("to", to_name);
    pn.getParam("from", from_name);
  }

  const auto to_frame_name {to_name + '_' + std::to_string(target_number)};
  const auto from_frame_name {from_name + '_' + std::to_string(target_number)};

  ros::Publisher pub {n.advertise<geometry_msgs::Point>(to_frame_name + "_direction", 1)};
  ros::Rate r {5};
  tf2_ros::Buffer tfBuffer {};
  tf2_ros::TransformListener tfListener {tfBuffer};

  while (ros::ok()) {
    try {
      const auto to_pos {tf2::transformToEigen(tfBuffer.lookupTransform(root_name, to_frame_name, ros::Time{0}))};
      const auto from_pos {tf2::transformToEigen(tfBuffer.lookupTransform(root_name, from_frame_name, ros::Time{0}))};
      const auto from_ypr {from_pos.rotation().eulerAngles(1, 0, 2)};
      const auto yaw_angle {from_ypr(0)};
      const Eigen::AngleAxisd yaw_inverse_rotation {-yaw_angle, Eigen::Vector3d::UnitY()};
      const auto stand_vec {to_pos.translation() - from_pos.translation()};
      const auto current_vec {yaw_inverse_rotation * stand_vec};

      pub.publish(tf2::toMsg(current_vec));
    } catch (tf2::TransformException &e) {
      ROS_WARN("%s", e.what());
    }

    r.sleep();
  }
}


#include "Cos.hh"
#include "TypesFunctions.hh"
#include <Eigen/Core>

/**
 * @brief Constructor.
 */
Cos::Cos() {
    transformation_("cos")
        .input("points")
	.output("result")
	.types(TypesFunctions::ifPoints<0>, TypesFunctions::pass<0>)
	.func(&Cos::calculate)
      ;
}

Cos::Cos(OutputDescriptor& output) : Cos() {
    transformations.front().inputs.front().connect(output);
}

/**
 * @brief Calculate the value of function.
 */
void Cos::calculate(FunctionArgs& fargs){
    fargs.rets[0].x = fargs.args[0].x.cos();
}