#define BOOST_TEST_MODULE capi_models
#include <boost/test/unit_test.hpp>
#include <util/test_macros.hpp>

#include <capi/TuriCore.h>
#include <algorithm>
#include <image/image_type.hpp>
#include <vector>
#include "capi_utils.hpp"

BOOST_AUTO_TEST_CASE(test_boosted_trees_double) {
  for (const char* model_name :
       {"boosted_trees_regression", "decision_tree_regression",
        "regression_linear_regression", "auto"}) {

    bool is_auto = std::string(model_name) == "auto";

    tc_error* error = NULL;

    tc_initialize("/tmp/", &error);
    TS_ASSERT(error == NULL);

    std::vector<std::pair<std::string, std::vector<double> > > data = {
        {"col1", {1., 1., 2., 2.}},
        {"col2", {1., 2., 1., 2.}},
        {"target", {0., 1., 2., 3.}}};

    tc_sframe* sf_1 = make_sframe_double(data);

    tc_sframe* sf_2 = tc_sframe_append(sf_1, sf_1, &error);
    TS_ASSERT(error == NULL);
    tc_sframe_destroy(sf_1);

    tc_sframe* sf_3 = tc_sframe_append(sf_2, sf_2, &error);
    TS_ASSERT(error == NULL);
    tc_sframe_destroy(sf_2);

    tc_sframe* sf = tc_sframe_append(sf_3, sf_3, &error);
    TS_ASSERT(error == NULL);
    tc_sframe_destroy(sf_3);

    tc_parameters* args = tc_parameters_create_empty(&error);

    TS_ASSERT(error == NULL);

    // Add in the sframe; then destroy it when we're done with it.
    {
      tc_parameters_add_sframe(args, "data", sf, &error);
      TS_ASSERT(error == NULL);
    }

    // Set the target column
    {
      tc_flexible_type* ft_name = tc_ft_create_from_cstring("target", &error);
      TS_ASSERT(error == NULL);
      tc_parameters_add_flexible_type(args, "target", ft_name, &error);
      TS_ASSERT(error == NULL);
      tc_ft_destroy(ft_name);
    }

    // Set the validation data to something empty
    {
      tc_sframe* ft_empty_sframe = tc_sframe_create_empty(&error);
      TS_ASSERT(error == NULL);

      tc_parameters_add_sframe(args, "validation_data", ft_empty_sframe, &error);
      TS_ASSERT(error == NULL);

      tc_sframe_destroy(ft_empty_sframe);
    }

    // Set the options
    {
      tc_flex_dict* fd = tc_flex_dict_create(&error);
      TS_ASSERT(error == NULL);

      tc_parameters_add_flex_dict(args, "options", fd, &error);
      TS_ASSERT(error == NULL);

      tc_flex_dict_destroy(fd);
    }

    tc_model* model;

    if(is_auto) {
      tc_variant* var_m =
          tc_function_call("_supervised_learning.create_automatic_regression_model", args, &error);

      TS_ASSERT(error == NULL);

      model = tc_variant_model(var_m, &error);
      TS_ASSERT(error == NULL);

      tc_variant_destroy(var_m);
    } else {
      // We now have enough to create the model.
      model = tc_model_new(model_name, &error);
      TS_ASSERT(error == NULL);

      tc_model_call_method(model, "train", args, &error);
      TS_ASSERT(error == NULL);
    }

    tc_parameters_destroy(args);

    TS_ASSERT(model != NULL);

    std::string ret_name = tc_model_name(model, &error);

    if(!is_auto) {
      TS_ASSERT(ret_name == model_name);
    }

    // Test predictions on the same data.  Should be almost completely
    // accurate...
    {
      tc_sframe* sf_2 = tc_sframe_create_copy(sf, &error);
      TS_ASSERT(error == NULL);

      tc_sframe_remove_column(sf_2, "target", &error);
      TS_ASSERT(error == NULL);

      tc_parameters* p_args = tc_parameters_create_empty(&error);

      tc_parameters_add_sframe(p_args, "data", sf_2, &error);
      TS_ASSERT(error == NULL);
      tc_sframe_destroy(sf_2);

      tc_variant* ret = tc_model_call_method(model, "predict", p_args, &error);
      TS_ASSERT(error == NULL);
      tc_parameters_destroy(p_args);

#ifdef TC_VARIANT_FUNCTIONS_DEFINED
      int is_sarray = tc_variant_is_sarray(ret);
      TS_ASSERT(is_sarray);

      tc_sarray* sa = tc_variant_sarray(ret, &error);
      TS_ASSERT(error == NULL);

      const auto& target_values = data.back().second;

      for (size_t i = 0; i < target_values.size(); ++i) {
        tc_flexible_type* ft = tc_sarray_extract_element(sa, i, &error);
        TS_ASSERT(error == NULL);

        double v = tc_ft_double(ft, &error);
        TS_ASSERT(error == NULL);
        tc_ft_destroy(ft);

        // Make sure they are close -- on a tiny dataset like this the default
        // setting
        TS_ASSERT_DELTA(v, target_values[i], 0.5);
      }
#endif

      {
        tc_parameters* export_args = tc_parameters_create_empty(&error);
        TS_ASSERT(error == NULL);

        // Set the l2 regression
        {
          tc_flexible_type* ft_name = tc_ft_create_from_cstring(
              "coreml_export_test_1_tmp.mlmodel", &error);
          TS_ASSERT(error == NULL);
          tc_parameters_add_flexible_type(export_args, "filename", ft_name,
                                          &error);
          TS_ASSERT(error == NULL);
          tc_ft_destroy(ft_name);
        }

        tc_variant* ret_2 = tc_model_call_method(model, "export_to_coreml",
                                                 export_args, &error);
        TS_ASSERT(error == NULL);
        tc_parameters_destroy(export_args);
      }
    }

    // Test saving and loading the model.
    {
      constexpr char MODEL_PATH[] = "save_test_1_tmp";
      tc_model_save(model, MODEL_PATH, &error);
      TS_ASSERT(error == nullptr);

      tc_model* loaded_model = tc_model_load(MODEL_PATH, &error);
      TS_ASSERT(error == nullptr);
      TS_ASSERT(loaded_model != nullptr);

      std::string ret_name = tc_model_name(loaded_model, &error);
      TS_ASSERT(error == nullptr);
      if(!is_auto) {
        TS_ASSERT(ret_name == model_name);
      }

      tc_model_destroy(loaded_model);
    }

    tc_model_destroy(model);
  }
}

BOOST_AUTO_TEST_CASE(test_auto_classification) {
    tc_error* error = NULL;

    tc_initialize("/tmp/", &error);
    TS_ASSERT(error == NULL);

    std::vector<std::pair<std::string, std::vector<double> > > features = {
        {"col1", {1., 1., 10., 10.}},
        {"col2", {2., 2., 20., 20.}}};
    tc_sframe* data = make_sframe_double(features);

    tc_flex_list* target_value = make_flex_list_string({"A", "A", "B", "B"});
    tc_sarray* target_sarray = tc_sarray_create_from_list(target_value, &error);
    TS_ASSERT(error == NULL);

    tc_sframe_add_column(data, "target", target_sarray, &error);
    TS_ASSERT(error == NULL);

    tc_parameters* args = tc_parameters_create_empty(&error);
    TS_ASSERT(error == NULL);

    { // Populate args
      tc_parameters_add_sframe(args, "data", data, &error);
      TS_ASSERT(error == NULL);

      // Set the target column
      tc_flexible_type* ft_name = tc_ft_create_from_cstring("target", &error);
      TS_ASSERT(error == NULL);
      tc_parameters_add_flexible_type(args, "target", ft_name, &error);
      TS_ASSERT(error == NULL);
      tc_ft_destroy(ft_name);

      // Set the validation data to something empty
      tc_sframe* ft_empty_sframe = tc_sframe_create_empty(&error);
      TS_ASSERT(error == NULL);

      tc_parameters_add_sframe(args, "validation_data", ft_empty_sframe, &error);
      TS_ASSERT(error == NULL);

      tc_sframe_destroy(ft_empty_sframe);

      // Set the options
      tc_flex_dict* fd = tc_flex_dict_create(&error);
      TS_ASSERT(error == NULL);

      tc_parameters_add_flex_dict(args, "options", fd, &error);
      TS_ASSERT(error == NULL);

      tc_flex_dict_destroy(fd);
    }

    { // Model selection without validation data
      tc_variant* var_m =
        tc_function_call("_supervised_learning.create_automatic_classifier_model", args, &error);
      TS_ASSERT(error == NULL);

      tc_model* model = tc_variant_model(var_m, &error);
      TS_ASSERT(error == NULL);
      TS_ASSERT(model != NULL);

      tc_model_destroy(model);
      tc_variant_destroy(var_m);
    }

    { // Model selection with validation data
      tc_parameters_add_sframe(args, "validation_data", data, &error);
      TS_ASSERT(error == NULL);

      tc_variant* var_m =
        tc_function_call("_supervised_learning.create_automatic_classifier_model", args, &error);
      TS_ASSERT(error == NULL);

      tc_model* model = tc_variant_model(var_m, &error);
      TS_ASSERT(error == NULL);
      TS_ASSERT(model != NULL);

      tc_model_destroy(model);
      tc_variant_destroy(var_m);
    }

    tc_sframe_destroy(data);
    tc_parameters_destroy(args);
}


/*!
@file
Defines `boost::hana::negate`.

@copyright Louis Dionne 2013-2017
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_HANA_NEGATE_HPP
#define BOOST_HANA_NEGATE_HPP

#include <boost/hana/fwd/negate.hpp>

#include <boost/hana/concept/group.hpp>
#include <boost/hana/config.hpp>
#include <boost/hana/core/dispatch.hpp>
#include <boost/hana/fwd/minus.hpp>
#include <boost/hana/zero.hpp>

#include <type_traits>

namespace boost { namespace hana {
    //! @cond
    template <typename X>
    constexpr decltype(auto) negate_t::operator()(X&& x) const {
        using G = typename hana::tag_of<X>::type;
        using Negate = BOOST_HANA_DISPATCH_IF(negate_impl<G>,
            hana::Group<G>::value
        );

    #ifndef BOOST_HANA_CONFIG_DISABLE_CONCEPT_CHECKS
        static_assert(hana::Group<G>::value,
        "hana::negate(x) requires 'x' to be in a Group");
    #endif

        return Negate::apply(static_cast<X&&>(x));
    }
    //! @endcond

    template <typename T, bool condition>
    struct negate_impl<T, when<condition>> : default_ {
        template <typename X>
        static constexpr decltype(auto) apply(X&& x)
        { return hana::minus(hana::zero<T>(), static_cast<X&&>(x)); }
    };

    //////////////////////////////////////////////////////////////////////////
    // Model for arithmetic data types
    //////////////////////////////////////////////////////////////////////////
    template <typename T>
    struct negate_impl<T, when<std::is_arithmetic<T>::value &&
                               !std::is_same<bool, T>::value>> {
        template <typename X>
        static constexpr decltype(auto) apply(X&& x)
        { return -static_cast<X&&>(x); }
    };
}} // end namespace boost::hana

#endif // !BOOST_HANA_NEGATE_HPP


#include <boost/test/unit_test.hpp>
#include "LtpRandomNumberGenerator.h"
#include <boost/bind/bind.hpp>
#include <boost/timer/timer.hpp>

BOOST_AUTO_TEST_CASE(LtpRandomNumberGeneratorTestCase)
{
    {
        LtpRandomNumberGenerator rng;
        rng.SetEngineIndex(131);
        for (uint64_t i = 1; i <= 65535; ++i) {
            uint64_t randomNumber = rng.GetRandomSession64();
            BOOST_REQUIRE(randomNumber > 0);
            BOOST_REQUIRE_EQUAL(randomNumber & 0xffffu, i);
            BOOST_REQUIRE_EQUAL((randomNumber >> 55) & 1U, 0); //bit 55 set to 0 to leave room for incrementing without rolling into the engineIndex
            BOOST_REQUIRE_EQUAL(static_cast<unsigned int>(LtpRandomNumberGenerator::GetEngineIndexFromRandomSessionNumber(randomNumber)), 131);
        }
        for (uint64_t i = 1; i <= 10; ++i) { //incremental part should have rolled around at this point
            uint64_t randomNumber = rng.GetRandomSession64();
            BOOST_REQUIRE(randomNumber > 0);
            BOOST_REQUIRE_EQUAL(randomNumber & 0xffffu, i);
            BOOST_REQUIRE_EQUAL((randomNumber >> 55) & 1U, 0); //bit 55 set to 0 to leave room for incrementing without rolling into the engineIndex
            BOOST_REQUIRE_EQUAL(static_cast<unsigned int>(LtpRandomNumberGenerator::GetEngineIndexFromRandomSessionNumber(randomNumber)), 131);
        }
    }

    //test with additional randomness
    {
        LtpRandomNumberGenerator rng;
        rng.SetEngineIndex(131);
        boost::random_device rd;
        for (uint64_t i = 1; i <= 65535; ++i) {
            uint64_t randomNumber = rng.GetRandomSession64(rd);
            BOOST_REQUIRE(randomNumber > 0);
            BOOST_REQUIRE_EQUAL(randomNumber & 0xffffu, i);
            BOOST_REQUIRE_EQUAL((randomNumber >> 55) & 1U, 0); //bit 55 set to 0 to leave room for incrementing without rolling into the engineIndex
            BOOST_REQUIRE_EQUAL(static_cast<unsigned int>(LtpRandomNumberGenerator::GetEngineIndexFromRandomSessionNumber(randomNumber)), 131);
        }
        for (uint64_t i = 1; i <= 10; ++i) { //incremental part should have rolled around at this point
            uint64_t randomNumber = rng.GetRandomSession64(rd);
            BOOST_REQUIRE(randomNumber > 0);
            BOOST_REQUIRE_EQUAL(randomNumber & 0xffffu, i);
            BOOST_REQUIRE_EQUAL((randomNumber >> 55) & 1U, 0); //bit 55 set to 0 to leave room for incrementing without rolling into the engineIndex
            BOOST_REQUIRE_EQUAL(static_cast<unsigned int>(LtpRandomNumberGenerator::GetEngineIndexFromRandomSessionNumber(randomNumber)), 131);
        }
    }

    {
        LtpRandomNumberGenerator rng;
        for (uint64_t i = 1; i <= 65535; ++i) {
            uint64_t randomNumber = rng.GetRandomSerialNumber64();
            BOOST_REQUIRE(randomNumber > 0);
            BOOST_REQUIRE_EQUAL(randomNumber & 0xffffu, 1);
            BOOST_REQUIRE_EQUAL(randomNumber >> 63, 0);
        }
        for (uint64_t i = 1; i <= 10; ++i) { //incremental part should have rolled around at this point
            uint64_t randomNumber = rng.GetRandomSerialNumber64();
            BOOST_REQUIRE(randomNumber > 0);
            BOOST_REQUIRE_EQUAL(randomNumber & 0xffffu, 1);
            BOOST_REQUIRE_EQUAL(randomNumber >> 63, 0);
        }
    }

    //test with additional randomness
    {
        LtpRandomNumberGenerator rng;
        boost::random_device rd;
        for (uint64_t i = 1; i <= 65535; ++i) {
            uint64_t randomNumber = rng.GetRandomSerialNumber64(rd);
            BOOST_REQUIRE(randomNumber > 0);
            BOOST_REQUIRE_EQUAL(randomNumber & 0xffffu, 1);
            BOOST_REQUIRE_EQUAL(randomNumber >> 63, 0);
        }
        for (uint64_t i = 1; i <= 10; ++i) { //incremental part should have rolled around at this point
            uint64_t randomNumber = rng.GetRandomSerialNumber64(rd);
            BOOST_REQUIRE(randomNumber > 0);
            BOOST_REQUIRE_EQUAL(randomNumber & 0xffffu, 1);
            BOOST_REQUIRE_EQUAL(randomNumber >> 63, 0);
        }
    }

    {
        LtpRandomNumberGenerator rng;
        rng.SetEngineIndex(131);
        for (uint64_t i = 1; i <= 65535; ++i) {
            uint32_t randomNumber = rng.GetRandomSession32();
            BOOST_REQUIRE(randomNumber > 0);
            BOOST_REQUIRE_EQUAL(randomNumber & 0xffffu, i);
            BOOST_REQUIRE_EQUAL((randomNumber >> 23) & 1U, 0); //bit 23 set to 0 to leave room for incrementing without rolling into the engineIndex
            BOOST_REQUIRE_EQUAL(static_cast<unsigned int>(LtpRandomNumberGenerator::GetEngineIndexFromRandomSessionNumber(randomNumber)), 131);
        }
        for (uint64_t i = 1; i <= 10; ++i) { //incremental part should have rolled around at this point
            uint32_t randomNumber = rng.GetRandomSession32();
            BOOST_REQUIRE(randomNumber > 0);
            BOOST_REQUIRE_EQUAL(randomNumber & 0xffffu, i);
            BOOST_REQUIRE_EQUAL((randomNumber >> 23) & 1U, 0); //bit 23 set to 0 to leave room for incrementing without rolling into the engineIndex
            BOOST_REQUIRE_EQUAL(static_cast<unsigned int>(LtpRandomNumberGenerator::GetEngineIndexFromRandomSessionNumber(randomNumber)), 131);
        }
    }

    //test with additional randomness
    {
        LtpRandomNumberGenerator rng;
        rng.SetEngineIndex(131);
        boost::random_device rd;
        for (uint64_t i = 1; i <= 65535; ++i) {
            uint32_t randomNumber = rng.GetRandomSession32(rd);
            BOOST_REQUIRE(randomNumber > 0);
            BOOST_REQUIRE_EQUAL(randomNumber & 0xffffu, i);
            BOOST_REQUIRE_EQUAL((randomNumber >> 23) & 1U, 0); //bit 23 set to 0 to leave room for incrementing without rolling into the engineIndex
            BOOST_REQUIRE_EQUAL(static_cast<unsigned int>(LtpRandomNumberGenerator::GetEngineIndexFromRandomSessionNumber(randomNumber)), 131);
        }
        for (uint64_t i = 1; i <= 10; ++i) { //incremental part should have rolled around at this point
            uint32_t randomNumber = rng.GetRandomSession32(rd);
            BOOST_REQUIRE(randomNumber > 0);
            BOOST_REQUIRE_EQUAL(randomNumber & 0xffffu, i);
            BOOST_REQUIRE_EQUAL((randomNumber >> 23) & 1U, 0); //bit 23 set to 0 to leave room for incrementing without rolling into the engineIndex
            BOOST_REQUIRE_EQUAL(static_cast<unsigned int>(LtpRandomNumberGenerator::GetEngineIndexFromRandomSessionNumber(randomNumber)), 131);
        }
    }

    {
        LtpRandomNumberGenerator rng;
        for (uint64_t i = 1; i <= 65535; ++i) {
            uint32_t randomNumber = rng.GetRandomSerialNumber32();
            BOOST_REQUIRE(randomNumber > 0);
            BOOST_REQUIRE_EQUAL(randomNumber & 0xffffu, 1);
            BOOST_REQUIRE_EQUAL(randomNumber >> 31, 0);
        }
        for (uint64_t i = 1; i <= 10; ++i) { //incremental part should have rolled around at this point
            uint32_t randomNumber = rng.GetRandomSerialNumber32();
            BOOST_REQUIRE(randomNumber > 0);
            BOOST_REQUIRE_EQUAL(randomNumber & 0xffffu, 1);
            BOOST_REQUIRE_EQUAL(randomNumber >> 31, 0);
        }
    }

    //test with additional randomness
    {
        LtpRandomNumberGenerator rng;
        boost::random_device rd;
        for (uint64_t i = 1; i <= 65535; ++i) {
            uint32_t randomNumber = rng.GetRandomSerialNumber32(rd);
            BOOST_REQUIRE(randomNumber > 0);
            BOOST_REQUIRE_EQUAL(randomNumber & 0xffffu, 1);
            BOOST_REQUIRE_EQUAL(randomNumber >> 31, 0);
        }
        for (uint64_t i = 1; i <= 10; ++i) { //incremental part should have rolled around at this point
            uint32_t randomNumber = rng.GetRandomSerialNumber32(rd);
            BOOST_REQUIRE(randomNumber > 0);
            BOOST_REQUIRE_EQUAL(randomNumber & 0xffffu, 1);
            BOOST_REQUIRE_EQUAL(randomNumber >> 31, 0);
        }
    }
    /*
    {
        volatile uint64_t randomNumber;
        std::cout << "speed random\n";
        boost::timer::auto_cpu_timer t;
        for (unsigned int i = 0; i < 1000000; ++i) {
            randomNumber = rng.GetRandom();
        }
        BOOST_REQUIRE(randomNumber > 0);
    }
    */
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_unsigned.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable: 4723) // potential divide by 0
#endif

  BOOST_DISPATCH_OVERLOAD ( divides_
                          , (typename A0)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const saturated_tag &,  A0 a0, A0 a1
                                    ) const BOOST_NOEXCEPT
    {
      return a0/a1;
    }
  };
  BOOST_DISPATCH_OVERLOAD ( divides_
                          , (typename A0)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bd::scalar_< bd::int_<A0> >
                          , bd::scalar_< bd::int_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const saturated_tag &,  A0 a0, A0 a1
                                    ) const BOOST_NOEXCEPT
    {
      typedef bd::as_unsigned_t<A0> utype;
      A0 const aa0 = a0 + !((a1 + One<A0>()) | ((utype)a0 + Valmin<A0>()));
      if (a1)
        return aa0/a1;
      else if (a0)
        return Valmax<A0>() + ((utype)a0 >> (sizeof(A0)*CHAR_BIT-1));
      else
        return Zero<A0>();
    }
  };
  BOOST_DISPATCH_OVERLOAD ( divides_
                          , (typename A0)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bd::scalar_< bd::uint_<A0> >
                          , bd::scalar_< bd::uint_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const saturated_tag &,  A0 a0, A0 a1
                                    ) const BOOST_NOEXCEPT
    {
      return a1 ? a0/a1 : genmask(a0);
    }
  };

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

} } }

#endif


#include <stdio.h>
#include <string>

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

#include <gpd/util/cloud.h>

#include <gpd/data_generator.h>
#include <gpd/grasp_detector.h>

namespace gpd {
namespace detect_grasps_python {

typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloudRGB;

struct Position {
  double x, y, z;

  //  Position();
  //
  //  Position(double x, double y, double z) : px(x), py(y), pz(z) { }
  //
  //  Position(const Eigen::Vector3d& v) : px(v(0)), py(v(1)), pz(v(2)) { }
};

struct Quaternion {
  double x, y, z, w;

  //  Quaternion();
  //
  //  Quaternion(double x, double y, double z, double w) : qx(x), qy(y), qz(z),
  //  qw(w) { }
  //
  //  Quaternion(const Eigen::Quaterniond& v) : qx(v.x()), qy(v.y()), qz(v.z()),
  //  qw(v.w()) { }
};

// struct Grasp
//{
//  Position pos;
//  Quaternion orient;
//  Position sample;
//  double score;
//  bool label;
//};

extern "C" struct Grasp {
  double *pos;
  double *orient;
  double *sample;
  double score;
  bool label;
  int *image;
};

extern "C" struct AugmentedCloud {
  float *points;
  float *normals;
  int *camera_index;
  float *view_points;
  int size;
  int num_view_points;
};

Eigen::Matrix3Xd viewPointsToMatrix(float *array, int n) {
  Eigen::Matrix3Xd mat(3, n);

  for (int i = 0; i < n; i++) {
    mat.col(i) << array[3 * i], array[3 * i + 1], array[3 * i + 2];
  }

  return mat;
}

Eigen::MatrixXi cameraSourceToMatrix(int *array, int rows, int cols) {
  Eigen::MatrixXi mat(rows, cols);

  for (int i = 0; i < cols; i++) {
    for (int j = 0; j < rows; j++) {
      mat(j, i) = array[i * rows + j];
    }
  }

  return mat;
}

int *cvMatToArray(const cv::Mat &image) {
  printf("HELLO\n");
  printf("%d, %d, %d\n", image.rows, image.rows, image.channels());
  int *array = new int[image.rows * image.cols * image.channels()];
  int l = 0;
  printf("%d, %d, %d\n", image.rows, image.rows, image.channels());

  for (int i = 0; i < image.rows; i++) {
    const uchar *ptr = image.ptr(i);

    for (int j = 0; j < image.cols; j++) {
      const uchar *uc_pixel = ptr;

      for (int k = 0; k < image.channels(); k++) {
        if ((int)uc_pixel[k] > 0) {
          printf("%d, %d, %d\n", i, j, k);
          std::cout << "uc_pixel:" << (int)uc_pixel[k] << "\n";
        }
        //        array[l] = uc_pixel[k];
        l++;
        //        printf(" %d\n", uc_pixel[k]);
        //        std::cout << image.at<uchar>(i, j, k) << "\n";
        //        image.at<uchar>(i, j, k)
        //        array[l] = (float) image.at<uchar>(i, j, k);
        //        printf("%.2f\n", array[l]);
        l++;
      }
    }
  }

  return array;
}

PointCloudRGB arrayToPCLPointCloud(float *points, int num_points) {
  PointCloudRGB cloud;
  cloud.resize(num_points);

  for (int i = 0; i < num_points; i++) {
    PointT p;
    p.x = points[3 * i + 0];
    p.y = points[3 * i + 1];
    p.z = points[3 * i + 2];
    cloud.at(i) = p;
  }

  return cloud;
}

Cloud augmentedCloudToCloud(AugmentedCloud *cloud) {
  PointCloudRGB::Ptr pcl_cloud(new PointCloudRGB);
  *pcl_cloud = arrayToPCLPointCloud(cloud->points, cloud->size);
  Eigen::MatrixXi camera_source = cameraSourceToMatrix(
      cloud->camera_index, cloud->num_view_points, cloud->size);
  Eigen::Matrix3Xd view_points =
      viewPointsToMatrix(cloud->view_points, cloud->num_view_points);
  Eigen::Matrix3Xd normals = viewPointsToMatrix(cloud->normals, cloud->size);

  Cloud cloud_cam(pcl_cloud, camera_source, view_points);
  cloud_cam.setNormals(normals);
  return cloud_cam;
}

Cloud createCloud(float *points, int *camera_index, float *view_points,
                  int size, int num_view_points) {
  PointCloudRGB::Ptr pcl_cloud(new PointCloudRGB);
  *pcl_cloud = arrayToPCLPointCloud(points, size);
  Eigen::MatrixXi camera_index_mat =
      cameraSourceToMatrix(camera_index, num_view_points, size);
  Eigen::Matrix3Xd view_points_mat =
      viewPointsToMatrix(view_points, num_view_points);

  Cloud cloud_cam(pcl_cloud, camera_index_mat, view_points_mat);

  return cloud_cam;
}

Cloud createCloudNormals(float *points, float *normals, int *camera_index,
                         float *view_points, int size, int num_view_points) {
  PointCloudRGB::Ptr pcl_cloud(new PointCloudRGB);
  *pcl_cloud = arrayToPCLPointCloud(points, size);
  Eigen::Matrix3Xd normals_mat = viewPointsToMatrix(normals, size);
  Eigen::MatrixXi camera_index_mat =
      cameraSourceToMatrix(camera_index, num_view_points, size);
  Eigen::Matrix3Xd view_points_mat =
      viewPointsToMatrix(view_points, num_view_points);

  Cloud cloud_cam(pcl_cloud, camera_index_mat, view_points_mat);
  cloud_cam.setNormals(normals_mat);

  return cloud_cam;
}

Cloud createGroundTruthCloud(float *points, float *normals, int size) {
  PointCloudRGB::Ptr pcl_cloud(new PointCloudRGB);
  *pcl_cloud = arrayToPCLPointCloud(points, size);
  Eigen::Matrix3Xd points_gt = viewPointsToMatrix(points, size);
  Eigen::Matrix3Xd normals_gt = viewPointsToMatrix(normals, size);
  Eigen::MatrixXi cam_sources_gt = Eigen::MatrixXi::Ones(1, size);
  Eigen::Matrix3Xd cam_pos_gt = Eigen::Matrix3Xd::Zero(3, 1);

  Cloud mesh_cloud(pcl_cloud, cam_sources_gt, cam_pos_gt);
  mesh_cloud.setNormals(normals_gt);

  return mesh_cloud;
}

Position vectorToPositionStruct(const Eigen::Vector3d &v) {
  Position p;
  p.x = v(0);
  p.y = v(1);
  p.z = v(2);
  return p;
}

Quaternion matrixToQuaternionStruct(const Eigen::Matrix3d &rot) {
  Eigen::Quaterniond quat_eig(rot);
  Quaternion q;
  q.x = quat_eig.x();
  q.y = quat_eig.y();
  q.z = quat_eig.z();
  q.w = quat_eig.w();
  return q;
}

Cloud initCloud(char *pcd_filename, char *normals_filename, float *view_points,
                int num_view_points) {
  // Set view points from which the camera has taken the point cloud.
  Eigen::Matrix3Xd view_points_mat =
      viewPointsToMatrix(view_points, num_view_points);

  // Load point cloud from file.
  Cloud cloud(pcd_filename, view_points_mat);
  if (cloud.getCloudOriginal()->size() == 0) {
    printf("Error: Input point cloud is empty or does not exist!\n");
    return cloud;
  }

  // Optional: load surface normals from file.
  if (std::string(normals_filename).size() > 0) {
    cloud.setNormalsFromFile(normals_filename);
    printf("Loaded surface normals from file: %s\n", normals_filename);
  }

  return cloud;
}

// Grasp* handsToGraspsStruct(const std::vector<Hand>& hands)
//{
//  Grasp* grasps = new Grasp[hands.size()];
//
//  for (int i = 0; i < hands.size(); i++)
//  {
//    Position pos = vectorToPositionStruct(hands[i].getGraspBottom());
//    Quaternion quat = matrixToQuaternionStruct(hands[i].getFrame());
//    Position sample = vectorToPositionStruct(hands[i].getSample());
//
//    grasps[i].pos = pos;
//    grasps[i].orient = quat;
//    grasps[i].sample = sample;
//    grasps[i].score = hands[i].getScore();
//    grasps[i].label = hands[i].isFullAntipodal();
//  }
//
//  return grasps;
//}

Grasp *handsToGraspsStruct(const std::vector<Hand> &hands) {
  Grasp *grasps = new Grasp[hands.size()];

  for (int i = 0; i < hands.size(); i++) {
    grasps[i].pos = new double[3];
    Eigen::Map<Eigen::Vector3d>(grasps[i].pos) = hands[i].getGraspBottom();
    grasps[i].orient = new double[4];
    Eigen::Quaterniond q(hands[i].getFrame());
    Eigen::Map<Eigen::Quaterniond>(grasps[i].orient) = q;
    grasps[i].sample = new double[3];
    grasps[i].score = hands[i].getScore();
    grasps[i].label = hands[i].isFullAntipodal();
    grasps[i].image = new int[0];
    grasps[i].image[0] = -1;
  }

  return grasps;
}

Grasp *handsToGraspsStruct(const std::vector<Hand> &hands,
                           const std::vector<cv::Mat> &images) {
  Grasp *grasps = new Grasp[hands.size()];

  for (int i = 0; i < hands.size(); i++) {
    grasps[i].pos = new double[3];
    Eigen::Map<Eigen::Vector3d>(grasps[i].pos) = hands[i].getGraspBottom();
    grasps[i].orient = new double[4];
    Eigen::Quaterniond q(hands[i].getFrame());
    Eigen::Map<Eigen::Quaterniond>(grasps[i].orient) = q;
    grasps[i].sample = new double[3];
    grasps[i].score = hands[i].getScore();
    grasps[i].label = hands[i].isFullAntipodal();
    printf("i: %d\n", i);
    grasps[i].image = cvMatToArray(
        images[0]);  // new
                     // int[images[i].rows*images[i].cols*images[i].channels()];
                     //    int* img = cvMatToArray(images[0]);
                     //    grasps[i].image = img;
                     //    delete[] img;
  }

  return grasps;
}

std::vector<Hand> detectGrasps(const std::string &config_filename,
                               Cloud &cloud_cam) {
  // Preprocess the point cloud.
  GraspDetector detector(config_filename);
  detector.preprocessPointCloud(cloud_cam);

  // Detect grasps.
  std::vector<Hand> hands = detector.detectGrasps(cloud_cam);

  return hands;
}

std::vector<Hand> generateGraspCandidates(const std::string &config_filename,
                                          Cloud &cloud_cam) {
  // Preprocess the point cloud.
  GraspDetector detector(config_filename);
  detector.preprocessPointCloud(cloud_cam);

  // Generate grasp candidates.
  std::vector<HandSet> hand_sets = detector.generateGraspCandidates(cloud_cam);

  std::vector<Hand> hands;
  for (int i = 0; i < hand_sets.size(); i++) {
    for (int j = 0; j < hand_sets[i].getHands().size(); j++) {
      if (hand_sets[i].getIsValid()[j]) {
        hands.push_back(hand_sets[i].getHands()[j]);
      }
    }
  }

  return hands;
}

void copyImageMatrix(const cv::Mat &src, cv::Mat &dst, int idx_in) {
  for (int j = 0; j < 60; j++) {
    for (int k = 0; k < 60; k++) {
      for (int l = 0; l < 15; l++) {
        int idx_src[3] = {j, k, l};
        int idx_dst[4] = {idx_in, j, k, l};
        dst.at<uchar>(idx_dst) = src.at<uchar>(idx_src);
      }
    }
  }
}

void handToTransform4x4(const Hand &hand, cv::Mat &tf, int idx_in) {
  const Eigen::Matrix3d orient = hand.getFrame();

  // Copy the orientation.
  int idx[3] = {idx_in, 0, 0};

  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      idx[1] = i;
      idx[2] = j;
      tf.at<float>(idx) = orient(i, j);
    }
  }

  // Copy the position.
  idx[2] = 3;

  for (int j = 0; j < 3; j++) {
    idx[1] = j;
    tf.at<float>(idx) = hand.getGraspBottom()(j);
  }
}

bool calcDescriptorsHelper(Cloud &cloud, GraspDetector &detector,
                           char *filename_out, int compress_level) {
  const std::string IMAGES_DS_NAME = "images";
  const std::string HANDS_DS_NAME = "hands";
  const std::string WIDTHS_DS_NAME = "widths";

  // Calculate grasps and grasp descriptors.
  std::vector<Hand> hands;
  std::vector<cv::Mat> images;
  bool found_hands = detector.createGraspImages(cloud, hands, images);

  if (not found_hands) {
    return false;
  }

  // Store the grasps and the grasp descriptors in a HDF5 file.
  int n_dims = 3;
  int dsdims_images[n_dims] = {images.size(), images[0].rows, images[0].cols};
  cv::Mat images_mat(n_dims, dsdims_images, CV_8UC(images[0].channels()),
                     cv::Scalar(0.0));

  int dsdims_hands[n_dims] = {hands.size(), 4, 4};
  cv::Mat hands_mat(n_dims, dsdims_hands, CV_32FC1, cv::Scalar(0.0));

  int n_dims_widths = 1;
  int dsdims_widths[n_dims_widths] = {hands.size()};
  cv::Mat gripper_widths_mat(n_dims_widths, dsdims_widths, CV_32FC1,
                             cv::Scalar(0.0));

  for (int i = 0; i < images.size(); i++) {
    copyImageMatrix(images[i], images_mat, i);
    handToTransform4x4(hands[i], hands_mat, i);
    gripper_widths_mat.at<float>(i) = hands[i].getGraspWidth();

    // ranges don't seem to work
    //    std::vector<cv::Range> ranges;
    //    ranges.push_back(cv::Range(i,i+1));
    //    ranges.push_back(cv::Range(0,60));
    //    ranges.push_back(cv::Range(0,60));
    //    data(&ranges[0]) = dataset[i].image_.clone();
    //    dataset[0].image_.copyTo(data(&ranges[0]));
    //    printf("dataset.label: %d,  labels(i): %d\n", dataset[i].label_,
    //    labels.at<uchar>(i));
  }

  cv::Ptr<cv::hdf::HDF5> h5io = cv::hdf::open(filename_out);
  printf("Storing dataset %s ...\n", IMAGES_DS_NAME.c_str());
  h5io->dscreate(n_dims, dsdims_images, CV_8UC(images[0].channels()),
                 IMAGES_DS_NAME, compress_level);
  h5io->dswrite(images_mat, IMAGES_DS_NAME);

  printf("Storing dataset %s ...\n", HANDS_DS_NAME.c_str());
  h5io->dscreate(n_dims, dsdims_hands, CV_32FC1, HANDS_DS_NAME, compress_level);
  h5io->dswrite(hands_mat, HANDS_DS_NAME);

  printf("Storing dataset %s ...\n", WIDTHS_DS_NAME.c_str());
  h5io->dscreate(n_dims_widths, dsdims_widths, CV_32FC1, WIDTHS_DS_NAME,
                 compress_level);
  h5io->dswrite(gripper_widths_mat, WIDTHS_DS_NAME);

  h5io->close();

  return true;
}

extern "C" int detectGraspsInCloud(char *config_filename, float *points,
                                   int *camera_index, float *view_points,
                                   int size, int num_view_points,
                                   struct Grasp **grasps_out) {
  Cloud cloud =
      createCloud(points, camera_index, view_points, size, num_view_points);

  // Detect grasp affordances.
  std::string config_filename_str = config_filename;
  std::vector<Hand> hands = detectGrasps(config_filename_str, cloud);

  // Convert output to array of structs.
  *grasps_out = handsToGraspsStruct(hands);
  int num_out = (int)hands.size();

  return num_out;
}

extern "C" int detectGraspsInCloudNormals(char *config_filename, float *points,
                                          float *normals, int *camera_index,
                                          float *view_points, int size,
                                          int num_view_points,
                                          struct Grasp **grasps_out) {
  Cloud cloud = createCloudNormals(points, normals, camera_index, view_points,
                                   size, num_view_points);

  // Detect grasp affordances.
  std::string config_filename_str = config_filename;
  std::vector<Hand> hands = detectGrasps(config_filename_str, cloud);

  // Convert output to array of structs.
  *grasps_out = handsToGraspsStruct(hands);
  int num_out = (int)hands.size();

  return num_out;
}

extern "C" int detectGraspsInFile(char *config_filename, char *pcd_filename,
                                  char *normals_filename, float *view_points,
                                  int num_view_points,
                                  struct Grasp **grasps_out) {
  // Initialize cloud.
  Cloud cloud =
      initCloud(pcd_filename, normals_filename, view_points, num_view_points);
  if (cloud.getCloudOriginal()->size() == 0) {
    return 0;
  }

  // Detect grasp affordances.
  std::string config_filename_str = config_filename;
  std::vector<Hand> hands = detectGrasps(config_filename_str, cloud);

  // Convert output to array of structs.
  *grasps_out = handsToGraspsStruct(hands);
  int num_out = (int)hands.size();

  return num_out;
}

extern "C" int detectAndEvalGrasps(char *config_filename, float *points,
                                   int *camera_index, float *view_points,
                                   int size, int num_view_points,
                                   float *points_gt, float *normals_gt,
                                   int size_gt, struct Grasp **grasps_out) {
  // Create point cloud for current view.
  Cloud cloud =
      createCloud(points, camera_index, view_points, size, num_view_points);

  // Create point cloud for ground truth mesh.
  Cloud mesh_cloud = createGroundTruthCloud(points_gt, normals_gt, size_gt);

  // Preprocess the point cloud.
  GraspDetector detector(config_filename);
  detector.preprocessPointCloud(cloud);

  // Generate grasp candidates.
  std::vector<Hand> hands;
  std::vector<cv::Mat> images;
  bool found_hands = detector.createGraspImages(cloud, hands, images);
  if (found_hands == false) {
    printf("No grasps found!\n");
    return 0;
  }
  printf("Created %d grasps and %d images.\n", (int)hands.size(),
         (int)images.size());

  // Evaluate candidates against ground truth.
  std::vector<Hand> labeled_hands = detector.evalGroundTruth(mesh_cloud, hands);
  printf("Done!!\n");

  // Convert output to array of structs.
  *grasps_out = handsToGraspsStruct(labeled_hands, images);
  int num_out = (int)hands.size();

  printf("%d\n", num_out);

  return num_out;
}

extern "C" int generateGraspCandidatesInFile(
    char *config_filename, char *pcd_filename, char *normals_filename,
    float *view_points, int num_view_points, struct Grasp **grasps_out) {
  // Initialize point cloud.
  Cloud cloud =
      initCloud(pcd_filename, normals_filename, view_points, num_view_points);
  if (cloud.getCloudOriginal()->size() == 0) {
    return 0;
  }

  // Detect grasp affordances.
  std::string config_filename_str = config_filename;
  std::vector<Hand> hands = generateGraspCandidates(config_filename_str, cloud);

  // Convert output to array of structs.
  *grasps_out = handsToGraspsStruct(hands);
  int num_out = (int)hands.size();

  return num_out;
}

extern "C" int calcGraspDescriptorsAtIndices(
    char *config_filename, float *points, int *camera_index, float *view_points,
    int size, int num_view_points, int *indices, int num_indices,
    int compress_level, char *filename_out) {
  // Initialize point cloud.
  Cloud cloud =
      createCloud(points, camera_index, view_points, size, num_view_points);
  if (cloud.getCloudOriginal()->size() == 0) {
    return 0;
  }

  // Set indices at which to sample grasps.
  std::vector<int> sample_indices;
  sample_indices.resize(num_indices);
  for (int i = 0; i < num_indices; i++) {
    sample_indices[i] = indices[i];
  }
  cloud.setSampleIndices(sample_indices);

  // Calculate grasps and grasp descriptors.
  GraspDetector detector(config_filename);
  if (calcDescriptorsHelper(cloud, detector, filename_out, compress_level)) {
    return 1;
  }

  return 0;
}

extern "C" int calcGraspDescriptors(char *config_filename, float *points,
                                    int *camera_index, float *view_points,
                                    int size, int num_view_points,
                                    int compress_level, char *filename_out) {
  Cloud cloud =
      createCloud(points, camera_index, view_points, size, num_view_points);

  // Preprocess the point cloud.
  GraspDetector detector(config_filename);
  detector.preprocessPointCloud(cloud);

  // Calculate grasps and grasp descriptors.
  if (calcDescriptorsHelper(cloud, detector, filename_out, compress_level)) {
    return 1;
  }

  return 0;
}

extern "C" int freeMemoryGrasps(Grasp *in) {
  delete[] in;
  return 0;
}

extern "C" int CopyAndFree(float *in, float *out, int n) {
  memcpy(out, in, sizeof(float) * n);
  delete[] in;
  return 0;
}

}  // namespace detect_grasps_python
}  // namespace gpd


#include "../include/franka_o80/driver.hpp"
#include <franka/exception.h>
#include <Eigen/Geometry>
#include <iostream>

void franka_o80::Driver::robot_write_output_(const franka::RobotState &robot_state)
{
    //Joints
    for (size_t i = 0; i < 7; i++)
    {
        output_.set(joint_position[i], robot_state.q[i]);
        output_.set(joint_velocity[i], robot_state.dq[i]);
        output_.set(joint_torque[i], robot_state.tau_J[i]);
        output_.set(joint_stiffness[i], input_.get(joint_stiffness[i]));
        output_.set(joint_damping[i], input_.get(joint_damping[i]));
    }
    output_.set(joint_position[6], robot_state.q[6] - M_PI / 4);

    //Cartesian
    Eigen::Affine3d transform(Eigen::Matrix<double, 4, 4>::Map(robot_state.O_T_EE.data()));
    Eigen::Vector3d position = transform.translation();
    Eigen::Quaterniond orientation(transform.linear());
    Eigen::Matrix<double, 6, 7> jacobian = Eigen::Matrix<double, 6, 7>::Map(model_->zeroJacobian(franka::Frame::kEndEffector, robot_state).data());
    Eigen::Matrix<double, 7, 1> joint_velocities = Eigen::Matrix<double, 7, 1>::Map(robot_state.dq.data());
    Eigen::Matrix<double, 6, 1> velocity = jacobian * joint_velocities;
    for (size_t i = 0; i < 3; i++)
    {
        output_.set(cartesian_position[i], position(i));
        output_.set(cartesian_orientation, orientation);
        output_.set(cartesian_velocity[i], velocity(i));
        output_.set(cartesian_rotation[i], velocity(i + 3));
        output_.set(cartesian_stiffness[i], input_.get(cartesian_stiffness[i]));
        output_.set(cartesian_damping[i], input_.get(cartesian_damping[i]));
        output_.set(cartesian_stiffness[i + 3], input_.get(cartesian_stiffness[i + 3]));
        output_.set(cartesian_damping[i + 3], input_.get(cartesian_damping[i + 3]));
    }

    //Other
    output_.set(control_reset, input_.get(control_reset).get_real() > 0.0 ? 1.0 : 0.0);
}

void franka_o80::Driver::robot_dummy_control_function_(const franka::RobotState &robot_state, franka::JointVelocities *velocities)
{
    //Locking
    std::lock_guard<std::mutex> guard(input_output_mutex_);

    //Reading input
    bool input_reset = input_.get(control_reset).get_real() > 0.0;
    RobotMode input_mode = input_.get(robot_mode).get_robot_mode();
    bool input_finished = input_finished_;
    Error input_error = output_.get(control_error).get_error();
    bool input_stiffness_change = false;
    for (size_t i = 0; i < 7; i++)
    {
        if (joint_stiffness_(i) != input_.get(joint_stiffness[i]).get_real()) { input_stiffness_change = true; break; }
    }

    //Exit
    if (input_finished || (input_error == Error::ok && !input_reset && input_mode != RobotMode::invalid) || input_stiffness_change)
    {
        velocities->motion_finished = true;
    }

    //Writing output
    robot_write_output_(robot_state);
    output_.set(robot_mode, RobotMode::invalid);
    if (input_reset) output_.set(control_error, Error::ok);
}

void franka_o80::Driver::robot_torque_control_function_(const franka::RobotState &robot_state, franka::Torques *torques)
{
    //Locking
    std::lock_guard<std::mutex> guard(input_output_mutex_);

    //Reading input
    bool input_reset = input_.get(control_reset).get_real() > 0.0;
    RobotMode input_mode = input_.get(robot_mode).get_robot_mode();
    bool input_finished = input_finished_;
    Error input_error = output_.get(control_error).get_error();
    //I assume that stiffness and damping don't have influence on torque functions, so they are not checked

    //Exit
    bool reenter = input_mode != robot_mode_ && !
    ((robot_mode_ == RobotMode::torque || robot_mode_ == RobotMode::intelligent_position || robot_mode_ == RobotMode::intelligent_cartesian_position) &&
    (input_mode == RobotMode::torque || input_mode == RobotMode::intelligent_position || input_mode == RobotMode::intelligent_cartesian_position));
    if (reenter || input_finished || input_error != Error::ok || input_reset)
    {
        //Reading more input
        Eigen::Matrix<double, 7, 1> input_damping;
        for (size_t i = 0; i < 7; i++) input_damping(i) = input_.get(joint_damping[i]).get_real();

        //Calculating damping torque
        Eigen::Matrix<double, 7, 1> joint_velocities = Eigen::Matrix<double, 7, 1>::Map(robot_state.dq.data());
        Eigen::Matrix<double, 7, 1> coriolis = Eigen::Matrix<double, 7, 1>::Map(model_->coriolis(robot_state).data());
        Eigen::Matrix<double, 7, 1> joint_torques = -(input_damping.array() * joint_velocities.array()).matrix() + coriolis;
        Eigen::Matrix<double, 7, 1>::Map(&torques->tau_J[0]) = joint_torques;
        torques->motion_finished = true;
    }
    //Intelligent position
    else if (input_mode == RobotMode::intelligent_position)
    {
        //Reading more input
        Eigen::Matrix<double, 7, 1> input_target_positions, input_stiffness, input_damping;
        for (size_t i = 0; i < 7; i++)
        {
            input_target_positions(i) = input_.get(joint_position[i]).get_real();
            input_stiffness(i) = input_.get(joint_stiffness[i]).get_real();
            input_damping(i) = input_.get(joint_damping[i]).get_real();
        }
        input_target_positions(6) = input_.get(joint_position[6]).get_real() + M_PI / 4;

        //Calculating torque
        Eigen::Matrix<double, 7, 1> joint_positions = Eigen::Matrix<double, 7, 1>::Map(robot_state.q.data());
        Eigen::Matrix<double, 7, 1> joint_velocities = Eigen::Matrix<double, 7, 1>::Map(robot_state.dq.data());
        Eigen::Matrix<double, 7, 1> coriolis = Eigen::Matrix<double, 7, 1>::Map(model_->coriolis(robot_state).data());
        Eigen::Matrix<double, 7, 1> joint_torques =
            -(input_stiffness.array() * (joint_positions - input_target_positions).array()).matrix()
            -(input_damping.array() * joint_velocities.array()).matrix()
        + coriolis;
        Eigen::Matrix<double, 7, 1>::Map(&torques->tau_J[0]) = joint_torques;
    }
    //Intelligent cartesian position
    else if (input_mode == RobotMode::intelligent_cartesian_position)
    {
        //Reading more input
        Eigen::Matrix<double, 3, 1> input_target_position;
        for (size_t i = 0; i < 3; i++) input_target_position(i) = input_.get(cartesian_position[i]).get_real();
        Eigen::Quaterniond input_target_orientation = input_.get(cartesian_orientation).get_quaternion();
        Eigen::Matrix<double, 6, 1> input_stiffness, input_damping;
        for (size_t i = 0; i < 6; i++)
        {
            input_stiffness(i) = input_.get(cartesian_stiffness[i]).get_real();
            input_damping(i) = input_.get(cartesian_damping[i]).get_real();
        }

        //Calculating torque
        Eigen::Matrix<double, 7, 1> joint_velocities = Eigen::Matrix<double, 7, 1>::Map(robot_state.dq.data());
        Eigen::Affine3d transform(Eigen::Matrix<double, 4, 4>::Map(robot_state.O_T_EE.data()));
        Eigen::Vector3d position = transform.translation();
        Eigen::Quaterniond orientation; orientation = transform.linear();
        Eigen::Matrix<double, 7, 1> coriolis = Eigen::Matrix<double, 7, 1>::Map(model_->coriolis(robot_state).data());
        Eigen::Matrix<double, 6, 7> jacobian = Eigen::Matrix<double, 6, 7>::Map(model_->zeroJacobian(franka::Frame::kEndEffector, robot_state).data());
        Eigen::Matrix<double, 6, 1> error;
        error.head(3) << position - input_target_position;
        if (input_target_orientation.coeffs().dot(orientation.coeffs()) < 0.0) orientation.coeffs() << -orientation.coeffs();
        Eigen::Quaterniond orientation_error = orientation.inverse() * input_target_orientation;
        error.tail(3) << orientation_error.x(), orientation_error.y(), orientation_error.z();
        error.tail(3) << -transform.linear() * error.tail(3);
        Eigen::Matrix<double, 7, 1> joint_torques = jacobian.transpose() * (
            -(input_stiffness.array() * error.array()).matrix()
            -(input_damping.array() * (jacobian * joint_velocities).array()).matrix()
        ) + coriolis;
        Eigen::Matrix<double, 7, 1>::Map(&torques->tau_J[0]) = joint_torques;
    }
    //Simple torque
    else
    {
        //Reading input and calculating torques
        for (size_t i = 0; i < 7; i++) torques->tau_J[i] = input_.get(joint_torque[i]).get_real();
    }

    //Writing output
    robot_write_output_(robot_state);
    output_.set(robot_mode, torques->motion_finished ? RobotMode::invalid : input_mode);
    output_.set(control_reset, input_reset ? 1.0 : 0.0);
    if (input_reset) output_.set(control_error, Error::ok);
}

void franka_o80::Driver::robot_position_control_function_(const franka::RobotState &robot_state, franka::JointPositions *positions)
{
    //Locking
    std::lock_guard<std::mutex> guard(input_output_mutex_);

    //Reading input
    bool input_reset = input_.get(control_reset).get_real() > 0.0;
    RobotMode input_mode = input_.get(robot_mode).get_robot_mode();
    bool input_finished = input_finished_;
    Error input_error = output_.get(control_error).get_error();
    bool input_stiffness_change = false;
    for (size_t i = 0; i < 7; i++)
    {
        if (joint_stiffness_(i) != input_.get(joint_stiffness[i]).get_real()) { input_stiffness_change = true; break; }
    }

    //Exit
    if (input_mode != robot_mode_ || input_finished || input_error != Error::ok || input_reset || input_stiffness_change)
    {
        for (size_t i = 0; i < 7; i++) positions->q[i] = robot_state.q[i];
        positions->motion_finished = true;
    }
    //Normal
    else
    {
        for (size_t i = 0; i < 7; i++) positions->q[i] = input_.get(joint_position[i]).get_real();
        positions->q[6] = input_.get(joint_position[6]).get_real() + M_PI / 4;
    }

   //Writing output
    robot_write_output_(robot_state);
    output_.set(robot_mode, positions->motion_finished ? RobotMode::invalid : input_mode);
    output_.set(control_reset, input_reset ? 1.0 : 0.0);
    if (input_reset) output_.set(control_error, Error::ok);
}

void franka_o80::Driver::robot_velocity_control_function_(const franka::RobotState &robot_state, franka::JointVelocities *velocities)
{
    //Locking
    std::lock_guard<std::mutex> guard(input_output_mutex_);

    //Reading input
    bool input_reset = input_.get(control_reset).get_real() > 0.0;
    RobotMode input_mode = input_.get(robot_mode).get_robot_mode();
    bool input_finished = input_finished_;
    Error input_error = output_.get(control_error).get_error();
    bool input_stiffness_change = false;
    for (size_t i = 0; i < 7; i++)
    {
        if (joint_stiffness_(i) != input_.get(joint_stiffness[i]).get_real()) { input_stiffness_change = true; break; }
    }

    //Exit
    if (input_mode != robot_mode_ || input_finished || input_error != Error::ok || input_reset || input_stiffness_change)
    {
        for (size_t i = 0; i < 7; i++) velocities->dq[i] = 0.0;
        velocities->motion_finished = true;
    }
    //Normal
    else
    {
        for (size_t i = 0; i < 7; i++) velocities->dq[i] = input_.get(joint_velocity[i]).get_real();
    }

    //Writing output
    robot_write_output_(robot_state);
    output_.set(robot_mode, velocities->motion_finished ? RobotMode::invalid : input_mode);
    output_.set(control_reset, input_reset ? 1.0 : 0.0);
    if (input_reset) output_.set(control_error, Error::ok);
}

void franka_o80::Driver::robot_cartesian_position_control_function_(const franka::RobotState &robot_state, franka::CartesianPose *positions)
{
    //Locking
    std::lock_guard<std::mutex> guard(input_output_mutex_);

    //Reading input
    bool input_reset = input_.get(control_reset).get_real() > 0.0;
    RobotMode input_mode = input_.get(robot_mode).get_robot_mode();
    bool input_finished = input_finished_;
    Error input_error = output_.get(control_error).get_error();
    bool input_stiffness_change = false;
    for (size_t i = 0; i < 6; i++)
    {
        if (cartesian_stiffness_(i) != input_.get(cartesian_stiffness[i]).get_real()) { input_stiffness_change = true; break; }
    }

    //Exit
    if (input_mode != robot_mode_ || input_finished || input_error != Error::ok || input_reset || input_stiffness_change)
    {
        for (size_t i = 0; i < 16; i++) positions->O_T_EE[i] = robot_state.O_T_EE[i];
        for (size_t i = 0; i < 2; i++) positions->elbow[i] = robot_state.elbow[i];
        positions->motion_finished = true;
    }
    //Normal
    else
    {
        Eigen::Affine3d transform;
        transform.translation() = Eigen::Vector3d(input_.get(cartesian_position[0]).get_real(), input_.get(cartesian_position[1]).get_real(), input_.get(cartesian_position[2]).get_real());
        transform.linear() = input_.get(cartesian_orientation).get_quaternion().toRotationMatrix();
        Eigen::Matrix<double, 4, 4>::Map(&positions->O_T_EE[0]) = transform.matrix();
        positions->elbow[0] = input_.get(joint_position[2]).get_real();
        positions->elbow[1] = input_.get(joint_position[3]).get_real() > 0.0 ? 1.0 : -1.0;
    }

    //Writing output
    robot_write_output_(robot_state);
    output_.set(robot_mode, positions->motion_finished ? RobotMode::invalid : input_mode);
    output_.set(control_reset, input_reset ? 1.0 : 0.0);
    if (input_reset) output_.set(control_error, Error::ok);
}

void franka_o80::Driver::robot_cartesian_velocity_control_function_(const franka::RobotState &robot_state, franka::CartesianVelocities *velocities)
{
    //Locking
    std::lock_guard<std::mutex> guard(input_output_mutex_);

    //Reading input
    bool input_reset = input_.get(control_reset).get_real() > 0.0;
    RobotMode input_mode = input_.get(robot_mode).get_robot_mode();
    bool input_finished = input_finished_;
    Error input_error = output_.get(control_error).get_error();
    bool input_stiffness_change = false;
    for (size_t i = 0; i < 6; i++)
    {
        if (cartesian_stiffness_(i) != input_.get(cartesian_stiffness[i]).get_real()) { input_stiffness_change = true; break; }
    }

    //Exit
    if (input_mode != robot_mode_ || input_finished || input_error != Error::ok || input_reset || input_stiffness_change)
    {
        for (size_t i = 0; i < 6; i++) velocities->O_dP_EE[i] = 0.0;
        velocities->motion_finished = true;
    }
    //Normal
    else
    {
        for (size_t i = 0; i < 3; i++)
        {
            velocities->O_dP_EE[i] = input_.get(cartesian_velocity[i]).get_real();
            velocities->O_dP_EE[i + 3] = input_.get(cartesian_rotation[i]).get_real();
        }
        velocities->elbow[0] = input_.get(joint_position[2]).get_real();
        velocities->elbow[1] = input_.get(joint_position[3]).get_real() > 0.0 ? 1.0 : -1.0;
    }

    //Writing output
    robot_write_output_(robot_state);
    output_.set(robot_mode, velocities->motion_finished ? RobotMode::invalid : input_mode);
    output_.set(control_reset, input_reset ? 1.0 : 0.0);
    if (input_reset) output_.set(control_error, Error::ok);
}

void franka_o80::Driver::robot_control_function_()
{
    Error output_error = Error::ok; //One round error, exists here because loop goes after communication
    while (true)
    {
        bool input_reset;
        RobotMode input_mode;
        bool input_finished;
        Error input_error;
        std::array<double, 7> input_joint_stiffness;
        std::array<double, 6> input_cartesian_stiffness;
        {
            //Locking
            std::lock_guard<std::mutex> guard(input_output_mutex_);

            //Reading input
            input_reset = input_.get(control_reset).get_real() > 0.0;
            input_mode = input_.get(robot_mode).get_robot_mode();
            input_finished = input_finished_;
            input_error = output_.get(control_error).get_error();
            for (size_t i = 0; i < 7; i++) input_joint_stiffness[i] = input_.get(joint_stiffness[i]).get_real();
            for (size_t i = 0; i < 6; i++) input_cartesian_stiffness[i] = input_.get(cartesian_stiffness[i]).get_real();

            //Writing output
            if (input_reset) { output_.set(control_error, Error::ok); input_error = Error::ok; }
            else if (input_error == Error::ok && output_error != Error::ok) { output_.set(control_error, output_error); input_error = output_error; }
        }

        //Entering control loop
        try
        {
            //Resetting
            if (input_finished) return;
            else if (input_reset) robot_->automaticErrorRecovery();

            //Setting per-loop constants
            joint_stiffness_ = Eigen::Matrix<double, 7, 1>::Map(&input_joint_stiffness[0]);
            robot_->setJointImpedance(input_joint_stiffness);
            cartesian_stiffness_ = Eigen::Matrix<double, 6, 1>::Map(&input_cartesian_stiffness[0]);
            robot_->setCartesianImpedance(input_cartesian_stiffness);
            robot_mode_ = input_mode;

            //Entering loop
            Driver *driver = this;
            output_error = Error::ok;
            if (input_reset || input_error != Error::ok || input_mode == RobotMode::invalid) robot_->control(
                [driver](const franka::RobotState &robot_state, franka::Duration) -> franka::JointVelocities
                {
                    franka::JointVelocities velocities(std::array<double, 7>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}));
                    driver->robot_dummy_control_function_(robot_state, &velocities);
                    return velocities;
                });
            else if (input_mode == RobotMode::torque || input_mode == RobotMode::intelligent_position || input_mode == RobotMode::intelligent_cartesian_position) robot_->control(
                [driver](const franka::RobotState &robot_state, franka::Duration) -> franka::Torques
                {
                    franka::Torques torques(std::array<double, 7>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}));
                    driver->robot_torque_control_function_(robot_state, &torques);
                    return torques;
                });
            else if (input_mode == RobotMode::torque_position) robot_->control(
                [driver](const franka::RobotState &robot_state, franka::Duration) -> franka::Torques
                {
                    franka::Torques torques(std::array<double, 7>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}));
                    driver->robot_torque_control_function_(robot_state, &torques);
                    return torques;
                },
                [driver](const franka::RobotState &robot_state, franka::Duration) -> franka::JointPositions
                {
                    franka::JointPositions positions(std::array<double, 7>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}));
                    driver->robot_position_control_function_(robot_state, &positions);
                    return positions;
                });
            else if (input_mode == RobotMode::torque_velocity) robot_->control(
                [driver](const franka::RobotState &robot_state, franka::Duration) -> franka::Torques
                {
                    franka::Torques torques(std::array<double, 7>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}));
                    driver->robot_torque_control_function_(robot_state, &torques);
                    return torques;
                },
                [driver](const franka::RobotState &robot_state, franka::Duration) -> franka::JointVelocities
                {
                    franka::JointVelocities velocities(std::array<double, 7>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}));
                    driver->robot_velocity_control_function_(robot_state, &velocities);
                    return velocities;
                });
            else if (input_mode == RobotMode::torque_cartesian_position) robot_->control(
                [driver](const franka::RobotState &robot_state, franka::Duration) -> franka::Torques
                {
                    franka::Torques torques(std::array<double, 7>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}));
                    driver->robot_torque_control_function_(robot_state, &torques);
                    return torques;
                },
                [driver](const franka::RobotState &robot_state, franka::Duration) -> franka::CartesianPose
                {
                    franka::CartesianPose cartesian_position(std::array<double, 16>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), std::array<double, 2>({0.0, 0.0}));
                    driver->robot_cartesian_position_control_function_(robot_state, &cartesian_position);
                    return cartesian_position;
                });
            else if (input_mode == RobotMode::torque_cartesian_velocity) robot_->control(
                [driver](const franka::RobotState &robot_state, franka::Duration) -> franka::Torques
                {
                    franka::Torques torques(std::array<double, 7>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}));
                    driver->robot_torque_control_function_(robot_state, &torques);
                    return torques;
                },
                [driver](const franka::RobotState &robot_state, franka::Duration) -> franka::CartesianVelocities
                {
                    franka::CartesianVelocities cartesian_velocity(std::array<double, 6>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), std::array<double, 2>({0.0, 0.0}));
                    driver->robot_cartesian_velocity_control_function_(robot_state, &cartesian_velocity);
                    return cartesian_velocity;
                });
            else if (input_mode == RobotMode::position) robot_->control(
                [driver](const franka::RobotState &robot_state, franka::Duration) -> franka::JointPositions
                {
                    franka::JointPositions positions(std::array<double, 7>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}));
                    driver->robot_position_control_function_(robot_state, &positions);
                    return positions;
                });
            else if (input_mode == RobotMode::velocity) robot_->control(
                [driver](const franka::RobotState &robot_state, franka::Duration) -> franka::JointVelocities
                {
                    franka::JointVelocities velocities(std::array<double, 7>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}));
                    driver->robot_velocity_control_function_(robot_state, &velocities);
                    return velocities;
                });
            else if (input_mode == RobotMode::cartesian_position) robot_->control(
                [driver](const franka::RobotState &robot_state, franka::Duration) -> franka::CartesianPose
                {
                    franka::CartesianPose cartesian_position(std::array<double, 16>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), std::array<double, 2>({0.0, 0.0}));
                    driver->robot_cartesian_position_control_function_(robot_state, &cartesian_position);
                    return cartesian_position;
                });
            else if (input_mode == RobotMode::cartesian_velocity) robot_->control(
                [driver](const franka::RobotState &robot_state, franka::Duration) -> franka::CartesianVelocities
                {
                    franka::CartesianVelocities cartesian_velocity(std::array<double, 6>({0.0, 0.0, 0.0, 0.0, 0.0, 0.0}), std::array<double, 2>({0.0, 0.0}));
                    driver->robot_cartesian_velocity_control_function_(robot_state, &cartesian_velocity);
                    return cartesian_velocity;
                });
        }
        catch (franka::CommandException &e)
        {
            output_error = Error::robot_command_exception;
            std::cout << e.what() << std::endl;
        }
        catch (franka::ControlException &e)
        {
            output_error = Error::robot_control_exception;
            std::cout << e.what() << std::endl;
        }
        catch (franka::InvalidOperationException &e)
        {
            output_error = Error::robot_invalid_operation_exception;
            std::cout << e.what() << std::endl;
        }
        catch (franka::NetworkException &e)
        {
            output_error = Error::robot_network_exception;
            std::cout << e.what() << std::endl;
        }
        catch (franka::RealtimeException &e)
        {
            output_error = Error::robot_realtime_exception;
            std::cout << e.what() << std::endl;
        }
        catch (std::invalid_argument &e)
        {
            output_error = Error::robot_invalid_argument_exception;
            std::cout << e.what() << std::endl;
        }
        catch (...)
        {
            output_error = Error::robot_other_exception;
        }

        //Waiting if failed
        if (output_error != Error::ok) std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
}

void franka_o80::Driver::gripper_control_function_()
{
    Error output_error = Error::ok; //one round error
    double previous_input_width = std::numeric_limits<double>::quiet_NaN();

    while (true)
    {
        //Reading state
        double output_width;
        double output_temperature;
        try
        {
            franka::GripperState state = gripper_->readOnce();
            output_width = state.width;
            output_temperature = state.temperature;
        }
        catch (franka::NetworkException &e)
        {
            if (output_error == Error::ok) output_error = Error::gripper_network_exception;
            std::cout << e.what() << std::endl;
        }
        catch (franka::InvalidOperationException &e)
        {
            if (output_error == Error::ok) output_error = Error::gripper_invalid_operation_exception;
            std::cout << e.what() << std::endl;
        }
        catch (...)
        {
            if (output_error == Error::ok) output_error = Error::gripper_invalid_operation_exception;
        }

        //Communicating
        bool input_reset;
        GripperMode input_mode;
        bool input_finished;
        Error input_error;
        double input_width;
        double input_velocity;
        double input_force;
        {
            //Locking
            std::lock_guard<std::mutex> guard(input_output_mutex_);

            //Reading input
            input_reset = input_.get(control_reset).get_real() > 0.0;
            input_mode = input_.get(gripper_mode).get_gripper_mode();
            input_finished = input_finished_;
            input_error = output_.get(control_error).get_error();
            input_width = input_.get(gripper_width).get_real();
            input_velocity = input_.get(gripper_velocity).get_real();
            input_force = input_.get(gripper_force).get_real();

            //Writing output
            output_.set(gripper_temperature, output_temperature);
            output_.set(gripper_width, output_width);
            output_.set(gripper_velocity, input_velocity);
            output_.set(gripper_force, input_force);
            output_.set(control_reset, input_reset ? 1.0 : 0.0);
            if (input_reset) { output_.set(control_error, Error::ok); input_error = Error::ok; }
            else if (input_error == Error::ok && output_error != Error::ok) { output_.set(control_error, output_error); input_error = output_error; }
            if (input_reset || input_finished || input_error != Error::ok) { output_.set(gripper_mode, GripperMode::invalid); input_mode = GripperMode::invalid; }
            else output_.set(gripper_mode, input_mode);
        }

        //Action
        output_error = Error::ok;
        if (input_finished) return;
        else if (!input_reset && input_error == Error::ok && input_mode != GripperMode::invalid && input_width != previous_input_width) try
        {
            if (input_mode == GripperMode::grasp) gripper_->grasp(input_width, input_velocity, input_force, 0.1, 0.1);
            else gripper_->move(input_width, input_velocity);
        }
        catch (franka::NetworkException &e)
        {
            output_error = Error::gripper_network_exception;
        }
        catch (franka::InvalidOperationException)
        {
            output_error = Error::gripper_invalid_operation_exception;
        }
        catch (...)
        {
            output_error = Error::gripper_invalid_operation_exception;
        }
        previous_input_width = input_width;
    }
}

franka_o80::Driver::Driver(std::string ip) : ip_(ip)
{
}

void franka_o80::Driver::start()
{
    if (started_) return;
    started_ = true;

    //Initialize robot
    robot_ = std::unique_ptr<franka::Robot>(new franka::Robot(ip_));
    model_ = std::unique_ptr<franka::Model>(new franka::Model(robot_->loadModel()));
    robot_->automaticErrorRecovery();
    franka::RobotState robot_state = robot_->readOnce();
    if (robot_state.robot_mode != franka::RobotMode::kIdle) throw std::runtime_error("Driver cannot be started in current robot mode");
    robot_write_output_(robot_state);
    robot_->setCollisionBehavior(
      {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
      {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
      {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
      {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});

    //Initialize gripper
    gripper_ = std::unique_ptr<franka::Gripper>(new franka::Gripper(ip_));
    franka::GripperState gripper_state = gripper_->readOnce();
    output_.set(gripper_width, gripper_state.width);
    output_.set(gripper_temperature, gripper_state.temperature);
    gripper_->homing();

    //Start
    input_ = output_;
    robot_control_thread_ = std::thread([](Driver *driver) -> void { driver->robot_control_function_(); }, this);
    gripper_control_thread_ = std::thread([](Driver *driver) -> void { driver->gripper_control_function_(); }, this);
    ok_ = true;
}

void franka_o80::Driver::set(const DriverInput &input)
{
    if (!ok_) throw std::runtime_error("Driver is not properly initialized");
    {
        std::lock_guard<std::mutex> guard(input_output_mutex_);
        input_ = input;
    }
}

franka_o80::DriverOutput franka_o80::Driver::get()
{
    if (!ok_) throw std::runtime_error("Driver is not properly initialized");
    DriverOutput output;
    {
        std::lock_guard<std::mutex> guard(input_output_mutex_);
        output = output_;
    }
    return output;
}

void franka_o80::Driver::stop()
{
    if (!started_) return;
    started_ = false;

    //Finalizing robot
    {
        std::lock_guard<std::mutex> guard(input_output_mutex_);
        input_finished_ = true;
    }
    if (robot_control_thread_.joinable()) robot_control_thread_.join();

    //Finalizing gripper
    if (ok_) gripper_->stop();
    if (gripper_control_thread_.joinable()) gripper_control_thread_.join();

    ok_ = false;
}


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is licensed under the Apache License,
//  Version 2.0 (the "License"); you may not use this file except in
//  compliance with the License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

//
#include <vw/Core/Exception.h>
#include <vw/Core/Log.h>
#include <vw/Core/TypeDeduction.h>
#include <vw/Math/Matrix.h>
#include <vw/Math/Quaternion.h>
#include <vw/Math/Vector.h>
#include <vw/Camera/CAHVModel.h>
#include <vw/Camera/PinholeModel.h>

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

namespace vw {
namespace camera {

  /// This constructor takes a filename and reads in a camera model
  /// from the file.  The file may contain either CAHV parameters or
  /// pinhole camera parameters.
  CAHVModel::CAHVModel(std::string const& filename) {
    if (filename.empty())
      vw_throw( IOErr() << "CAHVModel: null file name passed to constructor." );

    if (boost::ends_with(filename, ".cahv"))
      read_cahv(filename);
    else if (boost::ends_with(filename, ".pin"))
      read_pinhole(filename);
    else
      vw_throw( IOErr() << "CAHVModel: Unknown camera file suffix." );
  }

  /// Initialize the CAHV vectors directly in the native CAHV format.
  CAHVModel::CAHVModel(Vector3 const& C_vec,
                       Vector3 const& A_vec,
                       Vector3 const& H_vec,
                       Vector3 const& V_vec) :
    C(C_vec), A(A_vec), H(H_vec), V(V_vec) {}

  /// Initialize a CAHV model from a pinhole model
  CAHVModel::CAHVModel(PinholeModel const& pin_model) {
    //  Intrinsic parametes (in pixel units)
    Vector2 focal = pin_model.focal_length();
    Vector2 offset = pin_model.point_offset();

    Vector3 u,v,w;
    pin_model.coordinate_frame(u,v,w);

    Matrix<double,3,3> R = pin_model.camera_pose().rotation_matrix();

    Vector3 Hvec = R*u;
    Vector3 Vvec = R*v;

    C = pin_model.camera_center();
    A = R*w;
    H = focal[0]*Hvec + offset[0]*A;
    V = focal[1]*Vvec + offset[1]*A;
  }

  std::string
  CAHVModel::type() const { return "CAHV"; }

  // FIXME -- Double check anything related to PinholeModel
  CAHVModel CAHVModel::operator= (PinholeModel const& pin_model) {

    //  Pinhole model parameters (in pixel units)
    double fH, fV, Hc, Vc;
    pin_model.intrinsic_parameters(fH, fV, Hc, Vc);

    //  Unit vectors defining camera coordinate frame
    Vector3 u,v,w;
    pin_model.coordinate_frame(u,v,w);

    //  The true rotation between world and camera coordinate
    //  frames includes the rotation R --AND-- a rotation from
    //  specifying the directions of increasing u,v,w pixels
    Matrix<double,3,3> R = pin_model.camera_pose().rotation_matrix();

    //  Now create the components of the CAHV model...
    Vector3 Hvec = R*u;
    Vector3 Vvec = R*v;

    C = pin_model.camera_center();
    A = R*w;
    H = fH*Hvec + Hc*A;
    V = fV*Vvec + Vc*A;

    return *this;
  }

  CAHVModel::CAHVModel(double f, Vector2 const& pixel_size,
                       double xmin, double /*xmax*/, double ymin, double /*ymax*/,
                       Matrix<double, 4, 4> const& view_matrix) {

    double fH = f/pixel_size.x();
    double fV = f/pixel_size.y();
    double Hc = -xmin;
    double Vc = -ymin;

    Vector3 Hvec(view_matrix[0][0], view_matrix[0][1], view_matrix[0][2]);
    Vector3 Vvec(view_matrix[1][0], view_matrix[1][1], view_matrix[1][2]);

    C = Vector3(view_matrix[3][0], view_matrix[3][1], view_matrix[3][2]);
    A = Vector3(view_matrix[2][0], view_matrix[2][1], view_matrix[2][2]);
    H = fH*Hvec + Hc*A;
    V = fV*Vvec + Vc*A;
  }

  CAHVModel::CAHVModel(double f, Vector2 const& pixel_size, double Hc, double Vc,
                       Vector3 const& Cinit, Vector3 const& Ainit,
                       Vector3 const& Hvec, Vector3 const& Vvec) {
    double fH = f/pixel_size.x();
    double fV = f/pixel_size.y();
    C = Cinit;
    A = Ainit;
    H = fH*Hvec + Hc*A;
    V = fV*Vvec + Vc*A;
  }

  Vector2 CAHVModel::point_to_pixel(Vector3 const& point) const {
    double dDot = dot_prod(point-C, A);
    return Vector2( dot_prod(point-C, H) / dDot,
                    dot_prod(point-C, V) / dDot );
  }

  Vector3 CAHVModel::pixel_to_vector(Vector2 const& pix) const {

    // Find vector
    Vector3 vec =
      normalize(cross_prod(V - pix.y() * A,
                           H - pix.x() * A));

    // The vector VxH should be pointing in the same directions as A,
    // if it isn't (because we have a left handed system), flip the
    // vector.
    if (dot_prod(cross_prod(V, H), A) < 0.0)
      vec *= -1.0;
    return vec;
  }

  Vector3 CAHVModel::camera_center(Vector2 const& pix) const {
    return C;
  };

  // --------------------------------------------------
  //                 Private Methods
  // --------------------------------------------------
  void CAHVModel::read_cahv(std::string const& filename) {

    try {
      std::ifstream input(filename.c_str(), std::ifstream::in);
      input.exceptions(std::ifstream::failbit | std::ifstream::badbit);

      vw_out(InfoMessage, "camera") << "Reading CAHV file: "
                                    << filename << ".\n";

      char r1, r2;

      while (true) {
        input.ignore(1024, 'C');
        input >> r1;
        if (r1 == '=')
          break;
      }
      input >> C[0] >> C[1] >> C[2];

      input >> r1 >> r2;
      if (r1 != 'A' || r2 != '=')
        vw_throw( IOErr() << "CAHVModel: Could not read A vector\n" );
      input >> A(0) >> A(1) >> A(2);

      input >> r1 >> r2;
      if (r1 != 'H' || r2 != '=')
        vw_throw( IOErr() << "CAHVModel: Could not read H vector\n" );
      input >> H(0) >> H(1) >> H(2);

      input >> r1 >> r2;
      if (r1 != 'V' || r2 != '=')
        vw_throw( IOErr() << "CAHVModel: Could not read V vector\n" );
      input >> V(0) >> V(1) >> V(2);

    } catch ( const std::ifstream::failure& e ) {
      vw_throw( IOErr() << "CAHVModel: Could not read file: " << filename << " (" << e.what() << ")" );
    }
  }

  void CAHVModel::read_pinhole(std::string const& filename) {
    FILE *camFP = fopen(filename.c_str(), "r");

    if (camFP == 0)
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not open file\n" );

    char line[2048];
    double f, fH, fV, Hc, Vc;
    Vector2 pixelSize;
    Vector3 Hvec, Vvec;

    // Read intrinsic parameters
    if (!fgets(line, sizeof(line), camFP) ||
        sscanf(line,"f = %lf", &f) != 1) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read focal length\n" );
      fclose(camFP);
    }

    if (!fgets(line, sizeof(line), camFP) ||
        sscanf(line,"SP = %lf %lf", &pixelSize.x(), &pixelSize.y()) != 2) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read pixel size\n" );
      fclose(camFP);
    }

    if (!fgets(line, sizeof(line), camFP) ||
        sscanf(line,"IC = %lf %lf", &Hc, &Vc) != 2) {
      vw_throw( IOErr() << "CAHVModel::ReadPinhole: Could not read image center pos\n" );
      fclose(camFP);
    }

    // Read extrinsic parameters
    if (!fgets(line, sizeof(line), camFP) ||
        sscanf(line,"C = %lf %lf %lf", &C(0), &C(1), &C(2)) != 3) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read C vector\n" );
      fclose(camFP);
    }

    if (!fgets(line, sizeof(line), camFP) ||
        sscanf(line,"A = %lf %lf %lf", &A(0), &A(1), &A(2)) != 3) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read A vector\n" );
      fclose(camFP);
    }

    if (!fgets(line, sizeof(line), camFP) ||
        sscanf(line,"Hv = %lf %lf %lf", &Hvec(0), &Hvec(1), &Hvec(2)) != 3) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read Hvec\n" );
      fclose(camFP);
    }

    if (!fgets(line, sizeof(line), camFP) ||
        sscanf(line,"Vv = %lf %lf %lf", &Vvec(0), &Vvec(1), &Vvec(2)) != 3) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read Vvec\n" );
      fclose(camFP);
    }

    // In the future, we should also read in a view matrix -- LJE
    //     double dummy
    //     if (sscanf(line, "VM = %lf %lf %lf %f %lf %lf %lf %f "
    //        "%lf %lf %lf %f %lf %lf %lf %f ",
    //        &Hvec(0), &Hvec(1), &Hvec(2), &dummy,
    //        &Vvec(0), &Vvec(1), &Vvec(2), &dummy,
    //        &A(0), &A(1), &A(2), &dummy,
    //        &C(0), &C(1), &C(2), &dummy) != 16)
    //     {
    //       vw_throw( IOErr()
    //  << "CAHVModel::ReadPinhole: Could not read view matrix\n" );
    //       fclose(camFP);
    //     }

    fH = f/pixelSize.x();
    fV = f/pixelSize.y();

    H = fH*Hvec + Hc*A;
    V = fV*Vvec + Vc*A;

    fclose(camFP);
  }

  void epipolar(CAHVModel const src_camera0, CAHVModel const src_camera1,
                CAHVModel &dst_camera0, CAHVModel &dst_camera1) {

    // Compute a common image center and scale for the two models
    double hc = dot_prod(src_camera0.H, src_camera0.A) / 2.0 +
      dot_prod(src_camera1.H, src_camera1.A) / 2.0;
    double vc = dot_prod(src_camera0.V, src_camera0.A) / 2.0 +
      dot_prod(src_camera1.V, src_camera1.A) / 2.0;

    double hs = norm_2(cross_prod(src_camera0.A, src_camera0.H))/2.0 +
      norm_2(cross_prod(src_camera1.A, src_camera1.H))/2.0;

    double vs = norm_2(cross_prod(src_camera0.A, src_camera0.V))/2.0 +
      norm_2(cross_prod(src_camera1.A, src_camera1.V))/2.0;

    // Use common center and scale to construct common A, H, V
    Vector3 app  = src_camera0.A + src_camera1.A;

    // Note the directionality of 1 to 0, for consistency later
    Vector3 f = cross_prod(cross_prod(app, src_camera1.C - src_camera0.C), app);

    Vector3 hp;
    if (dot_prod(f, src_camera0.H) > 0)
      hp = f * hs / (norm_2(f));
    else
      hp = -f * hs / (norm_2(f));

    app *= 0.5;
    Vector3 g = hp * dot_prod(app,hp) / (hs * hs);
    Vector3 a = normalize(app - g);
    Vector3 vp = cross_prod(a, hp) * vs / hs;

    dst_camera0.C = src_camera0.C;
    dst_camera1.C = src_camera1.C;

    dst_camera0.A = dst_camera1.A = a;
    dst_camera0.H = dst_camera1.H = hp + hc * a;
    dst_camera0.V = dst_camera1.V = vp + vc * a;
  }

  void CAHVModel::write(std::string const& filename ) {
    try {
      std::ofstream output(filename.c_str(), std::ofstream::out);
      output.exceptions(std::ofstream::failbit | std::ofstream::badbit);
      output.precision(20);

      vw_out(InfoMessage, "camera") << "Writing CAHV file: " << filename << "\n";

      output << "C = " << C[0] << " " << C[1] << " " << C[2] << "\n"
             << "A = " << A[0] << " " << A[1] << " " << A[2] << "\n"
             << "H = " << H[0] << " " << H[1] << " " << H[2] << "\n"
             << "V = " << V[0] << " " << V[1] << " " << V[2] << "\n";
    } catch ( const std::ofstream::failure& e ) {
      vw_throw( IOErr() << "CAHVModel: Could not write file: " << filename << "(" << e.what() << ")" );
    }
  }

  std::ostream& operator<<(std::ostream& str, CAHVModel const& model) {
    str << "CAHV camera: \n";
    str << "\tC: " << model.C << "\n";
    str << "\tA: " << model.A << "\n";
    str << "\tH: " << model.H << "\n";
    str << "\tV: " << model.V << "\n";
    return str;
  }

}} // namespace vw::camera


/*
 Copyright (C) 2021 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/
#include <qle/pricingengines/crossccyswapengine.hpp>
#ifdef QL_USE_INDEXED_COUPON
#include <ql/cashflows/floatingratecoupon.hpp>
#endif

#include <qle/termstructures/crossccyfixfloatmtmresetswaphelper.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {

namespace {
    void no_deletion(YieldTermStructure*) {}
} // namespace

CrossCcyFixFloatMtMResetSwapHelper::CrossCcyFixFloatMtMResetSwapHelper(
    const Handle<Quote>& rate, const Handle<Quote>& spotFx, Natural settlementDays, const Calendar& paymentCalendar,
    BusinessDayConvention paymentConvention, const Period& tenor, const Currency& fixedCurrency,
    Frequency fixedFrequency, BusinessDayConvention fixedConvention, const DayCounter& fixedDayCount,
    const boost::shared_ptr<IborIndex>& index, const Handle<YieldTermStructure>& floatDiscount,
    const Handle<Quote>& spread, bool endOfMonth, bool resetsOnFloatLeg)
    : RelativeDateRateHelper(rate), spotFx_(spotFx), settlementDays_(settlementDays), paymentCalendar_(paymentCalendar),
    paymentConvention_(paymentConvention), tenor_(tenor), fixedCurrency_(fixedCurrency),
    fixedFrequency_(fixedFrequency), fixedConvention_(fixedConvention), fixedDayCount_(fixedDayCount), index_(index),
    floatDiscount_(floatDiscount), spread_(spread), endOfMonth_(endOfMonth), resetsOnFloatLeg_(resetsOnFloatLeg){

    QL_REQUIRE(!spotFx_.empty(), "Spot FX quote cannot be empty.");
    QL_REQUIRE(fixedCurrency_ != index_->currency(), "Fixed currency should not equal float leg currency.");

    registerWith(spotFx_);
    registerWith(index_);
    registerWith(floatDiscount_);
    registerWith(spread_);

    initializeDates();
}

void CrossCcyFixFloatMtMResetSwapHelper::initializeDates() {

    // Swap start and end
    Date referenceDate = evaluationDate_ = Settings::instance().evaluationDate();
    referenceDate = paymentCalendar_.adjust(referenceDate);
    Date start = paymentCalendar_.advance(referenceDate, settlementDays_ * Days);
    Date end = start + tenor_;

    // Fixed schedule
    Schedule fixedSchedule(start, end, Period(fixedFrequency_), paymentCalendar_, fixedConvention_, fixedConvention_,
        DateGeneration::Backward, endOfMonth_);

    // Float schedule
    Schedule floatSchedule(start, end, index_->tenor(), paymentCalendar_, paymentConvention_, paymentConvention_,
        DateGeneration::Backward, endOfMonth_);

    Real nominal = 1.0;

    // build an FX index for forward rate projection (TODO - review settlement and calendar)
    Natural paymentLag = 0;
    Spread floatSpread = spread_.empty() ? 0.0 : spread_->value();
    boost::shared_ptr<FxIndex> fxIdx;
    if (resetsOnFloatLeg_) {
        fxIdx = boost::make_shared<FxIndex>("dummy", settlementDays_, fixedCurrency_, index_->currency(),  paymentCalendar_,
            spotFx_, termStructureHandle_, floatDiscount_);
    } else {
        fxIdx = boost::make_shared<FxIndex>("dummy", settlementDays_, index_->currency(), fixedCurrency_, paymentCalendar_,
            spotFx_, floatDiscount_, termStructureHandle_);
    }

    swap_ = boost::make_shared<CrossCcyFixFloatMtMResetSwap>(nominal, fixedCurrency_, fixedSchedule, 0.0, fixedDayCount_, paymentConvention_,
        paymentLag, paymentCalendar_, index_->currency(), floatSchedule, index_, floatSpread, paymentConvention_,
        paymentLag, paymentCalendar_, fxIdx, resetsOnFloatLeg_);

    // Attach engine
    boost::shared_ptr<PricingEngine> engine = boost::make_shared<CrossCcySwapEngine>(
        fixedCurrency_, termStructureHandle_, index_->currency(), floatDiscount_, spotFx_);
    swap_->setPricingEngine(engine);

    earliestDate_ = swap_->startDate();
    latestDate_ = swap_->maturityDate();

    /* May need to adjust latestDate_ if you are projecting libor based
        on tenor length rather than from accrual date to accrual date. */
#ifdef QL_USE_INDEXED_COUPON
    Size numCashflows = swap_->leg(1).size();
    Date endDate = latestDate_;
    if (numCashflows > 0) {
        for (Size i = numCashflows - 1; i >= 0; i--) {
            boost::shared_ptr<FloatingRateCoupon> lastFloating =
                boost::dynamic_pointer_cast<FloatingRateCoupon>(swap_->leg(1)[i]);
            if (!lastFloating)
                continue;
            else {
                Date fixingValueDate = domesticCcyIndex_->valueDate(lastFloating->fixingDate());
                endDate = domesticCcyIndex_->maturityDate(fixingValueDate);
                Date endValueDate = domesticCcyIndex_->maturityDate(fixingValueDate);
                latestDate_ = std::max(latestDate_, endValueDate);
                break;
            }
        }
    }
#endif
}

void CrossCcyFixFloatMtMResetSwapHelper::setTermStructure(YieldTermStructure* t) {
    boost::shared_ptr<YieldTermStructure> temp(t, no_deletion);
    termStructureHandle_.linkTo(temp, false);
    RelativeDateRateHelper::setTermStructure(t);
}
void CrossCcyFixFloatMtMResetSwapHelper::update() {
    // Maybe FX spot quote or spread quote changed
    if (!close(spotFx_->value(), swap_->nominal()) ||
        (!spread_.empty() && !close(spread_->value(), swap_->floatSpread()))) {
        initializeDates();
    }

    // Maybe evaluation date changed. RelativeDateRateHelper will take care of this
    // Note: if initializeDates() was called in above if statement, it will not be called
    // again in RelativeDateRateHelper::update() because evaluationDate_ is set in
    // initializeDates(). So, redundant instrument builds are avoided.
    RelativeDateRateHelper::update();
}

Real CrossCcyFixFloatMtMResetSwapHelper::impliedQuote() const {
    QL_REQUIRE(termStructure_, "Term structure needs to be set");
    swap_->recalculate();
    return swap_->fairFixedRate();
}

void CrossCcyFixFloatMtMResetSwapHelper::accept(AcyclicVisitor& v) {
    Visitor<CrossCcyFixFloatMtMResetSwapHelper>* v1 = dynamic_cast<Visitor<CrossCcyFixFloatMtMResetSwapHelper>*>(&v);
    if (v1)
        v1->visit(*this);
    else
        RateHelper::accept(v);
}
} // namespace QuantExt