/*===================================================================

MSI applications for interactive analysis in MITK (M2aia)

Copyright (c) Jonas Cordes

All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt for details.

===================================================================*/

#include "mitkIOUtil.h"
#include <m2MedianAbsoluteDeviation.h>
#include <mitkTestFixture.h>
#include <mitkTestingMacros.h>
#include <random>

//#include <boost/algorithm/string.hpp>

class m2MedianAbsoluteDeviationTestSuite : public mitk::TestFixture
{
  CPPUNIT_TEST_SUITE(m2MedianAbsoluteDeviationTestSuite);
  MITK_TEST(ApplyMAD_SeededGaussianNoise_shouldReturnTrue);

  CPPUNIT_TEST_SUITE_END();

private:
public:
  void ApplyMAD_SeededGaussianNoise_shouldReturnTrue()
  {

    std::vector<double> signal = {5, 5, 9, 5, 5, 5, 5, 0, 4, 4, 4, 6, 6, 6};
    double noiseLevel = m2::Signal::mad(signal);
    CPPUNIT_ASSERT_DOUBLES_EQUAL(double(1.4825999999999999), noiseLevel, mitk::eps);
  }
};

MITK_TEST_SUITE_REGISTRATION(m2MedianAbsoluteDeviation)


/*

 [begin_description]
 Test case for issue 149:
 Error C2582 with msvc-10 when using iterator-based integration
 [end_description]

 Copyright 2011-2015 Karsten Ahnert
 Copyright 2011-2015 Mario Mulansky

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

// disable checked iterator warning for msvc

#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE odeint_regression_147

#include <utility>
#include <iostream>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>
#include <boost/range/algorithm/find_if.hpp>

#include <boost/numeric/odeint.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;
namespace mpl = boost::mpl;

typedef std::vector<double> state_type;

void rhs( const state_type &x , state_type &dxdt , const double t )
{
}

template<class Stepper>
struct perform_test
{
    void operator()( void )
    {
        bulirsch_stoer< state_type > stepper( 1e-9, 0.0, 0.0, 0.0 );
        state_type x( 3, 10.0 );

        auto iter = boost::find_if(
            make_adaptive_time_range( stepper, rhs, x, 0.0, 1.0, 0.01 ),
            []( const std::pair< const state_type &, double > &x )
        { return ( x.first[0] < 0.0 ); } );

        std::cout << iter->second << "\t" << iter->first[0] << "\t"
                  << iter->first[1] << "\t" << iter->first[2] << "\n";
    }
};

typedef mpl::vector<
    euler< state_type > ,
    runge_kutta4< state_type > ,
    runge_kutta_cash_karp54< state_type > ,
    runge_kutta_dopri5< state_type > ,
    runge_kutta_fehlberg78< state_type > ,
    bulirsch_stoer< state_type >
    > steppers;

BOOST_AUTO_TEST_SUITE( regression_147_test )

BOOST_AUTO_TEST_CASE_TEMPLATE( regression_147_test , Stepper,
                               steppers )
{
    perform_test< Stepper > tester;
    tester();
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef CANNON_CONTROL_PID_H
#define CANNON_CONTROL_PID_H

/*!
 * \file cannon/control/pid.hpp
 * \brief File containing PidController class definition.
 */

#include <Eigen/Dense>

#include <cannon/utils/class_forward.hpp>

using namespace Eigen;

namespace cannon {

  namespace geom {
    CANNON_CLASS_FORWARD(Trajectory);
    CANNON_CLASS_FORWARD(ControlledTrajectory);
  }

  using namespace cannon::geom;

  namespace control {

    /*!
     * \brief Class representing a PID controller around a reference.
     */
    class PidController {
      public:

        /*!
         * \brief Constructor taking the state and control dimensionality of
         * the system to be controlled, as well as sampling time for
         * digital control.
         */
        PidController(unsigned int state_dim = 1, unsigned int control_dim = 1,
                      double timestep = 0.01);

        /*!
         * \brief Get PID control for the input state. Also affects integral
         * and derivative estimation.
         *
         * \param state The state to compute PID control for. Note that this is
         * in the global frame, not the error frame.
         *
         * \returns The computed PID control.
         */
        VectorXd get_control(const Ref<const VectorXd>& state);

        /*!
         * \brief Reset the internal derivative and integral estimation of this
         * PID controller.
         */
        void reset();

        /*!
         * \brief Get a modifiable reference to the reference set for this PID
         * controller. The dimension of this vector should not be changed.
         *
         * \returns Reference to the reference vector.
         */
        Ref<VectorXd> ref() {
          return ref_;
        }

        /*!
         * \brief Get a modifiable reference to the proportional gain matrix
         * for this PID controller. Matrix dimensions shouldn't be changed.
         *
         * \returns Modifiable reference to proportional gain matrix.
         */
        Ref<MatrixXd> proportional_gain() {
          return Kp_;
        }

        /*!
         * \brief Get a modifiable reference to the integral gain matrix
         * for this PID controller. Matrix dimensions shouldn't be changed.
         *
         * \returns Modifiable reference to integral gain matrix.
         */
        Ref<MatrixXd> integral_gain() {
          return Ki_;
        }

        /*!
         * \brief Get a modifiable reference to the derivative gain matrix
         * for this PID controller. Matrix dimensions shouldn't be changed.
         *
         * \returns Modifiable reference to derivative gain matrix.
         */
        Ref<MatrixXd> derivative_gain() {
          return Kd_;
        }

      private:
        unsigned int state_dim_; //!< State dimension of system to be controlled
        unsigned int control_dim_; //!< Control dimension of system to be controlled
        double time_step_; //!< Control sampling timestep

        VectorXd ref_; //!< Current reference
        VectorXd penultimate_error_state_; //!< Second-to-last observed error state
        VectorXd last_error_state_; //!< Last observed error state, used for discrete derivative
        VectorXd last_control_; //!< Previous computed control

        MatrixXd Kp_; //!< Proportional term gain matrix
        MatrixXd Ki_; //!< Integral term gain matrix
        MatrixXd Kd_; //!< Derivative term gain matrix

    };

    /*!
     * \brief Construct a controlled trajectory tracking the input geometric
     * path using the input PID controller and the input model.
     *
     * \param system Dynamic system simulator to compute controlled trajectory in. Note that there are no checks on the dimensions of the arguments to this system, so watch for dimension errors.
     * \param traj Geometric trajectory to track.
     * \param controller PID controller to track trajectory with.
     * \param controlled_dims Number of state dimensions to feed to PID controller.
     * \param total_dims Total number of state dimensions
     * \param timestep Integration timestep of the input system.
     *
     * \returns The generated controlled trajectory in the input simulator.
     */
    ControlledTrajectory get_pid_controlled_trajectory(
        std::function<VectorXd(const Ref<const VectorXd> &,
                               const Ref<const VectorXd> &)>
            system,
        const Trajectory &traj, PidController &controller,
        unsigned int controlled_dims, unsigned int total_dims,
        double timestep = 0.01);
  }
}

#endif /* ifndef CANNON_CONTROL_PID_H */


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing round capabilities

    round(x,n) rounds aways from 0 to n digits:

    @par semantic:
    For any given value @c x of type @c T and integer n :

    @code
    T r = round(x, n);
    @endcode

    is equivalent to

    @code
    T r = round(x*exp10(n)*exp10(-n));
    @endcode

    @par Note:

    - n > 0: round to n digits to the right of the decimal point.

    - n = 0: round to the nearest integer.

    - n < 0: round to n digits to the left of the decimal point.

    aways from 0 means that half integer values are rounded to the nearest
    integer of greatest absolute value

  **/
  const boost::dispatch::functor<tag::round_> round = {};
} }
#endif

#include <boost/simd/function/scalar/round.hpp>
#include <boost/simd/function/simd/round.hpp>

#endif


//
// Created by Zhongshi Jiang on 5/8/17.
//

#include <igl/copyleft/tetgen/tetrahedralize.h>
#include <Eigen/Core>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/triangle/triangulate.h>
#include <igl/cat.h>
#include <igl/writeOBJ.h>
#include <igl/readOBJ.h>
#include <igl/writeMESH.h>
#include <igl/readMESH.h>
#include <igl/barycenter.h>
#include <igl/read_triangle_mesh.h>
#include <iostream>
#include <igl/Timer.h>
#include <fstream>
#include <igl/boundary_loop.h>
#include <igl/harmonic.h>
#include <igl/map_vertices_to_circle.h>
#include <igl/doublearea.h>
#include <igl/PI.h>
#include <igl/flipped_triangles.h>
#include <igl/file_dialog_open.h>

void parameterization_init(std::string filename, Eigen::MatrixXd& V_ref,
                           Eigen::MatrixXi &F_ref,
                           Eigen::MatrixXd& V_all, Eigen::MatrixXi &F_scaf,
  Eigen::VectorXi &frame_id, Eigen::MatrixXi& display_F) {
  using namespace std;
  using namespace Eigen;
  while (!igl::read_triangle_mesh(filename, V_ref,
                                  F_ref)) {
    std::cerr << "Cannot Open Mesh!" << std::endl;
    filename = igl::file_dialog_open();
  }

  Eigen::MatrixXd uv_init;
  Eigen::VectorXi bnd;
  Eigen::MatrixXd bnd_uv;
  igl::Timer timer;
  timer.start();
  igl::boundary_loop(F_ref, bnd);
  cout << "bndloop = " << timer.getElapsedTime() << endl;
  timer.start();

  timer.start();
  VectorXd M;
  igl::doublearea(V_ref, F_ref, M);
 std::cout<<"sqrtM/2pi"<< sqrt(M.sum()/(2*igl::PI))<<std::endl;
//  M /= M.sum()/igl::PI;

  igl::map_vertices_to_circle(V_ref, bnd, bnd_uv);
  V_ref *= 2;
  bnd_uv *= sqrt(M.sum()/(2*igl::PI));
  cout << "v2circle = " << timer.getElapsedTime() << endl;
  igl::harmonic(V_ref, F_ref, bnd, bnd_uv, 1, uv_init);
  if (igl::flipped_triangles(uv_init, F_ref).size() != 0) {
    igl::harmonic(F_ref, bnd, bnd_uv, 1, uv_init); // use uniform laplacian
  }
  cout << "Harmonic = " << timer.getElapsedTime() << endl;

//  scaffold_generator(uv_init, F_ref, scaf_data.density, Vall, Fscaf);

  MatrixXd V_bnd;
  V_bnd.resize(bnd.size(), uv_init.cols());
  for (int i = 0; i < bnd.size(); i++) // redoing step 1.
  {
    V_bnd.row(i) = uv_init.row(bnd(i));
  }
  Matrix2d ob;// = rect_corners;
  {
    VectorXd uv_max = uv_init.colwise().maxCoeff();
    VectorXd uv_min = uv_init.colwise().minCoeff();
    VectorXd uv_mid = (uv_max + uv_min) / 2.;

    double scaf_range = 8;
    ob.row(0) = uv_mid + scaf_range * (uv_min - uv_mid);
    ob.row(1) = uv_mid + scaf_range * (uv_max - uv_mid);
  }
  Vector2d rect_len;
  rect_len << ob(1, 0) - ob(0, 0), ob(1, 1) - ob(0, 1);

  int frame_points = 5;
  MatrixXd V_rect;
  V_rect.resize(4 * frame_points, 2);
  for (int i = 0; i < frame_points; i++) {
    // 0,0;0,1
    V_rect.row(i) << ob(0, 0), ob(0, 1) + i * rect_len(1) / frame_points;
    // 0,0;1,1
    V_rect.row(i + frame_points)
        << ob(0, 0) + i * rect_len(0) / frame_points, ob(1, 1);
    // 1,0;1,1
    V_rect.row(i + 2 * frame_points) << ob(1, 0), ob(1, 1) - i * rect_len(1) /
        frame_points;
    // 1,0;0,1
    V_rect.row(i + 3 * frame_points)
        << ob(1, 0) - i * rect_len(0) / frame_points, ob(0, 1);
    // 0,0;0,1
  }

  // Concatenate Vert and Edge
  MatrixXd V;
  MatrixXi E;
  igl::cat(1, V_bnd, V_rect, V);
  E.resize(V.rows(), 2);
  for (int i = 0; i < E.rows(); i++)
    E.row(i) << i, i + 1;
  E(bnd.size() - 1, 1) = 0;
  E(V.rows() - 1, 1) = static_cast<int>(bnd.size());

  MatrixXd H = MatrixXd::Zero(10, 2);
  for (int f = 0; f < H.rows(); f++)
    for (int i = 0; i < 3; i++)
      H.row(f) += uv_init.row(F_ref(f, i)); // redoing step 2
  H /= 3.;
  timer.start();
  MatrixXd uv2;
  igl::triangle::triangulate(V, E, H, "qYYQ", uv2, F_scaf);

  auto bnd_n = bnd.size();
  V_all.resize(uv_init.rows() - bnd_n + uv2.rows(), 2);
  V_all.topRows(uv_init.rows()) = uv_init;
  V_all.bottomRows(uv2.rows() - bnd_n) = uv2.bottomRows(-bnd_n + uv2.rows());

  for (auto i = 0; i < F_scaf.rows(); i++)
    for (auto j = 0; j < F_scaf.cols(); j++) {
      auto &x = F_scaf(i, j);
      if (x < bnd_n) x = bnd(x);
      else x += uv_init.rows() - bnd_n;
    }

  cout << "New Cat = " << timer.getElapsedTime() << endl;

  frame_id = Eigen::VectorXi::LinSpaced(
      V_rect.rows(), V_ref.rows(), V_ref.rows() + V_rect.rows() - 1);
  igl::cat(1, F_ref, F_scaf, display_F);
}


/*
 * Waypoint controller
 * Given a list of 3D points, the controller generate altitude commands
 * and send it over to crazyflie using crazyflie_ros package.
 *
 * Topics:
 *   Sub:
 *     /vrpn_client_node/body1/Pose		geometry_msgs::PoseStamped  Current position
 *     "~/target_position				geometry_msgs::Pose         Target position
 *     "~/state							std_msgs::UInt8             See controller.hpp
 *
 *   Pub:
 *     /crazyflie/cmd_vel				geometry_msgs::Twist        Altitude commands
 *
 *   By Jason Chan
 *   Aug 18, 2017
 *
 */

#include <ros/ros.h>
#include <ros/console.h>

#include "std_msgs/UInt8.h"
#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/Float64MultiArray.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"

#include <cmath>
#include <Eigen/Dense>
#include "Controller.hpp"
#include "AdaptiveController.hpp"

using namespace Eigen;

// command parameters
#define POSITION_LOOP_RATE	200.0
#define POSITION_LOOP_DT	(double)(1.0/POSITION_LOOP_RATE)

////////////////////////////////////////////////////////
// * Adaptive controller related settings
////////////////////////////////////////////////////////
#define USE_ADAPTIVE_CONTROLLER
#define USE_RBF
////////////////////////////////////////////////////////

////////////////////////////////////////////////////////
// * Phi function, used in AdaptiveController Class
////////////////////////////////////////////////////////
#ifndef USE_RBF
    void phi_functions(VectorXd &phi, const VectorXd states)
    {
        double z = states(0);
        phi = VectorXd::Zero(2);
        phi(0) = z;
        phi(1) = 1;
    }
#else
    const double rbf_center[] = {-0.0, -0.2, -0.4, -0.6, -0.8, -1.0};
    //const double rbf_center[] = {-0.0, -0.4, -0.8};
    const double rbf_width = 0.6;
	const int nrbf = sizeof(rbf_center)/sizeof(double) + 1;

    inline double rbf(double x, double center, double base_width)
    {
      	return exp(-pow((x-center)/(base_width/4), 2));
    }

    void phi_functions(VectorXd &phi, const VectorXd states)
    {
        double z = states(0);

        phi = VectorXd::Zero(nrbf);
        for (int i=0; i<(nrbf-1); i++)
        {
            phi(i) = rbf(z, rbf_center[i], rbf_width);
        	//ROS_INFO("%d, %.4f, %.4f, %.4f, %.4f", i, phi(i), z, rbf_center[i], rbf_width);
        }
        phi(nrbf-1) = 1;

    }
#endif

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

////////////////////////////////////////////////////////
// * Global Variabls
////////////////////////////////////////////////////////
static ros::Publisher pub_command;
#ifdef USE_ADAPTIVE_CONTROLLER
	static AdaptiveController hover_controller(phi_functions);
	static ros::Publisher pub_ref_states;
	static ros::Publisher pub_adp_gains;
#else
	static Controller hover_controller;
#endif
static uint8_t last_state = 0;
////////////////////////////////////////////////////////

////////////////////////////////////////////////////////
// * Callback functions
////////////////////////////////////////////////////////
void sub_flight_state_callback(const std_msgs::UInt8::ConstPtr& _state) {
	uint8_t new_state = _state->data;
	if(last_state != new_state)
	{
		switch (new_state) {
			case 0:
				hover_controller.switch_to_standby();
				break;
			case 1:
				hover_controller.switch_to_tracking();
				break;
			default:
				hover_controller.switch_to_emergency();
		}
		last_state = _state->data;
	}
}

#ifdef USE_ADAPTIVE_CONTROLLER
void pub_ref_states_callback(const ros::TimerEvent& e) {
	geometry_msgs::PoseStamped pose;

	VectorXd states;
	hover_controller.get_reference_states(states);

	pose.header.stamp = ros::Time::now();
	pose.pose.position.x = 0.0;
	pose.pose.position.y = 0.0;
	pose.pose.position.z = states(0);
	pose.pose.orientation.x = 0.0;
	pose.pose.orientation.y = 0.0;
	pose.pose.orientation.z = 0.0;
	pose.pose.orientation.w = 0.0;

	pub_ref_states.publish(pose);
}

void pub_adp_gains_callback(const ros::TimerEvent& e) {
	std_msgs::Float64MultiArray msg_gains;

	MatrixXd m_gains;
	hover_controller.get_adaptive_gains(m_gains);

	const int m = m_gains.rows();
	const int n = m_gains.cols();
	for (int i=0;i<m;i++)
	{
		for (int j=0;j<n;j++)
		{
			msg_gains.data.push_back(m_gains(i, j));
		}
	}

	pub_adp_gains.publish(msg_gains);
}
#endif

void pub_cmd_callback(const ros::TimerEvent& e) {
	geometry_msgs::Twist cmd_vel;

	hover_controller.compute_outputs(&cmd_vel);
	pub_command.publish(cmd_vel);
}
////////////////////////////////////////////////////////

////////////////////////////////////////////////////////
// * Main
////////////////////////////////////////////////////////
int main(int argc, char **argv) {
	ros::init(argc, argv, "hover_controller");
	ros::NodeHandle n("~");

	PID_Config_t config_x;
	PID_Config_t config_y;
	PID_Config_t config_z;

	ReferenceModel_Config_t config_model_z;
	AdaptiveLaw_Config_t config_law_z;

	// MatLab + tuned
	//n.param("Kpx", config_x.Kp,  -0.259246154*1.5);
	//n.param("Kix", config_x.Ki,  -0.049913621*1.5);
	//n.param("Kdx", config_x.Kd,  -0.336624385*1.5);
	//n.param("Kpy", config_y.Kp,  -0.259246154*1.5);
	//n.param("Kiy", config_y.Ki,  -0.049913621*1.5);
	//n.param("Kdy", config_y.Kd,  -0.336624385*1.5);
	//n.param("Kpz", config_z.Kp,  42692.24448);
	//n.param("Kiz", config_z.Ki,  8219.695803);
	//n.param("Kdz", config_z.Kd,  55434.76858);

	// MatLab
	n.param("Kpx", config_x.Kp,  -0.259246154*1.5);
	n.param("Kix", config_x.Ki,  -0.049913621*1.5);
	n.param("Kdx", config_x.Kd,  -0.336624385*1.5);
	n.param("Kpy", config_y.Kp,  -0.259246154*1.5);
	n.param("Kiy", config_y.Ki,  -0.049913621*1.5);
	n.param("Kdy", config_y.Kd,  -0.336624385*1.5);
	n.param("Kpz", config_z.Kp,  42692.24448);
	n.param("Kiz", config_z.Ki,  8219.695803);
	n.param("Kdz", config_z.Kd,  55434.76858);

	config_x.dt 		= POSITION_LOOP_DT;
	config_x.threshold	= 1.0e-4;
	config_x.max_output	= 4.0e4;
	config_y.dt 		= POSITION_LOOP_DT;
	config_y.threshold	= 1.0e-4;
	config_y.max_output	= 4.0e4;
	config_z.dt 		= POSITION_LOOP_DT;
	config_z.threshold	= 1.0e-4;
	config_z.max_output	= 4.0e4;

    // Reference model
	config_model_z.A		= MatrixXd::Zero(2,2);
    config_model_z.B		= MatrixXd::Zero(2,1);
	config_model_z.states0	= VectorXd::Zero(2);
	config_model_z.dt		= POSITION_LOOP_DT;
    config_model_z.A(0,0)	=  0.0;
    config_model_z.A(0,1)	=  1.0;
    config_model_z.A(1,0)	= -5.416;
    config_model_z.A(1,1)	= -7.027;
    config_model_z.B(0,0)	= -0.1145;
    config_model_z.B(1,0)	=  6.273;
	config_model_z.states0(0) = -0.03; // ground is not at zero

    // Adaptive Law
#ifndef USE_RBF
	config_law_z.Gamma	= 6.0e-4*MatrixXd::Identity(2,2);
#else
	config_law_z.Gamma	= 6.0e-4*MatrixXd::Identity(nrbf,nrbf);
#endif
	config_law_z.P		= MatrixXd::Zero(2,2);
	config_law_z.B		= MatrixXd::Zero(2,1);
	config_law_z.dt		= POSITION_LOOP_DT;
    config_law_z.P(0,0) = 72.373382399897039;
    config_law_z.P(0,1) =  9.231905465288030;
    config_law_z.P(1,0) =  9.231905465288030;
    config_law_z.P(1,1) =  1.384930335176893;
    config_law_z.B(0,0) =  0.0;
    config_law_z.B(1,0) = 26.232948583420782;

#ifdef USE_ADAPTIVE_CONTROLLER
    hover_controller.initialize(config_x, config_y, config_z,
								config_model_z, config_law_z,
								POSITION_LOOP_DT);
#else
	hover_controller.initialize(config_x, config_y, config_z);
#endif

    ros::Subscriber sub_position = n.subscribe<geometry_msgs::PoseStamped>(
        "current_pose", 1, &Controller::update_position, (Controller*) &hover_controller);
    ros::Subscriber sub_target = n.subscribe<geometry_msgs::Pose>(
        "target_pose", 1, &Controller::update_target, (Controller*) &hover_controller);
	ros::Subscriber sub_flight_state = n.subscribe<std_msgs::UInt8>(
        "state", 1, sub_flight_state_callback);

	pub_command = n.advertise<geometry_msgs::Twist>("/crazyflie/cmd_vel", 1);
	ros::Timer timer_pub_cmd = n.createTimer(
        ros::Duration(POSITION_LOOP_DT), pub_cmd_callback);
#ifdef USE_ADAPTIVE_CONTROLLER
	pub_ref_states = n.advertise<geometry_msgs::PoseStamped>("reference_states", 1);
	ros::Timer timer_pub_ref_states = n.createTimer(
        ros::Duration(POSITION_LOOP_DT), pub_ref_states_callback);

	pub_adp_gains = n.advertise<std_msgs::Float64MultiArray>("adaptive_gains", 1);
	ros::Timer timer_pub_adp_gains = n.createTimer(
        ros::Duration(POSITION_LOOP_DT), pub_adp_gains_callback);
#endif

	ros::spin();

	return 0;
}
////////////////////////////////////////////////////////


/* ===-- fixdfti.c - Implement __fixdfti -----------------------------------===
 *
 *                     The LLVM Compiler Infrastructure
 *
 * This file is dual licensed under the MIT and the University of Illinois Open
 * Source Licenses. See LICENSE.TXT for details.
 *
 * ===----------------------------------------------------------------------===
 */

#include "fp64.h"

#ifdef _MSC_VER
#include <boost/multiprecision/cpp_int.hpp>
typedef boost::multiprecision::int128_t fixint_t;
typedef boost::multiprecision::uint128_t fixuint_t;
#else
typedef __int128 fixint_t;
typedef unsigned __int128 fixuint_t;
#endif

fixint_t ___fixdfti(uint64_t a) {
    const fixint_t fixint_max = (fixint_t)((~(fixuint_t)0) / 2);
    const fixint_t fixint_min = -fixint_max - 1;
    // Break a into sign, exponent, significand
    const rep_t aRep = a;
    const rep_t aAbs = aRep & absMask;
    const fixint_t sign = aRep & signBit ? -1 : 1;
    const int exponent = (aAbs >> significandBits) - exponentBias;
    const rep_t significand = (aAbs & significandMask) | implicitBit;

    // If exponent is negative, the result is zero.
    if (exponent < 0)
        return 0;

    // If the value is too large for the integer type, saturate.
    if ((unsigned)exponent >= sizeof(fixint_t) * CHAR_BIT)
        return sign == 1 ? fixint_max : fixint_min;

    // If 0 <= exponent < significandBits, right shift to get the result.
    // Otherwise, shift left.
    if (exponent < significandBits)
        return sign * (significand >> (significandBits - exponent));
    else
        return sign * ((fixint_t)significand << (exponent - significandBits));

}


#include "proseco_planning/action/noiseGenerator.h"

#include <boost/math/policies/policy.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <memory>

#include "proseco_planning/action/action.h"
#include "proseco_planning/config/computeOptions.h"
#include "proseco_planning/config/configuration.h"
#include "proseco_planning/math/mathlib.h"

namespace proseco_planning {

/**
 * @brief Constructs a new Noise Generator object
 *
 */
NoiseGenerator::NoiseGenerator() {
  m_distributionY =
      std::normal_distribution<float>(cOpt().action_noise.meanY, cOpt().action_noise.sigmaY);
  m_distributionVx =
      std::normal_distribution<float>(cOpt().action_noise.meanVx, cOpt().action_noise.sigmaVx);
  m_normalDensityY  = boost::math::normal_distribution<float>(cOpt().action_noise.meanY,
                                                             cOpt().action_noise.sigmaY);
  m_normalDensityVx = boost::math::normal_distribution<float>(cOpt().action_noise.meanVx,
                                                              cOpt().action_noise.sigmaVx);
}

/**
 * @brief Creates a noisy action using the noise parameters for the Gaussian distribution from the
 * configuration.
 *
 * @param action The action which is to become noisy.
 * @return ActionPtr The action with noise.
 */
ActionPtr NoiseGenerator::createNoisyAction(const ActionPtr& action) {
  float epsilonY{m_distributionY(math::Random::engine())};
  float epsilonVx{m_distributionVx(math::Random::engine())};
  float likelihoodY{boost::math::pdf(m_normalDensityY, epsilonY)};
  float likelihoodVx{boost::math::pdf(m_normalDensityVx, epsilonVx)};

  auto noisyAction = std::make_shared<Action>(action->m_velocityChange + epsilonVx,
                                              action->m_lateralChange + epsilonY);

  noisyAction->noise.m_likelihoodY   = likelihoodY;
  noisyAction->noise.m_likelihoodVx  = likelihoodVx;
  noisyAction->noise.m_muY           = action->m_lateralChange;
  noisyAction->noise.m_muVx          = action->m_velocityChange;
  noisyAction->noise.m_sigmaY        = cOpt().action_noise.sigmaY;
  noisyAction->noise.m_sigmaVx       = cOpt().action_noise.sigmaVx;
  noisyAction->m_selectionLikelihood = action->m_selectionLikelihood;
  return noisyAction;
}

/**
 * @brief Create noisy versions of all actions of the action set using the noise generator.
 *
 * @param actionSet The action set which is to become noisy.
 * @return ActionSet The action set with noise.
 */
ActionSet NoiseGenerator::createNoisyActions(const ActionSet& actionSet) {
  ActionSet noisyActionSet;
  for (const auto& action : actionSet) {
    noisyActionSet.push_back(createNoisyAction(action));
  }
  return noisyActionSet;
}
}  // namespace proseco_planning


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Vincent Rouvreau
 *
 *    Copyright (C) 2020 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE "delaunay_complex"
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <CGAL/Epick_d.h>
#include <CGAL/Epeck_d.h>

#include <vector>
#include <limits>  // NaN
#include <cmath>

#include <gudhi/Alpha_complex.h>
// to construct a simplex_tree from Delaunay_triangulation
#include <gudhi/graph_simplicial_complex.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Unitary_tests_utils.h>
#include <gudhi/random_point_generators.h>

// Use dynamic_dimension_tag for the user to be able to set dimension
typedef CGAL::Epeck_d< CGAL::Dynamic_dimension_tag > Exact_kernel_d;
// Use static dimension_tag for the user not to be able to set dimension
typedef CGAL::Epeck_d< CGAL::Dimension_tag<5> > Exact_kernel_s;
// Use dynamic_dimension_tag for the user to be able to set dimension
typedef CGAL::Epick_d< CGAL::Dynamic_dimension_tag > Inexact_kernel_d;
// Use static dimension_tag for the user not to be able to set dimension
typedef CGAL::Epick_d< CGAL::Dimension_tag<5> > Inexact_kernel_s;
// The triangulation uses the default instantiation of the TriangulationDataStructure template parameter

typedef boost::mpl::list<Exact_kernel_d, Exact_kernel_s, Inexact_kernel_d, Inexact_kernel_s> list_of_kernel_variants;

using Simplex_tree = Gudhi::Simplex_tree<>;
using Simplex_handle = Simplex_tree::Simplex_handle;

BOOST_AUTO_TEST_CASE_TEMPLATE(Alpha_complex_from_OFF_file, TestedKernel, list_of_kernel_variants) {
  std::cout << "*****************************************************************************************************";
  using Point = typename TestedKernel::Point_d;
  std::vector<Point> points;
  // 50 points on a 4-sphere
  points = Gudhi::generate_points_on_sphere_d<TestedKernel>(10, 5, 1.);

  Gudhi::alpha_complex::Alpha_complex<TestedKernel> alpha_complex(points);

  // Alpha complex
  Simplex_tree stree_from_alpha_complex;
  BOOST_CHECK(alpha_complex.create_complex(stree_from_alpha_complex));

  // Delaunay complex
  Simplex_tree stree_from_delaunay_complex;
  BOOST_CHECK(alpha_complex.create_complex(stree_from_delaunay_complex, 0., false, true));

  // Check all the simplices from alpha complex are in the Delaunay complex
  for (auto f_simplex : stree_from_alpha_complex.complex_simplex_range()) {
    Simplex_handle sh = stree_from_delaunay_complex.find(stree_from_alpha_complex.simplex_vertex_range(f_simplex));
    BOOST_CHECK(std::isnan(stree_from_delaunay_complex.filtration(sh)));
    BOOST_CHECK(sh != stree_from_delaunay_complex.null_simplex());
  }
}


/*
Copyright (c) 2014, Aaron S Wishnick
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 Aaron S Wishnick, iZotope, Inc., 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 THE COPYRIGHT HOLDER OR CONTRIBUTORS 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 <iostream>
#include <cmath>
#include <array>
#include <vector>
#include <iomanip>
#include <string>
#include <complex>
#include <functional>
#include <map>
#include <random>
#include <Eigen/Dense>
#include <memory>
#include "sndfile.h"
using namespace std;
using namespace Eigen;

static array<double, 5> lowpass_coefficients(double w0, double q) {
  array<double, 5> coeffs;

  const double cosw0 = cos(w0);
  const double alpha = sin(w0) / (2 * q);

  const double a0 = 1 + alpha;

  coeffs[0] = 0.5 * (1 - cosw0);
  coeffs[1] = 1 - cosw0;
  coeffs[2] = 0.5 * (1 - cosw0);
  coeffs[3] = -2 * cosw0;
  coeffs[4] = 1 - alpha;

  for (auto& x : coeffs) {
    x /= a0;
  }

  return coeffs;
}

static array<double, 5> peaking_coefficients(double w0, double q, double gain) {
  array<double, 5> coeffs;

  const double A = pow(10.0, gain / 40);
  const double alpha = sin(w0) / (2 * q);

  coeffs[0] = 1 + alpha * A;
  coeffs[1] = -2 * cos(w0);
  coeffs[2] = 1 - alpha * A;
  double a0 = 1 + alpha / A;
  coeffs[3] = -2 * cos(w0);
  coeffs[4] = 1 - alpha / A;

  for (auto& x : coeffs) {
    x /= a0;
  }

  return coeffs;
}

static vector<double> generate_sine_wave(int length, double cycles_per_sample) {
  vector<double> signal(length);

  if (cycles_per_sample == 0) {
    signal.assign(length, 1.0);
    return signal;
  }

  for (int i = 0; i < length; ++i) {
    signal[i] = sin(2 * M_PI * i * cycles_per_sample);
  }
  return signal;
}

// Smooth a signal with a linear-phase Hann filter.
static vector<double> smooth_hann(const vector<double>& x, int radius) {
  int n = x.size();
  int m = 2 * radius + 1;

  vector<double> y(n + m - 1);

  vector<double> h(m);
  double norm = 0;
  for (int i = 0; i < m; ++i) {
    h[i] = 0.5 - 0.5 * cos((i + 0.5) * 2.0 * M_PI / m);
    norm += h[i];
  }
  for (int i = 0; i < m; ++i) {
    h[i] /= norm;
  }

  for (int i = 0; i < n + m - 1; ++i) {
    double sum = 0;
    for (int j = 0; j < m; ++j) {
      int idx = radius + i - j;
      if (idx < 0) {
        break;
      }
      sum += h[j] * x[idx];
    }
    y[i] = sum;
  }

  return y;
}

// Generate a Hann window.
static vector<double> generate_hann_window(int n) {
  vector<double> w(n);
  for (int i = 0; i < n; ++i) {
    w[i] = 0.5 - 0.5 * cos(2.0 * M_PI * i / (n - 1));
  }
  return w;
}

// Compute the gain of a biquad at frequency w0.
static double biquad_gain(array<double, 5> coeffs, double w0) {
  const double b0 = coeffs[0], b1 = coeffs[1], b2 = coeffs[2];
  const double a1 = coeffs[3], a2 = coeffs[4];

  auto z = polar(1.0, w0);
  auto z2 = z * z;

  auto h = (b0 + b1 / z + b2 / z2) / (1.0 + a1 / z + a2 / z2);
  return abs(h);
}

// High-quality reference signal. Assumes that the filter input is a sinusoid.
// Smooths the gain of the filter coefficients, and applies it to the signal.
static vector<double> reference(vector<double> x,
                                const vector<array<double, 5>>& coeffs,
                                double w0, int radius) {
  const int n = x.size();

  vector<double> gains(n);
  transform(begin(coeffs), end(coeffs), begin(gains),
            [w0](array<double, 5> coeffs) { return biquad_gain(coeffs, w0); });

  gains = smooth_hann(gains, radius);

  transform(begin(x), end(x), begin(gains), begin(x),
            [](double sig, double gain) { return sig * gain; });

  return x;
}

// Low-quality anchor. Applies the gain to the signal, with no smoothing, and
// inserts an impulse whenever coefficients change.
static vector<double>
anchor(vector<double> x, const vector<array<double, 5>>& coeffs, double w0) {
  const int n = x.size();

  vector<double> gains(n);
  transform(begin(coeffs), end(coeffs), begin(gains),
            [w0](array<double, 5> coeffs) { return biquad_gain(coeffs, w0); });

  double max_gain = accumulate(begin(gains), end(gains), 0.0,
                               [](double a, double b) { return max(a, b); });
  double transient_gain = 3 * max_gain;

  for (int i = 0; i < n; ++i) {
    if (i != 0 && gains[i] != gains[i - 1]) {
      // If there was a change in gain, insert a click.
      x[i] += transient_gain;
    } else {
      x[i] *= gains[i];
    }
  }

  return x;
}

// Vanilla Direct Form II implementation.
static vector<double> time_varying_df2(vector<double> x,
                                       const vector<array<double, 5>>& coeffs) {
  double w1 = 0, w2 = 0;
  const int n = x.size();
  for (int i = 0; i < n; ++i) {
    auto cur_coeffs = coeffs[i];

    double w = x[i] - cur_coeffs[3] * w1 - cur_coeffs[4] * w2;
    double y = cur_coeffs[0] * w + cur_coeffs[1] * w1 + cur_coeffs[2] * w2;
    x[i] = y;

    w2 = w1;
    w1 = w;
  }

  return x;
}

// Vanilla SVF implementation.
static vector<double> time_varying_svf(vector<double> x,
                                       const vector<array<double, 5>>& coeffs) {
  double s1 = 0, s2 = 0;
  const int n = x.size();

  for (int i = 0; i < n; ++i) {
    const double b0 = coeffs[i][0], b1 = coeffs[i][1], b2 = coeffs[i][2];
    const double a1 = coeffs[i][3], a2 = coeffs[i][4];

    const complex<double> neg_sqrt = sqrt(complex<double>(-1 - a1 - a2));
    const complex<double> pos_sqrt = sqrt(complex<double>(-1 + a1 - a2));

    const double g = (neg_sqrt / pos_sqrt).real();
    const double R = (a2 - 1) / (neg_sqrt * pos_sqrt).real();
    const double TwoR = 2 * R;
    const double g2 = g * g;
    const double chp = (b0 - b1 + b2) / (1 - a1 + a2);
    const double cbp = -2 * (b0 - b2) / (neg_sqrt * pos_sqrt).real();
    const double clp = (b0 + b1 + b2) / (1 + a1 + a2);

    double u1 = (x[i] - (g + TwoR) * s1 - s2) / (1 + g2 + TwoR * g);
    double u2 = g * u1 + s1;
    double u3 = g * u2 + s2;

    s1 += 2.0 * g * u1;
    s2 += 2.0 * g * u2;

    x[i] = chp * u1 + cbp * u2 + clp * u3;
  }

  return x;
}

// SVF, with the stabilization method from "Stability of Recursive Time-Varying
// Digital Filters by State Vector Transformation."
static vector<double>
svf_rabenstein_czarnach(vector<double> x,
                        const vector<array<double, 5>>& coeffs) {
  Matrix<double, 2, 1> s = Matrix<double, 2, 1>::Zero();
  const int n = x.size();

  double R_prev;

  for (int i = 0; i < n; ++i) {
    const double b0 = coeffs[i][0], b1 = coeffs[i][1], b2 = coeffs[i][2];
    const double a1 = coeffs[i][3], a2 = coeffs[i][4];

    const complex<double> neg_sqrt = sqrt(complex<double>(-1 - a1 - a2));
    const complex<double> pos_sqrt = sqrt(complex<double>(-1 + a1 - a2));

    const double g = (neg_sqrt / pos_sqrt).real();
    const double R = (a2 - 1) / (neg_sqrt * pos_sqrt).real();
    const double g2 = g * g;

    const double chp = (b0 - b1 + b2) / (1 - a1 + a2);
    const double cbp = -2 * (b0 - b2) / (neg_sqrt * pos_sqrt).real();
    const double clp = (b0 + b1 + b2) / (1 + a1 + a2);

    Matrix<double, 2, 2> A;
    A(0, 0) = -2 * R;
    A(0, 1) = -1;
    A(1, 0) = 1;
    A(1, 1) = 0;

    Matrix<double, 2, 1> B;
    B(0, 0) = 1;
    B(1, 0) = 0;

    Matrix<double, 2, 2> H =
        (Matrix<double, 2, 2>::Identity() - g * A).inverse();

    Matrix<double, 2, 2> Pa = Matrix<double, 2, 2>::Identity() + 2 * g * A * H;
    Matrix<double, 2, 1> Pb = 2 * g2 * A * H * B + 2 * g * B;
    Matrix<double, 2, 1> Pc;
    Pc(0, 0) = cbp - 2 * R * chp;
    Pc(1, 0) = clp - chp;
    double Pd = chp;

    Matrix<double, 2, 2> TT;
    double Rn = i != 0 ? R_prev : R;
    TT(0, 0) = (sqrt(complex<double>(Rn * Rn - 1)) /
                sqrt(complex<double>(R * R - 1))).real();
    TT(0, 1) = -(complex<double>(0, 1) * (R - Rn) /
                 sqrt(complex<double>(R * R - 1))).real();
    TT(1, 0) = 0;
    TT(1, 1) = 1;

    s = TT * s;

    // Compute output.
    double y = Pc.transpose() * s + Pd * x[i];

    // State update
    s = Pa * s + Pb * x[i];

    R_prev = R;

    x[i] = y;
  }

  return x;
}

template <int N>
static Matrix<double, N, N>
ComputeRabensteinK(Matrix<double, N, N> A1, Matrix<double, N, 1> b1,
                   Matrix<double, N, N> A2, Matrix<double, N, 1> b2) {
  Matrix<double, N, N> K = b1 * b2.transpose();

  auto Ae1 = A1;
  auto Ae2 = A2;
  int i = 0;
  while (i++ < 1000000 && (Ae1.template lpNorm<Infinity>() > 1e-12 ||
                           Ae2.template lpNorm<Infinity>() > 1e-12)) {
    Matrix<double, N, 1> f1 = Ae1 * b1;
    Matrix<double, N, 1> f2 = Ae2 * b2;

    K += f1 * f2.transpose();

    Ae1 *= A1;
    Ae2 *= A2;
  }

  return K;
}

// Direct Form 2 Transposed
static vector<double> tdf2(vector<double> x,
                           const vector<array<double, 5>>& coeffs) {
  const int n = x.size();
  Matrix<double, 2, 1> s = Matrix<double, 2, 1>::Zero();

  for (int i = 0; i < n; ++i) {
    const double b0 = coeffs[i][0], b1 = coeffs[i][1], b2 = coeffs[i][2];
    const double a1 = coeffs[i][3], a2 = coeffs[i][4];

    Matrix<double, 2, 2> A;
    A << -a1, 1.0, -a2, 0.0;

    Matrix<double, 2, 1> b;
    b << b1 - b0* a1, b2 - b0* a2;

    Matrix<double, 2, 1> c;
    c << 1.0, 0.0;

    double d = b2;

    double y = c.transpose() * s + d * x[i];
    s = A * s + b * x[i];

    x[i] = y;
  }

  return x;
}

static vector<double>
tdf2_rabenstein_czarnach(vector<double> x,
                         const vector<array<double, 5>>& coeffs) {
  const int n = x.size();
  Matrix<double, 2, 1> s = Matrix<double, 2, 1>::Zero();

  for (int i = 0; i < n; ++i) {
    const double b0 = coeffs[i][0], b1 = coeffs[i][1], b2 = coeffs[i][2];
    const double a1 = coeffs[i][3], a2 = coeffs[i][4];

    Matrix<double, 2, 2> A;
    A << -a1, 1.0, -a2, 0.0;

    Matrix<double, 2, 1> b;
    b << b1 - b0* a1, b2 - b0* a2;

    Matrix<double, 2, 1> c;
    c << 1.0, 0.0;

    double d = b2;

    double r = sqrt(a2);
    double t = acos(a1 / (-2 * r));

    auto prev_coeffs = i != 0 ? coeffs[i - 1] : coeffs[i];
    double rp = sqrt(prev_coeffs[4]);
    double tp = acos(prev_coeffs[3] / (-2 * rp));

    Matrix<double, 2, 2> TT;
    TT << 1, 0, r* sin(t - tp) / sin(tp), r * sin(t) / (rp * sin(tp));
    s = TT * s;

    double y = c.transpose() * s + d * x[i];
    s = A * s + b * x[i];

    x[i] = y;
  }

  return x;
}

// SVF, with transient minimization as "Minimization of Transient Signals in
// Recursive Time-Varying Digital Filters".
static vector<double> svf_rabenstein(vector<double> x,
                                     const vector<array<double, 5>>& coeffs) {
  Matrix<double, 2, 1> s = Matrix<double, 2, 1>::Zero();
  const int n = x.size();

  Matrix<double, 2, 2> Pa_prev;
  Matrix<double, 2, 1> Pb_prev;

  for (int i = 0; i < n; ++i) {
    const double b0 = coeffs[i][0], b1 = coeffs[i][1], b2 = coeffs[i][2];
    const double a1 = coeffs[i][3], a2 = coeffs[i][4];

    const complex<double> neg_sqrt = sqrt(complex<double>(-1 - a1 - a2));
    const complex<double> pos_sqrt = sqrt(complex<double>(-1 + a1 - a2));

    const double g = (neg_sqrt / pos_sqrt).real();
    const double R = (a2 - 1) / (neg_sqrt * pos_sqrt).real();
    const double g2 = g * g;

    const double chp = (b0 - b1 + b2) / (1 - a1 + a2);
    const double cbp = -2 * (b0 - b2) / (neg_sqrt * pos_sqrt).real();
    const double clp = (b0 + b1 + b2) / (1 + a1 + a2);

    Matrix<double, 2, 2> A;
    A(0, 0) = -2 * R;
    A(0, 1) = -1;
    A(1, 0) = 1;
    A(1, 1) = 0;

    Matrix<double, 2, 1> B;
    B(0, 0) = 1;
    B(1, 0) = 0;

    Matrix<double, 2, 2> H =
        (Matrix<double, 2, 2>::Identity() - g * A).inverse();

    Matrix<double, 2, 2> Pa = Matrix<double, 2, 2>::Identity() + 2 * g * A * H;
    Matrix<double, 2, 1> Pb = 2 * g2 * A * H * B + 2 * g * B;
    Matrix<double, 2, 1> Pc;
    Pc(0, 0) = cbp - 2 * R * chp;
    Pc(1, 0) = clp - chp;
    double Pd = chp;

    if (i != 0 && coeffs[i] != coeffs[i - 1]) {
      auto Knn = ComputeRabensteinK(Pa_prev, Pb_prev, Pa_prev, Pb_prev);
      auto Knp = ComputeRabensteinK(Pa_prev, Pb_prev, Pa, Pb);

      Pa_prev = Pa;
      Pb_prev = Pb;

      Matrix<double, 2, 2> T = Knp.transpose() * Knn.inverse();
      Pa = Pa * T;
      Pc = (Pc.transpose() * T).transpose();
    } else {
      Pa_prev = Pa;
      Pb_prev = Pb;
    }

    // Compute output.
    double y = Pc.transpose() * s + Pd * x[i];

    // State update
    s = Pa * s + Pb * x[i];

    x[i] = y;
  }

  return x;
}

// Truncated output switching, as in "Suppression of Transients in Time-Varying
// Recursive Filters for Audio Signals".
static vector<double> truncated_output_switching_filter(
    vector<double> x, const vector<array<double, 5>> coeffs, int Na) {
  const int n = x.size();
  vector<double> ys(n);
  for (int i = 0; i < n; ++i) {
    auto cur_coeffs = coeffs[i];
    double w1 = 0, w2 = 0;
    double y = 0;

    int start = max(i - Na + 1, 0);
    for (int j = start; j <= i; ++j) {
      double w = x[j] - cur_coeffs[3] * w1 - cur_coeffs[4] * w2;
      y = cur_coeffs[0] * w + cur_coeffs[1] * w1 + cur_coeffs[2] * w2;

      w2 = w1;
      w1 = w;
    }

    ys[i] = y;
  }

  return ys;
}

// Output switching, equivalent to Zetterberg-Zhang.
static vector<double>
output_switching_filter(vector<double> x,
                        const vector<array<double, 5>> coeffs) {
  return truncated_output_switching_filter(x, coeffs, x.size() + 1);
  const int n = x.size();
  vector<double> ys(n);
  for (int i = 0; i < n; ++i) {
    auto cur_coeffs = coeffs[i];
    double w1 = 0, w2 = 0;
    double y = 0;

    for (int j = 0; j <= i; ++j) {
      double w = x[j] - cur_coeffs[3] * w1 - cur_coeffs[4] * w2;
      y = cur_coeffs[0] * w + cur_coeffs[1] * w1 + cur_coeffs[2] * w2;

      w2 = w1;
      w1 = w;
    }

    ys[i] = y;
  }

  return ys;
}

// Gold/Rader or coupled/normal form.
static vector<double> gold_rader(vector<double> x,
                                 const vector<array<double, 5>> coeffs) {
  const int n = x.size();
  double u1 = 0, u2 = 0;

  for (int i = 0; i < n; ++i) {
    const double a1 = coeffs[i][3], a2 = coeffs[i][4];
    double alpha = -a1 / 2;
    double beta = -0.5 * sqrt(4 * a2 - a1 * a1);

    const double b0 = coeffs[i][0], b1 = coeffs[i][1], b2 = coeffs[i][2];
    double alpha2 = alpha * alpha, beta2 = beta * beta;
    double k1 = b0 - b2 / (alpha2 + beta2);
    double k2 = (b2 * alpha + (b1 + b0 * alpha) * (alpha2 + beta2)) /
                (beta * (alpha2 + beta2));
    double k3 = b2 / (alpha2 + beta2);

    double y = k1 * u1 + k2 * u2 + k3 * x[i];

    double u1_new = alpha * u1 - beta * u2 + x[i];
    double u2_new = beta * u1 + alpha * u2;
    u1 = u1_new;
    u2 = u2_new;
    x[i] = y;
  }

  return x;
}

static void write_wav(const char* path, double sampling_rate, int channel_count,
                      const vector<double>& samples) {
  SF_INFO sfinfo;
  sfinfo.samplerate = static_cast<int>(sampling_rate + 0.5);
  sfinfo.channels = channel_count;
  sfinfo.format = SF_FORMAT_WAV | SF_FORMAT_FLOAT;
  unique_ptr<SNDFILE, int (*)(SNDFILE*)> file(sf_open(path, SFM_WRITE, &sfinfo),
                                              &sf_close);

  if (!file) {
    cerr << "Error opening \"" << path << "\" for writing. "
         << sf_strerror(nullptr) << endl;
    return;
  }

  // Make the data multichannel.
  vector<float> multichannel_samples;
  multichannel_samples.reserve(channel_count * samples.size());
  for (auto x : samples) {
    for (int i = 0; i < channel_count; ++i) {
      multichannel_samples.push_back(x);
    }
  }

  sf_write_float(file.get(), multichannel_samples.data(),
                 multichannel_samples.size());
}

int main(int argc, char* argv[]) {
  if (argc != 4) {
    cerr << "Usage: " << argv[0]
         << " [filter type] [sine_hz|dc] [output_dir]\n";
    return -1;
  }

  double cycles_per_second;
  bool dc_input;
  try {
    if (strcmp(argv[2], "dc") == 0) {
      dc_input = true;
      cycles_per_second = 100;
    } else {
      dc_input = false;
      cycles_per_second = stod(argv[2]);
    }
  }
  catch (...) {
    cerr << "Invalid stimulus \"" << argv[2] << "\".\n";
    return -2;
  }

  string output_dir(argv[3]);

  const double sampling_rate = 48000;
  const double cycles_per_sample = cycles_per_second / sampling_rate;
  const double q = 6.0;
  const double cutoff_scale = 0.2;
  const double start_cutoff =
      max(cycles_per_second, 100.0) * (1 - cutoff_scale);
  const double end_cutoff = max(cycles_per_second, 100.0) * (1 + cutoff_scale);
  const int constant_length = 48000;
  const int end_constant_length = sampling_rate;
  const int start_vary_pos = constant_length;
  const int max_ramp_length = 200;
  const int length = constant_length + max_ramp_length + end_constant_length;

  auto ramp_fn = [=](int i, int ramp_length, double start_val, double end_val) {
    if (i < start_vary_pos) {
      return start_val;
    }
    if (i < start_vary_pos + ramp_length) {
      return start_val + static_cast<double>(i - start_vary_pos) / ramp_length *
                             (end_val - start_val);
    }
    return end_val;
  };

  const double start_w0 = start_cutoff * 2 * M_PI / sampling_rate;
  const double end_w0 = end_cutoff * 2 * M_PI / sampling_rate;
  auto vary_freq_fn = [=](int i, int ramp_length = 0) {
    return ramp_fn(i, ramp_length, start_w0, end_w0);
  };

  const double start_gain = -4, end_gain = 4;
  const double w0 = cycles_per_second * 2 * M_PI / sampling_rate;
  auto vary_gain_fn = [=](int i, int ramp_length = 0) {
    return ramp_fn(i, ramp_length, start_gain, end_gain);
  };

  const double start_q = 0.6, end_q = 4;
  auto vary_q_fn = [=](int i, int ramp_length = 0) {
    return ramp_fn(i, ramp_length, start_q, end_q);
  };

  auto input_signal =
      generate_sine_wave(length, dc_input ? 0 : cycles_per_sample);

  map<string, function<array<double, 5>(int i)>> filter_types = {
      {"lowpass_freq",
       [=](int i) { return lowpass_coefficients(vary_freq_fn(i), q); }},
      {"lowpass_q",
       [=](int i) { return lowpass_coefficients(w0, vary_q_fn(i)); }},
      {"peaking_freq", [=](int i) {
        return peaking_coefficients(vary_freq_fn(i), q, end_gain);
      }},
      {"peaking_gain",
       [=](int i) { return peaking_coefficients(w0, q, vary_gain_fn(i)); }},
      {"peaking_q", [=](int i) {
        return peaking_coefficients(end_w0, vary_q_fn(i), end_gain);
      }},
  };

  auto filter_iter = filter_types.find(argv[1]);
  if (filter_iter == end(filter_types)) {
    cerr << "Invalid filter type \"" << argv[1] << "\". Types:\n";
    for (const auto& p : filter_types) {
      cerr << "\t" << p.first << endl;
    }
    return -1;
  }
  auto coeffs_fn = filter_iter->second;

  vector<array<double, 5>> coeffs(length);
  for (int i = 0; i < length; ++i) {
    coeffs[i] = coeffs_fn(i);
  }

  typedef function<vector<double>(vector<double>,
                                  const vector<array<double, 5>>&)> filter_fn;
  map<string, filter_fn> methods = {
      {"df2", &time_varying_df2},
      {"gr", &gold_rader},
      {"svf", &time_varying_svf},
      {"svf_r", &svf_rabenstein},
      {"svf_rc", &svf_rabenstein_czarnach},
      {"tdf2", &tdf2},
      {"tdf2_rc", &tdf2_rabenstein_czarnach},
      {"zz", &output_switching_filter},
      {"reference",
       [=](vector<double> x, const vector<array<double, 5>>& coeffs) {
        return reference(std::move(x), coeffs, w0,
                         static_cast<int>(sampling_rate * 0.01));
      }},
      {
       "anchor", [=](vector<double> x, const vector<array<double, 5>>& coeffs) {
                   return anchor(std::move(x), coeffs, w0);
                 },
      }};

  // Reference and anchor aren't needed for testing DC input.
  if (dc_input) {
    methods.erase("reference");
    methods.erase("anchor");
  }

  // Process with each filter structure.
  map<string, vector<double>> outputs;
  transform(begin(methods), end(methods), inserter(outputs, outputs.end()),
            [&](pair<string, filter_fn> cur_method) {
    return make_pair(cur_method.first, cur_method.second(input_signal, coeffs));
  });

  // Normalize peak levels.
  double max_level =
      accumulate(begin(outputs), end(outputs), 0.0,
                 [](double cur, const pair<string, vector<double>>& p) {
        double new_max_level =
            accumulate(begin(p.second), end(p.second), 0.0,
                       [](double cur, double x) { return max(cur, abs(x)); });
        return max(cur, new_max_level);
      });
  for (auto& p : outputs) {
    transform(begin(p.second), end(p.second), begin(p.second),
              [max_level](double x) { return x / max_level; });
  }

  // Apply fade-in and fade-out, so the signal starting and ending doesn't
  // distract from the filter transition.
  // This only gets done for sinusoidal input, since it's used for the
  // subjective tests. For the objective tests, the transition doesn't matter
  // because there's no listening component.
  if (!dc_input) {
    auto fade_samples = static_cast<int>(sampling_rate * 0.05 + 0.5);
    auto hann_window = generate_hann_window(2 * fade_samples);
    hann_window.resize(fade_samples);
    for (auto& p : outputs) {
      auto& xs = p.second;
      transform(begin(xs), begin(xs) + fade_samples, begin(hann_window),
                begin(xs), std::multiplies<double>());
      transform(end(xs) - fade_samples, end(xs), begin(hann_window),
                end(xs) - fade_samples,
                [](double x, double w) { return x * (1.0 - w); });
    }
  }

  // Write the output.
  const int channel_count = 2;
  for (auto& output : outputs) {
    auto output_path = output_dir + "/" + output.first + ".wav";
    write_wav(output_path.c_str(), sampling_rate, channel_count, output.second);
  }
}


#include <CGAL/Simple_cartesian.h>

#include <CGAL/Surface_mesh.h>

#include <CGAL/Surface_mesh_parameterization/IO/File_off.h>
#include <CGAL/Surface_mesh_parameterization/Circular_border_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Discrete_authalic_parameterizer_3.h>
#include <CGAL/Surface_mesh_parameterization/Error_code.h>
#include <CGAL/Surface_mesh_parameterization/parameterize.h>

#include <CGAL/Polygon_mesh_processing/measure.h>

#include <boost/foreach.hpp>

#include <cstdlib>
#include <iostream>
#include <fstream>

typedef CGAL::Simple_cartesian<double>       Kernel;
typedef Kernel::Point_2                      Point_2;
typedef Kernel::Point_3                      Point_3;
typedef CGAL::Surface_mesh<Kernel::Point_3>  SurfaceMesh;

typedef boost::graph_traits<SurfaceMesh>::halfedge_descriptor  halfedge_descriptor;
typedef boost::graph_traits<SurfaceMesh>::vertex_descriptor    vertex_descriptor;
typedef boost::graph_traits<SurfaceMesh>::face_descriptor      face_descriptor;

namespace SMP = CGAL::Surface_mesh_parameterization;

int main(int argc, char * argv[])
{
  std::ifstream in((argc>1) ? argv[1] : "data/three_peaks.off");
  if(!in) {
    std::cerr << "Problem loading the input data" << std::endl;
    return 1;
  }

  SurfaceMesh sm;
  in >> sm;

  // A halfedge on the border
  halfedge_descriptor bhd = CGAL::Polygon_mesh_processing::longest_border(sm).first;

  // The 2D points of the uv parametrisation will be written into this map
  typedef SurfaceMesh::Property_map<vertex_descriptor, Point_2>  UV_pmap;
  UV_pmap uv_map = sm.add_property_map<vertex_descriptor, Point_2>("v:uv").first;

  typedef SMP::Circular_border_arc_length_parameterizer_3<SurfaceMesh>  Border_parameterizer;
  typedef SMP::Discrete_authalic_parameterizer_3<SurfaceMesh, Border_parameterizer> Parameterizer;

  SMP::Error_code err = SMP::parameterize(sm, Parameterizer(), bhd, uv_map);

  if(err != SMP::OK) {
    std::cerr << "Error: " << SMP::get_error_message(err) << std::endl;
    return 1;
  }

  std::ofstream out("result.off");
  SMP::IO::output_uvmap_to_off(sm, bhd, uv_map, out);

  return 0;
}


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [roundings]
#include <boost/simd/arithmetic.hpp>
#include <boost/simd/pack.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft =  bs::pack <float, 8>;

int main()
{
  pack_ft p = {-1.1f, -1.5f, -1.6f, -2.1f, -2.5f, -2.6f,  1.1f, 1.5f };
  std::cout << " p =  " << p << std::endl
            <<  " -> bs::ceil(p) =       " << bs::ceil(p)      << std::endl
            <<  " -> bs::floor(p) =      " << bs::floor(p)     << std::endl
            <<  " -> bs::fix(p) =        " << bs::fix(p)       << std::endl
            <<  " -> bs::round(p) =      " << bs::round(p)     << std::endl
            <<  " -> bs::nearbyint(p) =  " << bs::nearbyint(p) << std::endl;
  return 0;
}
//! [roundings]


// Copyright (c) 2009 INRIA Sophia-Antipolis (France).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org).
// You can redistribute it and/or modify it under the terms of the GNU
// General Public License as published by the Free Software Foundation,
// either version 3 of the License, or (at your option) any later version.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// $URL$
// $Id$
//
//
// Author(s)     : Stéphane Tayeb
//
//******************************************************************************
// File Description :
// Outputs to out.mesh a mesh of implicit domains. These domains are defined
// by a vector of functions. Each n-uplet of sign of function values defines a
// subdomain.
//******************************************************************************

#include "debug.h"
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Mesh_triangulation_3.h>
#include <CGAL/Mesh_complex_3_in_triangulation_3.h>
#include <CGAL/Mesh_criteria_3.h>

#include <CGAL/Implicit_to_labeling_function_wrapper.h>
#include <CGAL/Labeled_mesh_domain_3.h>
#include <CGAL/make_mesh_3.h>
#include "../examples/Mesh_3/implicit_functions.h"

#include <CGAL/Mesh_3/Mesh_global_optimizer.h>

// IO
#include <CGAL/IO/File_medit.h>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

using namespace CGAL::parameters;

// Domain
struct K: public CGAL::Exact_predicates_inexact_constructions_kernel {};
typedef FT_to_point_function_wrapper<K::FT, K::Point_3> Function;
typedef CGAL::Implicit_multi_domain_to_labeling_function_wrapper<Function>
                                                        Function_wrapper;
typedef Function_wrapper::Function_vector Function_vector;
typedef CGAL::Labeled_mesh_domain_3<Function_wrapper, K> Mesh_domain;

// Triangulation
typedef CGAL::Mesh_triangulation_3<Mesh_domain>::type Tr;
typedef CGAL::Mesh_complex_3_in_triangulation_3<Tr> C3t3;

// Mesh Criteria
typedef CGAL::Mesh_criteria_3<Tr> Mesh_criteria;
typedef Mesh_criteria::Facet_criteria    Facet_criteria;
typedef Mesh_criteria::Cell_criteria     Cell_criteria;

template <typename T>
double set_arg(const std::string& param_name,
               const std::string& param_string,
               const po::variables_map& vm)
{
  T param_value(0);

  if ( vm.count(param_name) )
  {
    param_value = vm[param_name].as<T>();
    std::cout << param_string << ": " << param_value << "\n";
  }
  else
  {
    std::cout << param_string << " ignored.\n";
  }

  return param_value;
}

void set_function(Function_vector& v,
                  Function& f,
                  const std::string& function_name,
                  const po::variables_map& vm)
{
  if ( vm.count(function_name) )
  {
    v.push_back(f);
    std::cout << function_name << " ";
  }
}

int main(int argc, char* argv[])
{
  po::options_description generic("Generic options");
  generic.add_options() ("help", "Produce help message");

  po::options_description mesh("Mesh generation parameters");
  mesh.add_options()("facet_angle", po::value<double>(), "Set facet angle bound")
  ("facet_size", po::value<double>(), "Set facet size bound")
  ("facet_error", po::value<double>(), "Set facet approximation error bound")
  ("tet_shape", po::value<double>(), "Set tet radius-edge bound")
  ("tet_size", po::value<double>(), "Set tet size bound");

  po::options_description functions("Implicit functions");
  functions.add_options()("torus", "Mesh torus function")
  ("sphere", "Mesh sphere function")
  ("chair", "Mesh chair function")
  ("tanglecube", "Mesh tanglecube function")
  ("cube", "Mesh cube function")
  ("ellipsoid", "Mesh ellipsoid function")
  ("heart", "Mesh heart function")
  ("octic", "Mesh octic function");

  po::options_description desc("Options");
  desc.add_options()
  ("exude", po::value<double>(), "Exude mesh after refinement. arg is time_limit.")
  ("perturb", po::value<double>(), "Perturb (sliver removal) mesh after refinement. arg is time_limit.")
  ("lloyd", po::value<int>(), "Lloyd-smoothing after refinement. arg is max_iteration_nb")
  ("odt", po::value<int>(), "ODT-smoothing after refinement. arg is max_iteration_nb")
  ("convergence", po::value<double>()->default_value(0.02), "Convergence ratio for smoothing functions")
  ("min_displacement", po::value<double>()->default_value(0.01), "Minimal displacement ratio for smoothing functions (moves that are below that ratio will not be done)")
  ("time_limit", po::value<double>()->default_value(0), "Max time for smoothing functions")
  ("no_label_rebind", "Don't rebind cell labels in medit output")
  ("show_patches", "Show surface patches in medit output");

  po::options_description cmdline_options("Usage",1);
  cmdline_options.add(generic).add(mesh).add(functions).add(desc);

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
  po::notify(vm);

  if (vm.count("help") || argc < 2)
  {
    std::cout << cmdline_options << std::endl;
    return 1;
  }

  std::cout << "=========== Params ==========="<< std::endl;

  double facet_angle = set_arg<double>("facet_angle","Facet angle",vm);
  double facet_size = set_arg<double>("facet_size","Facet size",vm);
  double facet_error = set_arg<double>("facet_error","Facet approximation error",vm);

  double tet_shape = set_arg<double>("tet_shape","Tet shape (radius-edge)",vm);
  double tet_size = set_arg<double>("tet_size","Tet size",vm);

  // Define functions
  Function f1(&torus_function);
  Function f2(&sphere_function<3>);
  Function f3(&chair_function);
  Function f4(&tanglecube_function);
  Function f5(&cube_function);
  Function f6(&ellipsoid_function);
  Function f7(&heart_function);
  Function f8(&octic_function);

  Function_vector v;

  std::cout << "\nFunction(s): ";

  set_function(v,f1,"torus",vm);
  set_function(v,f2,"sphere",vm);
  set_function(v,f3,"chair",vm);
  set_function(v,f4,"tanglecube",vm);
  set_function(v,f5,"cube",vm);
  set_function(v,f6,"ellipsoid",vm);
  set_function(v,f7,"heart",vm);
  set_function(v,f8,"octic",vm);

  std::cout << "\n=============================="<< std::endl;
  std::cout << std::endl;

  if ( v.empty() )
  {
    std::cout << "No function set. Exit.\n";
    return 0;
  }

  // Domain (Warning: Sphere_3 constructor uses square radius !)
  Mesh_domain domain(v, K::Sphere_3(CGAL::ORIGIN, 7.*7.), 1e-8);

  // Set mesh criteria
  Facet_criteria facet_criteria(facet_angle, facet_size, facet_error); // angle, size, approximation
  Cell_criteria cell_criteria(tet_shape, tet_size); // radius-edge ratio, size
  Mesh_criteria criteria(facet_criteria, cell_criteria);

  // Mesh generation
  C3t3 c3t3 = CGAL::make_mesh_3<C3t3>(domain, criteria, no_exude(), no_perturb());

  // Odt
  if (  vm.count("odt") )
  {
    CGAL::odt_optimize_mesh_3(c3t3, domain,
                              max_iteration_number=vm["odt"].as<int>(),
                              convergence=vm["convergence"].as<double>(),
                              sliver_bound=vm["min_displacement"].as<double>(),
                              time_limit=vm["time_limit"].as<double>());
  }

  // Lloyd
  if ( vm.count("lloyd") )
  {
    CGAL::lloyd_optimize_mesh_3(c3t3, domain,
                                max_iteration_number=vm["lloyd"].as<int>(),
                                convergence=vm["convergence"].as<double>(),
                                sliver_bound=vm["min_displacement"].as<double>(),
                                time_limit=vm["time_limit"].as<double>());
  }

  // Perturbation
  if ( vm.count("perturb") )
  {
    CGAL::perturb_mesh_3(c3t3, domain, time_limit = vm["perturb"].as<double>() );
  }

  // Exudation
  if ( vm.count("exude") )
  {
    CGAL::exude_mesh_3(c3t3, time_limit = vm["exude"].as<double>());
  }

  double min_angle = 181.;
  for ( C3t3::Cell_iterator cit = c3t3.cells_begin() ;
       cit != c3t3.cells_end() ;
       ++cit )
  {
    min_angle = (std::min)(min_angle,
                           CGAL::to_double(CGAL::Mesh_3::minimum_dihedral_angle(c3t3.triangulation().tetrahedron(cit))));
  }

  std::cerr << "Min angle: " << min_angle << std::endl;

  // Output
  std::ofstream medit_file("out.mesh");
  CGAL::output_to_medit(medit_file, c3t3, !vm.count("no_label_rebind"), vm.count("show_patches"));

  return 0;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testScenario.cpp
 * @brief   Unit test Scenario class
 * @author  Frank Dellaert
 */

#include <gtsam/base/numericalDerivative.h>
#include <gtsam/navigation/Scenario.h>

#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include <cmath>

using namespace std;
using namespace gtsam;

static const double kDegree = M_PI / 180.0;

/* ************************************************************************* */
TEST(Scenario, Spin) {
  //  angular velocity 6 kDegree/sec
  const double w = 6 * kDegree;
  const Vector3 W(0, 0, w), V(0, 0, 0);
  const ConstantTwistScenario scenario(W, V);

  const double T = 10;
  EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
  EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
  EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));

  const Pose3 T10 = scenario.pose(T);
  EXPECT(assert_equal(Vector3(0, 0, 60 * kDegree), T10.rotation().xyz(), 1e-9));
  EXPECT(assert_equal(Point3(0, 0, 0), T10.translation(), 1e-9));
}

/* ************************************************************************* */
TEST(Scenario, Forward) {
  const double v = 2;  // m/s
  const Vector3 W(0, 0, 0), V(v, 0, 0);
  const ConstantTwistScenario scenario(W, V);

  const double T = 15;
  EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
  EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
  EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));

  const Pose3 T15 = scenario.pose(T);
  EXPECT(assert_equal(Vector3(0, 0, 0), T15.rotation().xyz(), 1e-9));
  EXPECT(assert_equal(Point3(30, 0, 0), T15.translation(), 1e-9));
}

/* ************************************************************************* */
TEST(Scenario, Circle) {
  // Forward velocity 2m/s, angular velocity 6 kDegree/sec around Z
  const double v = 2, w = 6 * kDegree;
  const Vector3 W(0, 0, w), V(v, 0, 0);
  const ConstantTwistScenario scenario(W, V);

  const double T = 15;
  EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
  EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
  EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));

  // R = v/w, so test if circle is of right size
  const double R = v / w;
  const Pose3 T15 = scenario.pose(T);
  EXPECT(assert_equal(Vector3(0, 0, 90 * kDegree), T15.rotation().xyz(), 1e-9));
  EXPECT(assert_equal(Point3(R, R, 0), T15.translation(), 1e-9));
}

/* ************************************************************************* */
TEST(Scenario, Loop) {
  // Forward velocity 2m/s
  // Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
  const double v = 2, w = 6 * kDegree;
  const Vector3 W(0, -w, 0), V(v, 0, 0);
  const ConstantTwistScenario scenario(W, V);

  const double T = 30;
  EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
  EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
  EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));

  // R = v/w, so test if loop crests at 2*R
  const double R = v / w;
  const Pose3 T30 = scenario.pose(30);
  EXPECT(assert_equal(Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9));
#ifdef GTSAM_USE_QUATERNIONS
  EXPECT(assert_equal(Vector3(-M_PI, 0, -M_PI), T30.rotation().xyz()));
#else
  EXPECT(assert_equal(Vector3(M_PI, 0, M_PI), T30.rotation().xyz()));
#endif
  EXPECT(assert_equal(Point3(0, 0, 2 * R), T30.translation(), 1e-9));
}

/* ************************************************************************* */
TEST(Scenario, LoopWithInitialPose) {
  // Forward velocity 2m/s
  // Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
  const double v = 2, w = 6 * kDegree;
  const Vector3 W(0, -w, 0), V(v, 0, 0);
  const Rot3 nRb0 = Rot3::Yaw(M_PI);
  const Pose3 nTb0(nRb0, Point3(1, 2, 3));
  const ConstantTwistScenario scenario(W, V, nTb0);

  const double T = 30;
  EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
  EXPECT(assert_equal(V, scenario.velocity_b(T), 1e-9));
  EXPECT(assert_equal(W.cross(V), scenario.acceleration_b(T), 1e-9));

  // R = v/w, so test if loop crests at 2*R
  const double R = v / w;
  const Pose3 T30 = scenario.pose(30);
  EXPECT(
      assert_equal(nRb0 * Rot3::Rodrigues(0, M_PI, 0), T30.rotation(), 1e-9));
  EXPECT(assert_equal(Point3(1, 2, 3 + 2 * R), T30.translation(), 1e-9));
}

/* ************************************************************************* */
TEST(Scenario, Accelerating) {
  // Set up body pointing towards y axis, and start at 10,20,0 with velocity
  // going in X. The body itself has Z axis pointing down
  const Rot3 nRb(Point3(0, 1, 0), Point3(1, 0, 0), Point3(0, 0, -1));
  const Point3 P0(10, 20, 0);
  const Vector3 V0(50, 0, 0);

  const double a = 0.2;  // m/s^2
  const Vector3 A(0, a, 0), W(0.1, 0.2, 0.3);
  const AcceleratingScenario scenario(nRb, P0, V0, A, W);

  const double T = 3;
  EXPECT(assert_equal(W, scenario.omega_b(T), 1e-9));
  EXPECT(assert_equal(Vector3(V0 + T * A), scenario.velocity_n(T), 1e-9));
  EXPECT(assert_equal(A, scenario.acceleration_n(T), 1e-9));

  {
    // Check acceleration in nav
    Matrix expected = numericalDerivative11<Vector3, double>(
        boost::bind(&Scenario::velocity_n, scenario, _1), T);
    EXPECT(assert_equal(Vector3(expected), scenario.acceleration_n(T), 1e-9));
  }

  const Pose3 T3 = scenario.pose(3);
  EXPECT(assert_equal(nRb.expmap(T * W), T3.rotation(), 1e-9));
  EXPECT(assert_equal(Point3(10 + T * 50, 20 + a * T * T / 2, 0),
                      T3.translation(), 1e-9));
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


/* Copyright (c) 2021, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#define BOOST_TEST_MODULE DataAssimilator

#include <DataAssimilator.hh>
#include <Geometry.hh>

#include <deal.II/fe/fe_q.h>
#include <deal.II/lac/la_parallel_vector.h>

#include "main.cc"

namespace adamantine
{
class DataAssimilatorTester
{
public:
  void test_constructor()
  {
    boost::property_tree::ptree database;

    // First checking the dealii default values
    DataAssimilator da0(database);

    double tol = 1.0e-12;
    BOOST_CHECK_SMALL(da0._solver_control.tolerance() - 1.0e-10, tol);
    BOOST_CHECK(da0._solver_control.max_steps() == 100);
    BOOST_CHECK(da0._additional_data.max_n_tmp_vectors == 30);

    // Now explicitly setting them
    database.put("solver.convergence_tolerance", 1.0e-6);
    database.put("solver.max_iterations", 25);
    database.put("solver.max_number_of_temp_vectors", 4);
    DataAssimilator da1(database);
    BOOST_CHECK_SMALL(da1._solver_control.tolerance() - 1.0e-6, tol);
    BOOST_CHECK(da1._solver_control.max_steps() == 25);
    BOOST_CHECK(da1._additional_data.max_n_tmp_vectors == 4);
  };

  void test_calc_kalman_gain()
  {
    // Create the DoF mapping
    MPI_Comm communicator = MPI_COMM_WORLD;

    boost::property_tree::ptree database;
    database.put("import_mesh", false);
    database.put("length", 1);
    database.put("length_divisions", 2);
    database.put("height", 1);
    database.put("height_divisions", 2);
    adamantine::Geometry<2> geometry(communicator, database);
    dealii::parallel::distributed::Triangulation<2> const &tria =
        geometry.get_triangulation();

    dealii::FE_Q<2> fe(1);
    dealii::DoFHandler<2> dof_handler(tria);
    dof_handler.distribute_dofs(fe);

    unsigned int sim_size = 5;
    unsigned int expt_size = 2;

    dealii::Vector<double> expt_vec(2);
    expt_vec(0) = 2.5;
    expt_vec(1) = 9.5;

    std::pair<std::vector<int>, std::vector<int>> indices_and_offsets;
    indices_and_offsets.first.resize(2);
    indices_and_offsets.second.resize(3); // Offset vector is one longer
    indices_and_offsets.first[0] = 1;
    indices_and_offsets.first[1] = 3;
    indices_and_offsets.second[0] = 0;
    indices_and_offsets.second[1] = 1;
    indices_and_offsets.second[2] = 2;

    boost::property_tree::ptree solver_settings_database;
    DataAssimilator da(solver_settings_database);
    da._sim_size = sim_size;
    da._expt_size = expt_size;
    da._num_ensemble_members = 3;
    da.update_dof_mapping<2>(dof_handler, indices_and_offsets);

    // Create the simulation data
    std::vector<dealii::LA::distributed::Vector<double>> data(3);
    data[0].reinit(5);
    data[0](0) = 1.0;
    data[0](1) = 3.0;
    data[0](2) = 6.0;
    data[0](3) = 9.0;
    data[0](4) = 11.0;
    data[1].reinit(5);
    data[1](0) = 1.5;
    data[1](1) = 3.2;
    data[1](2) = 6.3;
    data[1](3) = 9.7;
    data[1](4) = 11.9;
    data[2].reinit(5);
    data[2](0) = 1.1;
    data[2](1) = 3.1;
    data[2](2) = 6.1;
    data[2](3) = 9.1;
    data[2](4) = 11.1;

    // Build the sparse experimental covariance matrix
    dealii::SparsityPattern pattern(expt_size, expt_size, 1);
    pattern.add(0, 0);
    pattern.add(1, 1);
    pattern.compress();

    dealii::SparseMatrix<double> R(pattern);
    R.add(0, 0, 0.002);
    R.add(1, 1, 0.001);

    // Create the (perturbed) innovation
    std::vector<dealii::Vector<double>> perturbed_innovation(3);
    for (unsigned int sample = 0; sample < perturbed_innovation.size();
         ++sample)
    {
      perturbed_innovation[sample].reinit(expt_size);
      dealii::Vector<double> temp = da.calc_Hx(data[sample]);
      for (unsigned int i = 0; i < expt_size; ++i)
      {
        perturbed_innovation[sample][i] = expt_vec[i] - temp[i];
      }
    }

    perturbed_innovation[0][0] = perturbed_innovation[0][0] + 0.0008;
    perturbed_innovation[0][1] = perturbed_innovation[0][1] - 0.0005;
    perturbed_innovation[1][0] = perturbed_innovation[1][0] - 0.001;
    perturbed_innovation[1][1] = perturbed_innovation[1][1] + 0.0002;
    perturbed_innovation[2][0] = perturbed_innovation[2][0] + 0.0002;
    perturbed_innovation[2][1] = perturbed_innovation[2][1] - 0.0009;

    // Apply the Kalman gain
    std::vector<dealii::LA::distributed::Vector<double>> forecast_shift =
        da.apply_kalman_gain(data, R, perturbed_innovation);

    double tol = 1.0e-4;

    // Reference solution calculated using Python
    BOOST_CHECK_CLOSE(forecast_shift[0][0], 0.21352564, tol);
    BOOST_CHECK_CLOSE(forecast_shift[0][1], -0.14600986, tol);
    BOOST_CHECK_CLOSE(forecast_shift[0][2], -0.02616469, tol);
    BOOST_CHECK_CLOSE(forecast_shift[0][3], 0.45321598, tol);
    BOOST_CHECK_CLOSE(forecast_shift[0][4], 0.69290631, tol);
    BOOST_CHECK_CLOSE(forecast_shift[1][0], -0.27786325, tol);
    BOOST_CHECK_CLOSE(forecast_shift[1][1], -0.32946285, tol);
    BOOST_CHECK_CLOSE(forecast_shift[1][2], -0.31226298, tol);
    BOOST_CHECK_CLOSE(forecast_shift[1][3], -0.24346351, tol);
    BOOST_CHECK_CLOSE(forecast_shift[1][4], -0.20906377, tol);
    BOOST_CHECK_CLOSE(forecast_shift[2][0], 0.12767094, tol);
    BOOST_CHECK_CLOSE(forecast_shift[2][1], -0.20319395, tol);
    BOOST_CHECK_CLOSE(forecast_shift[2][2], -0.09290565, tol);
    BOOST_CHECK_CLOSE(forecast_shift[2][3], 0.34824753, tol);
    BOOST_CHECK_CLOSE(forecast_shift[2][4], 0.56882413, tol);
  };

  void test_update_dof_mapping()
  {
    MPI_Comm communicator = MPI_COMM_WORLD;

    boost::property_tree::ptree database;
    database.put("import_mesh", false);
    database.put("length", 1);
    database.put("length_divisions", 2);
    database.put("height", 1);
    database.put("height_divisions", 2);
    adamantine::Geometry<2> geometry(communicator, database);
    dealii::parallel::distributed::Triangulation<2> const &tria =
        geometry.get_triangulation();

    dealii::FE_Q<2> fe(1);
    dealii::DoFHandler<2> dof_handler(tria);
    dof_handler.distribute_dofs(fe);

    unsigned int sim_size = 4;
    unsigned int expt_size = 3;

    std::pair<std::vector<int>, std::vector<int>> indices_and_offsets;
    indices_and_offsets.first.resize(3);
    indices_and_offsets.second.resize(4); // offset vector is one longer
    indices_and_offsets.first[0] = 0;
    indices_and_offsets.first[1] = 1;
    indices_and_offsets.first[2] = 3;
    indices_and_offsets.second[0] = 0;
    indices_and_offsets.second[1] = 1;
    indices_and_offsets.second[2] = 2;
    indices_and_offsets.second[3] = 3;

    boost::property_tree::ptree solver_settings_database;
    DataAssimilator da(solver_settings_database);
    da._sim_size = sim_size;
    da._expt_size = expt_size;
    da.update_dof_mapping<2>(dof_handler, indices_and_offsets);

    BOOST_CHECK(da._expt_to_dof_mapping.first[0] == 0);
    BOOST_CHECK(da._expt_to_dof_mapping.first[1] == 1);
    BOOST_CHECK(da._expt_to_dof_mapping.first[2] == 2);
    BOOST_CHECK(da._expt_to_dof_mapping.second[0] == 0);
    BOOST_CHECK(da._expt_to_dof_mapping.second[1] == 1);
    BOOST_CHECK(da._expt_to_dof_mapping.second[2] == 3);
  };

  void test_calc_H()
  {
    MPI_Comm communicator = MPI_COMM_WORLD;

    boost::property_tree::ptree database;
    database.put("import_mesh", false);
    database.put("length", 1);
    database.put("length_divisions", 2);
    database.put("height", 1);
    database.put("height_divisions", 2);
    adamantine::Geometry<2> geometry(communicator, database);
    dealii::parallel::distributed::Triangulation<2> const &tria =
        geometry.get_triangulation();

    dealii::FE_Q<2> fe(1);
    dealii::DoFHandler<2> dof_handler(tria);
    dof_handler.distribute_dofs(fe);

    unsigned int sim_size = 4;
    unsigned int expt_size = 3;

    std::pair<std::vector<int>, std::vector<int>> indices_and_offsets;
    indices_and_offsets.first.resize(3);
    indices_and_offsets.second.resize(4); // offset vector is one longer
    indices_and_offsets.first[0] = 0;
    indices_and_offsets.first[1] = 1;
    indices_and_offsets.first[2] = 3;
    indices_and_offsets.second[0] = 0;
    indices_and_offsets.second[1] = 1;
    indices_and_offsets.second[2] = 2;
    indices_and_offsets.second[3] = 3;

    boost::property_tree::ptree solver_settings_database;
    DataAssimilator da(solver_settings_database);
    da._sim_size = sim_size;
    da._expt_size = expt_size;
    da.update_dof_mapping<2>(dof_handler, indices_and_offsets);

    dealii::SparsityPattern pattern(expt_size, sim_size, expt_size);

    dealii::SparseMatrix<double> H = da.calc_H(pattern);

    double tol = 1e-12;
    for (unsigned int i = 0; i < expt_size; ++i)
    {
      for (unsigned int j = 0; j < sim_size; ++j)
      {
        if (i == 0 && j == 0)
          BOOST_CHECK_CLOSE(H(i, j), 1.0, tol);
        else if (i == 1 && j == 1)
          BOOST_CHECK_CLOSE(H(i, j), 1.0, tol);
        else if (i == 2 && j == 3)
          BOOST_CHECK_CLOSE(H(i, j), 1.0, tol);
        else
          BOOST_CHECK_CLOSE(H.el(i, j), 0.0, tol);
      }
    }
  };
  void test_calc_Hx()
  {
    MPI_Comm communicator = MPI_COMM_WORLD;

    boost::property_tree::ptree database;
    database.put("import_mesh", false);
    database.put("length", 1);
    database.put("length_divisions", 2);
    database.put("height", 1);
    database.put("height_divisions", 2);
    adamantine::Geometry<2> geometry(communicator, database);
    dealii::parallel::distributed::Triangulation<2> const &tria =
        geometry.get_triangulation();

    dealii::FE_Q<2> fe(1);
    dealii::DoFHandler<2> dof_handler(tria);
    dof_handler.distribute_dofs(fe);

    int sim_size = 4;
    int expt_size = 3;

    dealii::LA::distributed::Vector<double> sim_vec(dof_handler.n_dofs());
    sim_vec(0) = 2.0;
    sim_vec(1) = 4.0;
    sim_vec(2) = 5.0;
    sim_vec(3) = 7.0;

    dealii::Vector<double> expt_vec(3);
    expt_vec(0) = 2.5;
    expt_vec(1) = 4.5;
    expt_vec(2) = 8.5;

    std::pair<std::vector<int>, std::vector<int>> indices_and_offsets;
    indices_and_offsets.first.resize(3);
    indices_and_offsets.second.resize(4); // Offset vector is one longer
    indices_and_offsets.first[0] = 0;
    indices_and_offsets.first[1] = 1;
    indices_and_offsets.first[2] = 3;
    indices_and_offsets.second[0] = 0;
    indices_and_offsets.second[1] = 1;
    indices_and_offsets.second[2] = 2;
    indices_and_offsets.second[3] = 3;

    boost::property_tree::ptree solver_settings_database;
    DataAssimilator da(solver_settings_database);
    da._sim_size = sim_size;
    da._expt_size = expt_size;
    da.update_dof_mapping<2>(dof_handler, indices_and_offsets);
    dealii::Vector<double> Hx = da.calc_Hx(sim_vec);

    double tol = 1e-10;
    BOOST_CHECK_CLOSE(Hx(0), 2.0, tol);
    BOOST_CHECK_CLOSE(Hx(1), 4.0, tol);
    BOOST_CHECK_CLOSE(Hx(2), 7.0, tol);
  };

  void test_calc_sample_covariance_dense()
  {
    double tol = 1e-10;

    // Trivial case of identical vectors, covariance should be the zero matrix
    std::vector<dealii::LA::distributed::Vector<double>> data1(3);
    data1[0].reinit(4);
    data1[0](0) = 1.0;
    data1[0](1) = 3.0;
    data1[0](2) = 6.0;
    data1[0](3) = 9.0;
    data1[1].reinit(4);
    data1[1](0) = 1.0;
    data1[1](1) = 3.0;
    data1[1](2) = 6.0;
    data1[1](3) = 9.0;
    data1[2].reinit(4);
    data1[2](0) = 1.0;
    data1[2](1) = 3.0;
    data1[2](2) = 6.0;
    data1[2](3) = 9.0;

    boost::property_tree::ptree solver_settings_database;
    DataAssimilator da(solver_settings_database);
    dealii::FullMatrix<double> cov = da.calc_sample_covariance_dense(data1);

    // Check results
    for (unsigned int i = 0; i < 4; ++i)
    {
      for (unsigned int j = 0; j < 4; ++j)
      {
        BOOST_CHECK_SMALL(std::abs(cov(i, j)), tol);
      }
    }

    // Non-trivial case, using NumPy solution as the reference
    std::vector<dealii::LA::distributed::Vector<double>> data2(3);
    data2[0].reinit(5);
    data2[0](0) = 1.0;
    data2[0](1) = 3.0;
    data2[0](2) = 6.0;
    data2[0](3) = 9.0;
    data2[0](4) = 11.0;
    data2[1].reinit(5);
    data2[1](0) = 1.5;
    data2[1](1) = 3.2;
    data2[1](2) = 6.3;
    data2[1](3) = 9.7;
    data2[1](4) = 11.9;
    data2[2].reinit(5);
    data2[2](0) = 1.1;
    data2[2](1) = 3.1;
    data2[2](2) = 6.1;
    data2[2](3) = 9.1;
    data2[2](4) = 11.1;

    da._sim_size = 5;
    dealii::FullMatrix<double> cov2 = da.calc_sample_covariance_dense(data2);

    BOOST_CHECK_CLOSE(cov2(0, 0), 0.07, tol);
    BOOST_CHECK_CLOSE(cov2(1, 0), 0.025, tol);
    BOOST_CHECK_CLOSE(cov2(2, 0), 0.04, tol);
    BOOST_CHECK_CLOSE(cov2(3, 0), 0.1, tol);
    BOOST_CHECK_CLOSE(cov2(4, 0), 0.13, tol);
    BOOST_CHECK_CLOSE(cov2(0, 1), 0.025, tol);
    BOOST_CHECK_CLOSE(cov2(1, 1), 0.01, tol);
    BOOST_CHECK_CLOSE(cov2(2, 1), 0.015, tol);
    BOOST_CHECK_CLOSE(cov2(3, 1), 0.035, tol);
    BOOST_CHECK_CLOSE(cov2(4, 1), 0.045, tol);
    BOOST_CHECK_CLOSE(cov2(0, 2), 0.04, tol);
    BOOST_CHECK_CLOSE(cov2(1, 2), 0.015, tol);
    BOOST_CHECK_CLOSE(cov2(2, 2), 0.02333333333333, tol);
    BOOST_CHECK_CLOSE(cov2(3, 2), 0.05666666666667, tol);
    BOOST_CHECK_CLOSE(cov2(4, 2), 0.07333333333333, tol);
    BOOST_CHECK_CLOSE(cov2(0, 3), 0.1, tol);
    BOOST_CHECK_CLOSE(cov2(1, 3), 0.035, tol);
    BOOST_CHECK_CLOSE(cov2(2, 3), 0.05666666666667, tol);
    BOOST_CHECK_CLOSE(cov2(3, 3), 0.14333333333333, tol);
    BOOST_CHECK_CLOSE(cov2(4, 3), 0.18666666666667, tol);
    BOOST_CHECK_CLOSE(cov2(0, 4), 0.13, tol);
    BOOST_CHECK_CLOSE(cov2(1, 4), 0.045, tol);
    BOOST_CHECK_CLOSE(cov2(2, 4), 0.07333333333333, tol);
    BOOST_CHECK_CLOSE(cov2(3, 4), 0.18666666666667, tol);
    BOOST_CHECK_CLOSE(cov2(4, 4), 0.24333333333333, tol);
  };

  void test_fill_noise_vector()
  {
    boost::property_tree::ptree solver_settings_database;
    DataAssimilator da(solver_settings_database);

    dealii::SparsityPattern pattern(3, 3, 3);
    pattern.add(0, 0);
    pattern.add(1, 0);
    pattern.add(1, 1);
    pattern.add(0, 1);
    pattern.add(2, 2);
    pattern.compress();

    dealii::SparseMatrix<double> R(pattern);

    R.add(0, 0, 0.1);
    R.add(1, 0, 0.3);
    R.add(1, 1, 1.0);
    R.add(0, 1, 0.3);
    R.add(2, 2, 0.2);

    std::vector<dealii::Vector<double>> data;
    dealii::Vector<double> ensemble_member(3);
    for (unsigned int i = 0; i < 1000; ++i)
    {
      da.fill_noise_vector(ensemble_member, R);
      data.push_back(ensemble_member);
    }

    dealii::FullMatrix<double> Rtest = da.calc_sample_covariance_dense(data);

    double tol = 20.; // Loose 20% tolerance because this is a statistical check
    BOOST_CHECK_CLOSE(R(0, 0), Rtest(0, 0), tol);
    BOOST_CHECK_CLOSE(R(1, 0), Rtest(1, 0), tol);
    BOOST_CHECK_CLOSE(R(1, 1), Rtest(1, 1), tol);
    BOOST_CHECK_CLOSE(R(0, 1), Rtest(0, 1), tol);
    BOOST_CHECK_CLOSE(R(2, 2), Rtest(2, 2), tol);
  };

  void test_update_ensemble()
  {
    // Create the DoF mapping
    MPI_Comm communicator = MPI_COMM_WORLD;

    boost::property_tree::ptree database;
    database.put("import_mesh", false);
    database.put("length", 1);
    database.put("length_divisions", 2);
    database.put("height", 1);
    database.put("height_divisions", 2);
    adamantine::Geometry<2> geometry(communicator, database);
    dealii::parallel::distributed::Triangulation<2> const &tria =
        geometry.get_triangulation();

    dealii::FE_Q<2> fe(1);
    dealii::DoFHandler<2> dof_handler(tria);
    dof_handler.distribute_dofs(fe);

    int sim_size = 5;
    int expt_size = 2;

    std::vector<double> expt_vec(2);
    expt_vec[0] = 2.5;
    expt_vec[1] = 9.5;

    std::pair<std::vector<int>, std::vector<int>> indices_and_offsets;
    indices_and_offsets.first.resize(2);
    indices_and_offsets.second.resize(3); // Offset vector is one longer
    indices_and_offsets.first[0] = 1;
    indices_and_offsets.first[1] = 3;
    indices_and_offsets.second[0] = 0;
    indices_and_offsets.second[1] = 1;
    indices_and_offsets.second[2] = 2;

    boost::property_tree::ptree solver_settings_database;
    DataAssimilator da(solver_settings_database);
    da._sim_size = sim_size;
    da._expt_size = expt_size;
    da._num_ensemble_members = 3;

    da.update_dof_mapping<2>(dof_handler, indices_and_offsets);

    // Create the simulation data
    std::vector<dealii::LA::distributed::Vector<double>> data(3);
    data[0].reinit(5);
    data[0](0) = 1.0;
    data[0](1) = 3.0;
    data[0](2) = 6.0;
    data[0](3) = 9.0;
    data[0](4) = 11.0;
    data[1].reinit(5);
    data[1](0) = 1.5;
    data[1](1) = 3.2;
    data[1](2) = 6.3;
    data[1](3) = 9.7;
    data[1](4) = 11.9;
    data[2].reinit(5);
    data[2](0) = 1.1;
    data[2](1) = 3.1;
    data[2](2) = 6.1;
    data[2](3) = 9.1;
    data[2](4) = 11.1;

    // Build the sparse experimental covariance matrix
    dealii::SparsityPattern pattern(expt_size, expt_size, 1);
    pattern.add(0, 0);
    pattern.add(1, 1);
    pattern.compress();

    dealii::SparseMatrix<double> R(pattern);
    R.add(0, 0, 0.002);
    R.add(1, 1, 0.001);

    // Save the data at the observation points before assimilation
    std::vector<double> sim_at_expt_pt_1_before(3);
    sim_at_expt_pt_1_before.push_back(data[0][1]);
    sim_at_expt_pt_1_before.push_back(data[1][1]);
    sim_at_expt_pt_1_before.push_back(data[2][1]);

    std::vector<double> sim_at_expt_pt_2_before(3);
    sim_at_expt_pt_2_before.push_back(data[0][3]);
    sim_at_expt_pt_2_before.push_back(data[1][3]);
    sim_at_expt_pt_2_before.push_back(data[2][3]);

    // Update the simulation data
    da.update_ensemble(data, expt_vec, R);

    // Save the data at the observation points after assimilation
    std::vector<double> sim_at_expt_pt_1_after(3);
    sim_at_expt_pt_1_after.push_back(data[0][1]);
    sim_at_expt_pt_1_after.push_back(data[1][1]);
    sim_at_expt_pt_1_after.push_back(data[2][1]);

    std::vector<double> sim_at_expt_pt_2_after(3);
    sim_at_expt_pt_2_after.push_back(data[0][3]);
    sim_at_expt_pt_2_after.push_back(data[1][3]);
    sim_at_expt_pt_2_after.push_back(data[2][3]);

    // Check the solution
    // The observed points should get closer to the experimental values
    // Large entries in R could make these fail spuriously
    for (int member = 0; member < 3; ++member)
    {
      BOOST_CHECK(std::abs(expt_vec[0] - sim_at_expt_pt_1_after[member]) <=
                  std::abs(expt_vec[0] - sim_at_expt_pt_1_before[member]));
      BOOST_CHECK(std::abs(expt_vec[1] - sim_at_expt_pt_2_after[member]) <=
                  std::abs(expt_vec[1] - sim_at_expt_pt_2_before[member]));
    }
  };
};

BOOST_AUTO_TEST_CASE(data_assimilator)
{
  DataAssimilatorTester dat;

  dat.test_constructor();
  dat.test_update_dof_mapping();
  dat.test_calc_sample_covariance_dense();
  dat.test_fill_noise_vector();
  dat.test_calc_H();
  dat.test_calc_Hx();
  dat.test_calc_kalman_gain();
  dat.test_update_ensemble();
}
} // namespace adamantine


//---------------------------------------------------------------------------//
// Copyright (c) 2014 Benoit Dequidt <benoit.dequidt@gmail.com>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <iostream>
#include <cstdlib>

#include <boost/compute/core.hpp>
#include <boost/compute/algorithm/copy.hpp>
#include <boost/compute/algorithm/inclusive_scan.hpp>
#include <boost/compute/container/vector.hpp>
#include <boost/compute/type_traits/type_name.hpp>
#include <boost/compute/utility/source.hpp>

namespace compute = boost::compute;

/// warning precision is not precise due
/// to the float error accumulation when size is large enough
/// for more precision use double
/// or a kahan sum else results can diverge
/// from the CPU implementation
compute::program make_sma_program(const compute::context &context)
{
    const char source[] = BOOST_COMPUTE_STRINGIZE_SOURCE(
        __kernel void SMA(__global const float *scannedValues, int size, __global float *output, int wSize) {
            const int gid = get_global_id(0);

            float cumValues = 0.f;
            int endIdx = gid + wSize / 2;
            int startIdx = gid - 1 - wSize / 2;

            if (endIdx > size - 1)
                endIdx = size - 1;

            cumValues += scannedValues[endIdx];
            if (startIdx < 0)
                startIdx = -1;
            else
                cumValues -= scannedValues[startIdx];

            output[gid] = (float)(cumValues / (float)(endIdx - startIdx));
        });

    // create sma program
    return compute::program::build_with_source(source, context);
}

bool check_results(const std::vector<float, mi_stl_allocator<float>> &values, const std::vector<float, mi_stl_allocator<float>> &smoothValues, unsigned int wSize)
{
    int size = values.size();
    if (size != (int)smoothValues.size())
        return false;

    int semiWidth = wSize / 2;

    bool ret = true;
    for (int idx = 0; idx < size; ++idx)
    {
        int start = (std::max)(idx - semiWidth, 0);
        int end = (std::min)(idx + semiWidth, size - 1);
        float res = 0;
        for (int j = start; j <= end; ++j)
        {
            res += values[j];
        }

        res /= float(end - start + 1);

        if (std::abs(res - smoothValues[idx]) > 1e-3)
        {
            std::cout << "idx = " << idx << " -- expected = " << res << " -- result = " << smoothValues[idx] << std::endl;
            ret = false;
        }
    }

    return ret;
}

// generate a uniform law over [0,10]
float myRand()
{
    static const double divisor = double(RAND_MAX) + 1.;
    return double(rand()) / divisor * 10.;
}

int main()
{
    unsigned int size = 1024;
    // wSize must be odd
    unsigned int wSize = 21;
    // get the default device
    compute::device device = compute::system::default_device();
    // create a context for the device
    compute::context context(device);
    // get the program
    compute::program program = make_sma_program(context);

    // create vector of random numbers on the host
    std::vector<float, mi_stl_allocator<float>> host_vector(size);
    std::vector<float, mi_stl_allocator<float>> host_result(size);
    std::generate(host_vector.begin(), host_vector.end(), myRand);

    compute::vector<float> a(size, context);
    compute::vector<float> b(size, context);
    compute::vector<float> c(size, context);
    compute::command_queue queue(context, device);

    compute::copy(host_vector.begin(), host_vector.end(), a.begin(), queue);

    // scan values
    compute::inclusive_scan(a.begin(), a.end(), b.begin(), queue);
    // sma kernel
    compute::kernel kernel(program, "SMA");
    kernel.set_arg(0, b.get_buffer());
    kernel.set_arg(1, (int)b.size());
    kernel.set_arg(2, c.get_buffer());
    kernel.set_arg(3, (int)wSize);

    using compute::uint_;
    uint_ tpb = 128;
    uint_ workSize = size;
    queue.enqueue_1d_range_kernel(kernel, 0, workSize, tpb);

    compute::copy(c.begin(), c.end(), host_result.begin(), queue);

    bool res = check_results(host_vector, host_result, wSize);
    std::string status = res ? "results are equivalent" : "GPU results differs from CPU one's";
    std::cout << status << std::endl;

    return 0;
}


//  Copyright (c) 2015 Boost.Test team
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE tolerance_06
#include <boost/test/included/unit_test.hpp>
namespace utf = boost::unit_test;
namespace tt = boost::test_tools;

BOOST_AUTO_TEST_CASE(test1, * utf::tolerance(0.1415 / 3)) // == 0.047166667
{
  double x = 3.141592404915836;
  // x is 'double' which is tolerance based, 3 is 'int' which is arithmetic:
  // tolerance based comparison will be used.
  // Type of tolerance for this comparison will be boost::common_type<double, int>::type == double
  // Value for this tolerance type is set by the decorator.
  BOOST_TEST(x == 3);
}
//]


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 1999 - 2015 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE at
 * the top level of the deal.II distribution.
 *
 * ---------------------------------------------------------------------
 *
 * based on deal.II step-2
 */

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_renumbering.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/manifold_lib.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/sparse_matrix.h>

#include <fstream>

using namespace dealii;

void
make_grid(Triangulation<2> &triangulation)
{
  const Point<2> center(1, 0);
  const double   inner_radius = 0.5, outer_radius = 1.0;
  GridGenerator::hyper_shell(
    triangulation, center, inner_radius, outer_radius, 5);

  static const SphericalManifold<2> manifold_description(center);
  triangulation.set_all_manifold_ids(0);
  triangulation.set_manifold(0, manifold_description);

  for (unsigned int step = 0; step < 3; ++step)
    {
      Triangulation<2>::active_cell_iterator cell =
                                               triangulation.begin_active(),
                                             endc = triangulation.end();

      for (; cell != endc; ++cell)
        for (unsigned int v = 0; v < GeometryInfo<2>::vertices_per_cell; ++v)
          {
            const double distance_from_center =
              center.distance(cell->vertex(v));

            if (std::fabs(distance_from_center - inner_radius) < 1e-10)
              {
                cell->set_refine_flag();
                break;
              }
          }

      triangulation.execute_coarsening_and_refinement();
    }
}

void
make_square_grid(Triangulation<2> &triangulation)
{
  GridGenerator::hyper_cube(triangulation);
  triangulation.refine_global(3);
}

SparsityPattern
distribute_dofs(DoFHandler<2> &dof_handler)
{
  static const FE_Q<2> finite_element(1);
  dof_handler.distribute_dofs(finite_element);

  DynamicSparsityPattern dynamic_sparsity_pattern(dof_handler.n_dofs(),
                                                  dof_handler.n_dofs());

  DoFTools::make_sparsity_pattern(dof_handler, dynamic_sparsity_pattern);

  SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from(dynamic_sparsity_pattern);

  std::ofstream out("sparsity_pattern1.svg");
  sparsity_pattern.print_svg(out);

  return sparsity_pattern;
}

SparsityPattern
renumber_dofs(DoFHandler<2> &dof_handler)
{
  DoFRenumbering::Cuthill_McKee(dof_handler);

  DynamicSparsityPattern dynamic_sparsity_pattern(dof_handler.n_dofs(),
                                                  dof_handler.n_dofs());
  DoFTools::make_sparsity_pattern(dof_handler, dynamic_sparsity_pattern);

  SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from(dynamic_sparsity_pattern);

  std::ofstream out("sparsity_pattern2.svg");
  sparsity_pattern.print_svg(out);

  return sparsity_pattern;
}

void
row_lenths(const SparsityPattern &sparsity_pattern)
{
  for (unsigned int i = 0; i < sparsity_pattern.n_rows(); i++)
    {
      std::cout << "Row" << i << "- - row length "
                << sparsity_pattern.row_length(i) << std::endl;
    }
}

std::tuple<int, int, double, double>
compute_pattern_statistics(const SparsityPattern &sparsity_pattern)
{
  double avarage_per_row = 0.;
  for (unsigned int i = 0; i < sparsity_pattern.n_rows(); i++)
    {
      avarage_per_row += sparsity_pattern.row_length(i);
    }

  double fill_ratio = avarage_per_row / (double)(sparsity_pattern.n_rows() *
                                                 sparsity_pattern.n_cols());
  avarage_per_row /= (double)sparsity_pattern.n_rows();
  return std::make_tuple(sparsity_pattern.n_rows(),
                         sparsity_pattern.bandwidth(),
                         avarage_per_row,
                         fill_ratio);
}

int
main()
{
  Triangulation<2> triangulation;
  // make_grid(triangulation);
  make_square_grid(triangulation);

  DoFHandler<2> dof_handler(triangulation);

  auto sparcity_pattern  = distribute_dofs(dof_handler);
  auto sparcity_pattern2 = renumber_dofs(dof_handler);

  row_lenths(sparcity_pattern2);

  std::cout << sparcity_pattern.row_length(41) << std::endl;

  auto patern_statistics = compute_pattern_statistics(sparcity_pattern2);
  std::cout << std::get<0>(patern_statistics) << " "
            << std::get<1>(patern_statistics) << "  "
            << std::get<2>(patern_statistics) << "  "
            << std::get<3>(patern_statistics) << std::endl;
}