///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh, New York University,
//                          Max Planck Gesellschaft
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_NUMDIFF_COST_HPP_
#define CROCODDYL_CORE_NUMDIFF_COST_HPP_

#include <boost/function.hpp>
#include "crocoddyl/multibody/fwd.hpp"
#include "crocoddyl/core/cost-base.hpp"

namespace crocoddyl {

template <typename _Scalar>
class CostModelNumDiffTpl : public CostModelAbstractTpl<_Scalar> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef CostDataAbstractTpl<Scalar> CostDataAbstract;
  typedef CostModelAbstractTpl<Scalar> Base;
  typedef CostDataNumDiffTpl<Scalar> Data;
  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;
  typedef typename MathBaseTpl<Scalar>::MatrixXs MatrixXs;
  typedef boost::function<void(const VectorXs&)> ReevaluationFunction;

  /**
   * @brief Initialize the numdiff cost model
   *
   * @param model
   */
  explicit CostModelNumDiffTpl(const boost::shared_ptr<Base>& model);

  /**
   * @brief Initialize the numdiff cost model
   */
  virtual ~CostModelNumDiffTpl();

  /**
   * @brief @copydoc CostModelAbstract::calc()
   */
  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                    const Eigen::Ref<const VectorXs>& u);

  /**
   * @brief @copydoc CostModelAbstract::calcDiff()
   */
  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                        const Eigen::Ref<const VectorXs>& u);

  /**
   * @brief Create a Data object
   *
   * @param data  Data collector used by the original model
   * @return the cost data
   */
  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);

  /**
   * @brief Return the original cost model
   */
  const boost::shared_ptr<Base>& get_model() const;

  /**
   * @brief Return the disturbance value used by the numdiff routine
   */
  const Scalar& get_disturbance() const;

  /**
   * @brief Modify the disturbance value used by the numdiff routine
   */
  void set_disturbance(const Scalar& disturbance);

  /**
   * @brief Identify if the Gauss approximation is going to be used or not.
   *
   * @return true
   * @return false
   */
  bool get_with_gauss_approx();

  /**
   * @brief Register functions that updates the shared data computed for a system rollout
   * The updated data is used to evaluate of the gradient and hessian.
   *
   * @param reevals are the registered functions.
   */
  void set_reevals(const std::vector<ReevaluationFunction>& reevals);

 protected:
  using Base::activation_;
  using Base::nu_;
  using Base::state_;
  using Base::unone_;

  /** @brief Model of the cost. */
  boost::shared_ptr<Base> model_;

  /** @brief Numerical disturbance used in the numerical differentiation. */
  Scalar disturbance_;

  /** @brief Functions that needs execution before calc or calcDiff. */
  std::vector<ReevaluationFunction> reevals_;

 private:
  /**
   * @brief Make sure that when we finite difference the Cost Model, the user
   * does not face unknown behaviour because of the finite differencing of a
   * quaternion around pi. This behaviour might occur if CostModelState and
   * FloatingInContact differential model are used together.
   *
   * For full discussions see issue
   * https://gepgitlab.laas.fr/loco-3d/crocoddyl/issues/139
   *
   * @param x is the state at which the check is performed.
   */
  void assertStableStateFD(const Eigen::Ref<const VectorXs>& /*x*/);
};

template <typename _Scalar>
struct CostDataNumDiffTpl : public CostDataAbstractTpl<_Scalar> {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef CostDataAbstractTpl<Scalar> Base;
  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
  typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
  typedef typename MathBaseTpl<Scalar>::VectorXs VectorXs;

  template <template <typename Scalar> class Model>
  explicit CostDataNumDiffTpl(Model<Scalar>* const model, DataCollectorAbstract* const shared_data)
      : Base(model, shared_data),
        dx(model->get_state()->get_ndx()),
        xp(model->get_state()->get_nx()),
        du(model->get_nu()),
        up(model->get_nu()) {
    dx.setZero();
    xp.setZero();
    du.setZero();
    up.setZero();

    const std::size_t& ndx = model->get_model()->get_state()->get_ndx();
    const std::size_t& nu = model->get_model()->get_nu();
    data_0 = model->get_model()->createData(shared_data);
    for (std::size_t i = 0; i < ndx; ++i) {
      data_x.push_back(model->get_model()->createData(shared_data));
    }
    for (std::size_t i = 0; i < nu; ++i) {
      data_u.push_back(model->get_model()->createData(shared_data));
    }
  }

  virtual ~CostDataNumDiffTpl() {}

  using Base::activation;
  using Base::cost;
  using Base::Lu;
  using Base::Luu;
  using Base::Lx;
  using Base::Lxu;
  using Base::Lxx;
  using Base::r;
  using Base::Ru;
  using Base::Rx;
  using Base::shared;

  VectorXs dx;  //!< State disturbance.
  VectorXs xp;  //!< The integrated state from the disturbance on one DoF "\f$ \int x dx_i \f$".
  VectorXs du;  //!< Control disturbance.
  VectorXs up;  //!< The integrated control from the disturbance on one DoF "\f$ \int u du_i = u + du \f$".
  boost::shared_ptr<Base> data_0;                //!< The data at the approximation point.
  std::vector<boost::shared_ptr<Base> > data_x;  //!< The temporary data associated with the state variation.
  std::vector<boost::shared_ptr<Base> > data_u;  //!< The temporary data associated with the control variation.
};

}  // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/numdiff/cost.hxx"

#endif  // CROCODDYL_CORE_NUMDIFF_COST_HPP_


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

using namespace sam;

struct SetUp {
  ActiveWindow<size_t> aw;

  // Creating an active window with a limit of 6
  SetUp() : aw(6)
  {
    // Adding test data to the active window
    aw.update(1);
    aw.update(1);
    aw.update(1);
    aw.update(3);
    aw.update(3);
    aw.update(2);
  }

  ~SetUp() {}

};

// This tests adding an element to a full active window.
// It should return false;
BOOST_FIXTURE_TEST_CASE( active_window_test_limit, SetUp )
{
  BOOST_CHECK_EQUAL(false, aw.update(1));
}

BOOST_FIXTURE_TEST_CASE( active_window_test_get_num_elements, SetUp )
{
  BOOST_CHECK_EQUAL(6, aw.getNumElements());
}

BOOST_FIXTURE_TEST_CASE( active_window_test_topk, SetUp )
{
  std::vector<std::pair<size_t, size_t>> list1 = aw.topk(1);
  std::vector<std::pair<size_t, size_t>> list2 = aw.topk(2);
  std::vector<std::pair<size_t, size_t>> list3 = aw.topk(3);
  std::vector<std::pair<size_t, size_t>> list4 = aw.topk(4);

  BOOST_CHECK_EQUAL(1, list1.size());
  BOOST_CHECK_EQUAL(2, list2.size());
  BOOST_CHECK_EQUAL(3, list3.size());
  BOOST_CHECK_EQUAL(3, list4.size());

  BOOST_CHECK_EQUAL(1, list1[0].first);
  BOOST_CHECK_EQUAL(3, list1[0].second);
  BOOST_CHECK_EQUAL(1, list2[0].first);
  BOOST_CHECK_EQUAL(3, list2[0].second);
  BOOST_CHECK_EQUAL(3, list2[1].first);
  BOOST_CHECK_EQUAL(2, list2[1].second);
  BOOST_CHECK_EQUAL(1, list3[0].first);
  BOOST_CHECK_EQUAL(3, list3[0].second);
  BOOST_CHECK_EQUAL(3, list3[1].first);
  BOOST_CHECK_EQUAL(2, list3[1].second);
  BOOST_CHECK_EQUAL(2, list3[2].first);
  BOOST_CHECK_EQUAL(1, list3[2].second);
  BOOST_CHECK_EQUAL(1, list4[0].first);
  BOOST_CHECK_EQUAL(3, list4[0].second);
  BOOST_CHECK_EQUAL(3, list4[1].first);
  BOOST_CHECK_EQUAL(2, list4[1].second);
  BOOST_CHECK_EQUAL(2, list4[2].first);
  BOOST_CHECK_EQUAL(1, list4[2].second);
}


// File: dense2D.cpp

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

int main(int, char**)
{
    using namespace mtl;

    // A is a row-major matrix
    dense2D<double>               A(10, 10);

    // Matrices are not initialized by default
    A= 0.0;

    // Assign a value to a matrix element
    A(2, 3)= 7.0;

    // You can also use a more C-like notation
    A[2][4]= 3.0;

    std::cout << "A is \n" << A << "\n";

    // B is a column-major matrix
    dense2D<float, mat::parameters<tag::col_major> > B(10, 10);

    // Assign the identity matrix times 3 to B
    B= 3;
    std::cout << "B is \n" << B << "\n";

    return 0;
}


#pragma once

////////////////////////////////////////////////////////////////////////////////
#include <polysolve/LinearSolver.hpp>
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <vector>

////////////////////////////////////////////////////////////////////////////////
//

namespace polysolve
{

    class SaddlePointSolver : public LinearSolver
    {

    public:
        SaddlePointSolver();
        ~SaddlePointSolver();

    private:
        POLYSOLVE_DELETE_MOVE_COPY(SaddlePointSolver)

    public:
        //////////////////////
        // Public interface //
        //////////////////////

        // Set solver parameters
        virtual void setParameters(const json &params) override;

        // Retrieve memory information from Pardiso
        virtual void getInfo(json &params) const override;

        // Analyze sparsity pattern
        virtual void analyzePattern(const StiffnessMatrix &A, const int precond_num) override { precond_num_ = precond_num; }

        // Factorize system matrix
        virtual void factorize(const StiffnessMatrix &A) override;

        // Solve the linear system Ax = b
        virtual void solve(const Ref<const VectorXd> b, Ref<VectorXd> x) override;

        // Name of the solver type (for debugging purposes)
        virtual std::string name() const override { return "SaddleProblemSolver"; }

    private:
        StiffnessMatrix Ain_;

        StiffnessMatrix As;
        StiffnessMatrix Bs;
        StiffnessMatrix BsT;
        StiffnessMatrix Cs;
        StiffnessMatrix Ss;

        StiffnessMatrix Wm;
        StiffnessMatrix Wc;

        int max_iter_;
        double conv_tol_;

        int precond_num_;

        std::string asymmetric_solver_name_;
        std::string symmetric_solver_name_;

        json asymmetric_solver_params_;
        json symmetric_solver_params_;

        double final_res_norm_;
        int num_iterations_;
    };

} // namespace polysolve


#include "stdafx.h"

#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/toolbox/metafunctions/get_num_bits.hpp>

#include <boost/test/unit_test.hpp>

#include "base_types.hpp"

using namespace boost;
using namespace gil;

BOOST_AUTO_TEST_SUITE( gil_test )

//
// packed_channel_value<12> represents a 12 bit value type.
//
// These tests make sure that a value is represented correctly.
//

template< int K >
void test_packed_channel()
{
    typedef packed_channel_value<K> bits_t;

    BOOST_CHECK_EQUAL( channel_traits<bits_t>::min_value(),                                         0 );
    BOOST_CHECK_EQUAL( channel_traits<bits_t>::max_value(),                ( uint64_t( 1 ) << K ) - 1 );
    BOOST_CHECK_EQUAL( gil::detail::unsigned_integral_max_value<bits_t>::value, ( uint64_t( 1 ) << K ) - 1 );

    BOOST_CHECK_EQUAL( sizeof( bits_t ), K / 8 + 1 );
    BOOST_STATIC_ASSERT(( boost::is_integral< bits_t >::value ));

    {
        typedef packed_channel_value< K > bits_t;
        bits_t::value_type v = bits_t::min_value();
    }

    {
        unsigned int v = 10;

        typedef packed_channel_value< 5 > bits_t;
        bits_t b( v );

        bits_t::integer_t v2 = b;
        BOOST_CHECK_EQUAL( v, v2 );
    }

    {
        typedef packed_channel_value< 12 > bits_t;
        bits_t a(5), b(5), c;

        c = channel_multiply( a, b );

        BOOST_CHECK_EQUAL( channel_multiply( bits_t( 5 ), bits_t( 5 ) ), bits_t( 0 ) );
    }

    {
        unsigned int value = 0;

        typedef packed_channel_reference< unsigned short, 0, 5, 1 > channel_t;
        channel_t c( &value );
        c = channel_traits<channel_t>::min_value();
    }
}

BOOST_AUTO_TEST_CASE( packed_channel_test )
{
    // use BOOST_PP_REPEAT_FROM_TO for complete test
    test_packed_channel<  1 >();
    test_packed_channel<  7 >();
    test_packed_channel< 24 >();
    test_packed_channel< 31 >();
    test_packed_channel< 63 >();

    mpl::false_ oo;
    mpl::true_ pp;
    auto ll = is_integral< unsigned char >::type();
    auto kk = mpl::not_< is_class< unsigned char > >::type();
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (C) 2011 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include "common_tests_for_spaces.hpp"
#include "../check_arrays_are_close.hpp"
#include "../type_template.hpp"
#include "../assembly/create_regular_grid.hpp"

#include "assembly/assembly_options.hpp"
#include "assembly/context.hpp"
#include "assembly/grid_function.hpp"
#include "assembly/l2_norm.hpp"
#include "assembly/numerical_quadrature_strategy.hpp"
#include "assembly/surface_normal_independent_function.hpp"

#include "common/scalar_traits.hpp"

#include "grid/grid.hpp"
#include "grid/grid_factory.hpp"

#include "space/raviart_thomas_0_vector_space.hpp"

#include <boost/type_traits/is_complex.hpp>
#include <boost/test/floating_point_comparison.hpp>

using namespace Bempp;

/* Grid:

  -----------------
  |  /|  /|  /|  /|
  | / | / | / | / |
  |/  |/  |/  |/  |
  -----------------
  |  /|  /|  /|  /|
  | / | / | / | / |
  |/  |/  |/  |/  |
  -----------------
*/

// This function is composed of all the basis functions having a nonzero
// normal to the diagonal edges of the grid.

template <typename ValueType_>
class MyFunction
{
public:
    typedef ValueType_ ValueType;
    typedef typename ScalarTraits<ValueType>::RealType CoordinateType;

    int argumentDimension() const { return 3; }
    int resultDimension() const { return 3; }

    inline void evaluate(const arma::Col<CoordinateType>& point,
                         arma::Col<ValueType>& result) const {
        CoordinateType x = point(0), y = point(1);
        CoordinateType red_x = fmod(x, 0.2);
        CoordinateType red_y = fmod(y, 0.2);
        if (red_x  > red_y) {
            result(0) = 0.2 - red_x;
            result(1) = -red_y;
        } else {
            result(0) = red_x;
            result(1) = -0.2 + red_y;
        }
        result(2) = 0.;
    }
};

// Tests

BOOST_AUTO_TEST_SUITE(RaviartThomas0VectorSpace_)

BOOST_AUTO_TEST_CASE_TEMPLATE(local2global_matches_global2local_for_closed_surface_without_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.1.msh", false /* verbose */);

    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(grid));

    local2global_matches_global2local(*space);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(local2global_matches_global2local_for_open_surface_with_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    shared_ptr<Grid> grid = createRegularTriangularGrid(5, 10, 1., 2.);
    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(grid, true /* put dofs on boundary */));

    local2global_matches_global2local(*space);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(local2global_matches_global2local_for_closed_surface_with_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.1.msh", false /* verbose */);

    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(grid, true /* put dofs on boundary */));

    local2global_matches_global2local(*space);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(global2local_matches_local2global_for_closed_surface_without_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.1.msh", false /* verbose */);

    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(grid));

    global2local_matches_local2global(*space);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(global2local_matches_local2global_for_open_surface_with_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    shared_ptr<Grid> grid = createRegularTriangularGrid(5, 10, 1., 2.);
    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(grid, true /* put dofs on boundary */));

    global2local_matches_local2global(*space);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(global2local_matches_local2global_for_open_surface_without_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    shared_ptr<Grid> grid = createRegularTriangularGrid(5, 10, 1., 2.);
    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(grid));

    global2local_matches_local2global(*space);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(local2global_matches_global2local_for_closed_surface_with_segment_with_edge_on_segment_with_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.1.msh", false /* verbose */);

    GridSegment segment = gridSegmentWithPositiveX(*grid);
    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(
                    grid, segment, true /* put dofs on boundary */,
                    EDGE_ON_SEGMENT));

    local2global_matches_global2local(*space);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(global2local_matches_local2global_for_closed_surface_with_segment_with_edge_on_segment_without_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.1.msh", false /* verbose */);

    GridSegment segment = gridSegmentWithPositiveX(*grid);
    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(
                    grid, segment, false /* don't put dofs on boundary */,
                    EDGE_ON_SEGMENT));

    global2local_matches_local2global(*space);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(local2global_matches_global2local_for_closed_surface_with_segment_with_element_on_segment_with_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.1.msh", false /* verbose */);

    GridSegment segment = gridSegmentWithPositiveX(*grid);
    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(
                    grid, segment, true /* put dofs on boundary */,
                    ELEMENT_ON_SEGMENT));

    local2global_matches_global2local(*space);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(global2local_matches_local2global_for_closed_surface_with_segment_with_element_on_segment_without_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.1.msh", false /* verbose */);

    GridSegment segment = gridSegmentWithPositiveX(*grid);
    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(
                    grid, segment, false /* don't put dofs on boundary */,
                    ELEMENT_ON_SEGMENT));

    global2local_matches_local2global(*space);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(local2global_matches_global2local_for_closed_surface_with_segment_with_edge_and_element_on_segment_with_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.1.msh", false /* verbose */);

    GridSegment segment = gridSegmentWithPositiveX(*grid);
    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(
                    grid, segment, true /* put dofs on boundary */,
                    EDGE_ON_SEGMENT | ELEMENT_ON_SEGMENT));

    local2global_matches_global2local(*space);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(global2local_matches_local2global_for_closed_surface_with_segment_with_edge_and_element_on_segment_without_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.1.msh", false /* verbose */);

    GridSegment segment = gridSegmentWithPositiveX(*grid);
    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(
                    grid, segment, false /* don't put dofs on boundary */,
                    EDGE_ON_SEGMENT | ELEMENT_ON_SEGMENT));

    global2local_matches_local2global(*space);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(local2global_matches_global2local_for_closed_surface_with_segment_with_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.1.msh", false /* verbose */);

    GridSegment segment = gridSegmentWithPositiveX(*grid);
    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(grid, segment, true /* put dofs on boundary */));

    local2global_matches_global2local(*space);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(global2local_matches_local2global_for_closed_surface_with_segment_without_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;
    shared_ptr<Grid> grid = GridFactory::importGmshGrid(
        params, "../../meshes/sphere-h-0.1.msh", false /* verbose */);

    GridSegment segment = gridSegmentWithPositiveX(*grid);
    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(grid, segment));

    global2local_matches_local2global(*space);
}

//BOOST_AUTO_TEST_CASE_TEMPLATE(global2local_matches_local2global_for_open_surface_with_segment_with_dofs_on_boundary, ResultType, result_types)
//{
//    typedef ResultType RT;
//    typedef typename ScalarTraits<RT>::RealType BFT;
//    typedef typename ScalarTraits<RT>::RealType CT;

//    shared_ptr<Grid> grid = createRegularTriangularGrid(5, 10, 1., 2.);
//    shared_ptr<Space<BFT> > space(
//        new RaviartThomas0VectorSpace<BFT>(grid, true /* put dofs on boundary */));

//    global2local_matches_local2global(*space);
//}

BOOST_AUTO_TEST_CASE_TEMPLATE(global2local_matches_local2global_for_open_surface_with_segment_without_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    shared_ptr<Grid> grid = createRegularTriangularGrid(5, 10, 1., 2.);
    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(grid));

    global2local_matches_local2global(*space);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(my_function_can_be_expanded_in_rt_space_with_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    shared_ptr<Grid> grid = createRegularTriangularGrid(5, 10, 1., 2.);

    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(grid,
                                           true /* put DOFs on boundary */));

    AccuracyOptions accuracyOptions;
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
        new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));
    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    GridFunction<BFT, RT> function(
        context, space, space,
        surfaceNormalIndependentFunction(
                    MyFunction<RT>()));
    CT absoluteError, relativeError;
    estimateL2Error(
        function, surfaceNormalIndependentFunction(
                    MyFunction<RT>()),
        *quadStrategy, absoluteError, relativeError);
    BOOST_CHECK_SMALL(relativeError,
                      1000 * std::numeric_limits<CT>::epsilon() /* percent */);
}

BOOST_AUTO_TEST_CASE_TEMPLATE(my_function_can_be_expanded_in_rt_space_without_dofs_on_boundary, ResultType, result_types)
{
    typedef ResultType RT;
    typedef typename ScalarTraits<RT>::RealType BFT;
    typedef typename ScalarTraits<RT>::RealType CT;

    shared_ptr<Grid> grid = createRegularTriangularGrid(5, 10, 1., 2.);

    shared_ptr<Space<BFT> > space(
        new RaviartThomas0VectorSpace<BFT>(grid,
                                           false /* put DOFs on boundary */));

    AccuracyOptions accuracyOptions;
    accuracyOptions.singleRegular.setRelativeQuadratureOrder(2);
    shared_ptr<NumericalQuadratureStrategy<BFT, RT> > quadStrategy(
        new NumericalQuadratureStrategy<BFT, RT>(accuracyOptions));
    AssemblyOptions assemblyOptions;
    assemblyOptions.setVerbosityLevel(VerbosityLevel::LOW);
    shared_ptr<Context<BFT, RT> > context(
        new Context<BFT, RT>(quadStrategy, assemblyOptions));

    GridFunction<BFT, RT> function(
        context, space, space,
        surfaceNormalIndependentFunction(
                    MyFunction<RT>()));
    CT absoluteError, relativeError;
    estimateL2Error(
        function, surfaceNormalIndependentFunction(
                    MyFunction<RT>()),
        *quadStrategy, absoluteError, relativeError);
    BOOST_CHECK_SMALL(relativeError,
                      1000 * std::numeric_limits<CT>::epsilon() /* percent */);
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_DYN_LINK

// clang-format off
#include <boost/test/unit_test.hpp>
// clang-format on

#include <nuschl/s_exp.hpp>
#include <nuschl/memory/s_exp_pool.hpp>

#include <nuschl/unittests/unique.hpp>

#include <sstream>

using namespace nuschl;

BOOST_AUTO_TEST_SUITE(S_Exp)

BOOST_AUTO_TEST_CASE(Nil) {
    BOOST_CHECK(s_exp::nil->is_nil());
    BOOST_CHECK(!s_exp(s_exp::nil, s_exp::nil).is_nil());
    BOOST_CHECK(!s_exp(nullptr, nullptr).is_nil());
}

BOOST_AUTO_TEST_CASE(Create_from_atom) {
    const atom_ptr a = make_atom("hello");
    s_exp e(a);

    BOOST_REQUIRE(e.is_atom());
    BOOST_REQUIRE(e.get_atom()->is_symbol());
    BOOST_CHECK_EQUAL(e.get_atom()->get_symbol().get_value(), "hello");

    BOOST_CHECK(!e.is_cell());
    BOOST_CHECK(!e.is_primitive());
    BOOST_CHECK(!e.is_lambda());
    BOOST_CHECK(!e.is_nil());
}

BOOST_AUTO_TEST_CASE(Create_list) {
    const atom_ptr a1 = make_atom(number{1});
    const atom_ptr a2 = make_atom(number{2});
    const atom_ptr a3 = make_atom(number{3});

    s_exp e3(a3);
    s_exp e2(a2);
    s_exp e1(a1);
    s_exp l3(&e3, s_exp::nil);
    s_exp l2(&e2, &l3);
    s_exp l(&e1, &l2);

    BOOST_REQUIRE(l.is_cell());
    BOOST_REQUIRE(l2.is_cell());
    BOOST_REQUIRE(l3.is_cell());
    BOOST_CHECK_EQUAL(l.car(), &e1);
    BOOST_CHECK_EQUAL(l.cdr(), &l2);
    BOOST_CHECK_EQUAL(l2.car(), &e2);
    BOOST_CHECK_EQUAL(l2.cdr(), &l3);
    BOOST_CHECK_EQUAL(l3.car(), &e3);
    BOOST_CHECK_EQUAL(l3.cdr(), s_exp::nil);

    BOOST_CHECK(!l.is_atom());
    BOOST_CHECK(!l.is_primitive());
    BOOST_CHECK(!l.is_lambda());
}

BOOST_AUTO_TEST_CASE(Ostream_nil) {
    std::stringstream ss;
    ss << s_exp::nil;
    BOOST_CHECK_EQUAL(ss.str(), "nil");
}

BOOST_AUTO_TEST_CASE(Ostream_atom) {
    std::stringstream ss;
    const atom_ptr a = make_atom("hello");
    s_exp e(a);
    ss << &e;
    BOOST_CHECK_EQUAL(ss.str(), "hello");
    ss.str("");

    const atom_ptr b = make_atom(number{1});
    e = s_exp(b);
    ss << &e;
    BOOST_CHECK_EQUAL(ss.str(), "1");
}

BOOST_AUTO_TEST_CASE(Ostream_list) {
    std::stringstream ss;

    const atom_ptr a1 = make_atom(number{1});
    const atom_ptr a2 = make_atom(number{2});
    const atom_ptr a3 = make_atom(number{3});

    s_exp e3(a3);
    s_exp e2(a2);
    s_exp e1(a1);
    s_exp l3(&e3, s_exp::nil);
    s_exp l2(&e2, &l3);
    s_exp l(&e1, &l2);

    ss << &l;
    BOOST_CHECK_EQUAL(ss.str(), "(1, (2, (3, nil)))");
}

BOOST_AUTO_TEST_CASE(Equality_Nil) {
    s_exp s{s_exp::nil, s_exp::nil};
    BOOST_CHECK(*s_exp::nil != s);
    BOOST_CHECK_EQUAL(*s_exp::nil, *s_exp::nil);
}

BOOST_AUTO_TEST_CASE(Equality_Empty) {
    s_exp s{s_exp::nil, s_exp::nil};
    s_exp t{s_exp::nil, s_exp::nil};
    BOOST_CHECK_EQUAL(s, s);
    BOOST_CHECK_EQUAL(t, s);
}

BOOST_AUTO_TEST_CASE(Equality_Atom) {
    auto e1 = test::make_unique_num(1);
    auto e2 = test::make_unique_num(1);
    auto e3 = test::make_unique_num(2);

    BOOST_CHECK(e1 != e2);
    BOOST_CHECK_EQUAL(*e1.get(), *e1.get());
    BOOST_CHECK_EQUAL(*e1.get(), *e2.get());
    BOOST_CHECK_NE(*e1.get(), *e3.get());
}

memory::s_exp_pool pool;

BOOST_AUTO_TEST_CASE(Equality_List) {
    auto e1 = pool.create(s_exp::nil, s_exp::nil);
    auto e2 = pool.create(s_exp::nil, s_exp::nil);
    auto a1 = pool.create(make_atom(number{1}));
    auto b1 = pool.create(make_atom(number{1}));
    auto a = pool.create(a1, e1);
    auto b = pool.create(b1, e2);
    auto c2 = pool.create(make_atom(number{2}));
    auto d2 = pool.create(make_atom(number{2}));
    auto c = pool.create(c2, a);
    auto d = pool.create(d2, b);
    BOOST_CHECK_EQUAL(*c, *d);
}

BOOST_AUTO_TEST_SUITE_END()


// kate: replace-tabs off; indent-width 4; indent-mode normal
// vim: ts=4:sw=4:noexpandtab
/*

Copyright (c) 2010--2012,
François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
You can contact the authors at <f dot pomerleau at gmail dot com> and
<stephane at magnenat dot net>

All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the <organization> nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include "Histogram.h"

#include <iostream>
#include <fstream>
#include <iomanip>
#include <limits>
#include <algorithm>
#include <boost/format.hpp>

namespace PointMatcherSupport
{
	using namespace std;

	template<typename T>
	Histogram<T>::Histogram(const size_t binCount, const std::string& name, const std::string&
		filePrefix, const bool dumpStdErrOnExit):
		binCount(binCount),
		name(name),
		filePrefix(filePrefix),
		dumpStdErrOnExit(dumpStdErrOnExit)
	{
	}

	template<typename T>
	Histogram<T>::~Histogram()
	{
		T meanV, varV, medianV, lowQt, highQt, minV, maxV;
		uint64_t maxBinC;
		if (!dumpStdErrOnExit && filePrefix.empty())
			return;

		const vector<uint64_t> bins(computeStats(meanV, varV, medianV, lowQt, highQt, minV, maxV, maxBinC));

		if (!filePrefix.empty())
		{
			std::cerr << "writing to " << (filePrefix + name + "Stats.csv") << std::endl;
			std::ofstream ofs_stats((filePrefix + name + "Stats.csv").c_str());
			dumpStatsHeader(ofs_stats);
			ofs_stats << endl;
			dumpStats(ofs_stats);

			std::cerr << "writing to " << (filePrefix + name + ".csv") << std::endl;
			std::ofstream ofs((filePrefix + name + ".csv").c_str());
			for (size_t i = 0; i < this->size(); ++i)
				ofs << ((*this)[i]) << "\n";
		}

		if (dumpStdErrOnExit)
		{
			std::cerr.precision(4);
			std::cerr.fill(' ');
			std::cerr.flags(std::ios::left);
			std::cerr << "Histogram " << name << ":\n";
			std::cerr << "  count: " << this->size() << ", mean: " << meanV << ", var: " << varV << ", median: " << medianV << ", min: " << minV << ", max: " << maxV << ", lowQt: " << lowQt << ", highQt: " << highQt << ", maxBinC: " << maxBinC << "\n";
			if(this->size() > 1)
			{
				for (size_t i = 0; i < binCount; ++i)
				{
					const T v(minV + i * (maxV - minV) / T(binCount));
					std::cerr << "  " << std::setw(10) << v << " (" << std::setw(6) << bins[i] << ") : ";
					//std::cerr << (bins[i] * 60) / maxBinC << " " ;
					if (maxBinC > 0) {
						for (size_t j = 0; j < (bins[i] * 60) / maxBinC; ++j)
							std::cerr << "*";
					}
					std::cerr << "\n";
				}
				std::cerr << std::endl;
			}
		}
	}

	template<typename T>
	vector<uint64_t> Histogram<T>::computeStats(T& meanV, T& varV, T& medianV, T& lowQt, T& highQt, T& minV, T& maxV, uint64_t& maxBinC)
	{
		typedef typename std::vector<T>::iterator Iterator;
		vector<uint64_t> bins(binCount, 0);

		//assert(this->size() > 0);
		if(this->size() > 0)
		{
			// basic stats
			meanV = 0;
			minV = std::numeric_limits<T>::max();
			maxV = std::numeric_limits<T>::min();
			for (size_t i = 0; i < this->size(); ++i)
			{
				const T v((*this)[i]);
				meanV += v;
				minV = std::min<T>(minV, v);
				maxV = std::max<T>(maxV, v);
			}
			meanV /= T(this->size());
			// var and hist
			std::fill(bins.begin(), bins.end(), uint64_t(0));
			maxBinC = 0;
			varV = 0;
			if (minV == maxV)
			{
				medianV = lowQt = highQt = minV;
				return bins;
			}
			for (size_t i = 0; i < this->size(); ++i)
			{
				const T v((*this)[i]);
				varV += (v - meanV)*(v - meanV);
				const size_t index((v - minV) * (binCount) / ((maxV - minV) * (1+std::numeric_limits<T>::epsilon()*10)));
				//std::cerr << "adding value " << v << " to index " << index << std::endl;
				++bins[index];
				maxBinC = std::max<uint64_t>(maxBinC, bins[index]);
			}
			varV /= T(this->size());
			// median
			std::vector<T> hystCpy((*this));
			const Iterator lowQtIt(hystCpy.begin() + (hystCpy.size() / 4));
			const Iterator medianIt(hystCpy.begin() + (hystCpy.size() / 2));
			const Iterator highQtIt(hystCpy.begin() + (3*hystCpy.size() / 4));
			std::nth_element(hystCpy.begin(), medianIt, hystCpy.end());
			medianV = *medianIt;
			std::nth_element(hystCpy.begin(), lowQtIt, hystCpy.end());
			lowQt = *lowQtIt;
			std::nth_element(hystCpy.begin(), highQtIt, hystCpy.end());
			highQt = *highQtIt;
		}
		else
		{
			meanV = std::numeric_limits<T>::quiet_NaN();
			varV = std::numeric_limits<T>::quiet_NaN();
			medianV = std::numeric_limits<T>::quiet_NaN();
			lowQt = std::numeric_limits<T>::quiet_NaN();
			highQt = std::numeric_limits<T>::quiet_NaN();
			minV = std::numeric_limits<T>::quiet_NaN();
			maxV = std::numeric_limits<T>::quiet_NaN();
			maxBinC = 0;
		}
		return bins;
	}

	template<typename T>
	void Histogram<T>::dumpStats(std::ostream& os)
	{
		T meanV, varV, medianV, lowQt, highQt, minV, maxV;
		uint64_t maxBinC;
		const vector<uint64_t> bins(computeStats(meanV, varV, medianV, lowQt, highQt, minV, maxV, maxBinC));
		os << this->size() << ", " << meanV << ", " << varV << ", " << medianV << ", " << lowQt << ", " << highQt << ", " << minV << ", " << maxV << ", " << binCount << ", ";

		for (size_t i = 0; i < binCount; ++i)
			os << bins[i] << ", ";
		os << maxBinC;
	}

	template<typename T>
	void Histogram<T>::dumpStatsHeader(std::ostream& os) const
	{
		os << name + "_count, ";
		os << name + "_mean, ";
		os << name + "_var, ";
		os << name + "_median, ";
		os << name + "_low_quartile, ";
		os << name + "_high_quartile, ";
		os << name + "_min_value, ";
		os << name + "_max_value, ";
		os << name + "_bin_count, ";
		for (size_t i = 0; i < binCount; ++i)
			os << (boost::format("%1%_bin_%2%,") % name % i).str();
		os << name + "_max_elements_per_bin ";
	}

	template struct Histogram<unsigned>;
	template struct Histogram<float>;
	template struct Histogram<double>;
} // namespace PointMatcherSupport


// Copyright 2017-2019 the Autoware Foundation
//
// 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 Apex.AI, Inc.

#ifndef TEST_MAP_PUBLISHER_HPP_
#define TEST_MAP_PUBLISHER_HPP_

#include <ndt/ndt_map.hpp>
#include <pcl/io/pcd_io.h>
#include <gtest/gtest.h>
#include <geometry/spatial_hash_config.hpp>
#include <voxel_grid/config.hpp>
#include <geometry_msgs/msg/point32.hpp>
#include <lidar_utils/point_cloud_utils.hpp>
#include <Eigen/Core>
#include <vector>
#include <set>
#include <map>
#include "common/types.hpp"

using autoware::common::types::float32_t;
using autoware::common::types::float64_t;
using autoware::common::types::PointXYZI;
using autoware::common::lidar_utils::CloudModifier;
using autoware::common::lidar_utils::CloudView;

PointXYZI get_point_from_vector(const Eigen::Vector3d & v)
{
  return PointXYZI{
    static_cast<float32_t>(v(0)),
    static_cast<float32_t>(v(1)),
    static_cast<float32_t>(v(2)),
    0.0};
}

class DenseNDTMapContext
{
protected:
  static constexpr int POINTS_PER_DIM{5U};
  // how much should the points diverge from the center. It's fixed as there's no randomness.
  static constexpr float32_t FIXED_DEVIATION{0.3f};
  using PointXYZ = geometry_msgs::msg::Point32;

// add the point `center` and 4 additional points in a fixed distance from the center
// resulting in 7 points with random but bounded covariance
  void add_cell(
    CloudModifier & msg_wrapper,
    const Eigen::Vector3d & center, float64_t fixed_deviation)
  {
    msg_wrapper.push_back(get_point_from_vector(center));

    std::vector<Eigen::Vector3d> points;
    for (auto idx = 0U; idx < 3U; idx++) {
      for (auto mode = 0u; mode < 2u; mode++) {
        auto deviated_pt = center;
        if (mode == 0U) {
          deviated_pt(idx) += fixed_deviation;
        } else {
          deviated_pt(idx) -= fixed_deviation;
        }
        points.push_back(deviated_pt);
        msg_wrapper.push_back(get_point_from_vector(deviated_pt));
      }
    }
  }

  DenseNDTMapContext()
  : m_pc_wrapper{m_pc, "map"}
  {
    // Grid and spatial hash uses these boundaries. The setup allows for a grid of 125 cells: 5x5x5
    // where the centroid coordinates range from the integers 1 to 5 and the voxel size is 1
    m_min_point.x = 0.5F;
    m_min_point.y = 0.5F;
    m_min_point.z = 0.5F;
    m_max_point.x = 5.5F;
    m_max_point.y = 5.5F;
    m_max_point.z = 5.5F;
    m_voxel_size.x = 1.0F;
    m_voxel_size.y = 1.0F;
    m_voxel_size.z = 1.0F;
  }

  void build_pc(const autoware::perception::filters::voxel_grid::Config & cfg)
  {
    for (auto x = 1U; x <= POINTS_PER_DIM; ++x) {
      for (auto y = 1U; y <= POINTS_PER_DIM; ++y) {
        for (auto z = 1U; z <= POINTS_PER_DIM; ++z) {
          Eigen::Vector3d center{static_cast<float64_t>(x), static_cast<float64_t>(y),
            static_cast<float64_t>(z)};
          add_cell(m_pc_wrapper, center, static_cast<float64_t>(FIXED_DEVIATION));
          m_voxel_centers[cfg.index(center)] = center;
        }
      }
    }
  }

  uint32_t m_pc_idx{0U};
  sensor_msgs::msg::PointCloud2 m_pc;
  CloudModifier m_pc_wrapper;
  std::map<uint64_t, Eigen::Vector3d> m_voxel_centers;
  PointXYZ m_min_point;
  PointXYZ m_max_point;
  PointXYZ m_voxel_size;
  uint64_t m_capacity{1024U};
};

class MapPublisherTest : public DenseNDTMapContext, public ::testing::Test
{
protected:
  void SetUp()
  {
    rclcpp::init(0, nullptr);
  }
  void TearDown()
  {
    rclcpp::shutdown();
  }
};

pcl::PointCloud<pcl::PointXYZI> from_pointcloud2(const sensor_msgs::msg::PointCloud2 & msg)
{
  pcl::PointCloud<pcl::PointXYZI> res{};
  CloudView msg_view{msg};

  for (const auto & pt_in : msg_view) {
    pcl::PointXYZI pt;
    pt.x = pt_in.x;
    pt.y = pt_in.y;
    pt.z = pt_in.z;
    res.push_back(pt);
  }
  return res;
}

#endif  // TEST_MAP_PUBLISHER_HPP_


#include <Eigen/Core>
#include "GradientVar.h"
#include "testUtil.h"
#include <iostream>
#include <cmath>

using namespace Eigen;
using namespace std;

void test()
{
  const int rows = 2;
  const int cols = 3;

  GradientVar<double, rows, cols> var1(rows, cols);
  var1.value().setConstant(1.0);
  std::cout << var1.value() << std::endl << std::endl;
  std::cout << "maxOrder: " << var1.maxOrder() << std::endl << std::endl;

  const int nq = 5;
  GradientVar<double, rows, cols> var2(rows, cols, nq, 1);
  var2.value().setConstant(2.0);
  std::cout << var2.value() << std::endl << std::endl;
  var2.gradient().value().setConstant(3.0);
  std::cout << var2.gradient().value() << std::endl << std::endl;
  std::cout << "maxOrder: " << var2.maxOrder() << std::endl << std::endl;

  GradientVar<double, rows, cols> var3(rows, cols, nq, 5);
//  std::cout << "var3 rows: " << var3.secondDerivative().rows() << ", cols: " << var3.secondDerivative().cols() << std::endl;
  var3.gradient().gradient().value().setConstant(4.0);
  std::cout << var3.gradient().gradient().value() << std::endl << std::endl;
  std::cout << "maxOrder: " << var3.maxOrder() << std::endl << std::endl;

  GradientVar<double, 6, Eigen::Dynamic> var4(6, 3);
  var4.value().setConstant(5.0);
  std::cout << var4.value() << std::endl << std::endl;

  GradientVar<double, rows, cols> var3_copy(var3);
  std::cout << var3_copy.gradient().gradient().value() << std::endl << std::endl;
}

int main(int argc, char **argv)
{
  test();

  return 0;
}


#ifndef MIOPEN_MDG_EXPR_H
#define MIOPEN_MDG_EXPR_H

// #define BOOST_SPIRIT_DEBUG

#include <miopen/common.hpp>
#include <miopen/errors.hpp>
#include <miopen/logger.hpp>

#include <cassert>
#include <memory>
#include <iostream>
#include <unordered_map>
// Workaround tidy issues when using BOOST_FOREACH
#ifdef MIOPEN_USE_CLANG_TIDY
#define BOOST_FOREACH(x, y) for(x : y) // NOLINT
#endif
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <boost/fusion/adapted.hpp>
#include <boost/spirit/include/support_utree.hpp>
#include <miopen/fusion_ops.hpp>

namespace miopen {

namespace qi     = boost::spirit::qi;
namespace ascii  = boost::spirit::ascii;
namespace phx    = boost::phoenix;
namespace spirit = boost::spirit;

using Iterator = std::string::const_iterator;

struct MDGExprParser : qi::grammar<Iterator, spirit::utree(), ascii::space_type>
{
    struct MakeBinaryExpression
    {
        spirit::utree operator()(spirit::utf8_symbol_type op,
                                 spirit::utree const& lhs,
                                 spirit::utree const& rhs) const
        {
            spirit::utree expr;
            expr.push_back(op);
            expr.push_back(lhs);
            expr.push_back(rhs);
            return expr;
        }
    };

    phx::function<MakeBinaryExpression> makebinary;

    MDGExprParser();
    qi::rule<Iterator, spirit::utree(), ascii::space_type> expression;
    qi::rule<Iterator, spirit::utree(), ascii::space_type> additive_expr;
    qi::rule<Iterator, spirit::utree(), ascii::space_type> primary_expr;
    qi::rule<Iterator, spirit::utree(), ascii::space_type> constant;
    qi::rule<Iterator, spirit::utf8_symbol_type(), ascii::space_type> ops;
    qi::rule<Iterator, std::string(), ascii::space_type> variable;
};

struct visit_res
{
    int res         = 0;
    bool b_res      = false;
    MDGraph_op_t op = OpAny;
    std::string sym;
    std::unordered_map<std::string, int> tabl;
};

struct tree_visit
{
    std::unordered_map<std::string, int> tabl;
    std::function<bool(const std::string&, int&)> var_lookup;

    using result_type = visit_res;
    tree_visit(){};
    tree_visit(std::function<bool(const std::string&, int&)> f) : var_lookup(f){};
    tree_visit(std::function<bool(const std::string&, int&)> f,
               std::unordered_map<std::string, int> t)
        : tabl(t), var_lookup(f){};
    visit_res operator()(spirit::utree::invalid_type) const { return {}; }

    visit_res operator()(spirit::utree::nil_type) const { return {}; };

    visit_res operator()(double d)
    {
        visit_res r;
        r.res = static_cast<int>(d);
        return r;
    }

    visit_res operator()(int i)
    {
        visit_res r;
        r.res = i;
        return r;
    }

    template <typename T>
    visit_res operator()(T /*val*/)
    {
        return {};
    }

    visit_res operator()(bool b)
    {
        visit_res r;
        r.b_res = b;
        return r;
    }

    visit_res operator()(spirit::binary_range_type const& /*b*/) const { return {}; }

    visit_res operator()(spirit::utf8_string_range_type const& str)
    {
        visit_res r;
        using iterator = spirit::utf8_string_range_type::const_iterator;
        iterator i     = str.begin();
        std::string sym(i, str.end());
        int v = 0;
        if(var_lookup(sym, v))
        {
            r.res = v;
        }
        else
        {
            if(tabl.count(sym) == 1)
            {
                r.res = tabl.at(sym);
            }
            else
                r.sym = sym;
        }
        return r;
    }

    visit_res operator()(spirit::utf8_symbol_range_type const& str)
    {
        using iterator = spirit::utf8_symbol_range_type::const_iterator;
        iterator i     = str.begin();
        std::string sym(i, str.end());
        // This should return a function instead of a data struct
        visit_res r;
        if(sym == "+")
            r.op = OpAdd;
        else if(sym == "-")
            r.op = OpSub;
        else if(sym == "*")
            r.op = OpMul;
        else if(sym == "/")
            r.op = OpDiv;
        else if(sym == "%")
            r.op = OpModulo;
        else if(sym == ">=")
            r.op = OpGTE;
        else if(sym == "<=")
            r.op = OpLTE;
        else if(sym == "====")
            r.op = OpEqual;
        else if(sym == "!=")
            r.op = OpNotEqual;
        else if(sym == "^")
            r.op = OpPow;
        else if(sym == "&")
            r.op = OpAnd;
        else if(sym == "|")
            r.op = OpOr;
        else if(sym == "~")
            r.op = OpCeil;
        else if(sym == "===")
            r.op = OpAssign;
        else if(sym == ">>")
            r.op = OpGT;
        else if(sym == "<<")
            r.op = OpLT;
        else
        {
            MIOPEN_THROW(miopenStatusInternalError, "Parsing error: Unknown operator: " + sym);
        }
        return r;
    }

    template <typename Iterator>
    visit_res operator()(boost::iterator_range<Iterator> const& range)
    {
        std::vector<spirit::utree> v(range.begin(), range.end());
        assert(v.size() == 3);
        visit_res op_res, lhs_res, rhs_res, r;
        op_res  = boost::spirit::utree::visit(v[0], *this);
        lhs_res = boost::spirit::utree::visit(v[1], *this);
        rhs_res = boost::spirit::utree::visit(v[2], *this);

        if(op_res.op != OpAssign && !lhs_res.sym.empty())
        {
            std::string sym = lhs_res.sym;
            MIOPEN_THROW("Invalid variable access: " + sym);
        }

        switch(op_res.op)
        {
        // Arith ops
        case OpAdd: r.res = lhs_res.res + rhs_res.res; break;
        case OpSub: r.res = lhs_res.res - rhs_res.res; break;
        case OpMul: r.res = lhs_res.res * rhs_res.res; break;
        case OpDiv: r.res = lhs_res.res / rhs_res.res; break;
        case OpModulo: r.res = lhs_res.res % rhs_res.res; break;
        case OpPow: r.res = static_cast<int>(std::pow(lhs_res.res, rhs_res.res)); break;
        case OpCeil: {
            int vv = lhs_res.res;
            int mm = rhs_res.res;
            r.res  = (vv % mm != 0) ? (vv / mm + 1) * mm : vv;
            break;
        }
        case OpAssign: {
            int val = 0;
            if(var_lookup(lhs_res.sym, val))
                MIOPEN_THROW("Invalid variable assignment: " + lhs_res.sym);
            MIOPEN_LOG_I2(" Adding variable: " + lhs_res.sym);
            r.tabl[lhs_res.sym] = rhs_res.res;
            r.b_res             = true;
            break;
        }
        // Logical ops
        case OpEqual:
            r.b_res = lhs_res.res == rhs_res.res;
            r.res   = static_cast<int>(r.b_res);
            break;
        case OpNotEqual:
            r.b_res = lhs_res.res != rhs_res.res;
            r.res   = static_cast<int>(r.b_res);
            break;
        case OpGTE:
            r.b_res = lhs_res.res >= rhs_res.res;
            r.res   = static_cast<int>(r.b_res);
            break;
        case OpLTE:
            r.b_res = lhs_res.res <= rhs_res.res;
            r.res   = static_cast<int>(r.b_res);
            break;
        case OpGT:
            r.b_res = lhs_res.res > rhs_res.res;
            r.res   = static_cast<int>(r.b_res);
            break;
        case OpLT:
            r.b_res = lhs_res.res < rhs_res.res;
            r.res   = static_cast<int>(r.b_res);
            break;
        case OpAnd:
            r.b_res = lhs_res.b_res && rhs_res.b_res;
            r.res   = static_cast<int>(r.b_res);
            break;
        case OpOr:
            r.b_res = lhs_res.b_res || rhs_res.b_res;
            r.res   = static_cast<int>(r.b_res);
            break;
        case OpAny:
        case OpEval: MIOPEN_THROW("Unsupported op");
        }
        return r;
    }

    visit_res operator()(spirit::any_ptr const&) const { return {}; }

    visit_res operator()(spirit::function_base const&) const { return {}; }
};
} // namespace miopen

#endif


#ifndef _VELODYNE_PLUGIN_HH_
#define _VELODYNE_PLUGIN_HH_

#include <random>
#include <gazebo/gazebo.hh>
#include <gazebo/common/common.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <thread>
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "ros/subscribe_options.h"
#include "std_msgs/Float32.h"
#include "geometry_msgs/Pose.h"
#include <geometry_msgs/Point.h>
#include <obstacles/cv_model.h>
#include <Eigen/Dense>

namespace gazebo
{
  /// \brief A plugin to control a Velodyne sensor.
  class CV_Sphere : public ModelPlugin
  {
    /// \brief Constructor
    public: CV_Sphere() {}

    /// \brief The load function is called by Gazebo when the plugin is
    /// inserted into simulation
    /// \param[in] _model A pointer to the model that this plugin is
    /// attached to.
    /// \param[in] _sdf A pointer to the plugin's SDF element.
    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
    {

      // Store the model pointer for convenience.
      this->model = _model;
      // Check that the velocity element exists, then read the value
      this->desired_y_velo = 0;
      if (_sdf->HasElement("velocity"))
        this->desired_y_velo = _sdf->Get<double>("velocity");

      ignition::math::Vector3d start_vel = ignition::math::Vector3d(0, this->desired_y_velo, 0);
      this->SetVelocity(start_vel);
      this->sigma_model_noise = 0.001;
      if (_sdf->HasElement("sigma_model_noise"))
        this->sigma_model_noise = _sdf->Get<double>("sigma_model_noise");
      this->sigma_sensor_noise = 0.01;
      if (_sdf->HasElement("sigma_sensor_noise"))
        this->sigma_sensor_noise = _sdf->Get<double>("sigma_sensor_noise");
      // Define random generator with Gaussian distribution
      const double mean = 0.0;
      this->model_noise = std::normal_distribution<double>(mean, this->sigma_model_noise);
      this->sensor_noise = std::normal_distribution<double>(mean, this->sigma_sensor_noise);
      // Set up tracking with cv model
      this->cv_model = CVModel(1, this->sigma_model_noise, this->sigma_sensor_noise);
      Eigen::Vector3d start_pos;
      start_pos << this->model->WorldPose().X()+this->sensor_noise(this->generator),
              this->model->WorldPose().Y()+this->sensor_noise(this->generator),
              this->model->WorldPose().Z()+this->sensor_noise(this->generator);
      Eigen::Vector3d s_v;
      s_v << 0.0, this->desired_y_velo, 0.0;
      this->cv_model.init_target(start_pos, s_v, this->sigma_model_noise, this->sigma_sensor_noise);
      // Create the node
      this->node = transport::NodePtr(new transport::Node());
      #if GAZEBO_MAJOR_VERSION < 8
      this->node->Init(this->model->GetWorld()->GetName());
      #else
      this->node->Init(this->model->GetWorld()->Name());
      #endif

      // Listen to the update event. This event is broadcast every
      // simulation iteration.
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
          std::bind(&CV_Sphere::OnUpdate, this));

      // Initialize ros, if it has not already bee initialized.
      if (!ros::isInitialized())
      {
        int argc = 0;
        char **argv = NULL;
        ros::init(argc, argv, "gazebo_client",
            ros::init_options::NoSigintHandler);
      }

      // Create our ROS node. This acts in a similar manner to
      // the Gazebo node
      this->rosNode.reset(new ros::NodeHandle("gazebo_client"));

      // Create a named topic, and subscribe to it.
      ros::SubscribeOptions so_vel =
        ros::SubscribeOptions::create<std_msgs::Float32>(
            "/" + this->model->GetName() + "/vel_cmd",
            1,
            boost::bind(&CV_Sphere::OnRosVelMsg, this, _1),
            ros::VoidPtr(), &this->rosQueue);
      this->rosSub = this->rosNode->subscribe(so_vel);

      // Create a named topic, and subscribe to it.
      ros::SubscribeOptions so_pose =
        ros::SubscribeOptions::create<geometry_msgs::Pose>(
            "/" + this->model->GetName() + "/pose_cmd",
            1,
            boost::bind(&CV_Sphere::OnRosPoseMsg, this, _1),
            ros::VoidPtr(), &this->rosQueue);
      this->rosSubPose = this->rosNode->subscribe(so_pose);

      std::string topic_name = "/" + this->model->GetName() + "/position";
      publisher = this->rosNode->advertise<geometry_msgs::Point>(topic_name, 100);

      // Spin up the queue helper thread.
      this->rosQueueThread =
        std::thread(std::bind(&CV_Sphere::QueueThread, this));

      std::cout << "Fyling sphere plugin connected.";
      ROS_INFO("Fyling sphere plugin connected.");
    }

    // Called by the world update start event
    public: void OnUpdate()
    {
      // Simulate a noisy measurement

      Eigen::Vector3d true_pos;
      true_pos << this->model->WorldPose().X(),
                  this->model->WorldPose().Y(),
                  this->model->WorldPose().Z();

      Eigen::Vector3d meas;
      meas << true_pos(0)+this->sensor_noise(this->generator),
              true_pos(1)+this->sensor_noise(this->generator),
              true_pos(2)+this->sensor_noise(this->generator);
      // Use the CV KF to estimate the true position
      // Currently the KF uses quite a bit of performance, so we turned it off.
      /*
      this->cv_model.perform_prediction_update(meas);
      Eigen::Vector3d pos = this->cv_model.get_pos();
      */
      // Publish the filtered sphere position
      geometry_msgs::Point pos_msg;
      pos_msg.x = meas(0);
      pos_msg.y = meas(1);
      pos_msg.z = meas(2);
      publisher.publish(pos_msg);

      // Debugging
      // Difference between estimated pos and true pos should be lower than difference between measured pos and true pos on average.
      /*
      Eigen::Vector3d diff_est = (pos - true_pos).cwiseAbs();
      Eigen::Vector3d diff_meas = (meas - true_pos).cwiseAbs();
      Eigen::Vector3d best = diff_meas - diff_est;
      ROS_INFO_STREAM("Difference between erros diff_meas - diff_est, these should be positive most of the time. x: " << best(0) <<
                      ", y: " << best(1) <<
                      ", z: " << best(2) << "\n");
      */
      // Add model noise
      // Generate gaussian noise
      ignition::math::Vector3d current_vel = this->model->WorldLinearVel();
      this->desired_y_velo += this->model_noise(this->generator);
      current_vel.Set(
        current_vel.X()+this->model_noise(this->generator),
        this->desired_y_velo,
        current_vel.Z()+this->model_noise(this->generator)
      );
      // Apply a small linear velocity to the model.
      this->model->SetLinearVel(current_vel);

    }

    /// \brief Set the velocity of the Velodyne
    /// \param[in] _vel New target velocity
    public: void SetVelocity(const ignition::math::Vector3d &_vel)
    {
      // Set the joint's target velocity.
      this->model->SetLinearVel(_vel);
    }

    /// \brief Set the pose of the Velodyne
    /// \param[in] _pose New pose
    public: void SetPose(const ignition::math::Pose3d &_pose)
    {
      ignition::math::Pose3d curr_pose = this->model->WorldPose();
      this->model->SetWorldPose(_pose);
    }

    /// \brief Handle an incoming message from ROS
    /// \param[in] _msg A float value that is used to set the y velocity
    /// of the sphere.
    public: void OnRosVelMsg(const std_msgs::Float32ConstPtr &_msg)
    {
      this->desired_y_velo = _msg->data;
      this->SetVelocity(ignition::math::Vector3d(0, this->desired_y_velo, 0));
    }

    /// \brief Handle an incoming pose message from ROS
    /// \param[in] _msg A geometry_msgs::Pose
    public: void OnRosPoseMsg(const geometry_msgs::PoseConstPtr &_msg)
    {
      std::cerr << "Pose msg received.";
      ignition::math::Pose3d _pose = ignition::math::Pose3d(
        _msg->position.x,
        _msg->position.y,
        _msg->position.z,
        _msg->orientation.w,
        _msg->orientation.x,
        _msg->orientation.y,
        _msg->orientation.z
      );
      std::cerr << "Pose created.";
      this->SetPose(_pose);
    }

    /// \brief ROS helper function that processes messages
    private: void QueueThread()
    {
      static const double timeout = 0.01;
      while (this->rosNode->ok())
      {
        this->rosQueue.callAvailable(ros::WallDuration(timeout));
      }
    }

    /// \brief Pointer to the model.
    private: physics::ModelPtr model;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;

    /// \brief Gaussian noise of velocity noise
    private: double sigma_model_noise;

    /// \brief Gaussian noise of position sensor
    private: double sigma_sensor_noise;

    private: std::default_random_engine generator;
    private: std::normal_distribution<double> model_noise;
    private: std::normal_distribution<double> sensor_noise;
    /// \brief A node used for transport
    private: transport::NodePtr node;

    /// \brief A subscriber to a named topic.
    private: transport::SubscriberPtr sub;

    /// \brief A subscriber to a named topic.
    private: transport::SubscriberPtr pose_sub;

    /// \brief A node use for ROS transport
    private: std::unique_ptr<ros::NodeHandle> rosNode;

    /// \brief A ROS subscriber
    private: ros::Subscriber rosSub;

    /// \brief A ROS subscriber
    private: ros::Subscriber rosSubPose;

    /// \brief ROS publisher that publishes a gazebo collision
    private: ros::Publisher publisher;

    /// \brief A ROS callbackqueue that helps process messages
    private: ros::CallbackQueue rosQueue;

    /// \brief A thread the keeps running the rosQueue
    private: std::thread rosQueueThread;

    /// \brief Desired y velocity (needed for initial velo jump)
    private: double desired_y_velo;

    /// \brief CV KF tracker
    private: CVModel cv_model;

  };

  // Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin.
  GZ_REGISTER_MODEL_PLUGIN(CV_Sphere)
}
#endif


/*******************************************************************************
Copyright(c) 2015-2018 LORD Corporation. All rights reserved.

MIT Licensed. See the included LICENSE.txt for a copy of the full MIT License.
*******************************************************************************/
#include "mscl/MicroStrain/MIP/Packets/MipFieldParser.h"
#include "mscl/MicroStrain/MIP/Packets/MipDataPacket.h"
#include "mscl/MicroStrain/MIP/MipDataPoint.h"
#include "mscl/MicroStrain/Inertial/Packets/InertialFieldParser_GNSS.h"
#include "mscl/MicroStrain/Inertial/ExposedInertialTypes.h"

#include <boost/test/unit_test.hpp>

using namespace mscl;

BOOST_AUTO_TEST_SUITE(InertialFieldParser_GPS_Test)

BOOST_AUTO_TEST_CASE(InertialFieldParser_LLHPosition_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_double(1.2);        //latitude
    bytes.append_double(3.4);        //longitude
    bytes.append_double(5.67);        //height above ellipsoid
    bytes.append_double(7.89);        //height above MSL
    bytes.append_float(1.23f);        //horizontal accuracy
    bytes.append_float(2.34f);        //vertical accuracy
    bytes.append_uint16(29);        //valid flags (0001 1101)

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_GNSS_LLH_POSITION, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 6);

    //Latitude
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_GNSS_LLH_POSITION);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_LATITUDE);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_double);
    BOOST_CHECK_EQUAL(data.at(0).valid(), true);
    BOOST_CHECK_CLOSE(data.at(0).as_double(), 1.2, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    //Longitude
    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_GNSS_LLH_POSITION);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_LONGITUDE);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_double);
    BOOST_CHECK_EQUAL(data.at(1).valid(), true);
    BOOST_CHECK_CLOSE(data.at(1).as_double(), 3.4, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    //Height Above Ellipsoid
    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_GNSS_LLH_POSITION);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_HEIGHT_ABOVE_ELLIPSOID);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_double);
    BOOST_CHECK_EQUAL(data.at(2).valid(), false);
    BOOST_CHECK_CLOSE(data.at(2).as_double(), 5.67, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());

    //Height Above MSL
    BOOST_CHECK_EQUAL(data.at(3).field(), MipTypes::CH_FIELD_GNSS_LLH_POSITION);
    BOOST_CHECK_EQUAL(data.at(3).qualifier(), MipTypes::CH_HEIGHT_ABOVE_MSL);
    BOOST_CHECK_EQUAL(data.at(3).storedAs(), valueType_double);
    BOOST_CHECK_EQUAL(data.at(3).valid(), true);
    BOOST_CHECK_CLOSE(data.at(3).as_double(), 7.89, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(3).channelName());

    //Horizontal Accuracy
    BOOST_CHECK_EQUAL(data.at(4).field(), MipTypes::CH_FIELD_GNSS_LLH_POSITION);
    BOOST_CHECK_EQUAL(data.at(4).qualifier(), MipTypes::CH_HORIZONTAL_ACCURACY);
    BOOST_CHECK_EQUAL(data.at(4).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(4).valid(), true);
    BOOST_CHECK_CLOSE(data.at(4).as_double(), 1.23, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(4).channelName());

    //Vertical Accuracy
    BOOST_CHECK_EQUAL(data.at(5).field(), MipTypes::CH_FIELD_GNSS_LLH_POSITION);
    BOOST_CHECK_EQUAL(data.at(5).qualifier(), MipTypes::CH_VERTICAL_ACCURACY);
    BOOST_CHECK_EQUAL(data.at(5).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(5).valid(), true);
    BOOST_CHECK_CLOSE(data.at(5).as_double(), 2.34, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(5).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_ECEFPosition_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_double(1.2);        //x position
    bytes.append_double(3.4);        //y position
    bytes.append_double(5.67);        //z position
    bytes.append_float(7.89f);        //position accuracy
    bytes.append_uint16(2);            //valid flags (0000 0010)

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_GNSS_ECEF_POSITION, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 4);

    //x position
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_GNSS_ECEF_POSITION);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_X);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_double);
    BOOST_CHECK_EQUAL(data.at(0).valid(), false);
    BOOST_CHECK_CLOSE(data.at(0).as_double(), 1.2, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    //y position
    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_GNSS_ECEF_POSITION);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_Y);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_double);
    BOOST_CHECK_EQUAL(data.at(1).valid(), false);
    BOOST_CHECK_CLOSE(data.at(1).as_double(), 3.4, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    //z position
    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_GNSS_ECEF_POSITION);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_Z);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_double);
    BOOST_CHECK_EQUAL(data.at(2).valid(), false);
    BOOST_CHECK_CLOSE(data.at(2).as_double(), 5.67, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());

    //position accuracy
    BOOST_CHECK_EQUAL(data.at(3).field(), MipTypes::CH_FIELD_GNSS_ECEF_POSITION);
    BOOST_CHECK_EQUAL(data.at(3).qualifier(), MipTypes::CH_POSITION_ACCURACY);
    BOOST_CHECK_EQUAL(data.at(3).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(3).valid(), true);
    BOOST_CHECK_CLOSE(data.at(3).as_double(), 7.89, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(3).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_NEDVelocity_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(1.1f);    //north
    bytes.append_float(2.2f);    //east
    bytes.append_float(3.3f);    //down
    bytes.append_float(4.4f);    //speed
    bytes.append_float(5.5f);    //ground speed
    bytes.append_float(6.6f);    //heading
    bytes.append_float(7.7f);    //speed accuracy
    bytes.append_float(8.8f);    //heading accuracy
    bytes.append_uint16(41);    //valid flags (0010 1001)

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_GNSS_NED_VELOCITY, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 8);

    //north
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_GNSS_NED_VELOCITY);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_NORTH);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(0).valid(), true);
    BOOST_CHECK_CLOSE(data.at(0).as_float(), 1.1, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    //east
    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_GNSS_NED_VELOCITY);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_EAST);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(1).valid(), true);
    BOOST_CHECK_CLOSE(data.at(1).as_float(), 2.2, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    //down
    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_GNSS_NED_VELOCITY);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_DOWN);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(2).valid(), true);
    BOOST_CHECK_CLOSE(data.at(2).as_float(), 3.3, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());

    //speed
    BOOST_CHECK_EQUAL(data.at(3).field(), MipTypes::CH_FIELD_GNSS_NED_VELOCITY);
    BOOST_CHECK_EQUAL(data.at(3).qualifier(), MipTypes::CH_SPEED);
    BOOST_CHECK_EQUAL(data.at(3).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(3).valid(), false);
    BOOST_CHECK_CLOSE(data.at(3).as_float(), 4.4, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(3).channelName());

    //ground speed
    BOOST_CHECK_EQUAL(data.at(4).field(), MipTypes::CH_FIELD_GNSS_NED_VELOCITY);
    BOOST_CHECK_EQUAL(data.at(4).qualifier(), MipTypes::CH_GROUND_SPEED);
    BOOST_CHECK_EQUAL(data.at(4).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(4).valid(), false);
    BOOST_CHECK_CLOSE(data.at(4).as_float(), 5.5, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(4).channelName());

    //heading
    BOOST_CHECK_EQUAL(data.at(5).field(), MipTypes::CH_FIELD_GNSS_NED_VELOCITY);
    BOOST_CHECK_EQUAL(data.at(5).qualifier(), MipTypes::CH_HEADING);
    BOOST_CHECK_EQUAL(data.at(5).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(5).valid(), true);
    BOOST_CHECK_CLOSE(data.at(5).as_float(), 6.6, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(5).channelName());

    //speed accuracy
    BOOST_CHECK_EQUAL(data.at(6).field(), MipTypes::CH_FIELD_GNSS_NED_VELOCITY);
    BOOST_CHECK_EQUAL(data.at(6).qualifier(), MipTypes::CH_SPEED_ACCURACY);
    BOOST_CHECK_EQUAL(data.at(6).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(6).valid(), false);
    BOOST_CHECK_CLOSE(data.at(6).as_float(), 7.7, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(6).channelName());

    //heading accuracy
    BOOST_CHECK_EQUAL(data.at(7).field(), MipTypes::CH_FIELD_GNSS_NED_VELOCITY);
    BOOST_CHECK_EQUAL(data.at(7).qualifier(), MipTypes::CH_HEADING_ACCURACY);
    BOOST_CHECK_EQUAL(data.at(7).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(7).valid(), true);
    BOOST_CHECK_CLOSE(data.at(7).as_float(), 8.8, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(7).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_ECEFVelocity_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(1.1f);    //x
    bytes.append_float(2.2f);    //y
    bytes.append_float(3.3f);    //z
    bytes.append_float(4.4f);    //velocity accuracy
    bytes.append_uint16(1);        //valid flags (0000 0001)

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_GNSS_ECEF_VELOCITY, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 4);

    //X
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_GNSS_ECEF_VELOCITY);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_X);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(0).valid(), true);
    BOOST_CHECK_CLOSE(data.at(0).as_float(), 1.1, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    //Y
    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_GNSS_ECEF_VELOCITY);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_Y);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(1).valid(), true);
    BOOST_CHECK_CLOSE(data.at(1).as_float(), 2.2, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    //Z
    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_GNSS_ECEF_VELOCITY);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_Z);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(2).valid(), true);
    BOOST_CHECK_CLOSE(data.at(2).as_float(), 3.3, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());

    //ACCURACY
    BOOST_CHECK_EQUAL(data.at(3).field(), MipTypes::CH_FIELD_GNSS_ECEF_VELOCITY);
    BOOST_CHECK_EQUAL(data.at(3).qualifier(), MipTypes::CH_VELOCITY_ACCURACY);
    BOOST_CHECK_EQUAL(data.at(3).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(3).valid(), false);
    BOOST_CHECK_CLOSE(data.at(3).as_float(), 4.4, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(3).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_DOP_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(1.1f);    //geometric
    bytes.append_float(2.2f);    //position
    bytes.append_float(3.3f);    //horizontal
    bytes.append_float(4.4f);    //vertical
    bytes.append_float(5.5f);    //time
    bytes.append_float(6.6f);    //northing
    bytes.append_float(7.7f);    //easting
    bytes.append_uint16(112);    //valid flags (0111 0000)

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_GNSS_DOP, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 7);

    //GEOMETRIC
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_GNSS_DOP);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_GEOMETRIC_DOP);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(0).valid(), false);
    BOOST_CHECK_CLOSE(data.at(0).as_float(), 1.1, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    //position
    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_GNSS_DOP);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_POSITION_DOP);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(1).valid(), false);
    BOOST_CHECK_CLOSE(data.at(1).as_float(), 2.2, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    //horizontal
    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_GNSS_DOP);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_HORIZONTAL_DOP);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(2).valid(), false);
    BOOST_CHECK_CLOSE(data.at(2).as_float(), 3.3, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());

    //vertical
    BOOST_CHECK_EQUAL(data.at(3).field(), MipTypes::CH_FIELD_GNSS_DOP);
    BOOST_CHECK_EQUAL(data.at(3).qualifier(), MipTypes::CH_VERTICAL_DOP);
    BOOST_CHECK_EQUAL(data.at(3).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(3).valid(), false);
    BOOST_CHECK_CLOSE(data.at(3).as_float(), 4.4, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(3).channelName());

    //time
    BOOST_CHECK_EQUAL(data.at(4).field(), MipTypes::CH_FIELD_GNSS_DOP);
    BOOST_CHECK_EQUAL(data.at(4).qualifier(), MipTypes::CH_TIME_DOP);
    BOOST_CHECK_EQUAL(data.at(4).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(4).valid(), true);
    BOOST_CHECK_CLOSE(data.at(4).as_float(), 5.5, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(4).channelName());

    //northing
    BOOST_CHECK_EQUAL(data.at(5).field(), MipTypes::CH_FIELD_GNSS_DOP);
    BOOST_CHECK_EQUAL(data.at(5).qualifier(), MipTypes::CH_NORTHING_DOP);
    BOOST_CHECK_EQUAL(data.at(5).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(5).valid(), true);
    BOOST_CHECK_CLOSE(data.at(5).as_float(), 6.6, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(5).channelName());

    //easting
    BOOST_CHECK_EQUAL(data.at(6).field(), MipTypes::CH_FIELD_GNSS_DOP);
    BOOST_CHECK_EQUAL(data.at(6).qualifier(), MipTypes::CH_EASTING_DOP);
    BOOST_CHECK_EQUAL(data.at(6).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(6).valid(), true);
    BOOST_CHECK_CLOSE(data.at(6).as_float(), 7.7, 0.0001);
    BOOST_CHECK_NO_THROW(data.at(6).channelName());

}

BOOST_AUTO_TEST_CASE(InertialFieldParser_UTCTime_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_uint16(2013);    //year
    bytes.append_uint8(3);        //month
    bytes.append_uint8(18);        //day
    bytes.append_uint8(13);        //hour
    bytes.append_uint8(26);        //minute
    bytes.append_uint8(57);        //seconds
    bytes.append_uint32(123);    //milliseconds
    bytes.append_uint16(3);        //flags (0000 0011)

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_GNSS_UTC_TIME, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 2);

    //timestamp
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_GNSS_UTC_TIME);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_TIMESTAMP);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_Timestamp);
    BOOST_CHECK_EQUAL(data.at(0).valid(), true);
    BOOST_CHECK_EQUAL(data.at(0).as_Timestamp().nanoseconds(), 1363613217123000000);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    //flags
    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_GNSS_UTC_TIME);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_FLAGS);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_uint16);
    BOOST_CHECK_EQUAL(data.at(1).valid(), true);
    BOOST_CHECK_EQUAL(data.at(1).as_uint16(), 3);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_GPSTime_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_double(753.159);    //time of week
    bytes.append_uint16(129);        //week number
    bytes.append_uint16(1);            //flags (0000 0001)

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_GNSS_GPS_TIME, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 2);

    //timestamp
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_GNSS_GPS_TIME);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_TIME_OF_WEEK);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_double);
    BOOST_CHECK_EQUAL(data.at(0).valid(), true);
    BOOST_CHECK_EQUAL(data.at(0).as_double(), 753.159);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    //week number
    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_GNSS_GPS_TIME);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_WEEK_NUMBER);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_uint16);
    BOOST_CHECK_EQUAL(data.at(1).valid(), false);
    BOOST_CHECK_EQUAL(data.at(1).as_uint16(), 129);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_ClockInfo_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_double(1.234);        //clock bias
    bytes.append_double(2.345);        //clock drift
    bytes.append_double(3.456);        //accuracy estimate
    bytes.append_uint16(3);            //flags (0000 0011)

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_GNSS_CLOCK_INFO, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 3);

    //clock bias
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_GNSS_CLOCK_INFO);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_BIAS);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_double);
    BOOST_CHECK_EQUAL(data.at(0).valid(), true);
    BOOST_CHECK_EQUAL(data.at(0).as_double(), 1.234);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    //clock drift
    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_GNSS_CLOCK_INFO);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_DRIFT);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_double);
    BOOST_CHECK_EQUAL(data.at(1).valid(), true);
    BOOST_CHECK_EQUAL(data.at(1).as_double(), 2.345);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    //accuracy estimate
    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_GNSS_CLOCK_INFO);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_ACCURACY_ESTIMATE);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_double);
    BOOST_CHECK_EQUAL(data.at(2).valid(), false);
    BOOST_CHECK_EQUAL(data.at(2).as_double(), 3.456);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_GPSFixInfo_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_uint8(2);        //fix type
    bytes.append_uint8(200);    //number of SVs used for solution
    bytes.append_uint16(1);        //fix flags
    bytes.append_uint16(7);        //flags (0000 0111)

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_GNSS_FIX_INFO, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 3);

    //fix type
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_GNSS_FIX_INFO);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_FIX_TYPE);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_uint8);
    BOOST_CHECK_EQUAL(data.at(0).valid(), true);
    BOOST_CHECK_EQUAL(data.at(0).as_uint8(), InertialTypes::FIXTYPE_TIMEONLY);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    //number of SVs used for solution
    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_GNSS_FIX_INFO);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_SV_COUNT);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_uint8);
    BOOST_CHECK_EQUAL(data.at(1).valid(), true);
    BOOST_CHECK_EQUAL(data.at(1).as_uint8(), 200);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    //fix flags
    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_GNSS_FIX_INFO);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_FLAGS);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_uint16);
    BOOST_CHECK_EQUAL(data.at(2).valid(), true);
    BOOST_CHECK_EQUAL(data.at(2).as_double(), 1);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_SpaceVehicleInfo_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_uint8(1);        //channel
    bytes.append_uint8(2);        //vehicle id
    bytes.append_uint16(3);        //carrier to noise ratio
    bytes.append_int16(-123);    //azimuth
    bytes.append_int16(-32000);    //elevation
    bytes.append_uint16(2);        //space vehicle flags
    bytes.append_uint16(45);    //flags (0010 1101)

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_GNSS_SPACE_VEHICLE_INFO, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 6);

    //channel
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_GNSS_SPACE_VEHICLE_INFO);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_CHANNEL);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_uint8);
    BOOST_CHECK_EQUAL(data.at(0).valid(), true);
    BOOST_CHECK_EQUAL(data.at(0).as_uint8(), 1);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    //space vehicle id
    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_GNSS_SPACE_VEHICLE_INFO);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_ID);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_uint8);
    BOOST_CHECK_EQUAL(data.at(1).valid(), false);
    BOOST_CHECK_EQUAL(data.at(1).as_uint8(), 2);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    //carrier to noise ratio
    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_GNSS_SPACE_VEHICLE_INFO);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_CARRIER_TO_NOISE_RATIO);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_uint16);
    BOOST_CHECK_EQUAL(data.at(2).valid(), true);
    BOOST_CHECK_EQUAL(data.at(2).as_uint16(), 3);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());

    //azimuth
    BOOST_CHECK_EQUAL(data.at(3).field(), MipTypes::CH_FIELD_GNSS_SPACE_VEHICLE_INFO);
    BOOST_CHECK_EQUAL(data.at(3).qualifier(), MipTypes::CH_AZIMUTH);
    BOOST_CHECK_EQUAL(data.at(3).storedAs(), valueType_int16);
    BOOST_CHECK_EQUAL(data.at(3).valid(), true);
    BOOST_CHECK_EQUAL(data.at(3).as_int16(), -123);
    BOOST_CHECK_NO_THROW(data.at(3).channelName());

    //elevation
    BOOST_CHECK_EQUAL(data.at(4).field(), MipTypes::CH_FIELD_GNSS_SPACE_VEHICLE_INFO);
    BOOST_CHECK_EQUAL(data.at(4).qualifier(), MipTypes::CH_ELEVATION);
    BOOST_CHECK_EQUAL(data.at(4).storedAs(), valueType_int16);
    BOOST_CHECK_EQUAL(data.at(4).valid(), false);
    BOOST_CHECK_EQUAL(data.at(4).as_int16(), -32000);
    BOOST_CHECK_NO_THROW(data.at(4).channelName());

    //space vehicle flags
    BOOST_CHECK_EQUAL(data.at(5).field(), MipTypes::CH_FIELD_GNSS_SPACE_VEHICLE_INFO);
    BOOST_CHECK_EQUAL(data.at(5).qualifier(), MipTypes::CH_FLAGS);
    BOOST_CHECK_EQUAL(data.at(5).storedAs(), valueType_uint16);
    BOOST_CHECK_EQUAL(data.at(5).valid(), true);
    BOOST_CHECK_EQUAL(data.at(5).as_uint16(), 2);
    BOOST_CHECK_NO_THROW(data.at(5).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_HardwareStatus_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_uint8(0);        //sensor state
    bytes.append_uint8(1);        //antenna state
    bytes.append_uint8(2);        //antenna power
    bytes.append_uint16(7);        //flags (0000 0111)

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_GNSS_HARDWARE_STATUS, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 3);

    //sensor state
    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_GNSS_HARDWARE_STATUS);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_SENSOR_STATE);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_uint8);
    BOOST_CHECK_EQUAL(data.at(0).valid(), true);
    BOOST_CHECK_EQUAL(data.at(0).as_uint8(), InertialTypes::SENSORSTATE_OFF);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    //antenna state
    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_GNSS_HARDWARE_STATUS);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_ANTENNA_STATE);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_uint8);
    BOOST_CHECK_EQUAL(data.at(1).valid(), true);
    BOOST_CHECK_EQUAL(data.at(1).as_uint8(), InertialTypes::ANTENNASTATE_INIT);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    //antenna power
    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_GNSS_HARDWARE_STATUS);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_ANTENNA_POWER);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_uint8);
    BOOST_CHECK_EQUAL(data.at(2).valid(), true);
    BOOST_CHECK_EQUAL(data.at(2).as_uint8(), InertialTypes::ANTENNAPOWER_UNKNOWN);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_DgpsInfo_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_float(5.4f);    //newest age
    bytes.append_int16(1);        //base station id
    bytes.append_int16(2);        //base station status
    bytes.append_uint16(3);        //number of dgps channels
    bytes.append_uint16(7);        //flags (0000 0111)

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_GNSS_DGNSS_INFO, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 4);

    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_GNSS_DGNSS_INFO);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_AGE);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(0).valid(), true);
    BOOST_CHECK_CLOSE(data.at(0).as_float(), 5.4, 0.001);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_GNSS_DGNSS_INFO);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_ID);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_int16);
    BOOST_CHECK_EQUAL(data.at(1).valid(), true);
    BOOST_CHECK_EQUAL(data.at(1).as_int16(), 1);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_GNSS_DGNSS_INFO);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_STATUS);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_int16);
    BOOST_CHECK_EQUAL(data.at(2).valid(), true);
    BOOST_CHECK_EQUAL(data.at(2).as_int16(), 2);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());

    BOOST_CHECK_EQUAL(data.at(3).field(), MipTypes::CH_FIELD_GNSS_DGNSS_INFO);
    BOOST_CHECK_EQUAL(data.at(3).qualifier(), MipTypes::CH_NUM_CHANNELS);
    BOOST_CHECK_EQUAL(data.at(3).storedAs(), valueType_uint16);
    BOOST_CHECK_EQUAL(data.at(3).valid(), false);
    BOOST_CHECK_EQUAL(data.at(3).as_uint16(), 3);
    BOOST_CHECK_NO_THROW(data.at(3).channelName());
}

BOOST_AUTO_TEST_CASE(InertialFieldParser_DgpsChannelStatus_parse)
{
    ByteStream bytes;

    //add bytes to the ByteStream
    bytes.append_uint8(1);        //space vehicle id
    bytes.append_float(1.23f);    //age
    bytes.append_float(7.125f);    //pseudorange correction
    bytes.append_float(457.01f);//pseudorange rate correction
    bytes.append_uint16(15);    //flags (0000 1111)

    //Create the Data Field
    MipDataField field(MipTypes::CH_FIELD_GNSS_DGNSS_CHANNEL_STATUS, bytes.data());

    MipDataPoints data;

    MipFieldParser::parseField(field, data);

    //verify the field was parsed correctly
    BOOST_CHECK_EQUAL(data.size(), 4);

    BOOST_CHECK_EQUAL(data.at(0).field(), MipTypes::CH_FIELD_GNSS_DGNSS_CHANNEL_STATUS);
    BOOST_CHECK_EQUAL(data.at(0).qualifier(), MipTypes::CH_ID);
    BOOST_CHECK_EQUAL(data.at(0).storedAs(), valueType_uint8);
    BOOST_CHECK_EQUAL(data.at(0).valid(), true);
    BOOST_CHECK_EQUAL(data.at(0).as_uint8(), 1);
    BOOST_CHECK_NO_THROW(data.at(0).channelName());

    BOOST_CHECK_EQUAL(data.at(1).field(), MipTypes::CH_FIELD_GNSS_DGNSS_CHANNEL_STATUS);
    BOOST_CHECK_EQUAL(data.at(1).qualifier(), MipTypes::CH_AGE);
    BOOST_CHECK_EQUAL(data.at(1).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(1).valid(), true);
    BOOST_CHECK_CLOSE(data.at(1).as_float(), 1.23, 0.001);
    BOOST_CHECK_NO_THROW(data.at(1).channelName());

    BOOST_CHECK_EQUAL(data.at(2).field(), MipTypes::CH_FIELD_GNSS_DGNSS_CHANNEL_STATUS);
    BOOST_CHECK_EQUAL(data.at(2).qualifier(), MipTypes::CH_CORRECTION);
    BOOST_CHECK_EQUAL(data.at(2).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(2).valid(), true);
    BOOST_CHECK_CLOSE(data.at(2).as_float(), 7.125, 0.001);
    BOOST_CHECK_NO_THROW(data.at(2).channelName());

    BOOST_CHECK_EQUAL(data.at(3).field(), MipTypes::CH_FIELD_GNSS_DGNSS_CHANNEL_STATUS);
    BOOST_CHECK_EQUAL(data.at(3).qualifier(), MipTypes::CH_RATE_CORRECTION);
    BOOST_CHECK_EQUAL(data.at(3).storedAs(), valueType_float);
    BOOST_CHECK_EQUAL(data.at(3).valid(), true);
    BOOST_CHECK_CLOSE(data.at(3).as_float(), 457.01, 0.001);
    BOOST_CHECK_NO_THROW(data.at(3).channelName());
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/accumulators/statistics/peaks_over_threshold.hpp>