#include <GL/glew.h>
#include "SplineWindow.h"
#include "dart/external/lodepng/lodepng.h"
#include "Functions.h"
#include <algorithm>
#include <fstream>
#include <boost/filesystem.hpp>
#include <GL/glut.h>
using namespace GUI;
using namespace dart::simulation;
using namespace dart::dynamics;

SplineWindow::
SplineWindow(std::string motion, std::string record, std::string record_type)
	:GLUTWindow(),mTrackCamera(false),mIsRotate(false),mIsAuto(false), mTimeStep(1 / 30.0), mDrawRef2(false)
{
	this->mTotalFrame = 0;

	std::string skel_path = std::string(CAR_DIR)+std::string("/character/") + std::string(REF_CHARACTER_TYPE) + std::string(".xml");

	this->mRef = new DPhy::Character(skel_path);
	this->mRef3 = new DPhy::Character(skel_path);

	DPhy::SetSkeletonColor(this->mRef->GetSkeleton(), Eigen::Vector4d(235./255., 235./255., 235./255., 1.0));
	DPhy::SetSkeletonColor(this->mRef3->GetSkeleton(), Eigen::Vector4d(87./255., 235./255., 87./255., 1.0));

	if(record_type.compare("position") == 0) {
		mDrawRef2 = true;
		this->mRef2 = new DPhy::Character(skel_path);

		DPhy::SetSkeletonColor(this->mRef2->GetSkeleton(), Eigen::Vector4d(235./255., 87./255., 87./255., 1.0));
	}

	int dof = this->mRef->GetSkeleton()->getPositions().rows();

	DPhy::ReferenceManager* referenceManager = new DPhy::ReferenceManager(this->mRef);
	referenceManager->LoadMotionFromBVH(std::string("/motion/") + motion);
	referenceManager->InitOptimization(1, "");

	std::vector<double> knots = referenceManager->GetKnots();

	std::vector<int> nc;
	nc.push_back(3);
	nc.push_back(5);

	DPhy::MultilevelSpline* s = new DPhy::MultilevelSpline(1, referenceManager->GetPhaseLength(), nc);
	s->SetKnots(0, knots);

	std::ifstream is(record);

	char buffer[256];

	int length = 0;
	double reward = 0;

	std::vector<Eigen::VectorXd> pos;
	std::vector<double> step;

	int n = 0;
	std::vector<Eigen::VectorXd> cps_mean;
	std::vector<Eigen::VectorXd> cps;

	for(int i = 0; i < knots.size() + 3 ; i++) {
		cps_mean.push_back(Eigen::VectorXd::Zero(dof));
	}

	Eigen::VectorXd targetBase = referenceManager->GetTargetBase();
	Eigen::VectorXd targetUnit = referenceManager->GetTargetUnit();
	Eigen::VectorXd targetIdx(targetBase.size());
	targetIdx(0) = 0;
	while(!is.eof()) {
		if(record_type.compare("spline") == 0) {
			// cps number
			is >> buffer;
			Eigen::VectorXd tp(targetBase.rows());
			Eigen::VectorXd idx(targetBase.rows());
			for(int j = 0; j < targetBase.rows(); j++)
			{
				is >> buffer;
				tp[j] = atof(buffer);
				idx[j] = std::floor((tp[j] - targetBase[j]) / targetUnit[j]);
			}
			// comma
			is >> buffer;

			Eigen::VectorXd cp(dof);
			for(int j = 0; j < dof; j++)
			{
				is >> buffer;
				cp[j] = atof(buffer);
			}
			// comma
			is >> buffer;
			// reward
			is >> buffer;

			cps.push_back(cp);

			if(cps.size() == knots.size() + 3) {
				s->SetControlPoints(0, cps);
				std::vector<Eigen::VectorXd> displacement = s->ConvertSplineToMotion();
				std::vector<Eigen::VectorXd> new_pos;
				referenceManager->AddDisplacementToBVH(displacement, new_pos);

				for(int i = 0; i < new_pos.size(); i++) {
					length += 1;
					mMemoryRef.push_back(new_pos[i]);
				}
				for(int i = 0; i < cps.size(); i++) {
					cps_mean[i] += cps[i];
				}

				n += 1;
				cps.clear();
			}
		} else if(record_type.compare("position") == 0) {
			Eigen::VectorXd p(dof);
			for(int j = 0; j < dof; j++)
			{
				is >> buffer;
				p[j] = atof(buffer);
			}

			is >> buffer;
			double cur_step = atof(buffer);
			is >> buffer;
			double cur_reward = atof(buffer);
			is >> buffer;
			double cur_reward2 = atof(buffer);
			if(reward == 0)
				reward = cur_reward;
			is >> buffer;

			// next phase
			if(cur_reward != reward) {
				std::cout << cur_reward << " " << cur_reward2 << std::endl;
				reward = cur_reward;

				std::vector<std::pair<Eigen::VectorXd,double>> displacement;
				pos = DPhy::Align(pos, referenceManager->GetPosition(std::fmod(step[0], referenceManager->GetPhaseLength())).segment<6>(0));
				std::vector<std::pair<Eigen::VectorXd,double>> trajectory;
				for(int i = 0; i < pos.size(); i++) {
					trajectory.push_back(std::pair<Eigen::VectorXd,double>(pos[i], step[i]));
				}
				referenceManager->GetDisplacementWithBVH(trajectory, displacement);
				if(mMemoryRef.size() == 0) {
					for(int i = 0; i < trajectory.size(); i++) {
						std::cout << trajectory[i].second << " "<< trajectory[i].first.segment<3>(3).transpose() << " "<< displacement[i].first.segment<3>(3).transpose() << std::endl;
					}
				}

				s->ConvertMotionToSpline(displacement);

				std::vector<Eigen::VectorXd> new_displacement = s->ConvertSplineToMotion();
				std::vector<Eigen::VectorXd> new_pos;
				referenceManager->AddDisplacementToBVH(new_displacement, new_pos);
				if(mMemoryRef.size() == 0) {
					for(int i = 0; i < new_displacement.size(); i++) {
						std::cout << i  << " "<< new_pos[i].segment<3>(3).transpose() << " "<< new_displacement[i].segment<3>(3).transpose() << std::endl;
					}
				}

				cps = s->GetControlPoints(0);
				for(int i = 0; i < cps.size(); i++) {
					cps_mean[i] += cps[i];
				}
				n += 1;

				int l = std::min(pos.size(), new_pos.size());
				for(int i = 0; i < l; i++) {
					length += 1;
					mMemoryRef.push_back(pos[i]);
					new_pos[i][3] += 1.5;
					mMemoryRef2.push_back(new_pos[i]);
				}
				pos.clear();
				step.clear();
			}

			pos.push_back(p);
			step.push_back(cur_step);
		}
	}
	is.close();

	for(int i = 0; i < cps_mean.size(); i++) {
		cps_mean[i] /= n;
	}
	s->SetControlPoints(0, cps_mean);
	std::vector<Eigen::VectorXd> displacement = s->ConvertSplineToMotion();
	std::vector<Eigen::VectorXd> new_pos;
	referenceManager->AddDisplacementToBVH(displacement, new_pos);
	for(int i = 0; i < referenceManager->GetPhaseLength(); i++) {
		new_pos[i][3] += 3;
	}
	for(int i = 0; i < n; i++) {
		for(int j = 0; j < referenceManager->GetPhaseLength(); j++) {
			mMemoryRef3.push_back(new_pos[j]);
		}
	}

	if(this->mTotalFrame == 0 || length < mTotalFrame) {
		mTotalFrame = length;
	}

	this->mCurFrame = 0;
	this->mDisplayTimeout = 33;

	this->SetFrame(this->mCurFrame);

}
void
SplineWindow::
SetFrame(int n)
{
	if( n < 0 || n >= this->mTotalFrame )
	{
	 	std::cout << "Frame exceeds limits" << std::endl;
	 	return;
	}

    mRef->GetSkeleton()->setPositions(mMemoryRef[n]);
    if(mDrawRef2)  {
    	mRef2->GetSkeleton()->setPositions(mMemoryRef2[n]);
    }
    mRef3->GetSkeleton()->setPositions(mMemoryRef3[n]);

}

void
SplineWindow::
NextFrame()
{
	this->mCurFrame+=1;
	if (this->mCurFrame >= this->mTotalFrame) {
        this->mCurFrame = 0;
    }
	this->SetFrame(this->mCurFrame);
}
void
SplineWindow::
PrevFrame()
{
	this->mCurFrame-=1;
	if( this->mCurFrame < 0 ) {
        this->mCurFrame = this->mTotalFrame - 1;
    }
	this->SetFrame(this->mCurFrame);
}
void
SplineWindow::
DrawSkeletons()
{
	GUI::DrawSkeleton(this->mRef->GetSkeleton(), 0);
	if(mDrawRef2) {
		GUI::DrawSkeleton(this->mRef2->GetSkeleton(), 0);
	}
	GUI::DrawSkeleton(this->mRef3->GetSkeleton(), 0);

}
void
SplineWindow::
DrawGround()
{
	Eigen::Vector3d com_root;
	com_root = this->mRef->GetSkeleton()->getRootBodyNode()->getCOM();
	GUI::DrawGround((int)com_root[0], (int)com_root[2], 0);
}
void
SplineWindow::
Display()
{

	glClearColor(1.0, 1.0, 1.0, 1);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glEnable(GL_DEPTH_TEST);

	dart::dynamics::SkeletonPtr skel = this->mRef->GetSkeleton();
	Eigen::Vector3d com_root = skel->getRootBodyNode()->getCOM();
	Eigen::Vector3d com_front = skel->getRootBodyNode()->getTransform()*Eigen::Vector3d(0.0, 0.0, 2.0);

	if(this->mTrackCamera){
		Eigen::Vector3d com = skel->getRootBodyNode()->getCOM();
		Eigen::Isometry3d transform = skel->getRootBodyNode()->getTransform();
		com[1] = 0.8;

		Eigen::Vector3d camera_pos;
		camera_pos << -3, 1, 1.5;
		camera_pos = camera_pos + com;
		camera_pos[1] = 2;

		mCamera->SetCenter(com);
	}
	mCamera->Apply();

	glUseProgram(program);
	glPushMatrix();
	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    glScalef(1.0, -1.0, 1.0);
	initLights(com_root[0], com_root[2], com_front[0], com_front[2]);
	// DrawSkeletons();
	glPopMatrix();
	initLights(com_root[0], com_root[2], com_front[0], com_front[2]);
	// glColor4f(0.7, 0.0, 0.0, 0.40);  /* 40% dark red floor color */
	DrawGround();
	DrawSkeletons();
	glDisable(GL_BLEND);

	glUseProgram(0);
	glutSwapBuffers();

}
void
SplineWindow::
Reset()
{
	this->mCurFrame = 0;
	this->SetFrame(this->mCurFrame);

}
void
SplineWindow::
Keyboard(unsigned char key,int x,int y)
{
	switch(key)
	{
		case '`' :mIsRotate= !mIsRotate;break;
		case '[': mIsAuto=false;this->PrevFrame();break;
		case ']': mIsAuto=false;this->NextFrame();break;
		case 'o': this->mCurFrame-=99; this->PrevFrame();break;
		case 'p': this->mCurFrame+=99; this->NextFrame();break;
		case 's': std::cout << this->mCurFrame << std::endl;break;
		case 'r': Reset();break;
		case 't': mTrackCamera = !mTrackCamera; this->SetFrame(this->mCurFrame); break;
		case ' ':
			mIsAuto = !mIsAuto;
			break;
		case 27: exit(0);break;
		default : break;
	}
	// this->SetFrame(this->mCurFrame);

	// glutPostRedisplay();
}
void
SplineWindow::
Mouse(int button, int state, int x, int y)
{
	if(button == 3 || button == 4){
		if (button == 3)
		{
			mCamera->Pan(0,-5,0,0);
		}
		else
		{
			mCamera->Pan(0,5,0,0);
		}
	}
	else{
		if (state == GLUT_DOWN)
		{
			mIsDrag = true;
			mMouseType = button;
			mPrevX = x;
			mPrevY = y;
		}
		else
		{
			mIsDrag = false;
			mMouseType = 0;
		}
	}

	// glutPostRedisplay();
}
void
SplineWindow::
Motion(int x, int y)
{
	if (!mIsDrag)
		return;

	int mod = glutGetModifiers();
	if (mMouseType == GLUT_LEFT_BUTTON)
	{
		mCamera->Translate(x,y,mPrevX,mPrevY);
	}
	else if (mMouseType == GLUT_RIGHT_BUTTON)
	{
		mCamera->Rotate(x,y,mPrevX,mPrevY);

	}
	mPrevX = x;
	mPrevY = y;
}
void
SplineWindow::
Reshape(int w, int h)
{
	glViewport(0, 0, w, h);
	mCamera->Apply();
}

void
SplineWindow::
Step()
{
	this->mCurFrame++;
	this->SetFrame(this->mCurFrame);
}
void
SplineWindow::
Timer(int value)
{
	std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();

	if( mIsAuto && this->mCurFrame == this->mTotalFrame - 1){
         Step();
	} else if( mIsAuto && this->mCurFrame < this->mTotalFrame - 1){
        this->mCurFrame++;
        SetFrame(this->mCurFrame);

    }

	std::chrono::steady_clock::time_point end= std::chrono::steady_clock::now();
	double elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count()/1000.;

	glutTimerFunc(std::max(0.0,mDisplayTimeout-elapsed), TimerEvent,1);
	glutPostRedisplay();

}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_NEGATE_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_NEGATE_HPP_INCLUDED
#include <boost/simd/toolbox/ieee/functions/negate.hpp>
#include <boost/simd/include/functions/simd/is_ltz.hpp>
#include <boost/simd/include/functions/simd/is_nez.hpp>
#include <boost/simd/include/functions/simd/is_nan.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/seladd.hpp>
#include <boost/simd/include/functions/simd/unary_minus.hpp>
#include <boost/simd/include/functions/simd/if_else_zero.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::negate_, tag::cpu_,
                         (A0)(X),
                         ((simd_<arithmetic_<A0>,X>))
                         ((simd_<arithmetic_<A0>,X>))
                        )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return  if_else(is_ltz(a1),-a0,if_else_zero(is_nez(a1), a0));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::negate_, tag::cpu_,
                         (A0)(X),
                         ((simd_<unsigned_<A0>,X>))
                         ((simd_<unsigned_<A0>,X>))
                        )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return if_else_zero(is_nez(a1), a0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::negate_, tag::cpu_,
                         (A0)(X),
                         ((simd_<floating_<A0>,X>))
                         ((simd_<floating_<A0>,X>))
                        )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      A0 tmp = if_else_zero(is_nez(a1), a0);
      tmp = if_else(is_ltz(a1), -a0, tmp);
      return select(is_nan(a1), a1, tmp); //TODO signed Nan ?
    }
  };
} } }
#endif


#include <ros/ros.h>
#include "imu_spatial/imu_spatial.h"

#include "std_msgs/String.h"

#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"
#include "sensor_msgs/Temperature.h"
#include "diagnostic_updater/diagnostic_updater.h"
#include "diagnostic_updater/DiagnosticStatusWrapper.h"
#include <tf/tf.h>
#include <tf/transform_datatypes.h>
#include <tf/LinearMath/Vector3.h>
#include <tf/LinearMath/Quaternion.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
/*#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>*/
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>

#include <fstream>
#include <boost/filesystem.hpp>

#include <jetsoncar_utils/jetsoncar_utils.h>

class IMUNode {
private:
    IMU imu;

    bool publish_tf_;
    bool include_standard_deviations_;
    bool enable_position_fusion_;
    bool enable_heading_fusion_;
    int odom_downsample_rate_;
    int odom_sample_count_{0};
    float odom_position_stddev_m_;
    float odom_heading_stddev_deg_;

    ros::Time imu_t0;
    IMU::Orientation est_orientation_;
    IMU::QuaternionStdDev est_quaternion_stddev_;
    IMU::StateEstimate est_acceleration_;
    IMU::StateEstimate est_angular_velocity_;
    IMU::StateEstimate est_body_velocity_;
    IMU::StateEstimate est_position_NED_;
    IMU::CombinedSensorsMeasurement raw_;

    ros::NodeHandle nh_;
    ros::NodeHandle nParam_;
    std::string world_frame_;
    std::string imu_frame_;
    std::string position_imu_frame_; // mainly for visualization

    // Publishers
    ros::Publisher pub_imu_;
    ros::Publisher pub_imu_pose_;
    ros::Publisher pub_imu_raw_;
    ros::Publisher pub_magnetic_field_;

    // Subscribers
    ros::Subscriber sub_odom_in_;

    tf::TransformBroadcaster tf_broadcaster_;
    tf::TransformListener tf_listener_;

    // Logging
    bool loggingEnabled{false};
    std::ofstream log_raw;
    std::ofstream log_estimate;
    std::ofstream log_angular_velocity;
    std::ofstream log_odom_pose;

private:
#if 0
    void publish_imu_msg() {
        sensor_msgs::Imu imu_msg;
        geometry_msgs::Quaternion quaternion_msg;
        float orientation_covariance[9] = {pow((quaternion_std_packet_.standard_deviation[0]), 2.0), 0, 0,
                                           0, pow((quaternion_std_packet_.standard_deviation[1]), 2.0), 0,
                                           0, 0, pow((quaternion_std_packet_.standard_deviation[2]), 2.0)};
        geometry_msgs::Vector3 angular_velocity;

        geometry_msgs::Vector3 linear_acceleration;

        imu_msg.header.stamp = ros::Time::now();
        imu_msg.header.frame_id = frame_id_;

        imu_msg.orientation.w = quaternion_packet_.orientation[0];
        imu_msg.orientation.x = quaternion_packet_.orientation[1];
        imu_msg.orientation.y = quaternion_packet_.orientation[2];
        imu_msg.orientation.z = quaternion_packet_.orientation[3];
        imu_msg.orientation_covariance[0] = orientation_covariance[0];
        imu_msg.orientation_covariance[4] = orientation_covariance[4];
        imu_msg.orientation_covariance[8] = orientation_covariance[8];

        imu_msg.angular_velocity.x = angular_velocity_packet_.angular_velocity[0];
        imu_msg.angular_velocity.y = angular_velocity_packet_.angular_velocity[1];
        imu_msg.angular_velocity.z = angular_velocity_packet_.angular_velocity[2];
        imu_msg.angular_velocity_covariance[0] = -1;

        imu_msg.linear_acceleration.x = acceleration_packet_.acceleration[0];
        imu_msg.linear_acceleration.y = acceleration_packet_.acceleration[1];
        imu_msg.linear_acceleration.z = acceleration_packet_.acceleration[2];
        imu_msg.linear_acceleration_covariance[0] = -1;

        imu_pub_.publish(imu_msg);
    }
    void publish_imu_orienation_tf() {
        tf::Quaternion q_attitude;
        q_attitude.setW(quaternion_packet_.orientation[0]);
        q_attitude.setX(quaternion_packet_.orientation[2]);
        q_attitude.setY(quaternion_packet_.orientation[1]);
        q_attitude.setZ(-quaternion_packet_.orientation[3]);

        tf::Transform attitudeTf;
        attitudeTf.setIdentity();
        attitudeTf.setRotation(q_attitude);

        /* Send transfrom from "heading" frame to "base_link" frame */
        geometry_msgs::TransformStamped tf_tilt_msg;
        tf_tilt_msg.header.frame_id = "heading";
        tf_tilt_msg.child_frame_id = "base_link";
        tf_tilt_msg.header.stamp = ros::Time::now();
        tf_tilt_msg.transform.translation.x = 0;
        tf_tilt_msg.transform.translation.y = 0;
        tf_tilt_msg.transform.translation.z = 0.0;
        tf_tilt_msg.transform.rotation.w = q_attitude.w();
        tf_tilt_msg.transform.rotation.x = q_attitude.x();
        tf_tilt_msg.transform.rotation.y = q_attitude.y();
        tf_tilt_msg.transform.rotation.z = q_attitude.z();
        tf_broadcaster_.sendTransform(tf_tilt_msg);

        /* Send odometry message */
        //   This represents an estimate of a position and velocity in free space.
        //   The pose in this message should be specified in the coordinate frame given by header.frame_id
        //   The twist in this message should be specified in the coordinate frame given by the child_frame_id
        nav_msgs::Odometry odom_msg;
        odom_msg.header.stamp = ros::Time::now();
        odom_msg.header.frame_id = "odom";
        odom_msg.child_frame_id = "imu";
        odom_msg.pose.pose.position.x = 0; // inertial frame position
        odom_msg.pose.pose.position.y = 0;
        odom_msg.pose.pose.position.z = 0;
        odom_msg.pose.pose.orientation.w = q_attitude.w();
        odom_msg.pose.pose.orientation.x = q_attitude.x();
        odom_msg.pose.pose.orientation.y = q_attitude.y();
        odom_msg.pose.pose.orientation.z = q_attitude.z();
        odom_msg.twist.twist.linear.x = 0; // body frame velocity
        odom_msg.twist.twist.linear.y = 0;
        odom_msg.twist.twist.linear.z = 0;
        odom_msg.twist.twist.angular.x = angular_velocity_packet_.angular_velocity[0];
        odom_msg.twist.twist.angular.y = angular_velocity_packet_.angular_velocity[1];
        odom_msg.twist.twist.angular.z = angular_velocity_packet_.angular_velocity[2];
        pub_odom_.publish(odom_msg);
    }
    void publish_imu_raw_msg() {
        sensor_msgs::Imu imu_msg;
        geometry_msgs::Vector3 angular_velocity;
        geometry_msgs::Vector3 linear_acceleration;

        imu_msg.header.stamp = ros::Time::now();
        imu_msg.header.frame_id = frame_id_;

        imu_msg.orientation_covariance[0] = -1;

        imu_msg.angular_velocity.x = raw_sensors_packet_.gyroscopes[0];
        imu_msg.angular_velocity.y = raw_sensors_packet_.gyroscopes[1];
        imu_msg.angular_velocity.z = raw_sensors_packet_.gyroscopes[2];
        imu_msg.angular_velocity_covariance[0] = -1;

        imu_msg.linear_acceleration.x = raw_sensors_packet_.accelerometers[0];
        imu_msg.linear_acceleration.y = raw_sensors_packet_.accelerometers[1];
        imu_msg.linear_acceleration.z = raw_sensors_packet_.accelerometers[2];
        imu_msg.linear_acceleration_covariance[0] = -1;

        imu_raw_pub_.publish(imu_msg);
    }
    void publish_magnetics_msg() {
        sensor_msgs::MagneticField magnetic_field_msg;

        magnetic_field_msg.header.stamp = ros::Time::now();
        magnetic_field_msg.header.frame_id = frame_id_;

        magnetic_field_msg.magnetic_field.x = raw_sensors_packet_.magnetometers[0] * (1.0e-7);
        magnetic_field_msg.magnetic_field.y = raw_sensors_packet_.magnetometers[1] * (1.0e-7);
        magnetic_field_msg.magnetic_field.z = raw_sensors_packet_.magnetometers[2] * (1.0e-7);

        magnetic_field_pub_.publish(magnetic_field_msg);
    }
#endif

    void RawSensorsUpdate(IMU::CombinedSensorsMeasurement meas)
    {
        auto time = imu_t0 + ros::Duration(meas.timestamp);

        if (log_raw.is_open()) {
            log_raw << time.toNSec() << "\t";
            log_raw << std::setprecision(10) << meas.timestamp << "\t";
            log_raw << std::setprecision(10) << meas.accelerometer[0] << "\t";
            log_raw << std::setprecision(10) << meas.accelerometer[1] << "\t";
            log_raw << std::setprecision(10) << meas.accelerometer[2] << "\t";
            log_raw << std::setprecision(10) << meas.gyroscope[0] << "\t";
            log_raw << std::setprecision(10) << meas.gyroscope[1] << "\t";
            log_raw << std::setprecision(10) << meas.gyroscope[2] << "\t";
            log_raw << std::setprecision(10) << meas.magnetometer[0] << "\t";
            log_raw << std::setprecision(10) << meas.magnetometer[1] << "\t";
            log_raw << std::setprecision(10) << meas.magnetometer[2];
            log_raw << std::endl;
        }

        raw_ = meas;

        // Publish IMU message
        sensor_msgs::Imu imu_msg;
        imu_msg.header.stamp = imu_t0 + ros::Duration(meas.timestamp);
        imu_msg.header.frame_id = imu_frame_;

        imu_msg.orientation_covariance[0] = -1;

        imu_msg.angular_velocity.x = meas.gyroscope[0];
        imu_msg.angular_velocity.y = meas.gyroscope[1];
        imu_msg.angular_velocity.z = meas.gyroscope[2];
        imu_msg.angular_velocity_covariance[0] = -1;

        imu_msg.linear_acceleration.x = meas.accelerometer[0];
        imu_msg.linear_acceleration.y = meas.accelerometer[1];
        imu_msg.linear_acceleration.z = meas.accelerometer[2];
        imu_msg.linear_acceleration_covariance[0] = -1;

        pub_imu_raw_.publish(imu_msg);

        // Publish magnetometer message
        sensor_msgs::MagneticField magnetic_field_msg;

        magnetic_field_msg.header.stamp = ros::Time::now();
        magnetic_field_msg.header.frame_id = imu_frame_;

        magnetic_field_msg.magnetic_field.x = meas.magnetometer[0] * (1.0e-7); // milliGauss to Tesla
        magnetic_field_msg.magnetic_field.y = meas.magnetometer[1] * (1.0e-7);
        magnetic_field_msg.magnetic_field.z = meas.magnetometer[2] * (1.0e-7);

        pub_magnetic_field_.publish(magnetic_field_msg);
    }

    void PublishEstimateIfReady()
    {
        if (est_orientation_.timestamp > 0 &&
           (est_orientation_.timestamp == est_quaternion_stddev_.timestamp || !include_standard_deviations_) &&
            est_orientation_.timestamp == est_angular_velocity_.timestamp &&
           (est_orientation_.timestamp == est_body_velocity_.timestamp || !enable_position_fusion_) &&
           (est_orientation_.timestamp == est_position_NED_.timestamp || !enable_position_fusion_))
        {
            auto time = imu_t0 + ros::Duration(est_orientation_.timestamp);

            tf::Quaternion q_imu;
            q_imu.setX(est_orientation_.quaternion[0]);
            q_imu.setY(est_orientation_.quaternion[1]);
            q_imu.setZ(est_orientation_.quaternion[2]);
            q_imu.setW(est_orientation_.quaternion[3]);

            if (fabs(q_imu.length() - 1.0) > 0.01) {
                ROS_WARN_STREAM("[IMU] Internal estimator not ready yet");
                return; // error
            }

            tf::Quaternion q_flip({1,0,0}, deg2rad(180));
            tf::Quaternion q_orientation = q_flip * q_imu; // apply this flip to convert the quaternion estimate from a NED frame into the world NWU frame

            auto rpy = IMU::GetPoseEulerRPY(est_orientation_.quaternion, true);
            ROS_DEBUG_STREAM("RPY = " << rpy[0] << ", " << rpy[1] << ", " << rpy[2]);

            /* Send transfrom from "world" frame to "imu" frame */
            if (publish_tf_) {
                geometry_msgs::TransformStamped tf_imu_msg;
                tf_imu_msg.header.frame_id = world_frame_;
                tf_imu_msg.child_frame_id = imu_frame_;
                tf_imu_msg.header.stamp = time;
                tf_imu_msg.transform.translation.x = 0;
                tf_imu_msg.transform.translation.y = 0;
                tf_imu_msg.transform.translation.z = 0;
                tf_imu_msg.transform.rotation.x = q_orientation.x();
                tf_imu_msg.transform.rotation.y = q_orientation.y();
                tf_imu_msg.transform.rotation.z = q_orientation.z();
                tf_imu_msg.transform.rotation.w = q_orientation.w();
                tf_broadcaster_.sendTransform(tf_imu_msg);
            }

            // Publish IMU estimates
            sensor_msgs::Imu imu_msg;
            geometry_msgs::Quaternion quaternion_msg;
            // Factor of 2 since the quaternion elements are = sin(1/2*angle)
            imu_msg.header.stamp = time;
            imu_msg.header.frame_id = imu_frame_;

            imu_msg.orientation.x = q_orientation.x();
            imu_msg.orientation.y = q_orientation.y();
            imu_msg.orientation.z = q_orientation.z();
            imu_msg.orientation.w = q_orientation.w();
            imu_msg.orientation_covariance[0] = -1;

            if (include_standard_deviations_) {
                double orientation_covariance[9] = {pow((2.0 * est_quaternion_stddev_.stddev[0]), 2.0), 0, 0,
                                                    0, pow((2.0 * est_quaternion_stddev_.stddev[1]), 2.0), 0,
                                                    0, 0, pow((2.0 * est_quaternion_stddev_.stddev[2]), 2.0)};
                imu_msg.orientation_covariance[0] = orientation_covariance[0];
                imu_msg.orientation_covariance[4] = orientation_covariance[4];
                imu_msg.orientation_covariance[8] = orientation_covariance[8];
            }

            imu_msg.angular_velocity.x = est_angular_velocity_.estimate[0];
            imu_msg.angular_velocity.y = est_angular_velocity_.estimate[1];
            imu_msg.angular_velocity.z = est_angular_velocity_.estimate[2];
            imu_msg.angular_velocity_covariance[0] = -1;

            // est_acceleration_.estimate doesn't contain any acceleration if the internal position estimator within the IMU isn't running
            /*
            imu_msg.linear_acceleration.x = est_acceleration_.estimate[0];
            imu_msg.linear_acceleration.y = est_acceleration_.estimate[1];
            imu_msg.linear_acceleration.z = est_acceleration_.estimate[2];
            */
            // Instead we correct the accelerometer measurement with the orientation quaternion and report that as the linear acceleration
            /*auto accelerometer = tf::Vector3(raw_.accelerometer[0], raw_.accelerometer[1], raw_.accelerometer[2]);
            auto acceleration = accelerometer - tf::quatRotate(q_imu.inverse(), {0,0,-gravity_});
            imu_msg.linear_acceleration.x = acceleration.x();
            imu_msg.linear_acceleration.y = acceleration.y();
            imu_msg.linear_acceleration.z = acceleration.z();*/
            imu_msg.linear_acceleration_covariance[0] = -1;

            pub_imu_.publish(imu_msg);

            // Send position and velocity estimates
            if (enable_position_fusion_) {
                geometry_msgs::TransformStamped tf_imu_msg;
                tf_imu_msg.header.frame_id = world_frame_;
                tf_imu_msg.child_frame_id = position_imu_frame_;
                tf_imu_msg.header.stamp = ros::Time::now();
                tf_imu_msg.transform.translation.x = est_position_NED_.estimate[0]; // From NED to ENU
                tf_imu_msg.transform.translation.y = -est_position_NED_.estimate[1];
                tf_imu_msg.transform.translation.z = -est_position_NED_.estimate[2];
                tf_imu_msg.transform.rotation.x = q_orientation.x();
                tf_imu_msg.transform.rotation.y = q_orientation.y();
                tf_imu_msg.transform.rotation.z = q_orientation.z();
                tf_imu_msg.transform.rotation.w = q_orientation.w();
                tf_broadcaster_.sendTransform(tf_imu_msg);

                /* Send IMU Pose odometry message */
                //   This represents an estimate of a position and velocity in free space.
                //   The pose in this message should be specified in the coordinate frame given by header.frame_id
                //   The twist in this message should be specified in the coordinate frame given by the child_frame_id
                nav_msgs::Odometry odom_msg;
                odom_msg.header.stamp = ros::Time::now();
                odom_msg.header.frame_id = world_frame_;
                odom_msg.child_frame_id = position_imu_frame_;
                odom_msg.pose.pose.position.x = est_position_NED_.estimate[0]; // From NED to ENU
                odom_msg.pose.pose.position.y = -est_position_NED_.estimate[1];
                odom_msg.pose.pose.position.z = -est_position_NED_.estimate[2];
                odom_msg.pose.pose.orientation.w = q_orientation.w();
                odom_msg.pose.pose.orientation.x = q_orientation.x();
                odom_msg.pose.pose.orientation.y = q_orientation.y();
                odom_msg.pose.pose.orientation.z = q_orientation.z();
                odom_msg.twist.twist.linear.x = est_body_velocity_.estimate[0]; // body frame velocity
                odom_msg.twist.twist.linear.y = est_body_velocity_.estimate[1]; // OBS! Needs to be converted
                odom_msg.twist.twist.linear.z = est_body_velocity_.estimate[2];
                odom_msg.twist.twist.angular.x = est_angular_velocity_.estimate[0]; // these are already body angular velocities
                odom_msg.twist.twist.angular.y = est_angular_velocity_.estimate[1];
                odom_msg.twist.twist.angular.z = est_angular_velocity_.estimate[2];
                pub_imu_pose_.publish(odom_msg);
            }

            // Invalidate stored estimates
            est_orientation_.timestamp = 0;
            est_quaternion_stddev_.timestamp = 0;
            est_angular_velocity_.timestamp = 0;
            est_body_velocity_.timestamp = 0;
            est_position_NED_.timestamp = 0;
        }
    }

    void OrientationUpdate(IMU::Orientation orientation)
    {
        auto time = imu_t0 + ros::Duration(orientation.timestamp);

        if (log_estimate.is_open()) {
            log_estimate << time.toNSec() << "\t";
            log_estimate << std::setprecision(10) << orientation.timestamp << "\t";
            log_estimate << std::setprecision(10) << orientation.quaternion[3] << "\t"; // w
            log_estimate << std::setprecision(10) << orientation.quaternion[0] << "\t"; // x
            log_estimate << std::setprecision(10) << orientation.quaternion[1] << "\t"; // y
            log_estimate << std::setprecision(10) << orientation.quaternion[2];         // z
            log_estimate << std::endl;
        }

        est_orientation_ = orientation;
        PublishEstimateIfReady();
    }

    void QuaternionStdDevUpdate(IMU::QuaternionStdDev stddev)
    {
        est_quaternion_stddev_ = stddev;
        PublishEstimateIfReady();
    }

    void AccelerationUpdate(IMU::StateEstimate estimate)
    {
        est_acceleration_ = estimate;
        PublishEstimateIfReady();
    }

    void AngularVelocityUpdate(IMU::StateEstimate estimate)
    {
        auto time = imu_t0 + ros::Duration(estimate.timestamp);

        if (log_angular_velocity.is_open()) {
            log_angular_velocity << time.toNSec() << "\t";
            log_angular_velocity << std::setprecision(10) << estimate.timestamp << "\t";
            log_angular_velocity << std::setprecision(10) << estimate.estimate[0] << "\t";
            log_angular_velocity << std::setprecision(10) << estimate.estimate[1] << "\t";
            log_angular_velocity << std::setprecision(10) << estimate.estimate[2];
            log_angular_velocity << std::endl;
        }

        est_angular_velocity_ = estimate;
        PublishEstimateIfReady();
    }

    void BodyVelocityUpdate(IMU::StateEstimate estimate)
    {
        est_body_velocity_ = estimate;
        PublishEstimateIfReady();
    }

    void PositionNEDUpdate(IMU::StateEstimate estimate)
    {
        est_position_NED_ = estimate;
        PublishEstimateIfReady();
    }

    void odom_in_callback(const nav_msgs::Odometry::ConstPtr& msg)
    {
        if (log_odom_pose.is_open()) {
            log_odom_pose << msg->header.stamp.toNSec() << "\t";
            log_odom_pose << std::setprecision(10) << msg->pose.pose.orientation.w << "\t";
            log_odom_pose << std::setprecision(10) << msg->pose.pose.orientation.x << "\t";
            log_odom_pose << std::setprecision(10) << msg->pose.pose.orientation.y << "\t";
            log_odom_pose << std::setprecision(10) << msg->pose.pose.orientation.z << "\t";
            log_odom_pose << std::setprecision(10) << msg->pose.pose.position.x << "\t";
            log_odom_pose << std::setprecision(10) << msg->pose.pose.position.y << "\t";
            log_odom_pose << std::setprecision(10) << msg->pose.pose.position.z << "\t";
            log_odom_pose << std::setprecision(10) << msg->twist.twist.linear.x << "\t";
            log_odom_pose << std::setprecision(10) << msg->twist.twist.linear.y << "\t";
            log_odom_pose << std::setprecision(10) << msg->twist.twist.linear.z << "\t";
            log_odom_pose << std::setprecision(10) << msg->twist.twist.angular.x << "\t";
            log_odom_pose << std::setprecision(10) << msg->twist.twist.angular.y << "\t";
            log_odom_pose << std::setprecision(10) << msg->twist.twist.angular.z;
            log_odom_pose << std::endl;
        }

        odom_sample_count_++;
        if (odom_sample_count_ % odom_downsample_rate_ != 0) return;

        tf::Pose W_T_realsense;
        tf::poseMsgToTF(msg->pose.pose, W_T_realsense);

        tf::StampedTransform imu_T_realsense;
        try {
            tf_listener_.lookupTransform(imu_frame_, msg->child_frame_id, msg->header.stamp, imu_T_realsense);

            // Note: The part below could also have been made smarter simply by looking up the imu_frame position in the world frame and reporting that
            auto imu_orientation = imu_T_realsense.getRotation() * W_T_realsense.getRotation();

            auto realsense_position_NED = tf::Vector3(W_T_realsense.getOrigin().x(),
                                                      -W_T_realsense.getOrigin().y(),
                                                      -W_T_realsense.getOrigin().z());

            auto W_T_imu = W_T_realsense * imu_T_realsense.inverse();

            auto imu_position_NED = tf::Vector3(W_T_imu.getOrigin().x(),
                                               -W_T_imu.getOrigin().y(),
                                               -W_T_imu.getOrigin().z());

            // Compute heading from received Realsense orientation quaternion
            tf::Vector3 xVector = tf::quatRotate(imu_orientation, tf::Vector3(1,0,0));
            float heading = atan2(xVector.y(), xVector.x());

            imu.SetHeading(heading, deg2rad(odom_heading_stddev_deg_));
            imu.SetPositionNED(imu_position_NED.x(), imu_position_NED.y(), imu_position_NED.z(), odom_position_stddev_m_);

            ROS_DEBUG("X,Y,Z - Heading = %f, %f, %f - %f", imu_position_NED.x(), imu_position_NED.y(), imu_position_NED.z(), rad2deg(heading));
        }
        catch (tf::TransformException ex){
            ROS_ERROR("%s", ex.what());
        }
    }

    void OpenLogFiles()
    {
        // Prepare logs folder
        if (!boost::filesystem::is_directory(boost::filesystem::path(std::string(getenv("HOME")) + "/logs"))) {
            if (boost::filesystem::exists(boost::filesystem::path(std::string(getenv("HOME")) + "/logs"))) {
                printf("Log path (~/logs) already exists but without write permissions\n");
                return;
            } else {
                if (!boost::filesystem::create_directory(boost::filesystem::path(std::string(getenv("HOME")) + "/logs"))) {
                    printf("Could not create Log folder (~/logs)\n");
                    return;
                }
                else
                    printf("Successfully created log folder (~/logs)\n");
            }
        }

        std::string logTimestamp = utils::GetLogFormattedTimestamp(utils::utime());
        log_raw.open(std::string(getenv("HOME")) + "/logs/" + logTimestamp + "_imu_raw.txt", std::ofstream::trunc);
        if (!log_raw.is_open()) {
            std::cout << "Could not create " << logTimestamp + "_imu_accel.txt" << std::endl;
        }

        log_estimate.open(std::string(getenv("HOME")) + "/logs/" + logTimestamp + "_imu_estimate.txt", std::ofstream::trunc);
        if (!log_estimate.is_open()) {
            std::cout << "Could not create " << logTimestamp + "_imu_estimate.txt" << std::endl;
        }

        log_angular_velocity.open(std::string(getenv("HOME")) + "/logs/" + logTimestamp + "_imu_angular_velocity.txt", std::ofstream::trunc);
        if (!log_angular_velocity.is_open()) {
            std::cout << "Could not create " << logTimestamp + "_imu_angular_velocity.txt" << std::endl;
        }

        log_odom_pose.open(std::string(getenv("HOME")) + "/logs/" + logTimestamp + "_odom_pose.txt", std::ofstream::trunc);
        if (!log_odom_pose.is_open()) {
            std::cout << "Could not create " << logTimestamp + "_odom_pose.txt" << std::endl;
        }

        loggingEnabled = true;
    }

    void CloseLogFiles()
    {
        log_raw.close();
        log_estimate.close();
        log_odom_pose.close();

        loggingEnabled = false;
    }

public:
    IMUNode(ros::NodeHandle nh, ros::NodeHandle nParam, std::string world_frame, std::string imu_frame, std::string position_imu_frame,  bool enable_position_fusion, bool enable_heading_fusion, float odom_position_stddev_m, float odom_heading_stddev_deg, bool publish_tf, std::string log_path, int odom_downsample_rate = 1)
            : include_standard_deviations_{false}
            , enable_position_fusion_{enable_position_fusion}
            , enable_heading_fusion_{enable_heading_fusion}
            , odom_downsample_rate_{odom_downsample_rate}
            , odom_position_stddev_m_{odom_position_stddev_m}
            , odom_heading_stddev_deg_{odom_heading_stddev_deg}
            , nh_(nh)
            , nParam_(nParam)
            , world_frame_{world_frame}
            , imu_frame_{imu_frame}
            , position_imu_frame_{position_imu_frame}
            , publish_tf_{publish_tf}
            , pub_imu_{nh.advertise<sensor_msgs::Imu>("imu/est", 10)}
            , pub_imu_pose_{nh.advertise<nav_msgs::Odometry>("imu/pose", 10)}
            , pub_imu_raw_{nh.advertise<sensor_msgs::Imu>("imu/raw", 10)}
            , pub_magnetic_field_{nh.advertise<sensor_msgs::MagneticField>("imu/mag", 10)}
            , sub_odom_in_{nh.subscribe("odom_in", 50, &IMUNode::odom_in_callback, this)}
    {
        imu.RegisterCallback_RawSensors(std::bind(&IMUNode::RawSensorsUpdate, this, std::placeholders::_1));
        imu.RegisterCallback_Orientation(std::bind(&IMUNode::OrientationUpdate, this, std::placeholders::_1));
        imu.RegisterCallback_AngularVelocity(std::bind(&IMUNode::AngularVelocityUpdate, this, std::placeholders::_1));

        if (enable_position_fusion_)
            imu.RegisterCallback_Acceleration(std::bind(&IMUNode::AccelerationUpdate, this, std::placeholders::_1));

        imu.LoadLog(log_path);
    }
    IMUNode(ros::NodeHandle nh, ros::NodeHandle nParam, std::string world_frame, std::string imu_frame, std::string position_imu_frame, bool enable_position_fusion, bool enable_heading_fusion, float odom_position_stddev_m, float odom_heading_stddev_deg, bool publish_tf, int raw_data_rate = 200, int estimate_data_rate = 100, int odom_downsample_rate = 1, bool include_standard_deviations = false, std::string port_name = "/dev/ttyUSB0", uint32_t baud_rate = 460800)
            : imu(port_name, baud_rate)
            , include_standard_deviations_{include_standard_deviations}
            , enable_position_fusion_{enable_position_fusion}
            , enable_heading_fusion_{enable_heading_fusion}
            , odom_downsample_rate_{odom_downsample_rate}
            , odom_position_stddev_m_{odom_position_stddev_m}
            , odom_heading_stddev_deg_{odom_heading_stddev_deg}
            , nh_(nh)
            , nParam_(nParam)
            , world_frame_{world_frame}
            , imu_frame_{imu_frame}
            , position_imu_frame_{position_imu_frame}
            , publish_tf_{publish_tf}
            , pub_imu_{nh.advertise<sensor_msgs::Imu>("imu/est", 10)}
            , pub_imu_pose_{nh.advertise<nav_msgs::Odometry>("imu/pose", 10)}
            , pub_imu_raw_{nh.advertise<sensor_msgs::Imu>("imu/raw", 10)}
            , pub_magnetic_field_{nh.advertise<sensor_msgs::MagneticField>("imu/mag", 10)}
            , sub_odom_in_{nh.subscribe("imu/odom_in", 50, &IMUNode::odom_in_callback, this)} // odom_in
    {
        if (raw_data_rate < 0) raw_data_rate = 200;
        if (estimate_data_rate < 0) estimate_data_rate = 200;

        imu.Connect();
        imu.Configure(IMU::OutputType::RawAndIndividualEstimates, raw_data_rate, estimate_data_rate, enable_heading_fusion_, enable_position_fusion_, include_standard_deviations_);
        imu.SynchronizeTime(); // synchronize time again manually
        imu_t0 = ros::Time().now();

        imu.RegisterCallback_RawSensors(std::bind(&IMUNode::RawSensorsUpdate, this, std::placeholders::_1));
        imu.RegisterCallback_Orientation(std::bind(&IMUNode::OrientationUpdate, this, std::placeholders::_1));
        imu.RegisterCallback_AngularVelocity(std::bind(&IMUNode::AngularVelocityUpdate, this, std::placeholders::_1));

        if (include_standard_deviations_)
            imu.RegisterCallback_QuaternionStdDev(std::bind(&IMUNode::QuaternionStdDevUpdate, this, std::placeholders::_1));

        if (enable_position_fusion_) {
            imu.RegisterCallback_Acceleration(std::bind(&IMUNode::AccelerationUpdate, this, std::placeholders::_1));
            imu.RegisterCallback_Velocity(std::bind(&IMUNode::BodyVelocityUpdate, this, std::placeholders::_1));
            imu.RegisterCallback_PositionNED(std::bind(&IMUNode::PositionNEDUpdate, this, std::placeholders::_1));
        }
    }

    ~IMUNode()
    {
        CloseLogFiles();
    }

    void StartLogging()
    {
        if (loggingEnabled) return;
        OpenLogFiles();
        imu.RecordANPPLog();
    }

    void StopLogging()
    {
        if (!loggingEnabled) return;
        CloseLogFiles();
        imu.StopANPPLog();
    }

};

int main(int argc, char **argv) {
    ros::init(argc, argv, "imu_node");
    ros::NodeHandle nh;
    ros::NodeHandle nParam("~"); // default/current namespace node handle

    int raw_data_rate = 100; // Hz
    if (!nParam.getParam("raw_data_rate", raw_data_rate)) {
        ROS_WARN_STREAM("IMU Raw data rate not set (Parameter: raw_data_rate). Defaults to: " << raw_data_rate);
    }

    int estimate_data_rate = 100; // Hz
    if (!nParam.getParam("estimate_data_rate", estimate_data_rate)) {
        ROS_WARN_STREAM("IMU Estimate data rate not set (Parameter: estimate_data_rate). Defaults to: " << estimate_data_rate);
    }

    bool publish_tf = true;
    if (!nParam.getParam("publish_tf", publish_tf)) {
        ROS_WARN_STREAM("Publish Transform not set (Parameter: publish_tf). Defaults to: " << publish_tf);
    }

    std::string world_frame = "world";
    if (!nParam.getParam("world_frame", world_frame)) {
        ROS_WARN_STREAM("World frame not set (Parameter: world_frame). Defaults to: " << world_frame);
    }

    std::string imu_frame = "imu_link";
    if (!nParam.getParam("imu_frame", imu_frame)) {
        ROS_WARN_STREAM("IMU frame not set (Parameter: imu_frame). Defaults to: " << imu_frame);
    }

    std::string position_imu_frame = "imu";
    if (!nParam.getParam("position_imu_frame", position_imu_frame)) {
        ROS_WARN_STREAM("IMU publish frame not set (Parameter: position_imu_frame). Defaults to: " << position_imu_frame);
    }

    bool enable_position_fusion = false;
    if (!nParam.getParam("enable_position_fusion", enable_position_fusion)) {
        ROS_WARN_STREAM("IMU Position fusion flag not set (Parameter: enable_position_fusion). Defaults to: " << enable_position_fusion);
    }

    bool enable_heading_fusion = false;
    if (!nParam.getParam("enable_heading_fusion", enable_heading_fusion)) {
        ROS_WARN_STREAM("IMU Heading fusion flag not set (Parameter: enable_heading_fusion). Defaults to: " << enable_heading_fusion);
    }

    int odom_downsample_rate = 10; // number of samples to skip + 1
    if (!nParam.getParam("odom_downsample_rate", odom_downsample_rate)) {
        ROS_WARN_STREAM("IMU Odometry downsample rate not set (Parameter: odom_downsample_rate). Defaults to: " << odom_downsample_rate);
    }

    float odom_position_stddev_m = 0.1; // m
    if (!nParam.getParam("odom_position_stddev_m", odom_position_stddev_m)) {
        ROS_WARN_STREAM("IMU Odometry position standard deviation not set (Parameter: odom_position_stddev_m). Defaults to: " << odom_position_stddev_m);
    }

    float odom_heading_stddev_deg = 1; // deg
    if (!nParam.getParam("odom_heading_stddev_deg", odom_heading_stddev_deg)) {
        ROS_WARN_STREAM("IMU Odometry heading standard deviation not set (Parameter: odom_heading_stddev_deg). Defaults to: " << odom_heading_stddev_deg);
    }

    try {
        IMUNode node(nh, nParam, world_frame, imu_frame, position_imu_frame, enable_position_fusion, enable_heading_fusion, odom_position_stddev_m, odom_heading_stddev_deg, publish_tf, raw_data_rate, estimate_data_rate, odom_downsample_rate);
        //IMUNode node(nh, nParam, world_frame, imu_frame, position_imu_frame, enable_position_fusion, enable_heading_fusion, odom_position_stddev_m, odom_heading_stddev_deg, publish_tf, "SpatialLog_20-06-17_15-56-07.anpp");
        node.StartLogging();
        ros::spin();
    } catch(std::exception& e){
        ROS_FATAL_STREAM("Exception thrown: " << e.what());
    }
}


#include <stan/math/rev.hpp>
#include <boost/mp11.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/rev/functor/test_fixture_ode.hpp>
#include <test/unit/math/rev/functor/test_fixture_dae_index_3.hpp>
#include <test/unit/math/rev/functor/dae_test_functors.hpp>

/**
 *
 * Use same solver functor type for both w & w/o tolerance control
 */
template <typename solve_type, typename... Ts>
using ode_test_tuple = std::tuple<solve_type, solve_type, Ts...>;

/**
 * Outer product of test types
 */
using dae_test_types = boost::mp11::mp_product<
    ode_test_tuple, ::testing::Types<dae_functor>,
    ::testing::Types<double>,                                  // t
    ::testing::Types<double, stan::math::var_value<double> >,  // yy0
    ::testing::Types<double, stan::math::var_value<double> >,  // yp0
    ::testing::Types<double, stan::math::var_value<double> >   // theta
    >;

TYPED_TEST_SUITE_P(index_3_dae_test);
TYPED_TEST_P(index_3_dae_test, solver_failure) {
  EXPECT_THROW_MSG(this->apply_solver(), std::runtime_error,
                   "Error test failures occurred too many times");
  this->ts = {0.0001};
  EXPECT_THROW_MSG(this->apply_solver_tol(), std::runtime_error,
                   "Error test failures occurred too many times");
}

REGISTER_TYPED_TEST_SUITE_P(index_3_dae_test, solver_failure);
INSTANTIATE_TYPED_TEST_SUITE_P(StanDAE, index_3_dae_test, dae_test_types);


// -*- lsst-c++ -*-

/*
 * LSST Data Management System
 * Copyright 2008, 2009, 2010 LSST Corporation.
 *
 * This product includes software developed by the
 * LSST Project (http://www.lsst.org/).
 *
 * This program is free software: 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.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the LSST License Statement and
 * the GNU General Public License along with this program.  If not,
 * see <http://www.lsstcorp.org/LegalNotices/>.
 */
#include "lsst/afw/geom/ellipses/BaseCore.h"
#include "lsst/afw/geom/ellipses/Quadrupole.h"
#include "lsst/afw/geom/ellipses/Axes.h"
#include "lsst/geom/Angle.h"
#include <boost/format.hpp>
#include <map>

namespace lsst {
namespace afw {
namespace geom {
namespace ellipses {

namespace {

typedef std::map<std::string, std::shared_ptr<BaseCore> > RegistryMap;

RegistryMap& getRegistry() {
    static RegistryMap instance;
    return instance;
}

std::shared_ptr<BaseCore> getRegistryCopy(std::string const& name) {
    RegistryMap::iterator i = getRegistry().find(name);
    if (i == getRegistry().end()) {
        throw LSST_EXCEPT(lsst::pex::exceptions::InvalidParameterError,
                          (boost::format("Ellipse core with name '%s' not found in registry.") % name).str());
    }
    return i->second->clone();
}

}  // namespace

std::shared_ptr<BaseCore> BaseCore::make(std::string const& name) {
    std::shared_ptr<BaseCore> result = getRegistryCopy(name);
    *result = Quadrupole();
    return result;
}

std::shared_ptr<BaseCore> BaseCore::make(std::string const& name, ParameterVector const& parameters) {
    std::shared_ptr<BaseCore> result = getRegistryCopy(name);
    result->setParameterVector(parameters);
    return result;
}

std::shared_ptr<BaseCore> BaseCore::make(std::string const& name, double v1, double v2, double v3) {
    std::shared_ptr<BaseCore> result = getRegistryCopy(name);
    result->setParameterVector(ParameterVector(v1, v2, v3));
    return result;
}

std::shared_ptr<BaseCore> BaseCore::make(std::string const& name, BaseCore const& other) {
    std::shared_ptr<BaseCore> result = getRegistryCopy(name);
    *result = other;
    return result;
}

std::shared_ptr<BaseCore> BaseCore::make(std::string const& name, Transformer const& other) {
    std::shared_ptr<BaseCore> result = getRegistryCopy(name);
    other.apply(*result);
    return result;
}

std::shared_ptr<BaseCore> BaseCore::make(std::string const& name, Convolution const& other) {
    std::shared_ptr<BaseCore> result = getRegistryCopy(name);
    other.apply(*result);
    return result;
}

void BaseCore::registerSubclass(std::shared_ptr<BaseCore> const& example) {
    getRegistry()[example->getName()] = example;
}

void BaseCore::grow(double buffer) {
    double a, b, theta;
    _assignToAxes(a, b, theta);
    a += buffer;
    b += buffer;
    _assignFromAxes(a, b, theta);
}

void BaseCore::scale(double factor) {
    double a, b, theta;
    _assignToAxes(a, b, theta);
    a *= factor;
    b *= factor;
    _assignFromAxes(a, b, theta);
}

double BaseCore::getArea() const {
    double a, b, theta;
    _assignToAxes(a, b, theta);
    return a * b * lsst::geom::PI;
}

double BaseCore::getDeterminantRadius() const {
    double a, b, theta;
    _assignToAxes(a, b, theta);
    return std::sqrt(a * b);
}

double BaseCore::getTraceRadius() const {
    double ixx, iyy, ixy;
    _assignToQuadrupole(ixx, iyy, ixy);
    return std::sqrt(0.5 * (ixx + iyy));
}

lsst::geom::Extent2D BaseCore::computeDimensions() const {
    double a, b, theta;
    _assignToAxes(a, b, theta);
    double c = std::cos(theta);
    double s = std::sin(theta);
    c *= c;
    s *= s;
    b *= b;
    a *= a;
    lsst::geom::Extent2D dimensions(std::sqrt(b * s + a * c), std::sqrt(a * s + b * c));
    dimensions *= 2;
    return dimensions;
}

BaseCore::ParameterVector const BaseCore::getParameterVector() const {
    ParameterVector r;
    writeParameters(r.data());
    return r;
}

void BaseCore::setParameterVector(ParameterVector const& p) { readParameters(p.data()); }

bool BaseCore::operator==(BaseCore const& other) const {
    return getParameterVector() == other.getParameterVector() && getName() == other.getName();
}

BaseCore& BaseCore::operator=(BaseCore const& other) {
    if (&other != this) {
        // We use Axes instead of Quadrupole here because it allows us to copy Axes without
        // implicitly normalizing them.
        double a, b, theta;
        other._assignToAxes(a, b, theta);
        _assignFromAxes(a, b, theta);
    }
    return *this;
}
// Delegate to copy-constructor for backward-compatibility
BaseCore& BaseCore::operator=(BaseCore&& other) { return *this = other; }

BaseCore::Jacobian BaseCore::dAssign(BaseCore const& other) {
    if (getName() == other.getName()) {
        this->operator=(other);
        return Jacobian::Identity();
    }
    // We use Quadrupole instead of Axes here because the ambiguity of the position angle
    // in the circular case causes some of the Jacobians to/from Axes to be undefined for
    // exact circles.  Quadrupoles don't have that problem, and the Axes-to-Axes case is
    // handled by the above if block.
    double ixx, iyy, ixy;
    Jacobian rhs = other._dAssignToQuadrupole(ixx, iyy, ixy);
    Jacobian lhs = _dAssignFromQuadrupole(ixx, iyy, ixy);
    return lhs * rhs;
}

void BaseCore::_assignQuadrupoleToAxes(double ixx, double iyy, double ixy, double& a, double& b,
                                       double& theta) {
    double xx_p_yy = ixx + iyy;
    double xx_m_yy = ixx - iyy;
    double t = std::sqrt(xx_m_yy * xx_m_yy + 4 * ixy * ixy);
    a = std::sqrt(0.5 * (xx_p_yy + t));
    b = std::sqrt(0.5 * (xx_p_yy - t));
    theta = 0.5 * std::atan2(2.0 * ixy, xx_m_yy);
}

BaseCore::Jacobian BaseCore::_dAssignQuadrupoleToAxes(double ixx, double iyy, double ixy, double& a,
                                                      double& b, double& theta) {
    double xx_p_yy = ixx + iyy;
    double xx_m_yy = ixx - iyy;
    double t2 = xx_m_yy * xx_m_yy + 4.0 * ixy * ixy;
    Eigen::Vector3d dt2(2.0 * xx_m_yy, -2.0 * xx_m_yy, 8.0 * ixy);
    double t = std::sqrt(t2);
    a = std::sqrt(0.5 * (xx_p_yy + t));
    b = std::sqrt(0.5 * (xx_p_yy - t));
    theta = 0.5 * std::atan2(2.0 * ixy, xx_m_yy);
    Jacobian m = Jacobian::Zero();
    m(0, 0) = 0.25 * (1.0 + 0.5 * dt2[0] / t) / a;
    m(0, 1) = 0.25 * (1.0 + 0.5 * dt2[1] / t) / a;
    m(0, 2) = 0.25 * (0.5 * dt2[2] / t) / a;
    m(1, 0) = 0.25 * (1.0 - 0.5 * dt2[0] / t) / b;
    m(1, 1) = 0.25 * (1.0 - 0.5 * dt2[1] / t) / b;
    m(1, 2) = 0.25 * (-0.5 * dt2[2] / t) / b;

    m.row(2).setConstant(1.0 / (t * t));
    m(2, 0) *= -ixy;
    m(2, 1) *= ixy;
    m(2, 2) *= xx_m_yy;
    return m;
}

void BaseCore::_assignAxesToQuadrupole(double a, double b, double theta, double& ixx, double& iyy,
                                       double& ixy) {
    a *= a;
    b *= b;
    double c = std::cos(theta);
    double s = std::sin(theta);
    ixy = (a - b) * c * s;
    c *= c;
    s *= s;
    ixx = c * a + s * b;
    iyy = s * a + c * b;
}

BaseCore::Jacobian BaseCore::_dAssignAxesToQuadrupole(double a, double b, double theta, double& ixx,
                                                      double& iyy, double& ixy) {
    Jacobian m;
    m.col(0).setConstant(2 * a);
    m.col(1).setConstant(2 * b);
    a *= a;
    b *= b;
    m.col(2).setConstant(a - b);
    double c = std::cos(theta);
    double s = std::sin(theta);
    double cs = c * s;
    ixy = (a - b) * c * s;
    c *= c;
    s *= s;
    ixx = c * a + s * b;
    iyy = s * a + c * b;
    m(0, 0) *= c;
    m(0, 1) *= s;
    m(0, 2) *= -2.0 * cs;
    m(1, 0) *= s;
    m(1, 1) *= c;
    m(1, 2) *= 2.0 * cs;
    m(2, 0) *= cs;
    m(2, 1) *= -cs;
    m(2, 2) *= (c - s);
    return m;
}
}  // namespace ellipses
}  // namespace geom
}  // namespace afw
}  // namespace lsst


/*
 * Copyright 2020 Robert Bosch GmbH
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * SPDX-License-Identifier: Apache-2.0
 */
/**
 * \file osi_omni_sensor.hpp
 * \see  osi_omni_sensor.cpp
 */

#pragma once

#include <map>      // for map<>
#include <memory>   // for shared_ptr<>, unique_ptr<>
#include <string>   // for string
#include <utility>  // for move

#include <Eigen/Geometry>  // for Isometry3d, Vector3d

#include <cloe/component/lane_boundary.hpp>  // for LaneBoundary
#include <cloe/component/object.hpp>         // for Object
#include <cloe/core.hpp>                     // for Json, Duration
#include <cloe/simulator.hpp>                // for ModelError
#include <cloe/sync.hpp>                     // for Sync

#include "osi_common.pb.h"           // for Timestamp, Identifier, BaseMoving, ..
#include "osi_detectedobject.pb.h"   // for DetectedMovingObject
#include "osi_hostvehicledata.pb.h"  // for HostVehicleData
#include "osi_object.pb.h"           // for MovingObject
#include "osi_sensordata.pb.h"       // for SensorData, DetectedEntityHeader

#include "osi_ground_truth.hpp"  // for OsiGroundTruth
#include "osi_transceiver.hpp"   // for OsiTransceiver
#include "osi_utils.hpp"

namespace osii {

/**
 * Convert OSI timestamp to Cloe time format.
 */
cloe::Duration osi_timestamp_to_time(const osi3::Timestamp& timestamp);

void from_osi_identifier(const osi3::Identifier& osi_id, int& id);

/**
 * OSI host vehicle coordinates/orientations are relative to the global ground
 * truth coordinate system. Here, this data is stored in a Cloe object.
 */
void from_osi_host_vehicle_data(const osi3::HostVehicleData& osi_hv, cloe::Object& obj);

/**
 * Map the OSI data fields without taking care of transformations to the Cloe
 * reference frame convention.
 * Note that the OSI reference frame may differ for different object data types.
 */
void from_osi_base_moving(const osi3::BaseMoving& osi_bm, cloe::Object& obj);

/**
 * As from_osi_base_moving, but use ground truth information if required data is
 * not provided by sensor (model).
 */
void from_osi_base_moving_alt(const osi3::BaseMoving& osi_bm,
                              const osi3::BaseMoving& osi_bm_gt,
                              cloe::Object& obj);

template <typename T>
void from_osi_mov_obj_type_classification(const T& osi_mo, cloe::Object::Class& oc);
void from_osi_mov_obj_type_classification(
    const osi3::MovingObject::Type& osi_ot,
    const osi3::MovingObject::VehicleClassification::Type& osi_vt,
    cloe::Object::Class& oc);

void from_osi_detected_moving_object_alt(const osi3::DetectedMovingObject& osi_mo,
                                         const OsiGroundTruth& ground_truth, cloe::Object& obj);

void from_osi_boundary_points(const osi3::LaneBoundary& osi_lb, cloe::LaneBoundary& lb);

void transform_ego_coord_from_osi_data(const Eigen::Vector3d& dimensions_gt, cloe::Object& obj);

/**
 * \param sensor_pose: Relation between the sensor frame and the ego vehicle
 *                     frame, expressed in the ego vehicle frame.
 * \param obj When enter: Geometric information in ego vehicle frame.
 *            When return: Geometric information in sensor frame.
 */
void transform_obj_coord_from_osi_data(const Eigen::Isometry3d& sensor_pose,
                                       const Eigen::Vector3d& dimensions_gt,
                                       cloe::Object& obj);

Eigen::Isometry3d osi_position_orientation_to_pose_alt(const osi3::BaseMoving& base,
                                                       const osi3::BaseMoving& base_gt);

Eigen::Vector3d osi_vehicle_attrib_rear_offset_to_vector3d(
    const osi3::MovingObject::VehicleAttributes& osi_va);

/**
 * OSI messages of the listed data types may be overwritten by ground truth
 * information, if requested by the user.
 */
enum class SensorMockTarget { MountingPosition, DetectedMovingObject, DetectedLaneBoundary };

/**
 * SensorMockLevel determines to which degree an OSI message of a certain data type
 * should be overwritten by ground truth information:
 *  - `Zero` means that the message is not altered (default behavior).
 *  - `MissingData` means that unavailable data fields are filled.
 *  - `All` means that the entire message is overwritten.
 */
enum class SensorMockLevel { OverwriteNone, InterpolateMissing, OverwriteAll };

// clang-format off
ENUM_SERIALIZATION(SensorMockLevel, ({
  {SensorMockLevel::OverwriteNone, "overwrite_none"},
  {SensorMockLevel::InterpolateMissing, "interpolate_missing"},
  {SensorMockLevel::OverwriteAll, "overwrite_all"},
}))
// clang-format on

/**
 * Configure sensor mock level.
 */
struct SensorMockConf : public cloe::Confable {
  using Target = SensorMockTarget;
  using Level = SensorMockLevel;

  SensorMockConf() = default;

  virtual ~SensorMockConf() noexcept = default;

  std::map<Target, Level> level = {{Target::MountingPosition, Level::OverwriteNone},
                                   {Target::DetectedMovingObject, Level::OverwriteNone},
                                   {Target::DetectedLaneBoundary, Level::OverwriteNone}};

  CONFABLE_SCHEMA(SensorMockConf) {
    return fable::Schema{
        // clang-format off
        {"mounting_position", cloe::Schema(&level[Target::MountingPosition], "mock level for sensor mounting position")},
        {"detected_moving_objects", cloe::Schema(&level[Target::DetectedMovingObject], "mock level for detected moving objects")},
        {"detected_lane_boundaries", cloe::Schema(&level[Target::DetectedLaneBoundary], "mock level for detected lane boundaries")},
        // clang-format on
    };
  }

  void to_json(cloe::Json& j) const override {
    j["mounting_position"] = level.at(SensorMockTarget::MountingPosition);
    j["detected_moving_objects"] = level.at(SensorMockTarget::DetectedMovingObject);
    j["detected_lane_boundaries"] = level.at(SensorMockTarget::DetectedLaneBoundary);
  }
};

/**
 * Base class for an OSI sensor which is connected via TCP.
 */
class OsiOmniSensor {
 public:
  virtual ~OsiOmniSensor() = default;
  OsiOmniSensor() = delete;

  /**
   * Create a new instance of OsiOmniSensor with the given OsiTransceiver.
   */
  OsiOmniSensor(std::unique_ptr<OsiTransceiver>&& osi_transceiver, uint64_t owner_id)
      : osi_comm_(std::move(osi_transceiver)), owner_id_(owner_id) {
    ground_truth_ = std::make_unique<OsiGroundTruth>();
  }

  /**
   * Create a new instance of OsiOmniSensor with the given new OsiTransceiver.
   *
   * WARNING: If you use this constructor, please realize that OsiOmniSensor
   *          takes ownership of the pointer you pass in.
   */
  OsiOmniSensor(OsiTransceiver* osi_transceiver, uint64_t owner_id)
      : osi_comm_(osi_transceiver), owner_id_(owner_id) {
    ground_truth_ = std::make_unique<OsiGroundTruth>();
  }

  /**
   * Receive and process the incoming messages.
   */
  virtual void step(const cloe::Sync& s, const bool& restart, cloe::Duration& sim_time);

  /**
   * Store the initial timestamp.
   * Note that the osi time does not necessarily start at zero.
   */
  virtual void process(const osi3::Timestamp& timestamp);

  /**
   * Translate OSI SensorData to Cloe data objects.
   *
   * \param osi_sd SensorData message to be processed.
   * \param sim_time Simulation time to be set.
   */
  virtual void process(osi3::SensorData* osi_sd, cloe::Duration& sim_time);

  /**
   * Translate OSI SensorView to Cloe data objects.
   *
   * \param osi_sv SensorView message to be processed, including ground truth.
   */
  virtual void process(const osi3::SensorView& osi_sv);

  /**
   * Translate OSI ego base information made available to the sensor model
   * from other components, or use ground truth.
   * \param osi_hv HostVehicleData message to be processed (if available).
   * \param osi_mo MovingObject (ground truth) used as fallback.
   */
  virtual void process(const bool has_veh_data,
                       const osi3::HostVehicleData& osi_hv,
                       const osi3::MovingObject& osi_ego);

  /**
   * Translate OSI detected moving object information to Cloe data objects.
   *
   * \param osi_eh DetectedEntityHeader message to be processed (if available).
   * \param osi_mo DetectedMovingObject message to be processed.
   */
  virtual void process(const bool has_eh,
                       const osi3::DetectedEntityHeader& osi_eh,
                       const osi3::DetectedMovingObject& osi_mo);

  void mock_detected_lane_boundaries();

  void from_osi_boundary_points(const osi3::LaneBoundary& osi_lb, cloe::LaneBoundary& lb);

  /**
   * Store the ego object that should be passed to Cloe.
   *
   * \param ego_obj Ego object to be stored.
   */
  virtual void store_ego_object(std::shared_ptr<cloe::Object> ego_obj) = 0;

  /**
   * Store a detected object in a list of Cloe data objects.
   *
   * \param obj Object to be stored.
   */
  virtual void store_object(std::shared_ptr<cloe::Object> obj) = 0;

  /**
   * Store a detected lane boundary in a map of Cloe data objects.
   *
   * \param lb Lane boundary to be stored.
   */
  virtual void store_lane_boundary(const cloe::LaneBoundary& lb) = 0;

  /**
   * Store the sensor pose etc. in the corresponding Cloe sensor component.
   */
  virtual void store_sensor_meta_data(const Eigen::Vector3d& bbcenter_to_veh_origin,
                                      const Eigen::Vector3d& ego_dimensions) = 0;

  /**
  * Get the current simulation time (t-t0).
  */
  cloe::Duration osi_timestamp_to_simtime(const osi3::Timestamp& timestamp) const;

  /**
  * Get sensor pose in OSI vehicle reference frame, e.g. from simulator configuration.
  */
  virtual Eigen::Isometry3d get_static_mounting_position(
      const Eigen::Vector3d& bbcenter_to_veh_origin, const Eigen::Vector3d& ego_dimensions) = 0;

  virtual void set_mock_conf(std::shared_ptr<const SensorMockConf> mock) = 0;

  SensorMockLevel get_mock_level(SensorMockTarget trg_type) const {
    return mock_->level.at(trg_type);
  }

  friend void to_json(cloe::Json& j, const OsiOmniSensor& c) {
    j = cloe::Json{
        {"osi_connection", *c.osi_comm_},
    };
  }

 protected:
  /// Connection via TCP to simulator.
  /// Should always be valid.
  std::unique_ptr<OsiTransceiver> osi_comm_;

  /// Access to osi ground truth, e.g. for mock-ups.
  std::unique_ptr<OsiGroundTruth> ground_truth_;

  /// Id of the sensor's owner (ego).
  uint64_t owner_id_;

  /// Store ego pose (reference point is rear axle center, not ground level).
  Eigen::Isometry3d osi_ego_pose_;

  /// Store sensor pose relative to the ego frame (rear axle center, not ground level).
  Eigen::Isometry3d osi_sensor_pose_;

  /// Initial simulation time.
  cloe::Duration init_time_ = cloe::Duration(-1);

  /// Use alternative source for required data or overwrite incoming data, if requested.
  std::shared_ptr<const SensorMockConf> mock_{nullptr};
};
}  // namespace osii


#ifndef STAN_MCMC_HMC_HAMILTONIANS_AUTO_E_METRIC_HPP
#define STAN_MCMC_HMC_HAMILTONIANS_AUTO_E_METRIC_HPP

#include <stan/callbacks/logger.hpp>
#include <stan/math/prim.hpp>
#include <stan/mcmc/hmc/hamiltonians/base_hamiltonian.hpp>
#include <stan/mcmc/hmc/hamiltonians/auto_e_point.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>

namespace stan {
namespace mcmc {

// Euclidean manifold with dense metric
template <class Model, class BaseRNG>
class auto_e_metric : public base_hamiltonian<Model, auto_e_point, BaseRNG> {
 public:
  explicit auto_e_metric(const Model& model)
      : base_hamiltonian<Model, auto_e_point, BaseRNG>(model) {}

  double T(auto_e_point& z) {
    return 0.5 * z.p.transpose() * z.inv_e_metric_ * z.p;
  }

  double tau(auto_e_point& z) { return T(z); }

  double phi(auto_e_point& z) { return this->V(z); }

  double dG_dt(auto_e_point& z, callbacks::logger& logger) {
    return 2 * T(z) - z.q.dot(z.g);
  }

  Eigen::VectorXd dtau_dq(auto_e_point& z, callbacks::logger& logger) {
    return Eigen::VectorXd::Zero(this->model_.num_params_r());
  }

  Eigen::VectorXd dtau_dp(auto_e_point& z) {
    if (z.is_diagonal_) {
      return z.inv_e_metric_.diagonal().cwiseProduct(z.p);
    } else {
      return z.inv_e_metric_ * z.p;
    }
  }

  Eigen::VectorXd dphi_dq(auto_e_point& z, callbacks::logger& logger) {
    return z.g;
  }

  void sample_p(auto_e_point& z, BaseRNG& rng) {
    typedef typename stan::math::index_type<Eigen::VectorXd>::type idx_t;
    boost::variate_generator<BaseRNG&, boost::normal_distribution<> > rand_gaus(
        rng, boost::normal_distribution<>());

    if (z.is_diagonal_) {
      for (int i = 0; i < z.p.size(); ++i)
        z.p(i) = rand_gaus() / sqrt(z.inv_e_metric_(i, i));
    } else {
      Eigen::VectorXd u(z.p.size());

      for (idx_t i = 0; i < u.size(); ++i)
        u(i) = rand_gaus();

      z.p = z.inv_e_metric_.llt().matrixU().solve(u);
    }
  }
};

}  // namespace mcmc
}  // namespace stan
#endif


// Boost.Geometry

// Copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_TEST_SRS_PROJ4_HPP
#define BOOST_GEOMETRY_TEST_SRS_PROJ4_HPP

#ifdef TEST_WITH_PROJ4

#include <string>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <proj_api.h>

struct pj_ptr
{
    explicit pj_ptr(projPJ ptr)
        : m_ptr(ptr)
    {
        if (ptr == NULL)
            throw std::runtime_error("bleh");
    }

    projPJ get() const
    {
        return m_ptr;
    }

    ~pj_ptr()
    {
        if (m_ptr)
            pj_free(m_ptr);
    }

private:
    projPJ m_ptr;
};

struct pj_projection
{
    pj_projection(std::string const& prj)
        : m_ptr(pj_init_plus(prj.c_str()))
    {}

    template <typename In, typename Out>
    void forward(In const& in, Out & out) const
    {
        double x = bg::get_as_radian<0>(in);
        double y = bg::get_as_radian<1>(in);

        projUV p1;
        projUV p2;

        p1.u = x;
        p1.v = y;

        p2 = pj_fwd(p1, m_ptr.get());

        bg::set_from_radian<0>(out, p2.u);
        bg::set_from_radian<1>(out, p2.v);
    }

    template <typename In, typename Out>
    void inverse(In const& in, Out & out) const
    {
        double lon = bg::get_as_radian<0>(in);
        double lat = bg::get_as_radian<1>(in);

        projUV p1;
        projUV p2;

        p1.u = lon;
        p1.v = lat;

        p2 = pj_inv(p1, pj_prj.get());

        bg::set_from_radian<0>(out, p2.u);
        bg::set_from_radian<1>(out, p2.v);
    }

private:
    pj_ptr m_ptr;
};

struct pj_transformation
{
    pj_transformation(std::string const& from, std::string const& to)
        : m_from(pj_init_plus(from.c_str()))
        , m_to(pj_init_plus(to.c_str()))
    {}

    template <typename In, typename Out>
    void forward(In const& in, Out & out) const
    {
        double x = bg::get_as_radian<0>(in);
        double y = bg::get_as_radian<1>(in);

        pj_transform(m_from.get(), m_to.get(), 1, 0, &x, &y, NULL);

        bg::set_from_radian<0>(out, x);
        bg::set_from_radian<1>(out, y);
    }

    void forward(std::vector<double> in_x,
                 std::vector<double> in_y,
                 std::vector<double> & out_x,
                 std::vector<double> & out_y) const
    {
        assert(in_x.size() == in_y.size());
        pj_transform(m_from.get(), m_to.get(), in_x.size(), 1, &in_x[0], &in_y[0], NULL);
        out_x = in_x;
        out_y = in_y;
    }

private:
    pj_ptr m_from;
    pj_ptr m_to;
};

#endif // TEST_WITH_PROJ4

#endif // BOOST_GEOMETRY_TEST_SRS_PROJ4_HPP


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_TetMeshTrackLengthFluxEstimator.hpp
//! \author Alex Robinson, Eli Moll
//! \brief  Tet mesh flux estimator class declaration.
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_TET_MESH_TRACK_LENGTH_FLUX_ESTIMATOR_HPP
#define MONTE_CARLO_TET_MESH_TRACK_LENGTH_FLUX_ESTIMATOR_HPP

// Std Lib Includes
#include <string>

// Boost Includes
#include <boost/mpl/vector.hpp>
#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>

// Moab Includes
#include <moab/Interface.hpp>
#include <moab/AdaptiveKDTree.hpp>
#include <moab/Matrix3.hpp>

// Trilinos Includes
#include <Teuchos_RCP.hpp>

// FRENSIE Includes
#include "MonteCarlo_StandardEntityEstimator.hpp"
#include "MonteCarlo_ParticleSubtrackEndingGlobalEventObserver.hpp"
#include "MonteCarlo_EstimatorContributionMultiplierPolicy.hpp"
#include "Geometry_ModuleTraits.hpp"
#include "MonteCarlo_ParticleState.hpp"

namespace MonteCarlo{

/*! The tet-mesh track length flux estimator class
 * \details This class is based off of the TrackLengthMeshTally written by
 * Kerry Dunn (UW-Madison CNERG group). The DAGMC repo that contains her
 * class can be found at https://github.com/svalinn/DAGMC.
 */
template<typename ContributionMutliplierPolicy = WeightMultiplier>
class TetMeshTrackLengthFluxEstimator : public StandardEntityEstimator<moab::EntityHandle>,
  public ParticleSubtrackEndingGlobalEventObserver
{

public:

  //! Typedef for the cell id type
  typedef Geometry::ModuleTraits::InternalCellHandle cellIdType;

  //! Typedef for event tags used for quick dispatcher registering
  typedef boost::mpl::vector<ParticleSubtrackEndingGlobalEventObserver::EventTag>
  EventTags;

  //! Constructor
  TetMeshTrackLengthFluxEstimator(
		     const Estimator::idType id,
		     const double multiplier,
		     const std::string input_mesh_file_name,
		     const std::string output_mesh_file_name = "tetmesh.h5m" );

  //! Destructor
  ~TetMeshTrackLengthFluxEstimator()
  { /* ... */ }

  //! Set the response functions
  void setResponseFunctions(
  const Teuchos::Array<Teuchos::RCP<ResponseFunction> >& response_functions );

  //! Set the particle types that can contribute to the estimator
  void setParticleTypes( const Teuchos::Array<ParticleType>& particle_types );

  //! Add current history estimator contribution
  void updateFromGlobalParticleSubtrackEndingEvent(
						 const ParticleState& particle,
						 const double start_point[3],
						 const double end_point[3] );

  //! Export the estimator data
  void exportData( EstimatorHDF5FileHandler& hdf5_file,
		   const bool process_data ) const;

  //! Print the estimator data
  void print( std::ostream& os ) const;

  //! Get all tet elements
  const moab::Range getAllTetElements() const;

  //! Test if a point is in the mesh
  bool isPointInMesh( const double point[3] );

  //! Determine which tet the point is in
  moab::EntityHandle whichTetIsPointIn( const double point[3] );

private:

  // Assign bin boundaries to an estimator dimension
  void assignBinBoundaries(
	const Teuchos::RCP<EstimatorDimensionDiscretization>& bin_boundaries );

  // The tolerance used for geometric tests
  static const double s_tol;

  // The moab instance that stores all mesh data
  Teuchos::RCP<moab::Interface> d_moab_interface;

  // The tet meshset
  moab::EntityHandle d_tet_meshset;

  // The kd-tree for finding point in tet
  Teuchos::RCP<moab::AdaptiveKDTree> d_kd_tree;

  // The root of the kd-tree
  moab::EntityHandle d_kd_tree_root;

  // The map of tet ids and barycentric coordinate transform matrices
  boost::unordered_map<moab::EntityHandle,moab::Matrix3>
  d_tet_barycentric_transform_matrices;

  // The map of tet ids and reference vertices
  boost::unordered_map<moab::EntityHandle, moab::CartVect>
  d_tet_reference_vertices;

  // The output mesh file name
  std::string d_output_mesh_name;
};

} // end MonteCarlo namespace

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "MonteCarlo_TetMeshTrackLengthFluxEstimator_def.hpp"

//---------------------------------------------------------------------------//

#endif // end MONTE_CARLO_TET_MESH_TRACK_LENGTH_FLUX_ESTIMATOR_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_TetMeshTrackLengthFluxEstimator.hpp
//---------------------------------------------------------------------------//


#include "ag_CumDistributionFunctionView.h"

// Library headers.
#include <boost/smart_ptr.hpp>
#include <QApplication>
#include <QPen>
#include <qwt_plot_canvas.h>
#include <qwt_scale_div.h>

// PCRaster library headers.
#include "dal_DataSpace.h"
#include "dal_Table.h"
#include "dal_MathUtils.h"
#include "com_userdefinedclassifier.h"

// Module headers.
#include "ag_DataObject.h"
#include "ag_DataProperties.h"
#include "ag_Raster.h"
#include "ag_RasterDataSources.h"
#include "ag_VisEngine.h"

/*!
  \file
  This file contains the implementation of the CumDistributionFunctionView class.
*/

namespace ag {

//------------------------------------------------------------------------------
// DEFINITION OF STATIC CUMDISTRIBUTIONFUNCTIONVIEW MEMBERS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DEFINITION OF CUMDISTRIBUTIONFUNCTIONVIEW MEMBERS
//------------------------------------------------------------------------------

CumDistributionFunctionView::CumDistributionFunctionView(
         DataObject* object, QWidget* parent, const char* name)

  : PlotVisualisation(object, "Cumulative Distribution Function View",
         parent, name)

{
  // Supported data types.
  std::vector<geo::DataType> dataTypes;
  dataTypes.push_back(geo::STACK);
  dataTypes.push_back(geo::FEATURE);
  setSupportedDataTypes(dataTypes);

  std::vector<CSF_VS> valueScales;
  valueScales.push_back(VS_SCALAR);
  setSupportedValueScales(valueScales);

  trackDragPoint();

  canvas()->setCursor(Qt::PointingHandCursor);
}

CumDistributionFunctionView::~CumDistributionFunctionView()
{
}

void CumDistributionFunctionView::rescan()
{
  visualisationEngine().rescan(dataObject());
}

void CumDistributionFunctionView::process()
{
  if(visualisationEngine().change() & VisEngine::OTHERATTRIB ||
         visualisationEngine().change() & VisEngine::DRAWPROPS ||
         visualisationEngine().change() & VisEngine::SELECTION ||
         visualisationEngine().change() & VisEngine::TIME ||
         visualisationEngine().change() & VisEngine::RASTER_CELL) {
    clearPlot();
    createPlot();
  }
  else {
    if(visualisationEngine().change() & VisEngine::QUANTILE) {
      assert(!(visualisationEngine().change() & VisEngine::VALUE_SELECTION));

      dal::DataSpace const& space(dataObject().dataSpace());

      if(space.hasCumProbabilities()) {
        dal::DataSpaceAddress const& address(dataObject().dataSpaceAddress());
        size_t index = space.indexOf(dal::CumulativeProbabilities);
        double quantile = address.coordinate<float>(index);

        setYMarker(quantile);
      }
    }
    else if(visualisationEngine().change() & VisEngine::VALUE_SELECTION) {
      assert(!(visualisationEngine().change() & VisEngine::QUANTILE));
      assert(dataObject().hasSelectedValue());

      setXMarker(dataObject().selectedValue());
    }
  }

  if(visualisationEngine().change() & VisEngine::BACKGROUND_COLOUR) {
    if(!dataObject().backgroundColour().isValid()) {
      setPalette(QPalette());
    }
    else {
      QPalette palette;
      palette.setColor(backgroundRole(), dataObject().backgroundColour());
      setPalette(palette);
    }
  }
}

void CumDistributionFunctionView::visualise()
{
  if(visualisationEngine().change() & VisEngine::OTHERATTRIB ||
         visualisationEngine().change() & VisEngine::DRAWPROPS ||
         visualisationEngine().change() & VisEngine::VALUE_SELECTION ||
         visualisationEngine().change() & VisEngine::SELECTION ||
         visualisationEngine().change() & VisEngine::RASTER_CELL ||
         visualisationEngine().change() & VisEngine::TIME ||
         visualisationEngine().change() & VisEngine::QUANTILE ||
         visualisationEngine().change() & VisEngine::BACKGROUND_COLOUR) {
    replot();
  }

  visualisationEngine().finishedScanning(dataObject());
}

void CumDistributionFunctionView::addAttribute(
         DataGuide const& dataGuide) {
  testDataGuide(dataGuide);
  visualisationEngine().addAttribute(dataObject(), dataGuide);
}

void CumDistributionFunctionView::setXAxisTitle()
{
  QwtText title;

  title.setFont(QApplication::font());
  title.setText(QString("Value"));
  setAxisTitle(xBottom, title);
}

void CumDistributionFunctionView::setYAxisTitle()
{
  QwtText title;

  title.setFont(QApplication::font());

  if(onlyCumulativeProbabilitiesShown()) {
    title.setText(QString("Cumulative probability"));
  }
  else if(onlyExceedanceProbabilitiesShown()) {
    title.setText(QString("Exceedance probability"));
  }
  else {
    title.setText(QString("Probability"));
  }

  setAxisTitle(yLeft, title);
}

void CumDistributionFunctionView::setXAxisScale()
{
  double min, max;
  pcr::setMV(min);
  pcr::setMV(max);
  bool extremesInitialised = false;

  if(!dataObject().hasSelectedValue()) {
    BOOST_FOREACH(DataGuide const& guide, visualisationEngine().dataGuides()) {
      assert(guide.valueScale() == VS_SCALAR);

      RangeDrawProps const& properties(
         dataObject().properties().rangeDrawProperties(guide));

      if(properties.cutoffsAreValid()) {
        if(!extremesInitialised) {
          min = properties.minCutoff();
          max = properties.maxCutoff();
          extremesInitialised = true;
        }
        else {
          min = std::min(min, properties.minCutoff());
          max = std::max(max, properties.maxCutoff());
        }
      }
    }
  }
  else {
    SpatialDataset* dataset;

    BOOST_FOREACH(DataGuide const& guide, visualisationEngine().dataGuides()) {
      assert(guide.type() == geo::STACK || guide.type() == geo::FEATURE);
      assert(guide.valueScale() == VS_SCALAR);
      dataset = 0;

      switch(guide.type()) {
        case geo::STACK: {
          dataset = &dataObject().rasterDataSources().data(guide);
          break;
        }
        case geo::FEATURE: {
          dataset = &dataObject().featureDataSources().data(guide);
          break;
        }
        default: {
          assert(false);
          break;
        }
      }

      assert(dataset);
      assert(dataset->dataSpace().hasCumProbabilities());

      if(!dataset->allMV()) {
        if(!extremesInitialised) {
          min = dataset->min<REAL4>();
          max = dataset->max<REAL4>();
          extremesInitialised = true;
        }
        else {
          min = std::min(min, double(dataset->min<REAL4>()));
          max = std::max(max, double(dataset->max<REAL4>()));
        }
      }
    }
  }

  if(!pcr::isMV(min) && !pcr::isMV(max)) {
    assert(min <= max);
    setAxisScale(xBottom, min, max);
  }
}

void CumDistributionFunctionView::setYAxisScale()
{
  double min = 0.0;
  double max = 1.0;

  if(dataObject().hasSelectedValue()) {
    bool extremesInitialised = false;

    BOOST_FOREACH(DataGuide const& guide, visualisationEngine().dataGuides()) {
      assert(guide.valueScale() == VS_SCALAR);

      RangeDrawProps const& properties(
         dataObject().properties().rangeDrawProperties(guide));

      if(properties.cutoffsAreValid()) {
        if(!extremesInitialised) {
          min = properties.minCutoff();
          max = properties.maxCutoff();
          extremesInitialised = true;
        }
        else {
          min = std::min(min, properties.minCutoff());
          max = std::max(max, properties.maxCutoff());
        }
      }
    }
  }

  setAxisScale(yLeft, min, max);
}

void CumDistributionFunctionView::configureXAxis()
{
  setXAxisTitle();
  setXAxisScale();
}

void CumDistributionFunctionView::configureYAxis()
{
  setYAxisTitle();
  setYAxisScale();
}

void CumDistributionFunctionView::drawPlots()
{
  DataObject& object(dataObject());
  dal::DataSpace const& space(object.dataSpace());
  dal::DataSpaceAddress const& address(dataObject().dataSpaceAddress());

  SpatialDataset* dataset;

  BOOST_FOREACH(DataGuide const& guide, visualisationEngine().dataGuides()) {
    assert(guide.type() == geo::STACK || guide.type() == geo::FEATURE);
    assert(guide.valueScale() == VS_SCALAR);

    dataset = 0;

    switch(guide.type()) {
      case geo::STACK: {
        dataset = &object.rasterDataSources().data(guide);
        break;
      }
      case geo::FEATURE: {
        dataset = &object.featureDataSources().data(guide);
        break;
      }
      default: {
        assert(false);
        break;
      }
    }

    assert(dataset);
    assert(dataset->dataSpace().hasCumProbabilities());

    if(!dataset->allMV()) {
      // Create table for data values at the quantile levels.
      dal::Table table;
      dataset->readCumulativeProbabilities(space, address, table);

      dal::Array<REAL4> const& quantileCol(table.col<REAL4>(0));
      dal::Array<REAL4> const& attrCol(table.col<REAL4>(1));
      boost::scoped_array<double> x(new double[table.nrRecs()]);
      boost::scoped_array<double> y(new double[table.nrRecs()]);

      RangeDrawProps const& properties(
        object.properties().rangeDrawProperties(guide));

      for(size_t i = 0; i < table.nrRecs(); ++i) {
        y[i] = quantileCol[i];

        if(properties.probabilityScale() ==
               RangeDrawProps::ExceedanceProbabilities) {
          y[i] = 1.0 - y[i];
        }

        if(pcr::isMV(attrCol[i])) {
          pcr::setMV(x[i]);
        }
        else {
          x[i] = attrCol[i];
        }
      }

      QPen pen;

      if(object.isSelected(guide)) {
        pen = QPen(object.properties().colour(guide), 2, Qt::SolidLine);
      }
      else {
        pen = QPen(object.properties().colour(guide), 1, Qt::SolidLine);
      }

      drawCurve(guide, x.get(), y.get(), table.nrRecs(), pen);
    }
  }

  if(object.hasSelectedValue(/* guide */)) {
    // A value is selected. Use the first guide for the properties of
    // the marker. Mark data values.
    setXMarker(object.selectedValue(/* guide */));
    enableMarker(xMarker());
    disableMarker(yMarker());
  }
  else {
    size_t indexOfCumProbabilities = space.indexOf(
         dal::CumulativeProbabilities);
    assert(address.isValid(indexOfCumProbabilities));
    float quantile = address.coordinate<float>(indexOfCumProbabilities);

    setYMarker(quantile);
    disableMarker(xMarker());
    enableMarker(yMarker());
  }
}

void CumDistributionFunctionView::createPlot()
{
  if(visualisationEngine().isEmpty()) {
    return;
  }

  if(!dataObject().dataSpace().hasCumProbabilities()) {
    return;
  }

  configureXAxis();
  configureYAxis();
  drawPlots();
  attachMarkers();
}

void CumDistributionFunctionView::appended(
         QPointF const& point)
{
  moved(point);
}

void CumDistributionFunctionView::moved(
         QPointF const& point)
{
  if(markerEnabled(xMarker())) {
    /// for(size_t i = 0; i < visualisationEngine().size(); ++i) {
    ///   DataGuide const& guide = visualisationEngine().guide(i);
    ///   // assert(guide.type() == geo::STACK);
    ///   assert(guide.valueScale() == VS_SCALAR);
    ///   dataObject().setSelectedValue<REAL4>(guide, static_cast<float>(point.x()), false);
    /// }

    dataObject().setSelectedValue(static_cast<REAL4>(point.x()), false);
    /// dataObject().notify();
  }
  else if(markerEnabled(yMarker())) {
    // Snap to closest quantile.
    dal::DataSpace const& space = dataObject().dataSpace();
    if(space.hasCumProbabilities()) {
      size_t index = space.indexOf(dal::CumulativeProbabilities);
      dal::Dimension const& dimension = space.dimension(index);
      dataObject().setQuantile(dimension.clamp<float>(static_cast<float>(point.y())), false);
    }
  }

  dataObject().notify();
}

QSize CumDistributionFunctionView::minimumSizeHint() const
{
  // Override QwtPlot one with the default.
  return QWidget::minimumSizeHint();
}

void CumDistributionFunctionView::toggleMarker()
{
  // Determine which marker is on: the one which iterates over the quantiles
  // or the one which iterates over the data values.
  // Then switch from the one marker to the other and adjust relevant settings.

  // The initial position of the new marker is the intersection of the current
  // marker with the plot of the first data guide.

  assert(!visualisationEngine().isEmpty());

  double x, y;

  assert(markerEnabled(xMarker()) || markerEnabled(yMarker()));
  assert(!(markerEnabled(xMarker()) && markerEnabled(yMarker())));

  // Take the first guide.
  DataGuide const& guide = visualisationEngine().guide(0);

  if(markerEnabled(xMarker())) {
    // xMarker iterates over the y-axis. Attribute values will be shown in
    // the map.
    if(!intersectMarker(&x, &y, xMarker(), guide)) {
      y = 0.5;
    }

    // Snap to closest quantile.
    dal::DataSpace const& space = dataObject().dataSpace();
    assert(space.hasCumProbabilities());
    size_t index = space.indexOf(dal::CumulativeProbabilities);
    dal::Dimension const& dimension = space.dimension(index);
    dataObject().setQuantile(dimension.clamp<float>(y), false);

    dataObject().unsetSelectedValue(false);

    for(size_t i = 0; i < visualisationEngine().size(); ++i) {
      DataGuide const& guide = visualisationEngine().guide(i);
      dataObject().popClassifiers(guide, false);
    }
  }
  else if(markerEnabled(yMarker())) {
    // double min, max;

    // extremes(&min, &max);

    // yMarker iterates over the x-axis. Probabilities will be shown in the
    // map.
    if(!intersectMarker(&x, &y, yMarker(), guide)) {
      // Marker does not intersect the curve of the first guide.
#if QWT_VERSION >= 0x060100
      // QwtPlot::axisScaleDiv returns a reference.
      QwtScaleDiv const& scaleDiv = axisScaleDiv(xMarker());
#else
      // QwtPlot::axisScaleDiv returns a pointer.
      QwtScaleDiv const& scaleDiv = *axisScaleDiv(xMarker());
#endif

      x = scaleDiv.lowerBound();

      if(scaleDiv.range() > 0.0) {
        // Use the middle value.
        x += scaleDiv.range() / 2.0;
      }
    }

    dataObject().setSelectedValue(x, false);

    for(size_t i = 0; i < visualisationEngine().size(); ++i) {
      DataGuide const& guide = visualisationEngine().guide(i);
      assert(guide.valueScale() == VS_SCALAR);

      com::Classifier classifier(0.0, 1.0);
      classifier.setNrClasses(100);
      classifier.installAlgorithm(com::Classifier::LIN);
      dataObject().pushClassifier(guide, classifier, false);
    }
  }

  dataObject().notify();
}

//------------------------------------------------------------------------------
// DEFINITION OF FREE OPERATORS
//------------------------------------------------------------------------------

//------------------------------------------------------------------------------
// DEFINITION OF FREE FUNCTIONS
//------------------------------------------------------------------------------

} // namespace ag


#ifndef __GYRO_ROCKET6G_HH__
#define __GYRO_ROCKET6G_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Non-Ideal Gyro Implementation from Rocket6G)
LIBRARY DEPENDENCY:
      ((../../src/gyro/gyro_rocket6g.cpp))
*******************************************************************************/

#include <armadillo>
#include "aux.hh"
#include "gyro.hh"

class GyroRocket6G : public Gyro
{
    TRICK_INTERFACE(GyroRocket6G);

public:
    GyroRocket6G();

    virtual ~GyroRocket6G() {}
    virtual void init(LaunchVehicle *VehicleIn){};
    virtual void algorithm(LaunchVehicle *VehicleIn);

    std::default_random_engine generator;
};

#endif  // __GYRO_ROCKET6G__


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-ieee
    Function object implementing bitfloating capabilities

    Transform a pattern of bits stored in an integer value
    in a floating point with different formulas according to
    the integer @ref bitofsign value.

    This function is the converse of @ref bitinteger

    @par Semantic:

    for any parameter @c x of integral type @c T

    @code
    as_floating_t<T> r = bitfloating(x);
    @endcode

    @par Note:
    This is an internal used utility function related to the computation of
    floating successors or predecessors.

    @see next, prev, successor,  predecessor,  nextafter, bitinteger

  **/
  as_floating_t<T> bitfloating(Value const & v0);
} }
#endif

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

#endif