/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      111103    S. Billemont      File created.
 *      111205    T. Secretin       Added functionalities to previous shell code.
 *      120813    P. Musegaas       Changed code to new root finding structure.
 *      130317    K. Kumar          Updated allocation of Vector6d to fix problem with Eigen types.
 *
 *    References
 *      Eigen. http://eigen.tuxfamily.org/dox/TopicUnalignedArrayAssert.html, last accessed: 18th
 *          March, 2013.
 *
 *    Notes
 *      This implementation uses the KeplerianElements class, which is marked for deprecation. The
 *      code will be updated to use a simple "Vector6d" object from the Eigen library instead.
 *
 */

#include <stdexcept>

#include <boost/exception/all.hpp>
#include <boost/make_shared.hpp>

#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/convertMeanToEccentricAnomalies.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"
#include "Tudat/Astrodynamics/Ephemerides/keplerStateExtractor.h"
#include "Tudat/InputOutput/parsedDataVectorUtilities.h"

namespace tudat
{
namespace ephemerides
{

//! Extract the Keplerian Elements.
boost::shared_ptr< basic_mathematics::Vector6d > KeplerStateExtractor::extract(
        ParsedDataLineMapPtr dataLineMap )
{
    // Short-hand notation.
    namespace parsed_data_vector_utilities = input_output::parsed_data_vector_utilities;
    using basic_mathematics::Vector6d;

    // Create a new KeplerianElements object.
    boost::shared_ptr< Vector6d > keplerianElements
            = boost::allocate_shared< Vector6d >( Eigen::aligned_allocator< Vector6d >( ) );

    // Find and set semi-major axis.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::semiMajorAxis ) )
    {
        ( *keplerianElements )( orbital_element_conversions::semiMajorAxisIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::semiMajorAxis );
    }

    else
    {
        boost::throw_exception( boost::enable_error_info(
                                    std::runtime_error( "No semi-major axis entry found." ) ) );
    }

    // Find and set eccentricity.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::eccentricity ) )
    {
        ( *keplerianElements )( orbital_element_conversions::eccentricityIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::eccentricity );
    }

    else
    {
        boost::throw_exception( boost::enable_error_info(
                                    std::runtime_error( "No eccentricity entry found." ) ) );
    }

    // Find and set inclination.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::inclination ) )
    {
        ( *keplerianElements )( orbital_element_conversions::inclinationIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::inclination );
    }

    else
    {
        boost::throw_exception( boost::enable_error_info(
                                    std::runtime_error( "No inclination entry found." ) ) );
    }

    // Find and set longitude of ascending node.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::longitudeOfAscendingNode ) )
    {
        ( *keplerianElements )( orbital_element_conversions::longitudeOfAscendingNodeIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::longitudeOfAscendingNode );
    }

    else
    {
        boost::throw_exception( boost::enable_error_info(
                                    std::runtime_error(
                                        "No longitude of ascending node entry found." ) ) );
    }

    // Find and set argument of periapsis.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::argumentOfPeriapsis ) )
    {
        ( *keplerianElements )( orbital_element_conversions::argumentOfPeriapsisIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::argumentOfPeriapsis );
    }

    else
    {
        boost::throw_exception( boost::enable_error_info(
                                    std::runtime_error(
                                        "No argument of periapsis entry found." ) ) );
    }

    // Find and set true anomaly.
    if ( checkOptionalFieldType( dataLineMap, 1,
                                 input_output::field_types::state::trueAnomaly ) )
    {
        ( *keplerianElements )( orbital_element_conversions::trueAnomalyIndex )
                = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::trueAnomaly );
    }

    // If the true anomaly is not present, check for mean anomaly.
    else if ( checkOptionalFieldType( dataLineMap, 1,
                                      input_output::field_types::state::meanAnomaly ) )
    {
        // Store mean anomaly.
        const double meanAnomaly = parsed_data_vector_utilities::getField< double >(
                    dataLineMap, input_output::field_types::state::meanAnomaly );

        // Retrieve eccentricity.
        const double eccentricity
                = ( *keplerianElements )( orbital_element_conversions::eccentricityIndex );

        // Convert to eccentric anomaly.
        const double eccentricAnomaly = orbital_element_conversions::
                convertMeanAnomalyToEccentricAnomaly( eccentricity, meanAnomaly );

        // Convert eccentric anomaly to true anomaly and set the latter.
        ( *keplerianElements )( orbital_element_conversions::trueAnomalyIndex )
                = orbital_element_conversions::
                convertEccentricAnomalyToTrueAnomaly( eccentricAnomaly, eccentricity );
    }

    else
    {
        boost::throw_exception( boost::enable_error_info(
                                    std::runtime_error(
                                        "No true anomaly or mean anomaly entries found." ) ) );
    }

    return keplerianElements;
}

} // namespace ephemerides
} // namespace tudat


/*
 * CSIGadget.cpp
 *
 *  Created on: Nov 11, 2014
 *      Author: dch
 */

#include "CSIGadget.h"
#include <ismrmrd/xml.h>
#include "cudaDeviceManager.h"
#include "cuNDArray_utils.h"
#include "cuNlcgSolver.h"
#include "eigenTester.h"
#include "CSfreqOperator.h"
#include "cuPartialDerivativeOperator.h"
#include "cuDWTOperator.h"
#include <boost/make_shared.hpp>
namespace Gadgetron {

CSIGadget::CSIGadget() {
	// TODO Auto-generated constructor stub

}

CSIGadget::~CSIGadget() {
	// TODO Auto-generated destructor stub
}

int CSIGadget::process_config(ACE_Message_Block *mb){
	//GDEBUG("gpuCgSenseGadget::process_config\n");

        device_number_ = deviceno.value();

	int number_of_devices = cudaDeviceManager::Instance()->getTotalNumberOfDevice();
	if (number_of_devices == 0) {
		GDEBUG( "Error: No available CUDA devices.\n" );
		return GADGET_FAIL;
	}

	if (device_number_ >= number_of_devices) {
		GDEBUG("Adjusting device number from %d to %d\n", device_number_,  (device_number_%number_of_devices));
		device_number_ = (device_number_%number_of_devices);
	}

	if (cudaSetDevice(device_number_)!= cudaSuccess) {
		GDEBUG( "Error: unable to set CUDA device.\n" );
		return GADGET_FAIL;
	}

	cg_limit_ = cg_limit.value();
	oversampling_factor_ = oversampling_factor.value();
	kernel_width_ = kernel_width.value();
	output_convergence_ = output_convergence.value();
	number_of_sb_iterations_ = number_of_sb_iterations.value();
	number_of_cg_iterations_ = number_of_cg_iterations.value();

	use_compressed_sensing_ = compressed_sensing.value();

	mu_ = mu.value();

	// Get the Ismrmrd header
	//
	ISMRMRD::IsmrmrdHeader h;
	ISMRMRD::deserialize(mb->rd_ptr(),h);

	if (h.encoding.size() != 1) {
		GDEBUG("This Gadget only supports one encoding space\n");
		return GADGET_FAIL;
	}

	// Get the encoding space and trajectory description
	ISMRMRD::EncodingSpace e_space = h.encoding[0].encodedSpace;
	ISMRMRD::EncodingSpace r_space = h.encoding[0].reconSpace;
	ISMRMRD::EncodingLimits e_limits = h.encoding[0].encodingLimits;

	img_dims_ = {r_space.matrixSize.x,r_space.matrixSize.y,r_space.matrixSize.z};

	matrix_size_ = vector_td<uint64_t,2>{r_space.matrixSize.x,r_space.matrixSize.y};

	unsigned int warp_size = cudaDeviceManager::Instance()->warp_size(device_number_);

	matrix_size_os_ =
			uint64d2(((static_cast<unsigned int>(std::ceil(matrix_size_[0]*oversampling_factor_))+warp_size-1)/warp_size)*warp_size,
					((static_cast<unsigned int>(std::ceil(matrix_size_[1]*oversampling_factor_))+warp_size-1)/warp_size)*warp_size);

		if (h.acquisitionSystemInformation) {
			channels_ = h.acquisitionSystemInformation->receiverChannels ? *h.acquisitionSystemInformation->receiverChannels : 1;
		} else {
			channels_ = 1;
		}

		/*if (~h.userParameters.is_present()){
			GDEBUG("CSI gadget requires userparameters to be set to obtain timesteps.");
			return GADGET_FAIL;
		}*/

		auto parameters = h.userParameters->userParameterDouble;
		auto bw = std::find_if(parameters.begin(),parameters.end(), [](ISMRMRD::UserParameterDouble d) { return d.name=="bw";});

		if (bw->name != "bw"){
			GDEBUG("CSI gadget: User parameter bw is missing.");
			return GADGET_FAIL;
		}

		auto dte = std::find_if(parameters.begin(),parameters.end(), [](ISMRMRD::UserParameterDouble d) { return d.name=="dte";});
		if (dte->name != "dte"){
			GDEBUG("CSI gadget: User parameter dte is missing.");
			return GADGET_FAIL;
		}

// Allocate encoding operator for non-Cartesian Sense
		E_ = boost::make_shared< CSIOperator<float> >(1/bw->value, dte->value);
		E_->set_weight(mu_);

		std::vector<float> freqs = frequencies.value();

		if (freqs.empty()){
			for (float f = frequency_min.value(); f <= frequency_max.value(); f+= frequency_step.value())
				freqs.push_back(f);
		}

		if (freqs.size() == 0)
			throw std::runtime_error("CSIGadget: Frequencies not set!");

		std::stringstream ss;
		ss << "Frequencies set: ";
		for (auto f : freqs)
				ss << f << " ";
		GDEBUG(ss.str().c_str());

		E_->set_frequencies(freqs);

		img_dims_[2]=freqs.size();

		S_ = boost::make_shared<cuNonCartesianSenseOperator<float,2>>();

		E_->set_senseOp(S_);

		auto idOp = boost::make_shared<identityOperator<cuNDArray<float_complext>>>();
		idOp->set_domain_dimensions(&img_dims_);
		idOp->set_codomain_dimensions(&img_dims_);
		idOp->set_weight(2*mu_);
		solver_.add_regularization_operator(idOp);

		auto dX = boost::make_shared<cuPartialDerivativeOperator<float_complext,3>>(0);
		dX->set_domain_dimensions(&img_dims_);
		dX->set_codomain_dimensions(&img_dims_);
		dX->set_weight(2*mu_);
		auto dY = boost::make_shared<cuPartialDerivativeOperator<float_complext,3>>(1);
		dY->set_domain_dimensions(&img_dims_);
		dY->set_codomain_dimensions(&img_dims_);
		dY->set_weight(2*mu_);
		auto dZ = boost::make_shared<cuPartialDerivativeOperator<float_complext,3>>(2);
		dZ->set_domain_dimensions(&img_dims_);
		dZ->set_codomain_dimensions(&img_dims_);
		dZ->set_weight(2*mu_);

		solver_.add_regularization_group_operator(dX);
		solver_.add_regularization_group_operator(dY);

		//solver_.add_regularization_group_operator(dZ);
		solver_.add_group();
/*
		auto W = boost::make_shared<cuDWTOperator<float_complext,3>>();
		W->set_domain_dimensions(&img_dims_);
		W->set_codomain_dimensions(&img_dims_);
		W->set_weight(2*mu_);

		auto W2 = boost::make_shared<cuDWTOperator<float_complext,3>>();
		W2->set_shift(2);
		W2->set_domain_dimensions(&img_dims_);
		W2->set_codomain_dimensions(&img_dims_);
		W2->set_weight(2*mu_);
		solver_.add_regularization_operator(W);
		solver_.add_regularization_operator(W2);
		*/
		// Setup solver
		solver_.set_encoding_operator( E_ );        // encoding matrix
		solver_.set_max_outer_iterations( number_of_sb_iterations_ );
		solver_.set_max_inner_iterations(1);
		solver_.get_inner_solver()->set_max_iterations(number_of_cg_iterations_);
		solver_.get_inner_solver()->set_tc_tolerance( cg_limit_ );
		solver_.set_output_mode( (output_convergence_) ? cuCgSolver<float_complext>::OUTPUT_VERBOSE : cuCgSolver<float_complext>::OUTPUT_SILENT);
		is_configured_ = true;
	return GADGET_OK;

}

int CSIGadget::process(GadgetContainerMessage<cuSenseData>* m1){

	if (!is_configured_) {
		GDEBUG("\nData received before configuration complete\n");
		return GADGET_FAIL;
	}

	GDEBUG("CSI is on the job\n");

	auto traj = m1->getObjectPtr()->traj;

	auto trajdims2 = std::vector<size_t>{traj->get_size(0),1};
	//Extract initial trajectory
	cuNDArray<floatd2> traj2(trajdims2,traj->get_data_ptr());

	auto data = m1->getObjectPtr()->data;
	auto csm =m1->getObjectPtr()->csm;
	auto dcw = m1->getObjectPtr()->dcw;
	//dcw.reset();

	if (dcw)
		sqrt_inplace(dcw.get());

	E_->set_domain_dimensions(&img_dims_);
	E_->set_codomain_dimensions(data->get_dimensions().get());

	std::vector<size_t> sense_dims = *data->get_dimensions();
	sense_dims[1] = img_dims_[2];

	S_->set_domain_dimensions(&img_dims_);
	S_->set_codomain_dimensions(&sense_dims);
/*
	{
		GDEBUG("Removing CSM maps");
		auto csm_dims = *csm->get_dimensions();
		csm_dims.pop_back();
		cuNDArray<float_complext> csm_view(csm_dims,csm->get_data_ptr());
		fill(&csm_view,complext<float>(1,0));
		size_t nelements = csm_view.get_number_of_elements();
		for (int  i = 1; i< csm->get_size(2); i++){
			cuNDArray<float_complext> csm_view2(csm_dims,csm->get_data_ptr()+i*nelements);
			clear(&csm_view2);
		}
	}
*/
	S_->set_csm(csm);
	S_->set_dcw(dcw);
	S_->setup( matrix_size_, matrix_size_os_, kernel_width_ );
	S_->preprocess(&traj2);

	GDEBUG("Setup done, solving....\n");
	/*
	eigenTester<cuNDArray<float_complext>> tester;
	std::vector<float> freqs{  -575.1223,-450.1223,-360.1223,  -183.1223,140.8777};
	auto T_ = boost::make_shared<CSfreqOperator>(E_->get_pointtime(),E_->get_echotime());
	T_->set_frequencies(freqs);
	T_->set_codomain_dimensions(data->get_dimensions().get());

	std::vector<size_t> tim_dims = *data->get_dimensions();
	tim_dims[2] = freqs.size();
	T_->set_domain_dimensions(&tim_dims);

	tester.add_encoding_operator(E_);

	float_complext eigVal = tester.get_smallest_eigenvalue();

	GDEBUG("Smallest eigenvalue: %f %f /n",real(eigVal),imag(eigVal));
*/
	/*
	cuNlcgSolver<float_complext> solv;
	//cuCgSolver<float_complext> solv;
	solv.set_output_mode(cuCgSolver<float_complext>::OUTPUT_VERBOSE);
	solv.set_max_iterations(10);
	solv.set_encoding_operator(E_);
	solv.set_tc_tolerance(1e-8f);
	*/
	boost::shared_ptr<cuNDArray<float_complext>> result;
	if (use_compressed_sensing_)
		result = solver_.solve(data.get());
	else {
		cgSolver<cuNDArray<float_complext>> cgsolver;
		cgsolver.set_max_iterations(solver_.get_inner_solver()->get_max_iterations());
		cgsolver.set_encoding_operator(E_);
		result = cgsolver.solve(data.get());
	}
	//auto result = solv.solve(data.get());

	//E_->mult_MH(data.get(),result.get(),false);

	GDEBUG("Image sum: %f \n",asum(result.get()));
	m1->release();

	GDEBUG("Solver done, next patient...");

	GadgetContainerMessage< hoNDArray< std::complex<float> > > *cm =
			new GadgetContainerMessage< hoNDArray< std::complex<float> > >();

	GadgetContainerMessage<ISMRMRD::ImageHeader> *m =
			new GadgetContainerMessage<ISMRMRD::ImageHeader>();

	m->cont(cm);

	result->to_host((hoNDArray<float_complext>*)cm->getObjectPtr());

	GDEBUG("Result size: %i %i %i \n",result->get_size(0),result->get_size(1),result->get_size(2));

	m->getObjectPtr()->matrix_size[0] = img_dims_[0];
	m->getObjectPtr()->matrix_size[1] = img_dims_[1];
	m->getObjectPtr()->matrix_size[2] = img_dims_[2];
	m->getObjectPtr()->channels       = 1;
	m->getObjectPtr()->image_index    = 1;
	m->getObjectPtr()->data_type = ISMRMRD::ISMRMRD_CXFLOAT;

	if (!this->next()->putq(m)){
		GDEBUG("Failed to put image on que");
		return GADGET_FAIL;
	}

	return GADGET_OK;
}

  GADGET_FACTORY_DECLARE(CSIGadget)

} /* namespace Gadgetron */


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013 MetaScale SAS
//
//          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_META_REGION_HPP_INCLUDED
#define BOOST_SIMD_META_REGION_HPP_INCLUDED

#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd
{
  /*!
    @brief Static definition of region of interest

    Defines a compile-time region of interest as a series of 1D or 2D offsets
    around a central value.

    @tparam I0 .
    @tparam I1 .
    @tparam J0 .
    @tparam J1 .
  **/
  template<int I0, int I1, int J0 = 0, int J1 = 0>
  struct region_
  {
    // Region corners
    static const int w_min    = I0;
    static const int w_max    = I1;
    static const int h_min    = J0;
    static const int h_max    = J1;

    // Region metric
    static const int width    = I1 - I0 + 1;
    static const int height   = J1 - J0 + 1;
    static const int surface  = width*height;
    static const int ndims    = height ? 2 : 1;
  };

  template<int I0, int I1> BOOST_FORCEINLINE region_<I0,I1> around()
  {
    return region_<I0,I1>();
  }
} }

#endif


#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor_shape.h"
#include "tensorflow/core/platform/default/logging.h"
#include "tensorflow/core/framework/shape_inference.h"

#include <Eigen/Core>

#include "celerite.h"

using namespace tensorflow;

REGISTER_OP("CeleriteFactorGrad")
  .Attr("T: {float, double}")
  .Input("u: T")
  .Input("p: T")
  .Input("d: T")
  .Input("w: T")
  .Input("s: T")
  .Input("bd: T")
  .Input("bw: T")
  .Output("ba: T")
  .Output("bu: T")
  .Output("bv: T")
  .Output("bp: T")
  .SetShapeFn([](shape_inference::InferenceContext* c) {

    shape_inference::ShapeHandle u, p, d, w, s, bd, bw;

    TF_RETURN_IF_ERROR(c->WithRank(c->input(0), 2, &u));
    TF_RETURN_IF_ERROR(c->WithRank(c->input(1), 2, &p));
    TF_RETURN_IF_ERROR(c->WithRank(c->input(2), 1, &d));
    TF_RETURN_IF_ERROR(c->WithRank(c->input(3), 2, &w));
    TF_RETURN_IF_ERROR(c->WithRank(c->input(4), 2, &s));
    TF_RETURN_IF_ERROR(c->WithRank(c->input(5), 1, &bd));
    TF_RETURN_IF_ERROR(c->WithRank(c->input(6), 2, &bw));

    TF_RETURN_IF_ERROR(c->Merge(u, w, &u));
    TF_RETURN_IF_ERROR(c->Merge(u, bw, &u));
    TF_RETURN_IF_ERROR(c->Merge(d, bd, &d));

    c->set_output(0, c->input(2));
    c->set_output(1, c->input(0));
    c->set_output(2, c->input(0));
    c->set_output(3, c->input(1));

    return Status::OK();
  });

template <typename T>
class CeleriteFactorGradOp : public OpKernel {
 public:
  explicit CeleriteFactorGradOp(OpKernelConstruction* context) : OpKernel(context) {}

  void Compute(OpKernelContext* context) override {
    typedef Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, 1>> c_vector_t;
    typedef Eigen::Map<const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> c_matrix_t;
    typedef Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, 1>> vector_t;
    typedef Eigen::Map<Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> matrix_t;

    const Tensor& U_t = context->input(0);
    const Tensor& P_t = context->input(1);
    const Tensor& d_t = context->input(2);
    const Tensor& W_t = context->input(3);
    const Tensor& S_t = context->input(4);
    const Tensor& bd_t = context->input(5);
    const Tensor& bW_t = context->input(6);

    OP_REQUIRES(context, U_t.dims() == 2, errors::InvalidArgument("U should be a matrix"));
    int64 N = U_t.dim_size(0),
          J = U_t.dim_size(1);

    OP_REQUIRES(context, ((P_t.dims() == 2) &&
                          (P_t.dim_size(0) == N-1) &&
                          (P_t.dim_size(1) == J)),
          errors::InvalidArgument("P should have shape (N-1, J)"));

    OP_REQUIRES(context, ((d_t.dims() == 1) && (d_t.dim_size(0) == N)),
        errors::InvalidArgument("d should have shape (N)"));

    OP_REQUIRES(context, ((W_t.dims() == 2) &&
                          (W_t.dim_size(0) == N) &&
                          (W_t.dim_size(1) == J)),
          errors::InvalidArgument("W should have shape (N, J)"));

    OP_REQUIRES(context, ((S_t.dims() == 2) &&
                          (S_t.dim_size(0) == N) &&
                          (S_t.dim_size(1) == J*J)),
          errors::InvalidArgument("S should have shape (N, J*J)"));

    OP_REQUIRES(context, ((bd_t.dims() == 1) && (bd_t.dim_size(0) == N)),
        errors::InvalidArgument("bd should have shape (N)"));

    OP_REQUIRES(context, ((bW_t.dims() == 2) &&
                          (bW_t.dim_size(0) == N) &&
                          (bW_t.dim_size(1) == J)),
          errors::InvalidArgument("bW should have shape (N, J)"));

    const auto U = c_matrix_t(U_t.template flat<T>().data(), N, J);
    const auto P = c_matrix_t(P_t.template flat<T>().data(), N-1, J);
    const auto d = c_vector_t(d_t.template flat<T>().data(), N);
    const auto W = c_matrix_t(W_t.template flat<T>().data(), N, J);
    const auto S = c_matrix_t(S_t.template flat<T>().data(), N, J*J);
    const auto bd = c_vector_t(bd_t.template flat<T>().data(), N);
    const auto bW = c_matrix_t(bW_t.template flat<T>().data(), N, J);

    // Create the outputs
    Tensor* ba_t = NULL;
    Tensor* bU_t = NULL;
    Tensor* bV_t = NULL;
    Tensor* bP_t = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(0, TensorShape({N}), &ba_t));
    OP_REQUIRES_OK(context, context->allocate_output(1, TensorShape({N, J}), &bU_t));
    OP_REQUIRES_OK(context, context->allocate_output(2, TensorShape({N, J}), &bV_t));
    OP_REQUIRES_OK(context, context->allocate_output(3, TensorShape({N-1, J}), &bP_t));

    auto ba = vector_t(ba_t->template flat<T>().data(), N);
    auto bU = matrix_t(bU_t->template flat<T>().data(), N, J);
    auto bV = matrix_t(bV_t->template flat<T>().data(), N, J);
    auto bP = matrix_t(bP_t->template flat<T>().data(), N-1, J);

    bU.setZero();
    bP.setZero();
    ba = bd;
    bV = bW;
    celerite::factor_grad(U, P, d, W, S, bU, bP, ba, bV);
  }
};

#define REGISTER_KERNEL(type)                                              \
  REGISTER_KERNEL_BUILDER(                                                 \
      Name("CeleriteFactorGrad").Device(DEVICE_CPU).TypeConstraint<type>("T"), \
      CeleriteFactorGradOp<type>)

REGISTER_KERNEL(float);
REGISTER_KERNEL(double);

#undef REGISTER_KERNEL


#pragma once

#include "implot.h" // NOLINT
#include <mutex>
#include <string>
#include <vector>
#include <set>
#include <unordered_set>
#include <boost/algorithm/string/join.hpp>
#include <rclcpp/time.hpp>
#include "quickplot/plot.hpp"

namespace quickplot
{

struct PlotViewOptions
{
  bool use_sim_time;
  rclcpp::Time t_start;
  rclcpp::Time t_end;
};

ImPlotPoint circular_buffer_get_item(void * data, int idx)
{
  auto buffer = static_cast<CircularBuffer::const_iterator *>(data);
  return buffer->operator[](idx);
}

struct PlotViewResult
{
  bool displayed;
  float time_scale_delta;

  PlotViewResult()
  : displayed(false), time_scale_delta(0.0f) {}
};

PlotViewResult BeginPlotView(const char * id, Plot & plot, const PlotViewOptions & options)
{
  for (ImPlotYAxis a = 0; a < static_cast<ImPlotYAxis>(plot.axes.size()); a++) {
    const auto & axis_config = plot.axes[a];
    ImPlot::SetNextPlotLimitsY(
      axis_config.y_min, axis_config.y_max,
      ImGuiCond_Once, a);
  }
  std::vector<double> history_tick_values_;
  std::vector<std::string> history_tick_labels_;

  size_t n_ticks = static_cast<size_t>((options.t_end - options.t_start).seconds());
  history_tick_values_.resize(n_ticks);
  if (n_ticks != history_tick_labels_.size()) {
    history_tick_labels_.resize(n_ticks);
    for (size_t i = 0; i < n_ticks; i++) {
      history_tick_labels_[i] =
        std::to_string(-static_cast<int>(n_ticks) + static_cast<int>(i));
    }
  }
  for (size_t i = 0; i < n_ticks; i++) {
    history_tick_values_[i] = options.t_end.seconds() - static_cast<int>(n_ticks) + i;
  }

  std::vector<char const *> tick_cstrings;
  tick_cstrings.reserve(history_tick_labels_.size());
  for (const auto & labels : history_tick_labels_) {
    tick_cstrings.push_back(const_cast<char *>(labels.c_str()));
  }

  auto start_sec = options.t_start.seconds();
  auto end_sec = options.t_end.seconds();
  ImPlot::SetNextPlotLimitsX(start_sec, end_sec, ImGuiCond_Always);
  ImPlot::SetNextPlotTicksX(
    history_tick_values_.data(),
    history_tick_values_.size(), tick_cstrings.data());

  std::stringstream x_label;
  x_label << "t (header.stamp sec)";
  if (options.use_sim_time) {
    x_label << " use_sim_time:=true";
  }

  std::unordered_set<std::string> time_warned_topics;
  PlotViewResult result;
  if (ImPlot::BeginPlot(
      id, x_label.str().c_str(), nullptr, ImVec2(-1, -1),
      (plot.axes.size() >= 2 ? ImPlotFlags_YAxis2 : 0) |
      (plot.axes.size() >= 3 ? ImPlotFlags_YAxis3 : 0) | ImPlotFlags_NoTitle))
  {
    if (ImPlot::IsPlotXAxisHovered()) {
      ImGui::BeginTooltip();
      ImGui::Text("now: %.2f", options.t_end.seconds());
      ImGui::EndTooltip();

      result.time_scale_delta = ImGui::GetIO().MouseWheel;
    }
    result.displayed = true;
  }
  return result;
}

bool PlotSeriesPopup(const std::string & id)
{
  bool removed = false;
  if (ImPlot::BeginLegendPopup(id.c_str())) {
    if (ImGui::Button("remove")) {
      removed = true;
    }
    ImPlot::EndLegendPopup();
  }
  return removed;
}

void PlotSource(const std::string & id, const ActiveDataSource & source)
{
  auto data = source.data->data();
  auto it = data->begin();
  ImPlot::PlotLineG(
    id.c_str(),
    &circular_buffer_get_item,
    &it,
    data->size());
}

void PlotSourceStddev(
  const std::string & id, const ActiveDataSource & source,
  const ActiveDataSource & stddev)
{
  auto stddev_data = stddev.data->data();
  auto source_data = source.data->data();

  std::vector<double> times(source_data->size());
  std::vector<double> lower(source_data->size());
  std::vector<double> upper(source_data->size());

  auto stddev_vals = sync_right(
    source_data->begin(), source_data->end(),
    stddev_data->begin(), stddev_data->end(), 0.0);
  assert(stddev_vals.size() == source_data->size());

  auto source_it = source_data->begin();
  for (size_t i = 0; i < lower.size(); i++) {
    ImPlotPoint data_val = source_it[i];
    times[i] = data_val.x;
    lower[i] = data_val.y - stddev_vals[i];
    upper[i] = data_val.y + stddev_vals[i];
  }

  ImPlot::PushStyleVar(ImPlotStyleVar_FillAlpha, 0.25f);
  ImPlot::PlotShaded(id.c_str(), times.data(), lower.data(), upper.data(), times.size());
  ImPlot::PopStyleVar();
}

static const char * UninitializedText = "data not received yet";
static const char * MessageTypeUnavailableText = "failed to load message type";
static const char * InvalidMemberText = "invalid message member";
static const char * TimeStampOutOfRangeText =
  "all samples are outside of the x range; if you are using sim time set use_sim_time:=true when launching quickplot";

const char * get_error_message(DataSourceError error)
{
  if (error == DataSourceError::None) {
    return UninitializedText;
  }
  if (error == DataSourceError::MessageTypeUnavailable) {
    return MessageTypeUnavailableText;
  }
  if (error == DataSourceError::InvalidMember) {
    return InvalidMemberText;
  }
  return nullptr;
}

const char * get_warning_message(DataWarning warning)
{
  if (warning == DataWarning::TimeStampOutOfRange) {
    return TimeStampOutOfRangeText;
  }
  return nullptr;
}

void PlotSeriesError(
  const TimeSeries & series, const char * message,
  const PlotViewOptions & plot_opts)
{
  // plot empty line to show the item in legend, even though it is not received yet
  // this allows the user to remove the source from the list
  ImPlot::HideNextItem(true, ImGuiCond_Always);
  ImPlot::PlotLine(series.id.c_str(), static_cast<float *>(nullptr), 0);
  auto warning_color = ImPlot::GetLastItemColor();

  auto start_sec = plot_opts.t_start.seconds();
  auto end_sec = plot_opts.t_end.seconds();

  auto warn_x_pos = (end_sec - start_sec) / 2.0;
  auto warn_y_pos = 0.5;
  auto warn_offset = ImVec2(0, 0);

  ImPlot::AnnotateClamped(
    warn_x_pos, warn_y_pos, warn_offset, warning_color, "[%s]: %s",
    series.topic_name().c_str(), message);
}

std::optional<SeriesPayload> PlotView(Plot & plot, const PlotViewOptions & plot_opts)
{
  std::optional<SeriesPayload> result;
  for (ImPlotYAxis a = 0; a < static_cast<ImPlotYAxis>(plot.axes.size()); a++) {
    for (auto series_it = plot.series.begin(); series_it != plot.series.end(); ) {
      auto axis = series_it->second;
      if (axis != a) {
        // ensure series occurs in this plot axis
        ++series_it;
        continue;
      }
      const auto & series = series_it->first;

      // set axis just before matching source was found, to ensure axis is only displayed
      // if corresponding source exists
      ImPlot::SetPlotYAxis(a);

      // display either data or detected errors related to the data
      std::visit(
        overloaded {
          [series, &plot_opts](const ActiveDataSource & active) {
            if (active.warning == DataWarning::None) {
              ImPlot::HideNextItem(false, ImGuiCond_Always);
              PlotSource(series.id, active);

              auto stddev_active = std::get_if<ActiveDataSource>(&series.stddev_source);
              if (stddev_active) {
                PlotSourceStddev(series.id, active, *stddev_active);
              }
            } else {
              PlotSeriesError(series, get_warning_message(active.warning), plot_opts);
            }
          },
          [series, &plot_opts](const SourceInfo & descriptor) {
            PlotSeriesError(series, get_error_message(descriptor.error), plot_opts);
          }
        }, series.source);

      if (PlotSeriesPopup(series.id)) {
        series_it = plot.series.erase(series_it);
      } else {
        ++series_it;
      }
    }
    auto & axis_config = plot.axes[a];
    // store the current plot limits, in case they were changed via user input
    auto limits = ImPlot::GetPlotLimits(a);
    axis_config.y_min = limits.Y.Min;
    axis_config.y_max = limits.Y.Max;
  }

  if (ImPlot::BeginDragDropTarget()) {
    if (const ImGuiPayload * payload = ImGui::AcceptDragDropPayload("topic_member")) {
      result = SeriesPayload {
        .member = *static_cast<MemberPayload *>(payload->Data),
        .axis = ImPlotYAxis_1,
      };
    }
    ImPlot::EndDragDropTarget();
  }
  for (auto axis : {ImPlotYAxis_1, ImPlotYAxis_2}) {
    if (ImPlot::BeginDragDropTargetY(axis)) {
      if (const ImGuiPayload * payload = ImGui::AcceptDragDropPayload("topic_member")) {
        result = SeriesPayload {
          .member = *static_cast<MemberPayload *>(payload->Data),
          .axis = axis,
        };
      }
      ImPlot::EndDragDropTarget();
    }
  }
  return result;
}

void EndPlotView()
{
  ImPlot::EndPlot();
}

} // namespace quickplot


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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_IEEE_FUNCTIONS_GENERIC_SATURATE_AT_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_GENERIC_SATURATE_AT_HPP_INCLUDED

#include <boost/simd/ieee/functions/saturate_at.hpp>
#include <boost/dispatch/functor/meta/make_functor.hpp>
#include <boost/simd/include/functions/simd/unary_minus.hpp>
#include <boost/simd/include/functions/simd/min.hpp>
#include <boost/simd/include/functions/simd/max.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT_G        ( boost::simd::tag::saturate_at_<Tag>
                                    , tag::cpu_
                                    , (A0)(Tag)
                                    , (generic_<signed_<A0> >)
                                   )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typename dispatch::make_functor<Tag, A0>::type callee;
      const A0 z = callee( dispatch::meta::as_<A0>() );
      return min(z, max(unary_minus(z), a0));
    }
  };

  BOOST_DISPATCH_IMPLEMENT_G        ( boost::simd::tag::saturate_at_<Tag>
                                    , tag::cpu_
                                    , (A0)(Tag)
                                    , (generic_<unsigned_<A0> >)
                                   )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typename dispatch::make_functor<Tag, A0>::type callee;
      return min(a0, callee( dispatch::meta::as_<A0>() ));
    }
  };
} } }

#endif


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

#include "pme/id.h"
#include "pme/ic3/ic3_solver.h"
#include "pme/util/proof_checker.h"
#include "pme/util/cardinality_constraint.h"
#include "pme/engine/transition_relation.h"
#include "pme/engine/debug_transition_relation.h"

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

#include <algorithm>

using namespace PME;
using namespace PME::IC3;

struct IC3Fixture
{
    aiger * aig;
    ExternalID l0, l1, l2, l3, a0, a1, a2, o0;
    VariableManager vars;
    InductiveTrace trace;
    std::unique_ptr<TransitionRelation> tr;
    std::unique_ptr<DebugTransitionRelation> debug_tr;
    std::unique_ptr<IC3Solver> solver;

    IC3Fixture()
    {
        aig = aiger_init();

        l0 = 2;
        l1 = 4;
        l2 = 6;
        l3 = 8;
        a0 = 10;
        a1 = 12;
        a2 = 14;

        // l0' = l3
        aiger_add_latch(aig, l0, l3, "l0");

        // l1' = l0
        aiger_add_latch(aig, l1, l0, "l1");

        // l2' = l1
        aiger_add_latch(aig, l2, l1, "l2");

        // l3' = l2
        aiger_add_latch(aig, l3, l2, "l3");

        // o0 = l3 & ~l2 & ~l1 & l0
        aiger_add_and(aig, a0, l3, aiger_not(l2));
        aiger_add_and(aig, a1, aiger_not(l1), l0);
        aiger_add_and(aig, a2, a1, a0);
        aiger_add_output(aig, a2, "o0");
        o0 = a2;

        tr.reset(new TransitionRelation(vars, aig));
        debug_tr.reset(new DebugTransitionRelation(vars, aig));
        prepareSolver();
    }

    void setInit(ID latch, ID val)
    {
        tr->setInit(latch, val);
    }

    void prepareSolver(TransitionRelation & tr_to_use)
    {
        solver.reset(new IC3Solver(vars, tr_to_use));
    }

    void prepareSolver()
    {
        prepareSolver(*tr);
    }

    ~IC3Fixture()
    {
        aiger_reset(aig);
        aig = nullptr;
    }
};

BOOST_AUTO_TEST_CASE(safe)
{
    IC3Fixture f;

    ID l0 = f.tr->toInternal(f.l0);
    ID l1 = f.tr->toInternal(f.l1);
    ID l2 = f.tr->toInternal(f.l2);
    ID l3 = f.tr->toInternal(f.l3);

    f.setInit(l0, ID_FALSE);
    f.setInit(l1, ID_FALSE);
    f.setInit(l2, ID_FALSE);
    f.setInit(l3, ID_FALSE);

    f.prepareSolver();

    SafetyResult result = f.solver->prove();
    BOOST_CHECK_EQUAL(result.result, SAFE);

    ProofChecker pc(*f.tr, result.proof);
    BOOST_CHECK(pc.checkProof());
}

BOOST_AUTO_TEST_CASE(trivial_unsafe)
{
    IC3Fixture f;

    ID l0 = f.tr->toInternal(f.l0);
    ID l1 = f.tr->toInternal(f.l1);
    ID l2 = f.tr->toInternal(f.l2);
    ID l3 = f.tr->toInternal(f.l3);

    f.setInit(l0, ID_TRUE);
    f.setInit(l1, ID_FALSE);
    f.setInit(l2, ID_FALSE);
    f.setInit(l3, ID_TRUE);

    f.prepareSolver();

    SafetyResult result = f.solver->prove();
    BOOST_CHECK_EQUAL(result.result, UNSAFE);

    // Check the counter-example
    SafetyCounterExample cex = result.cex;
    BOOST_CHECK(cex.size() == 1);

    Cube actual, expected;
    expected = {l0, negate(l1), negate(l2), l3};
    actual = cex[0].state;
    std::sort(expected.begin(), expected.end());
    std::sort(actual.begin(), actual.end());
    BOOST_CHECK(expected == actual);
}

BOOST_AUTO_TEST_CASE(unsafe)
{
    IC3Fixture f;

    ID l0 = f.tr->toInternal(f.l0);
    ID l1 = f.tr->toInternal(f.l1);
    ID l2 = f.tr->toInternal(f.l2);
    ID l3 = f.tr->toInternal(f.l3);

    f.setInit(l0, ID_FALSE);
    f.setInit(l1, ID_TRUE);
    f.setInit(l2, ID_TRUE);
    f.setInit(l3, ID_FALSE);

    f.prepareSolver();

    SafetyResult result = f.solver->prove();
    BOOST_CHECK_EQUAL(result.result, UNSAFE);

    std::vector<Cube> expected_states;
    expected_states.push_back({negate(l0), l1, l2, negate(l3)});
    expected_states.push_back({negate(l0), negate(l1), l2, l3});
    // Last state is the property violation and is empty
    expected_states.push_back({});

    // Check the counter-example
    SafetyCounterExample cex = result.cex;
    BOOST_CHECK(cex.size() == expected_states.size());

    for (size_t i = 0; i < expected_states.size(); ++i)
    {
        Cube actual, expected;
        expected = expected_states[i];
        actual = cex[i].state;
        BOOST_CHECK(cex[i].inputs.empty());
        std::sort(expected.begin(), expected.end());
        std::sort(actual.begin(), actual.end());
        BOOST_CHECK(expected == actual);
    }
}

BOOST_AUTO_TEST_CASE(lemma_access)
{
    IC3Fixture f;

    ID l0 = f.tr->toInternal(f.l0);
    ID l1 = f.tr->toInternal(f.l1);
    ID l2 = f.tr->toInternal(f.l2);
    ID l3 = f.tr->toInternal(f.l3);

    f.solver->addLemma({negate(l0)}, 1);
    f.solver->addLemma({negate(l1)}, 2);
    f.solver->addLemma({negate(l2)}, 2);
    f.solver->addLemma({negate(l3)}, 2);

    std::vector<Cube> f1 = f.solver->getFrameCubes(1);
    std::vector<Cube> f2 = f.solver->getFrameCubes(2);
    std::vector<Cube> f3 = f.solver->getFrameCubes(3);
    std::vector<Cube> finf = f.solver->getFrameCubes(LEVEL_INF);

    BOOST_CHECK_EQUAL(f1.size(), 1);
    BOOST_CHECK_EQUAL(f2.size(), 3);
    BOOST_CHECK_EQUAL(f3.size(), 0);
    BOOST_CHECK_EQUAL(finf.size(), 0);

    std::vector<Cube> expected = {{negate(l0)}};
    BOOST_CHECK(f1 == expected);

    expected = {{negate(l1)}, {negate(l2)}, {negate(l3)}};
    std::sort(expected.begin(), expected.end());
    std::sort(f2.begin(), f2.end());

    BOOST_CHECK(f2 == expected);

    f.solver->addLemma({negate(l0)}, LEVEL_INF);
    f.solver->addLemma({negate(l1)}, LEVEL_INF);
    f.solver->addLemma({negate(l2)}, LEVEL_INF);
    f.solver->addLemma({negate(l3)}, LEVEL_INF);

    f1 = f.solver->getFrameCubes(1);
    f2 = f.solver->getFrameCubes(2);
    f3 = f.solver->getFrameCubes(3);
    finf = f.solver->getFrameCubes(LEVEL_INF);

    BOOST_CHECK_EQUAL(f1.size(), 0);
    BOOST_CHECK_EQUAL(f2.size(), 0);
    BOOST_CHECK_EQUAL(f3.size(), 0);
    BOOST_CHECK_EQUAL(finf.size(), 4);
}

BOOST_AUTO_TEST_CASE(complex_init_state)
{
    IC3Fixture f;

    f.prepareSolver(*f.debug_tr);

    // A cardinality constraint over the debug latches
    TotalizerCardinalityConstraint cardinality(f.vars);
    std::vector<ID> debug_latches(f.debug_tr->begin_latches(), f.debug_tr->end_latches());
    for (ID id : debug_latches)
    {
        cardinality.addInput(id);
    }
    cardinality.setCardinality(2);

    ClauseVec cnf = cardinality.CNFize();

    for (const Clause & cls : cnf)
    {
        f.solver->restrictInitialStates(cls);
    }

    Cube leq1 = cardinality.assumeLEq(1);
    for (ID id : leq1)
    {
        Clause cls = {id};
        f.solver->restrictInitialStates(cls);
    }

    f.solver->initialStatesRestricted();

    SafetyResult result = f.solver->prove();
    BOOST_CHECK_EQUAL(result.result, UNSAFE);
}

BOOST_AUTO_TEST_CASE(clear_restrictions)
{
    IC3Fixture f;

    f.prepareSolver(*f.debug_tr);

    std::vector<ID> debug_latches(f.debug_tr->begin_latches(), f.debug_tr->end_latches());
    for (ID id : debug_latches)
    {
        Clause cls = {negate(id)};
        f.solver->restrictInitialStates(cls);
    }
    f.solver->initialStatesRestricted();

    SafetyResult result = f.solver->prove();
    BOOST_CHECK_EQUAL(result.result, SAFE);

    f.solver->clearRestrictions();
    f.solver->initialStatesExpanded();

    result = f.solver->prove();
    BOOST_CHECK_EQUAL(result.result, UNSAFE);
}


#include <string>

#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <boost/program_options.hpp>
#include <boost/progress.hpp>

#include "point_defs.h"
#include <pcl/io/pcd_io.h>
#include <pcl/common/transforms.h>

#include <octomap/octomap.h>
#include <octomap/Pointcloud.h>
#include <octomap/OccupancyOcTreeBase.h>

#include "utils/path_utils.h"
#include "utils/cloud_utils.h"
#include "utils/hdf_utils.h"

#include "parameters.h"

namespace po = boost::program_options;
namespace fs = boost::filesystem;

struct Options
{
    bool single;
    bool debug;

    po::options_description desc;
};

int options(int ac, char ** av, Options& opts)
{
  // Declare the supported options.
  po::options_description desc = opts.desc;
  desc.add_options()("help", "Produce help message.");
  desc.add_options()
    ("single", po::bool_switch(&opts.single)->default_value(false), "export octomaps from individual scans")
    ("debug", po::bool_switch(&opts.debug)->default_value(false), "debug flag")
    ;
  po::variables_map vm;
  po::store(po::parse_command_line(ac, av, desc), vm);

  if (vm.count("help"))
  {
    std::cout << desc << std::endl;
    return 1;
  }

  po::notify(vm);

  return 0;
}

template <typename PointT>
void pcl_to_octomap(boost::shared_ptr<pcl::PointCloud<PointT> > src_cloud, octomap::Pointcloud& octomap_cloud)
{
    octomap_cloud.clear();
    octomap::point3d octopt;
    BOOST_FOREACH(PointT pt, src_cloud->points)
    {
        octopt(0) = pt.x;
        octopt(1) = pt.y;
        octopt(2) = pt.z;
        octomap_cloud.push_back(octopt);
    }
}

int main(int argc, char** argv)
{
    params().initialize();

    // Set up paths

    Options opts;
    if (options(argc, argv, opts))
        return 1;

    int count = params().count;

    std::string transforms_dir = params().h5_dir;

    std::vector<std::string> pcd_paths;
    // Read transforms
    std::vector<std::string> transform_paths;

    get_range_files(params().pcd_downsampled_dir, 0, 1, count, "%1%.pcd", pcd_paths);
    get_range_files(transforms_dir, 0, 1, count, "%1%.transform", transform_paths);

    assert(pcd_paths.size() == transform_paths.size());

    // Transformations

    octomap::point3d sensor_origin;
    MatrixXfRowMajor transform;
    Eigen::Vector3f  init_pos;

    // Build the octomap

    boost::progress_display show_progress(pcd_paths.size());

    boost::shared_ptr<octomap::OcTree> octree(new octomap::OcTree(params().octree_res));
    if (opts.single)
    {
        // TODO PARAM
        octree->setProbHit(1.0);
        octree->setProbMiss(0.4);
        octree->setClampingThresMax(1.0);
        octree->setClampingThresMin(0.0);
    }
    else
    {
        octree->setProbHit(params().prob_hit);
        octree->setProbMiss(params().prob_miss);
        octree->setClampingThresMax(params().clamping_thres_max);
        octree->setClampingThresMin(params().clamping_thres_min);
    }

    boost::shared_ptr<octomap::OcTree> octree_centered(new octomap::OcTree(*octree));

    for (int k = 0; k < pcd_paths.size(); k++)
    {
        // Load point cloud and transforms

        std::string pcd_path = pcd_paths[k];
        std::string transform_path = transform_paths[k];

        if (!fs::exists(transform_path))
        {
            std::cout << transform_path << " does not exist, quitting" << std::endl;
            break;
        }

        H5::H5File transform_file(transform_path, H5F_ACC_RDONLY);
        load_hdf_dataset(transform_file, "transform", transform, H5::PredType::NATIVE_FLOAT);
        Eigen::Matrix4f transform_copy(transform);

        Eigen::Vector4f imu_origin = transform_copy.block<4, 1>(0, 3);
        //Eigen::Vector4f lidar_origin = params().T_from_i_to_l * imu_origin;

        sensor_origin(0) = imu_origin(0);
        sensor_origin(1) = imu_origin(1);
        sensor_origin(2) = imu_origin(2);

        pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>());
        //std::cout << "Reading " << pcd_path << std::endl;
        load_cloud(pcd_path, src_cloud);

        octomap::Pointcloud octomap_cloud;
        pcl_to_octomap(src_cloud, octomap_cloud);
        if (opts.debug)
            std::cout << "cloud size:" <<  octomap_cloud.size() << std::endl;
        octree->insertPointCloud(octomap_cloud, sensor_origin);

        // Following is for octovis so the map is close to centered
        pcl::PointCloud<pcl::PointXYZ>::Ptr centered_cloud(new pcl::PointCloud<pcl::PointXYZ>());
        if (k == 0)
            init_pos = transform_copy.block<3, 1>(0, 3);
        sensor_origin(0) -= init_pos(0);
        sensor_origin(1) -= init_pos(1);
        sensor_origin(2) -= init_pos(2);
        Eigen::Matrix4f T = Eigen::Matrix4f::Identity();
        T(0, 3) -= init_pos(0);
        T(1, 3) -= init_pos(1);
        T(2, 3) -= init_pos(2);
        pcl::transformPointCloud(*src_cloud, *centered_cloud, T);

        pcl_to_octomap(centered_cloud, octomap_cloud);
        octree_centered->insertPointCloud(octomap_cloud, sensor_origin);

        if (opts.single)
        {
            // Write individual octomaps and clear
            octree->write(params().octomap_single_files[k]);
            octree->clear();
            octree_centered->clear();
        }

        ++show_progress;
    }

    if (!opts.single)
    {
        std::cout << "Writing octree of size " << octree->size() << std::endl;
        octree->write(params().octomap_file);
        octree_centered->write(params().centered_octomap_file);
    }

    return 0;
}


#include <iostream>

#include <boost/filesystem.hpp>

#include "pagmo/algorithms/de1220.hpp"
#include "pagmo/algorithms/pso.hpp"
#include "pagmo/algorithms/de.hpp"
#include "pagmo/algorithms/bee_colony.hpp"
#include "pagmo/algorithms/sga.hpp"
#include "pagmo/algorithms/sea.hpp"
#include "pagmo/algorithms/sade.hpp"
#include "pagmo/algorithms/simulated_annealing.hpp"
#include "pagmo/algorithms/bee_colony.hpp"
#include "pagmo/algorithms/cmaes.hpp"
#include "pagmo/algorithms/nlopt.hpp"
#include "pagmo/algorithms/nsga2.hpp"
#include "pagmo/algorithms/moead.hpp"
#include "pagmo/problems/griewank.hpp"
#include "pagmo/problems/schwefel.hpp"
#include "pagmo/problems/rosenbrock.hpp"
#include "pagmo/problems/cec2013.hpp"
#include "pagmo/problems/zdt.hpp"
#include "pagmo/island.hpp"
#include "pagmo/problem.hpp"
#include "Problems/himmelblau.h"

template< typename OutputStream, typename ScalarType,
          int NumberOfRows, int NumberOfColumns, int Options, int MaximumRows, int MaximumCols >
void writeValueToStream( OutputStream& stream, const Eigen::Matrix< ScalarType,
                         NumberOfRows, NumberOfColumns, Options,
                         MaximumRows, MaximumCols >& value,
                         const int precision, const std::string& delimiter,
                         const bool endLineAfterRow  = 0 )
{
    for ( int i = 0; i < value.rows( ); i++ )
    {
        for ( int j = 0; j < value.cols( ); j++ )
        {
            stream << delimiter << " "
                   << std::setprecision( precision ) << std::left
                   << std::setw( precision + 1 )
                   << value( i, j );

        }
        if( endLineAfterRow )
        {
            stream << std::endl;
        }
    }
    stream << std::endl;
}

template< typename ScalarType, int NumberOfRows, int NumberOfColumns >
void writeMatrixToFile( Eigen::Matrix< ScalarType, NumberOfRows, NumberOfColumns > matrixToWrite,
                        const std::string& outputFilename,
                        const int precisionOfMatrixEntries = 16,
                        const boost::filesystem::path& outputDirectory =
        "/home/dominic/Software/optimizationBundle/tudatBundle/tudatExampleApplications/libraryExamples/Pagmo2/bin/applications/",
                        const std::string& delimiter = "\t",
                        const std::string& header = "" )
{
    // Check if output directory exists; create it if it doesn't.
    if ( !boost::filesystem::exists( outputDirectory ) )
    {
        boost::filesystem::create_directories( outputDirectory );
    }

    // Open output file.
    std::string outputDirectoryAndFilename = outputDirectory.string( ) + "/" + outputFilename;
    std::ofstream outputFile_( outputDirectoryAndFilename.c_str( ) );

    // Write header
    outputFile_ << header;

    writeValueToStream( outputFile_, matrixToWrite, precisionOfMatrixEntries,
                        delimiter, true );

    outputFile_.close( );
}

pagmo::algorithm getAlgorithm( const int index )
{
    switch( index )
    {
    case 0:
    {
        pagmo::algorithm algo{ pagmo::nsga2( ) };
        return algo;
        break;
    }
    case 1:
    {
        pagmo::algorithm algo{ pagmo::moead( ) };
        return algo;
        break;
    }
    case 2:
    {
        pagmo::algorithm algo{ pagmo::ihs( ) };
        return algo;
        break;
    }
    }
}

void printPopulationToFile( const int problemIndex, const int iterationIndex,
                            const std::vector< pagmo::vector_double >& population,
                            const bool isFitness )
{
    Eigen::MatrixXd matrixToPrint( population.size( ), population.at( 0 ).size( ) );
    for( unsigned int i = 0; i < population.size( ); i++ )
    {
        for( unsigned int j = 0; j < population.at( 0 ).size( ); j++ )
        {
            matrixToPrint( i, j ) = population.at( i ).at( j );
        }
    }

    if( !isFitness )
    {
        writeMatrixToFile( matrixToPrint, "population_mo_" + std::to_string( problemIndex ) + "_" + std::to_string( iterationIndex ) + ".dat" );
    }
    else
    {
        writeMatrixToFile( matrixToPrint, "fitness_mo_" + std::to_string( problemIndex ) + "_" + std::to_string( iterationIndex ) + ".dat" );
    }
}

int main( )
{
    pagmo::random_device::set_seed( 12345 );

    for( unsigned int i = 0; i < 3; i++ )
    {
        pagmo::problem prob{ pagmo::zdt( 3, 2 ) };//my_problem( 0, 5, 0, 5) };

        pagmo::algorithm algo = getAlgorithm( i );

        pagmo::island isl = pagmo::island{ algo, prob, 64 };

        for( int j = 1; j <= 64; j++ )
        {

            isl.evolve( );

            //isl.get_population( ).get_f()

            printPopulationToFile( i, j, isl.get_population( ).get_x( ), false );
            printPopulationToFile( i, j, isl.get_population( ).get_f( ), true );

            //std::cout << "Best x: " << isl.get_population().champion_x()[0] << std::endl;
            //std::cout << "Best y: " << isl.get_population().champion_x()[1] << std::endl;
            while( isl.status()!=pagmo::evolve_status::idle )
                isl.wait();

        }
        std::cout<<i<<" done"<<std::endl;
    }
    return 0;
}


/*
 * 	cut_vessel.hpp
 *
 *	Author(s): Tamas D. Nagy
 *	Created on: 2018-06-04
 *
 *  Vessel cutting exercise
 *  with two arms.
 *
 */

#ifndef CUT_VESSEL_HPP
#define CUT_VESSEL_HPP

#include <iostream>
#include <sstream>
#include <vector>
#include <map>
#include <cmath>
#include <ros/ros.h>
#include <ros/package.h>
#include <cmath>
#include <limits>

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <geometry_msgs/Point.h>
#include <geometry_msgs/Transform.h>

#include <irob_utils/tool_pose.hpp>
#include <irob_utils/utils.hpp>
#include <irob_msgs/GraspObject.h>
#include <irob_utils/abstract_directions.hpp>
#include <irob_motion/surgeme_client.hpp>
#include <irob_vision_support/vision_client.hpp>

#include <irob_subtask_logic/autosurg_agent.hpp>

namespace saf {

class CutVessel : public AutosurgAgent {

protected:

  VisionClient<geometry_msgs::Transform, Eigen::Affine3d> vision;

  Eigen::Vector3d board_t;
  std::vector<Eigen::Vector3d> vessel_ends;
  double object_h;
  double object_d;

  void loadBoardDescriptor(ros::NodeHandle);
  Eigen::Affine3d poseToCameraFrame(const Eigen::Affine3d&, const Eigen::Affine3d&);
  Eigen::Affine3d poseToWorldFrame(const Eigen::Affine3d&, const Eigen::Affine3d&);

public:
  CutVessel(ros::NodeHandle, ros::NodeHandle, std::vector<std::string>);
  ~CutVessel();
  void doVesselCutting();

};

}

#endif // CUT_VESSEL_HPP


/*
 ******************************************************************************
 Project:      OWA EPANET
 Version:      2.2
 Module:       test_hydraulics.cpp
 Description:  Tests EPANET toolkit api functions
 Authors:      see AUTHORS
 Copyright:    see AUTHORS
 License:      see LICENSE
 Last Updated: 03/21/2019
 ******************************************************************************
*/

#include <boost/test/unit_test.hpp>
#include <boost/filesystem.hpp>

#include "test_toolkit.hpp"

BOOST_AUTO_TEST_SUITE (test_hydraulics)

BOOST_FIXTURE_TEST_CASE(test_solveH, FixtureOpenClose)
{
    error = EN_solveH(ph);
    BOOST_REQUIRE(error == 0);
}

BOOST_FIXTURE_TEST_CASE(test_hyd_step, FixtureOpenClose)
{
    int flag = 00;
    long t, tstep;

    error = EN_openH(ph);
    BOOST_REQUIRE(error == 0);

    error = EN_initH(ph, flag);
    BOOST_REQUIRE(error == 0);

    do {
        error = EN_runH(ph, &t);
        BOOST_REQUIRE(error == 0);

        error = EN_nextH(ph, &tstep);
        BOOST_REQUIRE(error == 0);

    } while (tstep > 0);

    error = EN_closeH(ph);
    BOOST_REQUIRE(error == 0);
}

BOOST_FIXTURE_TEST_CASE(test_hydr_save, FixtureOpenClose)
{
    error = EN_solveH(ph);
    BOOST_REQUIRE(error == 0);

    error = EN_saveH(ph);
    BOOST_REQUIRE(error == 0);

    error = EN_report(ph);
    BOOST_REQUIRE(error == 0);
}

BOOST_FIXTURE_TEST_CASE(test_hydr_savefile, FixtureOpenClose)
{
    error = EN_solveH(ph);
    BOOST_REQUIRE(error == 0);

    error = EN_savehydfile(ph, "test_savefile.hyd");
    BOOST_REQUIRE(error == 0);

    BOOST_CHECK(boost::filesystem::exists("test_savefile.hyd") == true);
}

BOOST_FIXTURE_TEST_CASE(test_hydr_usefile, FixtureOpenClose, * boost::unit_test::depends_on("test_hydraulics/test_hydr_savefile"))
{
    error = EN_usehydfile(ph, "test_savefile.hyd");
    BOOST_REQUIRE(error == 0);

    error = EN_solveQ(ph);
    BOOST_REQUIRE(error == 0);
}

BOOST_FIXTURE_TEST_CASE(test_pda_model, FixtureOpenClose)

{
    int error = 0;
    int index;
    double count, reduction;

    // Set Demand Multiplier to 10 to cause negative pressures
    error = EN_setoption(ph, EN_DEMANDMULT, 10);
    BOOST_REQUIRE(error == 0);

    // Run single period analysis
    error = EN_settimeparam(ph, EN_DURATION, 0);
    BOOST_REQUIRE(error == 0);

    // Solve hydraulics with default DDA option
    // which will return with neg. pressure warning code
    error = EN_solveH(ph);
    BOOST_REQUIRE(error == 6);

    // Check that 4 demand nodes have negative pressures
    error = EN_getstatistic(ph, EN_DEFICIENTNODES, &count);
    BOOST_REQUIRE(error == 0);
    BOOST_CHECK(count == 4);

    // Switch to PDA with pressure limits of 20 - 100 psi
    error = EN_setdemandmodel(ph, EN_PDA, 20, 100, 0.5);
    BOOST_REQUIRE(error == 0);

    // Solve hydraulics again
    error = EN_solveH(ph);
    BOOST_REQUIRE(error == 0);

    // Check that 6 nodes had demand reductions totaling 32.66%
    error = EN_getstatistic(ph, EN_DEFICIENTNODES, &count);
    BOOST_REQUIRE(error == 0);
    BOOST_CHECK(count == 6);
    error = EN_getstatistic(ph, EN_DEMANDREDUCTION, &reduction);
    BOOST_REQUIRE(error == 0);
    BOOST_CHECK(abs(reduction - 32.66) < 0.01);

    // Check that Junction 12 had full demand
    error = EN_getnodeindex(ph, (char *)"12", &index);
    BOOST_REQUIRE(error == 0);
    error = EN_getnodevalue(ph, index, EN_DEMANDDEFICIT, &reduction);
    BOOST_REQUIRE(error == 0);
    BOOST_CHECK(abs(reduction) < 0.01);

    // Check that Junction 21 had a demand deficit of 413.67
    error = EN_getnodeindex(ph, (char *)"21", &index);
    BOOST_REQUIRE(error == 0);
    error = EN_getnodevalue(ph, index, EN_DEMANDDEFICIT, &reduction);
    BOOST_REQUIRE(error == 0);
    BOOST_CHECK(abs(reduction - 413.67) < 0.01);
}

BOOST_FIXTURE_TEST_CASE(test_tank_overflow, FixtureOpenClose)

{
    int error = 0;
    int Nindex, Lindex;
    double level, spillage, spillage2, inflow;
    char testFile[] = "test_overflow.inp";

    // Get index of the tank and its inlet/outlet pipe
    error = EN_getnodeindex(ph, (char *)"2", &Nindex);
    BOOST_REQUIRE(error == 0);
    error = EN_getlinkindex(ph, (char *)"110", &Lindex);
    BOOST_REQUIRE(error == 0);

    // Set initial & maximum level to 130
    error = EN_setnodevalue(ph, Nindex, EN_TANKLEVEL, 130);
    BOOST_REQUIRE(error == 0);
    error = EN_setnodevalue(ph, Nindex, EN_MAXLEVEL, 130);
    BOOST_REQUIRE(error == 0);

    // Set duration to 1 hr
    error = EN_settimeparam(ph, EN_DURATION, 3600);
    BOOST_REQUIRE(error == 0);

    // Solve hydraulics with default of no tank spillage allowed
    error = EN_solveH(ph);
    BOOST_REQUIRE(error == 0);

    // Check that tank remains full
    error = EN_getnodevalue(ph, Nindex, EN_TANKLEVEL, &level);
    BOOST_REQUIRE(error == 0);
    BOOST_CHECK(abs(level - 130.0) < 0.0001);

    // Check that there is no spillage
    error = EN_getnodevalue(ph, Nindex, EN_DEMAND, &spillage);
    BOOST_REQUIRE(error == 0);
    BOOST_CHECK(abs(spillage) < 0.0001);

    // Check that inflow link is closed
    error = EN_getlinkvalue(ph, Lindex, EN_FLOW, &inflow);
    BOOST_REQUIRE(error == 0);
    BOOST_CHECK(abs(inflow) < 0.0001);

    // Turn tank overflow option on
    error = EN_setnodevalue(ph, Nindex, EN_CANOVERFLOW, 1);
    BOOST_REQUIRE(error == 0);

    // Solve hydraulics again
    error = EN_solveH(ph);
    BOOST_REQUIRE(error == 0);

    // Check that tank remains full
    error = EN_getnodevalue(ph, Nindex, EN_TANKLEVEL, &level);
    BOOST_REQUIRE(error == 0);
    BOOST_CHECK(abs(level - 130.0) < 0.0001);

    // Check that there is spillage equal to tank inflow
    // (inflow has neg. sign since tank is start node of inflow pipe)
    error = EN_getnodevalue(ph, Nindex, EN_DEMAND, &spillage);
    BOOST_REQUIRE(error == 0);
    BOOST_CHECK(spillage > 0.0001);
    error = EN_getlinkvalue(ph, Lindex, EN_FLOW, &inflow);
    BOOST_REQUIRE(error == 0);
    BOOST_CHECK(abs(-inflow - spillage) < 0.0001);

    // Save project to file and then close it
    error = EN_saveinpfile(ph, testFile);
    BOOST_REQUIRE(error == 0);
    error = EN_close(ph);
    BOOST_REQUIRE(error == 0);

    // Re-open saved file & run it
    error = EN_open(ph, testFile, DATA_PATH_RPT, "");
    BOOST_REQUIRE(error == 0);
    error = EN_solveH(ph);
    BOOST_REQUIRE(error == 0);

    // Check that tank spillage has same value as before
    error = EN_getnodevalue(ph, Nindex, EN_DEMAND, &spillage2);
    BOOST_REQUIRE(error == 0);
    BOOST_CHECK(abs(spillage - spillage2) < 0.0001);
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <boost/format.hpp>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/MagneticField.h>
#include <tf/LinearMath/Quaternion.h>

#include "modbus_rtu_master.h"

#define G 9.801

class HWT9053Driver
{
public:
  HWT9053Driver(ros::NodeHandle nh, ros::NodeHandle private_nh);
  ~HWT9053Driver();
  void loop();

private:
  ros::NodeHandle nh_, private_nh_;
  sensor_msgs::Imu imu_msg_;
  sensor_msgs::MagneticField mag_msg_;
  ros::Publisher imu_msg_pub_;
  ros::Publisher mag_msg_pub_;
  bool pub_mag_;
  int32_t baudrate_;
  std::string frame_id_;
  std::string sensor_com_;
  ModbusRTUMaster *sensor_;
};

HWT9053Driver::HWT9053Driver(ros::NodeHandle nh, ros::NodeHandle private_nh)
    : nh_(nh), private_nh_(private_nh)
{

  private_nh_.param<int>("baudrate", baudrate_, 115200);
  private_nh_.param<bool>("pub_mag_msg", pub_mag_, false);
  private_nh_.param<std::string>("frame_id", frame_id_, "imu_link");
  private_nh_.param<std::string>("sensor_com", sensor_com_, "/dev/ttyUSB0");

  sensor_ = new ModbusRTUMaster(sensor_com_, baudrate_);
  std::this_thread::sleep_for(std::chrono::milliseconds(100));

  imu_msg_pub_ = nh_.advertise<sensor_msgs::Imu>("imu/data", 10);
  if (pub_mag_)
  {
    mag_msg_pub_ = nh_.advertise<sensor_msgs::MagneticField>("imu/mag", 10);
  }
}

HWT9053Driver::~HWT9053Driver()
{
  free(sensor_);
  sensor_ = nullptr;
}

void HWT9053Driver::loop()
{
  uint16_t data[256] = {0};
  uint8_t ret = 0;

  // get Atmospheric humidity and temperature
  ret = sensor_->GetMultipleRegisters(0x50, 0x0034, 0x0010, data, 1);
  if (!ret)
    ROS_WARN("get Atmospheric humidity&temperature faild!!");
  else
  {
    //IMU
    imu_msg_.header.stamp = ros::Time::now();
    imu_msg_.header.frame_id = frame_id_;

    imu_msg_.linear_acceleration.x = int16_t(data[0]) / 32768.0 * 16.0 * G;
    imu_msg_.linear_acceleration.y = int16_t(data[1]) / 32768.0 * 16.0 * G;
    imu_msg_.linear_acceleration.z = int16_t(data[2]) / 32768.0 * 16.0 * G;
    imu_msg_.linear_acceleration_covariance[0] = 0.01;
    imu_msg_.linear_acceleration_covariance[4] = 0.01;
    imu_msg_.linear_acceleration_covariance[8] = 0.01;

    imu_msg_.angular_velocity.x = int16_t(data[3]) / 32768.0 * 2000.0;
    imu_msg_.angular_velocity.y = int16_t(data[4]) / 32768.0 * 2000.0;
    imu_msg_.angular_velocity.z = int16_t(data[5]) / 32768.0 * 2000.0;
    imu_msg_.angular_velocity_covariance[0] = 0.006;
    imu_msg_.angular_velocity_covariance[4] = 0.006;
    imu_msg_.angular_velocity_covariance[8] = 0.006;

    double ang_x = int32_t((data[10] << 16) | data[9]) / 1000.0 * 3.1415926 / 180.0;
    double ang_y = int32_t((data[12] << 16) | data[11]) / 1000.0 * 3.1415926 / 180.0;
    double ang_z = int32_t((data[14] << 16) | data[13]) / 1000.0 * 3.1415926 / 180.0;

    tf::Quaternion quate;
    quate.setRPY(ang_x, ang_y, ang_z);
    imu_msg_.orientation.x = quate.x();
    imu_msg_.orientation.y = quate.y();
    imu_msg_.orientation.z = quate.z();
    imu_msg_.orientation.w = quate.w();
    imu_msg_.orientation_covariance[0] = 0.001;
    imu_msg_.orientation_covariance[4] = 0.001;
    imu_msg_.orientation_covariance[8] = 0.001;

    imu_msg_pub_.publish(imu_msg_);

    //Magnetic Field
    if (pub_mag_)
    {
      mag_msg_.header.stamp = ros::Time::now();
      mag_msg_.header.frame_id = frame_id_;

      mag_msg_.magnetic_field.x = int16_t(data[6]);
      mag_msg_.magnetic_field.y = int16_t(data[7]);
      mag_msg_.magnetic_field.z = int16_t(data[8]);

      mag_msg_.magnetic_field_covariance[0] = 0.001;
      mag_msg_.magnetic_field_covariance[4] = 0.001;
      mag_msg_.magnetic_field_covariance[8] = 0.001;

      mag_msg_pub_.publish(mag_msg_);
    }
  }
}

int main(int argc, char *argv[])
{
  ros::init(argc, argv, "hwt9053_driver_node");
  ros::NodeHandle nh, private_nh("~");

  int publish_rate;
  private_nh.param<int>("publish_rate", publish_rate, 100);

  HWT9053Driver imu(nh, private_nh);

  ros::Rate r(publish_rate);

  while (ros::ok())
  {
    ros::spinOnce();
    imu.loop();
    r.sleep();
  }

  ROS_INFO("All finish");

  return 0;
}