///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 1998-2011, Industrial Light & Magic, a division of Lucas
// Digital Ltd. LLC
//
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
// *       Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// *       Redistributions in binary form must reproduce the above
// copyright notice, this list of conditions and the following disclaimer
// in the documentation and/or other materials provided with the
// distribution.
// *       Neither the name of Industrial Light & Magic nor the names of
// its contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////

#include "PyIlmBaseConfigInternal.h"

#include <Python.h>
#include <boost/python.hpp>
#include <boost/python/make_constructor.hpp>
#include <boost/format.hpp>
#include <ImathVec.h>
#include <ImathQuat.h>
#include <ImathEuler.h>
#include <ImathFun.h>
#include <ImathMatrixAlgo.h>

#include <PyIexExport.h>
#include "PyImathFixedArray.h"
#include "PyImath.h"
#include "PyImathExport.h"
#include "PyImathBasicTypes.h"
#include "PyImathVec.h"
#include "PyImathMatrix.h"
#include "PyImathBox.h"
#include "PyImathFun.h"
#include "PyImathQuat.h"
#include "PyImathEuler.h"
#include "PyImathColor.h"
#include "PyImathFrustum.h"
#include "PyImathPlane.h"
#include "PyImathLine.h"
#include "PyImathRandom.h"
#include "PyImathShear.h"
#include "PyImathMathExc.h"
#include "PyImathAutovectorize.h"
#include "PyImathStringArrayRegister.h"
#include <PyIex.h>

using namespace boost::python;

namespace {

template <typename T>
IMATH_NAMESPACE::Box<IMATH_NAMESPACE::Vec3<T> >
computeBoundingBox(const PyImath::FixedArray<IMATH_NAMESPACE::Vec3<T> >& position)
{
    IMATH_NAMESPACE::Box<IMATH_NAMESPACE::Vec3<T> > bounds;
    int len = position.len();
    for (int i = 0; i < len; ++i)
        bounds.extendBy(position[i]);
    return bounds;
}

IMATH_NAMESPACE::M44d
procrustes1 (PyObject* from_input,
             PyObject* to_input,
             PyObject* weights_input = 0,
             bool doScale = false)
{
    // Verify the sequences:
    if (!PySequence_Check (from_input))
    {
        PyErr_SetString (PyExc_TypeError, "Expected a sequence type for 'from'");
        boost::python::throw_error_already_set();
    }

    if (!PySequence_Check (to_input))
    {
        PyErr_SetString (PyExc_TypeError, "Expected a sequence type for 'to'");
        boost::python::throw_error_already_set();
    }

    bool useWeights = PySequence_Check (weights_input);

    // Now verify the lengths:
    const Py_ssize_t n = PySequence_Length (from_input);
    if (n != PySequence_Length (to_input) ||
        (useWeights && n != PySequence_Length (weights_input)))
    {
        PyErr_SetString (PyExc_TypeError, "'from, 'to', and 'weights' should all have the same lengths.");
        boost::python::throw_error_already_set();
    }

    std::vector<IMATH_NAMESPACE::V3d> from;  from.reserve (n);
    std::vector<IMATH_NAMESPACE::V3d> to;    to.reserve (n);
    std::vector<double> weights;   weights.reserve (n);

    for (Py_ssize_t i = 0; i < n; ++i)
    {
        PyObject* f = PySequence_GetItem (from_input, i);
        PyObject* t = PySequence_GetItem (to_input, i);
        PyObject* w = 0;
        if (useWeights)
            w = PySequence_GetItem (weights_input, i);

        if (f == 0 || t == 0 || (useWeights && w == 0))
        {
            PyErr_SetString (PyExc_TypeError,
                             "Missing element in array");
            boost::python::throw_error_already_set();
        }

        from.push_back (boost::python::extract<IMATH_NAMESPACE::V3d> (f));
        to.push_back (boost::python::extract<IMATH_NAMESPACE::V3d> (t));
        if (useWeights)
            weights.push_back (boost::python::extract<double> (w));
    }

    if (useWeights)
        return IMATH_NAMESPACE::procrustesRotationAndTranslation (&from[0], &to[0], &weights[0], n, doScale);
    else
        return IMATH_NAMESPACE::procrustesRotationAndTranslation (&from[0], &to[0], n, doScale);
}

PyImath::FixedArray2D<int> rangeX(int sizeX, int sizeY)
{
    PyImath::FixedArray2D<int> f(sizeX, sizeY);
    for (int j=0; j<sizeY; j++)
        for (int i=0; i<sizeX; i++)
            f(i,j) = i;
    return f;
}

PyImath::FixedArray2D<int> rangeY(int sizeX, int sizeY)
{
    PyImath::FixedArray2D<int> f(sizeX, sizeY);
    for (int j=0; j<sizeY; j++)
        for (int i=0; i<sizeX; i++)
            f(i,j) = j;
    return f;
}

} // namespace

namespace PyImath {

void register_all()
{
    scope().attr("__doc__") = "Imath module";

    register_basicTypes();

    class_<IntArray2D> iclass2D = IntArray2D::register_("IntArray2D","Fixed length array of ints");
    add_arithmetic_math_functions(iclass2D);
    add_mod_math_functions(iclass2D);
    add_comparison_functions(iclass2D);
    add_ordered_comparison_functions(iclass2D);
    add_explicit_construction_from_type<float>(iclass2D);
    add_explicit_construction_from_type<double>(iclass2D);

    class_<IntMatrix> imclass = IntMatrix::register_("IntMatrix","Fixed size matrix of ints");
    add_arithmetic_math_functions(imclass);

    class_<FloatArray2D> fclass2D = FloatArray2D::register_("FloatArray2D","Fixed length 2D array of floats");
    add_arithmetic_math_functions(fclass2D);
    add_pow_math_functions(fclass2D);
    add_comparison_functions(fclass2D);
    add_ordered_comparison_functions(fclass2D);
    add_explicit_construction_from_type<int>(fclass2D);
    add_explicit_construction_from_type<double>(fclass2D);

    class_<FloatMatrix> fmclass = FloatMatrix::register_("FloatMatrix","Fixed size matrix of floats");
    add_arithmetic_math_functions(fmclass);
    add_pow_math_functions(fmclass);

    class_<DoubleArray2D> dclass2D = DoubleArray2D::register_("DoubleArray2D","Fixed length array of doubles");
    add_arithmetic_math_functions(dclass2D);
    add_pow_math_functions(dclass2D);
    add_comparison_functions(dclass2D);
    add_ordered_comparison_functions(dclass2D);
    add_explicit_construction_from_type<int>(dclass2D);
    add_explicit_construction_from_type<float>(dclass2D);

    class_<DoubleMatrix> dmclass = DoubleMatrix::register_("DoubleMatrix","Fixed size matrix of doubles");
    add_arithmetic_math_functions(dmclass);
    add_pow_math_functions(dmclass);

    def("rangeX", &rangeX);
    def("rangeY", &rangeY);

    //
    //  Vec2
    //
    register_Vec2<short>();
    register_Vec2<int>();
    register_Vec2<float>();
    register_Vec2<double>();
    class_<FixedArray<IMATH_NAMESPACE::V2s> > v2s_class = register_Vec2Array<short>();
    class_<FixedArray<IMATH_NAMESPACE::V2i> > v2i_class = register_Vec2Array<int>();
    class_<FixedArray<IMATH_NAMESPACE::V2f> > v2f_class = register_Vec2Array<float>();
    class_<FixedArray<IMATH_NAMESPACE::V2d> > v2d_class = register_Vec2Array<double>();
    add_explicit_construction_from_type<IMATH_NAMESPACE::V2f>(v2i_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V2d>(v2i_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V2i>(v2f_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V2d>(v2f_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V2i>(v2d_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V2f>(v2d_class);

    //
    //  Vec3
    //
    register_Vec3<unsigned char>();
    register_Vec3<short>();
    register_Vec3<int>();
    register_Vec3<float>();
    register_Vec3<double>();
    class_<FixedArray<IMATH_NAMESPACE::V3s> > v3s_class = register_Vec3Array<short>();
    class_<FixedArray<IMATH_NAMESPACE::V3i> > v3i_class = register_Vec3Array<int>();
    class_<FixedArray<IMATH_NAMESPACE::V3f> > v3f_class = register_Vec3Array<float>();
    class_<FixedArray<IMATH_NAMESPACE::V3d> > v3d_class = register_Vec3Array<double>();
    add_explicit_construction_from_type<IMATH_NAMESPACE::V3f>(v3i_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V3d>(v3i_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V3i>(v3f_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V3d>(v3f_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V3i>(v3d_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V3f>(v3d_class);

    //
    //  Vec4
    //
    register_Vec4<unsigned char>();
    register_Vec4<short>();
    register_Vec4<int>();
    register_Vec4<float>();
    register_Vec4<double>();
    class_<FixedArray<IMATH_NAMESPACE::V4s> > v4s_class = register_Vec4Array<short>();
    class_<FixedArray<IMATH_NAMESPACE::V4i> > v4i_class = register_Vec4Array<int>();
    class_<FixedArray<IMATH_NAMESPACE::V4f> > v4f_class = register_Vec4Array<float>();
    class_<FixedArray<IMATH_NAMESPACE::V4d> > v4d_class = register_Vec4Array<double>();
    add_explicit_construction_from_type<IMATH_NAMESPACE::V4f>(v4i_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V4d>(v4i_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V4i>(v4f_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V4d>(v4f_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V4i>(v4d_class);

    //
    //  Quat
    //
    register_Quat<float>();
    register_Quat<double>();
    class_<FixedArray<IMATH_NAMESPACE::Quatf> > quatf_class = register_QuatArray<float>();
    class_<FixedArray<IMATH_NAMESPACE::Quatd> > quatd_class = register_QuatArray<double>();
    add_explicit_construction_from_type<IMATH_NAMESPACE::Quatd>(quatf_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::Quatf>(quatd_class);

    //
    // Euler
    //
    register_Euler<float>();
    register_Euler<double>();
    class_<FixedArray<IMATH_NAMESPACE::Eulerf> > eulerf_class = register_EulerArray<float>();
    class_<FixedArray<IMATH_NAMESPACE::Eulerd> > eulerd_class = register_EulerArray<double>();
    add_explicit_construction_from_type<IMATH_NAMESPACE::Eulerd>(eulerf_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::Eulerf>(eulerd_class);

    //
    // Box2
    //
    register_Box2<IMATH_NAMESPACE::V2s>();
    register_Box2<IMATH_NAMESPACE::V2i>();
    register_Box2<IMATH_NAMESPACE::V2f>();
    register_Box2<IMATH_NAMESPACE::V2d>();
    class_<FixedArray<IMATH_NAMESPACE::Box2s> > b2s_class = register_BoxArray<IMATH_NAMESPACE::V2s>();
    class_<FixedArray<IMATH_NAMESPACE::Box2i> > b2i_class = register_BoxArray<IMATH_NAMESPACE::V2i>();
    class_<FixedArray<IMATH_NAMESPACE::Box2f> > b2f_class = register_BoxArray<IMATH_NAMESPACE::V2f>();
    class_<FixedArray<IMATH_NAMESPACE::Box2d> > b2d_class = register_BoxArray<IMATH_NAMESPACE::V2d>();

    //
    // Box3
    //
    register_Box3<IMATH_NAMESPACE::V3s>();
    register_Box3<IMATH_NAMESPACE::V3i>();
    register_Box3<IMATH_NAMESPACE::V3f>();
    register_Box3<IMATH_NAMESPACE::V3d>();
    class_<FixedArray<IMATH_NAMESPACE::Box3s> > b3s_class = register_BoxArray<IMATH_NAMESPACE::V3s>();
    class_<FixedArray<IMATH_NAMESPACE::Box3i> > b3i_class = register_BoxArray<IMATH_NAMESPACE::V3i>();
    class_<FixedArray<IMATH_NAMESPACE::Box3f> > b3f_class = register_BoxArray<IMATH_NAMESPACE::V3f>();
    class_<FixedArray<IMATH_NAMESPACE::Box3d> > b3d_class = register_BoxArray<IMATH_NAMESPACE::V3d>();

    //
    // Matrix22/33/44
    //
    register_Matrix22<float>();
    register_Matrix22<double>();
    register_Matrix33<float>();
    register_Matrix33<double>();
    register_Matrix44<float>();
    register_Matrix44<double>();

    //
    // M22/M33/44Array
    //
    class_<FixedArray<IMATH_NAMESPACE::M44d> > m44d_class = register_M44Array<double>();
    class_<FixedArray<IMATH_NAMESPACE::M44f> > m44f_class = register_M44Array<float>();
    add_explicit_construction_from_type< IMATH_NAMESPACE::Matrix44<double> >(m44d_class);
    add_explicit_construction_from_type< IMATH_NAMESPACE::Matrix44<float> > (m44f_class);

    class_<FixedArray<IMATH_NAMESPACE::M33d> > m33d_class = register_M33Array<double>();
    class_<FixedArray<IMATH_NAMESPACE::M33f> > m33f_class = register_M33Array<float>();
    add_explicit_construction_from_type< IMATH_NAMESPACE::Matrix33<double> >(m33d_class);
    add_explicit_construction_from_type< IMATH_NAMESPACE::Matrix33<float> > (m33f_class);

    class_<FixedArray<IMATH_NAMESPACE::M22d> > m22d_class = register_M22Array<double>();
    class_<FixedArray<IMATH_NAMESPACE::M22f> > m22f_class = register_M22Array<float>();
    add_explicit_construction_from_type< IMATH_NAMESPACE::Matrix22<double> >(m22d_class);
    add_explicit_construction_from_type< IMATH_NAMESPACE::Matrix22<float> > (m22f_class);

    //
    // String Array
    //
    register_StringArrays();

    //
    // Color3/4
    //
    register_Color3<unsigned char>();
    register_Color3<float>();
    register_Color4<unsigned char>();
    register_Color4<float>();

    //
    // C3/4Array
    //
    class_<FixedArray<IMATH_NAMESPACE::Color3f> > c3f_class = register_Color3Array<float>();
    class_<FixedArray<IMATH_NAMESPACE::Color3c> > c3c_class = register_Color3Array<unsigned char>();
    add_explicit_construction_from_type<IMATH_NAMESPACE::V3f>(c3f_class);
    add_explicit_construction_from_type<IMATH_NAMESPACE::V3d>(c3f_class);

    class_<FixedArray<IMATH_NAMESPACE::Color4f> > c4f_class = register_Color4Array<float>();
    class_<FixedArray<IMATH_NAMESPACE::Color4c> > c4c_class = register_Color4Array<unsigned char>();

    //
    // Color4Array
    //
    register_Color4Array2D<float>();
    register_Color4Array2D<unsigned char>();

    //
    // Frustum
    //
    register_Frustum<float>();
    register_Frustum<double>();
    register_FrustumTest<float>();
    register_FrustumTest<double>();

    //
    // Plane
    //
    register_Plane<float>();
    register_Plane<double>();

    //
    // Line
    //
    register_Line<float>();
    register_Line<double>();

    //
    // Shear
    //
    register_Shear<float>();
    register_Shear<double>();

    //
    // Utility Functions
    //
    register_functions();

    def("procrustesRotationAndTranslation", procrustes1,
        args("fromPts", "toPts", "weights", "doScale"),  // Can't use 'from' and 'to' because 'from' is a reserved keywork in Python
        "Computes the orthogonal transform (consisting only of rotation and translation) mapping the "
        "'fromPts' points as close as possible to the 'toPts' points in the least squares norm.  The 'fromPts' and "
        "'toPts' lists must be the same length or the function will error out.  If weights "
        "are provided, then the points are weighted (that is, some points are considered more important "
        "than others while computing the transform).  If the 'doScale' parameter is True, then "
        "the resulting matrix is also allowed to have a uniform scale.");

    //
    // Rand
    //
    register_Rand32();
    register_Rand48();

    //
    // Initialize constants
    //

    scope().attr("EULER_XYZ")    = IMATH_NAMESPACE::Eulerf::XYZ;
    scope().attr("EULER_XZY")    = IMATH_NAMESPACE::Eulerf::XZY;
    scope().attr("EULER_YZX")    = IMATH_NAMESPACE::Eulerf::YZX;
    scope().attr("EULER_YXZ")    = IMATH_NAMESPACE::Eulerf::YXZ;
    scope().attr("EULER_ZXY")    = IMATH_NAMESPACE::Eulerf::ZXY;
    scope().attr("EULER_ZYX")    = IMATH_NAMESPACE::Eulerf::ZYX;
    scope().attr("EULER_XZX")    = IMATH_NAMESPACE::Eulerf::XZX;
    scope().attr("EULER_XYX")    = IMATH_NAMESPACE::Eulerf::XYX;
    scope().attr("EULER_YXY")    = IMATH_NAMESPACE::Eulerf::YXY;
    scope().attr("EULER_YZY")    = IMATH_NAMESPACE::Eulerf::YZY;
    scope().attr("EULER_ZYZ")    = IMATH_NAMESPACE::Eulerf::ZYZ;
    scope().attr("EULER_ZXZ")    = IMATH_NAMESPACE::Eulerf::ZXZ;
    scope().attr("EULER_XYZr")   = IMATH_NAMESPACE::Eulerf::XYZr;
    scope().attr("EULER_XZYr")   = IMATH_NAMESPACE::Eulerf::XZYr;
    scope().attr("EULER_YZXr")   = IMATH_NAMESPACE::Eulerf::YZXr;
    scope().attr("EULER_YXZr")   = IMATH_NAMESPACE::Eulerf::YXZr;
    scope().attr("EULER_ZXYr")   = IMATH_NAMESPACE::Eulerf::ZXYr;
    scope().attr("EULER_ZYXr")   = IMATH_NAMESPACE::Eulerf::ZYXr;
    scope().attr("EULER_XZXr")   = IMATH_NAMESPACE::Eulerf::XZXr;
    scope().attr("EULER_XYXr")   = IMATH_NAMESPACE::Eulerf::XYXr;
    scope().attr("EULER_YXYr")   = IMATH_NAMESPACE::Eulerf::YXYr;
    scope().attr("EULER_YZYr")   = IMATH_NAMESPACE::Eulerf::YZYr;
    scope().attr("EULER_ZYZr")   = IMATH_NAMESPACE::Eulerf::ZYZr;
    scope().attr("EULER_ZXZr")   = IMATH_NAMESPACE::Eulerf::ZXZr;
    scope().attr("EULER_X_AXIS") = IMATH_NAMESPACE::Eulerf::X;
    scope().attr("EULER_Y_AXIS") = IMATH_NAMESPACE::Eulerf::Y;
    scope().attr("EULER_Z_AXIS") = IMATH_NAMESPACE::Eulerf::Z;

    scope().attr("INT_MIN")      = IMATH_NAMESPACE::limits<int>::min();
    scope().attr("INT_MAX")      = IMATH_NAMESPACE::limits<int>::max();
    scope().attr("INT_SMALLEST") = IMATH_NAMESPACE::limits<int>::smallest();
    scope().attr("INT_EPS")      = IMATH_NAMESPACE::limits<int>::epsilon();

    scope().attr("FLT_MIN")      = IMATH_NAMESPACE::limits<float>::min();
    scope().attr("FLT_MAX")      = IMATH_NAMESPACE::limits<float>::max();
    scope().attr("FLT_SMALLEST") = IMATH_NAMESPACE::limits<float>::smallest();
    scope().attr("FLT_EPS")      = IMATH_NAMESPACE::limits<float>::epsilon();

    scope().attr("DBL_MIN")      = IMATH_NAMESPACE::limits<double>::min();
    scope().attr("DBL_MAX")      = IMATH_NAMESPACE::limits<double>::max();
    scope().attr("DBL_SMALLEST") = IMATH_NAMESPACE::limits<double>::smallest();
    scope().attr("DBL_EPS")      = IMATH_NAMESPACE::limits<double>::epsilon();

    //
    // Register Exceptions
    //
    PyIex::registerExc<IMATH_NAMESPACE::NullVecExc,IEX_NAMESPACE::MathExc>("NullVecExc","imath");
    PyIex::registerExc<IMATH_NAMESPACE::NullQuatExc,IEX_NAMESPACE::MathExc>("NullQuatExc","imath");
    PyIex::registerExc<IMATH_NAMESPACE::SingMatrixExc,IEX_NAMESPACE::MathExc>("SingMatrixExc","imath");
    PyIex::registerExc<IMATH_NAMESPACE::ZeroScaleExc,IEX_NAMESPACE::MathExc>("ZeroScaleExc","imath");
    PyIex::registerExc<IMATH_NAMESPACE::IntVecNormalizeExc,IEX_NAMESPACE::MathExc>("IntVecNormalizeExc","imath");

    def("computeBoundingBox", &computeBoundingBox<float>,
        "computeBoundingBox(position) -- computes the bounding box from the position array.");

    def("computeBoundingBox", &computeBoundingBox<double>,
        "computeBoundingBox(position) -- computes the bounding box from the position array.");
}

} // namespace PyImath


#include <ros/ros.h>
#include <jsk_topic_tools/log_utils.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>

#include <Eigen/Dense>

#include "jsk_perception/WhiteBalance.h"
#include "jsk_perception/WhiteBalancePoints.h"

class WhiteBalanceConverter {

private:
  ros::NodeHandle nh_;

  ros::ServiceServer service_img_;
  ros::ServiceServer service_pts_;

  double cmat[12]; // convert_matrix
  double pmat[48]; // paramteter matrix
  //param
  int queue_size_;

  typedef union
  {
    struct /*anonymous*/
    {
      unsigned char Blue; // Blue channel
      unsigned char Green; // Green channel
      unsigned char Red; // Red channel
      unsigned char Alpha;
    };
    float float_value;
    long long_value;
  } RGBValue;

public:
  WhiteBalanceConverter() : nh_ () {
    ros::NodeHandle pnh("~");

    if (pnh.hasParam("parameter_matrix")) {
      XmlRpc::XmlRpcValue param_val;
      pnh.getParam("parameter_matrix", param_val);
      // ROS_INFO("MATRIX %d", param_val.size());
      if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray && param_val.size() == 48) {
        for ( int i = 0; i < 48; i++) {
          pmat[i] = param_val[i];
        }
      }
      for ( int i = 0; i < 12; i++ ) {
        ROS_INFO("[%f, %f, %f, %f]", pmat[4*i + 0], pmat[4*i + 1], pmat[4*i + 2], pmat[4*i + 3]);
      }
    }

    service_img_ = pnh.advertiseService(pnh.resolveName("convert_image"),
                                        &WhiteBalanceConverter::imageCallback, this);
    service_pts_ = pnh.advertiseService(pnh.resolveName("convert_points"),
                                        &WhiteBalanceConverter::pointsCallback, this);
  }

  bool imageCallback(jsk_perception::WhiteBalance::Request &req,
                     jsk_perception::WhiteBalance::Response &res) {
    req.reference_color.resize(3); // Quick Edit to avoid segfaults
    makeConvertMatrix(req.reference_color[0], req.reference_color[1], req.reference_color[2]);

    return true;
  }

  bool pointsCallback(jsk_perception::WhiteBalancePoints::Request &req,
                      jsk_perception::WhiteBalancePoints::Response &res) {
    req.reference_color.resize(3); // Quick Edit to avoid segfaults
    makeConvertMatrix(req.reference_color[0], req.reference_color[1], req.reference_color[2]);

    int rgb_offset = -1;
    for (size_t i = 0; i < req.input.fields.size(); i++) {
      if ( req.input.fields[i].name == "rgb" ) {
        rgb_offset = req.input.fields[i].offset;
        break;
      }
    }
    //ROS_INFO("offst = %d", rgb_offset);

    if ( rgb_offset < 0 ) {
      ROS_WARN("point cloud didn't have rgb field");
      return false;
    }

    res.output = req.input;
    int size = req.input.height * req.input.width;
    int step = req.input.point_step;
    unsigned char *sdata = (unsigned char *) &(req.input.data[0]);
    unsigned char *ddata = (unsigned char *) &(res.output.data[0]);

    for(int i = 0; i < size; i++, sdata += step, ddata += step) {
      RGBValue rgb;
      rgb.float_value = *((float *)(sdata + rgb_offset));
      int r = rgb.Red; int g = rgb.Green; int b = rgb.Blue;
      convertColor(r, g, b);
      rgb.long_value = 0;
      rgb.Red = (unsigned char)r;
      rgb.Green = (unsigned char)g;
      rgb.Blue = (unsigned char)b;
      *((float *)(ddata + rgb_offset)) = rgb.float_value;
    }

    return true;
  }

  inline void convertColor (int &r, int &g, int &b) {
    double rr = ( cmat[0] * r ) + ( cmat[1] * g ) + (  cmat[2] * b ) +  cmat[3]*256;
    double gg = ( cmat[4] * r ) + ( cmat[5] * g ) + (  cmat[6] * b ) +  cmat[7]*256;
    double bb = ( cmat[8] * r ) + ( cmat[9] * g ) + ( cmat[10] * b ) + cmat[11]*256;

    r = round(rr);
    g = round(gg);
    b = round(bb);

    if(r > 255) r = 255; if (r < 0) r = 0;
    if(g > 255) g = 255; if (g < 0) g = 0;
    if(b > 255) b = 255; if (b < 0) b = 0;
  }

  void makeConvertMatrix(float refr, float refg, float refb) {
    for(int i = 0; i < 12; i++) {
      cmat[i] =
        ( pmat[4*i + 0] * refr ) +
        ( pmat[4*i + 1] * refg ) +
        ( pmat[4*i + 2] * refb ) +
        pmat[4*i + 3] ;
    }
    ROS_INFO("%f %f %f %f", cmat[0], cmat[1], cmat[2], cmat[3]);
    ROS_INFO("%f %f %f %f", cmat[4], cmat[5], cmat[6], cmat[7]);
    ROS_INFO("%f %f %f %f", cmat[8], cmat[9], cmat[10], cmat[11]);

    Eigen::Matrix4d mat;
    mat(0,0) = cmat[0]; mat(0,1) = cmat[1]; mat(0,2) = cmat[2];  mat(0,3) = cmat[3];
    mat(1,0) = cmat[4]; mat(1,1) = cmat[5]; mat(1,2) = cmat[6];  mat(1,3) = cmat[7];
    mat(2,0) = cmat[8]; mat(2,1) = cmat[9]; mat(2,2) = cmat[10]; mat(2,3) = cmat[11];
    mat(3,0) = 0.0;     mat(3,1) = 0.0;     mat(3,2) = 0.0;      mat(3,3) = 1.0;

    Eigen::Matrix4d imat = mat.inverse();
    cmat[0] = imat(0, 0); cmat[1] = imat(0, 1); cmat[2]  = imat(0, 2); cmat[3]  = imat(0, 3);
    cmat[4] = imat(1, 0); cmat[5] = imat(1, 1); cmat[6]  = imat(1, 2); cmat[7]  = imat(1, 3);
    cmat[8] = imat(2, 0); cmat[9] = imat(2, 1); cmat[10] = imat(2, 2); cmat[11] = imat(2, 3);

    ROS_INFO("%f %f %f %f", cmat[0], cmat[1], cmat[2], cmat[3]);
    ROS_INFO("%f %f %f %f", cmat[4], cmat[5], cmat[6], cmat[7]);
    ROS_INFO("%f %f %f %f", cmat[8], cmat[9], cmat[10], cmat[11]);
  }
};

int main (int argc, char** argv)
{
  // ROS init
  ros::init (argc, argv, "white_balance_converter");

  WhiteBalanceConverter p;
  ros::spin ();

  return (0);
}


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
//#include"TaskSpace.h"
#include"taskspaceoffline.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
//#include <link.h>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"
#include <gazebo_msgs/LinkStates.h>
#include<termios.h>
#include<QTime>
#include<geometry_msgs/Wrench.h>
#include<sensor_msgs/JointState.h>
#include "qcgenerator.h"
#include<termios.h>

using namespace  std;
using namespace Eigen;
QTime timer;
int count;

double saturate(double a, double min, double max){
    if(a<min){return min;}
    else if(a>max){return max;}
    else{return a;}
}

int getch()
{

        static struct termios oldt, newt;
        tcgetattr( STDIN_FILENO, &oldt);           // save old settings
        newt = oldt;
        newt.c_lflag &= ~(ICANON);                 // disable buffering
        tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings

        int c = getchar();  // read character (non-blocking)

        tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
        return c;

}

int qc_offset[12];
bool qc_initial_bool;

void qc_initial(const sensor_msgs::JointState & msg){
    if (qc_initial_bool){

        for (int i = 0; i < 12; ++i) {
            qc_offset[i]=int(msg.position[i+1]);

        }

        qc_initial_bool=false;

        ROS_INFO("Offset=%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\nInitialized!",
                 qc_offset[0],qc_offset[1],qc_offset[2],qc_offset[3],qc_offset[4],
                qc_offset[5],qc_offset[6],qc_offset[7],qc_offset[8],qc_offset[9],
                qc_offset[10],qc_offset[11]);}
    }

double Fxl,Fyr, Fzl,Fzr,Mxl,Myl,Myr,Mxr;
VectorXd CoPR(2), CoPL(2);
void FT_left_feedback(const geometry_msgs::Wrench &msg){
 Fzl=msg.force.z;
 Mxl=-msg.torque.y;
 Myl=msg.torque.x;
 CoPL << Myl/Fzl,
         Mxl/Fzl;

}

void FT_right_feedback(const geometry_msgs::Wrench &msg){
 Fzr=msg.force.z;
 Mxr=-msg.torque.x;
 Myr=-msg.torque.y;
 CoPR << Myr/Fzr,
         Mxr/Fzr;

}

double IMU_roll,IMU_pitch,IMU_yaw;

void IMU_feedback(const sensor_msgs::Imu &msg){
IMU_roll=msg.orientation.x;
IMU_pitch=msg.orientation.y;
IMU_yaw=msg.orientation.z;
}

int main(int argc, char **argv)
{
double d=M_PI/180/20;//0.05 degree
    qc_initial_bool=true;
    QCgenerator QC;
    for (int i = 0; i < 12; ++i) {
        qc_offset[i]=0;
    }

int count = 0;
double t=0;
double dt=.005;

ros::init(argc, argv, "myNode");
ros::NodeHandle nh;
ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",1000);
ros::Subscriber ft_left = nh.subscribe("/surena/ft_l_state",1000,FT_left_feedback);
ros::Subscriber ft_right = nh.subscribe("/surena/ft_r_state",1000,FT_right_feedback);
ros::Subscriber qcinit = nh.subscribe("/surena/inc_joint_state", 1000, qc_initial);
ros::Subscriber imu = nh.subscribe("/surena/imu_state",1000,IMU_feedback);

ros::Rate loop_rate(200);

std_msgs::Int32MultiArray msg;
std_msgs::MultiArrayDimension msg_dim;

msg_dim.label = "joint_position";
msg_dim.size = 1;
msg.layout.dim.clear();
msg.layout.dim.push_back(msg_dim);

vector<double> q(13);
for (int var = 0; var < 13; ++var) {
    q[var]=0;
}
double qkamar=0;
qDebug()<<"press any key to start!";
getch();
qDebug()<<"started!";

while (ros::ok())
{

    if (qc_initial_bool) {
        ROS_INFO_ONCE("qc is initializing!");
        ros::spinOnce();
        continue;
    }

count++;
int c=0;
//if(count>5) {
//    c= getch();
//    count=0;
//    ROS_INFO("ascii=%d",c);
//}
qDebug()<<"\nright: HY='6,y' HR='5,t' HP='4,r' K='3,e' AP='2,w' AR='1,q'\nleft: HY='h,n' HR='g,b' HP='f,v' K='d,c' AP='s,x' AR='a,z'\nWP='/,'\t'`' for quit!";
double Fz=Fzl+Fzr;
double My=Myl+Myr;

c= getch();
qDebug()<<c;

if(c==96){break;}

if(c==49){q[7-1]+=d;}if(c==113){q[7-1]-=d;}
if(c==50){q[7-2]+=d;}if(c==119){q[7-2]-=d;}
if(c==51){q[7-3]+=d;}if(c==101){q[7-3]-=d;}
if(c==52){q[7-4]+=d;}if(c==114){q[7-4]-=d;}
if(c==53){q[7-5]+=d;}if(c==116){q[7-5]-=d;}
if(c==54){q[7-6]+=d;}if(c==121){q[7-6]-=d;}

if(c==97) {q[19-7]+=d;}   if(c==122){q[19-7]-=d;}
if(c==115){q[19-8]+=d;}   if(c==120){q[19-8]-=d;}
if(c==100){q[19-9]+=d;}    if(c==99){q[19-9]-=d;}
if(c==102){q[19-10]+=d;}  if(c==118){q[19-10]-=d;}
if(c==103){q[19-11]+=d;}   if(c==98){q[19-11]-=d;}
if(c==104){q[19-12]+=d;}  if(c==110){q[19-12]-=d;}

if(c==39){qkamar+=d;}  if(c==47){qkamar-=d;}
if(c==46){q[7-4]+=d;q[19-10]+=d;}  if(c==59){q[7-4]-=d;q[19-10]-=d;}

ROS_INFO("\nright: HY=%f HR=%f HP=%f K=%f AP=%f AR=%f\nleft: HY=%f HR=%f HP=%f K=%f AP=%f AR=%f WP=%f",
         q[1]*180/M_PI,q[2]*180/M_PI,q[3]*180/M_PI,q[4]*180/M_PI,q[5]*180/M_PI,q[6]*180/M_PI,
        q[7]*180/M_PI,q[8]*180/M_PI,q[9]*180/M_PI,q[10]*180/M_PI,q[11]*180/M_PI,q[12]*180/M_PI,qkamar*180/M_PI);
ROS_INFO("CoPL=(%f\t%f),Mxl=%f,Myl=%f,Fzl=%f",CoPL(0),CoPL(1),Mxl,Myl,Fzl);
ROS_INFO("CoPR=(%f\t%f),MxR=%f,MyR=%f,Fzr=%f",CoPR(0),CoPR(1),Mxr,Myr,Fzr);
ROS_INFO("CoPx=%f,My=%f,Fz=%f",My/Fz,My,Fz);
ROS_INFO("roll=%f,pitch=%f,yaw=%f",IMU_roll,IMU_pitch,IMU_yaw);
vector<int> qref(12);
qref=QC.ctrldata2qc(q);

int qrefkamar=int(qkamar*(1/(2*M_PI))*(2304)*50);

 msg.data.clear();
for(int  i = 0;i < 12;i++)
{
    msg.data.push_back(qref[i]+qc_offset[i]);
}
for(int  i = 12;i < 28;i++)
{
    msg.data.push_back(0);
}
msg.data.push_back(qrefkamar);

    chatter_pub.publish(msg);

ros::spinOnce();
loop_rate.sleep();
++count;
t+=dt;
}

return 0;
}


/*
 * Copyright 2018 Autoware Foundation. All rights reserved.
 *
 * 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.
 *
 *
 * v1.0 Yukihiro Saito
 */

#include "multi_object_tracker/tracker/model/vehicle_tracker.hpp"
#include "multi_object_tracker/utils/utils.hpp"
#include "tf2/LinearMath/Matrix3x3.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>

VehicleTracker::VehicleTracker(
  const ros::Time & time, const autoware_perception_msgs::DynamicObject & object)
: Tracker(time, object.semantic.type),
  filtered_yaw_(0.0),
  yaw_filter_gain_(0.7),
  is_fixed_yaw_(object.state.orientation_reliable),
  dim_filter_gain_(0.9),
  is_fixed_dim_(false),
  filtered_posx_(object.state.pose_covariance.pose.position.x),
  filtered_posy_(object.state.pose_covariance.pose.position.y),
  pos_filter_gain_(0.2),
  filtered_vx_(0.0),
  filtered_vy_(0.0),
  v_filter_gain_(0.7),
  area_filter_gain_(0.8),
  last_measurement_posx_(object.state.pose_covariance.pose.position.x),
  last_measurement_posy_(object.state.pose_covariance.pose.position.y),
  last_update_time_(time),
  last_measurement_time_(time)
{
  object_ = object;
  // yaw
  if (object.shape.type == autoware_perception_msgs::Shape::BOUNDING_BOX) {
    double roll, pitch, yaw;
    tf2::Quaternion quaternion;
    tf2::fromMsg(object.state.pose_covariance.pose.orientation, quaternion);
    tf2::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
    yaw = std::atan2(std::sin(yaw), std::cos(yaw));
    filtered_yaw_ = yaw;
  }
  // dim
  if (object.shape.type == autoware_perception_msgs::Shape::BOUNDING_BOX) {
    is_fixed_dim_ = true;
    filtered_dim_x_ = object.shape.dimensions.x;
    filtered_dim_y_ = object.shape.dimensions.y;
  }
  // area
  filtered_area_ = utils::getArea(object.shape);
}

bool VehicleTracker::predict(const ros::Time & time)
{
  double dt = (time - last_update_time_).toSec();
  if (dt < 0.0) dt = 0.0;
  double vel = std::cos(filtered_yaw_) * filtered_vx_ + std::sin(filtered_yaw_) * filtered_vy_;
  if (vel < 0.0 && !is_fixed_yaw_) {
    filtered_posx_ += std::cos(filtered_yaw_ + M_PI) * std::fabs(vel) * dt;
    filtered_posy_ += std::sin(filtered_yaw_ + M_PI) * std::fabs(vel) * dt;
    // filtered_posx_ += filtered_vx_ * dt;
    // filtered_posy_ += filtered_vy_ * dt;
  } else {
    filtered_posx_ += std::cos(filtered_yaw_) * vel * dt;
    filtered_posy_ += std::sin(filtered_yaw_) * vel * dt;
    // filtered_posx_ += filtered_vx_ * dt;
    // filtered_posy_ += filtered_vy_ * dt;
  }
  last_update_time_ = time;
  return true;
}
bool VehicleTracker::measure(
  const autoware_perception_msgs::DynamicObject & object, const ros::Time & time)
{
  int type = object.semantic.type;
  bool is_changed_unknown_object = false;
  if (type == autoware_perception_msgs::Semantic::UNKNOWN) {
    type = getType();
    is_changed_unknown_object = true;
  }
  object_ = object;
  setType(type);

  // yaw
  if (object.shape.type == autoware_perception_msgs::Shape::BOUNDING_BOX) {
    double roll, pitch, yaw;
    tf2::Quaternion quaternion;
    tf2::fromMsg(object.state.pose_covariance.pose.orientation, quaternion);
    tf2::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
    yaw = std::atan2(std::sin(yaw), std::cos(yaw));
    if (!is_fixed_yaw_ && object.state.orientation_reliable) {
      if (
        Eigen::Vector2d(std::cos(filtered_yaw_), std::sin(filtered_yaw_))
          .dot(Eigen::Vector2d(std::cos(yaw), std::sin(yaw))) < 0.0)
        filtered_yaw_ += M_PI;
    } else if (!is_fixed_yaw_ && !object.state.orientation_reliable) {
      if (
        Eigen::Vector2d(std::cos(filtered_yaw_), std::sin(filtered_yaw_))
          .dot(Eigen::Vector2d(std::cos(yaw), std::sin(yaw))) < 0.0)
        yaw += M_PI;
    } else if (is_fixed_yaw_ && object.state.orientation_reliable) {
    } else if (is_fixed_yaw_ && !object.state.orientation_reliable) {
      if (
        Eigen::Vector2d(std::cos(filtered_yaw_), std::sin(filtered_yaw_))
          .dot(Eigen::Vector2d(std::cos(yaw), std::sin(yaw))) < 0.0)
        yaw += M_PI;
    }

    if (object.state.orientation_reliable) is_fixed_yaw_ = object.state.orientation_reliable;

    Eigen::Vector2d filtered_yaw_vector =
      yaw_filter_gain_ * Eigen::Vector2d(std::cos(filtered_yaw_), std::sin(filtered_yaw_)) +
      (1.0 - yaw_filter_gain_) * Eigen::Vector2d(std::cos(yaw), std::sin(yaw));
    filtered_yaw_ = std::atan2(filtered_yaw_vector.y(), filtered_yaw_vector.x());
  }
  // dim
  if (object.shape.type == autoware_perception_msgs::Shape::BOUNDING_BOX) {
    if (!is_fixed_dim_) {
      filtered_dim_x_ = object.shape.dimensions.x;
      filtered_dim_y_ = object.shape.dimensions.y;
      is_fixed_dim_ = true;
    } else {
      filtered_dim_x_ =
        dim_filter_gain_ * filtered_dim_x_ + (1.0 - dim_filter_gain_) * object.shape.dimensions.x;
      filtered_dim_y_ =
        dim_filter_gain_ * filtered_dim_y_ + (1.0 - dim_filter_gain_) * object.shape.dimensions.y;
    }
  }
  // area
  filtered_area_ =
    area_filter_gain_ * filtered_area_ + (1.0 - area_filter_gain_) * utils::getArea(object.shape);

  // vx,vy
  double dt = (time - last_measurement_time_).toSec();
  last_measurement_time_ = time;
  last_update_time_ = time;
  if (0.0 < dt) {
    double current_vel = std::sqrt(
      (object.state.pose_covariance.pose.position.x - last_measurement_posx_) *
        (object.state.pose_covariance.pose.position.x - last_measurement_posx_) +
      (object.state.pose_covariance.pose.position.y - last_measurement_posy_) *
        (object.state.pose_covariance.pose.position.y - last_measurement_posy_));
    const double max_vel = 20.0; /* [m/s]*/
    const double vel_scale =
      current_vel < 0.01 ? 1.0 : std::min(max_vel, current_vel) / current_vel;

    if (is_changed_unknown_object) {
      filtered_vx_ =
        0.95 * filtered_vx_ +
        (1.0 - 0.95) *
          ((object.state.pose_covariance.pose.position.x - last_measurement_posx_) / dt) *
          vel_scale;
      filtered_vy_ =
        0.95 * filtered_vy_ +
        (1.0 - 0.95) *
          ((object.state.pose_covariance.pose.position.y - last_measurement_posy_) / dt) *
          vel_scale;
    } else {
      filtered_vx_ =
        v_filter_gain_ * filtered_vx_ +
        (1.0 - v_filter_gain_) *
          ((object.state.pose_covariance.pose.position.x - last_measurement_posx_) / dt) *
          vel_scale;
      filtered_vy_ =
        v_filter_gain_ * filtered_vy_ +
        (1.0 - v_filter_gain_) *
          ((object.state.pose_covariance.pose.position.y - last_measurement_posy_) / dt) *
          vel_scale;
      v_filter_gain_ = std::min(0.9, v_filter_gain_ + 0.15);
    }

    double vel = std::cos(filtered_yaw_) * filtered_vx_ + std::sin(filtered_yaw_) * filtered_vy_;
    filtered_vx_ = std::cos(filtered_yaw_) * vel;
    filtered_vy_ = std::sin(filtered_yaw_) * vel;
  }

  // pos x, pos y
  // filtered_posx_ = object.state.pose.pose.position.x;
  // filtered_posy_ = object.state.pose.pose.position.y;
  last_measurement_posx_ = object.state.pose_covariance.pose.position.x;
  last_measurement_posy_ = object.state.pose_covariance.pose.position.y;
  filtered_posx_ = pos_filter_gain_ * filtered_posx_ +
                   (1.0 - pos_filter_gain_) * object.state.pose_covariance.pose.position.x;
  filtered_posy_ = pos_filter_gain_ * filtered_posy_ +
                   (1.0 - pos_filter_gain_) * object.state.pose_covariance.pose.position.y;
  pos_filter_gain_ = std::min(0.8, pos_filter_gain_ + 0.05);

  return true;
}

bool VehicleTracker::getEstimatedDynamicObject(
  const ros::Time & time, autoware_perception_msgs::DynamicObject & object)
{
  object = object_;
  object.id = unique_id::toMsg(getUUID());
  object.semantic.type = getType();

  if (object.shape.type == autoware_perception_msgs::Shape::BOUNDING_BOX) {
    double roll, pitch, yaw;
    tf2::Quaternion quaternion;
    tf2::fromMsg(object.state.pose_covariance.pose.orientation, quaternion);
    tf2::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
    tf2::Quaternion filtered_quaternion;
    filtered_quaternion.setRPY(roll, pitch, filtered_yaw_);
    object.state.pose_covariance.pose.orientation = tf2::toMsg(filtered_quaternion);
    object.state.orientation_reliable = is_fixed_yaw_;
  }
  if (is_fixed_dim_ && object.shape.type == autoware_perception_msgs::Shape::BOUNDING_BOX) {
    object.shape.dimensions.x = filtered_dim_x_;
    object.shape.dimensions.y = filtered_dim_y_;
  }

  object.state.pose_covariance.pose.position.x = filtered_posx_;
  object.state.pose_covariance.pose.position.y = filtered_posy_;
  double dt = (time - last_update_time_).toSec();
  if (dt < 0.0) dt = 0.0;
  double vel = std::cos(filtered_yaw_) * filtered_vx_ + std::sin(filtered_yaw_) * filtered_vy_;
  if (vel < 0.0 && !is_fixed_yaw_) {
    object.state.pose_covariance.pose.position.x +=
      std::cos(filtered_yaw_ + M_PI) * std::fabs(vel) * dt;
    object.state.pose_covariance.pose.position.y +=
      std::sin(filtered_yaw_ + M_PI) * std::fabs(vel) * dt;
  } else {
    object.state.pose_covariance.pose.position.x +=
      std::cos(filtered_yaw_) * std::max(vel, 0.0) * dt;
    object.state.pose_covariance.pose.position.y +=
      std::sin(filtered_yaw_) * std::max(vel, 0.0) * dt;
  }

  double roll, pitch, yaw;
  tf2::Quaternion quaternion;
  tf2::fromMsg(object.state.pose_covariance.pose.orientation, quaternion);
  tf2::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
  if (object.shape.type == autoware_perception_msgs::Shape::BOUNDING_BOX) {
    object.state.twist_covariance.twist.linear.x =
      filtered_vx_ * std::cos(-yaw) - filtered_vy_ * std::sin(-yaw);
  } else {
    object.state.twist_covariance.twist.linear.x =
      filtered_vx_ * std::cos(-yaw) - filtered_vy_ * std::sin(-yaw);
    object.state.twist_covariance.twist.linear.y =
      filtered_vx_ * std::sin(-yaw) + filtered_vy_ * std::cos(-yaw);
  }

  object.state.twist_reliable = true;

  return true;
}

// geometry_msgs::Point VehicleTracker::getPosition(const ros::Time &time)
// {
//     geometry_msgs::Point position;
//     position.x = filtered_posx_;
//     position.y = filtered_posy_;
//     position.z = object_.state.pose_covariance.pose.position.z;
//     return position;
// }

// double VehicleTracker::getArea()
// {
//     return filtered_area_;
// }


#include "gtlcore.hpp"
#include <boost/cstdint.hpp>

namespace GameTextureLoader3
{
	// DXT decoding functionality
	int getChannelCount(GameTextureLoader3::ImgFormat format)
	{
		int rtn = 0;
		switch(format)
		{
		case GameTextureLoader3::FORMAT_DXT1:
			rtn = 3;
			break;
		case GameTextureLoader3::FORMAT_DXT3:
		case GameTextureLoader3::FORMAT_DXT5:
			rtn = 4;
			break;
		case GameTextureLoader3::FORMAT_3DC:
			rtn = 1;
			break;
		default:
			break;
		}
		return rtn;
	}

	void decodeColorBlock(unsigned char *dest, const int w, const int h, const int xOff, const int yOff, const GameTextureLoader3::ImgFormat format, unsigned char *src)
	{
		unsigned char colors[4][3];

		unsigned short c0 = *(unsigned short *) src;
		unsigned short c1 = *(unsigned short *) (src + 2);

		// Extract the two stored colors
		colors[0][0] = ((c0 >> 11) & 0x1F) << 3;
		colors[0][1] = ((c0 >>  5) & 0x3F) << 2;
		colors[0][2] =  (c0        & 0x1F) << 3;

		colors[1][0] = ((c1 >> 11) & 0x1F) << 3;
		colors[1][1] = ((c1 >>  5) & 0x3F) << 2;
		colors[1][2] =  (c1        & 0x1F) << 3;

		// Compute the other two colors
		if (c0 > c1 || format == GameTextureLoader3::FORMAT_DXT5){
			for (int i = 0; i < 3; i++){
				colors[2][i] = (2 * colors[0][i] +     colors[1][i] + 1) / 3;
				colors[3][i] = (    colors[0][i] + 2 * colors[1][i] + 1) / 3;
			}
		} else {
			for (int i = 0; i < 3; i++){
				colors[2][i] = (colors[0][i] + colors[1][i] + 1) >> 1;
				colors[3][i] = 0;
			}
		}

		src += 4;
		for (int y = 0; y < h; y++){
			unsigned char *dst = dest + yOff * y;
			unsigned int indexes = src[y];
			for (int x = 0; x < w; x++){
				unsigned int index = indexes & 0x3;
				dst[0] = colors[index][0];
				dst[1] = colors[index][1];
				dst[2] = colors[index][2];
				indexes >>= 2;

				dst += xOff;
			}
		}
	}

	void decodeDXT3AlphaBlock(unsigned char *dest, const int w, const int h, const int xOff, const int yOff, unsigned char *src)
	{
		for (int y = 0; y < h; y++){
			unsigned char *dst = dest + yOff * y;
			unsigned int alpha = ((unsigned short *) src)[y];
			for (int x = 0; x < w; x++){
				*dst = (alpha & 0xF) * 17;
				alpha >>= 4;
				dst += xOff;
			}
		}
	}
	void decodeDXT5AlphaBlock(unsigned char *dest, const int w, const int h, const int xOff, const int yOff, unsigned char *src)
	{
		unsigned char a0 = src[0];
		unsigned char a1 = src[1];
		boost::uint64_t alpha = (*(boost::uint64_t *) src) >> 16;

		for (int y = 0; y < h; y++){
			unsigned char *dst = dest + yOff * y;
			for (int x = 0; x < w; x++){
				int k = ((unsigned int) alpha) & 0x7;
				if (k == 0){
					*dst = a0;
				} else if (k == 1){
					*dst = a1;
				} else if (a0 > a1){
					*dst = ((8 - k) * a0 + (k - 1) * a1) / 7;
				} else if (k >= 6){
					*dst = (k == 6)? 0 : 255;
				} else {
					*dst = ((6 - k) * a0 + (k - 1) * a1) / 5;
				}
				alpha >>= 3;

				dst += xOff;
			}
			if (w < 4) alpha >>= (3 * (4 - w));
		}
	}
	void decodeCompressedImage(unsigned char *dest, unsigned char *src, const int width, const int height, const ImgFormat format)
	{
		int sx = (width  < 4)? width  : 4;
		int sy = (height < 4)? height : 4;

		int nChannels = getChannelCount(format);
		for (int y = 0; y < height; y += 4)
		{
			for (int x = 0; x < width; x += 4)
			{
				unsigned char *dst = dest + (y * width + x) * nChannels;
				if (format == GameTextureLoader3::FORMAT_DXT3)
				{
					decodeDXT3AlphaBlock(dst + 3, sx, sy, 4, width * 4, src);
					src += 8;
				} else if (format == GameTextureLoader3::FORMAT_DXT5)
				{
					decodeDXT5AlphaBlock(dst + 3, sx, sy, 4, width * 4, src);
					src += 8;
				}
				if (format <= GameTextureLoader3::FORMAT_DXT5)
				{
					decodeColorBlock(dst, sx, sy, nChannels, width * nChannels, format, src);
					src += 8;
				} else {
					if (format == /*FORMAT_ATI1N*/ GameTextureLoader3::FORMAT_3DC)
					{
						decodeDXT5AlphaBlock(dst, sx, sy, 1, width, src);
						src += 8;
					} else {
						decodeDXT5AlphaBlock(dst,     sx, sy, 2, width * 2, src + 8);
						decodeDXT5AlphaBlock(dst + 1, sx, sy, 2, width * 2, src);
						src += 16;
					}
				}
			}
		}
	}

	GameTextureLoader3::ImagePtr Decompress(GameTextureLoader3::ImagePtr img)
	{
		// Not a compressed type we know about, so just return a ptr to the original image
		GameTextureLoader3::ImgFormat srcFormat = img->getFormat();
		if (srcFormat > GameTextureLoader3::FORMAT_3DC || srcFormat < FORMAT_DXT1)
			return img;

		GameTextureLoader3::ImgFormat destFormat = (srcFormat == FORMAT_DXT1)? FORMAT_RGB : FORMAT_RGBA;
		int destbpp = (destFormat == FORMAT_RGBA) ? 4 : 3;	// bytes per pixel

		const int numImages = img->getNumImages();
		const int numMipMaps = img->getNumMipMaps();

		GameTextureLoader3::ImagePtr dest(new GTLCore::ImageImpl(*dynamic_cast<GTLCore::ImageImpl*>(img.get())));

		size_t totalspace = 0;
		for (int i = 0; i < numMipMaps; ++i)
		{
			int width = img->getWidth(i);
			int height = img->getHeight(i);
			int depth = img->getDepth(i);
			int size = width * height * (destbpp);
			size *= (depth != 0) ? depth : 1;
			totalspace += size;
		}

		totalspace *= numImages;

		GTLCore::ImageImpl * realdest = dynamic_cast<GTLCore::ImageImpl*>(dest.get());
		realdest->format_ = destFormat;
		realdest->colourdepth_ = destbpp * 8;

		realdest->imgdata_.reset(new unsigned char[totalspace]);

		for (int image = 0; image < numImages; ++image)
		{
			for(int mipmap = 0; mipmap < numMipMaps; ++mipmap)
			{
				const int width = img->getWidth(mipmap);
				const int height = img->getHeight(mipmap);
				int depth = img->getDepth(mipmap);
				depth = (depth != 0) ? depth : 1;

				unsigned char * src = img->getDataPtr(mipmap,image);
				unsigned char * dst = dest->getDataPtr(mipmap,image);

				int dstSliceSize = width * height * destbpp;
				int srcSliceSize = img->getSize(mipmap);	// get how far we have to skip for this mipmap level

				for (int slice = 0; slice < depth; slice++)
				{
					decodeCompressedImage(dst, src, width, height, srcFormat);
					// need to push forward a slice size
					dst += dstSliceSize;
					src += srcSliceSize;
				}
			}
		}

		return dest;
	}
}


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
// HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <boost/noncopyable.hpp>
#include <comma/base/exception.h>
#include <snark/math/gaussian_process/gaussian_process.h>

namespace snark{

gaussian_process::gaussian_process( const Eigen::MatrixXd& domains
                                , const Eigen::VectorXd& targets
                                , const gaussian_process::covariance& covariance
                                , double self_covariance )
    : domains_( domains )
    , targets_( targets )
    , covariance_( covariance )
    , self_covariance_( self_covariance )
    , offset_( targets.sum() / targets.rows() )
    , K_( domains.rows(), domains.rows() )
{
    if( domains.rows() != targets.rows() ) { COMMA_THROW( comma::exception, "expected " << domains.rows() << " row(s) in targets, got " << targets.rows() << " row(s)" ); }
    targets_.array() -= offset_; // normalise
    //use m_K as Kxx + variance*I, then invert it
    //fill Kxx with values from covariance function
    //for elements r,c in upper triangle
    for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r )
    {
        K_( r, r ) = self_covariance_;
        const Eigen::VectorXd& row = domains.row( r );
        for( std::size_t c = r + 1; c < std::size_t( domains.rows() ); ++c )
        {
            K_( c, r ) = K_( r, c ) = covariance_( row, domains.row( c ) );
        }
    }
    L_.compute( K_ ); // invert Kxx + variance * I to become (by definition) B
    alpha_ = L_.solve( targets_ );
}

std::pair< double, double > gaussian_process::evaluate( const Eigen::MatrixXd& domain ) const
{
    if( domain.rows() != 1 ) { COMMA_THROW( comma::exception, "expected 1 row in domain, got " << domain.rows() << " rows" ); }
    Eigen::VectorXd means( 1 );
    Eigen::VectorXd variances( 1 );
    evaluate( domain, means, variances );
    return std::make_pair( means( 0 ), variances( 0 ) );
}

void gaussian_process::evaluate( const Eigen::MatrixXd& domains, Eigen::VectorXd& means, Eigen::VectorXd& variances ) const
{
    if( domains.cols() != domains_.cols() ) { COMMA_THROW( comma::exception, "expected " << domains_.cols() << " column(s) in domains, got " << domains.cols() << std::endl ); }
    Eigen::MatrixXd Kxsx = Eigen::MatrixXd::Zero( domains.rows(), domains_.rows() );
    for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r )
    {
        const Eigen::VectorXd& row = domains.row( r );
        for( std::size_t c = 0; c < std::size_t( domains_.rows() ); ++c )
        {
            Kxsx( r, c ) = covariance_( row, domains_.row( c ) );
        }
    }
    means = Kxsx * alpha_;
    means.array() += offset_;
    Eigen::MatrixXd Kxxs = Kxsx.transpose();
    L_.matrixL().solveInPlace( Kxxs );
    Eigen::MatrixXd& variance = Kxxs;
    variance = variance.array() * variance.array();
    variances = variance.colwise().sum();
    // for each diagonal variance, set v(r) = -v(r,r) + Kxsxs
    for( std::size_t r = 0; r < std::size_t( domains.rows() ); ++r )
    {
        variances( r ) = -variances( r ) + self_covariance_;
    }
}

}  // namespace snark{


#include <mesh_array.h>
#include "mesh_array_data_path.h"

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(MESH_ARRAY);

BOOST_AUTO_TEST_CASE(tetgen_test)
{
  mesh_array::T3Mesh surface;
  mesh_array::t3_vertex_float_attribute inX;
  mesh_array::t3_vertex_float_attribute inY;
  mesh_array::t3_vertex_float_attribute inZ;

  mesh_array::read_obj(mesh_array::data_path + "objs/box.obj",surface, inX, inY, inZ);

  mesh_array::T4Mesh volume;
  mesh_array::t4_vertex_float_attribute outX;
  mesh_array::t4_vertex_float_attribute outY;
  mesh_array::t4_vertex_float_attribute outZ;

  mesh_array::tetgen( surface, inX, inY, inZ, volume, outX, outY, outZ);

  bool const is_pos = is_positive_orientation( volume, outX, outY, outZ );

  BOOST_CHECK(is_pos);

  mesh_array::write_matlab("test.m", volume, outX, outY, outZ);
}

BOOST_AUTO_TEST_SUITE_END();


/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2020,
 *  Max Planck Institute for Intelligent Systems (MPI-IS).
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the MPI-IS nor the names
 *     of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written
 *     permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Andreas Orthey, Sohaib Akbar */

#include <ompl/multilevel/planners/qrrt/QRRTStarImpl.h>
#include <ompl/multilevel/datastructures/PlannerDataVertexAnnotated.h>
#include <ompl/tools/config/SelfConfig.h>
#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
#include <boost/foreach.hpp>
#include <boost/math/constants/constants.hpp>
#include "ompl/util/GeometricEquations.h"
#include <memory>

#define foreach BOOST_FOREACH

ompl::multilevel::QRRTStarImpl::QRRTStarImpl(const base::SpaceInformationPtr &si, BundleSpace *parent_)
  : BaseT(si, parent_)
{
    setName("QRRTStarImpl" + std::to_string(id_));
    Planner::declareParam<double>("useKNearest_", this, &ompl::multilevel::QRRTStarImpl::setKNearest,
                                  &ompl::multilevel::QRRTStarImpl::getKNearest, "0,1");

    symmetric_ = getBundle()->getStateSpace()->hasSymmetricInterpolate();

    setImportance("exponential");
    // setGraphSampler("randomedge");
    setGraphSampler("randomvertex");
    setMetric("geodesic");
}

ompl::multilevel::QRRTStarImpl::~QRRTStarImpl()
{
}

void ompl::multilevel::QRRTStarImpl::setup()
{
    BaseT::setup();
    calculateRewiringLowerBounds();
}
void ompl::multilevel::QRRTStarImpl::clear()
{
    BaseT::clear();
    goalConfigurations_.clear();
}

void ompl::multilevel::QRRTStarImpl::grow()
{
    if (firstRun_)
    {
        init();
        firstRun_ = false;

        findSection();
    }

    //(1) Get Random Sample
    sampleBundleGoalBias(xRandom_->state);

    //(2) Get Nearest in Tree
    Configuration *q_nearest = nearestDatastructure_->nearest(xRandom_);

    //(3) Steer toward random
    Configuration *q_new = steerTowards_Range(q_nearest, xRandom_);

    if (q_new)
    {
        // (1) Find all neighbors of the new configuration in graph
        std::vector<Configuration *> nearestNbh;
        getNearestNeighbors(q_new, nearestNbh);

        // (2) Find neighbor with minimum Cost
        q_new->lineCost = getOptimizationObjectivePtr()->motionCost(q_nearest->state, q_new->state);
        q_new->cost = getOptimizationObjectivePtr()->combineCosts(q_nearest->cost, q_new->lineCost);
        q_new->parent = q_nearest;

        // (3) Rewire Tree
        base::Cost cost_nearest = q_new->cost;

        std::vector<int> validNeighbor(nearestNbh.size(), 0);

        // store the connection cost for later use, if space is symmetric
        std::vector<ompl::base::Cost> lineCosts;

        if (symmetric_)
        {
            lineCosts.resize(nearestNbh.size());
        }

        for (unsigned int i = 0; i < nearestNbh.size(); i++)
        {
            Configuration *q_near = nearestNbh.at(i);

            if (q_near == q_nearest)
            {
                validNeighbor.at(i) = 1;
                if (symmetric_)
                {
                    lineCosts[i] = cost_nearest;
                }
                continue;
            }
            validNeighbor.at(i) = 0;

            ompl::base::Cost line_cost = getOptimizationObjectivePtr()->motionCost(q_near->state, q_new->state);
            ompl::base::Cost new_cost = getOptimizationObjectivePtr()->combineCosts(q_near->cost, line_cost);

            if (symmetric_)
            {
                lineCosts[i] = line_cost;
            }

            if (getOptimizationObjectivePtr()->isCostBetterThan(new_cost, q_new->cost))
            {
                if ((!useKNearest_ || distance(q_near, q_new) < maxDistance_) &&
                    getBundle()->checkMotion(q_near->state, q_new->state))
                {
                    q_new->lineCost = line_cost;
                    q_new->cost = new_cost;
                    q_new->parent = q_near;
                    validNeighbor.at(i) = 1;
                }
                else
                    validNeighbor.at(i) = -1;
            }
        }
        //(4) Connect to minimum cost neighbor
        addConfiguration(q_new);
        q_new->parent->children.push_back(q_new);

        bool checkForSolution = false;

        // (5) Rewire the tree (if from q_new to q_near is lower cost)
        for (unsigned int i = 0; i < nearestNbh.size(); i++)
        {
            Configuration *q_near = nearestNbh.at(i);

            if (q_near != q_new->parent)
            {
                // (7a) compute cost q_new to q_near
                base::Cost line_cost;
                if (symmetric_)
                {
                    line_cost = lineCosts[i];
                }
                else
                {
                    line_cost = getOptimizationObjectivePtr()->motionCost(q_new->state, q_near->state);
                }
                base::Cost new_cost = getOptimizationObjectivePtr()->combineCosts(q_new->cost, line_cost);

                // (7b) check if new cost better than q_near->cost (over old
                // pathway)
                if (getOptimizationObjectivePtr()->isCostBetterThan(new_cost, q_near->cost))
                {
                    bool valid = (validNeighbor.at(i) == 1);
                    // check neighbor validity if it wasn´t checked before
                    if (validNeighbor.at(i) == 0)
                    {
                        valid = ((!useKNearest_ || distance(q_near, q_new) < maxDistance_) &&
                                 getBundle()->checkMotion(q_new->state, q_near->state));
                    }

                    // (7c) q_new to q_near is better way to reach q_near. Remove
                    // previous connection of q_near to old parent and set it to
                    // q_new
                    // pathway)
                    if (valid)
                    {
                        // (7d) remove q_near from children of its old parent node
                        removeFromParent(q_near);

                        // (7g) update costs of q_near
                        q_near->lineCost = line_cost;
                        q_near->cost = new_cost;
                        q_near->parent = q_new;
                        q_near->parent->children.push_back(q_near);

                        // (7h) update node's children costs
                        updateChildCosts(q_near);
                        checkForSolution = true;
                    }
                }
            }
        }

        // (8) check if this sample satisfies the goal
        double dist = 0.0;
        bool satisfied = pdef_->getGoal()->isSatisfied(q_new->state, &dist);
        if (satisfied)
        {
            goalConfigurations_.push_back(q_new);
            checkForSolution = true;
        }

        if (checkForSolution)
        {
            bool updatedSolution = false;
            if (!goalConfigurations_.empty() && qGoal_ == nullptr)
            {
                qGoal_ = q_new;
                bestCost_ = qGoal_->cost;
                updatedSolution = true;
            }
            else
            {
                for (unsigned int k = 0; k < goalConfigurations_.size(); k++)
                {
                    Configuration *qk = goalConfigurations_.at(k);

                    if (getOptimizationObjectivePtr()->isCostBetterThan(qk->cost, bestCost_))
                    {
                        qGoal_ = qk;
                        bestCost_ = qGoal_->cost;
                        updatedSolution = true;
                    }
                }
            }
            if (updatedSolution)
            {
                OMPL_INFORM("Found path with cost %f (level %d).", qGoal_->cost, getLevel());
                hasSolution_ = true;
            }
        }
    }
}

void ompl::multilevel::QRRTStarImpl::updateChildCosts(Configuration *q)
{
    for (std::size_t i = 0; i < q->children.size(); ++i)
    {
        q->children.at(i)->cost = getOptimizationObjectivePtr()->combineCosts(q->cost, q->children.at(i)->lineCost);
        updateChildCosts(q->children.at(i));
    }
}

void ompl::multilevel::QRRTStarImpl::removeFromParent(Configuration *q)
{
    for (auto it = q->parent->children.begin(); it != q->parent->children.end(); ++it)
    {
        if (*it == q)
        {
            q->parent->children.erase(it);
            break;
        }
    }
}

bool ompl::multilevel::QRRTStarImpl::getSolution(base::PathPtr &solution)
{
    if (hasSolution_)
    {
        solutionPath_ = std::make_shared<geometric::PathGeometric>(getBundle());

        Configuration *intermediate_node = qGoal_;
        while (intermediate_node != nullptr)
        {
            std::static_pointer_cast<geometric::PathGeometric>(solutionPath_)->append(intermediate_node->state);
            intermediate_node = intermediate_node->parent;
        }
        std::static_pointer_cast<geometric::PathGeometric>(solutionPath_)->reverse();
        solution = solutionPath_;
        return true;
    }
    else
    {
        return false;
    }
}

void ompl::multilevel::QRRTStarImpl::getNearestNeighbors(Configuration *x, std::vector<Configuration *> &nearest)
{
    auto cardDbl = static_cast<double>(nearestDatastructure_->size() + 1u);

    if (useKNearest_)
    {
        unsigned int k = std::ceil(k_rrt_Constant_ * log(cardDbl));
        nearestDatastructure_->nearestK(x, k, nearest);
    }
    else
    {
        double r = std::min(maxDistance_, r_rrt_Constant_ * std::pow(log(cardDbl) / cardDbl, 1 / d_));
        nearestDatastructure_->nearestR(x, r, nearest);
    }
}

void ompl::multilevel::QRRTStarImpl::calculateRewiringLowerBounds()
{
    d_ = (double)getBundle()->getStateDimension();
    double e = boost::math::constants::e<double>();
    // k > 2^(d + 1) * e * (1 + 1 / d).
    k_rrt_Constant_ = std::pow(2, d_ + 1) * e * (1.0 + 1.0 / d_);
    // γRRG > γRRG ∗ = 2*( 1 + 1/d)^1/d * ( μ( Xfree) / ζd)^1/d
    r_rrt_Constant_ =
        std::pow(2 * (1.0 + 1.0 / d_) * (getBundle()->getSpaceMeasure() / unitNBallMeasure(d_)), 1.0 / d_);
}

void ompl::multilevel::QRRTStarImpl::getPlannerData(base::PlannerData &data) const
{
    multilevel::PlannerDataVertexAnnotated pstart(qStart_->state);
    pstart.setLevel(getLevel());
    data.addStartVertex(pstart);

    if (hasSolution_)
    {
        multilevel::PlannerDataVertexAnnotated pgoal(qGoal_->state);
        pgoal.setLevel(getLevel());
        data.addGoalVertex(pgoal);
    }

    std::vector<Configuration *> motions;
    if (nearestDatastructure_)
        nearestDatastructure_->list(motions);

    foreach (const Configuration *q, motions)
    {
        if (q->parent != nullptr)
        {
            multilevel::PlannerDataVertexAnnotated p1(q->parent->state);
            multilevel::PlannerDataVertexAnnotated p2(q->state);
            p1.setLevel(getLevel());
            p2.setLevel(getLevel());
            data.addEdge(p1, p2);
        }
    }

    OMPL_DEBUG("Tree (level %d) has %d/%d vertices/edges", getLevel(), motions.size(), motions.size() - 1);
}


#include <pybind11/pybind11.h>

#include <boost/lexical_cast.hpp>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Delaunay_mesh_face_base_2.h>
#include <CGAL/Delaunay_mesh_vertex_base_2.h>
#include <CGAL/Delaunay_mesher_2.h>
#include <CGAL/Delaunay_mesh_size_criteria_2.h>
#include <CGAL/Triangulation_conformer_2.h>
#include <CGAL/lloyd_optimize_mesh_2.h>

#include <type_traits>
#include <typeinfo>
#ifndef _MSC_VER
#   include <cxxabi.h>
#endif
#include <memory>
#include <string>
#include <cstdlib>

#include <functional>

template <typename...> struct hash;

template<typename T>
struct hash<T>
        : public std::hash<T>
{
    using std::hash<T>::hash;
};

template <typename T, typename... Rest>
struct hash<T, Rest...>
{
    inline std::size_t operator()(const T& v, const Rest&... rest) {
        std::size_t seed = hash<Rest...>{}(rest...);
        seed ^= hash<T>{}(v) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
        return seed;
    }
};

template <class T>
std::string
type_name()
{
    typedef typename std::remove_reference<T>::type TR;
    std::unique_ptr<char, void(*)(void*)> own
            (
            nullptr,
            std::free
    );
    std::string r = own != nullptr ? own.get() : typeid(TR).name();
    if (std::is_const<TR>::value)
        r += " const";
    if (std::is_volatile<TR>::value)
        r += " volatile";
    if (std::is_lvalue_reference<T>::value)
        r += "&";
    else if (std::is_rvalue_reference<T>::value)
        r += "&&";
    return r;
}

namespace py = pybind11;

using K = CGAL::Exact_predicates_inexact_constructions_kernel;
using Vb = CGAL::Delaunay_mesh_vertex_base_2<K>;
using Fb = CGAL::Delaunay_mesh_face_base_2<K>;
using Tds = CGAL::Triangulation_data_structure_2<Vb, Fb>;
using CDT = CGAL::Constrained_Delaunay_triangulation_2<K, Tds>;
using Point = CDT::Point;
using Vertex_handle = CDT::Vertex_handle;
using Criteria = CGAL::Delaunay_mesh_size_criteria_2<CDT>;
using Mesher = CGAL::Delaunay_mesher_2<CDT, Criteria>;

template <typename T>
class TypedInputIterator
{
public:
    using iterator_category = std::input_iterator_tag;
    using difference_type = std::ptrdiff_t;
    using value_type = T;
    using pointer = T*;
    using reference = T&;

    explicit TypedInputIterator(py::iterator& py_iter) :
            py_iter_(py_iter)
    {
    }

    explicit TypedInputIterator(py::iterator&& py_iter) :
            py_iter_(py_iter)
    {
    }

    value_type operator*()
    {
        return (*py_iter_).template cast<value_type>();
    }

    TypedInputIterator operator++(int)
    {
        auto copy = *this;
        ++py_iter_;
        return copy;
    }

    TypedInputIterator& operator++()
    {
        ++py_iter_;
        return *this;
    }

    bool operator!=(TypedInputIterator &rhs)
    {
        return py_iter_ != rhs.py_iter_;
    }

    bool operator==(TypedInputIterator &rhs)
    {
        return py_iter_ == rhs.py_iter_;
    }

private:
    py::iterator py_iter_;
};

PYBIND11_MODULE(daidalus, m)
{

    py::class_<Point>(m, "Point")
        .def(py::init<int, int>(),  py::arg("x"), py::arg("y"))
        .def(py::init<double, double>(), py::arg("x"), py::arg("y"))
        .def_property_readonly("x", &Point::x)
        .def_property_readonly("y", &Point::y)
        .def("__repr__",
             [](const Point &p) {
                 std::string r("Point(");
                 r += boost::lexical_cast<std::string>(p.x());
                 r += ", ";
                 r += boost::lexical_cast<std::string>(p.y());
                 r += ")";
                 return r;
             })
        .def("__hash__",
             [](const Point &p) {
                 std::hash<double> double_hash;
                 auto x_hash = double_hash(p.x());
                 auto y_hash = double_hash(p.y());
                 return y_hash ^ x_hash + 0x9e3779b9 + (y_hash << 6) + (y_hash >> 2);
             }
        )
        .def("__eq__",
            [](const Point &p, const Point & q) {
                return p == q;
            }
        )
        ;

    py::class_<Vertex_handle>(m, "VertexHandle")
        .def_property_readonly(
            "point",
            [](const Vertex_handle& vertex_handle)
            {
                return vertex_handle->point();
            })
            ;

    py::class_<CDT::Finite_vertices_iterator::value_type>(m, "Vertex")
            .def_property_readonly(
                "point", [](CDT::Finite_vertices_iterator::value_type& vertex)
                {
                    return vertex.point();
                }
            )
            ;

    py::class_<CDT::Finite_faces_iterator::value_type>(m, "Face")
            .def("vertex_handle",
                [](CDT::Finite_faces_iterator::value_type& face, int index)
                {
                    return face.vertex(index);
                },
                py::arg("index")
            )
            ;

    m.def("print_faces_iterator_value_type", [](){
        std::cout << type_name<CDT::Finite_faces_iterator::value_type>();
    });

    py::class_<CDT>(m, "ConstrainedDelaunayTriangulation")
        .def(py::init())
        .def("insert", [](CDT & cdt, const Point & p) { return cdt.insert(p); })
        .def("insert_constraint",
             [](CDT & cdt, Vertex_handle a, Vertex_handle b)
             {
                 cdt.insert_constraint(a, b);
             })
        .def("number_of_vertices", &CDT::number_of_vertices)
        .def("number_of_faces", &CDT::number_of_faces)
        .def("finite_vertices", [](CDT & cdt) -> py::iterator
        {
            return py::make_iterator(cdt.finite_vertices_begin(), cdt.finite_vertices_end());
        })
        .def("finite_faces", [](CDT & cdt) -> py::iterator
        {
            return py::make_iterator(cdt.finite_faces_begin(), cdt.finite_faces_end());
        })
        ;

    py::class_<Criteria>(m, "Criteria")
            .def(py::init<double, double>(),
                 py::arg("aspect_bound") = 0.125,
                 py::arg("size_bound") = 0.0)
            .def_property("size_bound", &Criteria::size_bound, &Criteria::set_size_bound)
            .def_property("aspect_bound",
                          [](const Criteria & c) { c.bound(); },
                          [](Criteria & c, double bound) { c.set_bound(bound); })
            ;

    py::class_<Mesher>(m, "Mesher")
        .def(py::init<CDT&>())
        .def("seeds_from", [](Mesher & mesher, py::iterable iterable)
        {
            py::iterator iterator = py::iter(iterable);
            TypedInputIterator<Point> points_begin(iterator);
            TypedInputIterator<Point> points_end(py::iterator::sentinel());
            mesher.set_seeds(points_begin, points_end);
        })
        .def("refine_mesh", &Mesher::refine_mesh)
        .def_property(
                "criteria",
                &Mesher::get_criteria,
                [](Mesher& mesher, const Criteria & criteria)
                {
                    mesher.set_criteria(criteria);
                }
        )
        ;

    m.def("make_conforming_delaunay",
          &CGAL::make_conforming_Delaunay_2<CDT>,
          py::arg("cdt")
    );

    m.def("make_conforming_gabriel",
          &CGAL::make_conforming_Gabriel_2<CDT>,
          py::arg("cdt")
    );

    m.def("lloyd_optimize", [](
                  CDT& cdt,
                  int max_iteration_number,
                  double time_limit,
                  double convergence,
                  double freeze_bound)
          {
              CGAL::lloyd_optimize_mesh_2(cdt,
                                          CGAL::parameters::max_iteration_number = max_iteration_number,
                                          CGAL::parameters::time_limit = time_limit,
                                          CGAL::parameters::convergence = convergence,
                                          CGAL::parameters::freeze_bound = freeze_bound
              );
          },
          py::arg("cdt"),
          py::arg("max_iteration_number") = 0,
          py::arg("time_limit") = 0.0,
          py::arg("convergence") = 0.001,
          py::arg("freeze_bound") = 0.001
    );

}


#include <Eigen/Eigen>
#include <ndt_registration/ndt_matcher_d2d_feature.h>
#include <boost/bind.hpp>
#include <sys/time.h>

namespace perception_oru
{

bool
NDTMatcherFeatureD2D::covariance( NDTMap& targetNDT,
        NDTMap& sourceNDT,
        Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T,
        Eigen::Matrix<double,6,6> &cov)
{
    assert(false);
};

//DEPRECATED???
double
NDTMatcherFeatureD2D::scoreNDT(std::vector<NDTCell*> &sourceNDT, NDTMap &targetNDT,
        Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T)
{
    NUMBER_OF_ACTIVE_CELLS = 0;
    double score_here = 0;
    double det = 0;
    bool exists = false;
    NDTCell *cell;
    Eigen::Matrix3d covCombined, icov;
    Eigen::Vector3d meanFixed;
    Eigen::Vector3d meanMoving;
    Eigen::Matrix3d R = T.rotation();
    std::vector<std::pair<unsigned int, double> > scores;
    for(unsigned int j=0; j<_corr.size(); j++)
    {
        unsigned int i = _corr[j].second;
        if (_corr[j].second >= (int)sourceNDT.size())
        {
            std::cout << "second correspondance : " << _corr[j].second << ", " << sourceNDT.size() << std::endl;
        }
        if (sourceNDT[i] == NULL)
        {
            std::cout << "sourceNDT[i] == NULL!" << std::endl;
        }
        meanMoving = T*sourceNDT[i]->getMean();

        cell = targetNDT.getCellIdx(_corr[j].first);
        {

            if(cell == NULL)
            {
                std::cout << "cell== NULL!!!" << std::endl;
            }
            else
            {
                if(cell->hasGaussian_)
                {
                    meanFixed = cell->getMean();
                    covCombined = cell->getCov() + R.transpose()*sourceNDT[i]->getCov()*R;
                    covCombined.computeInverseAndDetWithCheck(icov,det,exists);
                    if(!exists) continue;
                    double l = (meanMoving-meanFixed).dot(icov*(meanMoving-meanFixed));
                    if(l*0 != 0) continue;
                    if(l > 120) continue;

                    double sh = -lfd1*(exp(-lfd2*l/2));

                    if(fabsf(sh) > 1e-10)
                    {
                        NUMBER_OF_ACTIVE_CELLS++;
                    }
                    scores.push_back(std::pair<unsigned int, double>(j, sh));
                    score_here += sh;
                    //score_here += l;
                }
            }
        }
    }

    if (_trimFactor == 1.)
    {
        return score_here;
    }
    else
    {
        // Determine the score value
        if (scores.empty()) // TODO, this happens(!), why??!??
            return score_here;

        score_here = 0.;
        unsigned int index = static_cast<unsigned int>(_trimFactor * (scores.size() - 1));
        //	std::nth_element (scores.begin(), scores.begin()+index, scores.end(), sort_scores()); //boost::bind(&std::pair<unsigned int, double>::second, _1) < boost::bind(&std::pair<unsigned int, double>::second, _2));
        std::nth_element (scores.begin(), scores.begin()+index, scores.end(), boost::bind(&std::pair<unsigned int, double>::second, _1) < boost::bind(&std::pair<unsigned int, double>::second, _2));
        std::fill(_goodCorr.begin(), _goodCorr.end(), false);
        //	std::cout << "_goodCorr.size() : " << _goodCorr.size() << " scores.size() : " << scores.size() << " index : " << index << std::endl;
        for (unsigned int i = 0; i < _goodCorr.size(); i++)
        {
            if (i <= index)
            {
                score_here += scores[i].second;
                _goodCorr[scores[i].first] = true;
            }
        }
        return score_here;
    }
}

double
NDTMatcherFeatureD2D::derivativesNDT(
    const std::vector<NDTCell*> &sourceNDT,
    const NDTMap &targetNDT,
    Eigen::MatrixXd &score_gradient,
    Eigen::MatrixXd &Hessian,
    bool computeHessian
)
{

    struct timeval tv_start, tv_end;
    double score_here = 0;

    gettimeofday(&tv_start,NULL);
    NUMBER_OF_ACTIVE_CELLS = 0;
    score_gradient.setZero();
    Hessian.setZero();

    pcl::PointXYZ point;
    Eigen::Vector3d transformed;
    Eigen::Vector3d meanMoving, meanFixed;
    Eigen::Matrix3d CMoving, CFixed, CSum, Cinv, R;
    NDTCell *cell;
    bool exists = false;
    double det = 0;
    for (unsigned int j = 0; j < _corr.size(); j++)
    {
        if (!_goodCorr[j])
            continue;

        unsigned int i = _corr[j].second;
        if (i >= sourceNDT.size())
        {
            std::cout << "sourceNDT.size() : " << sourceNDT.size() << ", i: " << i << std::endl;
        }
        assert(i < sourceNDT.size());

	meanMoving = sourceNDT[i]->getMean();
        CMoving= sourceNDT[i]->getCov();

	this->computeDerivatives(meanMoving, CMoving, computeHessian);

        point.x = meanMoving(0);
        point.y = meanMoving(1);
        point.z = meanMoving(2);

        cell = targetNDT.getCellIdx(_corr[j].first);
	if(cell == NULL)
	{
	    continue;
	}
	if(cell->hasGaussian_)
	{
	    transformed = meanMoving - cell->getMean();
	    CFixed = cell->getCov();
	    CSum = (CFixed+CMoving);
	    CSum.computeInverseAndDetWithCheck(Cinv,det,exists);
	    if(!exists)
	    {
		//delete cell;
		continue;
	    }
	    double l = (transformed).dot(Cinv*(transformed));
	    if(l*0 != 0)
	    {
		//delete cell;
		continue;
	    }
	    double sh = -lfd1*(exp(-lfd2*l/2));
	    //std::cout<<"m1 = ["<<meanMoving.transpose()<<"]';\n m2 = ["<<cell->getMean().transpose()<<"]';\n";
	    //std::cout<<"C1 = ["<<CMoving<<"];\n C2 = ["<<CFixed<<"];\n";
	    //update score gradient
	    if(!this->update_gradient_hessian(score_gradient,Hessian,transformed, Cinv, sh, computeHessian))
	    {
		continue;
	    }
	    score_here += sh;
	    cell = NULL;
	}
    }
    gettimeofday(&tv_end,NULL);

    //double time_load = (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
    //std::cout<<"time derivatives took is: "<<time_load<<std::endl;
    return score_here;

}

}


#ifndef __aarch64__
#include <boost/lexical_cast.hpp>
#include <ippi.h>
#include <ipp.h>

#include "json.hpp"
#include "jget.h"

using json = nlohmann::json;

#include "vnxvideoimpl.h"
#include "vnxvideologimpl.h"

#include "RawSample.h"

class CWarpTransform_8u {
public:
    CWarpTransform_8u(IppiSize size, const double coeffs[3][3], uint8_t border)
    : m_size(size) {
        IppStatus s;
        Ipp64f borderValue = border;

        int specSize;
        int bufSize;
        int initSize;
        s=ippiWarpPerspectiveGetSize(m_size, ippRectInfinite, m_size, ipp8u, coeffs, ippCubic, ippWarpForward, ippBorderConst,
            &specSize, &initSize);
        if (ippStsNoErr != s)
            throw std::runtime_error("ippiWarpPerspectiveGetSize failed, status = " + boost::lexical_cast<std::string>(s));

        m_spec.reset(reinterpret_cast<IppiWarpSpec*>(ippMalloc(specSize)), ippFree);
        std::shared_ptr<Ipp8u> initBuf(reinterpret_cast<Ipp8u*>(ippMalloc(initSize)), ippFree);

        s = ippiWarpPerspectiveCubicInit(m_size, ippRectInfinite, m_size, ipp8u, coeffs, ippWarpForward, 1, 0, 0,
            ippBorderConst, &borderValue, 0, m_spec.get(), initBuf.get());
        if (ippStsNoErr != s)
            throw std::runtime_error("ippiWarpPerspectiveCubicInit failed, status = " + boost::lexical_cast<std::string>(s));

        s = ippiWarpGetBufferSize(m_spec.get(), m_size, &bufSize);
        if (ippStsNoErr != s)
            throw std::runtime_error("ippiWarpGetBufferSize failed, status = " + boost::lexical_cast<std::string>(s));
        m_buf.reset(reinterpret_cast<Ipp8u*>(ippMalloc(bufSize)), ippFree);
    }
    void Apply(const Ipp8u* src, int srcStep, Ipp8u* dst, int dstStep) {
        IppStatus s=ippiWarpPerspectiveCubic_8u_C1R(src, srcStep, dst, dstStep, { 0,0 }, m_size, m_spec.get(), m_buf.get());
        if (ippStsNoErr != s)
            throw std::runtime_error("ippiWarpPerspectiveCubic_8u_C1R failed, status = " + boost::lexical_cast<std::string>(s));
    }
private:
    IppiSize m_size;
    std::shared_ptr<IppiWarpSpec> m_spec;
    std::shared_ptr<Ipp8u> m_buf;
};

class CDewarpProjective : public VnxVideo::IRawTransform {
public:
    CDewarpProjective(const std::vector<double>& tform)
        : m_width(0)
        , m_height(0) {

        if(tform.size()!=9)
            throw std::logic_error("Nine transform coefficients, row-wise, are expected");
        // row-wise means
        // 1 2 3
        // 4 5 6
        // 7 8 9
        // because this way it can be easily formatted in JSON as a single array

        for (int k = 0; k < 9; ++k) {
            m_warpCoeffs[k % 3][k / 3] = tform[k];
        }
    }

    static void scaleGeoTransformMatrix(int width, int height, const double from[3][3], double to[3][3]) {
        double diagF[3] = { double(width), double(height), 1 };
        double diagB[3] = { 1.0 / width, 1.0 / height, 1 };

        for (int k = 0; k < 3; ++k)
            for (int j = 0; j < 3; ++j) {
                to[j][k] = diagB[k] * from[j][k] * diagF[j];
            }
    }

    void SetFormat(EColorspace csp, int width, int height) {
        if(csp!=EMF_I420)
            throw std::logic_error("Sample format other than I420 is not supported");
        m_width = width;
        m_height = height;

        scaleGeoTransformMatrix(width, height, m_warpCoeffs, m_warpCoeffsY);
        scaleGeoTransformMatrix(width/2, height/2, m_warpCoeffs, m_warpCoeffsUV);

        m_warpY.reset();
        m_warpUV.reset();

        m_onFormat(csp, width, height);
    }
    void Process(VnxVideo::IRawSample* sample, uint64_t timestamp) {
        if (0 == m_width || 0 == m_height)
            return;

        int stridesIn[4];
        uint8_t* planesIn[4];
        sample->GetData(stridesIn, planesIn);

        CRawSample out(m_width, m_height);
        int stridesOut[4];
        uint8_t* planesOut[4];
        out.GetData(stridesOut, planesOut);

        const int sizeDivisor[3] = { 1, 2, 2 };
        double(*warpCoeffs[3])[3][3]  = { &m_warpCoeffsY, &m_warpCoeffsUV, &m_warpCoeffsUV };

        for (int k = 0; k < 3; ++k) {
            std::unique_ptr<CWarpTransform_8u>& warp = (k == 0) ? m_warpY : m_warpUV;

            IppiSize size = { m_width / sizeDivisor[k], m_height / sizeDivisor[k] };

            const uint8_t border = (k == 0 ? 0 : 0x80);
            if (warp.get() == nullptr) {
                warp.reset(new CWarpTransform_8u(size, *warpCoeffs[k], border));
            }

            warp->Apply(planesIn[k], stridesIn[k], planesOut[k], stridesOut[k]);
        }
        m_onFrame(&out, timestamp);
    }
    void Flush() {
    }
    virtual void Subscribe(VnxVideo::TOnFormatCallback onFormat, VnxVideo::TOnFrameCallback onFrame) {
        m_onFormat = onFormat;
        m_onFrame = onFrame;
    }
private:
    VnxVideo::TOnFormatCallback m_onFormat;
    VnxVideo::TOnFrameCallback m_onFrame;
    int m_width;
    int m_height;

    double m_warpCoeffs[3][3];
    double m_warpCoeffsY[3][3];
    double m_warpCoeffsUV[3][3];
    std::unique_ptr<CWarpTransform_8u> m_warpY;
    std::unique_ptr<CWarpTransform_8u> m_warpUV;
};

namespace VnxVideo {
    IRawTransform* CreateRawTransform_DewarpProjective(const std::vector<double>& tform) {
        return new CDewarpProjective(tform);
    }
    IRawTransform* CreateRawTransform(const nlohmann::json& config) {
        std::string type(jget<std::string>(config, "type"));
        if (type == "projective") {
            std::vector<double> tform(jget<std::vector<double> >(config, "matrix"));
            if (tform.size() != 9) {
                throw std::runtime_error("incorrect number of transform coefficiens, 3x3 matrix unwarped in a single array, row-wise, expected");
            }
            return CreateAsyncTransform(
                PRawTransform(VnxVideo::CreateRawTransform_DewarpProjective(tform)));
        }
        else
            throw std::runtime_error("unknown raw video transform type: " + type);
    }
}
#endif // aarch64