/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2017 by Rokko Developers https://github.com/t-sakashita/rokko
*
* 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)
*
*****************************************************************************/

#ifndef ROKKO_LAPACK_GEQRF_HPP
#define ROKKO_LAPACK_GEQRF_HPP

#include <complex>
#ifdef I
# undef I
#endif
#include <stdexcept>
#include <boost/type_traits/is_same.hpp>
#include <lapacke.h>
#undef I
#include <rokko/traits/value_t.hpp>
#include "complex_cast.hpp"

namespace rokko {
namespace lapack {

namespace {

template<typename T> struct geqrf_dispatch;

template<>
struct geqrf_dispatch<float> {
  template<typename MATRIX, typename VECTOR>
  static lapack_int geqrf(int matrix_layout, lapack_int m, lapack_int n,
                          MATRIX& a, VECTOR& tau) {
    return LAPACKE_sgeqrf(matrix_layout, m, n, storage(a), ld(a), storage(tau));
  }
  template<typename MATRIX, typename VECTOR>
  static lapack_int geqrf(int matrix_layout, lapack_int m, lapack_int n,
                          MATRIX& a, VECTOR& tau, VECTOR& work) {
    return LAPACKE_sgeqrf(matrix_layout, m, n, storage(a), ld(a), storage(tau),
                          storage(work), size(work));
  }
};

template<>
struct geqrf_dispatch<double> {
  template<typename MATRIX, typename VECTOR>
  static lapack_int geqrf(int matrix_layout, lapack_int m, lapack_int n,
                          MATRIX& a, VECTOR& tau) {
    return LAPACKE_dgeqrf(matrix_layout, m, n, storage(a), ld(a), storage(tau));
  }
  template<typename MATRIX, typename VECTOR>
  static lapack_int geqrf(int matrix_layout, lapack_int m, lapack_int n,
                          MATRIX& a, VECTOR& tau, VECTOR& work) {
    return LAPACKE_dgeqrf(matrix_layout, m, n, storage(a), ld(a), storage(tau),
                          storage(work), size(work));
  }
};

template<>
struct geqrf_dispatch<std::complex<float>> {
  template<typename MATRIX, typename VECTOR>
  static lapack_int geqrf(int matrix_layout, lapack_int m, lapack_int n,
                          MATRIX& a, VECTOR& tau) {
    return LAPACKE_cgeqrf(matrix_layout, m, n, complex_cast(storage(a)), ld(a),
                          complex_cast(storage(tau)));
  }
  template<typename MATRIX, typename VECTOR>
  static lapack_int geqrf(int matrix_layout, lapack_int m, lapack_int n,
                          MATRIX& a, VECTOR& tau, VECTOR& work) {
    return LAPACKE_cgeqrf(matrix_layout, m, n, complex_cast(storage(a)), ld(a),
                          complex_cast(storage(tau)), complex_cast(storage(work)),
                          size(work));
  }
};

template<>
struct geqrf_dispatch<std::complex<double>> {
  template<typename MATRIX, typename VECTOR>
  static lapack_int geqrf(int matrix_layout, lapack_int m, lapack_int n,
                          MATRIX& a, VECTOR& tau) {
    return LAPACKE_zgeqrf(matrix_layout, m, n, complex_cast(storage(a)), ld(a),
                          complex_cast(storage(tau)));
  }
  template<typename MATRIX, typename VECTOR>
  static lapack_int geqrf(int matrix_layout, lapack_int m, lapack_int n,
                          MATRIX& a, VECTOR& tau, VECTOR& work) {
    return LAPACKE_zgeqrf(matrix_layout, m, n, complex_cast(storage(a)), ld(a),
                          complex_cast(storage(tau)), complex_cast(storage(work)),
                          size(work));
  }
};

}

template<typename MATRIX, typename VECTOR>
lapack_int geqrf(MATRIX& a, VECTOR& tau) {
  static_assert(std::is_same<value_t<MATRIX>, value_t<VECTOR>>::value, "");
  lapack_int m = rows(a);
  lapack_int n = cols(a);
  lapack_int r = std::min(m, n);
  if (size(tau) != r)
    throw std::invalid_argument("vector tau size mismatch");
  return geqrf_dispatch<value_t<MATRIX>>
    ::geqrf((is_col_major(a) ? LAPACK_COL_MAJOR : LAPACK_ROW_MAJOR), m, n, a, tau);
}

template<typename MATRIX, typename VECTOR>
lapack_int geqrf(MATRIX& a, VECTOR& tau, VECTOR& work) {
  static_assert(std::is_same<value_t<MATRIX>, value_t<VECTOR>>::value, "");
  lapack_int m = rows(a);
  lapack_int n = cols(a);
  lapack_int r = std::min(m, n);
  if (size(tau) != r)
    throw std::invalid_argument("vector tau size mismatch");
  if (size(work) < std::max(1, n))
    throw std::invalid_argument("vector work size mismatch");
  return geqrf_dispatch<value_t<MATRIX>>
    ::geqrf((is_col_major(a) ? LAPACK_COL_MAJOR : LAPACK_ROW_MAJOR), m, n, a, tau, work);
}

} // end namespace lapack
} // end namespace rokko

#endif // ROKKO_LAPACK_GEQRF_HPP


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

// Note that this file contains quickbook mark-up as well as code
// and comments, don't change any of the special comment mark-ups!

//[policy_ref_snip2

#include <boost/math/distributions/normal.hpp>

using namespace boost::math::policies;
using namespace boost::math;

// Define a policy:
typedef policy<
      overflow_error<ignore_error>
      > my_policy;

// Define the distribution:
typedef normal_distribution<double, my_policy> my_norm;

// Get a quantile:
double q = quantile(my_norm(), 0.05);

//]

#include <iostream>

int main()
{
   std::cout << q << std::endl;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.
//
// Algorithm inspired by Nick Vannieuwenhoven, written by Cornelius Steinhardt

/*
 * The IMF preconditioner.
 *
 * References
 * 	[1] N. Vannieuwenhoven and K. Meerbergen, IMF: An incomplete multifron-
 *		tal LU-factorization for element-structured sparse linear systems,
 *		Tech. Rep. TW581, Department of Computer Science, KULeuven,
 *		December 2010.
 */

#ifndef MTL_IMF_PRECONDITIONER_INCLUDE
#define MTL_IMF_PRECONDITIONER_INCLUDE

#include <boost/numeric/itl/pc/matrix_algorithms.hpp>
#include <boost/numeric/itl/pc/solver.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/coordinate2D.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>

namespace itl {   namespace pc {

/// The IMF preconditioner, as described in [1].
template<class ValType>
class imf_preconditioner {

public:
	/// The type of the values.
	typedef ValType value_type;

	/// The type of the sparse data structure for the upper matrices.
	typedef mtl::mat::coordinate2D<value_type> sparse_type_upper;

	/// The type of the sparse data structure for the lower matrices.
	typedef mtl::mat::coordinate2D<value_type> sparse_type_lower;

	/// The type of the vectors.
	typedef mtl::dense_vector<value_type> vector_type;

	///The type of the permutation vector.
	typedef mtl::dense_vector<int>  index_type;

	/// The type of the matrices on the block diagonal.
	typedef mtl::mat::dense2D<value_type>  block_type;

	/// The type of the sequence of lower matrices.
	typedef std::vector<mtl::mat::compressed2D<value_type> > lower_matrix_coll_type;

	/// The type of the sequence of upper matrices.
	typedef std::vector<mtl::mat::compressed2D<value_type> > upper_matrix_coll_type;

	/// Constructor
	template< class ElementStructure >
	imf_preconditioner(
		const ElementStructure& element_structure ,
  		const int maxlofi=0,
		const bool copy_on=true
	)
	: 	m_nb_vars( element_structure.get_total_vars() ),
	  	m_ordering( element_structure.get_total_vars() ),
	  	m_diagonal_index(0),
	  	m_diagonal(0)
	{
 		mtl::vampir_trace<5053> tracer;
		if(copy_on){
		  ElementStructure es(element_structure);
		  factor(es, maxlofi);
		} else {
		  factor(element_structure, maxlofi);
		}
		P= permutation(m_ordering);
	}

	/// Destructor
	~imf_preconditioner()
        {
	  delete[] m_diagonal_index;
	  delete[] m_diagonal;
	}

private:
	/// Disallow the copy constructor and assignment.
	imf_preconditioner();
	imf_preconditioner(const imf_preconditioner& other);
	void operator=(const imf_preconditioner& other);

        /// Constructs the IMF preconditioner.Forward declaration
	template< class ElementStructure >
  	void factor(const ElementStructure&, const int);

public:

	/// Returns the number of levels (equals the number of lower and upper matrices.
	int get_nb_levels() const { return m_levels; }

	/// Returns the number of blocks on the diagonal.
	int get_nb_blocks() const { return m_nb_blocks; }

        /// Applies the preconditioner to the given matrix.
        template <typename VectorIn, typename VectorOut>
        void solve(const VectorIn& b, VectorOut& x) const
        {
	    VectorIn m(trans(P)*b), m_tmp(imf_apply(m));
	    x= P * m_tmp;
        }

private:
        /// Applies the preconditioner   prototype
	template< class Vector >
	Vector imf_apply(const Vector&) const;

	/// The number of variables (also the size of the m_ordering vector).
	unsigned int m_nb_vars;

	/// The number of blocks on the diagonal.
	unsigned int m_nb_blocks;

	/// A vector containing the renumbering of IMF.
	index_type m_ordering;

	/// A matrix containing the renumbering of IMF.
        mtl::mat::traits::permutation<>::type P;

	/// The number of levels (equals the number of entries in the diagonal index array minus one).
	int m_levels;

	 /** The index array for the matrices on the block diagonal. The i^th entry
	 * indicates where the i^th level of block diagonal matrices starts in the
	 * right hand side vector. */
	int* m_diagonal_index;

        /// The matrices on the block diagonal.
	block_type* m_diagonal;

	/// The sparse lower matrices of each level, sorted by level.
	lower_matrix_coll_type m_lower;

	/// The sparse upper matrices of each level, sorted by level.
	upper_matrix_coll_type m_upper;
};

/// Solve
template <typename Matrix, typename Vector>
solver<imf_preconditioner<Matrix>, Vector, false>
inline solve(const imf_preconditioner<Matrix>& P, const Vector& b)
{
    mtl::vpt::vampir_trace<5054> tracer;
    return solver<imf_preconditioner<Matrix>, Vector, false>(P, b);
}
}//namespace pc
}//namespace itl
#endif // MTL_IMF_PRECONDITIONER_INCLUDE


// Copyright 2015-2021 Tier IV, Inc. All rights reserved.
//
// 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.
//
// Co-developed by Tier IV, Inc. and Robotec.AI sp. z o.o.

#ifndef RANDOM_TEST_RUNNER__GOAL_REACHED_METRIC_H
#define RANDOM_TEST_RUNNER__GOAL_REACHED_METRIC_H

#include <boost/optional.hpp>
#include <iostream>

#include "geometry_msgs/msg/pose.hpp"
#include "random_test_runner/data_types.hpp"
#include "traffic_simulator_msgs/msg/entity_status.hpp"

class GoalReachedMetric
{
public:
  bool isGoalReached(const traffic_simulator_msgs::msg::EntityStatus & status)
  {
    if (!goal_pose_) {
      return false;
    }

    geometry_msgs::msg::Point current_position = status.pose.position;
    geometry_msgs::msg::Point goal_position = goal_pose_->position;

    if (distance(current_position, goal_position) < goal_reaching_accuracy_threshold_) {
      return true;
    }

    return false;
  }

  void setGoal(const geometry_msgs::msg::Pose & goal_pose) { goal_pose_ = goal_pose; }

private:
  const double goal_reaching_accuracy_threshold_ = 3.0;

  boost::optional<geometry_msgs::msg::Pose> goal_pose_;

  static double distance(
    const geometry_msgs::msg::Point & lhs, const geometry_msgs::msg::Point & rhs)
  {
    geometry_msgs::msg::Point diff;
    diff.x = lhs.x - rhs.x;
    diff.y = lhs.y - rhs.y;
    diff.z = lhs.z - rhs.z;
    return std::sqrt(diff.x * diff.x + diff.y * diff.y + diff.z * diff.z);
  }
};

#endif  // RANDOM_TEST_RUNNER__GOAL_REACHED_METRIC_H


#include <iostream>
//#include <boost/multiprecision/cpp_int.hpp>
//using namespace boost::multiprecision;
const int mx = 1e6 + 5;
const long int inf = 2e9;
typedef long long ll;
#define rep(i, n) for (i = 0; i < n; i++)
#define repp(i, a, b) for (i = a; i <= b; i++)
#define pii pair<int, int>
#define vpii vector<pii>
#define vi vector<int>
#define vll vector<ll>
#define r(x) scanf("%d", &x)
#define rs(s) scanf("%s", s)
#define gc getchar_unlocked
#define pc putchar_unlocked
#define mp make_pair
#define pb push_back
#define lb lower_bound
#define ub upper_bound
#define endl "\n"
#define fast                          \
	ios_base::sync_with_stdio(false); \
	cin.tie(NULL);                    \
	cout.tie(NULL);
using namespace std;
// void in(int &x)
// {
// 	register int c = gc();
// 	x = 0;
// 	int neg = 0;
// 	for (; ((c < 48 || c > 57) && c != '-'); c = gc())
// 		;
// 	if (c == '-')
// 	{
// 		neg = 1;
// 		c = gc();
// 	}
// 	for (; c > 47 && c < 58; c = gc())
// 	{
// 		x = (x << 1) + (x << 3) + c - 48;
// 	}
// 	if (neg)
// 		x = -x;
// }
// void out(int n)
// {
// 	int N = n, rev, count = 0;
// 	rev = N;
// 	if (N == 0)
// 	{
// 		pc('0');
// 		return;
// 	}
// 	while ((rev % 10) == 0)
// 	{
// 		count++;
// 		rev /= 10;
// 	}
// 	rev = 0;
// 	while (N != 0)
// 	{
// 		rev = (rev << 3) + (rev << 1) + N % 10;
// 		N /= 10;
// 	}
// 	while (rev != 0)
// 	{
// 		pc(rev % 10 + '0');
// 		rev /= 10;
// 	}
// 	while (count--)
// 		pc('0');
// }
ll parent[mx], arr[mx], node, edge;
vector<pair<ll, pair<ll, ll>>> v;
void initial()
{
	int i;
	rep(i, node + edge)
		parent[i] = i;
}
int root(int i)
{
	while (parent[i] != i)
	{
		parent[i] = parent[parent[i]];
		i = parent[i];
	}
	return i;
}
void join(int x, int y)
{
	int root_x = root(x); //Disjoint set union by rank
	int root_y = root(y);
	parent[root_x] = root_y;
}

/****************main kruskal function************************/
ll kruskal()
{
	ll mincost = 0, i, x, y;
	rep(i, edge)
	{
		x = v[i].second.first;
		y = v[i].second.second;
		if (root(x) != root(y))
		{
			mincost += v[i].first;
			join(x, y);
		}
	}
	return mincost;
}
int main()
{
	fast;
	while (1)
	{
		int i, j, from, to, cost, totalcost = 0;
		cin >> node >> edge; //Enter the nodes and edges
		if (node == 0 && edge == 0)
			break; //Enter 0 0 to break out
		initial(); //Initialise the parent array
		rep(i, edge)
		{
			cin >> from >> to >> cost;
			v.pb(mp(cost, mp(from, to)));
			totalcost += cost;
		}
		sort(v.begin(), v.end());
		// rep(i,v.size())
		// 	cout<<v[i].first<<"  ";
		cout << kruskal() << endl;
		v.clear();
	}
	return 0;
}

/********************My Own Implementation for CP ****************************/

// O(ElogE) or O(ElogV). Sorting of edges takes O(ELogE) time.
// After sorting, we iterate through all edges and apply the
// find-union algorithm. The find and union operations can
// take at most O(LogV) time. So overall complexity is
// O(ELogE + ELogV) time. The value of E can be at most O(V2),
// so O(LogV) is O(LogE) the same. Therefore,
// the overall time complexity is O(ElogE) or O(ElogV)

// Kruskal O(ElogE) works by sorting all the edges in the graph first;
// thus for a sparse graph this is indeed better than Prims..
// Prims O(E.logV) on the other hand doesn't need this
// sorting of edges. It works by adding a unique
// node to resultant set in each step. For a denser
// graph, this is certainly better than Kruskal..

#include<bits/stdc++.h>
using namespace std;
map<int,vector<pair<int,int>>> mp;
int parent[10001];
int r_ank[10001];
int f_ind(int a)          //path compression...
{
    if(parent[a]<0)
        return(a);
    return(parent[a]=f_ind(parent[a]));
}

void uni_on(int a,int b)           // union by rank..
{
    if(r_ank[a]>r_ank[b])
    {
        r_ank[a]+=r_ank[b];
        parent[b]=a;
    }
    else
    {
        r_ank[b]+=r_ank[a];
        parent[a]=b;
    }
}

struct edge
{
    int a;
    int b;
    int w;
};

edge edgelist[100001];
bool comp(edge a,edge b)
{
    if(a.w<b.w)
        return true;
    return false;
}
int main()
{
    int n,m,u,v,w,x;
    cin>>n>>m;

     memset(parent,-1,sizeof(parent));
     memset(r_ank,1,sizeof(r_ank));
      mp.clear();

    for(int i=0;i<m;i++)
    {
      //  cin>>edgelist[i].a>>edgelist[i].b>>edgelist[i].w;
      cin>>u>>v>>w;
      mp[w].push_back({u,v});
    }

  //  sort(edgelist,edgelist+m,comp);        // sorting user defined quantities...
    int sum=0;
    for(int i=1;i<1000;i++)
    {
        if(mp[i].empty()) continue;

      while(!mp[i].empty())                                   // this is kruskals algo being implemented...
      {
          int f=mp[i][mp[i].size()-1].first;   // vertex 1
        int s=mp[i][mp[i].size()-1].second;    // vertex 2
         mp[i].pop_back();
        if(f_ind(f)!=f_ind(s))
        {
            uni_on(f_ind(f),f_ind(s));
            sum+=i;
        }

       }

}cout<<sum<<"\n";
}

/******************** A more clear implementation ******************************/

const int MAX = 1e4 + 5;
int id[MAX], nodes, edges;
pair <long long, pair<int, int> > p[MAX];

void initialize()
{
    for(int i = 0;i < MAX;++i)
        id[i] = i;
}

int root(int x)
{
    while(id[x] != x)
    {
        id[x] = id[id[x]];
        x = id[x];
    }
    return x;
}

void union1(int x, int y)
{
    int p = root(x);
    int q = root(y);
    id[p] = id[q];
}

long long kruskal(pair<long long, pair<int, int> > p[])
{
    int x, y;
    long long cost, minimumCost = 0;
    for(int i = 0;i < edges;++i)
    {
        // Selecting edges one by one in increasing order from the beginning
        x = p[i].second.first;
        y = p[i].second.second;
        cost = p[i].first;
        // Check if the selected edge is creating a cycle or not
        if(root(x) != root(y))
        {
            minimumCost += cost;
            union1(x, y);
        }
    }
    return minimumCost;
}

int main()
{
    int x, y;
    long long weight, cost, minimumCost;
    initialize();
    cin >> nodes >> edges;
    for(int i = 0;i < edges;++i)
    {
        cin >> x >> y >> weight;
        p[i] = make_pair(weight, make_pair(x, y));
    }
    // Sort the edges in the ascending order
    sort(p, p + edges);
    minimumCost = kruskal(p);
    cout << minimumCost << endl;
    return 0;
}

/*************************************************************************/


#ifndef UnitConvert_UnitRegistry_hpp
#define UnitConvert_UnitRegistry_hpp

/** @file UnitRegistry.hpp
 * @brief
 * @author C.D. Clark III
 * @date 09/03/18
 */

#include <iostream>
#include <map>

#include <boost/spirit/include/phoenix_bind.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/qi.hpp>
namespace spt = boost::spirit;
namespace qi = boost::spirit::qi;
namespace phx = boost::phoenix;

#include "./Unit.hpp"

namespace UnitConvert
{
template <typename T>
class Quantity;

/**
 * A class for storing units according to their string representation.
 */
class UnitRegistry
{
 public:
  typedef std::pair<std::string, Unit> PairType;
  typedef std::map<std::string, Unit> StoreType;

 protected:
  StoreType m_UnitStore;

 public:
  enum class EXISTING_UNIT_POLICY { Warn, Throw, Ignore, Overwrite };
  EXISTING_UNIT_POLICY existing_unit_policy = EXISTING_UNIT_POLICY::Throw;

  /**
   * Return the number of entries in unit registry. This will not include
   * prefixed versions of all units that are implicitly defined, only prefixed
   * versions that have been explicitly added.
   */
  size_t size() const;

  /**
   * Add a base unit for a given dimension to the registry. Base units have a
   * scale of one. This function takes a template argument that specifies the
   * dimension. A base unit for the dimension will be created and added to the
   * registry under the name specified by the string argument.
   */
  template <Dimension::Name DIM>
  void addBaseUnit(const std::string& k);

  /**
   * Add a unit to the registry. This overload takes a string, which is the
   * name/symbol that will be added to the database, and a Unit instance, which
   * contains the scale, offset, and dimension that will be added.
   */
  void addUnit(const std::string& k, const Unit& v);

  /**
   * Add a unit to the registry. This overload will parse a unit equation,
   * which defines some unit in terms of other units already in the registry,
   * and add a unit to the registry. For example:
   *
   * addUnit("J = kg m^2 / s^2")
   *
   * will a unit "J" to the registry by building the unit defined by "kg m^2 /
   * s^2".
   */
  void addUnit(std::string unit_equation);

  /**
   * Load a set of units from a stream by reading lines from the stream and
   * passing them to the addUnit(std::string) function.
   */
  void loadUnits(std::istream& in);
  /**
   * Load a set of units from a text file. The text file should contain lines
   * that could be passed to the addUnit(std::string) function.
   */
  void loadUnits(std::string filename);

  /**
   * Querry the registry for a named unit and return
   * the unit it represents. If the named unit is not
   * found in the registry, an exception is thrown.
   */
  const Unit& getUnit(std::string a_unit) const;

  /**
   * Querry the registry for a named unit and return
   * a copy of the unit it represents. If unit isn't found
   * and a_trySIPrefix is true, then the function
   * checks if unit is an SI prefix'ed version of a unit
   * in the registry. If no unit is
   * found in the registry, an exception is thrown.
   */
  Unit getUnit(std::string a_unit, bool a_trySIPrefix) const;

  /**
   * Parse a string and return the unit it represents.
   *
   * All units in the string must already exist in the unit registry,
   * but the unit itself does not have to be.
   *
   * Example:
   *
   * 'kg m^2 / s^2'
   *
   * as long as 'g', 'm', and 's' are in the registry, this derived
   * unit can be built and returned.
   */
  Unit makeUnit(std::string a_unit) const;

  /**
   * Create a Quantity from a value and unit string. The string is parsed to
   * create a Unit, which is then used to create a quantity.
   */
  template <typename T>
  Quantity<T> makeQuantity(const T& val, const std::string& a_unit) const;

  /**
   * Create a Quantity from a string. This function first parses the string into
   * a value and unit string and then calls the version above.
   *
   * This function is useful for reading user input into a quantity.
   */
  template <typename T>
  Quantity<T> makeQuantity(std::string a_unit) const;

  /** Output operator.
   */
  friend std::ostream& operator<<(std::ostream& out, const UnitRegistry& reg);

  /**
   * Symbol table parser to get exponent for all SI prefixes.
   */
  struct SIPrefixParser : qi::symbols<char, int> {
    SIPrefixParser()
    {
      add("Y", 24)("yotta", 24)("Z", 21)("zetta", 21)("E", 18)("exa", 18)(
          "P", 15)("peta", 15)("T", 12)("tera", 12)("G", 9)("giga", 9)("M", 6)(
          "mega", 6)("k", 3)("kilo", 3)("h", 2)("hecto", 2)("da", 1)("deca", 1)(
          "d", -1)("deci", -1)("c", -2)("centi", -2)("m", -3)("milli", -3)(
          "u", -6)("micro", -6)("n", -9)("nano", -9)("p", -12)("pico", -12)(
          "f", -15)("femto", -15)("a", -18)("atto", -18)("z", -21)(
          "zepto", -21)("y", -24)("yocto", -24);
    }
  };

  /**
   * A Boost.Spirit grammar for parsing unit strings.
   */
  struct UnitParser : spt::qi::grammar<std::string::iterator, Unit()> {
    using Iterator = std::string::iterator;
    using ThisType = UnitParser;

    qi::rule<Iterator, Unit()> named_unit, factor, term, group, scale,
        expression, unit;
    qi::rule<Iterator, double()> offset;
    qi::rule<Iterator, int()> exponent;
    qi::rule<Iterator> mul, div, pow, add, sub;
    qi::rule<Iterator, char()> unit_name_begin_chars, unit_name_other_chars;

    const UnitRegistry& ureg;

    /**
     * compute a unit raised to an integer power)
     */
    Unit exponentiate(const Unit& b, const int e);

    /**
     * retrieve a named unit from the registry
     */
    Unit getUnitFromRegistry(const std::string& unit);

    UnitParser(const UnitRegistry& registry);
  };

  /**
   * Symbol table parser to return base dimension for dimension symbols.
   */
  struct BaseDimensionSymbolParser : qi::symbols<char, Dimension> {
    BaseDimensionSymbolParser()
    {
      add("L", Dimension::Name::Length)("M", Dimension::Name::Mass)(
          "T", Dimension::Name::Time)("I", Dimension::Name::ElectricalCurrent)(
          "THETA", Dimension::Name::Temperature)("N", Dimension::Name::Amount)(
          "J", Dimension::Name::LuminousIntensity)(
          "1", Dimension::Name::Dimensionless);
    }
  };

  /**
   * A Boost.Spirit grammar for parsing dimension strings.
   */
  struct DimensionParser
      : spt::qi::grammar<std::string::iterator, Dimension()> {
    using Iterator = std::string::iterator;
    using ThisType = DimensionParser;

    BaseDimensionSymbolParser base_dimension_symbol;
    qi::rule<Iterator, Dimension()> factor, term, group, dimension;
    qi::rule<Iterator, int()> exponent;
    qi::rule<Iterator> mul, div, pow, add, sub;

    /**
     * compute a dimension raised to an integer power)
     */
    Dimension exponentiate(const Dimension& b, const int e);

    DimensionParser();
  };

 protected:
  UnitParser m_UnitParser;
  SIPrefixParser m_SIPrefixParser;
  DimensionParser m_DimensionParser;

 public:
  UnitRegistry() : m_UnitParser(*this){};

  /**
   * Returns a reference to the registry's unit parser.
   */
  const UnitParser& getUnitParser() const { return m_UnitParser; }
};

template <Dimension::Name DIM>
void UnitRegistry::addBaseUnit(const std::string& k)
{
  this->addUnit(k, BaseUnit<DIM>());
}

template <typename T>
Quantity<T> UnitRegistry::makeQuantity(const T& val,
                                       const std::string& a_unit) const
{
  return Quantity<T>(val, makeUnit(a_unit), this);
}

template <typename T>
Quantity<T> UnitRegistry::makeQuantity(std::string a_quantity) const
{
  double value = 0;
  std::string unit;
  // parse quantity string
  // a quantity should have a numerical value and a unit that makeUnit will
  // parse
  auto it = a_quantity.begin();
  auto r = qi::parse(it, a_quantity.end(),
                     -qi::double_ >> qi::as_string[+qi::char_], value, unit);
  if(!r)
  {
    throw std::runtime_error(
        "ERROR: Could not parse string to form a quantity: "+ a_quantity+
        "\nQuantity strings should be a numerical value followed by a valid unit string..");

  }
  return Quantity<T>(static_cast<T>(value), makeUnit(unit), this);
}
}  // namespace UnitConvert

#endif  // include protector


#include "problems/transx_problem.h"

#include <cmath>
#include <iostream>
#include <numeric>
#include <string>
#include <vector>

#include <boost/math/constants/constants.hpp>
#include <boost/array.hpp>

#include <keplerian_toolbox/core_functions/array3D_operations.h>
#include <keplerian_toolbox/core_functions/fb_vel.h>

namespace pagmo { namespace problem {

transx_problem::transx_problem(const std::vector<kep_toolbox::planet::planet_ptr> seq,
    const double dep_altitude, const double arr_altitude, const bool circularize,
    const int dim, const int obj_dim) : base(dim, 0, obj_dim, 0, 0, 0.0), m_n_legs(seq.size() - 1), m_arr_altitude(arr_altitude), m_dep_altitude(dep_altitude), m_circularize(circularize)
{
  std::vector<double> mus(seq.size());
  for (std::vector<kep_toolbox::planet::planet_ptr>::size_type i = 0; i < seq.size(); ++i) {
    mus[i] = seq[i]->get_mu_central_body();
  }
  if ((unsigned int)std::count(mus.begin(), mus.end(), mus[0]) != mus.size()) {
    pagmo_throw(value_error, "The planets do not all have the same mu_central_body");
  }

  for (std::vector<kep_toolbox::planet::planet_ptr>::size_type i = 0; i < seq.size(); ++i) {
    m_seq.push_back(seq[i]->clone());
  }

  m_common_mu = seq[0]->get_mu_central_body();
  m_max_deltaV = 20000.0;
}

transx_problem::transx_problem(const transx_problem &p) : base(p.get_dimension(), 0, p.get_f_dimension(), 0, 0, 0, 0), m_seq(), m_common_mu(p.m_common_mu), m_n_legs(p.m_n_legs), m_circularize(p.m_circularize), m_arr_altitude(p.m_arr_altitude), m_dep_altitude(p.m_dep_altitude) {
  for (std::vector<kep_toolbox::planet::planet_ptr>::size_type i = 0; i < p.m_seq.size(); ++i) {
    m_seq.push_back(p.m_seq[i]->clone());
  }

  set_bounds(p.get_lb(),p.get_ub());
}

base_ptr transx_problem::clone() const {
  return base_ptr(new transx_problem(*this));
}
void transx_problem::transx_time_info(TransXTimes *times, std::vector<kep_toolbox::planet::planet_ptr> planets, std::vector<kep_toolbox::epoch> time_list) const {
  for (int i = 0; i < planets.size(); ++i) {
    times->add_planets(planets[i]->get_name());
    times->add_times(time_list[i].mjd());
  }
}

void transx_problem::transx_escape(TransXEscape *escape, kep_toolbox::planet::planet_ptr ref, kep_toolbox::array3D V_ref, kep_toolbox::array3D R_ref, kep_toolbox::array3D deltaV, double eject_T) const {

  kep_toolbox::array3D dVTransX = velocity_to_transx(V_ref, R_ref, deltaV);
  double dv = kep_toolbox::norm(deltaV);

  escape->set_planet(ref->get_name());
  escape->set_mjd(eject_T);

  escape->set_prograde(dVTransX[0]);
  escape->set_outward(dVTransX[1]);
  escape->set_plane(dVTransX[2]);

  escape->set_vinf(dv);

  escape->set_burn(burn_cost(ref, deltaV, false, true));
}

void transx_problem::transx_dsm(TransXDSM * dsm, kep_toolbox::array3D V_ref, kep_toolbox::array3D R_ref, kep_toolbox::array3D deltaV, kep_toolbox::array3D v, double dsm_T, int leg) const {
  kep_toolbox::array3D dVTransX = velocity_to_transx(V_ref, R_ref, deltaV);
  double dv = kep_toolbox::norm(deltaV);

  dsm->set_mjd(dsm_T);

  dsm->set_prograde(dVTransX[0]);
  dsm->set_outward(dVTransX[1]);
  dsm->set_plane(dVTransX[2]);

  dsm->set_vinf(kep_toolbox::norm(v));

  dsm->set_burn(dv);

  dsm->set_leg(leg);
}

void transx_problem::transx_flyby(TransXFlyBy *flyby, kep_toolbox::planet::planet_ptr ref, kep_toolbox::array3D V_ref, kep_toolbox::array3D R_ref, kep_toolbox::array3D Vin, kep_toolbox::array3D Vout, double enc_T) const {
  kep_toolbox::array3D VoutTransX = velocity_to_transx(V_ref, R_ref, Vout);
  double vx = VoutTransX[0];
  double vy = VoutTransX[1];
  double vz = VoutTransX[2];

  flyby->set_planet( ref->get_name() );
  flyby->set_mjd( enc_T );

  flyby->set_approach_vel( sqrt(kep_toolbox::dot(Vin, Vin)) );
  flyby->set_departure_vel( sqrt(kep_toolbox::dot(Vout, Vout)) );

  flyby->set_outward_angle( 180 * atan2(vy, vx) / M_PI );
  flyby->set_inclination( 180 * atan2(vz, sqrt(vx * vx + vy * vy)) / M_PI );

  double ta  = acos(kep_toolbox::dot(Vin, Vout)/sqrt(kep_toolbox::dot(Vin,Vin))/sqrt(kep_toolbox::dot(Vout,Vout)));
  flyby->set_turning_angle( ta * 180.0 / M_PI );

  double alt = (ref->get_mu_self() / kep_toolbox::dot(Vin,Vin)*(1/sin(ta/2)-1) - ref->get_radius())/1000;
  flyby->set_periapsis_altitude( alt );

  double enc_vel = 0.0;
  kep_toolbox::fb_vel(enc_vel, Vin, Vout, *ref);
  flyby->set_burn(enc_vel);
}

void transx_problem::transx_arrival(TransXArrival *arrival, kep_toolbox::planet::planet_ptr ref, kep_toolbox::array3D V_exc, double enc_T) const {

  arrival->set_planet(ref->get_name());
  arrival->set_mjd(enc_T);
  arrival->set_vinf(kep_toolbox::norm(V_exc));
  arrival->set_burn(burn_cost(ref, V_exc, true, m_circularize));
}

kep_toolbox::array3D transx_problem::velocity_to_transx(kep_toolbox::array3D v_ref, kep_toolbox::array3D v_rad, kep_toolbox::array3D v) const {

  kep_toolbox::array3D fward;
  kep_toolbox::array3D plane;
  kep_toolbox::array3D oward;

  kep_toolbox::vers(fward, v_ref);
  kep_toolbox::cross(plane, v_ref, v_rad);
  kep_toolbox::vers(plane, plane);
  kep_toolbox::cross(oward, plane, fward);

  kep_toolbox::array3D res;
  res[0] = kep_toolbox::dot(fward, v);
  res[1] = kep_toolbox::dot(oward, v);
  res[2] = kep_toolbox::dot(plane, v);

  return res;
}

double transx_problem::burn_cost(kep_toolbox::planet::planet_ptr ref, const kep_toolbox::array3D &exc, bool arr, bool circ) const {
  double mu = ref->get_mu_self();
  double r = ref->get_radius() + (arr ? m_arr_altitude : m_dep_altitude) * 1000;
  double norm_sqr = kep_toolbox::dot(exc, exc);
  return sqrt(norm_sqr + 2 * mu / r) - sqrt((circ ? 1 : 2) * mu / r);
}

void transx_problem::calc_objective(fitness_vector &f, const decision_vector &x, bool should_print, TransXSolution * solution) const {
  f[0] = 0;
  if (get_f_dimension() == 2) {
    f[1] = 0;
  }
}

void transx_problem::objfun_impl(fitness_vector &f, const decision_vector &x) const {
  calc_objective(f, x);
}

void transx_problem::fill_solution(TransXSolution * solution, const decision_vector &x, bool extended_output) const {
  fitness_vector f(get_f_dimension());
  calc_objective(f, x, true, solution);
  for (int i =0; i < x.size(); ++i) {
    solution->add_x(x[i]);
  }
  solution->set_problem(get_name());
}

std::string transx_problem::get_name() const {
  return "TransX base problem";
}

std::string transx_problem::human_readable_extra() const {
  return "TransX base problem - Do not use, subclass";
}

}} // namespaces

BOOST_CLASS_EXPORT_IMPLEMENT(pagmo::problem::transx_problem)


//  Copyright 2020 John Buonagurio
//
//  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

#pragma once

#include <xll/config.hpp>

#include <xll/detail/array_adaptor.hpp>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/storage.hpp>

/**
 * \file fp12.hpp
 * FP12 structure from XLCALL.H adapted to C++.
 */

namespace xll {

/// Original C-style variable length struct.

struct fp12
{
    int32_t rows; // INT32
    int32_t columns; // INT32
    double array[1];
};

/// uBLAS dense matrix adapted to the storage layout of the FP12 structure from
/// XLCALL.H: INT32 rows, INT32 columns, double array[1].
///
/// A::size_type size1_;
/// A::size_type size2_;
/// A data_;

template<std::size_t N>
using static_fp12_storage = detail::static_array<double, N, int32_t>;

template<std::size_t N = 1>
using static_fp12 = boost::numeric::ublas::matrix<double,
    boost::numeric::ublas::row_major,
    static_fp12_storage<N>>;

} // namespace xll


/* --------------------------------------------------------------------------
*
* (C) Copyright …
*
* ---------------------------------------------------------------------------
*/

/*!
 * @file CeresAdjustment.cpp
 * @date 06/06/2018
 * @author Alessandro Bianco
 */

/*!
 * @addtogroup DFNsTest
 *
 * Unit Test for the DFN implementation CeresAdjustment.
 *
 *
 * @{
 */

/* --------------------------------------------------------------------------
 *
 * Includes
 *
 * --------------------------------------------------------------------------
 */
#include <catch.hpp>
#include <BundleAdjustment/CeresAdjustment.hpp>
#include <Converters/MatToFrameConverter.hpp>
#include <Eigen/Dense>

using namespace CDFF::DFN::BundleAdjustment;
using namespace Converters;
using namespace CorrespondenceMap2DWrapper;
using namespace PoseWrapper;
using namespace BaseTypesWrapper;
using namespace PointCloudWrapper;

/* --------------------------------------------------------------------------
 *
 * Test Cases
 *
 * --------------------------------------------------------------------------
 */
TEST_CASE( "Call to process (Ceres Adjustment) and fail", "[failprocess]" )
{
	//Initialize Inputs
	CorrespondenceMaps2DSequencePtr correspondenceMapsSequence = NewCorrespondenceMaps2DSequence();
	for(int index = 0; index < 6; index++)
		{
		CorrespondenceMap2DPtr correspondenceMap = NewCorrespondenceMap2D();
		AddCorrespondenceMap(*correspondenceMapsSequence, *correspondenceMap);
		}

	// Instantiate DFN
	CeresAdjustment* ceres = new CeresAdjustment;

	// Send input data to DFN
	ceres->correspondenceMapsSequenceInput(*correspondenceMapsSequence);

	// Run DFN
	ceres->process();

	// Query output data from DFN
	const Poses3DSequence& output = ceres->posesSequenceOutput();
	bool success = ceres->successOutput();

	REQUIRE(!success);

	// Cleanup
	delete(ceres);
}

TEST_CASE( "Call to configure (Ceres Adjustment)", "[configure]" )
{
	// Instantiate DFN
	CeresAdjustment* ceres = new CeresAdjustment;

	// Setup DFN
	ceres->setConfigurationFile("../tests/ConfigurationFiles/DFNs/BundleAdjustment/CeresAdjustment_Conf1.yaml");
	ceres->configure();

	// Cleanup
	delete(ceres);
}

#define EPSILON 0.001
#define GROUND_TRUTH_EPSILON 0.01
#define CLOSE_GROUND_TRUTH(a, b) (b > a - GROUND_TRUTH_EPSILON && b < a + GROUND_TRUTH_EPSILON)
#define CLOSE(a, b) (b > a - EPSILON && b < a + EPSILON)
#define CLOSE_POSITION(pose, x, y, z) ( CLOSE(GetXPosition(pose), x) && CLOSE(GetYPosition(pose), y) && CLOSE(GetZPosition(pose), z))
#define CLOSE_HALF_ORIENTATION(pose, x, y, z, w) ( CLOSE(GetXOrientation(pose), x) && CLOSE(GetYOrientation(pose), y) && CLOSE(GetZOrientation(pose), z) && CLOSE(GetWOrientation(pose), w))
#define CLOSE_ORIENTATION(pose, x, y, z, w) ( CLOSE_HALF_ORIENTATION(pose, x, y, z, w) || CLOSE_HALF_ORIENTATION(pose, -x, -y, -z, -w) )

TEST_CASE( "Call to process (Ceres Adjustment) Translation", "[processOnTranslation]" )
{
	//Initialize Data
	int numberOfImages = 4;
	int numberOfPoints = 5;
	float baseline = 5;
	float smallTranslation = 5;
	Eigen::Matrix3f cameraMatrix;
	cameraMatrix << 1, 0, 0,   0, 1, 0,    0, 0, 1;
	Eigen::Vector3f secondCameraPosition(0, baseline, 0);
	Eigen::Vector3f baselineVector(baseline, 0, 0);
	Eigen::Quaternion<float> secondCameraOrientation =
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX()) *
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ());

	Eigen::Vector3f rightCameraPosition = secondCameraPosition + secondCameraOrientation.inverse() * baselineVector;
	Eigen::Vector3f pointsVector[5] = {Eigen::Vector3f(0, 0, 5), Eigen::Vector3f(50, 0, 5), Eigen::Vector3f(0, 50, 5), Eigen::Vector3f(50, 50, 5), Eigen::Vector3f(10, 10, 2.5) };
	Eigen::Vector3f transformedPointVector[4][5];
	for(int pointIndex = 0; pointIndex < 5; pointIndex++)
		{
		transformedPointVector[0][pointIndex] = cameraMatrix * pointsVector[pointIndex];
		transformedPointVector[1][pointIndex] = cameraMatrix * (pointsVector[pointIndex] - baselineVector);
		transformedPointVector[2][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - secondCameraPosition) );
		transformedPointVector[3][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - rightCameraPosition) );
		}

	//Initialize Inputs
	CorrespondenceMaps2DSequencePtr correspondenceMapsSequence = NewCorrespondenceMaps2DSequence();
	for(int firstImageIndex = 0; firstImageIndex < numberOfImages; firstImageIndex++)
		{
		for(int secondImageIndex = firstImageIndex+1; secondImageIndex < numberOfImages; secondImageIndex++)
			{
			CorrespondenceMap2DPtr correspondenceMap = NewCorrespondenceMap2D();
			for(int pointIndex = 0; pointIndex < numberOfPoints; pointIndex++)
				{
				Point2D sourcePoint, sinkPoint;
				sourcePoint.x = transformedPointVector[firstImageIndex][pointIndex](0) / transformedPointVector[firstImageIndex][pointIndex](2);
				sourcePoint.y = transformedPointVector[firstImageIndex][pointIndex](1) / transformedPointVector[firstImageIndex][pointIndex](2);
				sinkPoint.x = transformedPointVector[secondImageIndex][pointIndex](0) / transformedPointVector[secondImageIndex][pointIndex](2);
				sinkPoint.y = transformedPointVector[secondImageIndex][pointIndex](1) / transformedPointVector[secondImageIndex][pointIndex](2);
				AddCorrespondence(*correspondenceMap, sourcePoint, sinkPoint, 1);

				double mandatoryOutput;
				ASSERT(
					CLOSE(std::modf(std::abs(sourcePoint.x), &mandatoryOutput), 0) && CLOSE(std::modf(std::abs(sourcePoint.y), &mandatoryOutput), 0) &&
					CLOSE(std::modf(std::abs(sinkPoint.x), &mandatoryOutput), 0) && CLOSE(std::modf(std::abs(sinkPoint.y), &mandatoryOutput), 0)
					, "The test points should be interger pixel coordinates"
					);
				}
			AddCorrespondenceMap(*correspondenceMapsSequence, *correspondenceMap);
			}
		}

	// Instantiate DFN
	CeresAdjustment* ceres = new CeresAdjustment;

	// Setup DFN
	ceres->setConfigurationFile("../tests/ConfigurationFiles/DFNs/BundleAdjustment/CeresAdjustment_Conf1.yaml");
	ceres->configure();

	// Send input data to DFN
	ceres->correspondenceMapsSequenceInput(*correspondenceMapsSequence);

	// Run DFN
	ceres->process();

	// Query output data from DFN
	const Poses3DSequence& output = ceres->posesSequenceOutput();
	bool success = ceres->successOutput();

	REQUIRE(success);

	bool position1IsCorrect = CLOSE_POSITION(GetPose(output, 0), 0, 0, 0);
	bool position2IsCorrect = CLOSE_POSITION(GetPose(output, 1), 5, 0, 0);
	bool position3IsCorrect = CLOSE_POSITION(GetPose(output, 2), 0, 5, 0);
	bool position4IsCorrect = CLOSE_POSITION(GetPose(output, 3), 5, 5, 0);
	REQUIRE(position1IsCorrect);
	REQUIRE(position2IsCorrect);
	REQUIRE(position3IsCorrect);
	REQUIRE(position4IsCorrect);

	bool orientation1IsCorrect = CLOSE_ORIENTATION(GetPose(output, 0), 0, 0, 0, 1);
	bool orientation2IsCorrect = CLOSE_ORIENTATION(GetPose(output, 1), 0, 0, 0, 1);
	bool orientation3IsCorrect = CLOSE_ORIENTATION(GetPose(output, 2), 0, 0, 0, 1);
	bool orientation4IsCorrect = CLOSE_ORIENTATION(GetPose(output, 3), 0, 0, 0, 1);
	REQUIRE(orientation1IsCorrect);
	REQUIRE(orientation2IsCorrect);
	REQUIRE(orientation3IsCorrect);
	REQUIRE(orientation4IsCorrect);

	// Cleanup
	delete(ceres);
}

TEST_CASE( "Call to process (Ceres Adjustment) RotoTranslation", "[processOnRotoTranslation]" )
{
	//Initialize Data
	int numberOfImages = 4;
	int numberOfPoints = 5;
	float baseline = 5;
	float smallTranslation = 5;
	Eigen::Matrix3f cameraMatrix;
	cameraMatrix << 10, 0, 0,   0, 10, 0,    0, 0, 1;
	Eigen::Vector3f secondCameraPosition(0, -baseline, 3*baseline);
	Eigen::Vector3f baselineVector(baseline, 0, 0);
	Eigen::Quaternion<float> secondCameraOrientation =
		Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitX()) *
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ());

	Eigen::Vector3f rightCameraPosition = secondCameraPosition + secondCameraOrientation.inverse() * baselineVector;
	Eigen::Vector3f pointsVector[5] = {Eigen::Vector3f(5, 0, 10), Eigen::Vector3f(45, 450, 10), Eigen::Vector3f(20, 200, 10), Eigen::Vector3f(20, 150, 10), Eigen::Vector3f(20, 20, 5) };
	Eigen::Vector3f transformedPointVector[4][5];
	for(int pointIndex = 0; pointIndex < 5; pointIndex++)
		{
		transformedPointVector[0][pointIndex] = cameraMatrix * pointsVector[pointIndex];
		transformedPointVector[1][pointIndex] = cameraMatrix * (pointsVector[pointIndex] - baselineVector);
		transformedPointVector[2][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - secondCameraPosition) );
		transformedPointVector[3][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - rightCameraPosition) );
		}

	//Initialize Inputs
	CorrespondenceMaps2DSequencePtr correspondenceMapsSequence = NewCorrespondenceMaps2DSequence();
	for(int firstImageIndex = 0; firstImageIndex < numberOfImages; firstImageIndex++)
		{
		for(int secondImageIndex = firstImageIndex+1; secondImageIndex < numberOfImages; secondImageIndex++)
			{
			CorrespondenceMap2DPtr correspondenceMap = NewCorrespondenceMap2D();
			for(int pointIndex = 0; pointIndex < numberOfPoints; pointIndex++)
				{
				Point2D sourcePoint, sinkPoint;
				sourcePoint.x = transformedPointVector[firstImageIndex][pointIndex](0) / transformedPointVector[firstImageIndex][pointIndex](2);
				sourcePoint.y = transformedPointVector[firstImageIndex][pointIndex](1) / transformedPointVector[firstImageIndex][pointIndex](2);
				sinkPoint.x = transformedPointVector[secondImageIndex][pointIndex](0) / transformedPointVector[secondImageIndex][pointIndex](2);
				sinkPoint.y = transformedPointVector[secondImageIndex][pointIndex](1) / transformedPointVector[secondImageIndex][pointIndex](2);

				// This is jus trying to extract the integer part as pixel coordinates. I cannot use the normal cast because I got something like static_cast<int>(-10) = -9.
				double decimalPart[4] = { sourcePoint.x, sourcePoint.y, sinkPoint.x, sinkPoint.y };
				double integerPart[4] = {0, 0, 0, 0};
				for(int index = 0; index < 4; index++)
					{
					while (decimalPart[index] > 0.5 || decimalPart[index] <= -0.5)
						{
						integerPart[index] += (decimalPart[index] > 0.5 ? 1 : -1);
						decimalPart[index] += (decimalPart[index] > 0.5 ? -1 : 1);
						}
					ASSERT( CLOSE_GROUND_TRUTH(decimalPart[index], 0), "The test points should be interger pixel coordinates");
					}
				sourcePoint.x = integerPart[0];
				sourcePoint.y = integerPart[1];
				sinkPoint.x = integerPart[2];
				sinkPoint.y = integerPart[3];
				AddCorrespondence(*correspondenceMap, sourcePoint, sinkPoint, 1);
				}
			AddCorrespondenceMap(*correspondenceMapsSequence, *correspondenceMap);
			}
		}

	//Preparing initial pose estimates
	Poses3DSequencePtr initialPosesSequence = NewPoses3DSequence();
	Pose3DPtr secondCameraPose = NewPose3D();
	SetPosition(*secondCameraPose, 0.01, -4.99, 14.98);
	SetOrientation(*secondCameraPose, 0.99999, 0.00001, 0, 0);
	AddPose(*initialPosesSequence, *secondCameraPose);

	//Preparing initial points estimates;
	PointCloudPtr initialPointCloud = NewPointCloud();
	AddPoint(*initialPointCloud, 5.02, 0, 10);
	AddPoint(*initialPointCloud, 4.98, 450, 10);
	AddPoint(*initialPointCloud, 20.03, 200, 10);
	AddPoint(*initialPointCloud, 19.99, 150, 10);
	AddPoint(*initialPointCloud, 20, 20, 5.01);

	// Instantiate DFN
	CeresAdjustment* ceres = new CeresAdjustment;

	// Setup DFN
	ceres->setConfigurationFile("../tests/ConfigurationFiles/DFNs/BundleAdjustment/CeresAdjustment_Conf2.yaml");
	ceres->configure();

	// Send input data to DFN
	ceres->correspondenceMapsSequenceInput(*correspondenceMapsSequence);
	ceres->guessedPosesSequenceInput(*initialPosesSequence);
	ceres->guessedPointCloudInput(*initialPointCloud);

	// Run DFN
	ceres->process();

	// Query output data from DFN
	const Poses3DSequence& output = ceres->posesSequenceOutput();
	bool success = ceres->successOutput();

	REQUIRE(success);

	bool orientation1IsCorrect = CLOSE_ORIENTATION(GetPose(output, 0), 0, 0, 0, 1);
	bool orientation2IsCorrect = CLOSE_ORIENTATION(GetPose(output, 1), 0, 0, 0, 1);
	bool orientation3IsCorrect = CLOSE_ORIENTATION(GetPose(output, 2), secondCameraOrientation.x(), secondCameraOrientation.y(), secondCameraOrientation.z(), secondCameraOrientation.w());
	bool orientation4IsCorrect = CLOSE_ORIENTATION(GetPose(output, 3), secondCameraOrientation.x(), secondCameraOrientation.y(), secondCameraOrientation.z(), secondCameraOrientation.w());
	REQUIRE(orientation1IsCorrect);
	REQUIRE(orientation2IsCorrect);
	REQUIRE(orientation3IsCorrect);
	REQUIRE(orientation4IsCorrect);

	bool position1IsCorrect = CLOSE_POSITION(GetPose(output, 0), 0, 0, 0);
	bool position2IsCorrect = CLOSE_POSITION(GetPose(output, 1), 5, 0, 0);
	bool position3IsCorrect = CLOSE_POSITION(GetPose(output, 2), 0, -baseline, 3*baseline);
	bool position4IsCorrect = CLOSE_POSITION(GetPose(output, 3), baseline, -baseline, 3*baseline);
	REQUIRE(position1IsCorrect);
	REQUIRE(position2IsCorrect);
	REQUIRE(position3IsCorrect);
	REQUIRE(position4IsCorrect);

	// Cleanup
	delete(ceres);
}

TEST_CASE( "Call to process (Ceres Adjustment) Rotation", "[processOnRotation]" )
{
	//Initialize Data
	int numberOfImages = 4;
	int numberOfPoints = 5;
	float baseline = 5;
	float smallTranslation = 5;
	Eigen::Matrix3f cameraMatrix;
	cameraMatrix << 1, 0, 0,   0, 1, 0,    0, 0, 1;
	Eigen::Vector3f secondCameraPosition(0, 0, 0);
	Eigen::Vector3f baselineVector(baseline, 0, 0);
	Eigen::Quaternion<float> secondCameraOrientation =
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX()) *
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
		Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ());

	Eigen::Vector3f rightCameraPosition = secondCameraPosition + secondCameraOrientation.inverse() * baselineVector;
	Eigen::Vector3f pointsVector[5] = {Eigen::Vector3f(0, 0, 5), Eigen::Vector3f(50, 0, 5), Eigen::Vector3f(0, 50, 5), Eigen::Vector3f(50, 50, 5), Eigen::Vector3f(20, 20, 2.5) };
	Eigen::Vector3f transformedPointVector[4][5];
	for(int pointIndex = 0; pointIndex < 5; pointIndex++)
		{
		transformedPointVector[0][pointIndex] = cameraMatrix * pointsVector[pointIndex];
		transformedPointVector[1][pointIndex] = cameraMatrix * (pointsVector[pointIndex] - baselineVector);
		transformedPointVector[2][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - secondCameraPosition) );
		transformedPointVector[3][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - rightCameraPosition) );
		}

	//Initialize Inputs
	CorrespondenceMaps2DSequencePtr correspondenceMapsSequence = NewCorrespondenceMaps2DSequence();
	for(int firstImageIndex = 0; firstImageIndex < numberOfImages; firstImageIndex++)
		{
		for(int secondImageIndex = firstImageIndex+1; secondImageIndex < numberOfImages; secondImageIndex++)
			{
			CorrespondenceMap2DPtr correspondenceMap = NewCorrespondenceMap2D();
			for(int pointIndex = 0; pointIndex < numberOfPoints; pointIndex++)
				{
				Point2D sourcePoint, sinkPoint;
				sourcePoint.x = transformedPointVector[firstImageIndex][pointIndex](0) / transformedPointVector[firstImageIndex][pointIndex](2);
				sourcePoint.y = transformedPointVector[firstImageIndex][pointIndex](1) / transformedPointVector[firstImageIndex][pointIndex](2);
				sinkPoint.x = transformedPointVector[secondImageIndex][pointIndex](0) / transformedPointVector[secondImageIndex][pointIndex](2);
				sinkPoint.y = transformedPointVector[secondImageIndex][pointIndex](1) / transformedPointVector[secondImageIndex][pointIndex](2);

				// This is jus trying to extract the integer part as pixel coordinates. I cannot use the normal cast because I got something like static_cast<int>(-10) = -9.
				double decimalPart[4] = { sourcePoint.x, sourcePoint.y, sinkPoint.x, sinkPoint.y };
				double integerPart[4] = {0, 0, 0, 0};
				for(int index = 0; index < 4; index++)
					{
					while (decimalPart[index] > 0.5 || decimalPart[index] <= -0.5)
						{
						integerPart[index] += (decimalPart[index] > 0.5 ? 1 : -1);
						decimalPart[index] += (decimalPart[index] > 0.5 ? -1 : 1);
						}
					ASSERT( CLOSE(decimalPart[index], 0), "The test points should be interger pixel coordinates");
					}
				sourcePoint.x = integerPart[0];
				sourcePoint.y = integerPart[1];
				sinkPoint.x = integerPart[2];
				sinkPoint.y = integerPart[3];
				AddCorrespondence(*correspondenceMap, sourcePoint, sinkPoint, 1);
				}
			AddCorrespondenceMap(*correspondenceMapsSequence, *correspondenceMap);
			}
		}

	//Preparing initial pose estimates
	Poses3DSequencePtr initialPosesSequence = NewPoses3DSequence();
	Pose3DPtr secondCameraPose = NewPose3D();
	SetPosition(*secondCameraPose, 5, 0, 0);
	SetOrientation(*secondCameraPose, 0, 0, 1, 0);
	AddPose(*initialPosesSequence, *secondCameraPose);

	//Preparing initial points estimates;
	PointCloudPtr initialPointCloud = NewPointCloud();
	AddPoint(*initialPointCloud, 0, 0, 5);
	AddPoint(*initialPointCloud, 50, 0, 5);
	AddPoint(*initialPointCloud, 0, 50, 5);
	AddPoint(*initialPointCloud, 50, 50, 5);
	AddPoint(*initialPointCloud, 20, 20, 2.5);

	// Instantiate DFN
	CeresAdjustment* ceres = new CeresAdjustment;

	// Setup DFN
	ceres->setConfigurationFile("../tests/ConfigurationFiles/DFNs/BundleAdjustment/CeresAdjustment_Conf1.yaml");
	ceres->configure();

	// Send input data to DFN
	ceres->correspondenceMapsSequenceInput(*correspondenceMapsSequence);
	ceres->guessedPosesSequenceInput(*initialPosesSequence);
	ceres->guessedPointCloudInput(*initialPointCloud);

	// Run DFN
	ceres->process();

	// Query output data from DFN
	const Poses3DSequence& output = ceres->posesSequenceOutput();
	bool success = ceres->successOutput();

	REQUIRE(success);

	bool orientation1IsCorrect = CLOSE_ORIENTATION(GetPose(output, 0), 0, 0, 0, 1);
	bool orientation2IsCorrect = CLOSE_ORIENTATION(GetPose(output, 1), 0, 0, 0, 1);
	bool orientation3IsCorrect = CLOSE_ORIENTATION(GetPose(output, 2), secondCameraOrientation.x(), secondCameraOrientation.y(), secondCameraOrientation.z(), secondCameraOrientation.w());
	bool orientation4IsCorrect = CLOSE_ORIENTATION(GetPose(output, 3), secondCameraOrientation.x(), secondCameraOrientation.y(), secondCameraOrientation.z(), secondCameraOrientation.w());
	REQUIRE(orientation1IsCorrect);
	REQUIRE(orientation2IsCorrect);
	REQUIRE(orientation3IsCorrect);
	REQUIRE(orientation4IsCorrect);

	bool position1IsCorrect = CLOSE_POSITION(GetPose(output, 0), 0, 0, 0);
	bool position2IsCorrect = CLOSE_POSITION(GetPose(output, 1), 5, 0, 0);
	bool position3IsCorrect = CLOSE_POSITION(GetPose(output, 2), 0, 0, 0);
	bool position4IsCorrect = CLOSE_POSITION(GetPose(output, 3), -baseline, 0, 0);
	REQUIRE(position1IsCorrect);
	REQUIRE(position2IsCorrect);
	REQUIRE(position3IsCorrect);
	REQUIRE(position4IsCorrect);

	// Cleanup
	delete(ceres);
}

TEST_CASE( "Call to process (Ceres Adjustment) 90 Degree Rotation", "[processOn90DegreeRotation]" )
{
	//Initialize Data
	int numberOfImages = 4;
	int numberOfPoints = 5;
	float baseline = 5;
	float smallTranslation = 5;
	Eigen::Matrix3f cameraMatrix;
	cameraMatrix << 10, 0, 0,   0, 10, 0,    0, 0, 1;
	Eigen::Vector3f secondCameraPosition(-baseline, 0, baseline);
	Eigen::Vector3f baselineVector(baseline, 0, 0);
	Eigen::Quaternion<float> secondCameraOrientation =
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX()) *
		Eigen::AngleAxisf(-M_PI/2, Eigen::Vector3f::UnitY()) *
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ());

	Eigen::Vector3f rightCameraPosition = secondCameraPosition + secondCameraOrientation.inverse() * baselineVector;
	Eigen::Vector3f pointsVector[5] = {Eigen::Vector3f(5, 0, 10), Eigen::Vector3f(45, 450, 10), Eigen::Vector3f(20, 200, 10), Eigen::Vector3f(20, 150, 10), Eigen::Vector3f(20, 20, 12.5) };
	Eigen::Vector3f transformedPointVector[4][5];
	for(int pointIndex = 0; pointIndex < 5; pointIndex++)
		{
		transformedPointVector[0][pointIndex] = cameraMatrix * pointsVector[pointIndex];
		transformedPointVector[1][pointIndex] = cameraMatrix * (pointsVector[pointIndex] - baselineVector);
		transformedPointVector[2][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - secondCameraPosition) );
		transformedPointVector[3][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - rightCameraPosition) );
		}

	//Initialize Inputs
	CorrespondenceMaps2DSequencePtr correspondenceMapsSequence = NewCorrespondenceMaps2DSequence();
	for(int firstImageIndex = 0; firstImageIndex < numberOfImages; firstImageIndex++)
		{
		for(int secondImageIndex = firstImageIndex+1; secondImageIndex < numberOfImages; secondImageIndex++)
			{
			CorrespondenceMap2DPtr correspondenceMap = NewCorrespondenceMap2D();
			for(int pointIndex = 0; pointIndex < numberOfPoints; pointIndex++)
				{
				Point2D sourcePoint, sinkPoint;
				sourcePoint.x = transformedPointVector[firstImageIndex][pointIndex](0) / transformedPointVector[firstImageIndex][pointIndex](2);
				sourcePoint.y = transformedPointVector[firstImageIndex][pointIndex](1) / transformedPointVector[firstImageIndex][pointIndex](2);
				sinkPoint.x = transformedPointVector[secondImageIndex][pointIndex](0) / transformedPointVector[secondImageIndex][pointIndex](2);
				sinkPoint.y = transformedPointVector[secondImageIndex][pointIndex](1) / transformedPointVector[secondImageIndex][pointIndex](2);
				AddCorrespondence(*correspondenceMap, sourcePoint, sinkPoint, 1);

				double mandatoryOutput;
				ASSERT(
					CLOSE(std::modf(std::abs(sourcePoint.x), &mandatoryOutput), 0) && CLOSE(std::modf(std::abs(sourcePoint.y), &mandatoryOutput), 0) &&
					CLOSE(std::modf(std::abs(sinkPoint.x), &mandatoryOutput), 0) && CLOSE(std::modf(std::abs(sinkPoint.y), &mandatoryOutput), 0)
					, "The test points should be interger pixel coordinates"
					);
				}
			AddCorrespondenceMap(*correspondenceMapsSequence, *correspondenceMap);
			}
		}

	// Instantiate DFN
	CeresAdjustment* ceres = new CeresAdjustment;

	// Setup DFN
	ceres->setConfigurationFile("../tests/ConfigurationFiles/DFNs/BundleAdjustment/CeresAdjustment_Conf2.yaml");
	ceres->configure();

	// Send input data to DFN
	ceres->correspondenceMapsSequenceInput(*correspondenceMapsSequence);

	// Run DFN
	ceres->process();

	// Query output data from DFN
	const Poses3DSequence& output = ceres->posesSequenceOutput();
	bool success = ceres->successOutput();

	REQUIRE(success);

	bool orientation1IsCorrect = CLOSE_ORIENTATION(GetPose(output, 0), 0, 0, 0, 1);
	bool orientation2IsCorrect = CLOSE_ORIENTATION(GetPose(output, 1), 0, 0, 0, 1);
	bool orientation3IsCorrect = CLOSE_ORIENTATION(GetPose(output, 2), secondCameraOrientation.x(), secondCameraOrientation.y(), secondCameraOrientation.z(), secondCameraOrientation.w());
	bool orientation4IsCorrect = CLOSE_ORIENTATION(GetPose(output, 3), secondCameraOrientation.x(), secondCameraOrientation.y(), secondCameraOrientation.z(), secondCameraOrientation.w());
	REQUIRE(orientation1IsCorrect);
	REQUIRE(orientation2IsCorrect);
	REQUIRE(orientation3IsCorrect);
	REQUIRE(orientation4IsCorrect);

	bool position1IsCorrect = CLOSE_POSITION(GetPose(output, 0), 0, 0, 0);
	bool position2IsCorrect = CLOSE_POSITION(GetPose(output, 1), 5, 0, 0);
	bool position3IsCorrect = CLOSE_POSITION(GetPose(output, 2), -baseline, 0, baseline);
	bool position4IsCorrect = CLOSE_POSITION(GetPose(output, 3), -baseline, 0, 0);
	REQUIRE(position1IsCorrect);
	REQUIRE(position2IsCorrect);
	REQUIRE(position3IsCorrect);
	REQUIRE(position4IsCorrect);

	//TRY TO recompute the transform with the value of the computation
	// Cleanup
	delete(ceres);
}

TEST_CASE( "Call to process (Ceres Adjustment) Intruders", "[processOnIntruders]" )
{
	//Initialize Data
	int numberOfImages = 4;
	int numberOfPoints = 5;
	float baseline = 5;
	float smallTranslation = 5;
	Eigen::Matrix3f cameraMatrix;
	cameraMatrix << 1, 0, 0,   0, 1, 0,    0, 0, 1;
	Eigen::Vector3f secondCameraPosition(0, 0, 0);
	Eigen::Vector3f baselineVector(baseline, 0, 0);
	Eigen::Quaternion<float> secondCameraOrientation =
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitX()) *
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
		Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ());

	Eigen::Vector3f rightCameraPosition = secondCameraPosition + secondCameraOrientation.inverse() * baselineVector;
	Eigen::Vector3f pointsVector[5] = {Eigen::Vector3f(0, 0, 5), Eigen::Vector3f(50, 0, 5), Eigen::Vector3f(0, 50, 5), Eigen::Vector3f(50, 50, 5), Eigen::Vector3f(20, 20, 2.5) };
	Eigen::Vector3f transformedPointVector[4][5];
	for(int pointIndex = 0; pointIndex < 5; pointIndex++)
		{
		transformedPointVector[0][pointIndex] = cameraMatrix * pointsVector[pointIndex];
		transformedPointVector[1][pointIndex] = cameraMatrix * (pointsVector[pointIndex] - baselineVector);
		transformedPointVector[2][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - secondCameraPosition) );
		transformedPointVector[3][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - rightCameraPosition) );
		}

	//Initialize Inputs
	CorrespondenceMaps2DSequencePtr correspondenceMapsSequence = NewCorrespondenceMaps2DSequence();
	for(int firstImageIndex = 0; firstImageIndex < numberOfImages; firstImageIndex++)
		{
		for(int secondImageIndex = firstImageIndex+1; secondImageIndex < numberOfImages; secondImageIndex++)
			{
			CorrespondenceMap2DPtr correspondenceMap = NewCorrespondenceMap2D();
			for(int pointIndex = 0; pointIndex < numberOfPoints; pointIndex++)
				{
				Point2D sourcePoint, sinkPoint;
				sourcePoint.x = transformedPointVector[firstImageIndex][pointIndex](0) / transformedPointVector[firstImageIndex][pointIndex](2);
				sourcePoint.y = transformedPointVector[firstImageIndex][pointIndex](1) / transformedPointVector[firstImageIndex][pointIndex](2);
				sinkPoint.x = transformedPointVector[secondImageIndex][pointIndex](0) / transformedPointVector[secondImageIndex][pointIndex](2);
				sinkPoint.y = transformedPointVector[secondImageIndex][pointIndex](1) / transformedPointVector[secondImageIndex][pointIndex](2);

				// This is jus trying to extract the integer part as pixel coordinates. I cannot use the normal cast because I got something like static_cast<int>(-10) = -9.
				double decimalPart[4] = { sourcePoint.x, sourcePoint.y, sinkPoint.x, sinkPoint.y };
				double integerPart[4] = {0, 0, 0, 0};
				for(int index = 0; index < 4; index++)
					{
					while (decimalPart[index] > 0.5 || decimalPart[index] <= -0.5)
						{
						integerPart[index] += (decimalPart[index] > 0.5 ? 1 : -1);
						decimalPart[index] += (decimalPart[index] > 0.5 ? -1 : 1);
						}
					ASSERT( CLOSE(decimalPart[index], 0), "The test points should be interger pixel coordinates");
					}
				sourcePoint.x = integerPart[0];
				sourcePoint.y = integerPart[1];
				sinkPoint.x = integerPart[2];
				sinkPoint.y = integerPart[3];
				AddCorrespondence(*correspondenceMap, sourcePoint, sinkPoint, 1);

				//This is an intruder point, it represents a point appearing only in the first correspondence, it is supposed to be discarded
				if (firstImageIndex == 0 && secondImageIndex == 1 && pointIndex == 2)
					{
					Point2D sourceIntruderPoint, sinkIntruderPoint;
					sourceIntruderPoint.x = 58;
					sourceIntruderPoint.y = 58;
					sinkIntruderPoint.x = 109;
					sinkIntruderPoint.x = 109;
					AddCorrespondence(*correspondenceMap, sourceIntruderPoint, sinkIntruderPoint, 1);
					}
				}
			AddCorrespondenceMap(*correspondenceMapsSequence, *correspondenceMap);
			}
		}

	//Preparing initial pose estimates
	Poses3DSequencePtr initialPosesSequence = NewPoses3DSequence();
	Pose3DPtr secondCameraPose = NewPose3D();
	SetPosition(*secondCameraPose, 5, 0, 0);
	SetOrientation(*secondCameraPose, 0, 0, 1, 0);
	AddPose(*initialPosesSequence, *secondCameraPose);

	//Preparing initial points estimates;
	PointCloudPtr initialPointCloud = NewPointCloud();
	AddPoint(*initialPointCloud, 0, 0, 5);
	AddPoint(*initialPointCloud, 50, 0, 5);
	AddPoint(*initialPointCloud, 454.1, 50.99, 5.88); //Intruder point
	AddPoint(*initialPointCloud, 0, 50, 5);
	AddPoint(*initialPointCloud, 50, 50, 5);
	AddPoint(*initialPointCloud, 20, 20, 2.5);

	// Instantiate DFN
	CeresAdjustment* ceres = new CeresAdjustment;

	// Setup DFN
	ceres->setConfigurationFile("../tests/ConfigurationFiles/DFNs/BundleAdjustment/CeresAdjustment_Conf1.yaml");
	ceres->configure();

	// Send input data to DFN
	ceres->correspondenceMapsSequenceInput(*correspondenceMapsSequence);
	ceres->guessedPosesSequenceInput(*initialPosesSequence);
	ceres->guessedPointCloudInput(*initialPointCloud);

	// Run DFN
	ceres->process();

	// Query output data from DFN
	const Poses3DSequence& output = ceres->posesSequenceOutput();
	bool success = ceres->successOutput();

	REQUIRE(success);

	bool orientation1IsCorrect = CLOSE_ORIENTATION(GetPose(output, 0), 0, 0, 0, 1);
	bool orientation2IsCorrect = CLOSE_ORIENTATION(GetPose(output, 1), 0, 0, 0, 1);
	bool orientation3IsCorrect = CLOSE_ORIENTATION(GetPose(output, 2), secondCameraOrientation.x(), secondCameraOrientation.y(), secondCameraOrientation.z(), secondCameraOrientation.w());
	bool orientation4IsCorrect = CLOSE_ORIENTATION(GetPose(output, 3), secondCameraOrientation.x(), secondCameraOrientation.y(), secondCameraOrientation.z(), secondCameraOrientation.w());
	REQUIRE(orientation1IsCorrect);
	REQUIRE(orientation2IsCorrect);
	REQUIRE(orientation3IsCorrect);
	REQUIRE(orientation4IsCorrect);

	bool position1IsCorrect = CLOSE_POSITION(GetPose(output, 0), 0, 0, 0);
	bool position2IsCorrect = CLOSE_POSITION(GetPose(output, 1), 5, 0, 0);
	bool position3IsCorrect = CLOSE_POSITION(GetPose(output, 2), 0, 0, 0);
	bool position4IsCorrect = CLOSE_POSITION(GetPose(output, 3), -baseline, 0, 0);
	REQUIRE(position1IsCorrect);
	REQUIRE(position2IsCorrect);
	REQUIRE(position3IsCorrect);
	REQUIRE(position4IsCorrect);

	// Cleanup
	delete(ceres);
}

TEST_CASE( "Call to process (Ceres Adjustment) Pixel Accuracy Error", "[processOnPixelError]" )
{
	return; //This test does not pass, Ceres fails to converge, probably pixel error is too large
	//Initialize Data
	int numberOfImages = 4;
	const int NUMBER_OF_POINTS = 5;
	float baseline = 5;
	float smallTranslation = 5;
 	//std::default_random_engine generator;
  	//std::normal_distribution<double> distribution(0, 0.001);

	Eigen::Matrix3f cameraMatrix;
	cameraMatrix << 1000, 0, 0,   0, 1000, 0,    0, 0, 1;
	Eigen::Vector3f secondCameraPosition(0, -baseline, 0);
	Eigen::Vector3f baselineVector(baseline, 0, 0);
	Eigen::Quaternion<float> secondCameraOrientation =
		Eigen::AngleAxisf(M_PI/10, Eigen::Vector3f::UnitX()) *
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitY()) *
		Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ());

	Eigen::Vector3f rightCameraPosition = secondCameraPosition + secondCameraOrientation.inverse() * baselineVector;
	Eigen::Vector3f pointsVector[NUMBER_OF_POINTS];
	Eigen::Vector3f transformedPointVector[4][NUMBER_OF_POINTS];
	for(int pointIndex = 0; pointIndex < NUMBER_OF_POINTS; pointIndex++)
		{
		pointsVector[pointIndex][0] = 20 * pointIndex;
		pointsVector[pointIndex][1] = (pointIndex % 2 == 0 ? 10*pointIndex: std::abs(100 - 10*pointIndex) );
		pointsVector[pointIndex][2] = 5 + (pointIndex % 3 == 0 ? 2 * pointIndex : pointIndex);

		transformedPointVector[0][pointIndex] = cameraMatrix * pointsVector[pointIndex];
		transformedPointVector[1][pointIndex] = cameraMatrix * (pointsVector[pointIndex] - baselineVector);
		transformedPointVector[2][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - secondCameraPosition) );
		transformedPointVector[3][pointIndex] = cameraMatrix * (secondCameraOrientation * (pointsVector[pointIndex] - rightCameraPosition) );
		}

	//Initialize Inputs
	std::vector<int> validPointIndexList;
	CorrespondenceMaps2DSequencePtr correspondenceMapsSequence = NewCorrespondenceMaps2DSequence();
	float totalPixelError = 0;
	float numberOfPixels = 0;
	for(int firstImageIndex = 0; firstImageIndex < numberOfImages; firstImageIndex++)
		{
		for(int secondImageIndex = firstImageIndex+1; secondImageIndex < numberOfImages; secondImageIndex++)
			{
			CorrespondenceMap2DPtr correspondenceMap = NewCorrespondenceMap2D();
			for(int pointIndex = 0; pointIndex < NUMBER_OF_POINTS; pointIndex++)
				{
				Point2D sourcePoint, sinkPoint;
				sourcePoint.x = transformedPointVector[firstImageIndex][pointIndex](0) / transformedPointVector[firstImageIndex][pointIndex](2);
				sourcePoint.y = transformedPointVector[firstImageIndex][pointIndex](1) / transformedPointVector[firstImageIndex][pointIndex](2);
				sinkPoint.x = transformedPointVector[secondImageIndex][pointIndex](0) / transformedPointVector[secondImageIndex][pointIndex](2);
				sinkPoint.y = transformedPointVector[secondImageIndex][pointIndex](1) / transformedPointVector[secondImageIndex][pointIndex](2);

				// This is jus trying to extract the integer part as pixel coordinates. I cannot use the normal cast because I got something like static_cast<int>(-10) = -9.
				double decimalPart[4] = { sourcePoint.x, sourcePoint.y, sinkPoint.x, sinkPoint.y };
				double integerPart[4] = {0, 0, 0, 0};
				for(int index = 0; index < 4; index++)
					{
					if (decimalPart[index] > 0)
						{
						while (decimalPart[index] > 0.5)
							{
							integerPart[index] += +1;
							decimalPart[index] += -1;
							}
						}
					else
						{
						while (decimalPart[index] <= -0.5)
							{
							integerPart[index] += -1;
							decimalPart[index] += +1;
							}
						}
					totalPixelError += std::abs(decimalPart[index]);
					numberOfPixels += 1;
					//ASSERT( CLOSE_GROUND_TRUTH(decimalPart[index], 0), "The test points should be interger pixel coordinates");
					}
				sourcePoint.x = integerPart[0];
				sourcePoint.y = integerPart[1];
				sinkPoint.x = integerPart[2];
				sinkPoint.y = integerPart[3];

				bool repeated = false;
				for (int correspondenceIndex = 0; correspondenceIndex < GetNumberOfCorrespondences(*correspondenceMap) && !repeated; correspondenceIndex++)
					{
					BaseTypesWrapper::Point2D oldSource = GetSource(*correspondenceMap, correspondenceIndex);
					BaseTypesWrapper::Point2D oldSink = GetSink(*correspondenceMap, correspondenceIndex);
					repeated = (sourcePoint.x == oldSource.x && sourcePoint.y == oldSource.y) || (sinkPoint.x == oldSink.x && sinkPoint.y == oldSink.y);
					}
				if (!repeated)
					{
					if (firstImageIndex == 0 && secondImageIndex == 1)
						{
						validPointIndexList.push_back(pointIndex);
						}
					AddCorrespondence(*correspondenceMap, sourcePoint, sinkPoint, 1);
					}
				}
			AddCorrespondenceMap(*correspondenceMapsSequence, *correspondenceMap);
			}
		}
	PRINT_TO_LOG("average pixel error", (totalPixelError/numberOfPixels) );

	//Preparing initial pose estimates
	Poses3DSequencePtr initialPosesSequence = NewPoses3DSequence();
	Pose3DPtr secondCameraPose = NewPose3D();
	SetPosition(*secondCameraPose, 0, -baseline, 0);
	SetOrientation(*secondCameraPose, std::sin(M_PI/20), 0, 0, std::cos(M_PI/20) );
	AddPose(*initialPosesSequence, *secondCameraPose);

	//Preparing initial points estimates;
	PointCloudPtr initialPointCloud = NewPointCloud();
	for(int pointIndexIndex = 0; pointIndexIndex < validPointIndexList.size(); pointIndexIndex++)
		{
		int pointIndex = validPointIndexList.at(pointIndexIndex);
		float noisyX = pointsVector[pointIndex][0];
		float noisyY = pointsVector[pointIndex][1];
		float noisyZ = pointsVector[pointIndex][2];
		AddPoint(*initialPointCloud, noisyX, noisyY, noisyZ);
		}

	// Instantiate DFN
	CeresAdjustment* ceres = new CeresAdjustment;

	// Setup DFN
	ceres->setConfigurationFile("../tests/ConfigurationFiles/DFNs/BundleAdjustment/CeresAdjustment_Conf3.yaml");
	ceres->configure();

	// Send input data to DFN
	ceres->correspondenceMapsSequenceInput(*correspondenceMapsSequence);
	ceres->guessedPosesSequenceInput(*initialPosesSequence);
	ceres->guessedPointCloudInput(*initialPointCloud);

	// Run DFN
	ceres->process();

	// Query output data from DFN
	const Poses3DSequence& output = ceres->posesSequenceOutput();
	bool success = ceres->successOutput();

	REQUIRE(success);

	bool orientation1IsCorrect = CLOSE_ORIENTATION(GetPose(output, 0), 0, 0, 0, 1);
	bool orientation2IsCorrect = CLOSE_ORIENTATION(GetPose(output, 1), 0, 0, 0, 1);
	bool orientation3IsCorrect = CLOSE_ORIENTATION(GetPose(output, 2), secondCameraOrientation.x(), secondCameraOrientation.y(), secondCameraOrientation.z(), secondCameraOrientation.w());
	bool orientation4IsCorrect = CLOSE_ORIENTATION(GetPose(output, 3), secondCameraOrientation.x(), secondCameraOrientation.y(), secondCameraOrientation.z(), secondCameraOrientation.w());
	REQUIRE(orientation1IsCorrect);
	REQUIRE(orientation2IsCorrect);
	REQUIRE(orientation3IsCorrect);
	REQUIRE(orientation4IsCorrect);

	bool position1IsCorrect = CLOSE_POSITION(GetPose(output, 0), 0, 0, 0);
	bool position2IsCorrect = CLOSE_POSITION(GetPose(output, 1), 5, 0, 0);
	bool position3IsCorrect = CLOSE_POSITION(GetPose(output, 2), 0, -baseline, 3*baseline);
	bool position4IsCorrect = CLOSE_POSITION(GetPose(output, 3), baseline, -baseline, 3*baseline);
	REQUIRE(position1IsCorrect);
	REQUIRE(position2IsCorrect);
	REQUIRE(position3IsCorrect);
	REQUIRE(position4IsCorrect);

	// Cleanup
	delete(ceres);
}

/** @} */


#include <boost/python.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <Python.h>

using namespace std;
using namespace boost::python;

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT>::Ptr CloudPtr;

uint8_t interpolate(float z, float z1, float z2, uint8_t b1, uint8_t b2) {
    return (b2 * (z-z1) + b1 * (z2-z)) / (z2-z1);
}

void interpolateColor(PointT &pt, float min_z, float max_z) {
#define N 3
    float zpts[N] = {min_z, (min_z + max_z) / 2, max_z};
    uint8_t rgbs[N*3] = {
        255, 15, 15,
        15, 255, 15,
        15, 15, 255
    };
    for(size_t i(1); i < N; i++) {
        float z1 = zpts[i-1];
        float z2 = zpts[i];
        if(pt.z < z2) {
            uint8_t r1 = rgbs[3*(i-1)];
            uint8_t r2 = rgbs[3*i];
            uint8_t g1 = rgbs[3*(i-1)+1];
            uint8_t g2 = rgbs[3*i+1];
            uint8_t b1 = rgbs[3*(i-1)+2];
            uint8_t b2 = rgbs[3*i+2];
            uint8_t r = interpolate(pt.z, z1, z2, r1, r2);
            uint8_t g = interpolate(pt.z, z1, z2, g1, g2);
            uint8_t b = interpolate(pt.z, z1, z2, b1, b2);

            pt.rgb = (static_cast<uint32_t>(r) << 16 |
                    static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
            break;
        }
    }
}

void loadCloud(pcl::PointCloud<PointT>::Ptr cloud, object np_xyz) {
    Py_buffer buffer;
    if(
            len(np_xyz.attr("shape")) != 3 ||
            extract<int>(np_xyz.attr("shape")[0]) != 3) {
        throw ("expected numpy array to have shape (3, height, width)");
    }
    cloud->height = extract<int>(np_xyz.attr("shape")[1]);
    cloud->width = extract<int>(np_xyz.attr("shape")[2]);
    cloud->resize(cloud->width * cloud->height);

    int result = PyObject_GetBuffer(np_xyz.ptr(), &buffer, PyBUF_ND | PyBUF_C_CONTIGUOUS);
    if(result != 0) {
        throw ("expected np_xyz to be buffer interface");
    }

    float min_z = 100000;
    float max_z = -100000;
    for(size_t r = 0; r < cloud->height; r++) {
        void *offset = buffer.buf + r * buffer.strides[1];
        for(size_t c = 0; c < cloud->width; c++, offset += buffer.strides[2]) {
            void *px = 0 * buffer.strides[0] + offset;
            void *py = 1 * buffer.strides[0] + offset;
            void *pz = 2 * buffer.strides[0] + offset;
            PointT &pt = cloud->at(c, r);
            pt.x = *((float*)px) / 1000.;
            pt.y = *((float*)py) / 1000.;
            pt.z = *((float*)pz) / 1000.;
            if(min_z > pt.z) {
                min_z = pt.z;
            }
            if(max_z < pt.z) {
                max_z = pt.z;
            }
            //cout << "xyz[" << r << ", " << c << "]: [";
            //cout << pt.x << ", " << pt.y << ", " << pt.z << "]" << endl;
        }
    }

    for(size_t r(0); r < cloud->height; r++) {
        for(size_t c(0); c < cloud->width; c++) {
            PointT &pt = cloud->at(c, r);
            interpolateColor(pt, min_z, max_z);
        }
    }
}

CloudPtr cloudFromNumpyXYZ(object np_xyz) {
    pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
    loadCloud(cloud, np_xyz);
    return cloud;
}

struct Viewer {
    pcl::visualization::PCLVisualizer *viewer;
    string cloudName = "da cloud";
    CloudPtr cloud;

    Viewer() {
        viewer = new pcl::visualization::PCLVisualizer("Cloud Viewer");
        viewer->setBackgroundColor(0, 0, 0);
        viewer->addCoordinateSystem(1.0);
        viewer->initCameraParameters();
    }

    void setupCloud(CloudPtr _cloud) {
        this->cloud = _cloud;
        pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
        viewer->addPointCloud<PointT>(cloud, rgb, cloudName);
        viewer->setPointCloudRenderingProperties(
            pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
            1,
            cloudName
        );
    }

    bool wasStopped() {
        return viewer->wasStopped();
    }

    void spin() {
        viewer->spinOnce(100);
    }

    void updateCloud(object np_xyz) {
        loadCloud(cloud, np_xyz);
        viewer->updatePointCloud(cloud, cloudName);
    }

    void close() {
        viewer->close();
    }
};

Viewer *process(object np_xyz) {
    CloudPtr cloud = cloudFromNumpyXYZ(np_xyz);
    Viewer *viewer = new Viewer();
    viewer->setupCloud(cloud);

    return viewer;
}

BOOST_PYTHON_MODULE(libpclproc)
{
    def("process", process, return_value_policy<manage_new_object>());
    class_<Viewer>("Viewer")
        .def("wasStopped", &Viewer::wasStopped)
        .def("updateCloud", &Viewer::updateCloud)
        .def("close", &Viewer::close)
        .def("spin", &Viewer::spin);
}


// (C) Copyright Jeremy Siek 2001.
// Permission to copy, use, modify, sell and distribute this software
// is granted provided this copyright notice appears in all
// copies. This software is provided "as is" without express or
// implied warranty, and with no claim as to its suitability for any
// purpose.

#include <vector>
#include <algorithm> // for std::min
#include <fstream>
#include <boost/test/test_tools.hpp>

// Extract the bit at position n from num.
template <typename Block>
inline bool nth_bit(Block num, std::size_t n)
{
  // Move the nth bit to position 0 and then clear all other bits.
  return (num >> n) & 1;
}
inline unsigned long max_num(std::size_t num_bits)
{
  using namespace std; // for std::pow, VC++ workaround -JGS
  return (unsigned long)(pow((double)2, (double)num_bits));
}

// constructors
//   default (can't do this generically)

//   from unsigned long

template <typename Bitset>
struct bitset_test {

  static void from_unsigned_long(Bitset b, unsigned long num)
  {
    // initializes the first M bit position to the cooresponding bit
    // values in val. M is the smaller of N and the value CHAR_BIT *
    // sizeof(unsigned long)

    // missing from the std?
    //   if M < N then the remaining bit positions are initialized to zero

    std::size_t N = b.size();
    std::size_t M = std::min(N, CHAR_BIT * sizeof(unsigned long));
    std::size_t I;
    for (I = 0; I < M; ++I)
      BOOST_CHECK(b[I] == nth_bit(num, I));
    for (; I < N; ++I)
      BOOST_CHECK(b[I] == 0);
  }

  //   from string
  static void from_string(const std::string& str, std::size_t pos,
                          std::size_t n)
  {
    if (pos > str.size()) {
      // Not in range, doesn't satisfy precondition.
    } else {
      std::size_t rlen = std::min(n, str.size() - pos);

      // Throws invalid_argument if any of the rlen characters in str
      // beginning at position pos is other than 0 or 1.
      bool any_non_zero_or_one = false;
      for (std::size_t i = pos; i < pos + rlen; ++i)
        if (! (str[i] == '0' || str[i] == '1'))
          any_non_zero_or_one = true;
      if (any_non_zero_or_one) {
        // Input does not satisfy precondition.
      } else {
        // Construct an object, initializing the first M bit position to
        // values determined from the corresponding characters in the
        // str. M is the smaller of N and rlen.  Character position pos
        // + M - 1 corresponds to bit position zero.  Subsequent
        // decreasing character position correspond to increasing bit
        // positions.

        Bitset b(str, pos, n);
        std::size_t N = b.size();
        std::size_t M = std::min(N, rlen);
        std::size_t j;
        for (j = 0; j < M; ++j)
          BOOST_CHECK(b[j] == (str[pos + M - 1 - j] == '1'));
        // If M < N, remaining bit positions are initialize to zero
        for (; j < N; ++j)
          BOOST_CHECK(b[j] == 0);
      }
    }
  }

  typedef typename Bitset::block_type Block;

  // PRE: std::equal(first1, last1, first2) == true
  static void from_block_range(std::vector<Block> blocks)
  {
    {
      Bitset bset(blocks.begin(), blocks.end());
      std::size_t n = blocks.size();
      for (std::size_t b = 0; b < n; ++b) {
        for (std::size_t i = 0; i < sizeof(Block) * CHAR_BIT; ++i) {
          std::size_t bit = b * sizeof(Block) * CHAR_BIT + i;
          BOOST_CHECK(bset[bit] == nth_bit(blocks[b], i));
        }
      }
    }
    {
      Bitset bset(blocks.size() * sizeof(Block) * CHAR_BIT);
      boost::from_block_range(blocks.begin(), blocks.end(), bset);
      std::size_t n = blocks.size();
      for (std::size_t b = 0; b < n; ++b) {
        for (std::size_t i = 0; i < sizeof(Block) * CHAR_BIT; ++i) {
          std::size_t bit = b * sizeof(Block) * CHAR_BIT + i;
          BOOST_CHECK(bset[bit] == nth_bit(blocks[b], i));
        }
      }
    }
  }

  // copy constructor (absent from std::bitset)
  static void copy_constructor(const Bitset& b)
  {
    Bitset copy(b);
    BOOST_CHECK(b == copy);

    // Changes to the copy do not affect the original
    if (b.size() > 0) {
      std::size_t pos = copy.size() / 2;
      copy.flip(pos);
      BOOST_CHECK(copy[pos] != b[pos]);
    }
  }

  // assignment operator (absent from std::bitset)
  static void assignment_operator(const Bitset& lhs, const Bitset& rhs)
  {
    Bitset b(lhs);
    b = rhs;
    BOOST_CHECK(b == rhs);

    // Changes to the copy do not affect the original
    if (b.size() > 0) {
      std::size_t pos = b.size() / 2;
      b.flip(pos);
      BOOST_CHECK(b[pos] != rhs[pos]);
    }
  }

  static void resize(const Bitset& lhs)
  {
    Bitset b(lhs);

    // Test no change in size
    b.resize(lhs.size());
    BOOST_CHECK(b == lhs);

    // Test increase in size
    b.resize(lhs.size() * 2, true);

    std::size_t i;
    for (i = 0; i < lhs.size(); ++i)
      BOOST_CHECK(b[i] == lhs[i]);
    for (; i < b.size(); ++i)
      BOOST_CHECK(b[i] == true);

    // Test decrease in size
    b.resize(lhs.size());
    for (i = 0; i < lhs.size(); ++i)
      BOOST_CHECK(b[i] == lhs[i]);
  }

  static void clear(const Bitset& lhs)
  {
    Bitset b(lhs);
    b.clear();
    BOOST_CHECK(b.size() == 0);
  }

  static void append_bit(const Bitset& lhs)
  {
    Bitset b(lhs);
    b.push_back(true);
    BOOST_CHECK(b.size() == lhs.size() + 1);
    BOOST_CHECK(b[b.size() - 1] == true);
    for (std::size_t i = 0; i < lhs.size(); ++i)
      BOOST_CHECK(b[i] == lhs[i]);

    b.push_back(false);
    BOOST_CHECK(b.size() == lhs.size() + 2);
    BOOST_CHECK(b[b.size() - 1] == false);
    BOOST_CHECK(b[b.size() - 2] == true);
    for (std::size_t j = 0; j < lhs.size(); ++j)
      BOOST_CHECK(b[j] == lhs[j]);
  }

  static void append_block(const Bitset& lhs)
  {
    Bitset b(lhs);
    Block value(128);
    b.append(value);
    BOOST_CHECK(b.size() == lhs.size() + Bitset::bits_per_block);
    for (std::size_t i = 0; i < Bitset::bits_per_block; ++i)
      BOOST_CHECK(b[lhs.size() + i] == bool((value >> i) & 1));
  }

  static void append_block_range(const Bitset& lhs, std::vector<Block> blocks)
  {
    Bitset b(lhs), c(lhs);
    b.append(blocks.begin(), blocks.end());
    for (typename std::vector<Block>::iterator i = blocks.begin();
         i != blocks.end(); ++i)
      c.append(*i);
    BOOST_CHECK(b == c);
  }

  // operator[] and reference members
  // PRE: b[i] == bit_vec[i]
  static void operator_bracket(const Bitset& lhs, const std::vector<bool>& bit_vec)
  {
    Bitset b(lhs);
    std::size_t i, j, k;

    // x = b[i]
    // x = ~b[i]
    for (i = 0; i < b.size(); ++i) {
      bool x = b[i];
      BOOST_CHECK(x == bit_vec[i]);
      x = ~b[i];
      BOOST_CHECK(x == !bit_vec[i]);
    }
    Bitset prev(b);

    // b[i] = x
    for (j = 0; j < b.size(); ++j) {
      bool x = !prev[j];
      b[j] = x;
      for (k = 0; k < b.size(); ++k)
        if (j == k)
          BOOST_CHECK(b[k] == x);
        else
          BOOST_CHECK(b[k] == prev[k]);
      b[j] = prev[j];
    }
    b.flip();

    // b[i] = b[j]
    for (i = 0; i < b.size(); ++i) {
      b[i] = prev[i];
      for (j = 0; j < b.size(); ++j) {
        if (i == j)
          BOOST_CHECK(b[j] == prev[j]);
        else
          BOOST_CHECK(b[j] == !prev[j]);
      }
      b[i] = !prev[i];
    }

    // b[i].flip()
    for (i = 0; i < b.size(); ++i) {
      b[i].flip();
      for (j = 0; j < b.size(); ++j) {
        if (i == j)
          BOOST_CHECK(b[j] == prev[j]);
        else
          BOOST_CHECK(b[j] == !prev[j]);
      }
      b[i].flip();
    }
  }

  //===========================================================================
  // bitwise operators

  // bitwise and assignment

  // PRE: b.size() == rhs.size()
  static void and_assignment(const Bitset& b, const Bitset& rhs)
  {
    Bitset lhs(b);
    Bitset prev(lhs);
    lhs &= rhs;
    // Clears each bit in lhs for which the corresponding bit in rhs is
    // clear, and leaves all other bits unchanged.
    for (std::size_t I = 0; I < lhs.size(); ++I)
      if (rhs[I] == 0)
        BOOST_CHECK(lhs[I] == 0);
      else
        BOOST_CHECK(lhs[I] == prev[I]);
  }

  // PRE: b.size() == rhs.size()
  static void or_assignment(const Bitset& b, const Bitset& rhs)
  {
    Bitset lhs(b);
    Bitset prev(lhs);
    lhs |= rhs;
    // Sets each bit in lhs for which the corresponding bit in rhs is set, and
    // leaves all other bits unchanged.
    for (std::size_t I = 0; I < lhs.size(); ++I)
      if (rhs[I] == 1)
        BOOST_CHECK(lhs[I] == 1);
      else
        BOOST_CHECK(lhs[I] == prev[I]);
  }

  // PRE: b.size() == rhs.size()
  static void xor_assignment(const Bitset& b, const Bitset& rhs)
  {
    Bitset lhs(b);
    Bitset prev(lhs);
    lhs ^= rhs;
    // Flips each bit in lhs for which the corresponding bit in rhs is set,
    // and leaves all other bits unchanged.
    for (std::size_t I = 0; I < lhs.size(); ++I)
      if (rhs[I] == 1)
        BOOST_CHECK(lhs[I] == !prev[I]);
      else
        BOOST_CHECK(lhs[I] == prev[I]);
  }

  // PRE: b.size() == rhs.size()
  static void sub_assignment(const Bitset& b, const Bitset& rhs)
  {
    Bitset lhs(b);
    Bitset prev(lhs);
    lhs -= rhs;
    // Resets each bit in lhs for which the corresponding bit in rhs is set,
    // and leaves all other bits unchanged.
    for (std::size_t I = 0; I < lhs.size(); ++I)
      if (rhs[I] == 1)
        BOOST_CHECK(lhs[I] == 0);
      else
        BOOST_CHECK(lhs[I] == prev[I]);
  }

  static void shift_left_assignment(const Bitset& b, std::size_t pos)
  {
    Bitset lhs(b);
    Bitset prev(lhs);
    lhs <<= pos;
    // Replaces each bit at position I in lhs with the following value:
    // - If I < pos, the new value is zero
    // - If I >= pos, the new value is the previous value of the bit at
    //   position I - pos
    for (std::size_t I = 0; I < lhs.size(); ++I)
      if (I < pos)
        BOOST_CHECK(lhs[I] == 0);
      else
        BOOST_CHECK(lhs[I] == prev[I - pos]);
  }

  static void shift_right_assignment(const Bitset& b, std::size_t pos)
  {
    Bitset lhs(b);
    Bitset prev(lhs);
    lhs >>= pos;
    // Replaces each bit at position I in lhs with the following value:
    // - If pos >= N - I, the new value is zero
    // - If pos < N - I, the new value is the previous value of the bit at
    //    position I + pos
    std::size_t N = lhs.size();
    for (std::size_t I = 0; I < N; ++I)
      if (pos >= N - I)
        BOOST_CHECK(lhs[I] == 0);
      else
        BOOST_CHECK(lhs[I] == prev[I + pos]);
  }

  static void set_all(const Bitset& b)
  {
    Bitset lhs(b);
    lhs.set();
    for (std::size_t I = 0; I < lhs.size(); ++I)
      BOOST_CHECK(lhs[I] == 1);
  }

  static void set_one(const Bitset& b, std::size_t pos, bool value)
  {
    Bitset lhs(b);
    std::size_t N = lhs.size();
    if (pos < N) {
      Bitset prev(lhs);
      // Stores a new value in the bit at position pos in lhs.
      lhs.set(pos, value);
      BOOST_CHECK(lhs[pos] == value);

      // All other values of lhs remain unchanged
      for (std::size_t I = 0; I < N; ++I)
        if (I != pos)
          BOOST_CHECK(lhs[I] == prev[I]);
    } else {
      // Not in range, doesn't satisfy precondition.
    }
  }

  static void reset_all(const Bitset& b)
  {
    Bitset lhs(b);
    // Resets all bits in lhs
    lhs.reset();
    for (std::size_t I = 0; I < lhs.size(); ++I)
      BOOST_CHECK(lhs[I] == 0);
  }

  static void reset_one(const Bitset& b, std::size_t pos)
  {
    Bitset lhs(b);
    std::size_t N = lhs.size();
    if (pos < N) {
      Bitset prev(lhs);
      lhs.reset(pos);
      // Resets the bit at position pos in lhs
      BOOST_CHECK(lhs[pos] == 0);

      // All other values of lhs remain unchanged
      for (std::size_t I = 0; I < N; ++I)
        if (I != pos)
          BOOST_CHECK(lhs[I] == prev[I]);
    } else {
      // Not in range, doesn't satisfy precondition.
    }
  }

  static void operator_flip(const Bitset& b)
  {
    Bitset lhs(b);
    Bitset x(lhs);
    BOOST_CHECK(~lhs == x.flip());
  }

  static void flip_all(const Bitset& b)
  {
    Bitset lhs(b);
    std::size_t N = lhs.size();
    Bitset prev(lhs);
    lhs.flip();
    // Toggles all the bits in lhs
    for (std::size_t I = 0; I < N; ++I)
      BOOST_CHECK(lhs[I] == !prev[I]);
  }

  static void flip_one(const Bitset& b, std::size_t pos)
  {
    Bitset lhs(b);
    std::size_t N = lhs.size();
    if (pos < N) {
      Bitset prev(lhs);
      lhs.flip(pos);
      // Toggles the bit at position pos in lhs
      BOOST_CHECK(lhs[pos] == !prev[pos]);

      // All other values of lhs remain unchanged
      for (std::size_t I = 0; I < N; ++I)
        if (I != pos)
          BOOST_CHECK(lhs[I] == prev[I]);
    } else {
      // Not in range, doesn't satisfy precondition.
    }
  }

  // to_ulong()
  static void to_ulong(const Bitset& lhs)
  {
    std::size_t N = lhs.size();
    std::size_t n = CHAR_BIT * sizeof(unsigned long);
    bool will_overflow = false;
    for (std::size_t I = n; I < N; ++I)
      if (lhs[I] != 0)
        will_overflow = true;
    if (will_overflow) {
      try {
        (void)lhs.to_ulong();
        BOOST_CHECK(false); // It should have thrown and exception
      } catch (std::overflow_error) {
        // Good!
      } catch (...) {
        BOOST_CHECK(false); // threw the wrong exception
      }
    } else {
      unsigned long num = lhs.to_ulong();
      // Make sure the number is right
      for (std::size_t I = 0; I < N; ++I)
        BOOST_CHECK(lhs[I] == nth_bit(num, I));
    }
  }

  // to_string()
  static void to_string(const Bitset& b)
  {
    // Construct a string object of the appropriate type and initializes
    // it to a string of length N characters. Each character is determined
    // by the value of its corresponding bit position in b. Character
    // position N - 1 corresponds to bit position zero. Sebsequent
    // decreasing character positions correspond to increasing bit
    // positions. Bit value zero becomes the charactet 0, bit value one
    // becomes the character 1.
    std::string str;
    boost::to_string(b, str);
    std::size_t N = b.size();
    BOOST_CHECK(str.size() == b.size());
    for (std::size_t I = 0; I < b.size(); ++I)
      BOOST_CHECK(b[I] == 0 ? (str[N - 1 - I] == '0') : (str[N - 1 - I] == '1'));
  }

  static void count(const Bitset& b)
  {
    std::size_t c = b.count();
    std::size_t c_real = 0;
    for (std::size_t I = 0; I < b.size(); ++I)
      if (b[I])
        ++c_real;
    BOOST_CHECK(c == c_real);
  }

  static void size(const Bitset& b)
  {
    BOOST_CHECK(Bitset(b).set().count() == b.size());
  }

  static void any(const Bitset& b)
  {
    BOOST_CHECK(b.any() == (b.count() > 0));
  }

  static void none(const Bitset& b)
  {
    BOOST_CHECK(b.none() == (b.count() == 0));
  }

  static void subset(const Bitset& a, const Bitset& b)
  {
    if (a.is_subset_of(b)) {
      for (std::size_t I = 0; I < a.size(); ++I)
        if (a[I])
          BOOST_CHECK(b[I]);
    } else {
      bool is_subset = true;
      for (std::size_t I = 0; I < a.size(); ++I)
        if (a[I] && !b[I]) {
          is_subset = false;
          break;
        }
      BOOST_CHECK(is_subset == false);
    }
  }

  static void proper_subset(const Bitset& a, const Bitset& b)
  {
    if (a.is_proper_subset_of(b)) {
      for (std::size_t I = 0; I < a.size(); ++I)
        if (a[I])
          BOOST_CHECK(b[I]);
      BOOST_CHECK(a.count() < b.count());
    } else {
      bool is_subset = true;
      for (std::size_t I = 0; I < a.size(); ++I)
        if (a[I] && !b[I]) {
          is_subset = false;
          break;
        }
      BOOST_CHECK(is_subset == false || a.count() >= b.count());
    }
  }

  static void operator_equal(const Bitset& a, const Bitset& b)
  {
    if (a == b) {
      for (std::size_t I = 0; I < a.size(); ++I)
        BOOST_CHECK(a[I] == b[I]);
    } else {
      if (a.size() == b.size()) {
        bool diff = false;
        for (std::size_t I = 0; I < a.size(); ++I)
          if (a[I] != b[I]) {
            diff = true;
            break;
          }
        BOOST_CHECK(diff);
      }
    }
  }

  static void operator_not_equal(const Bitset& a, const Bitset& b)
  {
    if (a != b) {
      if (a.size() == b.size()) {
        bool diff = false;
        for (std::size_t I = 0; I < a.size(); ++I)
          if (a[I] != b[I]) {
            diff = true;
            break;
          }
        BOOST_CHECK(diff);
      }
    } else {
      for (std::size_t I = 0; I < a.size(); ++I)
        BOOST_CHECK(a[I] == b[I]);
    }
  }

  static bool less_than(const Bitset& a, const Bitset& b)
  {
    // Compare from most significant to least.
    // Careful, don't send unsigned int into negative territory!
    if (a.size() == 0)
      return false;

    std::size_t I;
    for (I = a.size() - 1; I > 0; --I)
      if (a[I] < b[I])
        return true;
      else if (a[I] > b[I])
        return false;
      // if (a[I] = b[I]) skip to next

    if (a[0] < b[0])
      return true;
    else
      return false;
  }

  static void operator_less_than(const Bitset& a, const Bitset& b)
  {
    if (less_than(a, b))
      BOOST_CHECK(a < b);
    else
      BOOST_CHECK(!(a < b));
  }

  static void operator_greater_than(const Bitset& a, const Bitset& b)
  {
    if (less_than(a, b) || a == b)
      BOOST_CHECK(!(a > b));
    else
      BOOST_CHECK(a > b);
  }

  static void operator_less_than_eq(const Bitset& a, const Bitset& b)
  {
    if (less_than(a, b) || a == b)
      BOOST_CHECK(a <= b);
    else
      BOOST_CHECK(!(a <= b));
  }

  static void operator_greater_than_eq(const Bitset& a, const Bitset& b)
  {
    if (less_than(a, b))
      BOOST_CHECK(!(a >= b));
    else
      BOOST_CHECK(a >= b);
  }

  static void test_bit(const Bitset& b, std::size_t pos)
  {
    Bitset lhs(b);
    std::size_t N = lhs.size();
    if (pos < N) {
      BOOST_CHECK(lhs.test(pos) == lhs[pos]);
    } else {
      // Not in range, doesn't satisfy precondition.
    }
  }

  static void operator_shift_left(const Bitset& lhs, std::size_t pos)
  {
    Bitset x(lhs);
    BOOST_CHECK((lhs << pos) == (x <<= pos));
  }

  static void operator_shift_right(const Bitset& lhs, std::size_t pos)
  {
    Bitset x(lhs);
    BOOST_CHECK((lhs >> pos) == (x >>= pos));
  }

  // operator|
  static
  void operator_or(const Bitset& lhs, const Bitset& rhs)
  {
    Bitset x(lhs);
    BOOST_CHECK((lhs | rhs) == (x |= rhs));
  }

  // operator&
  static
  void operator_and(const Bitset& lhs, const Bitset& rhs)
  {
    Bitset x(lhs);
    BOOST_CHECK((lhs & rhs) == (x &= rhs));
  }

  // operator^
  static
  void operator_xor(const Bitset& lhs, const Bitset& rhs)
  {
    Bitset x(lhs);
    BOOST_CHECK((lhs ^ rhs) == (x ^= rhs));
  }

  // operator-
  static
  void operator_sub(const Bitset& lhs, const Bitset& rhs)
  {
    Bitset x(lhs);
    BOOST_CHECK((lhs - rhs) == (x -= rhs));
  }

  // operator<<(ostream,
  // operator>>(istream,

  static
  void stream_read_write(const Bitset& out, const Bitset& in)
  {
    Bitset x(in);
    {
      std::ofstream f("tmp");
      f << out;
    }
    {
      std::ifstream f("tmp");
      f >> x;
      BOOST_CHECK(out == x);
    }
  }

};