//  Copyright (c) 2006-2009, Bernhard Reiter
//
//  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 LIBS_TREE_BINARY_TREE_TEST_DATA_HPP
#define LIBS_TREE_BINARY_TREE_TEST_DATA_HPP

#include <boost/tree/binary_tree.hpp>

template <class T = int>
struct test_binary_tree_fixture {
    test_binary_tree_fixture()
    {
        create_test_dataset1_tree(bt);
        create_test_dataset1_tree(bt2);

        typename boost::tree::binary_tree<T>::cursor d = bt2.root();

        // Just to make sure we won't be getting any false positives when
        // copying test_tree1 to test_tree2, we'll change one of test_tree2's
        // values.
        d = d.begin().end().begin();
        *d = T(29);
    }

    // Test data from http://en.wikipedia.org/wiki/Image:Binary_search_tree.svg
    // (With two additional nodes: 11 inserted left of 13; 12 right of 11)
    // and in combination with http://en.wikipedia.org/wiki/Tree_traversal#Examples
    // (as tree shapes are equal [apart from the extra nodes])
    static void create_test_dataset1_tree(boost::tree::binary_tree<T>& ret)
    {
        // For augmented trees. (Why is this necessary? Nothing here is explicit!)
        typedef typename boost::tree::binary_tree<T>::value_type value_type;

        typename boost::tree::binary_tree<T>::cursor cur = ret.insert(ret.root(), value_type(8));
        cur = ret.insert(cur.to_begin(), value_type(3));
        ret.insert(cur.to_begin(), value_type(1));
        cur = ret.insert(++cur, value_type(6));
        ret.insert(cur.to_begin(), value_type(4));
        ret.insert(++cur, value_type(7));
        cur = ret.insert(ret.root().end(), value_type(10));
        cur = ret.insert(ret.root().end().end(), value_type(14));
        //cur = ret.insert(cur.to_begin(), value_type(13));
        // First insert 11 as left child of 14, 12 as child of 11
        cur = ret.insert(cur.to_begin(), value_type(11));
        cur = ret.insert(++cur.to_begin(), value_type(12));
        // Now insert 13 as left child of 14, which makes it 11's parent
        ret.insert(ret.root().end().end().begin(), value_type(13));
    }

    boost::tree::binary_tree<T> bt, bt2;
};

#endif // LIBS_TREE_BINARY_TREE_TEST_DATA_HPP


#define PY_ARRAY_UNIQUE_SYMBOL pbcvt_ARRAY_API

#include <boost/python.hpp>
#include <pyboostcvconverter/pyboostcvconverter.hpp>

namespace pbcvt {

    using namespace boost::python;
    // using namespace cv;

/**
 * @brief Example function. Basic inner matrix product using explicit matrix conversion.
 * @param left left-hand matrix operand (NdArray required)
 * @param right right-hand matrix operand (NdArray required)
 * @return an NdArray representing the dot-product of the left and right operands
 */
    PyObject *dot(PyObject *left, PyObject *right) {

        cv::Mat leftMat, rightMat;
        leftMat = pbcvt::fromNDArrayToMat(left);
        rightMat = pbcvt::fromNDArrayToMat(right);
        auto c1 = leftMat.cols, r2 = rightMat.rows;
        // Check that the 2-D matrices can be legally multiplied.
        if (c1 != r2) {
            PyErr_SetString(PyExc_TypeError,
                            "Incompatible sizes for matrix multiplication.");
            throw_error_already_set();
        }
        cv::Mat result = leftMat * rightMat;
        PyObject *ret = pbcvt::fromMatToNDArray(result);
        return ret;
    }
/**
 * @brief Example function. Simply makes a new CV_16UC3 matrix and returns it as a numpy array.
 * @return The resulting numpy array.
 */

	PyObject* makeCV_16UC3Matrix(){
		cv::Mat image = cv::Mat::zeros(240,320, CV_16UC3);
		PyObject* py_image = pbcvt::fromMatToNDArray(image);
		return py_image;
	}

//
/**
 * @brief Example function. Basic inner matrix product using implicit matrix conversion.
 * @details This example uses Mat directly, but we won't need to worry about the conversion in the body of the function.
 * @param leftMat left-hand matrix operand
 * @param rightMat right-hand matrix operand
 * @return an NdArray representing the dot-product of the left and right operands
 */
    cv::Mat dot2(cv::Mat leftMat, cv::Mat rightMat) {
        auto c1 = leftMat.cols, r2 = rightMat.rows;
        if (c1 != r2) {
            PyErr_SetString(PyExc_TypeError,
                            "Incompatible sizes for matrix multiplication.");
            throw_error_already_set();
        }
        cv::Mat result = leftMat * rightMat;

        return result;
    }

    /**
     * \brief Example function. Increments all elements of the given matrix by one.
     * @details This example uses Mat directly, but we won't need to worry about the conversion anywhere at all,
     * it is handled automatically by boost.
     * \param matrix (numpy array) to increment
     * \return
     */
    cv::Mat increment_elements_by_one(cv::Mat matrix){
        matrix += 1.0;
        return matrix;
    }

    void thinningIteration(cv::Mat& img, int iter)
    {
        CV_Assert(img.channels() == 1);
        CV_Assert(img.depth() != sizeof(uchar));
        CV_Assert(img.rows > 3 && img.cols > 3);

        cv::Mat marker = cv::Mat::zeros(img.size(), CV_8UC1);

        int nRows = img.rows;
        int nCols = img.cols;

        if (img.isContinuous()) {
            nCols *= nRows;
            nRows = 1;
        }

        int x, y;
        uchar *pAbove;
        uchar *pCurr;
        uchar *pBelow;
        uchar *nw, *no, *ne;    // north (pAbove)
        uchar *we, *me, *ea;
        uchar *sw, *so, *se;    // south (pBelow)

        uchar *pDst;

        // initialize row pointers
        pAbove = NULL;
        pCurr  = img.ptr<uchar>(0);
        pBelow = img.ptr<uchar>(1);

        for (y = 1; y < img.rows-1; ++y) {
            // shift the rows up by one
            pAbove = pCurr;
            pCurr  = pBelow;
            pBelow = img.ptr<uchar>(y+1);

            pDst = marker.ptr<uchar>(y);

            // initialize col pointers
            no = &(pAbove[0]);
            ne = &(pAbove[1]);
            me = &(pCurr[0]);
            ea = &(pCurr[1]);
            so = &(pBelow[0]);
            se = &(pBelow[1]);

            for (x = 1; x < img.cols-1; ++x) {
                // shift col pointers left by one (scan left to right)
                nw = no;
                no = ne;
                ne = &(pAbove[x+1]);
                we = me;
                me = ea;
                ea = &(pCurr[x+1]);
                sw = so;
                so = se;
                se = &(pBelow[x+1]);

                int A  = (*no == 0 && *ne == 1) + (*ne == 0 && *ea == 1) +
                         (*ea == 0 && *se == 1) + (*se == 0 && *so == 1) +
                         (*so == 0 && *sw == 1) + (*sw == 0 && *we == 1) +
                         (*we == 0 && *nw == 1) + (*nw == 0 && *no == 1);
                int B  = *no + *ne + *ea + *se + *so + *sw + *we + *nw;
                int m1 = iter == 0 ? (*no * *ea * *so) : (*no * *ea * *we);
                int m2 = iter == 0 ? (*ea * *so * *we) : (*no * *so * *we);

                if (A == 1 && (B >= 2 && B <= 6) && m1 == 0 && m2 == 0)
                    pDst[x] = 1;
            }
        }

        img &= ~marker;
    }

    /**
     * Function for thinning the given binary image
     *
     * Parameters:
     *      src  The source image, binary with range = [0,255]
     *      dst  The destination image
     */
    void thinning(const cv::Mat& src, cv::Mat& dst)
    {
        dst = src.clone();
        dst /= 255;         // convert to binary image

        cv::Mat prev = cv::Mat::zeros(dst.size(), CV_8UC1);
        cv::Mat diff;

        do {
            thinningIteration(dst, 0);
            thinningIteration(dst, 1);
            cv::absdiff(dst, prev, diff);
            dst.copyTo(prev);
        }
        while (cv::countNonZero(diff) > 0);

        dst *= 255;
    }

    /**
     * This is an example on how to call the thinning funciton above
     */
    cv::Mat run(cv::Mat src)
    {
    //    auto start = system_clock::now();
        auto start = std::chrono::system_clock::now();
        // cv::Mat src = cv::imread(src_img);
        cv::Mat src_inv;
        // if (!src.data)
        //  return -1;
        cv::threshold(src,src_inv, 200, 255, CV_THRESH_BINARY_INV);
        morphologyEx(src_inv,src_inv,cv::MORPH_OPEN,cv::Mat(5,5,CV_8U),cv::Point(-1,-1),1);
        cv::Mat bw;
        cv::cvtColor(src_inv, bw, CV_BGR2GRAY);
        cv::threshold(bw, bw, 100, 255, CV_THRESH_BINARY);

        thinning(bw, bw);

    //  cv::imshow("src", src);
    //  cv::imshow("dst", bw);
        // cv::imwrite("test.png", bw);
        auto end = std::chrono::system_clock::now();
        double elapsed_seconds = std::chrono::duration_cast<
                std::chrono::duration<double> >(end - start).count();
        // std::cout <<  "consum " << elapsed_seconds << "s" << std::endl;
    //  cv::waitKey();
        // return elapsed_seconds;
        return bw;
    }

#if (PY_VERSION_HEX >= 0x03000000)

    static void *init_ar() {
#else
        static void init_ar(){
#endif
        Py_Initialize();

        import_array();
        return NUMPY_IMPORT_ARRAY_RETVAL;
    }

    BOOST_PYTHON_MODULE (pbcvt) {
        //using namespace XM;
        init_ar();

        //initialize converters
        to_python_converter<cv::Mat,pbcvt::matToNDArrayBoostConverter>();
        matFromNDArrayBoostConverter();

        //expose module-level functions
        def("dot", dot);
        def("dot2", dot2);
		def("makeCV_16UC3Matrix", makeCV_16UC3Matrix);

		//from PEP8 (https://www.python.org/dev/peps/pep-0008/?#prescriptive-naming-conventions)
        //"Function names should be lowercase, with words separated by underscores as necessary to improve readability."
        def("increment_elements_by_one", increment_elements_by_one);
        def("thinning", run);
    }

} //end namespace pbcvt


#include <boost/functional/hash.hpp>
#include <sstream>
#include <string>
#include <unordered_map>
#include <utility>

#include <doctest/doctest.h>

#include "solver_13_part1.h"

namespace {

using coord = std::pair<unsigned int, unsigned int>;

}// namespace

std::string Solver_13_part1::solve(std::istream &is)
{
  std::unordered_map<coord, bool, boost::hash<coord>> point_map_alpha;
  std::unordered_map<coord, bool, boost::hash<coord>> point_map_beta;

  // Fold boundary during fold operation, right / bottom boundaries any other time
  unsigned int x_fold = 0;
  unsigned int y_fold = 0;

  {
    unsigned int coord_x = 0;
    unsigned int coord_y = 0;
    while (is >> std::ws && is.peek() != 'f' && is >> coord_x && is.ignore(1) && is >> coord_y) {
      point_map_alpha.emplace(std::make_pair(coord_x, coord_y), true);

      if (coord_x + 1 > x_fold) {
        x_fold = coord_x + 1;
      }

      if (coord_y + 1 > y_fold) {
        y_fold = coord_y + 1;
      }
    }
  }

  // Ingest and perform fold commands
  {
    std::string command;
    is >> std::ws;
    std::getline(is, command, '=');

    const auto &source_point_map = point_map_alpha;
    auto &dest_point_map = point_map_beta;

    if (command == "fold along x") {
      is >> x_fold;

      for (const auto &point : source_point_map) {
        if (point.first.first > x_fold * 2 || point.first.second > y_fold) {
          // Point past fold boundaries (not including current fold operation)
          continue;
        }

        if (point.first.first > x_fold) {
          dest_point_map.emplace(std::make_pair(x_fold * 2 - point.first.first, point.first.second), true);
        } else {
          dest_point_map.emplace(std::make_pair(point.first.first, point.first.second), true);
        }
      }
    } else if (command == "fold along y") {
      is >> y_fold;

      for (const auto &point : source_point_map) {
        if (point.first.first > x_fold || point.first.second > y_fold * 2) {
          // Point past fold boundaries (not including current fold operation)
          continue;
        }

        if (point.first.second > y_fold) {
          dest_point_map.emplace(std::make_pair(point.first.first, y_fold * 2 - point.first.second), true);
        } else {
          dest_point_map.emplace(std::make_pair(point.first.first, point.first.second), true);
        }
      }
    } else {
      throw solver_runtime_error("Could not process fold command: " + command);
    }
  }

  const auto &point_map = point_map_beta;

  unsigned long result = 0;

  for (unsigned int y = 0; y <= y_fold; ++y) {
    for (unsigned int x = 0; x <= x_fold; ++x) {
      if (point_map.find(std::make_pair(x, y)) != point_map.end()) {
        result += 1;
      }
    }
  }

  return std::to_string(result);
}

TEST_CASE("testing solver for day 13 part 1 - dotted paper folding, just first fold")
{
  Solver_13_part1 solver;

  std::istringstream is(std::string{ R"(
      6,10
      0,14
      9,10
      0,3
      10,4
      4,11
      6,0
      6,12
      4,1
      0,13
      10,12
      3,4
      3,0
      8,4
      1,10
      2,14
      8,10
      9,0

      fold along y=7
      fold along x=5
    )" });

  CHECK(solver.solve(is) == "17");
}


              //
// Copyright (c) 2016-2018 CNRS
//

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/frames.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/utils/timer.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

template<typename Derived>
inline bool isFinite(const Eigen::MatrixBase<Derived> & x)
{
  return ((x - x).array() == (x - x).array()).all();
}

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE(frame_basic)
{
  using namespace pinocchio;
  Model model;
  buildModels::humanoidRandom(model);

  BOOST_CHECK(model.frames.size() >= size_t(model.njoints));
  for(Model::FrameVector::const_iterator it = model.frames.begin();
      it != model.frames.end(); ++it)
  {
    const Frame & frame = *it;
    BOOST_CHECK(frame == frame);
    Frame frame_copy(frame);
    BOOST_CHECK(frame_copy == frame);
  }

  std::ostringstream os;
  os << Frame() << std::endl;
  BOOST_CHECK(!os.str().empty());
}

BOOST_AUTO_TEST_CASE(cast)
{
  using namespace pinocchio;
  Frame frame("toto",0,0,SE3::Random(),OP_FRAME);

  BOOST_CHECK(frame.cast<double>() == frame);
  BOOST_CHECK(frame.cast<long double>().cast<double>() == frame);
}

BOOST_AUTO_TEST_CASE ( test_kinematics )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
  const SE3 & framePlacement = SE3::Random();
  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
  pinocchio::Data data(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.middleRows<4> (3).normalize();
  framesForwardKinematics(model, data, q);

  BOOST_CHECK(data.oMf[model.getFrameId(frame_name)].isApprox(data.oMi[parent_idx]*framePlacement));

}

BOOST_AUTO_TEST_CASE ( test_update_placements )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
  const SE3 & framePlacement = SE3::Random();
  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
  Model::FrameIndex frame_idx = model.getFrameId(frame_name);
  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.middleRows<4> (3).normalize();

  forwardKinematics(model, data, q);
  updateFramePlacements(model, data);

  framesForwardKinematics(model, data_ref, q);

  BOOST_CHECK(data.oMf[frame_idx].isApprox(data_ref.oMf[frame_idx]));
}

BOOST_AUTO_TEST_CASE ( test_update_single_placement )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
  const SE3 & framePlacement = SE3::Random();
  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
  Model::FrameIndex frame_idx = model.getFrameId(frame_name);
  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.middleRows<4> (3).normalize();

  forwardKinematics(model, data, q);
  updateFramePlacement(model, data, frame_idx);

  framesForwardKinematics(model, data_ref, q);

  BOOST_CHECK(data.oMf[frame_idx].isApprox(data_ref.oMf[frame_idx]));
}

BOOST_AUTO_TEST_CASE ( test_velocity )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
  const SE3 & framePlacement = SE3::Random();
  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
  Model::FrameIndex frame_idx = model.getFrameId(frame_name);
  pinocchio::Data data(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.middleRows<4> (3).normalize();
  VectorXd v = VectorXd::Ones(model.nv);
  forwardKinematics(model, data, q, v);

  Motion vf = getFrameVelocity(model, data, frame_idx);

  BOOST_CHECK(vf.isApprox(framePlacement.actInv(data.v[parent_idx])));
}

BOOST_AUTO_TEST_CASE ( test_acceleration )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
  const SE3 & framePlacement = SE3::Random();
  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
  Model::FrameIndex frame_idx = model.getFrameId(frame_name);
  pinocchio::Data data(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.middleRows<4> (3).normalize();
  VectorXd v = VectorXd::Ones(model.nv);
  VectorXd a = VectorXd::Ones(model.nv);
  forwardKinematics(model, data, q, v, a);

  Motion af = getFrameAcceleration(model, data, frame_idx);

  BOOST_CHECK(af.isApprox(framePlacement.actInv(data.a[parent_idx])));
}

BOOST_AUTO_TEST_CASE ( test_get_frame_jacobian )
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);
  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
  const SE3 & framePlacement = SE3::Random();
  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
  BOOST_CHECK(model.existFrame(frame_name));

  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  model.lowerPositionLimit.head<7>().fill(-1.);
  model.upperPositionLimit.head<7>().fill( 1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v = VectorXd::Ones(model.nv);

  /// In local frame
  Model::Index idx = model.getFrameId(frame_name);
  const Frame & frame = model.frames[idx];
  BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
  Data::Matrix6x Jjj(6,model.nv); Jjj.fill(0);
  Data::Matrix6x Jff(6,model.nv); Jff.fill(0);

  computeJointJacobians(model,data,q);
  updateFramePlacement(model, data, idx);
  getFrameJacobian(model,     data,        idx, LOCAL, Jff);
  computeJointJacobians(model,data_ref,q);
  getJointJacobian(model, data_ref, parent_idx, LOCAL, Jjj);

  Motion nu_frame = Motion(Jff*v);
  Motion nu_joint = Motion(Jjj*v);

  const SE3::ActionMatrixType jXf = frame.placement.toActionMatrix();
  Data::Matrix6x Jjj_from_frame(jXf * Jff);
  BOOST_CHECK(Jjj_from_frame.isApprox(Jjj));

  BOOST_CHECK(nu_frame.isApprox(frame.placement.actInv(nu_joint), 1e-12));

  // In world frame
  getFrameJacobian(model,data,idx,WORLD,Jff);
  getJointJacobian(model, data_ref, parent_idx,WORLD, Jjj);
  BOOST_CHECK(Jff.isApprox(Jjj));
}

BOOST_AUTO_TEST_CASE ( test_frame_jacobian )
{
  using namespace Eigen;
  using namespace pinocchio;

  Model model;
  buildModels::humanoidRandom(model);
  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
  const SE3 & framePlacement = SE3::Random();
  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
  BOOST_CHECK(model.existFrame(frame_name));

  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  model.lowerPositionLimit.head<7>().fill(-1.);
  model.upperPositionLimit.head<7>().fill( 1.);
  VectorXd q = randomConfiguration(model);
  VectorXd v = VectorXd::Ones(model.nv);

  Model::Index idx = model.getFrameId(frame_name);
  const Frame & frame = model.frames[idx];
  BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
  Data::Matrix6x Jf(6,model.nv); Jf.fill(0);
  Data::Matrix6x Jf_ref(6,model.nv); Jf_ref.fill(0);

  frameJacobian(model, data_ref, q, idx, Jf);

  computeJointJacobians(model, data_ref, q);
  updateFramePlacement(model,  data_ref, idx);
  getFrameJacobian(model,      data_ref, idx, LOCAL, Jf_ref);

  BOOST_CHECK(Jf.isApprox(Jf_ref));
}

BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
{
  using namespace Eigen;
  using namespace pinocchio;

  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  Model::Index parent_idx = model.existJointName("rarm2_joint")?model.getJointId("rarm2_joint"):(Model::Index)(model.njoints-1);
  const std::string & frame_name = std::string( model.names[parent_idx]+ "_frame");
  const SE3 & framePlacement = SE3::Random();
  model.addFrame(Frame (frame_name, parent_idx, 0, framePlacement, OP_FRAME));
  pinocchio::Data data(model);
  pinocchio::Data data_ref(model);

  VectorXd q = randomConfiguration(model, -1 * Eigen::VectorXd::Ones(model.nq), Eigen::VectorXd::Ones(model.nq) );
  VectorXd v = VectorXd::Random(model.nv);
  VectorXd a = VectorXd::Random(model.nv);

  computeJointJacobiansTimeVariation(model,data,q,v);
  updateFramePlacements(model,data);

  forwardKinematics(model,data_ref,q,v,a);
  updateFramePlacements(model,data_ref);

  BOOST_CHECK(isFinite(data.dJ));

  Model::Index idx = model.getFrameId(frame_name);
  const Frame & frame = model.frames[idx];
  BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));

  Data::Matrix6x J(6,model.nv); J.fill(0.);
  Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);

  // Regarding to the world origin
  getFrameJacobian(model,data,idx,WORLD,J);
  getFrameJacobianTimeVariation(model,data,idx,WORLD,dJ);

  Motion v_idx(J*v);
  const Motion & v_ref_local = frame.placement.actInv(data_ref.v[parent_idx]);
  const Motion & v_ref = data_ref.oMf[idx].act(v_ref_local);
  BOOST_CHECK(v_idx.isApprox(v_ref));

  Motion a_idx(J*a + dJ*v);
  const Motion & a_ref_local = frame.placement.actInv(data_ref.a[parent_idx]);
  const Motion & a_ref = data_ref.oMf[idx].act(a_ref_local);
  BOOST_CHECK(a_idx.isApprox(a_ref));

  J.fill(0.);  dJ.fill(0.);
  // Regarding to the local frame
  getFrameJacobian(model,data,idx,LOCAL,J);
  getFrameJacobianTimeVariation(model,data,idx,LOCAL,dJ);

  v_idx = (Motion::Vector6)(J*v);
  BOOST_CHECK(v_idx.isApprox(v_ref_local));

  a_idx = (Motion::Vector6)(J*a + dJ*v);
  BOOST_CHECK(a_idx.isApprox(a_ref_local));

  // compare to finite differencies
  {
    Data data_ref(model), data_ref_plus(model);

    const double alpha = 1e-8;
    Eigen::VectorXd q_plus(model.nq);
    q_plus = integrate(model,q,alpha*v);

    //data_ref
    Data::Matrix6x J_ref_world(6,model.nv), J_ref_local(6,model.nv);
    J_ref_world.fill(0.);     J_ref_local.fill(0.);
    computeJointJacobians(model,data_ref,q);
    updateFramePlacements(model,data_ref);
    const SE3 & oMf_q = data_ref.oMf[idx];
    getFrameJacobian(model,data_ref,idx,WORLD,J_ref_world);
    getFrameJacobian(model,data_ref,idx,LOCAL,J_ref_local);

    //data_ref_plus
    Data::Matrix6x J_ref_plus_world(6,model.nv), J_ref_plus_local(6,model.nv);
    J_ref_plus_world.fill(0.);    J_ref_plus_local.fill(0.);
    computeJointJacobians(model,data_ref_plus,q_plus);
    updateFramePlacements(model,data_ref_plus);
    const SE3 & oMf_qplus = data_ref_plus.oMf[idx];
    getFrameJacobian(model,data_ref_plus,idx,WORLD,J_ref_plus_world);
    getFrameJacobian(model,data_ref_plus,idx,LOCAL,J_ref_plus_local);

    //Move J_ref_plus_local to reference frame
    J_ref_plus_local = (oMf_q.inverse()*oMf_qplus).toActionMatrix()*(J_ref_plus_local);

    Data::Matrix6x dJ_ref_world(6,model.nv), dJ_ref_local(6,model.nv);
    dJ_ref_world.fill(0.);        dJ_ref_local.fill(0.);
    dJ_ref_world = (J_ref_plus_world - J_ref_world)/alpha;
    dJ_ref_local = (J_ref_plus_local - J_ref_local)/alpha;

    //data
    computeJointJacobiansTimeVariation(model,data,q,v);
    forwardKinematics(model,data,q,v);
    updateFramePlacements(model,data);
    Data::Matrix6x dJ_world(6,model.nv), dJ_local(6,model.nv);
    dJ_world.fill(0.);    dJ_local.fill(0.);
    getFrameJacobianTimeVariation(model,data,idx,WORLD,dJ_world);
    getFrameJacobianTimeVariation(model,data,idx,LOCAL,dJ_local);

    BOOST_CHECK(dJ_world.isApprox(dJ_ref_world,sqrt(alpha)));
    BOOST_CHECK(dJ_local.isApprox(dJ_ref_local,sqrt(alpha)));
  }
}

BOOST_AUTO_TEST_SUITE_END ()


/*! \file Face.hpp
  \brief Face between cells
  \author Elad Steinberg
*/

#ifndef FACE_HPP
#define FACE_HPP 1

#include <vector>
#include <boost/container/small_vector.hpp>
#include "Vector3D.hpp"
typedef boost::container::small_vector<Vector3D, 10> point_vec_v;

using std::vector;

//! \brief Interface between two cells
class Face
{
public:

  //! \brief Points at the ends of the edge
  point_vec_v vertices;

  //! \brief Neighboring cells
  std::pair<std::size_t,std::size_t> neighbors;

  /*! \brief Class constructor
    \param vert Position of the vertices
    \param neighbor1 Index of first neighbor cell
    \param neighbor2 Index of second neighbor cell
  */
  Face(point_vec_v const& vert,std::size_t neighbor1,std::size_t neighbor2);

  Face& operator=(const Face& other);

  Face(void);

  ~Face(void);

  /*! \brief Copy constructor
    \param other Source Face
  */
  Face(Face const& other);

  /*! \brief Returns the area of the face
    \return Length
  */
  double GetArea(void) const;
};

/*! \brief Calculates the centroid of aa face
  \param face The face
  \return Centroid
*/
Vector3D calc_centroid(const Face& face);

#endif	// FACE_HPP


// Copyright (C) 2017  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "opaque_types.h"
#include <dlib/python.h>
#include <dlib/global_optimization.h>
#include <dlib/matrix.h>

using namespace dlib;
using namespace std;
namespace py = pybind11;

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

std::vector<bool> list_to_bool_vector(
    const py::list& l
)
{
    std::vector<bool> result(len(l));
    for (long i = 0; i < result.size(); ++i)
    {
        result[i] = l[i].cast<bool>();
    }
    return result;
}

matrix<double,0,1> list_to_mat(
    const py::list& l
)
{
    matrix<double,0,1> result(len(l));
    for (long i = 0; i < result.size(); ++i)
        result(i) = l[i].cast<double>();
    return result;
}

py::list mat_to_list (
    const matrix<double,0,1>& m
)
{
    py::list l;
    for (long i = 0; i < m.size(); ++i)
        l.append(m(i));
    return l;
}

size_t num_function_arguments(py::object f)
{
    if (hasattr(f,"func_code"))
        return f.attr("func_code").attr("co_argcount").cast<std::size_t>();
    else
        return f.attr("__code__").attr("co_argcount").cast<std::size_t>();
}

double call_func(py::object f, const matrix<double,0,1>& args)
{
    const auto num = num_function_arguments(f);
    DLIB_CASSERT(num == args.size(),
        "The function being optimized takes a number of arguments that doesn't agree with the size of the bounds lists you provided to find_max_global()");
    DLIB_CASSERT(0 < num && num < 15, "Functions being optimized must take between 1 and 15 scalar arguments.");

#define CALL_WITH_N_ARGS(N) case N: return dlib::gopt_impl::_cwv(f,args,typename make_compile_time_integer_range<N>::type()).cast<double>();
    switch (num)
    {
        CALL_WITH_N_ARGS(1)
        CALL_WITH_N_ARGS(2)
        CALL_WITH_N_ARGS(3)
        CALL_WITH_N_ARGS(4)
        CALL_WITH_N_ARGS(5)
        CALL_WITH_N_ARGS(6)
        CALL_WITH_N_ARGS(7)
        CALL_WITH_N_ARGS(8)
        CALL_WITH_N_ARGS(9)
        CALL_WITH_N_ARGS(10)
        CALL_WITH_N_ARGS(11)
        CALL_WITH_N_ARGS(12)
        CALL_WITH_N_ARGS(13)
        CALL_WITH_N_ARGS(14)
        CALL_WITH_N_ARGS(15)

        default:
            DLIB_CASSERT(false, "oops");
            break;
    }
}

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

py::tuple py_find_max_global (
    py::object f,
    py::list bound1,
    py::list bound2,
    py::list is_integer_variable,
    unsigned long num_function_calls,
    double solver_epsilon = 0
)
{
    DLIB_CASSERT(len(bound1) == len(bound2));
    DLIB_CASSERT(len(bound1) == len(is_integer_variable));

    auto func = [&](const matrix<double,0,1>& x)
    {
        return call_func(f, x);
    };

    auto result = find_max_global(func, list_to_mat(bound1), list_to_mat(bound2),
        list_to_bool_vector(is_integer_variable), max_function_calls(num_function_calls),
        solver_epsilon);

    return py::make_tuple(mat_to_list(result.x),result.y);
}

py::tuple py_find_max_global2 (
    py::object f,
    py::list bound1,
    py::list bound2,
    unsigned long num_function_calls,
    double solver_epsilon = 0
)
{
    DLIB_CASSERT(len(bound1) == len(bound2));

    auto func = [&](const matrix<double,0,1>& x)
    {
        return call_func(f, x);
    };

    auto result = find_max_global(func, list_to_mat(bound1), list_to_mat(bound2), max_function_calls(num_function_calls), solver_epsilon);

    return py::make_tuple(mat_to_list(result.x),result.y);
}

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

py::tuple py_find_min_global (
    py::object f,
    py::list bound1,
    py::list bound2,
    py::list is_integer_variable,
    unsigned long num_function_calls,
    double solver_epsilon = 0
)
{
    DLIB_CASSERT(len(bound1) == len(bound2));
    DLIB_CASSERT(len(bound1) == len(is_integer_variable));

    auto func = [&](const matrix<double,0,1>& x)
    {
        return call_func(f, x);
    };

    auto result = find_min_global(func, list_to_mat(bound1), list_to_mat(bound2),
        list_to_bool_vector(is_integer_variable), max_function_calls(num_function_calls),
        solver_epsilon);

    return py::make_tuple(mat_to_list(result.x),result.y);
}

py::tuple py_find_min_global2 (
    py::object f,
    py::list bound1,
    py::list bound2,
    unsigned long num_function_calls,
    double solver_epsilon = 0
)
{
    DLIB_CASSERT(len(bound1) == len(bound2));

    auto func = [&](const matrix<double,0,1>& x)
    {
        return call_func(f, x);
    };

    auto result = find_min_global(func, list_to_mat(bound1), list_to_mat(bound2), max_function_calls(num_function_calls), solver_epsilon);

    return py::make_tuple(mat_to_list(result.x),result.y);
}

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

void bind_global_optimization(py::module& m)
{
    /*!
        requires
            - len(bound1) == len(bound2) == len(is_integer_variable)
            - for all valid i: bound1[i] != bound2[i]
            - solver_epsilon >= 0
            - f() is a real valued multi-variate function.  It must take scalar real
              numbers as its arguments and the number of arguments must be len(bound1).
        ensures
            - This function performs global optimization on the given f() function.
              The goal is to maximize the following objective function:
                 f(x)
              subject to the constraints:
                min(bound1[i],bound2[i]) <= x[i] <= max(bound1[i],bound2[i])
                if (is_integer_variable[i]) then x[i] is an integer.
            - find_max_global() runs until it has called f() num_function_calls times.
              Then it returns the best x it has found along with the corresponding output
              of f().  That is, it returns (best_x_seen,f(best_x_seen)).  Here best_x_seen
              is a list containing the best arguments to f() this function has found.
            - find_max_global() uses a global optimization method based on a combination of
              non-parametric global function modeling and quadratic trust region modeling
              to efficiently find a global maximizer.  It usually does a good job with a
              relatively small number of calls to f().  For more information on how it
              works read the documentation for dlib's global_function_search object.
              However, one notable element is the solver epsilon, which you can adjust.

              The search procedure will only attempt to find a global maximizer to at most
              solver_epsilon accuracy.  Once a local maximizer is found to that accuracy
              the search will focus entirely on finding other maxima elsewhere rather than
              on further improving the current local optima found so far.  That is, once a
              local maxima is identified to about solver_epsilon accuracy, the algorithm
              will spend all its time exploring the function to find other local maxima to
              investigate.  An epsilon of 0 means it will keep solving until it reaches
              full floating point precision.  Larger values will cause it to switch to pure
              global exploration sooner and therefore might be more effective if your
              objective function has many local maxima and you don't care about a super
              high precision solution.
            - Any variables that satisfy the following conditions are optimized on a log-scale:
                - The lower bound on the variable is > 0
                - The ratio of the upper bound to lower bound is > 1000
                - The variable is not an integer variable
              We do this because it's common to optimize machine learning models that have
              parameters with bounds in a range such as [1e-5 to 1e10] (e.g. the SVM C
              parameter) and it's much more appropriate to optimize these kinds of
              variables on a log scale.  So we transform them by applying log() to
              them and then undo the transform via exp() before invoking the function
              being optimized.  Therefore, this transformation is invisible to the user
              supplied functions.  In most cases, it improves the efficiency of the
              optimizer.
    !*/
    {
    m.def("find_max_global", &py_find_max_global,
"requires \n\
    - len(bound1) == len(bound2) == len(is_integer_variable) \n\
    - for all valid i: bound1[i] != bound2[i] \n\
    - solver_epsilon >= 0 \n\
    - f() is a real valued multi-variate function.  It must take scalar real \n\
      numbers as its arguments and the number of arguments must be len(bound1). \n\
ensures \n\
    - This function performs global optimization on the given f() function. \n\
      The goal is to maximize the following objective function: \n\
         f(x) \n\
      subject to the constraints: \n\
        min(bound1[i],bound2[i]) <= x[i] <= max(bound1[i],bound2[i]) \n\
        if (is_integer_variable[i]) then x[i] is an integer. \n\
    - find_max_global() runs until it has called f() num_function_calls times. \n\
      Then it returns the best x it has found along with the corresponding output \n\
      of f().  That is, it returns (best_x_seen,f(best_x_seen)).  Here best_x_seen \n\
      is a list containing the best arguments to f() this function has found. \n\
    - find_max_global() uses a global optimization method based on a combination of \n\
      non-parametric global function modeling and quadratic trust region modeling \n\
      to efficiently find a global maximizer.  It usually does a good job with a \n\
      relatively small number of calls to f().  For more information on how it \n\
      works read the documentation for dlib's global_function_search object. \n\
      However, one notable element is the solver epsilon, which you can adjust. \n\
 \n\
      The search procedure will only attempt to find a global maximizer to at most \n\
      solver_epsilon accuracy.  Once a local maximizer is found to that accuracy \n\
      the search will focus entirely on finding other maxima elsewhere rather than \n\
      on further improving the current local optima found so far.  That is, once a \n\
      local maxima is identified to about solver_epsilon accuracy, the algorithm \n\
      will spend all its time exploring the function to find other local maxima to \n\
      investigate.  An epsilon of 0 means it will keep solving until it reaches \n\
      full floating point precision.  Larger values will cause it to switch to pure \n\
      global exploration sooner and therefore might be more effective if your \n\
      objective function has many local maxima and you don't care about a super \n\
      high precision solution. \n\
    - Any variables that satisfy the following conditions are optimized on a log-scale: \n\
        - The lower bound on the variable is > 0 \n\
        - The ratio of the upper bound to lower bound is > 1000 \n\
        - The variable is not an integer variable \n\
      We do this because it's common to optimize machine learning models that have \n\
      parameters with bounds in a range such as [1e-5 to 1e10] (e.g. the SVM C \n\
      parameter) and it's much more appropriate to optimize these kinds of \n\
      variables on a log scale.  So we transform them by applying log() to \n\
      them and then undo the transform via exp() before invoking the function \n\
      being optimized.  Therefore, this transformation is invisible to the user \n\
      supplied functions.  In most cases, it improves the efficiency of the \n\
      optimizer."
        ,
	py::arg("f"), py::arg("bound1"), py::arg("bound2"), py::arg("is_integer_variable"), py::arg("num_function_calls"), py::arg("solver_epsilon")=0
    );
    }

    {
    m.def("find_max_global", &py_find_max_global2,
        "This function simply calls the other version of find_max_global() with is_integer_variable set to False for all variables.",
	py::arg("f"), py::arg("bound1"), py::arg("bound2"), py::arg("num_function_calls"), py::arg("solver_epsilon")=0
    );
    }

    {
    m.def("find_min_global", &py_find_min_global,
      "This function is just like find_max_global(), except it performs minimization rather than maximization."
        ,
	py::arg("f"), py::arg("bound1"), py::arg("bound2"), py::arg("is_integer_variable"), py::arg("num_function_calls"), py::arg("solver_epsilon")=0
    );
    }

    {
    m.def("find_min_global", &py_find_min_global2,
        "This function simply calls the other version of find_min_global() with is_integer_variable set to False for all variables.",
	py::arg("f"), py::arg("bound1"), py::arg("bound2"), py::arg("num_function_calls"), py::arg("solver_epsilon")=0
    );
    }

}


// Copyright (c) 2020 Graphcore Ltd. All rights reserved.

#define BOOST_TEST_MODULE ExprName
#include <../lib/popops/ExpressionGenerator.hpp>
#include <boost/test/unit_test.hpp>
#include <limits>
#include <poplar/TypeTraits.hpp>
#include <poplibs_support/TestDevice.hpp>
#include <popops/Expr.hpp>

using namespace poplar;
using namespace popops::expr;
using namespace popops;
using namespace poplibs_support;

static void checkNames(const Expr &A, const Expr &B, const bool match = false,
                       const bool inPlace = false) {

  auto device = createTestDevice(TEST_TARGET, 1, 4);
  auto target = device.getTarget();
  poplar::Graph graph(target);

  std::vector<Tensor> inputs;
  for (auto type : {HALF, FLOAT}) {
    for (unsigned i = 0; i < 3; ++i) {
      inputs.push_back(graph.addVariable(type, {i}, "some_name"));
    }
  }
  const auto aName =
      GenerateCodeletFromMapExpr::createVertexName(A, inputs, inPlace, false);

  const auto bName =
      GenerateCodeletFromMapExpr::createVertexName(B, inputs, inPlace, false);

  BOOST_CHECK((aName == bName) == match);
  std::cerr << "A " << aName << " ------ B " << bName
            << " (should match: " << (match ? "yes" : "no") << ")" << std::endl;
}

const int One = 1;
const short OtherOne = 1;
TypeTraits tt;

BOOST_AUTO_TEST_CASE(CheckNames) {
  checkNames(Mul(Add(_1, _2), _3), Mul(Add(_1, _1), _1));
  checkNames(Mul(Add(_1, _2), _3), Mul(Add(_4, _5), _6));
  checkNames(Mul(Add(_1, _2), _3), Mul(Add(_3, _2), _1));
  checkNames(Mul(Add(_1, _2), _3), Mul(Add(_1, _2), _3), true);
  checkNames(Select(_1, _2, _3), Clamp(_1, _2, _3));
  checkNames(Asin(Add(_1, _2)), Add(_1, _2));
  checkNames(Add(_1, _2), Add(_2, _1));
  checkNames(Sub(Add(_1, _1), _1), Sub(_1, Add(_1, _1)));
  checkNames(ConstHalf(4.0f), ConstHalf(4.0f), true);
  checkNames(Const(4.0f), ConstHalf(4.0f));
  checkNames(Const(One), Const(OtherOne), true);
  checkNames(Add(_1, Cast(_4, HALF)), Add(_2, Cast(_5, HALF)));
}

BOOST_AUTO_TEST_CASE(ConstFloatRoundTrip) {

  BOOST_CHECK(std::stof(Const(1.0f).printValue()) == 1.0f);
  BOOST_CHECK(std::stof(Const(1e-8f).printValue()) == 1e-8f);
  BOOST_CHECK(
      std::stof(Const(std::numeric_limits<float>::lowest()).printValue()) ==
      std::numeric_limits<float>::lowest());
  BOOST_CHECK(
      std::stof(Const(std::numeric_limits<float>::min()).printValue()) ==
      std::numeric_limits<float>::min());
  BOOST_CHECK(
      std::stof(Const(std::numeric_limits<float>::max()).printValue()) ==
      std::numeric_limits<float>::max());
}


/*******************************************************
 * Copyright (c) 2014, ArrayFire
 * All rights reserved.
 *
 * This file is distributed under 3-clause BSD license.
 * The complete license agreement can be obtained at:
 * http://arrayfire.com/licenses/BSD-3-Clause
 ********************************************************/

#pragma once
#include <kernel_headers/regions.hpp>
#include <program.hpp>
#include <af/defines.h>
#include <dispatch.hpp>
#include <err_opencl.hpp>
#include <debug_opencl.hpp>
#include <math.hpp>
#include <stdio.h>
#include <map>
#include <memory.hpp>

#include <boost/compute/container/vector.hpp>
#include <boost/compute/algorithm/adjacent_difference.hpp>
#include <boost/compute/algorithm/sort.hpp>
#include <boost/compute/iterator/counting_iterator.hpp>
#include <boost/compute/algorithm/exclusive_scan.hpp>
#include <boost/compute/algorithm/transform.hpp>
#include <boost/compute/lambda/placeholders.hpp>
#include <boost/compute/lambda.hpp>

using cl::Buffer;
using cl::Program;
using cl::Kernel;
using cl::EnqueueArgs;
using cl::NDRange;
namespace compute = boost::compute;

namespace opencl
{

namespace kernel
{

static const int THREADS_X = 16;
static const int THREADS_Y = 16;

template<typename T, bool full_conn, int n_per_thread>
void regions(Param out, Param in)
{
    try {
        static std::once_flag compileFlags[DeviceManager::MAX_DEVICES];
        static std::map<int, Program*> regionsProgs;
        static std::map<int, Kernel *>     ilKernel;
        static std::map<int, Kernel *>     frKernel;
        static std::map<int, Kernel *>     ueKernel;

        int device = getActiveDeviceId();

        static const int block_dim = 16;
        static const int num_warps = 8;

        std::call_once( compileFlags[device], [device] () {

                std::ostringstream options;
                if (full_conn) {
                    options << " -D T=" << dtype_traits<T>::getName()
                            << " -D BLOCK_DIM=" << block_dim
                            << " -D NUM_WARPS=" << num_warps
                            << " -D N_PER_THREAD=" << n_per_thread
                            << " -D LIMIT_MAX=" << limit_max<T>()
                            << " -D FULL_CONN";
                }
                else {
                    options << " -D T=" << dtype_traits<T>::getName()
                            << " -D BLOCK_DIM=" << block_dim
                            << " -D NUM_WARPS=" << num_warps
                            << " -D N_PER_THREAD=" << n_per_thread
                            << " -D LIMIT_MAX=" << limit_max<T>();
                }
                if (std::is_same<T, double>::value ||
                    std::is_same<T, cdouble>::value) {
                    options << " -D USE_DOUBLE";
                }

                Program prog;
                buildProgram(prog, regions_cl, regions_cl_len, options.str());
                regionsProgs[device] = new Program(prog);

                ilKernel[device] = new Kernel(*regionsProgs[device], "initial_label");
                frKernel[device] = new Kernel(*regionsProgs[device], "final_relabel");
                ueKernel[device] = new Kernel(*regionsProgs[device], "update_equiv");
            });

        const NDRange local(THREADS_X, THREADS_Y);

        const int blk_x = divup(in.info.dims[0], THREADS_X*2);
        const int blk_y = divup(in.info.dims[1], THREADS_Y*2);

        const NDRange global(blk_x * THREADS_X, blk_y * THREADS_Y);

        auto ilOp = make_kernel<Buffer, KParam,
                                Buffer, KParam> (*ilKernel[device]);

        ilOp(EnqueueArgs(getQueue(), global, local),
             *out.data, out.info, *in.data, in.info);

        CL_DEBUG_FINISH(getQueue());

        int h_continue = 1;
        cl::Buffer *d_continue = bufferAlloc(sizeof(int));

        while (h_continue) {
            h_continue = 0;
            getQueue().enqueueWriteBuffer(*d_continue, CL_TRUE, 0, sizeof(int), &h_continue);

            auto ueOp = make_kernel<Buffer, KParam,
                                    Buffer> (*ueKernel[device]);

            ueOp(EnqueueArgs(getQueue(), global, local),
                 *out.data, out.info, *d_continue);
            CL_DEBUG_FINISH(getQueue());

            getQueue().enqueueReadBuffer(*d_continue, CL_TRUE, 0, sizeof(int), &h_continue);
        }

        bufferFree(d_continue);

        // Now, perform the final relabeling.  This converts the equivalency
        // map from having unique labels based on the lowest pixel in the
        // component to being sequentially numbered components starting at
        // 1.
        int size = in.info.dims[0] * in.info.dims[1];

        compute::command_queue c_queue(getQueue()());

        // Wrap raw device ptr
        compute::context context(getContext()());
        compute::vector<T> tmp(size, context);
        clEnqueueCopyBuffer(getQueue()(), (*out.data)(), tmp.get_buffer().get(), 0, 0, size * sizeof(T), 0, NULL, NULL);

        // Sort the copy
        compute::sort(tmp.begin(), tmp.end(), c_queue);

        // Take the max element, this is the number of label assignments to
        // compute.
        //int num_bins = tmp[size - 1] + 1;
        T last_label;
        clEnqueueReadBuffer(getQueue()(), tmp.get_buffer().get(), CL_TRUE, (size - 1) * sizeof(T), sizeof(T), &last_label, 0, NULL, NULL);
        int num_bins = (int)last_label + 1;

        Buffer labels(getContext(), CL_MEM_READ_WRITE, num_bins * sizeof(T));
        compute::buffer c_labels(labels());
        compute::buffer_iterator<T> labels_begin = compute::make_buffer_iterator<T>(c_labels, 0);
        compute::buffer_iterator<T> labels_end   = compute::make_buffer_iterator<T>(c_labels, num_bins);

        // Find the end of each section of values
        compute::counting_iterator<T> search_begin(0);

        int tmp_size = size;
        BOOST_COMPUTE_CLOSURE(int, upper_bound_closure, (int v), (tmp, tmp_size),
        {
            int start = 0, n = tmp_size, i;
            while(start < n)
            {
                i = (start + n) / 2;
                if(v < tmp[i])
                {
                    n = i;
                }
                else
                {
                    start = i + 1;
                }
            }

            return start;
        });

        BOOST_COMPUTE_FUNCTION(int, clamp_to_one, (int i),
        {
            return (i >= 1) ? 1 : i;
        });

        compute::transform(search_begin, search_begin + num_bins,
                           labels_begin,
                           upper_bound_closure,
                           c_queue);
        compute::adjacent_difference(labels_begin, labels_end, labels_begin, c_queue);

        // Perform the scan -- this can computes the correct labels for each
        // component
        compute::transform(labels_begin, labels_end,
                           labels_begin,
                           clamp_to_one,
                           c_queue);
        compute::exclusive_scan(labels_begin,
                                labels_end,
                                labels_begin,
                                c_queue);

        // Apply the correct labels to the equivalency map
        auto frOp = make_kernel<Buffer, KParam,
                                Buffer, KParam,
                                Buffer> (*frKernel[device]);

        //Buffer labels_buf(tmp.get_buffer().get());
        frOp(EnqueueArgs(getQueue(), global, local),
             *out.data, out.info, *in.data, in.info, labels);
        CL_DEBUG_FINISH(getQueue());
    } catch (cl::Error err) {
        CL_TO_AF_ERROR(err);
        throw;
    }
}

} //namespace kernel

} //namespace opencl


#pragma once
//===----------------------------------------------------------------------===//
#include <dtl/dtl.hpp>
#include <dtl/math.hpp>

#include <boost/dynamic_bitset.hpp>

#include <cstddef>
//===----------------------------------------------------------------------===//
namespace dtl {
//===----------------------------------------------------------------------===//
/// -UN-compressed representation of a bitmap of variable length.
/// Wraps a boost dynamic_bitset. (used for comparisons)
template<typename _block_type = u64>
struct dynamic_bitmap {
  boost::dynamic_bitset<_block_type> bitmap_;

  // The number of bits.
  u64 n_;

  dynamic_bitmap() = default;

  /// Constructs a bitmap from a bitmap, thereby the set bits are copied
  /// one by one, similarly to the other implementations.
  explicit dynamic_bitmap(const boost::dynamic_bitset<_block_type>& in)
      : bitmap_(in.size(), 0), n_(in.size()) {
    auto i = in.find_first();
    while (i != boost::dynamic_bitset<$u32>::npos) {
      bitmap_[i] = true;
      i = in.find_next(i);
    }
  }

  ~dynamic_bitmap() = default;
  dynamic_bitmap(const dynamic_bitmap& other) = default;
  dynamic_bitmap(dynamic_bitmap&& other) noexcept = default;
  dynamic_bitmap& operator=(const dynamic_bitmap& other) = default;
  dynamic_bitmap& operator=(dynamic_bitmap&& other) noexcept = default;

  /// Return the size in bytes.
  std::size_t
  size_in_bytes() const {
    const auto bits_per_block = sizeof(_block_type) * 8;
    return ((n_ + bits_per_block - 1) / bits_per_block) * sizeof(_block_type) /* bitmap */
        + 4 /* length */;
  }

  /// Returns the length of the bitmap.
  inline std::size_t
  size() const {
    return n_;
  }

  /// Conversion to an boost::dynamic_bitset.
  boost::dynamic_bitset<_block_type>
  to_bitset() {
    boost::dynamic_bitset<_block_type> ret(bitmap_);
    return ret;
  }

  /// Bitwise AND
  dynamic_bitmap
  operator&(const dynamic_bitmap& other) const {
    dynamic_bitmap ret(*this);
    ret.bitmap_ &= other.bitmap_;
    return ret;
  }

  /// Bitwise AND (range encoding)
  dynamic_bitmap
  and_re(const dynamic_bitmap& other) const {
    return *this & other; // nothing special here. fall back to AND
  }

  /// Bitwise XOR
  dynamic_bitmap __forceinline__
  operator^(const dynamic_bitmap& other) const {
    dynamic_bitmap ret(bitmap_);
    ret.bitmap_ ^= other.bitmap_;
    return ret;
  }

  /// Bitwise XOR (range encoding)
  dynamic_bitmap __forceinline__
  xor_re(const dynamic_bitmap& other) const {
    return *this ^ other; // nothing special here. fall back to XOR
  }

  /// Computes (a XOR b) & this
  /// Note: this, a and b must be different instances. Otherwise, the behavior
  /// is undefined.
  __forceinline__ dynamic_bitmap&
  fused_xor_and(const dynamic_bitmap& a, const dynamic_bitmap& b) {
    auto x = a ^ b;
    bitmap_ &= x.bitmap_;
    return *this;
  }

  void
  print(std::ostream& os) const {
    os << "n/a";
  }

  static std::string
  name() {
    return "bitmap";
  }

  /// Returns the value of the bit at the position pos.
  u1 __forceinline__
  test(const std::size_t pos) const {
    return bitmap_.test(pos);
  }

  //===--------------------------------------------------------------------===//
  /// 1-fill iterator, with skip support.
  class iter {
    const dynamic_bitmap& outer_;

    //===------------------------------------------------------------------===//
    // Iterator state
    //===------------------------------------------------------------------===//
    /// points to the beginning of a 1-fill
    $u64 pos_;
    /// the length of the current 1-fill
    $u64 length_;
    //===------------------------------------------------------------------===//

  public:
    void __forceinline__
    next() {
      pos_ += length_;
      length_ = 0;
      pos_ = outer_.bitmap_.find_next(pos_);
      // determine the length of the current 1fill
      if (pos_ < outer_.n_) {
        length_ = 1;
        while (pos_ + length_ < outer_.n_
            && outer_.bitmap_.test(pos_ + length_)) {
          length_++;
        }
      }
      else {
        pos_ = outer_.n_;
        length_ = 0;
      }
    }

    explicit __forceinline__
    iter(const dynamic_bitmap& outer)
        : outer_(outer), pos_(outer_.bitmap_.find_first()), length_(0) {
      // determine the length of the current 1fill
      if (pos_ < outer_.n_) {
        length_ = 1;
        while (pos_ + length_ < outer_.n_
            && outer_.bitmap_.test(pos_ + length_)) {
          length_++;
        }
      }
      else {
        pos_ = outer_.n_;
        length_ = 0;
      }
    }

    void __forceinline__
    skip_to(const std::size_t to_pos) {
      if (to_pos >= outer_.n_) {
        pos_ = outer_.n_;
        length_ = 0;
        return;
      }
      pos_ = to_pos;
      length_ = 0;
      if (!outer_.bitmap_.test(pos_)) {
        pos_ = outer_.bitmap_.find_next(pos_);
      }
      if (pos_ < outer_.n_) {
        length_ = 1;
        while (pos_ + length_ < outer_.n_ && outer_.bitmap_.test(pos_ + length_)) {
          length_++;
        }
      }
      else {
        pos_ = outer_.n_;
        length_ = 0;
      }
    }

    u1
    end() const noexcept {
      return length_ == 0;
    }

    u64
    pos() const noexcept {
      return pos_;
    }

    u64
    length() const noexcept {
      return length_;
    }
  };
  //===--------------------------------------------------------------------===//

  using skip_iter_type = iter;
  using scan_iter_type = iter;

  skip_iter_type __forceinline__
  it() const {
    return skip_iter_type(*this);
  }

  scan_iter_type __forceinline__
  scan_it() const {
    return scan_iter_type(*this);
  }

  /// Returns the name of the instance including the most important parameters
  /// in JSON.
  std::string
  info() const {
    return "{\"name\":\"" + name() + "\""
        + ",\"n\":" + std::to_string(n_)
        + ",\"size\":" + std::to_string(size_in_bytes())
        + "}";
  }
};
//===----------------------------------------------------------------------===//
} // namespace dtl


#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/amos/amos.hpp>

template <class T>
int do_value_type()
{
  typedef T value_type;
  //boost::numeric::ublas::vector< value_type > z(10) ;
  value_type z ;
  boost::numeric::ublas::vector< value_type > cy(10) ;
  integer_t nz;
  boost::numeric::bindings::amos::besi( z, 1.0, 1, cy, nz ) ;

  return 0;
}

int main()
{
  if (do_value_type< std::complex<float> >()) return 255;
//  if (do_value_type< double >()) return 255;
  return 0;
}


#include <ratio>
#include <boost/hana/basic_tuple.hpp>
#include <boost/hana/flatten.hpp>
#include <boost/hana/transform.hpp>
#include <boost/hana/unpack.hpp>
#include <units/time.h>
#include <mia/concepts.hpp>
#include <mia/ext/std/chrono.hpp>

template <template <class...> class Concept, class... T>
constexpr void static_assert_f()
{
    static_assert(Concept<T...>{}); // O(1)
}

template <template <class...> class Concept, class... T, class... U>
constexpr void static_assert_f(boost::hana::basic_tuple<U...>)
{
    (..., static_assert_f<Concept, T..., U>()); // O(N)
}

template <template <class...> class Concept, class... T, class... U>
constexpr void static_assert_f(
    boost::hana::basic_tuple<T...>, boost::hana::basic_tuple<U...> u)
{
    (..., static_assert_f<Concept, T>(u)); // O(N*N)
}

template <template <class...> class Concept>
constexpr auto static_assert_ =
    [](auto... tuple) { static_assert_f<Concept>(tuple...); };

constexpr int test()
{
#if MIA_TEST_EXTENSIVELY
    constexpr boost::hana::basic_tuple<
        signed char, int, long long, unsigned char, unsigned,
        unsigned long long, float, double, long double>
        arithmetic_types;
#else
    constexpr boost::hana::basic_tuple<int, unsigned, double> arithmetic_types;
#endif

#if !MIA_TEST_EXTENSIVELY || MIA_TEST_STEP == 0
    static_assert_<mia::Quantity>(arithmetic_types);
    static_assert_<mia::QuantityWith>(arithmetic_types, arithmetic_types);
    static_assert_<mia::QuantityOneWith>(arithmetic_types, arithmetic_types);
    static_assert_<mia::QuantityOne>(arithmetic_types);
#endif

#if MIA_TEST_EXTENSIVELY
    constexpr boost::hana::basic_tuple<
        std::milli, std::ratio<2, 2000>, std::ratio<1>, std::ratio<2, 2>,
        std::ratio<60>, std::ratio<120, 2>>
        periods;
#else
    constexpr boost::hana::basic_tuple<
        std::milli, std::ratio<1>, std::ratio<60>>
        periods;
#endif

    [[maybe_unused]] constexpr boost::hana::basic_tuple durations{[=] {
        constexpr auto make_durations = [=](auto period) {
            return boost::hana::transform(arithmetic_types, [](auto arith) {
                return std::chrono::duration<
                    decltype(arith), decltype(period)>{};
            });
        };

        return boost::hana::unpack(periods, [=](auto... periods) {
            return boost::hana::flatten(
                boost::hana::make_basic_tuple(make_durations(periods)...));
        });
    }()};

#if !MIA_TEST_EXTENSIVELY || MIA_TEST_STEP == 1
    static_assert_<mia::Quantity>(durations);
    static_assert_<mia::QuantityWith>(durations, durations);
    static_assert_<mia::QuantityOneWith>(arithmetic_types, durations);
#endif

    [[maybe_unused]] constexpr boost::hana::basic_tuple time_units{[=] {
        constexpr auto make_units = [=](auto unit) {
            return boost::hana::transform(arithmetic_types, [](auto arith) {
                return units::traits::replace_underlying_t<
                    decltype(unit), decltype(arith)>{};
            });
        };

        constexpr auto time_unit = [](auto period) {
            return units::traits::strong_t<units::unit<units::conversion_factor<
                decltype(period), units::dimension::time>>>{};
        };

        return boost::hana::unpack(periods, [=](auto... periods) {
            return [=](auto... units) {
                return boost::hana::flatten(
                    boost::hana::make_basic_tuple(make_units(units)...));
            }(time_unit(periods)...);
        });
    }()};

#if !MIA_TEST_EXTENSIVELY || MIA_TEST_STEP == 2
    static_assert_<mia::Quantity>(time_units);
    static_assert_<mia::QuantityWith>(time_units, time_units);
    static_assert_<mia::QuantityOneWith>(arithmetic_types, time_units);
#endif

    [[maybe_unused]] constexpr boost::hana::basic_tuple dimensionless_units{
        boost::hana::transform(arithmetic_types, [](auto arith) {
            return units::dimensionless{arith};
        })};

#if !MIA_TEST_EXTENSIVELY || MIA_TEST_STEP == 3
    static_assert_<mia::Quantity>(dimensionless_units);
    static_assert_<mia::QuantityWith>(dimensionless_units, dimensionless_units);
    static_assert_<mia::QuantityOne>(dimensionless_units);
#endif

    return 0;
}

int main()
{
    constexpr int ret{test()};
    return ret;
}


#ifndef KINECT2CALIBRATION_HPP
#define KINECT2CALIBRATION_HPP

#include <opencv2/opencv.hpp>
#include <boost/array.hpp>
#include <Eigen/Core>

struct Kinect2Calibration
{
    /// IAI format
    cv::Mat rgb_camera_matrix;
    cv::Mat rgb_distortion_coefficients;
    cv::Mat rgb_rotation_matrix;
    cv::Mat rgb_projection_matrix;

    double  depth_shift;

    cv::Mat ir_camera_matrix;
    cv::Mat ir_distortion_coefficients;
    cv::Mat ir_rotation_matrix;
    cv::Mat ir_projection_matrix;

    cv::Mat extr_rotation_matrix;
    cv::Mat extr_translation_matrix;
    cv::Mat extr_essentaial_matrix;
    cv::Mat extr_fundamental_matrix;
    Eigen::Matrix4f extr_transformation;

    bool loadColorCalibration(const std::string &path)
    {
        try {
            cv::FileStorage fs(path, cv::FileStorage::READ);
            fs["cameraMatrix"]            >> rgb_camera_matrix;
            fs["distortionCoefficients"]  >> rgb_distortion_coefficients;
            fs["rotation"]                >> rgb_rotation_matrix;
            fs["projection"]              >> rgb_projection_matrix;
            fs.release();
        } catch (const std::exception &e) {
            std::cerr << "[Kinect2Calibration]: " << e.what() << std::endl;
            return false;
        }
        return true;
    }

    bool loadDepthCalibration(const std::string &path)
    {
        try {
            cv::FileStorage fs(path, cv::FileStorage::READ);
            fs["depthShift"] >> depth_shift;
            fs.release();
        } catch (const std::exception &e) {
            std::cerr << "[Kinect2Calibration]: " << e.what() << std::endl;
            return false;
        }
        return true;
    }

    bool loadIrCalibration(const std::string &path)
    {
        try {
            cv::FileStorage fs(path, cv::FileStorage::READ);
            fs["cameraMatrix"]            >> ir_camera_matrix;
            fs["distortionCoefficients"]  >> ir_distortion_coefficients;
            fs["rotation"]                >> ir_rotation_matrix;
            fs["projection"]              >> ir_projection_matrix;
            fs.release();
        } catch (const std::exception &e) {
            std::cerr << "[Kinect2Calibration]: " << e.what() << std::endl;
            return false;
        }
        return true;
    }

    bool loadExtrinsicCalibration(const std::string &path)
    {
        try {
            cv::FileStorage fs(path, cv::FileStorage::READ);
            fs["rotation"]     >> extr_rotation_matrix;
            fs["translation"]  >> extr_translation_matrix;
            fs["essential"]    >> extr_essentaial_matrix;
            fs["fundamental"]  >> extr_fundamental_matrix;
            fs.release();
        } catch (const std::exception &e) {
            std::cerr << "[Kinect2Calibration]: " << e.what() << std::endl;
            return false;
        }
        extr_transformation = Eigen::Matrix4f::Identity();
        extr_transformation(0,0) = extr_rotation_matrix.at<double>(0,0);
        extr_transformation(0,1) = extr_rotation_matrix.at<double>(0,1);
        extr_transformation(0,2) = extr_rotation_matrix.at<double>(0,2);
        extr_transformation(1,0) = extr_rotation_matrix.at<double>(1,0);
        extr_transformation(1,1) = extr_rotation_matrix.at<double>(1,1);
        extr_transformation(1,2) = extr_rotation_matrix.at<double>(1,2);
        extr_transformation(2,0) = extr_rotation_matrix.at<double>(2,0);
        extr_transformation(2,1) = extr_rotation_matrix.at<double>(2,1);
        extr_transformation(2,2) = extr_rotation_matrix.at<double>(2,2);
        extr_transformation(0,3) = extr_translation_matrix.at<double>(0);
        extr_transformation(1,3) = extr_translation_matrix.at<double>(1);
        extr_transformation(2,3) = extr_translation_matrix.at<double>(2);
        extr_transformation = extr_transformation.inverse().eval();
        return true;
    }

    void getDistortionCoefficientsIR(std::vector<double> &D) const
    {
        for(int i = 0 ; i < ir_distortion_coefficients.cols ; ++i)
            D.emplace_back(ir_distortion_coefficients.at<double>(i));
    }

    void getProjectionMatrixIR(boost::array<double, 12> &P) const
    {
        P[0]  = ir_projection_matrix.at<double>(0);
        P[2]  = ir_projection_matrix.at<double>(2);
        P[5]  = ir_projection_matrix.at<double>(5);
        P[6]  = ir_projection_matrix.at<double>(6);
        P[11] = 1.0;
    }

    void getRectificationMatrixIR(boost::array<double, 9> &R) const
    {
        R[0] = 1.0;
        R[4] = 1.0;
        R[8] = 1.0;
    }

    void getCameraMatrixIR(boost::array<double, 9> &K) const
    {
        K[0]  = ir_camera_matrix.at<double>(0);
        K[2]  = ir_camera_matrix.at<double>(2);
        K[5]  = ir_camera_matrix.at<double>(5);
        K[6]  = ir_camera_matrix.at<double>(6);
        K[8]  = 1.0;
    }

    void getDistortionCoefficientsRGB(std::vector<double> &D) const
    {
        for(int i = 0 ; i < ir_distortion_coefficients.cols ; ++i)
            D.emplace_back(rgb_distortion_coefficients.at<double>(i));
    }

    void getProjectionMatrixRGB(boost::array<double, 12> &P) const
    {
        P[0]  = rgb_projection_matrix.at<double>(0);
        P[2]  = rgb_projection_matrix.at<double>(2);
        P[5]  = rgb_projection_matrix.at<double>(5);
        P[6]  = rgb_projection_matrix.at<double>(6);
        P[11] = 1.0;
    }

    void getRectificationMatrixRGB(boost::array<double, 9> &R) const
    {
        R[0] = 1.0;
        R[4] = 1.0;
        R[8] = 1.0;
    }

    void getCameraMatrixRGB(boost::array<double, 9> &K) const
    {
        K[0]  = rgb_camera_matrix.at<double>(0);
        K[2]  = rgb_camera_matrix.at<double>(2);
        K[5]  = rgb_camera_matrix.at<double>(5);
        K[6]  = rgb_camera_matrix.at<double>(6);
        K[8]  = 1.0;
    }
};

#endif // KINECT2CALIBRATION_HPP


#include <sm/eigen/gtest.hpp>
#include <Eigen/Dense>
#include <aslam/backend/OptimizerBase.hpp>
#include <aslam/backend/OptimizationProblem.hpp>
#include <aslam/backend/util/OptimizerProblemManagerBase.hpp>
#include <aslam/backend/test/SampleDvAndError.hpp>

#include <sm/BoostPropertyTree.hpp>

using namespace aslam::backend;

struct TestOptimizerOptions : public OptimizerOptionsBase
{
  int testOption = -1;
};

struct TestOptimizerStatus : public OptimizerStatus
{
  using OptimizerStatus::reset;
  int testValue = -1;
};

class TestOptimizer : public OptimizerProblemManagerBase
{
 public:
  TestOptimizer(const TestOptimizerOptions& options) : _options(options) { }
  const OptimizerStatus& getStatus() const override { return _status; }
  const OptimizerOptionsBase& getOptions() const override { return _options; }
  void setOptions(const OptimizerOptionsBase& options) override { static_cast<OptimizerOptionsBase&>(_options) = options; }
  void setOptions(const TestOptimizerOptions& options) { _options = options; }
  using OptimizerProblemManagerBase::updateConvergenceStatus;
 private:
  void optimizeImplementation() override
  {
    _status.deltaError = _problemManager.evaluateError() - _status.error;
    _status.numErrorEvaluations++;
    _status.error += _status.deltaError;
    RowVectorType gradient;
    _problemManager.computeGradient(gradient, _options.numThreadsJacobian, true, true, true);
    _status.numJacobianEvaluations++;
    _status.gradientNorm = 0.0; // optimizer should stop due to this criterion
    _status.maxDeltaX = 1.0;
    _status.testValue += 1;
    _status.numIterations++;
    this->updateConvergenceStatus();
  }
 private:
  TestOptimizerOptions _options;
  TestOptimizerStatus _status;
  ProblemManager _problemManager;
};

void expectStatusInitialized(const OptimizerStatus& status)
{
  EXPECT_EQ(0, status.numIterations);
  EXPECT_EQ(0, status.numJacobianEvaluations);
  EXPECT_EQ(0, status.numErrorEvaluations);
  EXPECT_DOUBLE_EQ(std::numeric_limits<double>::max(), status.error);
  EXPECT_TRUE(std::isnan(status.deltaError));
  EXPECT_TRUE(std::isnan(status.maxDeltaX));
  EXPECT_TRUE(std::isnan(status.gradientNorm));
  EXPECT_FALSE(status.success());
  EXPECT_FALSE(status.failure());
  EXPECT_EQ(ConvergenceStatus::IN_PROGRESS, status.convergence);
}

TEST(OptimizerTestSuite, testOptimizerOptions)
{
  {
    OptimizerOptionsBase options;
    EXPECT_NO_THROW(options.check()); // Default options should be valid

    // Test some invalid options
    options.convergenceDeltaError = -1.0;
    EXPECT_ANY_THROW(options.check());
    options = OptimizerOptionsBase(); // reset to default

    options.convergenceDeltaX = -1.0;
    EXPECT_ANY_THROW(options.check());
    options = OptimizerOptionsBase(); // reset to default

    options.convergenceGradientNorm = -1.0;
    EXPECT_ANY_THROW(options.check());
    options = OptimizerOptionsBase(); // reset to default

    options.maxIterations = -2;
    EXPECT_ANY_THROW(options.check());
    options = OptimizerOptionsBase(); // reset to default
  }

  { // Test construction from property tree
    sm::BoostPropertyTree pt;
    pt.setDouble("convergenceGradientNorm", -1.0); // invalid
    pt.setDouble("convergenceDeltaX", 1.0);
    pt.setDouble("convergenceDeltaError", 1.0);
    pt.setInt("maxIterations", 1);
    pt.setInt("numThreadsJacobian", 1);
    pt.setInt("numThreadsError", 4);
    EXPECT_ANY_THROW(OptimizerOptionsBase options(pt)); // invalid option convergenceGradientNorm
    pt.setDouble("convergenceGradientNorm", 1.0);
    OptimizerOptionsBase options(pt);
    EXPECT_DOUBLE_EQ(pt.getDouble("convergenceGradientNorm"), options.convergenceGradientNorm);
    EXPECT_DOUBLE_EQ(pt.getDouble("convergenceDeltaX"), options.convergenceDeltaX);
    EXPECT_DOUBLE_EQ(pt.getDouble("convergenceDeltaError"), options.convergenceDeltaError);
    EXPECT_EQ(pt.getInt("maxIterations"), options.maxIterations);
    EXPECT_EQ(pt.getInt("numThreadsJacobian"), options.numThreadsJacobian);
    EXPECT_EQ(pt.getInt("numThreadsError"), options.numThreadsError);
  }
}

TEST(OptimizerTestSuite, testOptimizerStatus)
{
  TestOptimizerStatus status;
  expectStatusInitialized(status);

  status.numIterations++;
  status.numJacobianEvaluations++;
  status.numErrorEvaluations++;
  status.convergence = ConvergenceStatus::FAILURE;

  status.reset();
  expectStatusInitialized(status);
}

TEST(OptimizerTestSuite, testOptimizerBase)
{
  try
  {
    TestOptimizerOptions options;
    options.convergenceDeltaError = 0.0;
    options.convergenceDeltaX = 0.0;
    options.convergenceGradientNorm = 1e-12;
    options.maxIterations = 1;
    TestOptimizer testOptimizer(options);
    OptimizerBase& optimizer = testOptimizer;
    EXPECT_DOUBLE_EQ(options.convergenceGradientNorm, optimizer.getOptions().convergenceGradientNorm);

    EXPECT_FALSE(optimizer.isInitialized());
    EXPECT_ANY_THROW(optimizer.initialize()); // no optimization problem set
    EXPECT_FALSE(optimizer.isInitialized());
    testOptimizer.updateConvergenceStatus(); // should not yield any change in convergence
    expectStatusInitialized(optimizer.getStatus());
    EXPECT_FALSE(optimizer.isConverged());
    EXPECT_FALSE(optimizer.isFailed());
    EXPECT_TRUE(optimizer.isInProgress());

    // Test setting base options
    options.maxIterations = 2;
    optimizer.setOptions(options);
    EXPECT_EQ(options.maxIterations, optimizer.getOptions().maxIterations);

    // Create a design variable
    Point2d dv(Eigen::Vector2d::Random());
    dv.setBlockIndex(0);
    dv.setColumnBase(0);
    dv.setActive(true);

    // Create an error term
    TestNonSquaredError err(&dv, TestNonSquaredError::grad_t::Random());

    boost::shared_ptr<OptimizationProblem> problem(new OptimizationProblem());
    problem->addDesignVariable(&dv, false);
    problem->addErrorTerm(&err, false);
    optimizer.setProblem(problem);

    {
      SCOPED_TRACE("Testing optimize() without calling initialize()");
      optimizer.optimize();
      EXPECT_TRUE(optimizer.isInitialized());
      EXPECT_TRUE(optimizer.isConverged());
      EXPECT_EQ(ConvergenceStatus::GRADIENT_NORM, optimizer.getStatus().convergence);
    }
    {
      SCOPED_TRACE("Testing reset()");
      optimizer.reset();
      expectStatusInitialized(optimizer.getStatus());
    }
  }
  catch (const std::exception& e)
  {
    FAIL() << e.what();
  }
}


#include <vector>
#include <deque>
#include <string>
#include <algorithm>
#include <iostream>
#include <fstream>
using namespace std;

#include <unistd.h>
#include <sys/wait.h>

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

#include "opencv2/core/core.hpp"
#include "opencv2/core/operations.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <opencv2/video/tracking.hpp>

#include "../../../core/image.h"
#include "../../../tracker/stereo/experiment_factory.h"
#include "../../../tracker/stereo/stereo_tracker_base.h"
#include "../../../tracker/refiner/feature_refiner_base.cc"
#include "../../../tracker/refiner/feature_refiner_klt.h"
#include "../../../tracker/detector/feature_detector_base.h"
//#include "../../../tracker/descriptor/freak.h"
#include "../../../tracker/stereo/tracker_helper.h"
#include "../../../optimization/sba/sba_base.h"
//#include "../../../optimization/sba/sba_ros.h"
#include "../../../optimization/sba/feature_helper_sba.h"
#include "../../extern/libviso2/src/viso_stereo.h"
#include "../../extern/libviso2/src/matrix.h"
#include "../../math_helper.h"
#include "../../../tracker/base/eval_helper.h"
#include "../../../core/format_helper.h"
#include "../../helper_libviso.h"
#include "../../cv_plotter.h"

using namespace core;
using namespace vo;
using namespace track;

void run_visual_odometry(const std::string& source_folder,
                         const std::string& imagelistfn,
                         const std::string& experiment_config,
                         const std::string& cparams_file)
{
  vector<KeyPoint> features_full;
  vector<KeyPoint> features_left;
  vector<KeyPoint> features_right;

  deque<vector<uchar>> track_status;
  deque<vector<KeyPoint>> features_libviso_left;
  deque<vector<KeyPoint>> features_libviso_right;

  deque<Mat> extr_params; // pose mat
  deque<Mat> Rt_params;

  double cam_params[5];
  FormatHelper::readCameraParams(cparams_file, cam_params);

  std::string output_folder;
  FeatureDetectorBase* detector = nullptr;
  TrackerBase* mono_tracker = nullptr;
  StereoTrackerBase* stereo_tracker = nullptr;
  libviso::VisualOdometryStereo* viso = nullptr;
  bool use_ba; // use bundle adjustment
  int ba_frames;
  track::ExperimentFactory::create_experiment(experiment_config, cam_params, output_folder, &detector, &mono_tracker,
                                              &stereo_tracker, &viso, use_ba, ba_frames);

  std::vector<std::string> imagelist;
  bool ok = FormatHelper::readStringList(imagelistfn, imagelist);
  if(!ok || imagelist.empty())
    throw "can not open " + imagelistfn + " or the string list is empty\n";

  // TODO
  //param.match.refinement = 2;

  // optional
  //param.inlier_threshold = 0.01;       // 1.5
  //param.ransac_iters = RANSAC_ITERS;           // def: 100
  //param.bucket.bucket_height = 50;    // 50
  //param.bucket.bucket_width = 50;     // 50
  //param.bucket.max_features = 3;      // 2

  int start_frame = 0 * 2;
  //int start_frame = 1110 * 2;
  int end_frame = imagelist.size();
  //int end_frame = 10 * 2;

  cv::Mat img_left_prev, img_right_prev;
  cv::Mat img_left = cv::imread(source_folder + imagelist[start_frame], CV_LOAD_IMAGE_GRAYSCALE);
  cv::Mat img_right = cv::imread(source_folder + imagelist[start_frame+1], CV_LOAD_IMAGE_GRAYSCALE);

  optim::FeatureHelperSBA* sba;
  if(use_ba)
    sba = new optim::FeatureHelperSBA(img_left, img_right, ba_frames, stereo_tracker->countFeatures());

  libviso::Matrix pose = libviso::Matrix::eye(4);
  libviso::Matrix viso_cvtrack = libviso::Matrix::eye(4);
  //Matrix point_rt = Matrix::eye(4);
  cv::Mat pose_mat = cv::Mat::eye(4, 4, CV_64F);
  Mat Rt_inv;
  Mat mat_I = Mat::eye(4, 4, CV_64F);
  mat_I.copyTo(Rt_inv);
  Vec<double,7> trans_vec;
  extr_params.push_back(mat_I.clone());
  Mat Rt(4, 4, CV_64F);
  Mat Rt_gt(4, 4, CV_64F);
  Mat Rt_gt_prev = Mat::eye(4, 4, CV_64F);
  Mat Rt_gt_curr = Mat::eye(4, 4, CV_64F);
  MathHelper::invTrans(Rt_inv, Rt);
  Rt_params.push_back(Rt.clone());

  Mat prev_location_viso = Mat::zeros(4, 1, CV_64F);

  // init the tracker
  stereo_tracker->init(img_left, img_right);

  Mat disp_camera_left = Mat::zeros(img_left.rows, img_left.cols, CV_8UC3);
  Mat disp_camera_right = Mat::zeros(img_left.rows, img_left.cols, CV_8UC3);

  // matrix transforms points in current camera coordinate system to world coord system
  std::vector<cv::Mat> libviso_Rt_all;
  cv::Mat pose_libviso = Mat::eye(4, 4, CV_64F);
  libviso_Rt_all.push_back(pose_libviso.clone());

  std::string track_name;
  std::ifstream gt_file;
  if(experiment_config.find("tsukuba") != std::string::npos) {
    track_name = "00";
    gt_file.open("/home/kivan/Projects/datasets/Tsukuba/NewTsukubaStereoDataset/groundtruth/tsukuba_gt_crop.txt");
  }
  else {
    track_name = imagelistfn.substr(imagelistfn.size()-10,2);
    gt_file.open("/home/kivan/Projects/datasets/KITTI/poses/07.txt");
  }

  // skip first (identety) matrix
  FormatHelper::ReadNextRtMatrix(gt_file, Rt_gt_curr);

  //int gt_better_cnt = 0;
  int frame_num = start_frame / 2 + 1;
  for(unsigned i = start_frame + 2; i < end_frame; i+=(2)) {
    frame_num = i / 2 + 1;
    std::cout << "Frame: " << frame_num << " / " << imagelist.size()/2 << "\n";
    cout << source_folder + imagelist[i] << endl;
    cv::swap(img_left_prev, img_left);
    cv::swap(img_right_prev, img_right);
    img_left = cv::imread(source_folder + imagelist[i], CV_LOAD_IMAGE_GRAYSCALE);
    img_right = cv::imread(source_folder + imagelist[i+1], CV_LOAD_IMAGE_GRAYSCALE);

    stereo_tracker->track(img_left, img_right);

    std::vector<int> active_tracks;
    std::vector<libviso::Matcher::p_match> libviso_tracks;
    FeatureHelper::TrackerBaseToLibviso(stereo_tracker, libviso_tracks, active_tracks);

    if(viso->process(libviso_tracks)) {
      // is it better to kill or not to kill outliers in tracker?
      std::vector<int32_t>& inliers = viso->getInliers();
      std::cout << "Tracks before RANSAC: " << stereo_tracker->countActiveTracks() << "\n";
      FeatureHelper::FilterOutlierTracks(*stereo_tracker, active_tracks, inliers);
      std::cout << "Tracks after RANSAC: " << stereo_tracker->countActiveTracks() << "\n";
      // enable this only if the tracker doesn't do this already
      // TODO - dont need this
      //int bad_cnt = FeatureHelper::filterBadTracks(*stereo_tracker);
      //std::cout << "[VisualOdometry]: filtered bad tracks cnt: " << bad_cnt << "\n";

      //if(frame_num >= 1130) {
      //if(frame_num >= 1160) {
      //      // draw tracks
      //  cv::cvtColor(img_left_prev, disp_camera_left, COLOR_GRAY2RGB);
      //  cv::cvtColor(img_right_prev, disp_camera_right, COLOR_GRAY2RGB);
      //  FeatureHelper::drawStereoTracks(*stereo_tracker, disp_camera_left, disp_camera_right);
      //  //FeatureHelper::drawStereoRefinerTracks(tracker_basic, tracker, disp_camera_left, disp_camera_right);
      //  //tracker.debug();
      //  waitKey(0);
      //}

      //libviso::Matrix Rt_inv_libviso = libviso::Matrix::inv(viso.getMotion());
      //MathHelper::matrixToMat(Rt_inv_libviso, Rt_inv);
      libviso::Matrix Rt_libviso = viso->getMotion();
      MathHelper::matrixToMat(Rt_libviso, Rt);
      // TODO:
      //Eigen::Vector3d t, r;
      //core::MathHelper::GetEulerAngles(Rt, r);
      //core::MathHelper::GetTranslation(Rt, t);

      // if the motion is to small skip it
      // tracker.undo_state();
      MathHelper::invTrans(Rt, Rt_inv);    // better
      extr_params.push_back(Rt_inv.clone());
      // option 2: filter from initial odometry data
      // need to pass Rt - from prev to curr coord
      //cout << "Tracks before filtering: " << tracker.countActiveTracks() << "\n";
      ////TrackerHelper::printTrackerStats(tracker);
      //vector<int> outliers;
      //// use only if libviso is doing the uniform feature selection
      //FeatureHelper::filterOutlierTracks(tracker, Rt, cam_params, outliers, OUTLIER_TRACK_EPS);
      //cout << "Tracks after filtering: " << tracker.countActiveTracks() << "\n";
      //TrackerHelper::printTrackerStats(tracker);

      // on success, update current pose
      // TODO try without inverse
      //pose = pose * libviso::Matrix::inv(viso.getMotion());
      pose_mat = pose_mat * Rt_inv;
      //cout << pose << endl << endl;
      //MathHelper::matrixToMat(pose, pose_libviso);
      //libviso_Rt_all.push_back(pose_libviso.clone());
      libviso_Rt_all.push_back(pose_mat.clone());
      //viso_ptsfile << ~pose.extractCols(std::vector<int>(1,3)) << endl;

      // output some statistics
      //std::vector<libviso::Matcher::p_match>& libviso_features = viso->getFeatures();
      //vector<core::Point> points_lp, points_rp, points_lc, points_rc;
      //FeatureHelper::LibvisoInliersToPoints(libviso_features, inliers, points_lp, points_rp,
      //                                      points_lc, points_rc);
      //cout << points_lp.size() << ", " << points_rp.size() << ", " << points_lc.size() << ", "
      //     << points_rc.size() << endl;
      // TODO
      //Mat C = HelperLibviso::getCameraMatrix(param);
      //cv::Mat C;
      //cout << Rt_inv << "\n";

      // reading the GT trans
      FormatHelper::ReadNextRtMatrix(gt_file, Rt_gt_curr);
      Mat Rt_gt_inv = Rt_gt_prev * Rt_gt_curr;
      MathHelper::invTrans(Rt_gt_inv, Rt_gt);
      MathHelper::invTrans(Rt_gt_curr, Rt_gt_prev);
      //cout << Rt_gt_inv << endl;
      //cout << "Groundtruth:\n" << Rt_gt << endl;
      //cout << "Odometry:\n" << Rt << endl;
      // get camera Rt in for last movement

      if(use_ba) {
        // ----- SBA -----
        // need to pass Rt_inv - from curr to prev coord
        // TODO
        sba->updateTracks(img_left, img_right, *stereo_tracker, Rt_inv, cam_params);
        //sba->updateTracks(img_left, img_right, *stereo_tracker, mat_I, cam_params);
        if(frame_num >= ba_frames) {
          double libviso_error = EvalHelper::GetStereoReprojError(*stereo_tracker, cam_params, Rt);
          double gt_error = EvalHelper::GetStereoReprojError(*stereo_tracker, cam_params, Rt_gt);
          sba->runSBA();
          cv::Mat Rt_sba = sba->getCameraRt(sba->getNumberOfFrames() - 1);
          //if(frame_num > 10) {
          //  cv::Mat Rt_test = sba->getCameraRt(10);
          //  std::cout << Rt_test << "\n\n------";
          //}
          cv::Mat Rt_sba_inv;
          MathHelper::invTrans(Rt_sba, Rt_sba_inv);
          double ba_error = EvalHelper::GetStereoReprojError(*stereo_tracker, cam_params, Rt_sba_inv);
          printf("GT reproj error = %e\n", gt_error);
          printf("Libviso reproj error = %e\n", libviso_error);
          printf("BA reproj error = %e\n", ba_error);
          cout << "-------------------------------\n";
          //cout << "reprojection error (SBA): " << reproj_error_sba << "\n";
        }
      }

      //double reproj_error = EvalHelper::getStereoReprojError(points_lp, points_rp, points_lc, points_rc,
      //                                                       C, Rt, param.base);
      //cout << "reprojection error (libviso): " << reproj_error << "\n";

      //reprojerr_file << reproj_error << "\n";

      //double reproj_error_gt = EvalHelper::getStereoReprojError(points_lp, points_rp, points_lc, points_rc, C,
      //    Rt_gt, param.base);
      ///cout << "reprojection error (groundtruth): " << reproj_error_gt << "\n";
      //reprojerr_file_gt << reproj_error_gt << "\n";
      //if(reproj_error > reproj_error_gt) {
      //  cout << "Found better for GT!!!\n";
      //  gt_better_cnt++;
      //  //waitKey(0);
      //}

      double num_matches = viso->getNumberOfMatches();
      double num_inliers = viso->getNumberOfInliers();
      cout << "[Libviso] Matches: " << num_matches << ", Inliers: " << num_inliers << " -> "
           << 100.0*num_inliers/num_matches << " %" << endl;
      cout << "-----------------------------------------------------------------------\n\n";
      Mat location_viso(pose_libviso, Range(0,4), Range(3,4)); // extract 4-th column
      //cout << location_viso << endl;
      //Matrix location = ~point_rt.extractCols(std::vector<int>(1,3));
      //for(int k = 0; k < 3; k++) location.val[0][k] = - location.val[0][k];
      //cout << point_rt * ~location << endl;
      //cout << location << endl << endl;

      location_viso.copyTo(prev_location_viso);
    } else {
      cout << "libviso ... failed!" << endl;
      throw "Error\n";
      waitKey(0);
      //extr_params.push_back(Rt_inv.clone());
      //exit(1);
    }
  }

  //waitKey(0);
  //destroyAllWindows();

  //std::cout << "Groundtruth data cost is better then odometry in " << gt_better_cnt << " / "
  //          << imagelist.size() << " frames.\n";

  std::cout << "Writing track " << track_name << " to files\n";
  std::ofstream libviso_ofile(output_folder + '/' + track_name + ".txt");

  std::ofstream sba_ofile;
  cv::Mat sba_pose;
  if(use_ba) {
    std::string ba_folder = output_folder.substr(0, output_folder.size()-1) + "_ba/";
    sba_ofile.open(ba_folder + track_name + ".txt");
    sba_pose = cv::Mat::eye(4, 4, CV_64F);
  }
  for(size_t i = 0; i < libviso_Rt_all.size(); i++) {
    FormatHelper::writeMatRt(libviso_Rt_all[i], libviso_ofile);
    if(use_ba) {
      sba_pose = sba_pose * sba->getCameraRt(i);
      FormatHelper::writeMatRt(sba_pose, sba_ofile);
    }
  }

  if(use_ba)
    delete sba;
  delete stereo_tracker;
  delete viso;
}

int main(int argc, char** argv)
{
  std::string config_file;
  std::string experiment_config;
  std::string imagelistfn;
  std::string cam_params_file;
  std::string source_folder;

  try {
    po::options_description generic("Generic options");
    generic.add_options()
      ("help, h", "produce help message")
      ("config,c", po::value<string>(&config_file), "config filename")
      ("experiment_config,e", po::value<string>(&experiment_config), "experiment config filename");
    po::options_description cmdline_options;
    cmdline_options.add(generic);

    po::variables_map vm;
    //po::store(po::command_line_parser(argc, argv).options(cmdline_options).run(), vm);
    po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
    notify(vm);
    if(vm.count("help")) {
      cout << generic;
      return 0;
    }

    po::options_description config("Config file options");
    config.add_options()
      ("camera_params,p", po::value<string>(&cam_params_file)->default_value("camera_params.txt"), "camera params file")
      ("source_folder,s", po::value<string>(&source_folder), "folder with source")
      ("imglist,l", po::value<string>(&imagelistfn), "file with image list");

    std::ifstream ifs(config_file);
    if(!ifs)
      throw "can not open config file: " + config_file + "\n";
    else {
      po::store(parse_config_file(ifs, config, true), vm);
      notify(vm);
    }
  }
  catch(std::exception& e) {
    cout << e.what() << "\n";
    return -1;
  }

  run_visual_odometry(source_folder, imagelistfn, experiment_config, cam_params_file);

  return 0;
}