/*
 * test_infostat.cpp
 *
 *  Created on: 29 mai 2016
 *      Author: boubad
 */
#include <boost/test/unit_test.hpp>
/////////////////////////
#include <infostat.h>
/////////////////////////////
#include <mytestfixture.h>
////////////////////////////////////
#include <global_defs.h>
////////////////////
using namespace info;
using namespace std;
///////////////////////////////////
using MyFixture = MyTestFixture<IDTYPE, INTTYPE, STRINGTYPE, WEIGHTYPE>;
/////////////////////////////////////
using IndivType = typename MyFixture::IndivType;
using DataMap = typename MyFixture::DataMap;
using IndivTypePtr = typename MyFixture::IndivTypePtr;
using SourceType = typename MyFixture::SourceType;
////////////////////////////////
using StatInfoType = StatInfo<IDTYPE, STRINGTYPE>;
using StatSummatorType = StatSummator<IDTYPE,STRINGTYPE>;
using ints_vector = std::vector<IDTYPE>;
/////////////////////////////////////
BOOST_FIXTURE_TEST_SUITE(StatInfoTestSuite,MyFixture)
BOOST_AUTO_TEST_CASE(testMortalStatInfo) {
	SourceType *pProvider = this->mortal_source();
	BOOST_CHECK(pProvider != nullptr);
	//
	StatSummatorType oStat;
	pProvider->reset();
	size_t nTotal = 0;
	do {
		IndivTypePtr oInd = pProvider->next();
		if (oInd.get() == nullptr) {
			break;
		}
		oStat.add(oInd);
		++nTotal;
	} while (true);
	ints_vector keys;
	oStat.get_keys(keys);
	size_t nc = keys.size();
	BOOST_CHECK(nc > 0);
	const double epsilon = 0.000001;
	for (auto &key : keys) {
		bool bFound = oStat.has_key(key);
		BOOST_CHECK(bFound);
		StatInfoType oInfo;
		bool bRet = oStat.get(key, oInfo);
		BOOST_CHECK(bRet);
		size_t nz = oInfo.get_count();
		BOOST_CHECK(nz > 0);
		double vmin = oInfo.get_min();
		double vmax = oInfo.get_max();
		BOOST_CHECK(vmin < vmax);
		double vmean = oInfo.get_mean();
		BOOST_CHECK(vmean > vmin);
		BOOST_CHECK(vmean < vmax);
		double vcov = oInfo.get_variance();
		BOOST_CHECK(vcov > 0);
		double vstd = oInfo.get_deviation();
		BOOST_CHECK(vstd > 0);
		for (auto &key1 : keys) {
			double xxcov = 0;
			bRet = oStat.get_covariance(key, key1, xxcov);
			BOOST_CHECK(bRet);
			BOOST_CHECK(xxcov != 0);
			double xxcor = -5.0;
			bRet = oStat.get_correlation(key, key1, xxcor);
			BOOST_CHECK(bRet);
			double x = std::abs(xxcor);
			BOOST_CHECK(x <= (1.0 + epsilon));
		} // keye1
	} // key
} //testMortalMatElem

BOOST_AUTO_TEST_SUITE_END();


// standard libraries
#include <assert.h>
#include <Eigen/Dense>
#include <cmath>
#include <cstring>
#include <experimental/filesystem>
#include <fstream>
#include <iostream>
#include <iterator>
#include <sstream>
#include <thread>
#include <vector>

// Open3D
#include <Open3D/Geometry/KDTreeFlann.h>
#include <Open3D/Geometry/PointCloud.h>
#include <Open3D/IO/ClassIO/PointCloudIO.h>

// OMPL
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/config.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>

// TinyPly
#define TINYPLY_IMPLEMENTATION
#include "tinyply.h"

// flightlib
#include "flightlib/bridges/unity_bridge.hpp"
#include "flightlib/bridges/unity_message_types.hpp"
#include "flightlib/common/quad_state.hpp"
#include "flightlib/common/types.hpp"
#include "flightlib/objects/quadrotor.hpp"
#include "flightlib/sensors/rgb_camera.hpp"

namespace ob = ompl::base;
namespace og = ompl::geometric;

using namespace flightlib;

namespace motion_planning {

struct float3 {
  float x, y, z;
};

struct Bounds {
  float3 min;
  float3 max;
};

class MotionPlanner {
 public:
  MotionPlanner();
  ~MotionPlanner();
  void run();
  void readPointCloud();
  void getBounds();
  bool plan();
  void executePath();

 private:
  std::vector<ompl::base::State *> path_;
  std::vector<Eigen::Vector3d> vecs_;
  std::vector<float3> verts_;
  open3d::geometry::KDTreeFlann kd_tree_;
  Eigen::MatrixXd points_;
  Bounds bounds_;

  // unity
  SceneID scene_id_{UnityScene::NATUREFOREST};

  // ompl methods
  bool searchRadius(const Eigen::Vector3d &query_point, const double radius);
  Eigen::Vector3d stateToEigen(const ompl::base::State *state);
  bool isStateValid(const ob::State *state);
};

}  // namespace motion_planning


//=======================================================================
// Copyright (C) 2005 Jong Soo Park <jongsoo.park -at- gmail.com>
//
// 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)
//=======================================================================
#include <boost/test/minimal.hpp>
#include <iostream>
#include <algorithm>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dominator_tree.hpp>

using namespace std;

struct DominatorCorrectnessTestSet
{
  typedef pair<int, int> edge;

  int numOfVertices;
  vector<edge> edges;
  vector<int> correctIdoms;
};

using namespace boost;

typedef adjacency_list<
    listS,
    listS,
    bidirectionalS,
    property<vertex_index_t, std::size_t>, no_property> G;

int test_main(int, char*[])
{
  typedef DominatorCorrectnessTestSet::edge edge;

  DominatorCorrectnessTestSet testSet[7];

  // Tarjan's paper
  testSet[0].numOfVertices = 13;
  testSet[0].edges.push_back(edge(0, 1));
  testSet[0].edges.push_back(edge(0, 2));
  testSet[0].edges.push_back(edge(0, 3));
  testSet[0].edges.push_back(edge(1, 4));
  testSet[0].edges.push_back(edge(2, 1));
  testSet[0].edges.push_back(edge(2, 4));
  testSet[0].edges.push_back(edge(2, 5));
  testSet[0].edges.push_back(edge(3, 6));
  testSet[0].edges.push_back(edge(3, 7));
  testSet[0].edges.push_back(edge(4, 12));
  testSet[0].edges.push_back(edge(5, 8));
  testSet[0].edges.push_back(edge(6, 9));
  testSet[0].edges.push_back(edge(7, 9));
  testSet[0].edges.push_back(edge(7, 10));
  testSet[0].edges.push_back(edge(8, 5));
  testSet[0].edges.push_back(edge(8, 11));
  testSet[0].edges.push_back(edge(9, 11));
  testSet[0].edges.push_back(edge(10, 9));
  testSet[0].edges.push_back(edge(11, 0));
  testSet[0].edges.push_back(edge(11, 9));
  testSet[0].edges.push_back(edge(12, 8));
  testSet[0].correctIdoms.push_back((numeric_limits<int>::max)());
  testSet[0].correctIdoms.push_back(0);
  testSet[0].correctIdoms.push_back(0);
  testSet[0].correctIdoms.push_back(0);
  testSet[0].correctIdoms.push_back(0);
  testSet[0].correctIdoms.push_back(0);
  testSet[0].correctIdoms.push_back(3);
  testSet[0].correctIdoms.push_back(3);
  testSet[0].correctIdoms.push_back(0);
  testSet[0].correctIdoms.push_back(0);
  testSet[0].correctIdoms.push_back(7);
  testSet[0].correctIdoms.push_back(0);
  testSet[0].correctIdoms.push_back(4);

  // Appel. p441. figure 19.4
  testSet[1].numOfVertices = 7;
  testSet[1].edges.push_back(edge(0, 1));
  testSet[1].edges.push_back(edge(1, 2));
  testSet[1].edges.push_back(edge(1, 3));
  testSet[1].edges.push_back(edge(2, 4));
  testSet[1].edges.push_back(edge(2, 5));
  testSet[1].edges.push_back(edge(4, 6));
  testSet[1].edges.push_back(edge(5, 6));
  testSet[1].edges.push_back(edge(6, 1));
  testSet[1].correctIdoms.push_back((numeric_limits<int>::max)());
  testSet[1].correctIdoms.push_back(0);
  testSet[1].correctIdoms.push_back(1);
  testSet[1].correctIdoms.push_back(1);
  testSet[1].correctIdoms.push_back(2);
  testSet[1].correctIdoms.push_back(2);
  testSet[1].correctIdoms.push_back(2);

  // Appel. p449. figure 19.8
  testSet[2].numOfVertices = 13,
  testSet[2].edges.push_back(edge(0, 1));
  testSet[2].edges.push_back(edge(0, 2));
  testSet[2].edges.push_back(edge(1, 3));
  testSet[2].edges.push_back(edge(1, 6));
  testSet[2].edges.push_back(edge(2, 4));
  testSet[2].edges.push_back(edge(2, 7));
  testSet[2].edges.push_back(edge(3, 5));
  testSet[2].edges.push_back(edge(3, 6));
  testSet[2].edges.push_back(edge(4, 7));
  testSet[2].edges.push_back(edge(4, 2));
  testSet[2].edges.push_back(edge(5, 8));
  testSet[2].edges.push_back(edge(5, 10));
  testSet[2].edges.push_back(edge(6, 9));
  testSet[2].edges.push_back(edge(7, 12));
  testSet[2].edges.push_back(edge(8, 11));
  testSet[2].edges.push_back(edge(9, 8));
  testSet[2].edges.push_back(edge(10, 11));
  testSet[2].edges.push_back(edge(11, 1));
  testSet[2].edges.push_back(edge(11, 12));
  testSet[2].correctIdoms.push_back((numeric_limits<int>::max)());
  testSet[2].correctIdoms.push_back(0);
  testSet[2].correctIdoms.push_back(0);
  testSet[2].correctIdoms.push_back(1);
  testSet[2].correctIdoms.push_back(2);
  testSet[2].correctIdoms.push_back(3);
  testSet[2].correctIdoms.push_back(1);
  testSet[2].correctIdoms.push_back(2);
  testSet[2].correctIdoms.push_back(1);
  testSet[2].correctIdoms.push_back(6);
  testSet[2].correctIdoms.push_back(5);
  testSet[2].correctIdoms.push_back(1);
  testSet[2].correctIdoms.push_back(0);

  testSet[3].numOfVertices = 8,
  testSet[3].edges.push_back(edge(0, 1));
  testSet[3].edges.push_back(edge(1, 2));
  testSet[3].edges.push_back(edge(1, 3));
  testSet[3].edges.push_back(edge(2, 7));
  testSet[3].edges.push_back(edge(3, 4));
  testSet[3].edges.push_back(edge(4, 5));
  testSet[3].edges.push_back(edge(4, 6));
  testSet[3].edges.push_back(edge(5, 7));
  testSet[3].edges.push_back(edge(6, 4));
  testSet[3].correctIdoms.push_back((numeric_limits<int>::max)());
  testSet[3].correctIdoms.push_back(0);
  testSet[3].correctIdoms.push_back(1);
  testSet[3].correctIdoms.push_back(1);
  testSet[3].correctIdoms.push_back(3);
  testSet[3].correctIdoms.push_back(4);
  testSet[3].correctIdoms.push_back(4);
  testSet[3].correctIdoms.push_back(1);

  // Muchnick. p256. figure 8.21
  testSet[4].numOfVertices = 8,
  testSet[4].edges.push_back(edge(0, 1));
  testSet[4].edges.push_back(edge(1, 2));
  testSet[4].edges.push_back(edge(2, 3));
  testSet[4].edges.push_back(edge(2, 4));
  testSet[4].edges.push_back(edge(3, 2));
  testSet[4].edges.push_back(edge(4, 5));
  testSet[4].edges.push_back(edge(4, 6));
  testSet[4].edges.push_back(edge(5, 7));
  testSet[4].edges.push_back(edge(6, 7));
  testSet[4].correctIdoms.push_back((numeric_limits<int>::max)());
  testSet[4].correctIdoms.push_back(0);
  testSet[4].correctIdoms.push_back(1);
  testSet[4].correctIdoms.push_back(2);
  testSet[4].correctIdoms.push_back(2);
  testSet[4].correctIdoms.push_back(4);
  testSet[4].correctIdoms.push_back(4);
  testSet[4].correctIdoms.push_back(4);

  // Muchnick. p253. figure 8.18
  testSet[5].numOfVertices = 8,
  testSet[5].edges.push_back(edge(0, 1));
  testSet[5].edges.push_back(edge(0, 2));
  testSet[5].edges.push_back(edge(1, 6));
  testSet[5].edges.push_back(edge(2, 3));
  testSet[5].edges.push_back(edge(2, 4));
  testSet[5].edges.push_back(edge(3, 7));
  testSet[5].edges.push_back(edge(5, 7));
  testSet[5].edges.push_back(edge(6, 7));
  testSet[5].correctIdoms.push_back((numeric_limits<int>::max)());
  testSet[5].correctIdoms.push_back(0);
  testSet[5].correctIdoms.push_back(0);
  testSet[5].correctIdoms.push_back(2);
  testSet[5].correctIdoms.push_back(2);
  testSet[5].correctIdoms.push_back((numeric_limits<int>::max)());
  testSet[5].correctIdoms.push_back(1);
  testSet[5].correctIdoms.push_back(0);

  // Cytron's paper, fig. 9
  testSet[6].numOfVertices = 14,
  testSet[6].edges.push_back(edge(0, 1));
  testSet[6].edges.push_back(edge(0, 13));
  testSet[6].edges.push_back(edge(1, 2));
  testSet[6].edges.push_back(edge(2, 3));
  testSet[6].edges.push_back(edge(2, 7));
  testSet[6].edges.push_back(edge(3, 4));
  testSet[6].edges.push_back(edge(3, 5));
  testSet[6].edges.push_back(edge(4, 6));
  testSet[6].edges.push_back(edge(5, 6));
  testSet[6].edges.push_back(edge(6, 8));
  testSet[6].edges.push_back(edge(7, 8));
  testSet[6].edges.push_back(edge(8, 9));
  testSet[6].edges.push_back(edge(9, 10));
  testSet[6].edges.push_back(edge(9, 11));
  testSet[6].edges.push_back(edge(10, 11));
  testSet[6].edges.push_back(edge(11, 9));
  testSet[6].edges.push_back(edge(11, 12));
  testSet[6].edges.push_back(edge(12, 2));
  testSet[6].edges.push_back(edge(12, 13));
  testSet[6].correctIdoms.push_back((numeric_limits<int>::max)());
  testSet[6].correctIdoms.push_back(0);
  testSet[6].correctIdoms.push_back(1);
  testSet[6].correctIdoms.push_back(2);
  testSet[6].correctIdoms.push_back(3);
  testSet[6].correctIdoms.push_back(3);
  testSet[6].correctIdoms.push_back(3);
  testSet[6].correctIdoms.push_back(2);
  testSet[6].correctIdoms.push_back(2);
  testSet[6].correctIdoms.push_back(8);
  testSet[6].correctIdoms.push_back(9);
  testSet[6].correctIdoms.push_back(9);
  testSet[6].correctIdoms.push_back(11);
  testSet[6].correctIdoms.push_back(0);

  for (size_t i = 0; i < sizeof(testSet)/sizeof(testSet[0]); ++i)
  {
    const int numOfVertices = testSet[i].numOfVertices;

    G g(
      testSet[i].edges.begin(), testSet[i].edges.end(),
      numOfVertices);

    typedef graph_traits<G>::vertex_descriptor Vertex;
    typedef property_map<G, vertex_index_t>::type IndexMap;
    typedef
      iterator_property_map<vector<Vertex>::iterator, IndexMap>
      PredMap;

    vector<Vertex> domTreePredVector, domTreePredVector2;
    IndexMap indexMap(get(vertex_index, g));
    graph_traits<G>::vertex_iterator uItr, uEnd;
    int j = 0;
    for (tie(uItr, uEnd) = vertices(g); uItr != uEnd; ++uItr, ++j)
    {
      put(indexMap, *uItr, j);
    }

    // Lengauer-Tarjan dominator tree algorithm
    domTreePredVector =
      vector<Vertex>(num_vertices(g), graph_traits<G>::null_vertex());
    PredMap domTreePredMap =
      make_iterator_property_map(domTreePredVector.begin(), indexMap);

    lengauer_tarjan_dominator_tree(g, vertex(0, g), domTreePredMap);

    vector<int> idom(num_vertices(g));
    for (tie(uItr, uEnd) = vertices(g); uItr != uEnd; ++uItr)
    {
      if (get(domTreePredMap, *uItr) != graph_traits<G>::null_vertex())
        idom[get(indexMap, *uItr)] =
          get(indexMap, get(domTreePredMap, *uItr));
      else
        idom[get(indexMap, *uItr)] = (numeric_limits<int>::max)();
    }

    copy(idom.begin(), idom.end(), ostream_iterator<int>(cout, " "));
    cout << endl;

    // dominator tree correctness test
    BOOST_CHECK(equal(idom.begin(), idom.end(), testSet[i].correctIdoms.begin()));

    // compare results of fast version and slow version of dominator tree
    domTreePredVector2 =
      vector<Vertex>(num_vertices(g), graph_traits<G>::null_vertex());
    domTreePredMap =
      make_iterator_property_map(domTreePredVector2.begin(), indexMap);

    iterative_bit_vector_dominator_tree(g, vertex(0, g), domTreePredMap);

    vector<int> idom2(num_vertices(g));
    for (tie(uItr, uEnd) = vertices(g); uItr != uEnd; ++uItr)
    {
      if (get(domTreePredMap, *uItr) != graph_traits<G>::null_vertex())
        idom2[get(indexMap, *uItr)] =
          get(indexMap, get(domTreePredMap, *uItr));
      else
        idom2[get(indexMap, *uItr)] = (numeric_limits<int>::max)();
    }

    copy(idom2.begin(), idom2.end(), ostream_iterator<int>(cout, " "));
    cout << endl;

    size_t k;
    for (k = 0; k < num_vertices(g); ++k)
      BOOST_CHECK(domTreePredVector[k] == domTreePredVector2[k]);
  }

  return 0;
}


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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)

/**
\file

\brief composite_output.cpp

\details An example of textual representations of units.

Output:
@verbatim

//[conversion_output_output
2 dyn
2 dyn
2 dyne
cm g s^-1
centimeter gram second^-1
dyn
dyne
n
nano
n
nano
F
farad
1 F
1 farad
nF
nanofarad
1 nF
1 nanofarad
n(cm g s^-1)
nano(centimeter gram second^-1)
//]

@endverbatim
**/
#include <boost/units/quantity.hpp>
#include <boost/units/systems/cgs.hpp>
#include <boost/units/io.hpp>
#include <boost/units/scale.hpp>

#include <boost/units/detail/utility.hpp>

#include <boost/units/systems/si/capacitance.hpp>
#include <boost/units/systems/si/io.hpp>
#include <boost/units/systems/si/prefixes.hpp>

#include <iostream>
#include <sstream>

namespace boost {

namespace units {

//[composite_output_snippet_1

std::string name_string(const cgs::force&)
{
    return "dyne";
}

std::string symbol_string(const cgs::force&)
{
    return "dyn";
}

//]

}

}

int main()
{
    using namespace boost::units;
    using boost::units::cgs::centimeter;
    using boost::units::cgs::gram;
    using boost::units::cgs::second;
    using boost::units::cgs::dyne;

    //[composite_output_snippet_2]
    std::cout << 2.0 * dyne << std::endl
              << symbol_format << 2.0 * dyne << std::endl
              << name_format << 2.0 * dyne << std::endl
              << symbol_format << gram*centimeter/second << std::endl
              << name_format << gram*centimeter/second << std::endl
              << symbol_format << gram*centimeter/(second*second) << std::endl
              << name_format << gram*centimeter/(second*second) << std::endl
              << symbol_string(scale<10,static_rational<-9> >()) << std::endl
              << name_string(scale<10,static_rational<-9> >()) << std::endl
              << symbol_format << si::nano << std::endl
              << name_format << si::nano << std::endl
              << symbol_format << si::farad << std::endl
              << name_format << si::farad << std::endl
              << symbol_format << 1.0*si::farad << std::endl
              << name_format << 1.0*si::farad << std::endl
              << symbol_format << si::farad*si::nano << std::endl
              << name_format << si::farad*si::nano << std::endl
              << symbol_format << 1.0*si::farad*si::nano << std::endl
              << name_format << 1.0*si::farad*si::nano << std::endl
              << symbol_format << si::nano*gram*centimeter/second << std::endl
              << name_format << si::nano*gram*centimeter/second << std::endl;
    //]

    return 0;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestEqualRange
#include <boost/test/unit_test.hpp>

#include <utility>
#include <iterator>

#include <boost/compute/command_queue.hpp>
#include <boost/compute/algorithm/equal_range.hpp>
#include <boost/compute/container/vector.hpp>

#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(equal_range_int)
{
    int data[] = { 1, 2, 2, 2, 3, 3, 4, 5 };
    boost::compute::vector<int> vector(data, data + 8);

    typedef boost::compute::vector<int>::iterator iterator;

    std::pair<iterator, iterator> range0 =
        boost::compute::equal_range(vector.begin(), vector.end(), int(0));
    BOOST_CHECK(range0.first == vector.begin());
    BOOST_CHECK(range0.second == vector.begin());
    BOOST_CHECK_EQUAL(std::distance(range0.first, range0.second), ptrdiff_t(0));

    std::pair<iterator, iterator> range1 =
        boost::compute::equal_range(vector.begin(), vector.end(), int(1));
    BOOST_CHECK(range1.first == vector.begin());
    BOOST_CHECK(range1.second == vector.begin() + 1);
    BOOST_CHECK_EQUAL(std::distance(range1.first, range1.second), ptrdiff_t(1));

    std::pair<iterator, iterator> range2 =
        boost::compute::equal_range(vector.begin(), vector.end(), int(2));
    BOOST_CHECK(range2.first == vector.begin() + 1);
    BOOST_CHECK(range2.second == vector.begin() + 4);
    BOOST_CHECK_EQUAL(std::distance(range2.first, range2.second), ptrdiff_t(3));

    std::pair<iterator, iterator> range3 =
        boost::compute::equal_range(vector.begin(), vector.end(), int(3));
    BOOST_CHECK(range3.first == vector.begin() + 4);
    BOOST_CHECK(range3.second == vector.begin() + 6);
    BOOST_CHECK_EQUAL(std::distance(range3.first, range3.second), ptrdiff_t(2));

    std::pair<iterator, iterator> range4 =
        boost::compute::equal_range(vector.begin(), vector.end(), int(4));
    BOOST_CHECK(range4.first == vector.begin() + 6);
    BOOST_CHECK(range4.second == vector.begin() + 7);
    BOOST_CHECK_EQUAL(std::distance(range4.first, range4.second), ptrdiff_t(1));

    std::pair<iterator, iterator> range5 =
        boost::compute::equal_range(vector.begin(), vector.end(), int(5));
    BOOST_CHECK(range5.first == vector.begin() + 7);
    BOOST_CHECK(range5.second == vector.end());
    BOOST_CHECK_EQUAL(std::distance(range5.first, range5.second), ptrdiff_t(1));

    std::pair<iterator, iterator> range6 =
        boost::compute::equal_range(vector.begin(), vector.end(), int(6));
    BOOST_CHECK(range6.first == vector.end());
    BOOST_CHECK(range6.second == vector.end());
    BOOST_CHECK_EQUAL(std::distance(range6.first, range6.second), ptrdiff_t(0));
}

BOOST_AUTO_TEST_SUITE_END()


/*
Copyright (c) Facebook, Inc. and its affiliates.

This source code is licensed under the MIT license found in the
LICENSE file in the root directory of this source tree.
*/

// Without this flag, GTSAM Expressions currently fail within fbcode.
// We need EIGEN_MAKE_ALIGNED_OPERATOR_NEW in GTSAM to really fix this
#define EIGEN_DONT_ALIGN_STATICALLY

#include <boost/filesystem.hpp>

#include "Mesh.h"

#include <iostream>
#include <math.h>

namespace sumo {

using namespace std;

Mesh::Mesh(
    const vector<Index>& indices,
    const vector<Vertex>& vertices,
    const vector<Normal>& normals)
    : indices_(indices), vertices_(vertices), normals_(normals) {
  assert(vertices.size() == normals.size());
}

vector<Mesh::Normal> Mesh::CalculateFaceNormals(
    const vector<Index>& indices,
    const vector<Vertex>& vertices) {
  vector<Normal> faceNormals;
  const size_t numTriangles = indices.size() / 3;
  faceNormals.reserve(numTriangles);
  // Compute face normals using cross product method.
  for (size_t i = 0; i < numTriangles; i++) {
    // get three triangle vertex indices
    const size_t j = i * 3;
    const Index i0 = indices[j], i1 = indices[j + 1], i2 = indices[j + 2];
    Vertex a = vertices[i1] - vertices[i0];
    Vertex b = vertices[i2] - vertices[i0];

    Normal n = a.cross(b).normalized();
    faceNormals.push_back(n);
  }
  return faceNormals;
}

vector<Mesh::Normal> Mesh::EstimateNormals(
    const vector<Index>& indices,
    const vector<Vertex>& vertices) {
  vector<Normal> faceNormals = CalculateFaceNormals(indices, vertices);

  // Initialize normals to zero.
  size_t numVertices = vertices.size();
  vector<Mesh::Normal> normals(numVertices, Normal::Zero());

  // Add face normals to all involved vertices.
  // TODO use iterator
  size_t j = 0;
  for (const auto& n : faceNormals) {
    for (Index i : {indices[j], indices[j + 1], indices[j + 2]}) {
      assert(i < numVertices);
      normals[i] += n;
    }
    j += 3;
  }

  for (auto& n : normals) {
    n.normalize();
  }

  return normals;
}

void Mesh::cleanupLongEdges(float threshold) {
  vector<Index> new_indices;
  new_indices.reserve(numIndices());
  // define distance function
  const auto dist = [](Vertex x, Vertex y) { return (x - y).norm(); };
  // loop over all triangles in this mesh
  for (const auto& triangle : *this) {
    // check all edges
    if (dist(triangle.a, triangle.b) <= threshold &&
        dist(triangle.b, triangle.c) <= threshold &&
        dist(triangle.c, triangle.a) <= threshold) {
      // add triangle since valid
      new_indices.push_back(triangle.i);
      new_indices.push_back(triangle.j);
      new_indices.push_back(triangle.k);
    }
  }
  indices_ = new_indices;
}

void Mesh::cleanupEdgesToOrigin(const double precision) {
  vector<Index> new_indices;
  new_indices.reserve(numIndices());
  // loop over all triangles in this mesh
  for (const auto& triangle : *this) {
    // If any vertex is zero, we bail.
    if (triangle.a.isZero(precision) || triangle.b.isZero(precision) ||
        triangle.c.isZero(precision)) {
      continue;
    }
    // add triangle since valid
    new_indices.push_back(triangle.i);
    new_indices.push_back(triangle.j);
    new_indices.push_back(triangle.k);
  }
  indices_ = new_indices;
}

Mesh Mesh::Example(
    const double length /* = 2.0*/,
    const bool inward /* = true */) {
  vector<Index> indices;
  indices.insert(indices.end(), {1, 0, 2}); // Left face   (x = -1.0f)
  indices.insert(indices.end(), {2, 3, 1}); // Left face   (x = -1.0f)
  indices.insert(indices.end(), {4, 0, 1}); // Bottom face (y = -1.0f)
  indices.insert(indices.end(), {1, 5, 4}); // Bottom face (y = -1.0f)
  indices.insert(indices.end(), {2, 0, 4}); // Back face   (z = -1.0f)
  indices.insert(indices.end(), {4, 6, 2}); // Back face   (z = -1.0f)
  indices.insert(indices.end(), {5, 7, 6}); // Right face  (x = +1.0f)
  indices.insert(indices.end(), {6, 4, 5}); // Right face  (x = +1.0f)
  indices.insert(indices.end(), {6, 7, 3}); // Top face    (y = +1.0f)
  indices.insert(indices.end(), {3, 2, 6}); // Top face    (y = +1.0f)
  indices.insert(indices.end(), {3, 7, 5}); // Front face  (z = +1.0f)
  indices.insert(indices.end(), {5, 1, 3}); // Front face  (z = +1.0f)
  if (inward == false) {
    reverse(indices.begin(), indices.end());
  }

  vector<Vertex> vertices;
  const double radius = length / 2;
  vertices.push_back(Vertex(-1.0f, -1.0f, -1.0f) * radius); // 0th
  vertices.push_back(Vertex(-1.0f, -1.0f, +1.0f) * radius); // 1st
  vertices.push_back(Vertex(-1.0f, +1.0f, -1.0f) * radius); // 2nd
  vertices.push_back(Vertex(-1.0f, +1.0f, +1.0f) * radius); // 3rd
  vertices.push_back(Vertex(+1.0f, -1.0f, -1.0f) * radius); // 4th
  vertices.push_back(Vertex(+1.0f, -1.0f, +1.0f) * radius); // 5th
  vertices.push_back(Vertex(+1.0f, +1.0f, -1.0f) * radius); // 6th
  vertices.push_back(Vertex(+1.0f, +1.0f, +1.0f) * radius); // 7th

  return Mesh(indices, vertices, Mesh::EstimateNormals(indices, vertices));
}

void Mesh::merge(const Mesh& mesh2, size_t numCommonVertices) {
  // Merge the indices.
  const size_t offset = numVertices() - numCommonVertices;
  for (const size_t& index2 : mesh2.indices()) {
    indices_.push_back(index2 < numCommonVertices ? index2 : index2 + offset);
  }

  // Treat the case when mesh2 defines new vertices.
  if (mesh2.numVertices() > numCommonVertices) {
    const auto append = [numCommonVertices](auto& a, const auto& b) {
      a.insert(a.end(), b.begin() + numCommonVertices, b.end());
    };
    append(vertices_, mesh2.vertices());
    append(normals_, mesh2.normals());
  }
}

void Mesh::replaceGeometry(const Mesh& mesh2) {
  //Replace the geometry of the mesh.
  indices_.clear();
  vertices_.clear();
  normals_.clear();

  merge(mesh2, 0);

}

  /// Operator == for Parameter
  // ::: TODO: This function is not correct.  It does not check all aspects
  // of the parameter struct.  Furthermore, some aspects of the Parameter
  // struct are unstable (that is, they are not initialized to known values,
  // so comparing two Parameter objects is not really feasible with the
  // tiny_gltf design.
static bool parametersEqual(
    const tinygltf::Parameter& a,
    const tinygltf::Parameter& b) {
  return
    a.has_number_value == b.has_number_value &&
    a.string_value == b.string_value &&
    a.json_double_value == b.json_double_value;
}

/// Operator == for Parameter
static bool namedParametersEqual(
    const tinygltf::ParameterMap::value_type& a,
    const tinygltf::ParameterMap::value_type& b) {
  return a.first == b.first && parametersEqual(a.second, b.second);
}

/// Operator == for ParameterMap
bool operator==(
    const tinygltf::ParameterMap& a,
    const tinygltf::ParameterMap& b) {
  return a.size() == b.size() &&
      std::equal(a.begin(), a.end(), b.begin(), namedParametersEqual);
}

/// Operator == for Material
// Only comparing ParameterMaps and name.
// TODO: Compare extensions and extras.
bool operator==(const tinygltf::Material& a, const tinygltf::Material& b) {
  return a.additionalValues == b.additionalValues && a.values == b.values &&
      a.name == b.name;
}

bool Mesh::hasSameMaterial(const Mesh& other) const {
  return material_ == other.material_;
}

} // namespace sumo


///////////////////////////////////////////////////////////////////////////////
// mpl::example::rest.cpp                              					 	 //
//                                                                           //
//  Copyright 2008 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#include <iostream>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/statistics/detail/mpl/rest.hpp>
#include <boost/statistics/detail/mpl/most.hpp>
#include <libs/statistics/detail/range_ex/example/rest.h>

void example_nested_chain(std::cout);

	typedef boost::mpl::range_c<int,0,3> vec_;
	namespace mpl = boost::statistics::detail::mpl;

	typedef mpl::rest<vec_> meta_rest_;
    typedef meta_rest_::type rest_;
	typedef mpl::most<vec_> meta_most_;
    typedef meta_most_::type most_;

    BOOST_MPL_ASSERT((
		boost::mpl::equal<
        	boost::mpl::range_c<int,1,3>,
            rest_
        >
    ));
    BOOST_MPL_ASSERT((
		boost::mpl::equal<
        	boost::mpl::range_c<int,0,2>,
            most_
        >
    ));

}


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

#include <helib.h>

#include <intraSlot.h>
#include <binaryArith.h>

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

#ifdef DEBUG_PRINTOUT
#include <debugging.h>
#endif
// define flags FLAG_PRINT_ZZX, FLAG_PRINT_POLY, FLAG_PRINT_VEC, functions
//        decryptAndPrint(ostream, ctxt, sk, ea, flags)
//        decryptAndCompare(ctxt, sk, ea, pa);

namespace {

struct Parameters
{
  Parameters(long prm,
             long bitSize,
             long bitSize2,
             long outSize,
             bool bootstrap,
             long seed,
             long nthreads) :
      prm(prm),
      bitSize(bitSize),
      bitSize2(bitSize2),
      outSize(outSize),
      bootstrap(bootstrap),
      seed(seed),
      nthreads(nthreads){};

  long prm;       // parameter size (0-tiny,...,7-huge)
  long bitSize;   // bitSize of input integers (<=32)
  long bitSize2;  // bitSize of 2nd input integer (<=32)
  long outSize;   // bitSize of output integers, as many as needed
  bool bootstrap; // test multiplication with bootstrapping
  long seed;      // PRG seed
  long nthreads;  // number of threads

  friend std::ostream& operator<<(std::ostream& os, const Parameters& params)
  {
    return os << "{"
              << "prm=" << params.prm << ","
              << "bitSize=" << params.bitSize << ","
              << "bitSize2=" << params.bitSize2 << ","
              << "outSize=" << params.outSize << ","
              << "bootstrap=" << params.bootstrap << ","
              << "seed=" << params.seed << ","
              << "nthreads=" << params.nthreads << "}";
  };
};

class GTest_binaryArith :
    public ::testing::TestWithParam<std::tuple<Parameters, int>>
{
protected:
  static std::vector<helib::zzX> unpackSlotEncoding;
  constexpr static long mValues[8][15] = {
      // { p, phi(m),   m,   d, m1, m2, m3,    g1,   g2,   g3, ord1,ord2,ord3,
      // B,c}
      {2, 48, 105, 12, 3, 35, 0, 71, 76, 0, 2, 2, 0, 25, 2},
      {2, 600, 1023, 10, 11, 93, 0, 838, 584, 0, 10, 6, 0, 25, 2},
      {2, 2304, 4641, 24, 7, 3, 221, 3979, 3095, 3760, 6, 2, -8, 25, 3},
      {2, 5460, 8193, 26, 8193, 0, 0, 46, 0, 0, 210, 0, 0, 25, 3},
      {2, 8190, 8191, 13, 8191, 0, 0, 39, 0, 0, 630, 0, 0, 25, 3},
      {2, 10752, 11441, 48, 17, 673, 0, 4712, 2024, 0, 16, -14, 0, 25, 3},
      {2, 15004, 15709, 22, 23, 683, 0, 4099, 13663, 0, 22, 31, 0, 25, 3},
      {2,
       27000,
       32767,
       15,
       31,
       7,
       151,
       11628,
       28087,
       25824,
       30,
       6,
       -10,
       28,
       4}};

  static long correctBitSize(long minimum, long oldBitSize)
  {
    long newBitSize;
    if (oldBitSize <= 0)
      newBitSize = minimum;
    else if (oldBitSize > 32)
      newBitSize = 32;
    else
      newBitSize = oldBitSize;
    return newBitSize;
  };

  // Validates the prm value, throwing if invalid
  static long validatePrm(long prm)
  {
    if (prm < 0 || prm >= 5)
      throw std::invalid_argument("prm must be in the interval [0, 4]");
    return prm;
  };

  static NTL::Vec<long> calculateMvec(const long* vals)
  {
    NTL::Vec<long> mvec;
    NTL::append(mvec, vals[4]);
    if (vals[5] > 1)
      NTL::append(mvec, vals[5]);
    if (vals[6] > 1)
      NTL::append(mvec, vals[6]);
    return mvec;
  };

  static std::vector<long> calculateGens(const long* vals)
  {
    std::vector<long> gens;
    gens.push_back(vals[7]);
    if (vals[8] > 1)
      gens.push_back(vals[8]);
    if (vals[9] > 1)
      gens.push_back(vals[9]);
    return gens;
  };

  static std::vector<long> calculateOrds(const long* vals)
  {
    std::vector<long> ords;
    ords.push_back(vals[10]);
    if (abs(vals[11]) > 1)
      ords.push_back(vals[11]);
    if (abs(vals[12]) > 1)
      ords.push_back(vals[12]);
    return ords;
  };

  static long calculateLevels(bool bootstrap, long outSize, long bitSize)
  {
    long L;
    if (bootstrap)
      L = 900; // that should be enough
    else {
      double nBits =
          (outSize > 0 && outSize < 2 * bitSize) ? outSize : (2 * bitSize);
      double three4twoLvls = log(nBits / 2) / log(1.5);
      double add2NumsLvls = log(nBits) / log(2.0);
      L = (5 + ceil(three4twoLvls + add2NumsLvls)) * 30;
    }
    return L;
  };

  // Returns a reference to the passed-in context once it has been modified to
  // get it ready for test. This is not static because it uses quite a lot of
  // state of the object.
  helib::FHEcontext& prepareContext(helib::FHEcontext& context)
  {
    if (helib_test::verbose) {
      std::cout << "input bitSizes=" << bitSize << ',' << bitSize2
                << ", output size bound=" << outSize << std::endl;
      if (nthreads > 1)
        std::cout << "  using " << NTL::AvailableThreads() << " threads\n";
      std::cout << "computing key-independent tables..." << std::flush;
    }
    buildModChain(context, L, c, /*willBeBootstrappable=*/bootstrap);
    if (bootstrap) {
      context.makeBootstrappable(mvec, /*t=*/0);
    }
    buildUnpackSlotEncoding(unpackSlotEncoding, *context.ea);
    if (helib_test::verbose) {
      std::cout << " done.\n";
      context.zMStar.printout();
    }
    return context;
  };

  void prepareSecKey(helib::FHESecKey& secKey)
  {
    if (helib_test::verbose) {
      std::cout << " L=" << L << ", B=" << B << std::endl;
      std::cout << "\ncomputing key-dependent tables..." << std::flush;
    }
    secKey.GenSecKey();
    addSome1DMatrices(secKey); // compute key-switching matrices
    addFrbMatrices(secKey);
    if (bootstrap)
      secKey.genRecryptData();
    if (helib_test::verbose)
      std::cout << " done\n";
  };

  const long prm;
  const long bitSize;
  const long bitSize2;
  const long outSize;
  const bool bootstrap;
  const long seed;
  const long nthreads;

  const long* vals;
  const long p;
  const long m;
  const NTL::Vec<long> mvec;
  const std::vector<long> gens;
  const std::vector<long> ords;
  const long B;
  const long c;
  const long L;
  helib::FHEcontext context;
  helib::FHESecKey secKey;

  GTest_binaryArith() :
      prm(validatePrm(std::get<0>(GetParam()).prm)),
      bitSize(correctBitSize(5, std::get<0>(GetParam()).bitSize)),
      bitSize2(correctBitSize(bitSize, std::get<0>(GetParam()).bitSize2)),
      outSize(std::get<0>(GetParam()).outSize),
      bootstrap(std::get<0>(GetParam()).bootstrap),
      seed(std::get<0>(GetParam()).seed),
      nthreads(std::get<0>(GetParam()).nthreads),
      vals(mValues[prm]),
      p(vals[0]),
      m(vals[2]),
      mvec(calculateMvec(vals)),
      gens(calculateGens(vals)),
      ords(calculateOrds(vals)),
      B(vals[13]),
      c(vals[14]),
      L(calculateLevels(bootstrap, outSize, bitSize)),
      context(m, p, /*r=*/1, gens, ords),
      secKey(prepareContext(context)){};

  void SetUp() override
  {
    if (seed)
      NTL::SetSeed(NTL::ZZ(seed));
    if (nthreads > 1)
      NTL::SetNumThreads(nthreads);

    prepareSecKey(secKey);

    helib::activeContext = &context; // make things a little easier sometimes
#ifdef DEBUG_PRINTOUT
    helib::dbgEa = (helib::EncryptedArray*)context.ea;
    helib::dbgKey = &secKey;
#endif
  }

  virtual void TearDown() override
  {
#ifdef DEBUG_PRINTOUT
    helib::cleanupGlobals();
#endif
  }

  // This gets called once all of the tests have run
public:
  static void TearDownTestCase()
  {
    if (helib_test::verbose)
      helib::printAllTimers(std::cout);
  };
};
constexpr long GTest_binaryArith::mValues[8][15];
std::vector<helib::zzX> GTest_binaryArith::unpackSlotEncoding;

TEST_P(GTest_binaryArith, fifteenForFour)
{
  // Randomly generate up to 15 integers from {0,1} with some entries
  // randomly set to null.  We then encrypt and use the fifteenOrLess4Four
  // function to calculate the binary representation of their sum.  This is
  // then checked against the plaintext calculation.
  // In this case each ciphertext is considered to be the encryption of one
  // bit, however packing more into the slots is possible.
  // LSB is at the start (left) of the vector.

  // vector of ciphertexts corresponding to encrypted input bit vectors.
  std::vector<helib::Ctxt> inBuf(15, helib::Ctxt(secKey));
  std::vector<helib::Ctxt*> inPtrs(15, nullptr);

  // vector of ciphertexts corresponding to the summation of the input bit
  // vectors.
  std::vector<helib::Ctxt> outBuf(5, helib::Ctxt(secKey));

  // Randomly generate and encrypt the input vectors.
  long sum = 0;
  std::string inputBits = "(";
  for (int i = 0; i < 15; i++) {
    if (NTL::RandomBnd(10) > 0) { // Leave empty (null) with small probability.
      inPtrs[i] = &(inBuf[i]);
      long bit = NTL::RandomBnd(2); // Select a randomised bit.
      secKey.Encrypt(inBuf[i], NTL::ZZX(bit));
      inputBits += std::to_string(bit) + ",";
      sum += bit; // Keep track of the plaintext sum.
    } else
      inputBits += "-,"; // This represents a null bit.
  }
  inputBits += ")";

  if (helib_test::verbose) {
    std::cout << std::endl;
    helib::CheckCtxt(inBuf[helib::lsize(inBuf) - 1], "b4 15for4");
  }
  // Add the encrypted bits.
  long numOutputs = fifteenOrLess4Four(helib::CtPtrs_vectorCt(outBuf),
                                       helib::CtPtrs_vectorPt(inPtrs));
  if (helib_test::verbose)
    helib::CheckCtxt(outBuf[helib::lsize(outBuf) - 1], "after 15for4");

  // Check the result.
  long sum2 = 0;
  for (int i = 0; i < numOutputs; i++) {
    NTL::ZZX poly;
    secKey.Decrypt(poly, outBuf[i]);
    sum2 += to_long(ConstTerm(poly)) << i;
  }
  EXPECT_EQ(sum, sum2) << "inputs = " << inputBits << std::endl;
  if (helib_test::verbose) {
    std::cout << "15to4 succeeded, sum" << inputBits << "=" << sum2
              << std::endl;
  }
};

TEST_P(GTest_binaryArith, product)
{
  // Randomly generate a pair of numbers of a specified bit size and then
  // encrypt them in binary representation. Then use multTwoNumbers to
  // multiply the two positive binary numbers and then check against the
  // plaintext calculation. Next, use multTwoNumbers with binary numbers in
  // 2's complement to calculate the product where the multplier is negative
  // and then check against the plaintext calculation.
  //
  // In this case each ciphertext is considered to be the encryption of one
  // bit, however packing more into the slots is possible.
  // LSB is at the start (left) of the vector.

  const helib::EncryptedArray& ea = *context.ea;
  // outSize 1's on the least significant end of mask.
  long mask = (outSize ? ((1L << outSize) - 1) : -1);

  // Choose two random integers with correct bit sizes.
  long multiplicand_data = NTL::RandomBits_long(bitSize);
  long multiplier_data = NTL::RandomBits_long(bitSize2);

  // Encrypt the individual bits.
  NTL::Vec<helib::Ctxt> encrypted_product, encrypted_multiplicand,
      encrypted_multiplier;

  // Resizes the vector of ciphertexts (encrypted_bits) to match the input size.
  helib::resize(encrypted_multiplicand, bitSize, helib::Ctxt(secKey));
  for (long i = 0; i < bitSize; i++) {
    secKey.Encrypt(encrypted_multiplicand[i],
                   NTL::ZZX((multiplicand_data >> i) & 1));
    if (bootstrap) { // If bootstrapping then modulo down to a lower level.
      encrypted_multiplicand[i].bringToSet(context.getCtxtPrimes(5));
    }
  }
  helib::resize(encrypted_multiplier, bitSize2, helib::Ctxt(secKey));
  for (long i = 0; i < bitSize2; i++) {
    secKey.Encrypt(encrypted_multiplier[i],
                   NTL::ZZX((multiplier_data >> i) & 1));
    if (bootstrap) { // If bootstrapping then modulo down to a lower level.
      encrypted_multiplier[i].bringToSet(context.getCtxtPrimes(5));
    }
  }
  if (helib_test::verbose) {
    std::cout << "\n  bits-size " << bitSize << '+' << bitSize2;
    if (outSize > 0)
      std::cout << "->" << outSize;
    helib::CheckCtxt(encrypted_multiplier[0], "b4 multiplication");
  }
  std::vector<long> slots; // Vector that will hold the decrypted result.
  // Test multiplication with two positive numbers.
  // A scope which tests multTwoNumbers using wrappers around the encrypted
  // data.
  {
    helib::CtPtrs_VecCt output_wrapper(
        encrypted_product); // A wrapper around the output vector.
    helib::multTwoNumbers(output_wrapper,
                          helib::CtPtrs_VecCt(encrypted_multiplicand),
                          helib::CtPtrs_VecCt(encrypted_multiplier),
                          /*negative=*/false,
                          outSize,
                          &unpackSlotEncoding);
    helib::decryptBinaryNums(slots, output_wrapper, secKey, ea);
  } // output_wrapper is deleted once out of scope.
  if (helib_test::verbose)
    helib::CheckCtxt(encrypted_product[helib::lsize(encrypted_product) - 1],
                     "after multiplication");

  // Calculate the multiplication in the plain.
  long plaintext_product = multiplicand_data * multiplier_data;
  EXPECT_EQ(slots[0], ((multiplicand_data * multiplier_data) & mask))
      << "Positive product error: multiplicand_data=" << multiplicand_data
      << ", multiplier_data=" << multiplier_data << ", but product=" << slots[0]
      << " (should be " << plaintext_product << '&' << mask << '='
      << (plaintext_product & mask) << ")\n";

  if (helib_test::verbose) {
    std::cout << "positive product succeeded: ";
    if (outSize)
      std::cout << "bottom " << outSize << " bits of ";
    std::cout << multiplicand_data << "*" << multiplier_data << "=" << slots[0]
              << std::endl;
  }

  // Test multiplication of numbers in 2's complement where the multiplier is
  // negative.
  secKey.Encrypt(encrypted_multiplier[bitSize2 - 1], NTL::ZZX(1));
  decryptBinaryNums(slots,
                    helib::CtPtrs_VecCt(encrypted_multiplier),
                    secKey,
                    ea,
                    /*negative=*/true);
  multiplier_data = slots[0];
  encrypted_product.kill(); // Clear the data in encrypted_product.
  // A scope which tests multTwoNumbers (with a negative multiplier) using
  // wrappers around the encrypted data.
  {
    helib::CtPtrs_VecCt output_wrapper(
        encrypted_product); // A wrapper around the output vector.
    multTwoNumbers(output_wrapper,
                   helib::CtPtrs_VecCt(encrypted_multiplicand),
                   helib::CtPtrs_VecCt(encrypted_multiplier),
                   /*negative=*/true,
                   outSize,
                   &unpackSlotEncoding);
    decryptBinaryNums(slots, output_wrapper, secKey, ea, /*negative=*/true);
  } // output_wrapper is deleted once out of scope.

  if (helib_test::verbose)
    helib::CheckCtxt(encrypted_product[helib::lsize(encrypted_product) - 1],
                     "after multiplication");

  // Calculate the multiplication in the plain.
  plaintext_product = multiplicand_data * multiplier_data;
  EXPECT_EQ((slots[0] & mask), (plaintext_product & mask))
      << "Negative product error: multiplicand_data=" << multiplicand_data
      << ", multiplier_data=" << multiplier_data << ", but product=" << slots[0]
      << " (should be " << plaintext_product << '&' << mask << '='
      << (plaintext_product & mask) << ")\n";
  if (helib_test::verbose) {
    std::cout << "negative product succeeded: ";
    if (outSize)
      std::cout << "bottom " << outSize << " bits of ";
    std::cout << multiplicand_data << "*" << multiplier_data << "=" << slots[0]
              << std::endl;
  }

#ifdef DEBUG_PRINTOUT
  // Print out the ciphertext with the lowest level after multiplication
  // if DEBUG_PRINTOUT is defined.
  const helib::Ctxt* minCtxt = nullptr;
  long minLvl = 10000000;
  for (const helib::Ctxt& c : encrypted_product) {
    long lvl = c.logOfPrimeSet();
    if (lvl < minLvl) {
      minCtxt = &c;
      minLvl = lvl;
    }
  }
  decryptAndPrint(
      (std::cout << " after multiplication: "), *minCtxt, secKey, ea, 0);
  std::cout << std::endl;
#endif
};

TEST_P(GTest_binaryArith, add)
{
  // Randomly generate a pair of numbers of a specified bit size and then
  // encrypt them in binary representation. Then use addTwoNumbers to add the
  // two binary numbers and then check against the plaintext calculation.
  //
  // In this case each ciphertext is considered to be the encryption of one
  // bit, however packing more into the slots is possible.
  // LSB is at the start(left) of the vector.

  const helib::EncryptedArray& ea = *context.ea;
  // outSize 1's on the least significant end of mask.
  long mask = (outSize ? ((1L << outSize) - 1) : -1);

  // Choose two random n-bit integers.
  long addend_data = NTL::RandomBits_long(bitSize);
  long augend_data = NTL::RandomBits_long(bitSize2);

  // Encrypt the individual bits.
  NTL::Vec<helib::Ctxt> encrypted_sum, encrypted_addend, encrypted_augend;

  // Resizes the vector of ciphertexts (encrypted_bits) to match the input size.
  helib::resize(encrypted_addend, bitSize, helib::Ctxt(secKey));
  for (long i = 0; i < bitSize; i++) {
    secKey.Encrypt(encrypted_addend[i], NTL::ZZX((addend_data >> i) & 1));
    if (bootstrap) { // If bootstrapping then modulo down to a lower level.
      encrypted_addend[i].bringToSet(context.getCtxtPrimes(5));
    }
  }
  // Resizes the vector of ciphertexts (encrypted_bits) to match the input size.
  helib::resize(encrypted_augend, bitSize2, helib::Ctxt(secKey));
  for (long i = 0; i < bitSize2; i++) {
    secKey.Encrypt(encrypted_augend[i], NTL::ZZX((augend_data >> i) & 1));
    if (bootstrap) { // If bootstrapping then modulo down to a lower level.
      encrypted_augend[i].bringToSet(context.getCtxtPrimes(5));
    }
  }
  if (helib_test::verbose) {
    std::cout << "\n  bits-size " << bitSize << '+' << bitSize2;
    if (outSize > 0)
      std::cout << "->" << outSize;
    std::cout << std::endl;
    helib::CheckCtxt(encrypted_augend[0], "b4 addition");
  }

  std::vector<long>
      decrypted_result; // Vector that will hold the decrypted result.
  // Test addition.
  // A scope which tests addTwoNumbers using wrappers around the encrypted data.
  {
    helib::CtPtrs_VecCt output_wrapper(
        encrypted_sum); // A wrapper around the output vector.
    helib::addTwoNumbers(output_wrapper,
                         helib::CtPtrs_VecCt(encrypted_addend),
                         helib::CtPtrs_VecCt(encrypted_augend),
                         outSize,
                         &unpackSlotEncoding);
    helib::decryptBinaryNums(decrypted_result, output_wrapper, secKey, ea);
  } // output_wrapper is deleted once out of scope.

  if (helib_test::verbose)
    helib::CheckCtxt(encrypted_sum[helib::lsize(encrypted_sum) - 1],
                     "after addition");

  // Calculate the addition in the plain.
  long plaintext_sum = addend_data + augend_data;
  EXPECT_EQ(decrypted_result[0], ((addend_data + augend_data) & mask))
      << "addTwoNums error: addend_data=" << addend_data
      << ", augend_data=" << augend_data
      << ", but plaintext_sum=" << decrypted_result[0]
      << " (should be =" << (plaintext_sum & mask) << ")\n";
  if (helib_test::verbose) {
    std::cout << "addTwoNums succeeded: ";
    if (outSize)
      std::cout << "bottom " << outSize << " bits of ";
    std::cout << addend_data << "+" << augend_data << "=" << decrypted_result[0]
              << std::endl;
  }

#ifdef DEBUG_PRINTOUT
  // Print out the ciphertext with the lowest level after addition if
  // DEBUG_PRINTOUT is defined.
  const helib::Ctxt* minCtxt = nullptr;
  long minLvl = 1000;
  for (const helib::Ctxt& c : encrypted_sum) {
    long lvl = c.logOfPrimeSet();
    if (lvl < minLvl) {
      minCtxt = &c;
      minLvl = lvl;
    }
  }
  decryptAndPrint((std::cout << " after addition: "), *minCtxt, secKey, ea, 0);
  std::cout << std::endl;
#endif
};

TEST_P(GTest_binaryArith, addManyNumbers)
{
  // Randomly generate a vector of numbers of a specified bit size and then
  // encrypt them in binary representation. Then use addManyNumbers to add
  // them all up and then check against the plaintext calculation.
  //
  // In this case each ciphertext is considered to be the encryption of one
  // bit, however packing more into the slots is possible.
  // LSB is at the start(left) of the vector.

  const long num_summands = 5;
  const helib::EncryptedArray& ea = *context.ea;
  // outSize 1's on the least significant end of mask.
  long mask = (outSize ? ((1L << outSize) - 1) : -1);

  // Choose a set of random n-bit integers.
  std::vector<long> summands_data;
  for (long i = 0; i < num_summands; ++i)
    summands_data.push_back(NTL::RandomBits_long(bitSize));

  // Encrypt the individual bits.
  std::vector<helib::Ctxt> encrypted_sum;
  std::vector<std::vector<helib::Ctxt>> encrypted_summands;

  // Utility function for encrypting a number into a binary representation.
  const auto encrypt_binary_number =
      [&](const long num) -> std::vector<helib::Ctxt> {
    std::vector<helib::Ctxt> encrypted_num;
    // Resizes the vector of ciphertexts (encrypted_bits) to match the input
    // size.
    helib::resize(encrypted_num, bitSize, helib::Ctxt(secKey));
    for (long i = 0; i < bitSize; i++) {
      secKey.Encrypt(encrypted_num[i], NTL::ZZX((num >> i) & 1));
      if (bootstrap) { // If bootstrapping then modulo down to a lower level.
        encrypted_num[i].bringToSet(context.getCtxtPrimes(5));
      }
    }
    return encrypted_num;
  };

  // Encrypt the set of numbers into binary representation.
  for (long i = 0; i < num_summands; ++i)
    encrypted_summands.push_back(encrypt_binary_number(summands_data[i]));

  if (helib_test::verbose)
    helib::CheckCtxt(encrypted_summands[0][0], "b4 addition");

  std::vector<long>
      decrypted_result; // Vector that will hold the decrypted result.
  // Test summation.
  // A scope which tests addManyNumbers using wrappers around the encrypted
  // data.
  {
    helib::CtPtrs_vectorCt output_wrapper(
        encrypted_sum); // A wrapper around the output vector.
    helib::CtPtrMat_vectorCt summands_wrapper(
        encrypted_summands); // A wrapper around the output vector.
    helib::addManyNumbers(
        output_wrapper, summands_wrapper, outSize, &unpackSlotEncoding);
    helib::decryptBinaryNums(decrypted_result, output_wrapper, secKey, ea);
  } // output_wrapper is deleted once out of scope.

  if (helib_test::verbose)
    helib::CheckCtxt(encrypted_sum[helib::lsize(encrypted_sum) - 1],
                     "after addition");

  // Calculate the summation in the plain.
  long plaintext_sum = std::accumulate(
      summands_data.begin(), summands_data.end(), 0l, std::plus<long>());
  EXPECT_EQ(decrypted_result[0], plaintext_sum & mask);
  if (helib_test::verbose) {
    std::cout << "addManyNums succeeded: ";
    if (outSize)
      std::cout << "bottom " << outSize << " bits of ";
    std::cout << summands_data[0];
    for (long i = 1; i < num_summands; ++i)
      std::cout << "+" << summands_data[i];
    std::cout << "=" << decrypted_result[0] << std::endl;
  }
}

INSTANTIATE_TEST_SUITE_P(
    small_parameter_sizes_repeated,
    GTest_binaryArith,
    ::testing::Combine(
        ::testing::Values(
            // SLOW
            Parameters(1, 5, 0, 0, false, 0, 1)
            // FAST
            // Parameters(0, 5, 0, 0, false, 0, 1)), ::testing::Range(0,2)

            ),
        ::testing::Range(0, 2)) // The range is for repeats
);

} // anonymous namespace


/*******************************************************************************
 *
 * A simple class for representing intervals and performing interval
 * arithmetic.
 *
 ******************************************************************************/

#pragma once

#include <boost/optional.hpp>
#include <crab/common/stats.hpp>
#include <crab/common/types.hpp>
#include <crab/domains/linear_interval_solver.hpp>
#include <crab/numbers/bignums.hpp>

namespace ikos {

template <typename Number> class bound {
public:
  typedef bound<Number> bound_t;

private:
  bool _is_infinite;
  Number _n;

private:
  bound();

  bound(bool is_infinite, Number n) : _is_infinite(is_infinite), _n(n) {
    if (is_infinite) {
      if (n > 0)
        _n = 1;
      else
        _n = -1;
    }
  }

public:
  static bound_t min(bound_t x, bound_t y) { return (x.operator<=(y) ? x : y); }

  static bound_t min(bound_t x, bound_t y, bound_t z) {
    return min(x, min(y, z));
  }

  static bound_t min(bound_t x, bound_t y, bound_t z, bound_t t) {
    return min(x, min(y, z, t));
  }

  static bound_t max(bound_t x, bound_t y) { return (x.operator<=(y) ? y : x); }

  static bound_t max(bound_t x, bound_t y, bound_t z) {
    return max(x, max(y, z));
  }

  static bound_t max(bound_t x, bound_t y, bound_t z, bound_t t) {
    return max(x, max(y, z, t));
  }

  static bound_t plus_infinity() { return bound_t(true, 1); }

  static bound_t minus_infinity() { return bound_t(true, -1); }

public:
  bound(int n) : _is_infinite(false), _n(n) {}

  bound(std::string s) : _n(1) {
    if (s == "+oo") {
      _is_infinite = true;
    } else if (s == "-oo") {
      _is_infinite = true;
      _n = -1;
    } else {
      _is_infinite = false;
      _n = Number(s);
    }
  }

  bound(Number n) : _is_infinite(false), _n(n) {}

  bound(const bound_t &o) : _is_infinite(o._is_infinite), _n(o._n) {}

  bound_t &operator=(const bound_t &o) {
    if (this != &o) {
      _is_infinite = o._is_infinite;
      _n = o._n;
    }
    return *this;
  }

  bool is_infinite() const { return _is_infinite; }

  bool is_finite() const { return !_is_infinite; }

  bool is_plus_infinity() const { return (is_infinite() && _n > 0); }

  bool is_minus_infinity() const { return (is_infinite() && _n < 0); }

  bound_t operator-() const { return bound_t(_is_infinite, -_n); }

  bound_t operator+(bound_t x) const {
    if (is_finite() && x.is_finite()) {
      return bound_t(_n + x._n);
    } else if (is_finite() && x.is_infinite()) {
      return x;
    } else if (is_infinite() && x.is_finite()) {
      return *this;
    } else if (_n == x._n) {
      return *this;
    } else {
      CRAB_ERROR("Bound: undefined operation -oo + +oo");
    }
  }

  bound_t &operator+=(bound_t x) { return operator=(operator+(x)); }

  bound_t operator-(bound_t x) const { return operator+(x.operator-()); }

  bound_t &operator-=(bound_t x) { return operator=(operator-(x)); }

  bound_t operator*(bound_t x) const {
    if (x._n == 0)
      return x;
    else if (_n == 0)
      return *this;
    else
      return bound_t(_is_infinite || x._is_infinite, _n * x._n);
  }

  bound_t &operator*=(bound_t x) { return operator=(operator*(x)); }

  bound_t operator/(bound_t x) const {
    if (x._n == 0) {
      CRAB_ERROR("Bound: division by zero");
    } else if (is_finite() && x.is_finite()) {
      return bound_t(false, _n / x._n);
    } else if (is_finite() && x.is_infinite()) {
      if (_n > 0) {
        return x;
      } else if (_n == 0) {
        return *this;
      } else {
        return x.operator-();
      }
    } else if (is_infinite() && x.is_finite()) {
      if (x._n > 0) {
        return *this;
      } else {
        return operator-();
      }
    } else {
      return bound_t(true, _n * x._n);
    }
  }

  bound_t &operator/=(bound_t x) { return operator=(operator/(x)); }

  bool operator<(bound_t x) const { return !operator>=(x); }

  bool operator>(bound_t x) const { return !operator<=(x); }

  bool operator==(bound_t x) const {
    return (_is_infinite == x._is_infinite && _n == x._n);
  }

  bool operator!=(bound_t x) const { return !operator==(x); }

  /*	operator<= and operator>= use a somewhat optimized implementation.
   *	results include up to 20% improvements in performance in the octagon
   *domain over a more naive implementation.
   */
  bool operator<=(bound_t x) const {
    if (_is_infinite xor x._is_infinite) {
      if (_is_infinite) {
        return _n < 0;
      }
      return x._n > 0;
    }
    return _n <= x._n;
  }

  bool operator>=(bound_t x) const {
    if (_is_infinite xor x._is_infinite) {
      if (_is_infinite) {
        return _n > 0;
      }
      return x._n < 0;
    }
    return _n >= x._n;
  }

  bound_t abs() const {
    if (operator>=(0)) {
      return *this;
    } else {
      return operator-();
    }
  }

  boost::optional<Number> number() const {
    if (is_infinite()) {
      return boost::optional<Number>();
    } else {
      return boost::optional<Number>(_n);
    }
  }

  void write(crab::crab_os &o) const {
    if (is_plus_infinity()) {
      o << "+oo";
    } else if (is_minus_infinity()) {
      o << "-oo";
    } else {
      o << _n;
    }
  }

}; // class bound

template <typename Number>
inline crab::crab_os &operator<<(crab::crab_os &o, const bound<Number> &b) {
  b.write(o);
  return o;
}

typedef bound<z_number> z_bound;
typedef bound<q_number> q_bound;

namespace bounds_impl {
// Conversion between z_bound and q_bound
// template<class B1, class B2>
// inline void convert_bounds(B1 b1, B2& b2);

inline void convert_bounds(z_bound b1, z_bound &b2) { std::swap(b1, b2); }
inline void convert_bounds(q_bound b1, q_bound &b2) { std::swap(b1, b2); }
inline void convert_bounds(z_bound b1, q_bound &b2) {
  if (b1.is_plus_infinity())
    b2 = q_bound::plus_infinity();
  else if (b1.is_minus_infinity())
    b2 = q_bound::minus_infinity();
  else
    b2 = q_bound(q_number(*b1.number()));
}
inline void convert_bounds(q_bound b1, z_bound &b2) {
  if (b1.is_plus_infinity())
    b2 = z_bound::plus_infinity();
  else if (b1.is_minus_infinity())
    b2 = z_bound::minus_infinity();
  else
    b2 = z_bound((*(b1.number())).round_to_lower());
}
} // namespace bounds_impl

template <typename Number> class interval {

public:
  typedef bound<Number> bound_t;
  typedef interval<Number> interval_t;

private:
  bound_t _lb;
  bound_t _ub;

public:
  static interval_t top() {
    return interval_t(bound_t::minus_infinity(), bound_t::plus_infinity());
  }

  static interval_t bottom() { return interval_t(); }

private:
  interval() : _lb(0), _ub(-1) {}

  static Number abs(Number x) { return x < 0 ? -x : x; }

  static Number max(Number x, Number y) { return x.operator<=(y) ? y : x; }

  static Number min(Number x, Number y) { return x.operator<(y) ? x : y; }

public:
  interval(bound_t lb, bound_t ub) : _lb(lb), _ub(ub) {
    if (lb > ub) {
      _lb = 0;
      _ub = -1;
    }
  }

  interval(bound_t b) : _lb(b), _ub(b) {
    if (b.is_infinite()) {
      _lb = 0;
      _ub = -1;
    }
  }

  interval(Number n) : _lb(n), _ub(n) {}

  interval(std::string b) : _lb(b), _ub(b) {
    if (_lb.is_infinite()) {
      _lb = 0;
      _ub = -1;
    }
  }

  interval(const interval_t &i) : _lb(i._lb), _ub(i._ub) {}

  interval_t &operator=(interval_t i) {
    _lb = i._lb;
    _ub = i._ub;
    return *this;
  }

  bound_t lb() const { return _lb; }

  bound_t ub() const { return _ub; }

  bool is_bottom() const { return (_lb > _ub); }

  bool is_top() const { return (_lb.is_infinite() && _ub.is_infinite()); }

  interval_t lower_half_line() const {
    return interval_t(bound_t::minus_infinity(), _ub);
  }

  interval_t upper_half_line() const {
    return interval_t(_lb, bound_t::plus_infinity());
  }

  bool operator==(interval_t x) const {
    if (is_bottom()) {
      return x.is_bottom();
    } else {
      return (_lb == x._lb) && (_ub == x._ub);
    }
  }

  bool operator!=(interval_t x) const { return !operator==(x); }

  bool operator<=(interval_t x) const {
    if (is_bottom()) {
      return true;
    } else if (x.is_bottom()) {
      return false;
    } else {
      return (x._lb <= _lb) && (_ub <= x._ub);
    }
  }

  interval_t operator|(interval_t x) const {
    if (is_bottom()) {
      return x;
    } else if (x.is_bottom()) {
      return *this;
    } else {
      return interval_t(bound_t::min(_lb, x._lb), bound_t::max(_ub, x._ub));
    }
  }

  interval_t operator&(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return interval_t(bound_t::max(_lb, x._lb), bound_t::min(_ub, x._ub));
    }
  }

  interval_t operator||(interval_t x) const {
    if (is_bottom()) {
      return x;
    } else if (x.is_bottom()) {
      return *this;
    } else {
      return interval_t(x._lb < _lb ? bound_t::minus_infinity() : _lb,
                        _ub < x._ub ? bound_t::plus_infinity() : _ub);
    }
  }

  template <typename Thresholds>
  interval_t widening_thresholds(interval_t x, const Thresholds &ts) {
    if (is_bottom()) {
      return x;
    } else if (x.is_bottom()) {
      return *this;
    } else {
      bound_t lb = (x._lb < _lb ? ts.get_prev(x._lb) : _lb);
      bound_t ub = (_ub < x._ub ? ts.get_next(x._ub) : _ub);
      return interval_t(lb, ub);
    }
  }

  interval_t operator&&(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return interval_t(_lb.is_infinite() && x._lb.is_finite() ? x._lb : _lb,
                        _ub.is_infinite() && x._ub.is_finite() ? x._ub : _ub);
    }
  }

  interval_t operator+(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return interval_t(_lb + x._lb, _ub + x._ub);
    }
  }

  interval_t &operator+=(interval_t x) { return operator=(operator+(x)); }

  interval_t operator-() const {
    if (is_bottom()) {
      return bottom();
    } else {
      return interval_t(-_ub, -_lb);
    }
  }

  interval_t operator-(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return interval_t(_lb - x._ub, _ub - x._lb);
    }
  }

  interval_t &operator-=(interval_t x) { return operator=(operator-(x)); }

  interval_t operator*(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      bound_t ll = _lb * x._lb;
      bound_t lu = _lb * x._ub;
      bound_t ul = _ub * x._lb;
      bound_t uu = _ub * x._ub;
      return interval_t(bound_t::min(ll, lu, ul, uu),
                        bound_t::max(ll, lu, ul, uu));
    }
  }

  interval_t &operator*=(interval_t x) { return operator=(operator*(x)); }

  interval_t operator/(interval_t x) const;

  interval_t &operator/=(interval_t x) { return operator=(operator/(x)); }

  boost::optional<Number> singleton() const {
    if (!is_bottom() && _lb == _ub) {
      return _lb.number();
    } else {
      return boost::optional<Number>();
    }
  }

  bool operator[](Number n) const {
    if (is_bottom()) {
      return false;
    } else {
      bound_t b(n);
      return (_lb <= b) && (b <= _ub);
    }
  }

  void write(crab::crab_os &o) const {
    if (is_bottom()) {
      o << "_|_";
    } else {
      o << "[" << _lb << ", " << _ub << "]";
    }
  }

  // division and remainder operations

  interval_t UDiv(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

  interval_t SRem(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

  interval_t URem(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

  // bitwise operations
  interval_t And(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

  interval_t Or(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

  interval_t Xor(interval_t x) const { return Or(x); }

  interval_t Shl(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

  interval_t LShr(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

  interval_t AShr(interval_t x) const {
    if (is_bottom() || x.is_bottom()) {
      return bottom();
    } else {
      return top();
    }
  }

}; //  class interval

template <>
inline interval<q_number> interval<q_number>::
operator/(interval<q_number> x) const {
  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else {
    boost::optional<q_number> d = x.singleton();
    if (d && *d == 0) {
      // [_, _] / 0 = _|_
      return bottom();
    } else if (x[0]) {
      boost::optional<q_number> n = singleton();
      if (n && *n == 0) {
        // 0 / [_, _] = 0
        return interval_t(q_number(0));
      } else {
        return top();
      }
    } else {
      bound_t ll = _lb / x._lb;
      bound_t lu = _lb / x._ub;
      bound_t ul = _ub / x._lb;
      bound_t uu = _ub / x._ub;
      return interval_t(bound_t::min(ll, lu, ul, uu),
                        bound_t::max(ll, lu, ul, uu));
    }
  }
}

template <>
inline interval<z_number> interval<z_number>::
operator/(interval<z_number> x) const {
  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else {
    // Divisor is a singleton:
    //   the linear interval solver can perform many divisions where
    //   the divisor is a singleton interval. We optimize for this case.
    if (boost::optional<z_number> n = x.singleton()) {
      z_number c = *n;
      if (c == 1) {
        return *this;
      } else if (c > 0) {
        return interval_t(_lb / c, _ub / c);
      } else if (c < 0) {
        return interval_t(_ub / c, _lb / c);
      } else {
      }
    }
    // Divisor is not a singleton
    typedef interval<z_number> z_interval;
    if (x[0]) {
      z_interval l(x._lb, z_bound(-1));
      z_interval u(z_bound(1), x._ub);
      return (operator/(l) | operator/(u));
    } else if (operator[](0)) {
      z_interval l(_lb, z_bound(-1));
      z_interval u(z_bound(1), _ub);
      return ((l / x) | (u / x) | z_interval(z_number(0)));
    } else {
      // Neither the dividend nor the divisor contains 0
      z_interval a =
          (_ub < 0) ? (*this + ((x._ub < 0) ? (x + z_interval(z_number(1)))
                                            : (z_interval(z_number(1)) - x)))
                    : *this;
      bound_t ll = a._lb / x._lb;
      bound_t lu = a._lb / x._ub;
      bound_t ul = a._ub / x._lb;
      bound_t uu = a._ub / x._ub;
      return interval_t(bound_t::min(ll, lu, ul, uu),
                        bound_t::max(ll, lu, ul, uu));
    }
  }
}

template <>
inline interval<z_number> interval<z_number>::SRem(interval<z_number> x) const {
  // note that the sign of the divisor does not matter

  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else if (singleton() && x.singleton()) {
    z_number dividend = *singleton();
    z_number divisor = *x.singleton();

    if (divisor == 0) {
      return bottom();
    }

    return interval_t(dividend % divisor);
  } else if (x.ub().is_finite() && x.lb().is_finite()) {
    z_number max_divisor = max(abs(*x.lb().number()), abs(*x.ub().number()));

    if (max_divisor == 0) {
      return bottom();
    }

    if (lb() < 0) {
      if (ub() > 0) {
        return interval_t(-(max_divisor - 1), max_divisor - 1);
      } else {
        return interval_t(-(max_divisor - 1), 0);
      }
    } else {
      return interval_t(0, max_divisor - 1);
    }
  } else {
    return top();
  }
}

template <>
inline interval<z_number> interval<z_number>::URem(interval<z_number> x) const {

  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else if (singleton() && x.singleton()) {
    z_number dividend = *singleton();
    z_number divisor = *x.singleton();

    if (divisor < 0) {
      return top();
    } else if (divisor == 0) {
      return bottom();
    } else if (dividend < 0) {
      // dividend is treated as an unsigned integer.
      // we would need the size to be more precise
      return interval_t(0, divisor - 1);
    } else {
      return interval_t(dividend % divisor);
    }
  } else if (x.ub().is_finite() && x.lb().is_finite()) {
    z_number max_divisor = *x.ub().number();

    if (x.lb() < 0 || x.ub() < 0) {
      return top();
    } else if (max_divisor == 0) {
      return bottom();
    }

    return interval_t(0, max_divisor - 1);
  } else {
    return top();
  }
}

template <>
inline interval<z_number> interval<z_number>::And(interval<z_number> x) const {
  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else {
    boost::optional<z_number> left_op = singleton();
    boost::optional<z_number> right_op = x.singleton();

    if (left_op && right_op) {
      return interval_t((*left_op) & (*right_op));
    } else if (lb() >= 0 && x.lb() >= 0) {
      return interval_t(0, bound_t::min(ub(), x.ub()));
    } else {
      return top();
    }
  }
}

template <>
inline interval<z_number> interval<z_number>::Or(interval<z_number> x) const {
  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else {
    boost::optional<z_number> left_op = singleton();
    boost::optional<z_number> right_op = x.singleton();

    if (left_op && right_op) {
      return interval_t((*left_op) | (*right_op));
    } else if (lb() >= 0 && x.lb() >= 0) {
      boost::optional<z_number> left_ub = ub().number();
      boost::optional<z_number> right_ub = x.ub().number();

      if (left_ub && right_ub) {
        z_number m = (*left_ub > *right_ub ? *left_ub : *right_ub);
        return interval_t(0, m.fill_ones());
      } else {
        return interval_t(0, bound_t::plus_infinity());
      }
    } else {
      return top();
    }
  }
}

template <>
inline interval<z_number> interval<z_number>::Xor(interval<z_number> x) const {
  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else {
    boost::optional<z_number> left_op = singleton();
    boost::optional<z_number> right_op = x.singleton();

    if (left_op && right_op) {
      return interval_t((*left_op) ^ (*right_op));
    } else {
      return Or(x);
    }
  }
}

template <>
inline interval<z_number> interval<z_number>::Shl(interval<z_number> x) const {
  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else {
    if (boost::optional<z_number> shift = x.singleton()) {
      z_number k = *shift;
      if (k < 0) {
        // CRAB_ERROR("lshr shift operand cannot be negative");
        return top();
      }
      // Some crazy linux drivers generate shl instructions with
      // huge shifts.  We limit the number of times the loop is run
      // to avoid wasting too much time on it.
      if (k <= 128) {
        z_number factor = 1;
        for (int i = 0; k > i; i++) {
          factor *= 2;
        }
        return (*this) * factor;
      }
    }
    return top();
  }
}

template <>
inline interval<z_number> interval<z_number>::AShr(interval<z_number> x) const {
  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else {
    if (boost::optional<z_number> shift = x.singleton()) {
      z_number k = *shift;
      if (k < 0) {
        return top();
      }
      // Some crazy linux drivers generate ashr instructions with
      // huge shifts.  We limit the number of times the loop is run
      // to avoid wasting too much time on it.
      if (k <= 128) {
        z_number factor = 1;
        for (int i = 0; k > i; i++) {
          factor *= 2;
        }
        return (*this) / factor;
      }
    }
    return top();
  }
}

template <>
inline interval<z_number> interval<z_number>::LShr(interval<z_number> x) const {
  if (is_bottom() || x.is_bottom()) {
    return bottom();
  } else {
    if (boost::optional<z_number> shift = x.singleton()) {
      z_number k = *shift;
      if (k < 0) {
        return top();
      }
      if (lb() >= 0 && ub().is_finite()) {
        z_number lb = *this->lb().number();
        z_number ub = *this->ub().number();
        return interval<z_number>(lb >> k, ub >> k);
      }
    }
    return this->top();
  }
}

template <typename Number>
inline interval<Number> operator+(Number c, interval<Number> x) {
  return interval<Number>(c) + x;
}

template <typename Number>
inline interval<Number> operator+(interval<Number> x, Number c) {
  return x + interval<Number>(c);
}

template <typename Number>
inline interval<Number> operator*(Number c, interval<Number> x) {
  return interval<Number>(c) * x;
}

template <typename Number>
inline interval<Number> operator*(interval<Number> x, Number c) {
  return x * interval<Number>(c);
}

template <typename Number>
inline interval<Number> operator/(Number c, interval<Number> x) {
  return interval<Number>(c) / x;
}

template <typename Number>
inline interval<Number> operator/(interval<Number> x, Number c) {
  return x / interval<Number>(c);
}

template <typename Number>
inline interval<Number> operator-(Number c, interval<Number> x) {
  return interval<Number>(c) - x;
}

template <typename Number>
inline interval<Number> operator-(interval<Number> x, Number c) {
  return x - interval<Number>(c);
}

template <typename Number>
inline crab::crab_os &operator<<(crab::crab_os &o, const interval<Number> &i) {
  i.write(o);
  return o;
}

namespace linear_interval_solver_impl {

typedef interval<z_number> z_interval;
typedef interval<q_number> q_interval;

template <> inline z_interval trim_interval(z_interval i, z_interval j) {
  if (boost::optional<z_number> c = j.singleton()) {
    if (i.lb() == *c) {
      return z_interval(*c + 1, i.ub());
    } else if (i.ub() == *c) {
      return z_interval(i.lb(), *c - 1);
    } else {
    }
  }
  return i;
}

template <> inline q_interval trim_interval(q_interval i, q_interval /* j */) {
  // No refinement possible for disequations over rational numbers
  return i;
}

template <>
inline z_interval lower_half_line(z_interval i, bool /*is_signed*/) {
  return i.lower_half_line();
}

template <>
inline q_interval lower_half_line(q_interval i, bool /*is_signed*/) {
  return i.lower_half_line();
}

template <>
inline z_interval upper_half_line(z_interval i, bool /*is_signed*/) {
  return i.upper_half_line();
}

template <>
inline q_interval upper_half_line(q_interval i, bool /*is_signed*/) {
  return i.upper_half_line();
}
} // namespace linear_interval_solver_impl

} // namespace ikos


#ifndef ROSE499_SYLVESTERCONTROL_HPP
#define ROSE499_SYLVESTERCONTROL_HPP

#include <Eigen/Core>
#include "rose499/diffdrive.hpp"
#include "rose499/spline.hpp"

struct SylvesterController : public DriveController
{
    SylvesterController(DriveSystem&, Eigen::Matrix<ValueType, 2, 1> goal, ValueType goalRadius);
    virtual DriveController::ValueType genTurnControl(DriveController::StateType x, double t) override;

    Eigen::Matrix<DriveController::ValueType, 2, 1> const & linearizedState() const;

protected:
    std::ostream& printSpecificHeaders(std::ostream& s) const override;
    std::ostream& printSpecificData(std::ostream& s) const override;

private:
    Eigen::Matrix<DriveController::ValueType, 2, 1> mXi;
};

Eigen::Matrix<SimulatorTypes::ValueType, 2, 2> quinticHessian(  SimulatorTypes::ValueType A1,
                                                                SimulatorTypes::ValueType A2,
                                                                SimulatorTypes::ValueType A3,
                                                                SimulatorTypes::ValueType A4,
                                                                SimulatorTypes::ValueType A5,
                                                                SimulatorTypes::ValueType A6,
                                                                SimulatorTypes::ValueType B1,
                                                                SimulatorTypes::ValueType B2,
                                                                SimulatorTypes::ValueType B3,
                                                                SimulatorTypes::ValueType B4,
                                                                SimulatorTypes::ValueType B5,
                                                                SimulatorTypes::ValueType B6,
                                                                SimulatorTypes::ValueType x1,
                                                                SimulatorTypes::ValueType x2 );

Eigen::Matrix<SimulatorTypes::ValueType, 1, 2> quinticJacobian( SimulatorTypes::ValueType A1,
                                                                SimulatorTypes::ValueType A2,
                                                                SimulatorTypes::ValueType A3,
                                                                SimulatorTypes::ValueType A4,
                                                                SimulatorTypes::ValueType A5,
                                                                SimulatorTypes::ValueType A6,
                                                                SimulatorTypes::ValueType B1,
                                                                SimulatorTypes::ValueType B2,
                                                                SimulatorTypes::ValueType B3,
                                                                SimulatorTypes::ValueType B4,
                                                                SimulatorTypes::ValueType B5,
                                                                SimulatorTypes::ValueType B6,
                                                                SimulatorTypes::ValueType x1,
                                                                SimulatorTypes::ValueType x2 );

SimulatorTypes::ValueType                      quinticLevelSet( SimulatorTypes::ValueType A1,
                                                                SimulatorTypes::ValueType A2,
                                                                SimulatorTypes::ValueType A3,
                                                                SimulatorTypes::ValueType A4,
                                                                SimulatorTypes::ValueType A5,
                                                                SimulatorTypes::ValueType A6,
                                                                SimulatorTypes::ValueType B1,
                                                                SimulatorTypes::ValueType B2,
                                                                SimulatorTypes::ValueType B3,
                                                                SimulatorTypes::ValueType B4,
                                                                SimulatorTypes::ValueType B5,
                                                                SimulatorTypes::ValueType B6,
                                                                SimulatorTypes::ValueType x1,
                                                                SimulatorTypes::ValueType x2 );
#endif


//
//  Copyright (c) 2018, Cem Bassoy, cem.bassoy@gmail.com
//  Copyright (c) 2019, Amit Singh, amitsingh19975@gmail.com
//
//  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)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//
//  And we acknowledge the support from all contributors.

#include <iostream>
#include <algorithm>
#include <boost/numeric/ublas/tensor.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>

#include <boost/test/unit_test.hpp>

#include "utility.hpp"

// BOOST_AUTO_TEST_SUITE ( test_tensor_functions, * boost::unit_test::depends_on("test_tensor_contraction") )
BOOST_AUTO_TEST_SUITE ( test_tensor_extents_static_size_functions)

using test_types = zip<int,float,std::complex<float>>::with_t<boost::numeric::ublas::layout::first_order, boost::numeric::ublas::layout::last_order>;

//using test_types = zip<int>::with_t<boost::numeric::ublas::layout::first_order>;

struct fixture
{
  std::tuple<
    boost::numeric::ublas::extents<2>,
    boost::numeric::ublas::extents<2>,
    boost::numeric::ublas::extents<3>,
    boost::numeric::ublas::extents<3>,
    boost::numeric::ublas::extents<4>
    > extents_tuple{
      {1,1}, // 1
      {2,3}, // 2
      {2,3,1}, // 3
      {4,2,3}, // 4
      {4,2,3,5} // 5
    };

  std::vector<boost::numeric::ublas::extents<>> extents_vector =
    {
      {1,1}, // 1
      {2,3}, // 2
      {2,3,1}, // 3
      {4,2,3}, // 4
      {4,2,3,5} // 5
  };

};

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_extents_static_size_prod_vector, value,  test_types, fixture )
{
  namespace ublas    = boost::numeric::ublas;
  using value_t   = typename value::first_type;
  using layout_t  = typename value::second_type;

  for_each_in_tuple(extents_tuple,[](auto const& /*unused*/, auto const& n){

    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(n)>>;
    using tensor_t = ublas::tensor_static_rank<value_t, size, layout_t>;
    using vector_t = typename tensor_t::vector_type;
    auto a = tensor_t(n);
    a = 2;

    for (auto m = 0u; m < ublas::size(n); ++m) {
      auto b = vector_t(n[m], value_t{1});

      auto c = ublas::prod(a, b, m + 1);

      for (auto i = 0u; i < c.size(); ++i)
        BOOST_CHECK_EQUAL(c[i], value_t( static_cast< inner_type_t<value_t> >(n[m]) ) * a[i]);
    }
  });

}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_extents_static_size_prod_matrix, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t   = typename value::first_type;
  using layout_t  = typename value::second_type;

  for_each_in_tuple(extents_tuple,[](auto const& /*unused*/, auto const & n){
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(n)>>;
    using tensor_t = ublas::tensor_static_rank<value_t, size, layout_t>;
    using matrix_t = typename tensor_t::matrix_type;

    auto a = tensor_t(n);
    a = 2;
    for (auto m = 0u; m < ublas::size(n); ++m) {

      auto b = matrix_t  ( n[m], n[m], value_t{1} );
      auto c = ublas::prod(a, b, m + 1);

      for (auto i = 0u; i < c.size(); ++i){
        BOOST_CHECK_EQUAL(c[i], value_t( static_cast< inner_type_t<value_t> >(n[m]) ) * a[i]);
      }
    }
  });

}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_extents_static_size_prod_tensor_1, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t   = typename value::first_type;
  using layout_t  = typename value::second_type;

  auto check = [&]<std::size_t ... qs>(auto const& a, auto const& b, std::index_sequence<qs...> /*unused*/)
  {
    namespace ublas = boost::numeric::ublas;

    constexpr auto q = sizeof...(qs);

    using tensorA = std::decay_t<decltype(a)>;
    using tensorB = std::decay_t<decltype(b)>;

    using extentsA = typename tensorA::extents_type;
    using extentsB = typename tensorB::extents_type;

    static_assert(!ublas::is_static_v<extentsA> && !ublas::is_static_v<extentsB> );

    constexpr auto one_of_extents_is_resizable = ublas::is_dynamic_rank_v<extentsA> ||
                                                 ublas::is_dynamic_rank_v<extentsB>;

    using phi_type = std::conditional_t<one_of_extents_is_resizable,
                                        std::vector<std::size_t>,
                                        std::array<std::size_t,q> >;

    auto phi = phi_type{};
    if constexpr(std::is_same_v<phi_type,std::vector<std::size_t>>){
      phi.resize(q);
    }
    std::iota(phi.begin(), phi.end(), std::size_t{1});
    auto c = ublas::prod(a, b, phi);

    auto const& na = a.extents();
    auto acc = std::size_t{1};
    for (auto i = 0ul; i < q; ++i){
      acc *= na.at(phi.at(i)-1);
    }
    const auto v = value_t(acc) * a[0] * b[0];
    BOOST_CHECK( std::all_of(c.begin(),c.end(),[v](auto cc){ return cc == v;}));
  };

  for_each_in_tuple(extents_tuple,[&](auto const& /*I*/, auto const& n){
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(n)>>;
    constexpr auto modes = std::make_index_sequence<size>{};
    using tensor_t = ublas::tensor_static_rank<value_t, size, layout_t>;
    auto a = tensor_t(n);
    auto b = tensor_t(n);
    a = 2;
    b = 3;
    for_each_in_index(modes, a,b, check );
  });

  for_each_in_tuple(extents_tuple,[&](auto const& I, auto const& n){
    auto const& nA = n;
    auto const& nB = extents_vector[I];
    constexpr auto sizeA = std::tuple_size_v<std::decay_t<decltype(n)>>;
    constexpr auto modes = std::make_index_sequence<sizeA>{};
    using tensorA_type = ublas::tensor_static_rank<value_t, sizeA , layout_t>;
    using tensorB_type = ublas::tensor_dynamic<value_t, layout_t>;
    auto a = tensorA_type(nA);
    auto b = tensorB_type(nB);
    a = 2;
    b = 3;

    for_each_in_index(modes, a,b, check );
  });

  for_each_in_tuple(extents_tuple,[&](auto const& I, auto const& n){
    auto const& nA = extents_vector[I];
    auto const& nB = n;
    constexpr auto sizeB = std::tuple_size_v<std::decay_t<decltype(n)>>;
    constexpr auto modes = std::make_index_sequence<sizeB>{};
    using tensor_t_1 = ublas::tensor_dynamic<value_t, layout_t>;
    using tensor_t_2 = ublas::tensor_static_rank<value_t, sizeB, layout_t>;
    auto a = tensor_t_1(nA);
    auto b = tensor_t_2(nB);
    a = 2;
    b = 3;
    for_each_in_index(modes, a,b, check );

  });
}

// TODO:
#if 0

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_extents_static_size_prod_tensor_2, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t   = typename value::first_type;
  using layout_t  = typename value::second_type;

  constexpr auto to_array = []<std::size_t ... is>(std::index_sequence<is...>/*unused*/) {
    return std::array<std::size_t,sizeof...(is)>{is...};
  };

  auto compute_factorial = []<std::size_t ... is>(std::index_sequence<is...>/*unused*/) {
    return ( 1 * ... * is );
  };
  /*
  auto compute_factorial = [](auto const& p){
    auto f = 1ul;
    for(auto i = 1u; i <= p; ++i)
      f *= i;
    return f;
  };
*/
  auto permute_extents_dynamic_rank = [](auto const& pi, auto const& na){
    auto nb = ublas::extents<>(na.begin(),na.end());
    assert(std::size(pi) == ublas::size(na));
    for(auto j = 0u; j < std::size(pi); ++j)
      nb[pi[j]-1] = na[j];
    return nb;
  };

  auto permute_extents_static_rank = []<std::size_t size>(std::array<std::size_t,size> const& pi, auto const& na){
    //constexpr auto size = std::tuple_size_v<std::decay_t<decltype(na)>>;
    auto na_base = na.base();
    assert(std::size(pi) == size);
    for(auto j = 0u; j < std::size(pi); ++j)
      na_base[pi[j]-1] = na[j];
    return ublas::extents<size>(na_base.begin(),na_base.end());
  };

  for_each_in_tuple(extents_tuple,[&](auto const& /*unused*/, auto const& n){
    auto const& na = n;
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(n)>>;
    using tensorA_type = ublas::tensor_static_rank<value_t, size, layout_t>;
    auto a = tensorA_type(na);
    a = 2;
    assert(a.rank() == size);
    //    auto const pa = a.rank();
    auto pi  = to_array(std::make_index_sequence<size>{});
    constexpr auto factorial = compute_factorial(std::make_index_sequence<size>{});
    //    auto pi = std::vector<std::size_t>(pa);
    //    auto fac = compute_factorial(pa);
    //    std::iota(pi.begin(), pi.end(), 1);

    constexpr auto factorials = std::make_index_sequence<factorial>{};

    //    for_each_in_tuple(factorials,[&](auto const& /*unused*/, auto const& /*unused*/){
    //      using tensorB_type = ublas::tensor_dynamic<value_t, layout_t>;
    //      const auto nb = permute_extents_dynamic_rank(pi, na);
    //      const auto b = tensorB_type(nb, value_t{3});

    //      constexpr auto modes  = std::make_index_sequence<size>{};

    //      for_each_in_tuple(modes,[&](auto const& /*unused*/, auto const& /*unused*/){

    //      const auto phia = to_array(std::make_index_sequence<Q>);
    //      const auto phib = std::array<std::size_t>(q);

    //    });

    //    for (auto f = 0ul; f < fac; ++f) {
    //      for (auto q = 0ul; q <= pa; ++q) {

    //        auto phia = std::vector<std::size_t>(q);
    //        auto phib = std::vector<std::size_t>(q);

    //        std::iota(phia.begin(), phia.end(), 1ul);
    //        std::transform(phia.begin(), phia.end(), phib.begin(),
    //                       [&pi](std::size_t i) { return pi.at(i - 1); });

    //        auto c = ublas::prod(a, b, phia, phib);

    //        auto acc = value_t(1);
    //        for (auto i = 0ul; i < q; ++i)
    //          acc *= value_t( static_cast< inner_type_t<value_t> >( a.extents().at(phia.at(i) - 1) ) );

    //        for (auto i = 0ul; i < c.size(); ++i)
    //          BOOST_CHECK_EQUAL(c[i], acc *a[0] * b[0]);
    //      }

    //      std::next_permutation(pi.begin(), pi.end());
    //    }
  });

  for_each_in_tuple(extents_tuple,[&](auto const& /*unused*/, auto & /*n*/){
    //    auto const& na = n;
    //    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(n)>>;
    //    using tensor_t_1 = ublas::tensor_static_rank<value_t, size, layout_t>;
    //    auto a = tensor_t_1(na, value_t{2});
    //    auto const pa = a.rank();

    //    auto pi = std::vector<std::size_t>(pa);
    //    auto fac = compute_factorial(pa);
    //    std::iota(pi.begin(), pi.end(), 1);

    //    for (auto f = 0ul; f < fac; ++f) {
    //      auto nb = permute_extents_static_rank(pi, na);

    //      using tensor_t_2 = ublas::tensor_static_rank<value_t, size, layout_t>;
    //      auto b = tensor_t_2(nb, value_t{3});

    //      for (auto q = 0ul; q <= pa; ++q) {

    //        auto phia = std::vector<std::size_t>(q);
    //        auto phib = std::vector<std::size_t>(q);

    //        std::iota(phia.begin(), phia.end(), 1ul);
    //        std::transform(phia.begin(), phia.end(), phib.begin(),
    //                       [&pi](std::size_t i) { return pi.at(i - 1); });

    //        auto c = ublas::prod(a, b, phia, phib);

    //        auto acc = value_t(1);
    //        for (auto i = 0ul; i < q; ++i){
    //          acc *= value_t( static_cast< inner_type_t<value_t> >( a.extents().at(phia.at(i) - 1) ) );
    //        }

    //        for (auto i = 0ul; i < c.size(); ++i)
    //          BOOST_CHECK_EQUAL(c[i], acc *a[0] * b[0]);
    //      }

    //      std::next_permutation(pi.begin(), pi.end());
    //    }
  });

}
#endif

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_extents_static_size_inner_prod, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t   = typename value::first_type;
  using layout_t  = typename value::second_type;

  using dtensor_t = ublas::tensor_dynamic<value_t, layout_t>;

  auto const body = [&](auto const& a, auto const& b){
    auto c = ublas::inner_prod(a, b);
    auto r = std::inner_product(a.begin(),a.end(), b.begin(),value_t(0));
    BOOST_CHECK_EQUAL( c , r );
  };

  for_each_in_tuple(extents_tuple,[&](auto const& /*unused*/, auto & n){
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(n)>>;
    using stensor_t = ublas::tensor_static_rank<value_t, size, layout_t>;
    auto a  = stensor_t(n);
    auto b  = stensor_t(n);
    a = 2;
    b = 3;
    body(a,b);

  });

  for_each_in_tuple(extents_tuple,[&](auto const& I, auto & n){
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(n)>>;
    using stensor_t = ublas::tensor_static_rank<value_t, size, layout_t>;
    auto a  = stensor_t(n);
    auto b  = dtensor_t(extents_vector[I]);
    a = 2;
    b = 1;

    body(a,b);

  });

  for_each_in_tuple(extents_tuple,[&](auto const& I, auto & n){
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(n)>>;
    using stensor_t = ublas::tensor_static_rank<value_t, size, layout_t>;
    auto a  = dtensor_t(extents_vector[I]);
    auto b  = stensor_t(n);
    a = 2;
    b = 1;
    body(a,b);

  });

}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_extents_static_size_outer_prod, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t   = typename value::first_type;
  using layout_t  = typename value::second_type;

  for_each_in_tuple(extents_tuple,[&](auto const& /*unused*/, auto const& n1){
    constexpr auto size1 = std::tuple_size_v<std::decay_t<decltype(n1)>>;
    using tensor_t_1 = ublas::tensor_static_rank<value_t, size1, layout_t>;
    auto a  = tensor_t_1(n1);
    a = 2;
    for_each_in_tuple(extents_tuple,[&](auto const& /*J*/, auto const& n2){
      constexpr auto size2 = std::tuple_size_v<std::decay_t<decltype(n2)>>;
      using tensor_t_2 = ublas::tensor_static_rank<value_t, size2, layout_t>;
      auto b  = tensor_t_2(n2);
      b = 1;
      auto c  = ublas::outer_prod(a, b);

      BOOST_CHECK ( std::all_of(c.begin(),c.end(), [&a,&b](auto cc){return cc == a[0]*b[0];}) );

    });

  });

  for_each_in_tuple(extents_tuple,[&](auto const& I, auto const& /*n1*/){
    using tensor_t_1 = ublas::tensor_dynamic<value_t, layout_t>;
    auto a  = tensor_t_1(extents_vector[I]);
    a = 2;
    for_each_in_tuple(extents_tuple,[&](auto const& /*J*/, auto const& n2){
      constexpr auto size = std::tuple_size_v<std::decay_t<decltype(n2)>>;
      using tensor_t_2 = ublas::tensor_static_rank<value_t, size, layout_t>;
      auto b  = tensor_t_2(n2);
      b = 1;
      auto c  = ublas::outer_prod(a, b);
      BOOST_CHECK ( std::all_of(c.begin(),c.end(), [&a,&b](auto cc){return cc == a[0]*b[0];}) );

//      for(auto const& cc : c)
//        BOOST_CHECK_EQUAL( cc , a[0]*b[0] );
    });

  });

  for_each_in_tuple(extents_tuple,[&](auto const& /*unused*/, auto const& n1){
    constexpr auto size = std::tuple_size_v<std::decay_t<decltype(n1)>>;
    using tensor_t_1 = ublas::tensor_static_rank<value_t, size, layout_t>;
    auto a  = tensor_t_1(n1);
    a = 2;
    for(auto const& n2 : extents_vector){
      using tensor_t_2 = ublas::tensor_dynamic<value_t, layout_t>;
      auto b  = tensor_t_2(n2);
      b = 1;
      auto c  = ublas::outer_prod(a, b);

      BOOST_CHECK ( std::all_of(c.begin(),c.end(), [&a,&b](auto cc){return cc == a[0]*b[0];}) );

//      for(auto const& cc : c)
//        BOOST_CHECK_EQUAL( cc , a[0]*b[0] );
    }

  });

}

BOOST_AUTO_TEST_SUITE_END()