#include <Configuration.h> // Package Path
#include <avatar_locomanipulation/enable_pinocchio_with_hpp_fcl.h> // Enable HPP FCL
// Multibody
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/multibody/geometry.hpp"
// Algorithms
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/geometry.hpp"
#include "pinocchio/algorithm/model.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/frames.hpp" // Jacobian Frame Computation
// Parsers
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/parsers/srdf.hpp"
// Spatial
#include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"

// Standard
#include <math.h>
#include <map>
#include <iostream>
#include <boost/shared_ptr.hpp>

int main(int argc, char ** argv){
	// models and geomModels
	pinocchio::Model robot_model, cart_model;
  pinocchio::GeometryModel robot_geomModel, cart_geomModel;
  // data and geomdata
  std::unique_ptr<pinocchio::Data> robot_data, cart_data;
  std::unique_ptr<pinocchio::GeometryData> robot_geomData, cart_geomData;

  // Define the Robot--------
	std::string filename = THIS_PACKAGE_PATH"models/valkyrie_simplified_collisions.urdf";
  std::string srdf_filename = THIS_PACKAGE_PATH"models/valkyrie_disable_collisions.srdf";
  std::string meshDir  = THIS_PACKAGE_PATH"../val_model/";
  // build model and geomModel
  pinocchio::urdf::buildModel(filename, pinocchio::JointModelFreeFlyer(),robot_model);
  pinocchio::urdf::buildGeom(robot_model, filename, pinocchio::COLLISION, robot_geomModel, meshDir );
  // Add and remove collision pairs
  robot_geomModel.addAllCollisionPairs();
  pinocchio::srdf::removeCollisionPairs(robot_model, robot_geomModel, srdf_filename, false);
  // Define the data and geomData
  robot_data = std::unique_ptr<pinocchio::Data>(new pinocchio::Data(robot_model));
	robot_geomData = std::unique_ptr<pinocchio::GeometryData>(new pinocchio::GeometryData(robot_geomModel));
	// Define the configuration of Robot
  Eigen::VectorXd q_robot;
  q_robot = Eigen::VectorXd::Zero(robot_model.nq);

  double theta = 0;//M_PI/4.0;
  Eigen::AngleAxis<double> aa(theta, Eigen::Vector3d(0.0, 0.0, 1.0));
  Eigen::Quaternion<double> init_quat(1.0, 0.0, 0.0, 0.0); //Initialized to remember the w component comes first
  init_quat = aa;

  q_robot[3] = init_quat.x(); q_robot[4] = init_quat.y(); q_robot[5] = init_quat.z(); q_robot[6] = init_quat.w(); // Set up the quaternion in q

  q_robot[2] = 1.0; // set z value to 1.0, this is the pelvis location

  q_robot[7 + robot_model.getJointId("leftHipPitch") - 2] = -0.3;
  q_robot[7 + robot_model.getJointId("rightHipPitch") - 2] = -0.3;
  q_robot[7 + robot_model.getJointId("leftKneePitch") - 2] = 0.6;
  q_robot[7 + robot_model.getJointId("rightKneePitch") - 2] = 0.6;
  q_robot[7 + robot_model.getJointId("leftAnklePitch") - 2] = -0.3;
  q_robot[7 + robot_model.getJointId("rightAnklePitch") - 2] = -0.3;

  q_robot[7 + robot_model.getJointId("rightShoulderPitch") - 2] = -0.2;
  q_robot[7 + robot_model.getJointId("rightShoulderRoll") - 2] = 1.1;
  q_robot[7 + robot_model.getJointId("rightElbowPitch") - 2] = 0.4;
  q_robot[7 + robot_model.getJointId("rightForearmYaw") - 2] = 1.5;

  q_robot[7 + robot_model.getJointId("leftShoulderPitch") - 2] = -0.2;
  q_robot[7 + robot_model.getJointId("leftShoulderRoll") - 2] = -1.1;
  q_robot[7 + robot_model.getJointId("leftElbowPitch") - 2] = -0.4;
  q_robot[7 + robot_model.getJointId("leftForearmYaw") - 2] = 1.5;

  // Define the Cart--------
	filename = THIS_PACKAGE_PATH"models/test_cart.urdf";
  meshDir  = THIS_PACKAGE_PATH"models/cart/";

  ::urdf::ModelInterfaceSharedPtr urdfTree = ::urdf::parseURDFFile(filename);
  pinocchio::urdf::buildModel(urdfTree, robot_model);

  // build model and geomModel
  // pinocchio::urdf::buildModel(filename,robot_model);
 //  pinocchio::urdf::buildGeom(robot_model, filename, pinocchio::COLLISION, robot_geomModel, meshDir );

 //  // Add and remove collision pairs
 //  robot_geomModel.addAllCollisionPairs();
 //  pinocchio::srdf::removeCollisionPairs(robot_model, robot_geomModel, srdf_filename, false);
 //  // Define the data and geomData
 //  robot_data = std::unique_ptr<pinocchio::Data>(new pinocchio::Data(robot_model));
	// robot_geomData = std::unique_ptr<pinocchio::GeometryData>(new pinocchio::GeometryData(robot_geomModel));
}


#define OFDIS_INTERNAL
#include <Eigen/Core>
#include "litiv/3rdparty/ofdis/refine_variational.hpp"

inline void local_image_delete(image_t *src) {
    image_delete(src);
}

inline void local_image_delete(color_image_t *src) {
    color_image_delete(src);
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
ofdis::VarRefClass<eInput,eOutput>::VarRefClass(const float * im_ao_in, const float * im_ao_dx_in, const float * im_ao_dy_in,
                                                const float * im_bo_in, const float * im_bo_dx_in, const float * im_bo_dy_in,
                                                const camparam* cpt_in,const camparam* cpo_in,const optparam* op_in, float *flowout) :
        cpt(cpt_in), cpo(cpo_in), op(op_in) {
    // initialize parameters
    tvparams.alpha = op->tv_alpha;
    tvparams.beta = 0.0f;  // for matching term, not needed for us
    tvparams.gamma = op->tv_gamma;
    tvparams.delta = op->tv_delta;
    tvparams.n_inner_iteration = op->tv_innerit * (cpt->curr_lv+1);
    tvparams.n_solver_iteration = op->tv_solverit;//5;
    tvparams.sor_omega = op->tv_sor;
    tvparams.tmp_quarter_alpha = 0.25f*tvparams.alpha;
    tvparams.tmp_half_gamma_over3 = tvparams.gamma*0.5f/3.0f;
    tvparams.tmp_half_delta_over3 = tvparams.delta*0.5f/3.0f;
    tvparams.tmp_half_beta = tvparams.beta*0.5f;
    float deriv_filter[3] = {0.0f, -8.0f/12.0f, 1.0f/12.0f};
    deriv = convolution_new(2, deriv_filter, 0);
    float deriv_filter_flow[2] = {0.0f, -0.5f};
    deriv_flow = convolution_new(1, deriv_filter_flow, 0);
    // copy flow initialization into FV structs
    static int noparam = (eOutput==ofdis::FlowOutput_OpticalFlow)?2:1; // only horizontal displacements for stereo depth
    std::vector<image_t*> flow_sep(noparam);
    for(int i = 0; i < noparam; ++i )
        flow_sep[i] = image_new(cpt->width,cpt->height);
    for(int iy = 0; iy < cpt->height; ++iy) {
        for(int ix = 0; ix<cpt->width; ++ix) {
            int i = iy*cpt->width+ix;
            int is = iy*flow_sep[0]->stride+ix;
            for(int j = 0; j<noparam; ++j)
                flow_sep[j]->c1[is] = flowout[i*noparam+j];
        }
    }
    // copy image data into FV structs
    InputImageType* im_ao, *im_bo;
    im_ao = (InputImageType*)((eInput==ofdis::FlowInput_RGB)?(void*)color_image_new(cpt->width,cpt->height):(void*)image_new(cpt->width,cpt->height));
    im_bo = (InputImageType*)((eInput==ofdis::FlowInput_RGB)?(void*)color_image_new(cpt->width,cpt->height):(void*)image_new(cpt->width,cpt->height));
    copyimage(im_ao_in, im_ao);
    copyimage(im_bo_in, im_bo);
    // call solver
    if(eOutput==ofdis::FlowOutput_OpticalFlow)
        RefLevelOF(flow_sep[0], flow_sep[1], im_ao, im_bo);
    else
        RefLevelDE(flow_sep[0], im_ao, im_bo);
    // copy flow result back
    for(int iy = 0; iy < cpt->height; ++iy) {
        for(int ix = 0; ix<cpt->width; ++ix) {
            int i = iy*cpt->width+ix;
            int is = iy*flow_sep[0]->stride+ix;
            for(int j = 0; j<noparam; ++j)
                flowout[i*noparam+j] = flow_sep[j]->c1[is];
        }
    }
    // free FV structs
    for(int i = 0; i < noparam; ++i)
        image_delete(flow_sep[i]);
    convolution_delete(deriv);
    convolution_delete(deriv_flow);
    local_image_delete(im_ao);
    local_image_delete(im_bo);
}

inline void local_image_copy_pixel(image_t *img_t, int i, const float* img_st) {
    img_t->c1[i] = (*img_st);
}

inline void local_image_copy_pixel(color_image_t *img_t, int i, const float*& img_st) {
    img_t->c1[i] = (*img_st);
    ++img_st; img_t->c2[i] =  (*img_st);
    ++img_st; img_t->c3[i] =  (*img_st);
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::VarRefClass<eInput,eOutput>::copyimage(const float* img, InputImageType* img_t) {
    // remove image padding, start at first valid pixel
    const float* img_st = img+((eInput==ofdis::FlowInput_RGB)?3:1)*(cpt->tmp_w+1)*(cpt->imgpadding);
    for(int yi = 0; yi<cpt->height; ++yi) {
        for(int xi = 0; xi<cpt->width; ++xi,++img_st) {
            local_image_copy_pixel(img_t,yi*img_t->stride+xi,img_st);
        }
        img_st += ((eInput==ofdis::FlowInput_RGB)?3:1)*2*cpt->imgpadding;
    }
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::VarRefClass<eInput,eOutput>::RefLevelOF(image_t *wx, image_t *wy, const InputImageType* im1, const InputImageType* im2) {
    int i_inner_iteration;
    int width  = wx->width;
    int height = wx->height;
    int stride = wx->stride;
    image_t *du = image_new(width,height), *dv = image_new(width,height), // the flow increment
            *mask = image_new(width,height), // mask containing 0 if a point goes outside image boundary, 1 otherwise
            *smooth_horiz = image_new(width,height), *smooth_vert = image_new(width,height), // horiz: (i,j) contains the diffusivity coeff. from (i,j) to (i+1,j)
            *uu = image_new(width,height), *vv = image_new(width,height), // flow plus flow increment
            *a11 = image_new(width,height), *a12 = image_new(width,height), *a22 = image_new(width,height), // system matrix A of Ax=b for each pixel
            *b1 = image_new(width,height), *b2 = image_new(width,height); // system matrix b of Ax=b for each pixel
    const auto lImageCreator = [&](){return (InputImageType*)((eInput==ofdis::FlowInput_RGB)?(void*)color_image_new(width,height):(void*)image_new(width,height));};
    InputImageType *w_im2 = lImageCreator(), // warped second image
                   *Ix = lImageCreator(), *Iy = lImageCreator(), *Iz = lImageCreator(), // first order derivatives
                   *Ixx = lImageCreator(), *Ixy = lImageCreator(), *Iyy = lImageCreator(), *Ixz = lImageCreator(), *Iyz = lImageCreator(); // second order derivatives
    // warp second image
    fdf::image_warp<eInput>(w_im2, mask, im2, wx, wy);
    // compute derivatives
    fdf::get_derivatives<eInput>(im1, w_im2, deriv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz);
    // erase du and dv
    image_erase(du);
    image_erase(dv);
    // initialize uu and vv
    memcpy(uu->c1,wx->c1,wx->stride*wx->height*sizeof(float));
    memcpy(vv->c1,wy->c1,wy->stride*wy->height*sizeof(float));
    // inner fixed point iterations
    for(i_inner_iteration=0; i_inner_iteration<tvparams.n_inner_iteration; i_inner_iteration++) {
        //  compute robust function and system
        fdf::compute_smoothness(smooth_horiz, smooth_vert, uu, vv, deriv_flow, tvparams.tmp_quarter_alpha );
        //compute_data_and_match(a11, a12, a22, b1, b2, mask, wx, wy, du, dv, uu, vv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz, desc_weight, desc_flow_x, desc_flow_y, tvparams.tmp_half_delta_over3, tvparams.tmp_half_beta, tvparams.tmp_half_gamma_over3);
        fdf::compute_data<eInput>(a11, a12, a22, b1, b2, mask, wx, wy, du, dv, uu, vv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz, tvparams.tmp_half_delta_over3, tvparams.tmp_half_beta, tvparams.tmp_half_gamma_over3);
        fdf::sub_laplacian(b1, wx, smooth_horiz, smooth_vert);
        fdf::sub_laplacian(b2, wy, smooth_horiz, smooth_vert);
        // solve system
    #ifdef WITH_OPENMP
        sor_coupled_slow_but_readable(du, dv, a11, a12, a22, b1, b2, smooth_horiz, smooth_vert, tvparams.n_solver_iteration, tvparams.sor_omega); // slower but parallelized
    #else
        sor_coupled(du, dv, a11, a12, a22, b1, b2, smooth_horiz, smooth_vert, tvparams.n_solver_iteration, tvparams.sor_omega);
    #endif
        // update flow plus flow increment
        int i;
        v4sf *uup = (v4sf*) uu->c1, *vvp = (v4sf*) vv->c1, *wxp = (v4sf*) wx->c1, *wyp = (v4sf*) wy->c1, *dup = (v4sf*) du->c1, *dvp = (v4sf*) dv->c1;
        for(i=0 ; i<height*stride/4 ; i++) {
            (*uup) = (*wxp) + (*dup);
            (*vvp) = (*wyp) + (*dvp);
            uup+=1; vvp+=1; wxp+=1; wyp+=1;dup+=1;dvp+=1;
        }
    }
    // add flow increment to current flow
    memcpy(wx->c1,uu->c1,uu->stride*uu->height*sizeof(float));
    memcpy(wy->c1,vv->c1,vv->stride*vv->height*sizeof(float));
    // free memory
    image_delete(du);
    image_delete(dv);
    image_delete(mask);
    image_delete(smooth_horiz);
    image_delete(smooth_vert);
    image_delete(uu);
    image_delete(vv);
    image_delete(a11);
    image_delete(a12);
    image_delete(a22);
    image_delete(b1);
    image_delete(b2);
    local_image_delete(w_im2);
    local_image_delete(Ix);
    local_image_delete(Iy);
    local_image_delete(Iz);
    local_image_delete(Ixx);
    local_image_delete(Ixy);
    local_image_delete(Iyy);
    local_image_delete(Ixz);
    local_image_delete(Iyz);
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::VarRefClass<eInput,eOutput>::RefLevelDE(image_t *wx, const InputImageType* im1, const InputImageType* im2) {
    int i_inner_iteration;
    int width  = wx->width;
    int height = wx->height;
    int stride = wx->stride;
    image_t *du = image_new(width,height), *wy_dummy = image_new(width,height), // the flow increment
            *mask = image_new(width,height), // mask containing 0 if a point goes outside image boundary, 1 otherwise
            *smooth_horiz = image_new(width,height), *smooth_vert = image_new(width,height), // horiz: (i,j) contains the diffusivity coeff. from (i,j) to (i+1,j)
            *uu = image_new(width,height), // flow plus flow increment
            *a11 = image_new(width,height), // system matrix A of Ax=b for each pixel
            *b1 = image_new(width,height); // system matrix b of Ax=b for each pixel
    image_erase(wy_dummy);
    const auto lImageCreator = [&](){return (InputImageType*)((eInput==ofdis::FlowInput_RGB)?(void*)color_image_new(width,height):(void*)image_new(width,height));};
    InputImageType *w_im2 = lImageCreator(), // warped second image
                   *Ix = lImageCreator(), *Iy = lImageCreator(), *Iz = lImageCreator(), // first order derivatives
                   *Ixx = lImageCreator(), *Ixy = lImageCreator(), *Iyy = lImageCreator(), *Ixz = lImageCreator(), *Iyz = lImageCreator(); // second order derivatives
    // warp second image
    fdf::image_warp<eInput>(w_im2, mask, im2, wx, wy_dummy);
    // compute derivatives
    fdf::get_derivatives<eInput>(im1, w_im2, deriv, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz);
    // erase du and dv
    image_erase(du);
    // initialize uu and vv
    memcpy(uu->c1,wx->c1,wx->stride*wx->height*sizeof(float));
    // inner fixed point iterations
    for(i_inner_iteration=0; i_inner_iteration<tvparams.n_inner_iteration; i_inner_iteration++) {
        //  compute robust function and system
        fdf::compute_smoothness(smooth_horiz, smooth_vert, uu, wy_dummy, deriv_flow, tvparams.tmp_quarter_alpha );
        fdf::compute_data_DE<eInput>(a11, b1, mask, wx, du, uu, Ix, Iy, Iz, Ixx, Ixy, Iyy, Ixz, Iyz, tvparams.tmp_half_delta_over3, tvparams.tmp_half_beta, tvparams.tmp_half_gamma_over3);
        fdf::sub_laplacian(b1, wx, smooth_horiz, smooth_vert);
        // solve system
        sor_coupled_slow_but_readable_DE(du, a11, b1, smooth_horiz, smooth_vert, tvparams.n_solver_iteration, tvparams.sor_omega);
        // update flow plus flow increment
        int i;
        v4sf *uup = (v4sf*) uu->c1, *wxp = (v4sf*) wx->c1, *dup = (v4sf*) du->c1;
        if(cpt->camlr==0) { // check if right or left camera, needed to truncate values above/below zero
            for(i=0; i<height*stride/4; i++) {
                (*uup) = __builtin_ia32_minps(   (*wxp) + (*dup)   ,  op->zero);
                uup+=1; wxp+=1; dup+=1;
            }
        }
        else {
            for(i=0 ; i<height*stride/4; i++) {
                (*uup) = __builtin_ia32_maxps(   (*wxp) + (*dup)   ,  op->zero);
                uup+=1; wxp+=1; dup+=1;
            }
        }
    }
    // add flow increment to current flow
    memcpy(wx->c1,uu->c1,uu->stride*uu->height*sizeof(float));
    // free memory
    image_delete(du);
    image_delete(wy_dummy);
    image_delete(mask);
    image_delete(smooth_horiz);
    image_delete(smooth_vert);
    image_delete(uu);
    image_delete(a11);
    image_delete(b1);
    local_image_delete(w_im2);
    local_image_delete(Ix);
    local_image_delete(Iy);
    local_image_delete(Iz);
    local_image_delete(Ixx);
    local_image_delete(Ixy);
    local_image_delete(Iyy);
    local_image_delete(Ixz);
    local_image_delete(Iyz);
}

template class ofdis::VarRefClass<ofdis::FlowInput_Grayscale,ofdis::FlowOutput_OpticalFlow>;
template class ofdis::VarRefClass<ofdis::FlowInput_Gradient,ofdis::FlowOutput_OpticalFlow>;
template class ofdis::VarRefClass<ofdis::FlowInput_RGB,ofdis::FlowOutput_OpticalFlow>;
template class ofdis::VarRefClass<ofdis::FlowInput_Grayscale,ofdis::FlowOutput_StereoDepth>;
template class ofdis::VarRefClass<ofdis::FlowInput_Gradient,ofdis::FlowOutput_StereoDepth>;
template class ofdis::VarRefClass<ofdis::FlowInput_RGB,ofdis::FlowOutput_StereoDepth>;


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */
#include "Utils/Geometry.h"
#include <Utils/IO/ChemicalFileFormats/ChemicalFileHandler.h>
#include <gmock/gmock.h>
#include <Eigen/Geometry>

using namespace testing;
namespace Scine {
namespace Utils {
using namespace Geometry;
namespace Tests {

/**
 * @class Scine::Utils::Tests::InternalCoordinatesTest InternalCoordinatesTest.cpp
 * @brief Comprises tests for the class Scine::Utils::InternalCoordinates.
 * @test
 */
TEST(InternalCoordinatesTest, Formaldehyde) {
  auto elements = ElementTypeCollection{ElementType::O, ElementType::C, ElementType::H, ElementType::H};
  PositionCollection positions(4, 3);
  // clang-format off
  positions(0,0) = 0.0; positions(0,1) =  0.000000; positions(0,2) = -0.537500;
  positions(1,0) = 0.0; positions(1,1) =  0.000000; positions(1,2) =  0.662500;
  positions(2,0) = 0.0; positions(2,1) =  0.866025; positions(2,2) = -1.037500;
  positions(3,0) = 0.0; positions(3,1) = -0.866025; positions(3,2) = -1.037500;
  // clang-format on
  AtomCollection atoms(elements, positions);
  InternalCoordinates ic(atoms);
  auto internal = ic.coordinatesToInternal(positions);
  auto backtransformed = ic.coordinatesToCartesian(internal);
  for (unsigned int i = 0; i < 4; i++) {
    for (unsigned int j = 0; j < 3; j++) {
      EXPECT_NEAR(positions(i, j), backtransformed(i, j), 1e-9);
    }
  }
}

TEST(InternalCoordinatesTest, Water) {
  auto elements = ElementTypeCollection{ElementType::O, ElementType::H, ElementType::H};
  Eigen::MatrixX3d positions = Eigen::MatrixX3d::Zero(3, 3);
  // clang-format off
  positions(0,0) = 0.0; positions(0,1) = 0.0; positions(0,2) = 0.0;
  positions(1,0) = 0.0; positions(1,1) = 0.0; positions(1,2) = 1.0;
  positions(2,0) = 0.0; positions(2,1) = 1.0; positions(2,2) = 0.0;
  // clang-format on
  AtomCollection atoms(elements, positions);
  InternalCoordinates ic(atoms);
  ASSERT_EQ(3, ic.coordinatesToInternal(positions).size());
}

TEST(InternalCoordinatesTest, HessianGuessForSmallMolecule) {
  auto elements = ElementTypeCollection{ElementType::O, ElementType::H, ElementType::H};
  Eigen::MatrixX3d positions = Eigen::MatrixX3d::Zero(3, 3);
  // clang-format off
  positions(0,0) = 0.0; positions(0,1) = 0.0; positions(0,2) = 0.0;
  positions(1,0) = 0.0; positions(1,1) = 0.0; positions(1,2) = 1.0;
  positions(2,0) = 0.0; positions(2,1) = 1.0; positions(2,2) = 0.0;
  // clang-format on
  AtomCollection atoms(elements, positions);
  InternalCoordinates ic(atoms);
  auto guess = ic.hessianGuess();
  auto invguess = ic.inverseHessianGuess();
  Eigen::MatrixXd ref = Eigen::MatrixXd::Identity(3, 3);
  ASSERT_EQ((guess - ref).sum(), 0.0);
  ASSERT_EQ((invguess - ref).sum(), 0.0);
}

} /* namespace Tests */
} /* namespace Utils */
} /* namespace Scine */


///////////////////////////////////////////////////////////////////////////////
// distribution::survival::record::less_than.hpp                       		 //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RECORD_LESS_THAN_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_SURVIVAL_RECORD_LESS_THAN_HPP_ER_2009
#include <boost/utility/enable_if.hpp>
#include <boost/mpl/and.hpp>
#include <boost/fusion/sequence/intrinsic/has_key.hpp>
#include <boost/fusion/include/has_key.hpp>
#include <boost/fusion/sequence/intrinsic/at_key.hpp>
#include <boost/fusion/include/at_key.hpp>
#include <boost/statistics/detail/distribution/survival/record/key/entry_time.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace survival{
namespace record{

    struct less_than{
        typedef bool result_type;
        less_than(){}
        template<typename M,typename M1>
        result_type operator()(const M& m, const M1& m1)const
        {
            return boost::fusion::at_key<tag::entry_time>(m)
            	< boost::fusion::at_key<tag::entry_time>(m1);
        }
    };

}// record
}// survival
}// distribution
}// detail
}// statistics
}// boost

#endif


#include "SDPB_Parameters.hxx"
#include "../sdp_solve.hxx"
#include "../set_stream_precision.hxx"

#include <El.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/filesystem.hpp>

void save_solution(const SDP_Solver &solver, const SDP_Solver_Terminate_Reason,
                   const std::pair<std::string, Timer> &timer_pair,
                   const boost::filesystem::path &out_directory,
                   const Write_Solution &write_solution,
                   const std::vector<size_t> &block_indices,
                   const Verbosity &verbosity);

Timers solve(const Block_Info &block_info, const SDPB_Parameters &parameters)
{
  El::Grid grid(block_info.mpi_comm.value);
  SDP sdp(parameters.sdp_path, block_info, grid);
  SDP_Solver solver(parameters.solver, parameters.verbosity,
                    parameters.require_initial_checkpoint, block_info, grid,
                    sdp.dual_objective_b.Height());

  Timers timers(parameters.verbosity >= Verbosity::debug);
  const boost::property_tree::ptree parameters_tree(
    to_property_tree(parameters));
  SDP_Solver_Terminate_Reason reason(solver.run(
    parameters.solver, parameters.verbosity,
    parameters_tree, block_info, sdp, grid, timers));

  if(parameters.verbosity >= Verbosity::regular && El::mpi::Rank() == 0)
    {
      set_stream_precision(std::cout);
      std::cout << "-----" << reason << "-----\n"
                << '\n'
                << "primalObjective = " << solver.primal_objective << '\n'
                << "dualObjective   = " << solver.dual_objective << '\n'
                << "dualityGap      = " << solver.duality_gap << '\n'
                << "primalError     = " << solver.primal_error() << '\n'
                << "dualError       = " << solver.dual_error << '\n'
                << '\n';
    }

  if(!parameters.no_final_checkpoint)
    {
      solver.save_checkpoint(parameters.solver.checkpoint_out, parameters.verbosity,
                             parameters_tree);
    }
  save_solution(solver, reason, timers.front(), parameters.out_directory,
                parameters.write_solution, block_info.block_indices,
                parameters.verbosity);
  return timers;
}


#pragma once

#include <cblas.h>
#include <immintrin.h>
#include <x86intrin.h>
#include <Eigen/Dense>
#include <boost/align/aligned_allocator.hpp>
#include <boost/assert.hpp>
#include <vector>

#include "collision_tensor_dense_base.hpp"

namespace boltzmann {
namespace ct_dense {

class CollisionTensorZLastAMstorage : public CollisionTensorDenseBase
{
 public:
  typedef std::vector<double, boost::alignment::aligned_allocator<double, 32>> aligned_vector;

 public:
  /**
   * @brief constructor ....
   *
   * @param basis Polar-Laguerre basis
   * @param bufsize number of input vectors (for example local spatial DoFs)
   *
   */
  CollisionTensorZLastAMstorage(const basis_t &basis, int bufsize);

  virtual void import_entries(
      const std::vector<std::shared_ptr<Eigen::SparseMatrix<double>>> &slices);

  template <typename DERIVED1, typename DERIVED2>
  inline void pad(Eigen::DenseBase<DERIVED1> &out, const Eigen::DenseBase<DERIVED2> &in) const;

  template <typename DERIVED1, typename DERIVED2>
  inline void unpad(Eigen::DenseBase<DERIVED1> &out, const Eigen::DenseBase<DERIVED2> &in) const;

  inline int padded_vector_length() const;

  /**
   * @brief resize buffer
   *
   * @param number of input vectors (for example local spatial DoFs)
   */
  void resize_buffer(int bufsize);

  int get_buffer_size() const;

 protected:
  int bufsize_;
  mutable aligned_vector buffer_;

  //@{
  ///  helpers for padded input vectors
  std::vector<int> voffsets_;
  std::unordered_map<int, int> offset_map_;
  //@}
};

template <typename DERIVED1, typename DERIVED2>
inline void
CollisionTensorZLastAMstorage::pad(Eigen::DenseBase<DERIVED1> &out,
                                   const Eigen::DenseBase<DERIVED2> &in) const
{
  BOOST_ASSERT(voffsets_.size() > 0);
  static_assert(!DERIVED1::IsRowMajor, "expecting row major storage");
  static_assert(!DERIVED2::IsRowMajor, "expecting row major storage");

  int nvectors = in.cols();
  int Npadded = this->padded_vector_length();
  const double *ptr_in = in.derived().data();
  auto &vbcrs = sparsity_patterns_[0];
  if (out.cols() != in.cols() || out.rows() != Npadded) {
    out.derived().resize(Npadded, in.cols());
    // argh, need to set to zero
    out.derived().setZero();
  }
  double *ptr_out = out.derived().data();

  for (int k = 0; k < nvectors; ++k) {
    int unpadded_offset = 0;
    for (int i = 0; i < voffsets_.size() - 1; ++i) {
      int row_extent = vbcrs.get_row_info(i).extent;
      std::copy(ptr_in + unpadded_offset + k * N_,
                ptr_in + unpadded_offset + k * N_ + row_extent,
                ptr_out + k * Npadded + voffsets_[i]);
      unpadded_offset += row_extent;
    }
  }
}

template <typename DERIVED1, typename DERIVED2>
inline void
CollisionTensorZLastAMstorage::unpad(Eigen::DenseBase<DERIVED1> &out,
                                     const Eigen::DenseBase<DERIVED2> &in) const
{
  BOOST_ASSERT(voffsets_.size() > 0);
  static_assert(!DERIVED1::IsRowMajor, "expecting row major storage");
  static_assert(!DERIVED2::IsRowMajor, "expecting row major storage");

  int nvectors = in.cols();
  out.resize(N_, nvectors);
  int Npadded = this->padded_vector_length();
  const double *ptr_in = in.derived().data();
  double *ptr_out = out.derived().data();
  auto &vbcrs = sparsity_patterns_[0];

  for (int k = 0; k < nvectors; ++k) {
    int unpadded_offset = 0;
    for (int i = 0; i < voffsets_.size() - 1; ++i) {
      int row_extent = vbcrs.get_row_info(i).extent;
      std::copy(ptr_in + voffsets_[i] + k * Npadded,
                ptr_in + voffsets_[i] + k * Npadded + row_extent,
                ptr_out + k * N_ + unpadded_offset);
      unpadded_offset += row_extent;
    }
  }
}

inline int
CollisionTensorZLastAMstorage::padded_vector_length() const
{
  return (*voffsets_.rbegin());
}

}  // namespace ct_dense
}  // namespace boltzmann


#pragma once

#include "Core/Loaders/MultiViewDataLoader.hpp"

#include <Eigen/Core>

#include <string>
#include <vector>

class Camera;
class CameraSetup;
class CameraSetupDataset;
struct Point3D;

class OpenVSLAMLoader : public MultiViewDataLoader
{
public:
	OpenVSLAMLoader();

	/*!
	 * Loads OpenVSLAM multi-view geometry data from disk.
	 * The data includes camera pose of each frame and optionally reconstructed 3D points.
	 * @see [OpenVSLAM official website](https://openvslam.readthedocs.io/en/master/)
	 *
	 * @param  dataset The dataset object to store the loaded data in.
	 * @param  sfmFile Path of file storing the path of image, camera, 3D points and depth map folders.
	 * @param  pathToInputFolder Path to the input data files.
	 * @param  intrinsicScale Scale factor for the intrinsic parameters of the camera.
	 * @param  load3DPoints Whether to load 3D points.
	 * @param  loadSparsePointCloud Whether to load the reconstructed sparse 3D points.
	 * @return The status of the loading process.
	 */
	int loadData(
	    CameraSetupDataset* dataset,
	    const std::string& sfmFile,
	    const std::string& pathToInputFolder,
	    const float intrinsicScale,
	    const bool load3DPoints = false,
	    const bool loadSparsePointCloud = false) override;

	/*!
	 * Sets the number of digit characters in the image file name postfix.
	 *
	 * @param n The number of digits.
	 */
	void setImageNameDigits(int n) override { imageNameDigits = n; }

private:
	// the digit characters number in the postfix of the image file
	int imageNameDigits;

	// transform matrix from OpenVSLAM coordinate to system coordinate
	Eigen::Matrix3f openVSLAMTransform;

	/*!
	 * Sets camera pose to each frame (@see class: Camera).
	 */
	void loadCameraParameters(CameraSetup* camSetup,
	                          const float intrinsicScale,
	                          const std::string& pathToCameras,
	                          const std::string& pathToImages,
	                          bool& useEquirectImages);

	/*!
	 * Sets the image file name for each frame, extracting frame number from filename.
	 */
	void setCamNames(Camera* cam, std::string name, const int len);

	/*!
	 * Loads camera intrinsics from file.
	 */
	void loadCameraIntrinsicParameters(const std::string& pathToCameras, int& camera_dimX,
	                                   int& camera_dimY,
	                                   float& camera_focal,
	                                   float& camera_pX,
	                                   float& camera_pY,
	                                   bool& useEquirectImages);

	/*!
	 * Loads point cloud from OpenVSLAM *.msg file.
	 */
	void load3DPoints(const std::string& filePath, std::vector<Point3D*>& pointCloud, int maxLoad3Dpoints = -1);

	/*!
	* Loads the resource path from "modelFiles.openvslam".
	*/
	void readPathsFromModelFiles(const std::string& slamResultListPath,
	                             std::string& pathToCameras,
	                             std::string& pathToImages,
	                             std::string& pathTo3DPoints);
};


#include "generalizedassignmentsolver/algorithms/algorithms.hpp"

#include "generalizedassignmentsolver/algorithms/linrelax_clp.hpp"
#include "generalizedassignmentsolver/algorithms/lagrelax_volume.hpp"
#include "generalizedassignmentsolver/algorithms/lagrelax_lbfgs.hpp"
#include "generalizedassignmentsolver/algorithms/columngeneration.hpp"
#include "generalizedassignmentsolver/algorithms/milp_cbc.hpp"
#include "generalizedassignmentsolver/algorithms/milp_cplex.hpp"
#include "generalizedassignmentsolver/algorithms/milp_gurobi.hpp"
#include "generalizedassignmentsolver/algorithms/constraintprogramming_gecode.hpp"
#include "generalizedassignmentsolver/algorithms/constraintprogramming_cplex.hpp"
#include "generalizedassignmentsolver/algorithms/random.hpp"
#include "generalizedassignmentsolver/algorithms/greedy.hpp"
#include "generalizedassignmentsolver/algorithms/repair.hpp"
#include "generalizedassignmentsolver/algorithms/localsearch.hpp"
#include "generalizedassignmentsolver/algorithms/localsolver.hpp"

#include <boost/program_options.hpp>

using namespace generalizedassignmentsolver;
namespace po = boost::program_options;

std::string read_desiralibity_args(const std::vector<char*>& argv)
{
    ColumnGenerationOptionalParameters parameters;
    po::options_description desc("Allowed options");
    std::string desirability = "cij";
    desc.add_options()
        (",f", po::value<std::string>(&desirability), "")
        ;
    po::variables_map vm;
    po::store(po::parse_command_line((Counter)argv.size(), argv.data(), desc), vm);
    try {
        po::notify(vm);
    } catch (const po::required_option& e) {
        std::cout << desc << std::endl;;
        throw "";
    }
    return desirability;
}

ColumnGenerationOptionalParameters read_columngeneration_args(const std::vector<char*>& argv)
{
    ColumnGenerationOptionalParameters parameters;
    po::options_description desc("Allowed options");
    desc.add_options()
        ("linear-programming-solver,s", po::value<std::string>(&parameters.linear_programming_solver), "")
        ;
    po::variables_map vm;
    po::store(po::parse_command_line((Counter)argv.size(), argv.data(), desc), vm);
    try {
        po::notify(vm);
    } catch (const po::required_option& e) {
        std::cout << desc << std::endl;;
        throw "";
    }
    return parameters;
}

LocalSearchOptionalParameters read_localsearch_args(const std::vector<char*>& argv)
{
    LocalSearchOptionalParameters parameters;
    po::options_description desc("Allowed options");
    desc.add_options()
        ("threads,t", po::value<Counter>(&parameters.number_of_threads), "")
        ;
    po::variables_map vm;
    po::store(po::parse_command_line((Counter)argv.size(), argv.data(), desc), vm);
    try {
        po::notify(vm);
    } catch (const po::required_option& e) {
        std::cout << desc << std::endl;;
        throw "";
    }
    return parameters;
}

RepairOptionalParameters read_repair_args(const std::vector<char*>& argv)
{
    RepairOptionalParameters parameters;
    po::options_description desc("Allowed options");
    desc.add_options()
        ("initial-solution,i", po::value<RepairInitialSolution>(&parameters.initial_solution), "")
        (",l", po::value<Counter>(&parameters.l), "")
        ;
    po::variables_map vm;
    po::store(po::parse_command_line((Counter)argv.size(), argv.data(), desc), vm);
    try {
        po::notify(vm);
    } catch (const po::required_option& e) {
        std::cout << desc << std::endl;;
        throw "";
    }
    return parameters;
}

MilpCplexOptionalParameters read_milp_cplex_args(const std::vector<char*>& argv)
{
    MilpCplexOptionalParameters parameters;
    po::options_description desc("Allowed options");
    desc.add_options()
        ("only-linear-relaxation", "")
        ;
    po::variables_map vm;
    po::store(po::parse_command_line((Counter)argv.size(), argv.data(), desc), vm);
    try {
        po::notify(vm);
    } catch (const po::required_option& e) {
        std::cout << desc << std::endl;;
        throw "";
    }
    if (vm.count("only-linear-relaxation"))
        parameters.only_linear_relaxation = true;
    return parameters;
}

#if GUROBI_FOUND
MilpGurobiOptionalParameters read_milp_gurobi_args(const std::vector<char*>& argv)
{
    MilpGurobiOptionalParameters parameters;
    po::options_description desc("Allowed options");
    desc.add_options()
        ("only-linear-relaxation", "")
        ;
    po::variables_map vm;
    po::store(po::parse_command_line((Counter)argv.size(), argv.data(), desc), vm);
    try {
        po::notify(vm);
    } catch (const po::required_option& e) {
        std::cout << desc << std::endl;;
        throw "";
    }
    if (vm.count("only-linear-relaxation"))
        parameters.only_linear_relaxation = true;
    return parameters;
}
#endif

Output generalizedassignmentsolver::run(
        std::string algorithm,
        const Instance& instance,
        const Solution& initial_solution,
        std::mt19937_64& generator,
        Info info)
{
    std::vector<std::string> algorithm_args = po::split_unix(algorithm);
    std::vector<char*> algorithm_argv;
    for (Counter i = 0; i < (Counter)algorithm_args.size(); ++i)
        algorithm_argv.push_back(const_cast<char*>(algorithm_args[i].c_str()));

    if (algorithm.empty() || algorithm_args[0].empty()) {
        throw std::invalid_argument("Missing algorithm.");

    /*
     * Lower bounds
     */
#if COINOR_FOUND
    } else if (algorithm_args[0] == "linrelax_clp") {
        return linrelax_clp(instance, info);
#endif
#if COINOR_FOUND
    } else if (algorithm_args[0] == "lagrelax_knapsack_volume") {
        return lagrelax_knapsack_volume(instance, info);
#endif
    } else if (algorithm_args[0] == "lagrelax_knapsack_lbfgs") {
        return lagrelax_knapsack_lbfgs(instance, info);
#if COINOR_FOUND
    } else if (algorithm_args[0] == "lagrelax_assignment_volume") {
        return lagrelax_assignment_volume(instance, info);
#endif
    } else if (algorithm_args[0] == "lagrelax_assignment_lbfgs") {
        LagRelaxAssignmentLbfgsOptionalParameters parameters;
        parameters.info = info;
        return lagrelax_assignment_lbfgs(instance, parameters);
    } else if (algorithm_args[0] == "columngeneration") {
        auto parameters = read_columngeneration_args(algorithm_argv);
        parameters.info = info;
        return columngeneration(instance, parameters);

    /*
     * Exact algorithms
     */
#if COINOR_FOUND
    } else if (algorithm_args[0] == "milp_cbc") {
        MilpCbcOptionalParameters parameters;
        parameters.info = info;
        return milp_cbc(instance, parameters);
#endif
#if CPLEX_FOUND
    } else if (algorithm_args[0] == "milp_cplex") {
        auto parameters = read_milp_cplex_args(algorithm_argv);
        parameters.info = info;
        return milp_cplex(instance, parameters);
#endif
#if GUROBI_FOUND
    } else if (algorithm_args[0] == "milp_gurobi") {
        auto parameters = read_milp_gurobi_args(algorithm_argv);
        parameters.info = info;
        return milp_gurobi(instance, parameters);
#endif
#if GECODE_FOUND
    } else if (algorithm_args[0] == "constraintprogramming_gecode") {
        ConstraintProgrammingGecodeOptionalParameters parameters;
        parameters.info = info;
        return constraintprogramming_gecode(instance, parameters);
#endif
#if CPLEX_FOUND
    } else if (algorithm_args[0] == "constraintprogramming_cplex") {
        ConstraintProgrammingCplexOptionalParameters parameters;
        parameters.info = info;
        return constraintprogramming_cplex(instance, parameters);
#endif

    /*
     * Upper bounds
     */
    } else if (algorithm_args[0] == "random") {
        return random(instance, generator, info);
    } else if (algorithm_args[0] == "greedy") {
        std::string desirability_string = read_desiralibity_args(algorithm_argv);
        std::unique_ptr<Desirability> f = desirability(desirability_string, instance);
        return greedy(instance, *f, info);
    } else if (algorithm_args[0] == "greedyregret") {
        std::string desirability_string = read_desiralibity_args(algorithm_argv);
        std::unique_ptr<Desirability> f = desirability(desirability_string, instance);
        return greedyregret(instance, *f, info);
    } else if (algorithm_args[0] == "mthg") {
        std::string desirability_string = read_desiralibity_args(algorithm_argv);
        std::unique_ptr<Desirability> f = desirability(desirability_string, instance);
        return mthg(instance, *f, info);
    } else if (algorithm_args[0] == "mthgregret") {
        std::string desirability_string = read_desiralibity_args(algorithm_argv);
        std::unique_ptr<Desirability> f = desirability(desirability_string, instance);
        return mthgregret(instance, *f, info);
    } else if (algorithm_args[0] == "repair") {
        auto parameters = read_repair_args(algorithm_argv);
        parameters.info = info;
        return repair(instance, generator, parameters);
    } else if (algorithm_args[0] == "localsearch") {
        auto parameters = read_localsearch_args(algorithm_argv);
        parameters.info = info;
        parameters.initial_solution = &initial_solution;
        return localsearch(instance, generator, parameters);
#if LOCALocalSearchOLVER_FOUND
    } else if (algorithm_args[0] == "localsolver") {
        LocalSolverOptionalParameters parameters;
        parameters.info = info;
        return localsolver(instance, parameters);
#endif
    } else if (algorithm_args[0] == "columngenerationheuristic_greedy") {
        auto parameters = read_columngeneration_args(algorithm_argv);
        parameters.info = info;
        return columngenerationheuristic_greedy(instance, parameters);
    } else if (algorithm_args[0] == "columngenerationheuristic_limiteddiscrepancysearch") {
        auto parameters = read_columngeneration_args(algorithm_argv);
        parameters.info = info;
        return columngenerationheuristic_limiteddiscrepancysearch(instance, parameters);

    } else {
        throw std::invalid_argument(
                "Unknown algorithm \"" + algorithm_args[0] + "\".");
    }

}


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

#include <boost/assert.hpp>
#include <boost/core/lightweight_test.hpp>
#include <boost/histogram/axis.hpp>
#include <boost/histogram/axis/ostream.hpp>
#include <boost/histogram/ostream.hpp>
#include <boost/histogram/serialization.hpp>
#include <cmath>
#include <string>
#include "throw_exception.hpp"
#include "utility_histogram.hpp"
#include "utility_serialization.hpp"

using namespace boost::histogram;

template <typename Tag>
void run_tests(const std::string& filename) {
  // histogram_serialization
  namespace tr = axis::transform;
  using def = use_default;
  using axis::option::none_t;
  auto a =
      make(Tag(), axis::regular<double, def, def, none_t>(1, -1, 1, "reg"),
           axis::circular<float, def, none_t>(1, 0.0, 1.0, "cir"),
           axis::regular<double, tr::log, def, none_t>(1, 1, std::exp(2), "reg-log"),
           axis::regular<double, tr::pow, std::vector<int>, axis::option::overflow_t>(
               tr::pow(0.5), 1, 1, 100, {1, 2, 3}),
           axis::variable<double, def, none_t>({1.5, 2.5}, "var"),
           axis::category<int, def, none_t>{3, 1},
           axis::integer<int, axis::null_type, none_t>(1, 2));
  a(0.5, 0.2, 2, 20, 2.2, 1, 1);
  print_xml(filename, a);

  auto b = decltype(a)();
  BOOST_TEST_NE(a, b);
  load_xml(filename, b);
  BOOST_TEST_EQ(a, b);
}

int main(int argc, char** argv) {
  BOOST_ASSERT(argc == 2);
  run_tests<static_tag>(join(argv[1], "histogram_serialization_test_static.xml"));
  run_tests<dynamic_tag>(join(argv[1], "histogram_serialization_test_dynamic.xml"));
  return boost::report_errors();
}


#include <ros/ros.h>

#include "i2c_devices/ADXL345.h"
#include "i2c_devices/ITG3200.h"
#include "i2c_devices/HMC58X3.h"

#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.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 <fstream>
#include <boost/filesystem.hpp>

#include <jetsoncar_utils/jetsoncar_utils.h>

class IMUNode {
private:
    // Sensors
    ADXL345 acc;
    ITG3200 gyro;
    HMC58X3 mag;

    // Sensor parameters
    const float g = 9.82; // 1g -> m/s
    float accelScale_;
    const float deg2rad = M_PI / 180.0f;
    float gyroScale_;
    float magScale_;

    // ROS specific
    ros::NodeHandle nh_;
    ros::NodeHandle nParam_;
    int poll_rate_;
    ros::Timer timerPublish_;

    // Frames
    std::string world_frame_;
    std::string imu_frame_;

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

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

    typedef struct {
        float accelerometer[3]; // m/s^2
        float gyroscope[3];     // rad/s
        float magnetometer[3];  // milli Gauss
    } SensorMeasurements;

private:
    void RawSensorsUpdate(SensorMeasurements meas)
    {
        auto time = ros::Time().now();

        /*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;
        }*/

        // Publish IMU message
        sensor_msgs::Imu imu_msg;
        imu_msg.header.stamp = time;
        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 poll()
    {
        SensorMeasurements meas = {0};
        int16_t xa, ya, za;
        float xg, yg, zg, temperature;
        float xm, ym, zm;

        // Accelerometer Reading
        acc.readAccel(&xa, &ya, &za);         // Read the accelerometer values and store them in variables declared above x,y,z
        meas.accelerometer[0] = accelScale_ * xa;
        meas.accelerometer[1] = accelScale_ * ya;
        meas.accelerometer[2] = accelScale_ * za;
        ROS_DEBUG("Acc\tX,Y,Z = %04.1f,\t %04.1f,\t %04.1f", meas.accelerometer[0], meas.accelerometer[1], meas.accelerometer[2]);

        // Gyroscope Reading
        if (gyro.isRawDataReady()) {
            //gyro.readTemp(&temperature);
            gyro.readGyro(&xg, &yg, &zg);
            meas.gyroscope[0] = gyroScale_ * xg;
            meas.gyroscope[1] = gyroScale_ * yg;
            meas.gyroscope[2] = gyroScale_ * zg;
            ROS_DEBUG("Gyro\tX,Y,Z = %04.1f,\t %04.1f,\t %04.1f", meas.gyroscope[0], meas.gyroscope[1], meas.gyroscope[2]);
        }

        // Magnetometer reading
        mag.getValues(&xm, &ym, &zm);
        meas.magnetometer[0] = magScale_ * xm;
        meas.magnetometer[1] = magScale_ * ym;
        meas.magnetometer[2] = magScale_ * zm;
        ROS_DEBUG("Mag\tX,Y,Z = %04.1f,\t %04.1f,\t %04.1f", meas.magnetometer[0], meas.magnetometer[1], meas.magnetometer[2]);

        RawSensorsUpdate(meas);
    }

public:
    IMUNode(ros::NodeHandle nh, ros::NodeHandle nParam, std::string world_frame, std::string imu_frame, int poll_rate)
        : nh_(nh)
        , nParam_(nParam)
        , poll_rate_{poll_rate}
        , world_frame_{world_frame}
        , imu_frame_{imu_frame}
        , pub_imu_raw_{nh.advertise<sensor_msgs::Imu>("imu/raw", 10)}
        , pub_magnetic_field_{nh.advertise<sensor_msgs::MagneticField>("imu/mag", 10)}
    {
        /* Configure IMU sensors */
        auto configure_accelerometer = [&]() {
            // Accelerometer
            acc.powerOn();
            acc.setRangeSetting(2);           // Give the range settings
            accelScale_ = g * 2.0f / (float) std::pow(2, 9); // convert 10-bit to m/s
            // Accepted values are 2g, 4g, 8g or 16g
            // Higher Values = Wider Measurement Range
            // Lower Values = Greater Sensitivity
        };

        auto configure_gyroscope = [&]() {
            // Gyroscope
            // Use ITG3200_ADDR_AD0_HIGH or ITG3200_ADDR_AD0_LOW as the ITG3200 address
            // depending on how AD0 is connected on your breakout board, check its schematics for details
            gyro.init(ITG3200_ADDR_AD0_LOW);
            // Configure full range (+/- 2000 deg/s) and internal LPF filter setting
            gyroScale_ = deg2rad; // convert deg/s to rad/s
            //gyro.setFilterBW(BW256_SR8); // 256Khz BW and 8Khz SR
            gyro.setFilterBW(BW042_SR1); // 42 Hz BW and 1Khz SR

            ROS_INFO("Gyro: Calibrating for 5 seconds. Please wait...");
            int sample_time_ms = 1000 / poll_rate_;
            gyro.zeroCalibrate(5 * poll_rate_, sample_time_ms);
            ROS_INFO("Gyro: Finished calibrating");
        };

        auto configure_magnetometer = [&]() {
            // Magnetometer
            mag.init();
            ROS_INFO("Magnetometer: Calibrating, please wait...");
            mag.calibrate(0, 64);
            ROS_INFO("Magnetometer: Finished calibrating");
            mag.setMode(0); // set 75 Hz sample frequency, averaging of 8 samples and no artificial bias.
            magScale_ = 1000; // Gauss to milliGauss
        };

        // Call the 3 functions in parallel
        #pragma omp parallel // default(none) shared(arg)
        #pragma omp single
        {
            #pragma omp task
            configure_accelerometer();
            #pragma omp task
            configure_gyroscope();
            #pragma omp task
            configure_magnetometer();
        }

        /* Create a Poll timer for publishing the measurements */
        timerPublish_ = nh.createTimer(ros::Duration(1.0 / poll_rate_), std::bind(&IMUNode::poll, this));
    }
};

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

    // Enable debug verbosity by default
    //if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug)) ros::console::notifyLoggerLevelsChanged();

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

    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);
    }

    try {
        IMUNode node(nh, nParam, world_frame, imu_frame, poll_rate);
        ros::spin();
    } catch(std::exception& e){
        ROS_FATAL_STREAM("Exception thrown: " << e.what());
    }

	/*
    ros::Rate loop_rate(poll_rate);
    while (ros::ok())
    {
        node.poll();
        ros::spinOnce();
        loop_rate.sleep();
    }
	*/
}


#pragma once
#ifndef BASE_H
#define BASE_H

#include <iostream>
#include <fstream>
#include <complex>
#include <algorithm>
#include <fstream>
#include <tuple>
#include <iomanip>
#include <vector>
#include <cmath>

#include <fftw3.h>

#define EIGEN_DEFAULT_TO_ROW_MAJOR
#include <unsupported/Eigen/CXX11/Tensor>
#include <Eigen/Eigen>

namespace ionizing {
  using namespace Eigen;

  static_assert(sizeof(size_t) == sizeof(uint64_t),
      "Uncompatible compiler: sizeof(size_t) != 8 !");
  static_assert(sizeof(double) == 8,
      "Uncompatible compiler: sizeof(double) != 8 !");
  static_assert(sizeof(float)  == 4,
      "Uncompatible compiler: sizeof( float) != 4 !");
  static_assert(sizeof(int)    == 4,
      "Uncompatible compiler: sizeof(   int) != 4 !");
  static_assert(sizeof(std::complex<double>) == 16,
      "Uncompatible compiler: sizeof(complex<double>) != 16 !");
  static_assert(sizeof(std::complex< float>) ==  8,
      "Uncompatible compiler: sizeof(complex< float>) !=  8 !");

// global type aliasing
  using Vecd   = VectorXd;
  using Veccd  = VectorXcd;
  using Matd   = Matrix<              double, Dynamic, Dynamic, RowMajor>;
  using Matcd  = Matrix<std::complex<double>, Dynamic, Dynamic, RowMajor>;
  using Cubd   = Tensor<              double,                3, RowMajor>;
  using Cubcd  = Tensor<std::complex<double>,                3, RowMajor>;
  using Mat33d = Matrix<double,                     3,       3, RowMajor>;
  using MatX3d = Matrix<double,               Dynamic,       3, RowMajor>;

  template <typename T>
    using MatT    = Matrix<T, Dynamic, Dynamic, RowMajor>;

  template <typename T>
    using ColVecT = Matrix<T, Dynamic,       1, RowMajor>;

  template <typename T>
    using RowVecT = Matrix<T,       1, Dynamic, RowMajor>;

static IOFormat CommaInitFmt{StreamPrecision, DontAlignCols, ", ", ", ", "", "", " << ", ";"};
static IOFormat CleanFmt{4, 0, ", ", "\n", "[", "]"};
static IOFormat OctaveFmt{StreamPrecision, 0, ", ", ";\n", "", "", "[", "]"};
static IOFormat HeavyFmt{FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"};
}

#endif // BASE_H


#ifndef PARS_PIPELINE_HPP
#define PARS_PIPELINE_HPP

#define BM_MPI_SUPPORT

#include <utility>

#include <bm/bm.hpp>
#include <boost/mpi/cartesian_communicator.hpp>
#include <boost/mpi.hpp>
#include <pa/math/integral_curves.hpp>
#include <pa/math/scalar_field.hpp>
#include <pa/math/vector_field.hpp>
#include <pa/stages/partitioner.hpp>
#include <pa/stages/data_io.hpp>
#include <pa/stages/particle_tracer.hpp>
#include <tbb/tbb.h>

#include <pars/stages/ray_tracer.hpp>
#include <pars/export.hpp>

#include <image.pb.h>
#include <settings.pb.h>

namespace pars
{
class PARS_EXPORT pipeline
{
public:
  explicit pipeline  (const std::size_t thread_count = tbb::task_scheduler_init::default_num_threads());
  pipeline           (const pipeline&   that) = default;
  pipeline           (      pipeline&&  temp) = default;
 ~pipeline           ()                       = default;
  pipeline& operator=(const pipeline&   that) = default;
  pipeline& operator=(      pipeline&&  temp) = default;

  std::pair<image, bm::mpi_session<>> execute     (const settings& settings);
                   bm::mpi_session<>  execute_ftle(const settings& settings);

  boost::mpi::communicator*           communicator();

protected:
  boost::mpi::environment                        environment_           ;
  boost::mpi::communicator                       communicator_          ;

  tbb::task_scheduler_init                       task_scheduler_init_   ;

  pa::partitioner                                partitioner_           ;
  pa::data_io                                    data_io_               ;
  pa::particle_tracer                            particle_tracer_       ;
  ray_tracer                                     ray_tracer_            ;

  std::optional<settings>                        last_settings_         ;
  std::optional<pa::scalar_field>                local_scalar_field_    ;
  std::optional<pa::vector_field>                local_vector_field_    ;
  std::array<std::optional<pa::vector_field>, 6> neighbor_vector_fields_;
  std::vector<pa::particle>                      seeds_                 ;
  std::vector<pa::integral_curves>               integral_curves_       ;
};
}

#endif


/*-----------------------------------------------------------------------------+
Copyright (c) 2008-2010: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_TYPE_TRAITS_UNIT_ELEMENT_HPP_JOFA_080912
#define BOOST_ICL_TYPE_TRAITS_UNIT_ELEMENT_HPP_JOFA_080912

#include <string>
#include <boost/icl/type_traits/identity_element.hpp>
#include <boost/icl/type_traits/succ_pred.hpp>

namespace boost{ namespace icl
{
    template <class Type> struct unit_element{ static Type value(); };

    template<> inline bool   unit_element<bool>::value()   { return true;}
    template<> inline float  unit_element<float>::value()  { return 1.0; }
    template<> inline double unit_element<double>::value() { return 1.0; }
    template<> inline long double unit_element<long double>::value()
                                                           { return 1.0; }

    // Smallest 'visible' string that is greater than the empty string.
    template <>
    inline std::string unit_element<std::string>::value()
    { return std::string(" "); }

    template <class Type>
    inline Type unit_element<Type>::value()
    { return icl::succ(identity_element<Type>::value()); }

}} // namespace boost icl

#endif


/*!
 * @file     mppi_new.cpp
 * @author   Giuseppe Rizzi
 * @date     01.07.2020
 * @version  1.0
 * @brief    description
 */

#include <algorithm>

#include <Eigen/Core>
#include <chrono>
#include <iomanip>
#include <iostream>
#include <map>
#include <random>
#include <vector>

#include "mppi/core/config.h"
#include "mppi/core/cost.h"
#include "mppi/core/dynamics.h"
#include "mppi/core/rollout.h"
#include "mppi/core/solver.h"

#include "mppi/utils/logging.h"
#include "mppi/utils/savgol_filter.h"

#ifdef SIGNAL_LOGGER
#include <signal_logger/signal_logger.hpp>
#endif

namespace mppi {

Solver::Solver(dynamics_ptr dynamics, cost_ptr cost, policy_ptr policy,
               const Config& config)
    : dynamics_(std::move(dynamics)),
      cost_(std::move(cost)),
      policy_(std::move(policy)),
      config_(config) {
  init_data();
  init_threading();

  if (config_.display_update_freq) {
    start_time_ = std::chrono::high_resolution_clock::now();
  }

#ifdef SIGNAL_LOGGER
  signal_logger::add(min_cost_, "solver/rollouts/min_cost");
  signal_logger::add(max_cost_, "solver/rollouts/max_cost");
  signal_logger::add(rollouts_cost_, "solver/rollouts/costs");
  signal_logger::add(delay_steps_, "solver/delay_steps");
  signal_logger::add(rate_, "solver/rate");
  signal_logger::logger->updateLogger();
#endif
}

void Solver::init_data() {
  // TODO(giuseppe) this should be automatically computed when config is parsed
  steps_ = static_cast<int>(std::ceil(config_.horizon / config_.step_size));
  nx_ = dynamics_->get_state_dimension();
  nu_ = dynamics_->get_input_dimension();

  opt_roll_ = Rollout(steps_, nu_, nx_);
  opt_roll_cache_ = Rollout(steps_, nu_, nx_);
  nominal_.setZero(steps_, nu_);

  weights_.resize(config_.rollouts, 1.0 / config_.rollouts);
  rollouts_.resize(config_.rollouts, Rollout(steps_, nu_, nx_));
  cached_rollouts_ = std::ceil(config_.caching_factor * config_.rollouts);

  delay_steps_ = 0;
  step_count_ = 0;
  observation_set_ = false;
  reference_set_ = false;
}

void Solver::init_filter() {}

void Solver::init_threading() {
  if (config_.threads > 1) {
    std::cout << "Using multithreading. Number of threads: " << config_.threads
              << std::endl;
    pool_ = std::make_unique<ThreadPool>(config_.threads);
    futures_.resize(config_.threads);

    for (size_t i = 0; i < config_.threads; i++) {
      dynamics_v_.push_back(dynamics_->create());
      cost_v_.push_back(cost_->create());
    }
  }
}

void Solver::update_policy() {
  if (!observation_set_) {
    log_warning_throttle(1.0,
                         "Observation has never been set. Dropping update");
  } else if (!reference_set_) {
    log_warning_throttle(1.0, "Reference has never been set. Dropping update");
  } else {
    auto start = std::chrono::steady_clock::now();
    update_delay();
    copy_observation();

    for (size_t i = 0; i < config_.substeps; i++) {
      prepare_rollouts();
      update_reference();
      sample_trajectories();
      optimize();
      filter_input();

      stage_cost_ =
          cost_->get_stage_cost(x0_internal_, opt_roll_.uu[0], t0_internal_);
    }
    swap_policies();
    auto end = std::chrono::steady_clock::now();
    rate_ = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count() / 1e9;
  }
}

void Solver::time_it() {
  auto stop = std::chrono::high_resolution_clock::now();
  auto duration =
      std::chrono::duration_cast<std::chrono::microseconds>(stop - start_time_)
          .count();
  auto timing_string =
      "Time since optimization start: " + std::to_string(duration) + "[μs], " +
      std::to_string(duration / 1000000.0) + "[s], " +
      std::to_string(1 / (duration / 1000000.0)) + "[Hz]";
  log_info(timing_string);
  start_time_ = std::chrono::high_resolution_clock::now();
}

void Solver::set_observation(const observation_t& x, const double t) {
  {
    std::unique_lock<std::shared_mutex> lock(state_mutex_);
    x0_ = x;
    reset_time_ = t;
  }

  // initialization of rollouts data
  if (first_step_) {
    copy_observation();
    initialize_rollouts();
    first_step_ = false;
  }

  observation_set_ = true;
}

void Solver::update_delay() {
  delay_steps_ = std::ceil((reset_time_ - t0_internal_) / config_.step_size);
  policy_->update_delay(delay_steps_);
}

void Solver::copy_observation() {
  std::shared_lock<std::shared_mutex> lock(state_mutex_);
  x0_internal_ = x0_;
  t0_internal_ = reset_time_;
}

void Solver::initialize_rollouts() {
  std::shared_lock<std::shared_mutex> lock_state(state_mutex_);
  opt_roll_.clear();
  std::fill(opt_roll_.uu.begin(), opt_roll_.uu.end(),
            dynamics_->get_zero_input(x0_internal_));

  std::shared_lock<std::shared_mutex> lock(rollout_cache_mutex_);
  std::fill(opt_roll_cache_.xx.begin(), opt_roll_cache_.xx.end(), x0_internal_);
  std::fill(opt_roll_cache_.uu.begin(), opt_roll_cache_.uu.end(),
            dynamics_->get_zero_input(x0_internal_));
  for (int i = 0; i < steps_; i++) {
    opt_roll_cache_.tt[i] = t0_internal_ + config_.step_size * i;
  }
}

void Solver::prepare_rollouts() {
  // cleanup
  rollouts_cost_.clear();
  for (auto& roll : rollouts_) {
    roll.clear_cost();
    roll.valid = true;
  }
}

void Solver::set_reference_trajectory(mppi::reference_trajectory_t& ref) {
  if (ref.rr.size() != ref.tt.size()) {
    std::stringstream error;
    error << "The reference trajectory state and time dimensions do not match: "
          << ref.rr.size() << " != " << ref.tt.size();
    throw std::runtime_error(error.str());
  }
  std::unique_lock<std::shared_mutex> lock(reference_mutex_);
  rr_tt_ref_ = ref;
  reference_set_ = true;
}

void Solver::update_reference() {
  std::shared_lock<std::shared_mutex> lock(reference_mutex_);
  cost_->set_reference_trajectory(rr_tt_ref_);

  if (config_.threads > 1) {
    for (auto& cost : cost_v_) cost->set_reference_trajectory(rr_tt_ref_);
  }

}

void Solver::sample_noise(input_t& noise) { sampler_->get_sample(noise); }

void Solver::sample_trajectories_batch(dynamics_ptr& dynamics, cost_ptr& cost,
                                       const size_t start_idx,
                                       const size_t end_idx) {
  observation_t x;
  for (size_t k = start_idx; k < end_idx; k++) {
    dynamics->reset(x0_internal_, t0_internal_);
    x = x0_internal_;
    double ts;
    for (int t = 0; t < steps_; t++) {
      ts = t0_internal_ + t * config_.step_size;
      rollouts_[k].tt[t] = ts;
      rollouts_[k].uu[t] = policy_->sample(ts, k);

      // compute input-state stage cost
      double cost_temp;
      cost_temp = std::pow(config_.discount_factor, t) *
                  cost->get_stage_cost(x, rollouts_[k].uu[t], ts);

      if (std::isnan(cost_temp)) {
        std::stringstream ss;
        ss << "Something went wrong ... dynamics diverged?\n" << std::endl;
        ss << "Rollout#" << k << std::endl;
        ss << "Time  where diverged: " << t << std::endl;
        ss << "State where diverged: " << x.transpose() << std::endl;
        ss << "Previous state: " << rollouts_[k].xx[t - 1].transpose()
           << std::endl;
        ss << "Input where diverged: " << rollouts_[k].uu[t].transpose();
        ss << "Previous input: " << rollouts_[k].uu[t - 1].transpose()
           << std::endl;
        rollouts_[k].valid = false;
        rollouts_[k].total_cost = std::numeric_limits<double>::infinity();
        log_warning(ss.str());
        break;
      }

      // store data
      rollouts_[k].xx[t] = x;
      rollouts_[k].cc(t) = cost_temp;
      rollouts_[k].total_cost += cost_temp;

      // integrate dynamics    auto start = std::chrono::steady_clock::now();
      x = dynamics->step(rollouts_[k].uu[t], config_.step_size);
    }
  }
}

void Solver::sample_trajectories() {
  policy_->shift(t0_internal_);
  policy_->update_samples(weights_, cached_rollouts_);

  if (config_.threads == 1) {
    sample_trajectories_batch(dynamics_, cost_, 0, config_.rollouts);
  } else {
    for (size_t i = 0; i < config_.threads; i++) {
      futures_[i] = pool_->enqueue(
          std::bind(&Solver::sample_trajectories_batch, this,
                    std::placeholders::_1, std::placeholders::_2,
                    std::placeholders::_3, std::placeholders::_4),
          dynamics_v_[i], cost_v_[i],
          (size_t)i * config_.rollouts / config_.threads,
          (size_t)(i + 1) * config_.rollouts / config_.threads);
    }

    for (size_t i = 0; i < config_.threads; i++) futures_[i].get();
  }
}

void Solver::compute_weights() {
  // keep all non diverged rollouts
  double min_cost = std::numeric_limits<double>::max();
  double max_cost = -min_cost;

  for (size_t k = 0; k < config_.rollouts; k++) {
    if (rollouts_[k].valid) {
      const double& cost = rollouts_[k].total_cost;
      min_cost = (cost < min_cost) ? cost : min_cost;
      max_cost = (cost > max_cost) ? cost : max_cost;
    }
  }

  min_cost_ = min_cost;   //*std::min_element(rollouts_cost_.begin(), rollouts_cost_.end());
  max_cost_ = max_cost;   //*std::max_element(rollouts_cost_.begin(), rollouts_cost_.end());

  double sum = 0.0;
  for (size_t k = 0; k < config_.rollouts; k++) {
    double modified_cost = config_.h * (rollouts_[k].total_cost - min_cost_) /
                           (max_cost_ - min_cost_);

    weights_[k] = rollouts_[k].valid ? std::exp(-modified_cost) : 0.0;
    sum += weights_[k];
  }
  std::transform(weights_.begin(), weights_.end(), weights_.begin(),
                 [&sum](double v) -> double { return v / sum; });
}

void Solver::optimize() {
  // get new rollouts weights
  compute_weights();

  // update policy according to new weights
  policy_->update(weights_, config_.alpha);

  // retrieve the nominal policy for each time step
  for (int t = 0; t < steps_; t++) {
    opt_roll_.tt[t] = t0_internal_ + t * config_.step_size;
    opt_roll_.uu[t] = policy_->nominal(t0_internal_ + t * config_.step_size);
  }

}

void Solver::filter_input() {
  // only if filter is available reset to the initial opt time
  if (filter_) {
    filter_->reset(x0_internal_, t0_internal_);
  }

  // reset the dynamics such that we rollout the dynamics again
  // with the input we filter step after step (sequentially)
  dynamics_->reset(x0_internal_, t0_internal_);
  opt_roll_.xx[0] = x0_internal_;

  // sequential filtering otherwise just nominal rollout
  for (int t = 0; t < steps_ - 1; t++) {
    if (filter_) {
      filter_->apply(opt_roll_.xx[t], opt_roll_.uu[t], opt_roll_.tt[t]);
      nominal_.row(t) = opt_roll_.uu[t].transpose();
    }
    opt_roll_.xx[t + 1] = dynamics_->step(opt_roll_.uu[t], config_.step_size);
  }

  // filter the last input in the sequence (if filter is available)
  if (filter_) {
    filter_->apply(opt_roll_.xx.back(), opt_roll_.uu.back(),
                   opt_roll_.tt.back());
    nominal_.bottomRows(1) = opt_roll_.uu.back().transpose();
    policy_->set_nominal(nominal_);
  }
}

void Solver::get_input(const observation_t& x, input_t& u, const double t) {
  static double coeff;
  static size_t idx;
  {
    std::shared_lock<std::shared_mutex> lock(rollout_cache_mutex_);
    if (t < opt_roll_cache_.tt.front()) {
      std::stringstream warning;
      warning << "Queried time " << t << " smaller than first available time "
              << opt_roll_cache_.tt.front();
      log_warning_throttle(1.0, warning.str());
      u = dynamics_->get_zero_input(x);
    }

    auto lower = std::lower_bound(opt_roll_cache_.tt.begin(),
                                  opt_roll_cache_.tt.end(), t);
    if (lower == opt_roll_cache_.tt.end()) {
      std::stringstream warning;
      warning << "Queried time " << t << " larger than last available time "
              << opt_roll_cache_.tt.back();
      log_warning_throttle(1.0, warning.str());
      u = opt_roll_cache_.uu.back();
      return;
    }

    idx = std::distance(opt_roll_cache_.tt.begin(), lower);
    // first index (time)
    if (idx == 0) {
      u = opt_roll_cache_.uu.front();
    }
    // last index (time larget then last step)
    else if (idx > opt_roll_cache_.steps_) {
      u = opt_roll_cache_.uu.back();
    }
    // interpolate
    else {
      coeff = (t - *(lower - 1)) / (*lower - *(lower - 1));
      u = (1 - coeff) * opt_roll_cache_.uu[idx - 1] +
          coeff * opt_roll_cache_.uu[idx];
    }
  }
}

void Solver::get_diagonal_variance(Eigen::VectorXd& var) const {
  var = sampler_->sigma().diagonal();
}

void Solver::get_input_state(const observation_t& x, observation_t& x_nom,
                             input_t& u_nom, const double t) {
  {
    std::shared_lock<std::shared_mutex> lock(rollout_cache_mutex_);
    if (t < opt_roll_cache_.tt.front()) {
      std::stringstream warning;
      warning << "Queried time " << t << " smaller than first available time "
              << opt_roll_cache_.tt.front();
      log_warning_throttle(1.0, warning.str());
      x_nom = opt_roll_cache_.xx.front();
      u_nom = dynamics_->get_zero_input(x);
      return;
    }

    auto lower = std::lower_bound(opt_roll_cache_.tt.begin(),
                                  opt_roll_cache_.tt.end(), t);
    if (lower == opt_roll_cache_.tt.end()) {
      std::stringstream warning;
      warning << "Queried time " << t << " larger than last available time "
              << opt_roll_cache_.tt.back();
      log_warning_throttle(1.0, warning.str());
      x_nom = opt_roll_cache_.xx.back();
      u_nom = dynamics_->get_zero_input(x);
      return;
    }

    size_t idx = std::distance(opt_roll_cache_.tt.begin(), lower);

    // first
    if (idx == 0) {
      x_nom = opt_roll_cache_.xx.front();
      u_nom = opt_roll_cache_.uu.front();
    }
    // last
    else if (idx > opt_roll_cache_.steps_) {
      x_nom = opt_roll_cache_.xx.back();
      u_nom = opt_roll_cache_.uu.back();
    }
    // interpolate
    else {
      double coeff = (t - *(lower - 1)) / (*lower - *(lower - 1));
      u_nom = (1 - coeff) * opt_roll_cache_.uu[idx - 1] +
              coeff * opt_roll_cache_.uu[idx];
      x_nom = opt_roll_cache_.xx[idx];  // TODO offer a way to also do
                                        // interpolation of the state
    }
  }
}

bool Solver::get_optimal_rollout(observation_array_t& xx, input_array_t& uu) {
  std::shared_lock<std::shared_mutex> lock(rollout_cache_mutex_);
  auto lower = std::lower_bound(opt_roll_cache_.tt.begin(),
                                opt_roll_cache_.tt.end(), reset_time_);
  if (lower == opt_roll_cache_.tt.end()) return false;
  size_t offset = std::distance(opt_roll_cache_.tt.begin(), lower);

  // fill with portion of vector starting from current time
  xx = observation_array_t(opt_roll_cache_.xx.begin() + offset,
                           opt_roll_cache_.xx.end());
  uu = input_array_t(opt_roll_cache_.uu.begin() + offset,
                     opt_roll_cache_.uu.end());
  return true;
}

void Solver::get_optimal_rollout(Rollout& r) {
  std::shared_lock<std::shared_mutex> lock(rollout_cache_mutex_);

  // fill with portion of vector starting from current reset time
  r.tt = time_array_t(opt_roll_cache_.tt.begin(),
                      opt_roll_cache_.tt.end());

  r.xx = observation_array_t(opt_roll_cache_.xx.begin(),
                             opt_roll_cache_.xx.end());

  r.uu = input_array_t(opt_roll_cache_.uu.begin(),
                       opt_roll_cache_.uu.end());
}

void Solver::swap_policies() {
  std::unique_lock<std::shared_mutex> lock(rollout_cache_mutex_);
  opt_roll_cache_ = opt_roll_;
}

template <typename T>
void Solver::shift_back(std::vector<T>& v_out, const T& fill,
                        const int offset) {
  std::rotate(v_out.begin(), v_out.begin() + offset, v_out.end());
  std::fill(v_out.end() - offset, v_out.end(), fill);
}

// explicit instantiation
template void Solver::shift_back<Eigen::VectorXd>(
    std::vector<Eigen::VectorXd>& v_out, const Eigen::VectorXd& fill,
    const int offset);

void Solver::print_cost_histogram() const {
  constexpr int nbins = 10;
  double delta = (max_cost_ - min_cost_) / nbins;

  std::cout << "Rollouts cost histogram " << std::endl;
  std::cout << "Max: " << max_cost_ << ", Min: " << min_cost_
            << ", delta: " << delta << std::endl;
  std::map<int, int> hist{};
  for (const auto& cost : rollouts_cost_) {
    ++hist[std::round((cost - min_cost_) / delta)];
  }
  for (auto p : hist) {
    std::cout << std::setw(2) << p.first << ' ' << std::string(p.second, '*')
              << '\n';
  }
  std::cout << std::endl << std::endl;
}

}  // namespace mppi


//==============================================================================
//         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_IEEE_FUNCTIONS_SCALAR_BITFLOATING_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_BITFLOATING_HPP_INCLUDED

#include <boost/simd/ieee/functions/bitfloating.hpp>
#include <boost/simd/include/functions/scalar/bitwise_cast.hpp>
#include <boost/simd/include/functions/scalar/shli.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/signmask.hpp>
#include <boost/dispatch/meta/as_floating.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::bitfloating_, tag::cpu_,
                             (A0),
                             (scalar_<arithmetic_<A0> > )
                            )
  {
    typedef typename dispatch::meta::as_floating<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return a0 >= Zero<A0>() ?
      bitwise_cast<result_type>(a0) :
      bitwise_cast<result_type>(Signmask<A0>()-a0);
    }
  };

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is unsigned
/////////////////////////////////////////////////////////////////////////////

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::bitfloating_, tag::cpu_,
                             (A0),
                             (scalar_<unsigned_<A0> > )
                            )
  {
    typedef typename dispatch::meta::as_floating<A0>::type result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return boost::simd::bitwise_cast<result_type>(a0);
    }
  };
} } }
#endif


#include "shell_solid_solver.h"
#include <deal.II/grid/grid_out.h>
#include <fstream>

namespace Solid
{
  using namespace dealii;

  ShellSolidSolver::ShellSolidSolver(Triangulation<2, 3> &tria,
                                     const Parameters::AllParameters &params,
                                     libMesh::LibMeshInit *libmesh_init)
    : SolidSolver<2, 3>(tria, params),
      libmesh_init(libmesh_init),
      m_mesh(libmesh_init->comm(), 2)
  {
    shell_params.debug = false;
    shell_params.nu = parameters.nu[0];
    shell_params.em = parameters.E[0];
    shell_params.thickness = 0.1;
    shell_params.isOutfileSet = false;
  }

  void ShellSolidSolver::get_forcing_file(const std::string &file)
  {
    this->shell_params.force_filename = file;
  }

  void ShellSolidSolver::run()
  {
    setup_dofs();
    initialize_system();
    this->m_shell->run();
    grab_solution();
    grab_stress();
    this->m_shell->writeOutput();
    output_results(0);
  }

  void ShellSolidSolver::initialize_system()
  {
    current_displacement.reinit(dof_handler.n_dofs());
    current_drilling.reinit(dof_handler.n_dofs());
    strain = std::vector<std::vector<Vector<double>>>(
      3,
      std::vector<Vector<double>>(3,
                                  Vector<double>(scalar_dof_handler.n_dofs())));
    stress = std::vector<std::vector<Vector<double>>>(
      3,
      std::vector<Vector<double>>(3,
                                  Vector<double>(scalar_dof_handler.n_dofs())));

    // Set up cell property, which contains the FSI traction required in FSI
    // simulation
    cell_property.initialize(triangulation.begin_active(),
                             triangulation.end(),
                             GeometryInfo<2>::faces_per_cell);
  }

  void ShellSolidSolver::setup_dofs()
  {
    SolidSolver<2, 3>::setup_dofs();
    construct_mesh();
    // Construct BC map for internal solver
    std::map<libMesh::boundary_id_type, unsigned int> dirichlet_bcs;
    for (const auto &bc : parameters.solid_dirichlet_bcs)
      {
        dirichlet_bcs[static_cast<libMesh::boundary_id_type>(bc.first)] =
          bc.second;
      }
    this->m_shell->make_constraints(dirichlet_bcs);
  }

  void ShellSolidSolver::update_strain_and_stress() {}

  void ShellSolidSolver::assemble_system(bool initial_step)
  {
    // Do nothing. We don't assemble in the wrapper
    (void)initial_step;
  }

  void ShellSolidSolver::run_one_step(bool first_step) { (void)first_step; }

  void ShellSolidSolver::construct_mesh()
  {
    m_mesh.allow_renumbering(false);
    // Add nodes
    auto vertices = this->triangulation.get_vertices();
    m_mesh.reserve_nodes(static_cast<unsigned int>(vertices.size()));
    for (int i = 0; i < static_cast<int>(vertices.size()); ++i)
      {
        auto &v = vertices[i];
        libMesh::Real x(v(0)), y(v(1)), z(v(2));
        m_mesh.add_point(libMesh::Point(x, y, z), i);
      }
    // Add elements
    m_mesh.reserve_elem(triangulation.n_active_cells());
    for (auto cell = triangulation.begin_active(); cell != triangulation.end();
         ++cell)
      {
        libMesh::Elem *elem = libMesh::Elem::build(libMesh::QUAD4).release();
        elem->set_id(cell->index());
        elem->subdomain_id() =
          static_cast<libMesh::subdomain_id_type>(cell->material_id());
        elem = m_mesh.add_elem(elem);
        // Vertices order in dealii is different from libMesh
        std::vector<unsigned int> vertices{0, 1, 3, 2};
        unsigned int v_lm = 0;
        for (auto &&v : vertices)
          {
            elem->set_node(v_lm++) = m_mesh.node_ptr(cell->vertex_index(v));
          }
        // Add boundary ids
        std::vector<unsigned int> sides{3, 1, 0, 2};
        if (cell->at_boundary())
          {
            for (unsigned int f = 0; f < GeometryInfo<2>::faces_per_cell; ++f)
              {
                if (cell->face(f)->at_boundary())
                  {
                    m_mesh.get_boundary_info().add_side(
                      elem, sides[f], cell->face(f)->boundary_id());
                  }
              }
          }
      }
    m_mesh.set_mesh_dimension(2);
    m_mesh.set_spatial_dimension(3);
    m_mesh.prepare_for_use();
    // Create internal shell solid solver
    this->m_shell =
      std::make_unique<ShellSolid::shellsolid>(m_mesh, this->shell_params);
  }

  void ShellSolidSolver::synchronize()
  {
    grab_solution();
    grab_stress();
  }

  void ShellSolidSolver::grab_solution()
  {
    std::vector<bool> vertex_touched(triangulation.n_vertices(), false);
    const std::vector<libMesh::Number> &solution(m_shell->get_solution());
    AssertThrow(solution.size() == current_displacement.size() * 5,
                ExcMessage("Inconsistent solution size!"));
    // Copy the solutions
    for (auto cell = dof_handler.begin_active(); cell != dof_handler.end();
         ++cell)
      {
        for (unsigned int v = 0; v < GeometryInfo<2>::vertices_per_cell; ++v)
          {
            if (!vertex_touched[cell->vertex_index(v)])
              {
                vertex_touched[cell->vertex_index(v)] = true;
                for (unsigned int n : {0, 1, 2})
                  {
                    current_displacement(cell->vertex_dof_index(v, n)) =
                      solution[15 * cell->vertex_index(v) + n];
                    current_drilling(cell->vertex_dof_index(v, n)) =
                      solution[15 * cell->vertex_index(v) + 3 + n];
                  }
              }
          }
      }
  }

  void ShellSolidSolver::push_solution()
  {
    std::vector<bool> vertex_touched(triangulation.n_vertices(), false);
    std::vector<libMesh::Number> solution(current_displacement.size() * 2);
    // Copy the solutions
    for (auto cell = dof_handler.begin_active(); cell != dof_handler.end();
         ++cell)
      {
        for (unsigned int v = 0; v < GeometryInfo<2>::vertices_per_cell; ++v)
          {
            if (!vertex_touched[cell->vertex_index(v)])
              {
                vertex_touched[cell->vertex_index(v)] = true;
                for (unsigned int n : {0, 1, 2})
                  {
                    solution[6 * cell->vertex_index(v) + n] =
                      current_displacement(cell->vertex_dof_index(v, n));
                  }
              }
          }
      }
    this->m_shell->set_solution(solution);
  }

  void ShellSolidSolver::grab_stress()
  {
    std::vector<bool> vertex_touched(triangulation.n_vertices(), false);
    const std::vector<libMesh::Number> &solution(m_shell->get_solution());
    AssertThrow(solution.size() == current_displacement.size() * 5,
                ExcMessage("Inconsistent solution size!"));
    // Copy the solutions
    for (auto cell = scalar_dof_handler.begin_active();
         cell != scalar_dof_handler.end();
         ++cell)
      {
        for (unsigned int v = 0; v < GeometryInfo<2>::vertices_per_cell; ++v)
          {
            if (!vertex_touched[cell->vertex_index(v)])
              {
                vertex_touched[cell->vertex_index(v)] = true;
                for (unsigned int i : {0, 1, 2})
                  {
                    for (unsigned int j : {0, 1, 2})
                      stress[i][j](cell->vertex_dof_index(v, 0)) =
                        solution[15 * cell->vertex_index(v) + 3 * i + j];
                  }
              }
          }
      }
  }

  void ShellSolidSolver::output_results(const unsigned int output_index)
  {
    TimerOutput::Scope timer_section(timer, "Output results");

    std::vector<std::string> solution_names(3, "displacements");

    std::vector<DataComponentInterpretation::DataComponentInterpretation>
      data_component_interpretation(
        3, DataComponentInterpretation::component_is_part_of_vector);
    DataOut<2, DoFHandler<2, 3>> data_out;
    data_out.attach_dof_handler(dof_handler);

    // displacements
    data_out.add_data_vector(dof_handler,
                             current_displacement,
                             solution_names,
                             data_component_interpretation);
    // velocity
    solution_names = std::vector<std::string>(3, "drillings");
    data_out.add_data_vector(dof_handler,
                             current_drilling,
                             solution_names,
                             data_component_interpretation);

    // strain and stress
    data_out.add_data_vector(scalar_dof_handler, stress[0][0], "Sxx");
    data_out.add_data_vector(scalar_dof_handler, stress[0][1], "Sxy");
    data_out.add_data_vector(scalar_dof_handler, stress[1][1], "Syy");
    data_out.add_data_vector(scalar_dof_handler, stress[0][2], "Sxz");
    data_out.add_data_vector(scalar_dof_handler, stress[1][2], "Syz");
    data_out.add_data_vector(scalar_dof_handler, stress[2][2], "Szz");

    data_out.build_patches();

    std::string basename = "solid";
    std::string filename =
      basename + "-" + Utilities::int_to_string(output_index, 6) + ".vtu";

    std::ofstream output(filename);
    data_out.write_vtu(output);

    static std::vector<std::pair<double, std::string>> times_and_names;
    times_and_names.push_back({time.current(), filename});
    std::ofstream pvd_output(basename + ".pvd");
    DataOutBase::write_pvd_record(pvd_output, times_and_names);
  }
} // namespace Solid


#include <map>
#include <vector>
#include <mutex>
#include <stdexcept>
#include <random>
#include <algorithm>
#include <fstream>
#include <iostream>
#include <string>
#include <memory>
#include <cmath>
#include <thread>

#include <Eigen/Dense>

#include <viennacl/vector.hpp>
#include <viennacl/scalar.hpp>
#include <viennacl/matrix.hpp>

#include <cereal/cereal.hpp>
#include <cereal/types/polymorphic.hpp>
#include <cereal/types/vector.hpp>
#include <cereal/access.hpp>
#include <cereal/types/utility.hpp>
#include <cereal/types/base_class.hpp>
#include <cereal/archives/xml.hpp>
#include <cereal/archives/portable_binary.hpp>
#include <cereal/archives/json.hpp>

#include "IO/UseParentSerialization.hpp"
#include "IO/TemplateDeclaration.hpp"
#include "LinearAlgebra/Vector.hpp"
#include "LinearAlgebra/Matrix.hpp"
#include "LinearAlgebra/Scalar.hpp"


// Copyright (c) 2019 The Unit-e developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <esperanza/finalizationparams.h>
#include <test/test_unite.h>
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(finalization_params_tests)

BOOST_AUTO_TEST_CASE(get_epoch_start_height) {
  esperanza::FinalizationParams params;
  params.epoch_length = 5;

  BOOST_CHECK_EQUAL(params.GetEpochStartHeight(0), 0);
  BOOST_CHECK_EQUAL(params.GetEpochStartHeight(1), 1);
  BOOST_CHECK_EQUAL(params.GetEpochStartHeight(2), 6);
  BOOST_CHECK_EQUAL(params.GetEpochStartHeight(3), 11);

  params.epoch_length = 42;
  BOOST_CHECK_EQUAL(params.GetEpochStartHeight(0), 0);
  BOOST_CHECK_EQUAL(params.GetEpochStartHeight(1), 1);
  BOOST_CHECK_EQUAL(params.GetEpochStartHeight(2), 43);
  BOOST_CHECK_EQUAL(params.GetEpochStartHeight(3), 85);
}

BOOST_AUTO_TEST_CASE(get_epoch_checkpoint_height) {
  esperanza::FinalizationParams params;
  params.epoch_length = 5;

  BOOST_CHECK_EQUAL(params.GetEpochCheckpointHeight(0), 0);
  BOOST_CHECK_EQUAL(params.GetEpochCheckpointHeight(1), 5);
  BOOST_CHECK_EQUAL(params.GetEpochCheckpointHeight(2), 10);
  BOOST_CHECK_EQUAL(params.GetEpochCheckpointHeight(3), 15);

  params.epoch_length = 50;
  BOOST_CHECK_EQUAL(params.GetEpochCheckpointHeight(0), 0);
  BOOST_CHECK_EQUAL(params.GetEpochCheckpointHeight(1), 50);
  BOOST_CHECK_EQUAL(params.GetEpochCheckpointHeight(2), 100);
  BOOST_CHECK_EQUAL(params.GetEpochCheckpointHeight(3), 150);
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ExecutionPhaseInitOpAccumulatorTest

#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <iostream>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/ir.hpp>
#include <popart/op/init.hpp>
#include <popart/op/l1.hpp>
#include <popart/popx/devicex.hpp>
#include <popart/popx/executablex.hpp>
#include <popart/popx/irlowering.hpp>
#include <popart/testdevice.hpp>

using namespace popart;
using namespace std;

// This test simulates matmul with accumulation over multiple execution phases.
// It checks that the InitOp tensor layout is efficient for this use-case.
//
// 1/ the even and odd execution phases accumulate their own matmul result.
// 2/ a final reduce is made to sum even and odd results.
//
// The initOp should be laid out efficiently to minimise internal exchange
// for the addOp. In this case that requires a layout according to the addOp's
// RHS layout. This is the result of the matmul, so should be non-linear.
BOOST_AUTO_TEST_CASE(TestInitOpAccumulator) {
  int N         = 4;
  int size      = 4;
  bool training = false;

  auto builder     = Builder::create();
  auto aiOnnx      = builder->aiOnnxOpset9();
  auto aiGraphcore = builder->aiGraphcoreOpset1();

  // Weights are [size, size]
  TensorInfo wInfo{"FLOAT", std::vector<int64_t>{size, size}};
  std::vector<float> wData(wInfo.nelms(), 0);
  ConstVoidData wCVData{wData.data(), wInfo};

  // Input are [size, size]
  TensorInfo inInfo{"FLOAT", std::vector<int64_t>{size, size}};
  auto input = builder->addInputTensor(inInfo);

  // Even Accumulations are [size, size]
  auto init0 = aiGraphcore.init(std::vector<int64_t>{size, size},
                                static_cast<int64_t>(DataType::FLOAT),
                                static_cast<int64_t>(InitType::Zero),
                                "even_accumulator");
  builder->executionPhase(init0, 0);
  builder->virtualGraph(init0, 0);

  // Odd Accumulations are [size, size]
  auto init1 = aiGraphcore.init(std::vector<int64_t>{size, size},
                                static_cast<int64_t>(DataType::FLOAT),
                                static_cast<int64_t>(InitType::Zero),
                                "odd_accumulator");
  builder->executionPhase(init1, 1);
  builder->virtualGraph(init1, 1);

  // 2N phases
  for (int n = 0; n < 2 * N; ++n) {
    const VGraphId vgid = (n % 2);

    // Weights are common.
    auto w = builder->addInitializedInputTensor(wCVData);

    // inputs x w
    auto out = aiOnnx.matmul(
        {input, w}, logging::format("CHECKOP_MM: [{} {}]", n, vgid % 2));
    builder->executionPhase(out, n);
    builder->virtualGraph(out, vgid);

    out = aiOnnx.relu({out},
                      logging::format("CHECKOP_RELU: [{} {}]", n, vgid % 2));
    builder->executionPhase(out, n);
    builder->virtualGraph(out, vgid);

    if (n % 2) {
      // Odd phase accumulation
      init1 = aiOnnx.add({init1, out},
                         logging::format("CHECKOP_ACC: [{} {}]", n, vgid % 2));
      builder->executionPhase(init1, n);
      builder->virtualGraph(init1, vgid);
    } else {
      // Even phase accumulation (swapped operand order)
      init0 = aiOnnx.add({out, init0},
                         logging::format("CHECKOP_ACC: [{} {}]", n, vgid % 2));
      builder->executionPhase(init0, n);
      builder->virtualGraph(init0, vgid);
    }
  }

  // Reduce by adding even and odd accumulators together (within final phase)
  auto init = aiOnnx.add({init0, init1}, "reduce_add");
  builder->executionPhase(init, N * 2 - 1);
  builder->virtualGraph(init, 1);

  SessionOptions session_opts;

  // Large model settings
  session_opts.explicitRecomputation = true;
  session_opts.enableOutlining       = true;
  session_opts.aliasZeroCopy         = true;
  session_opts.constantWeights       = false;

  // TODO: T17972
  // AddOpx::getInputCreatorType currently has a constraint such that
  // it will only unwind if decomposeGradSum or batchSerialization is enabled.
  session_opts.decomposeGradSum = true;

  session_opts.executionPhaseSettings.phases = N * 2;
  session_opts.virtualGraphMode = VirtualGraphMode::ExecutionPhases;

  auto testDev = createTestDevice(TEST_TARGET, 2);

  Ir ir;
  ir.prepare({
      io::getModelFromString(builder->getModelProto()),
      InputShapeInfo(),
      DataFlow(1, {{init, AnchorReturnType("ALL")}}),
      {}, // no loss
      {}, // no optimizer
      *testDev,
      session_opts,
      {}, // Patterns({}).enableRuntimeAsserts(false)
  });

  // Compile.
  std::unique_ptr<popx::IrLowering> lowering;
  lowering.reset(new popx::IrLowering(ir, testDev));

  std::unique_ptr<popx::Executablex> executable =
      std::move(popx::Executablex::createFromLoweredIr(*lowering));

  std::unique_ptr<popx::Devicex> device;
  device.reset(new popx::Devicex(*executable, testDev));
  device->prepare();

  // Get resultant tensors (efficient v linear).
  auto efficient = device->getEfficientlyCreatedInputTensors();
  auto linear    = device->getLinearlyCreatedInputTensors();

  auto print = [](const string &prefix, const auto &ts) {
    cout << prefix;
    for (auto &&t : ts)
      cout << t << " ";
    cout << endl;
  };

  print("Efficient:", efficient);
  print("Linear:", linear);

  // Check the linearly mapped set.
  // Only "RemoteArg" or "Constant" should appear here.
  for (auto &&t : linear) {
    if (t.find("RemoteArg") == std::string::npos &&
        t.find("Constant") == std::string::npos)
      BOOST_ERROR(
          logging::format("Tensor should not be mapped linearly: {}", t));
  }
}


#include <boost/python.hpp>
#include <rstbx/bpcx/detector_model/sensor.h>
#include <boost/python/overloads.hpp>

namespace rstbx { namespace detector_model {

    using namespace boost::python;

    struct detector_model_wrappers
    {
        typedef scitbx::vec3<double> v3;
        typedef scitbx::vec2<double> v2;

        static void wrap()
        {
            class_<sensor>("sensor", init<const v3&,
                                          const v3&,
                                          const v3&,
                                          const v2&,
                                          const v2&>())
                .add_property("distance", &sensor::get_distance)
                .add_property("origin", &sensor::get_origin)
                .add_property("normal", &sensor::get_normal)
                .add_property("dir1", &sensor::get_dir1)
                .add_property("dir2", &sensor::get_dir2)
                .add_property("lim1", &sensor::get_lim1)
                .add_property("lim2", &sensor::get_lim2)
                .add_property("D", &sensor::get_D)
                .add_property("d", &sensor::get_d)
                .add_property("d_is_invertible",
                              &sensor::get_d_is_invertible)
                .def("set_frame", &sensor::set_frame)
                .def("set_origin", &sensor::set_origin);
        }
    };

    void init_module()
    {
        detector_model_wrappers::wrap();
    }

}} //namespace rstbx::detector_model

BOOST_PYTHON_MODULE(rstbx_bpcx_detector_model_ext)
{
    rstbx::detector_model::init_module();
}