// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_ASSIGN_HPP
#define BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_ASSIGN_HPP

#include <boost/geometry/algorithms/assign.hpp>

#include <boost/geometry/extensions/algebra/core/tags.hpp>

#include <boost/geometry/extensions/algebra/algorithms/detail.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch {

template <typename Vector>
struct assign_zero<vector_tag, Vector>
{
    static inline void apply(Vector & g)
    {
        detail::algebra::assign_value<
            Vector,
            typename coordinate_type<Vector>::type,
            0, dimension<Vector>::type::value
        >::apply(g, 0);
    }
};

template <typename GeometryTag, typename Geometry>
struct assign_identity
{
    BOOST_MPL_ASSERT_MSG(false, NOT_IMPLEMENTED_FOR_THIS_GEOMETRY, (GeometryTag, Geometry));
};

template <typename R>
struct assign_identity<rotation_quaternion_tag, R>
{
    static inline void apply(R & g)
    {
        set<0>(g, 1); set<1>(g, 0); set<2>(g, 0); set<3>(g, 0);
    }
};

template <typename R>
struct assign_identity<rotation_matrix_tag, R>
{
    static inline void apply(R & r)
    {
        detail::algebra::identity_matrix<
            R, 0, 0, dimension<R>::value, dimension<R>::value
        >::apply(r);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

// TODO
// Use assign_zero for initialization of 0-angle rotation instead of assign_identity?

/*!
\brief assign identity to Transformation
\ingroup assign
\details The assign_identity function initializes a rotation or transformation with values indicating no rotation
\tparam Rotation The rotation type.
\param rotation The rotation.
 */
template <typename Rotation>
inline void assign_identity(Rotation & rotation)
{
    concepts::check<Rotation>();

    dispatch::assign_identity<
        typename tag<Rotation>::type,
        Rotation
    >::apply(rotation);
}

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch {

template <typename V>
struct assign<vector_tag, V, 2>
{
    typedef typename coordinate_type<V>::type coordinate_type;

    template <typename T>
    static inline void apply(V& v, T const& c1, T const& c2)
    {
        set<0>(v, boost::numeric_cast<coordinate_type>(c1));
        set<1>(v, boost::numeric_cast<coordinate_type>(c2));
    }
};

template <typename V>
struct assign<vector_tag, V, 3>
{
    typedef typename coordinate_type<V>::type coordinate_type;

    template <typename T>
    static inline void apply(V& v, T const& c1, T const& c2, T const& c3)
    {
        set<0>(v, boost::numeric_cast<coordinate_type>(c1));
        set<1>(v, boost::numeric_cast<coordinate_type>(c2));
        set<2>(v, boost::numeric_cast<coordinate_type>(c3));
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_ALGEBRA_ALGORITHMS_ASSIGN_HPP


/*
***** BEGIN LICENSE BLOCK *****

Copyright 2010 Serge Aleynikov <saleyn at gmail dot com>

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 BLOCK *****
*/
#ifndef _EIXX_HASHTABLE_HPP_
#define _EIXX_HASHTABLE_HPP_

#include <eixx/util/sync.hpp>

#if defined(__GXX_EXPERIMENTAL_CXX0X__) || __cplusplus >= 201103L
#include <unordered_map>
#else
#include <boost/unordered_map.hpp>
#endif

namespace eixx {
namespace detail {

namespace src =
    #if defined(__GXX_EXPERIMENTAL_CXX0X__) || __cplusplus >= 201103L
    std;
    #else
    boost;
    #endif

/// Hash map class
template <typename K, typename V, typename Hash>
struct hash_map_base : public src::unordered_map<K, V, Hash>
{
    hash_map_base() {}
    hash_map_base(size_t n): src::unordered_map<K,V,Hash>(n)
    {}
    hash_map_base(size_t n, const Hash& h): src::unordered_map<K,V,Hash>(n, h)
    {}
};

/// Hash functor implemention hsieh hashing algorithm
// See http://www.azillionmonkeys.com/qed/hash.html
// Copyright 2004-2008 (c) by Paul Hsieh
struct hsieh_hash_fun {
    static uint16_t get16bits(const char* d) { return *(const uint16_t *)d; }

    size_t operator()(const char* data) const {
        size_t len = strlen(data);
        BOOST_ASSERT(len <= UINT32_MAX);
        uint32_t hash = (uint32_t)len, tmp;
        int rem;

        if (len <= 0 || data == NULL) return 0;

        rem = len & 3;
        len >>= 2;

        /* Main loop */
        for (;len > 0; len--) {
            hash  += get16bits (data);
            tmp    = (get16bits (data+2) << 11) ^ hash;
            hash   = (hash << 16) ^ tmp;
            data  += 2*sizeof (uint16_t);
            hash  += hash >> 11;
        }

        /* Handle end cases */
        switch (rem) {
            case 3: hash += get16bits (data);
                    hash ^= hash << 16;
                    hash ^= uint32_t(data[sizeof (uint16_t)]) << 18;
                    hash += hash >> 11;
                    break;
            case 2: hash += get16bits (data);
                    hash ^= hash << 11;
                    hash += hash >> 17;
                    break;
            case 1: hash += uint32_t(*data);
                    hash ^= hash << 10;
                    hash += hash >> 1;
        }

        /* Force "avalanching" of final 127 bits */
        hash ^= hash << 3;
        hash += hash >> 5;
        hash ^= hash << 4;
        hash += hash >> 17;
        hash ^= hash << 25;
        hash += hash >> 6;

        return static_cast<size_t>(hash);
    }
};

} // namespace detail

typedef detail::hash_map_base<const char*, size_t, detail::hsieh_hash_fun> char_int_hash_map;

} // namespace eixx

#endif // _EIXX_HASHTABLE_HPP_


#include <string>
#include <exception>
#include <stdexcept>
#include <stdio.h>
#include <stdlib.h>
#include <utility>

#include <glload/gl_3_3.hpp>
#include <glload/gl_load.hpp>
#include <GL/glfw.h>
#include <glm/glm.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtc/half_float.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/quaternion.hpp>
#include <glm/gtx/quaternion.hpp>
#include <glutil/glutil.h>
#include <glmesh/glmesh.h>
#include <glmesh/BoostDraw.h>
#include <boost/tuple/tuple.hpp>
#include <boost/range.hpp>
#include <boost/foreach.hpp>
#include "framework_draw.h"

typedef boost::tuple<glm::hvec4, glm::detail::tvec4<GLubyte> > VertexType;

namespace
{
	VertexType g_vertexData[] =
	{
		VertexType(glm::hvec4(0.75f, 0.75f, 0.0f, 1.0f), glm::detail::tvec4<GLubyte>(153, 204, 0, 255)),
		VertexType(glm::hvec4(0.75f, -0.75f, 0.0f, 1.0f), glm::detail::tvec4<GLubyte>(230, 51, 0, 255)),
		VertexType(glm::hvec4(-0.75f, -0.75f, 0.0f, 1.0f), glm::detail::tvec4<GLubyte>(26, 51, 179, 255)),
	};

	VertexType g_groundData[] =
	{
		VertexType(glm::hvec4(30.0f, 0.0f, 30.0f, 1.0f), glm::detail::tvec4<GLubyte>(51, 255, 51, 255)),
		VertexType(glm::hvec4(30.0f, 0.0f, -30.0f, 1.0f), glm::detail::tvec4<GLubyte>(51, 255, 51, 255)),
		VertexType(glm::hvec4(-30.0f, 0.0f, 30.0f, 1.0f), glm::detail::tvec4<GLubyte>(51, 255, 51, 255)),
		VertexType(glm::hvec4(-30.0f, 0.0f, -30.0f, 1.0f), glm::detail::tvec4<GLubyte>(51, 255, 51, 255)),
	};
}

class BasicDrawable : public Drawable
{
public:
	BasicDrawable()
		: m_streamBuf(1024)
	{
		gl::ClearColor(1.0f, 1.0f, 1.0f, 1.0f);
		gl::ClearDepth(1.0);

		gl::Enable(gl::DEPTH_TEST);
		gl::DepthFunc(gl::LEQUAL);
		gl::Enable(gl::DEPTH_CLAMP);

		const std::string vertexShader(
			"#version 330\n"
			"\n"
			"layout(location = 0) in vec4 position;\n"
			"layout(location = 1) in vec4 color;\n"
			"\n"
			"smooth out vec4 theColor;\n"
			"\n"
			"uniform mat4 cameraToClipMatrix;\n"
			"uniform mat4 modelToCameraMatrix;\n"
			"\n"
			"void main()\n"
			"{\n"
			"	vec4 cameraPos = modelToCameraMatrix * position;\n"
			"	gl_Position = cameraToClipMatrix * cameraPos;\n"
			"	theColor = color;\n"
			"}\n"
			);

		const std::string fragmentShader(
			"#version 330\n"
			"\n"
			"smooth in vec4 theColor;\n"
			"out vec4 outputColor;\n"
			"\n"
			"void main()\n"
			"{\n"
			"	outputColor = theColor;\n"
			"}\n"
			);

		GLuint vertShader = glutil::CompileShader(gl::VERTEX_SHADER, vertexShader);
		GLuint fragShader = glutil::CompileShader(gl::FRAGMENT_SHADER, fragmentShader);

		m_program = glutil::LinkProgram(vertShader, fragShader);

		gl::DeleteShader(vertShader);
		gl::DeleteShader(fragShader);

		m_unifModelToCameraMatrix = gl::GetUniformLocation(m_program, "modelToCameraMatrix");
		m_unifCameraToClipMatrix = gl::GetUniformLocation(m_program, "cameraToClipMatrix");

		glmesh::AttributeList attribs;
		attribs.push_back(glmesh::AttribDesc(0, 4, glmesh::VDT_HALF_FLOAT, glmesh::ADT_FLOAT));
		attribs.push_back(glmesh::AttribDesc(1, 4, glmesh::VDT_UNSIGN_BYTE, glmesh::ADT_NORM_FLOAT));
		m_vertFmt = glmesh::VertexFormat(attribs);
	}

	~BasicDrawable()
	{
		gl::DeleteProgram(m_program);
	}

	virtual void Draw(const glm::mat4 &cameraToClip, const glm::mat4 &worldToCamera,
		const glm::mat4 &modelToWorld)
	{
		gl::ClearColor(0.9f, 0.9f, 0.9f, 1.0f);
		gl::ClearDepth(1.0f);
		gl::Clear(gl::COLOR_BUFFER_BIT | gl::DEPTH_BUFFER_BIT);

		//Set uniforms.
		gl::UseProgram(m_program);
		gl::UniformMatrix4fv(m_unifCameraToClipMatrix, 1, gl::FALSE_,
			glm::value_ptr(cameraToClip));

		{
			gl::UniformMatrix4fv(m_unifModelToCameraMatrix, 1, gl::FALSE_,
				glm::value_ptr(worldToCamera * modelToWorld));

			glmesh::Draw immMode(gl::TRIANGLES, boost::size(g_vertexData), m_vertFmt, m_streamBuf);
			BOOST_FOREACH(const VertexType &vertex, g_vertexData)
			{
				glmesh::Attrib(immMode, vertex);
			}

			immMode.Render();
		}

		{
			gl::UniformMatrix4fv(m_unifModelToCameraMatrix, 1, gl::FALSE_,
				glm::value_ptr(worldToCamera));

			glmesh::Draw immMode(gl::TRIANGLE_STRIP, boost::size(g_groundData), m_vertFmt, m_streamBuf);
			BOOST_FOREACH(const VertexType &vertex, g_groundData)
			{
				glmesh::Attrib(immMode, vertex);
			}

			immMode.Render();
		}

		gl::UseProgram(0);
	}

private:
	glmesh::StreamBuffer m_streamBuf;
	glmesh::VertexFormat m_vertFmt;
	GLuint m_program;
	GLuint m_unifModelToCameraMatrix;
	GLuint m_unifCameraToClipMatrix;
};

Drawable *CreateDrawable()
{
	return new BasicDrawable();
}


#ifndef QUBUS_VALUE_SET_ANALYSIS_HPP
#define QUBUS_VALUE_SET_ANALYSIS_HPP

#include <qubus/axiom_analysis.hpp>
#include <qubus/task_invariants_analysis.hpp>

#include <qubus/affine_constraints.hpp>

#include <qubus/pass_manager.hpp>

#include <qubus/IR/expression.hpp>

#include <qubus/isl/set.hpp>

#include <boost/range/any_range.hpp>

#include <functional>
#include <memory>
#include <utility>

namespace qubus
{

class value_set
{
public:
    value_set(isl::set values_, std::shared_ptr<affine_expr_context> ctx_)
    : values_(std::move(values_)), ctx_(std::move(ctx_))
    {
    }

    const isl::set& values() const
    {
        return values_;
    }

    const std::shared_ptr<affine_expr_context>& ctx() const
    {
        return ctx_;
    }

private:
    isl::set values_;
    std::shared_ptr<affine_expr_context> ctx_;
};

class value_set_analysis_result
{
public:
    explicit value_set_analysis_result(
        const axiom_analysis_result& axiom_analysis_,
        const task_invariants_analysis_result& task_invariants_analysis_, isl::context& isl_ctx_);

    std::vector<value_set>
    determine_value_sets(boost::any_range<const expression&, boost::forward_traversal_tag> expressions,
                         const expression& context) const;
    value_set determine_value_set(const expression& expr, const expression& context) const;

private:
    const axiom_analysis_result* axiom_analysis_;
    const task_invariants_analysis_result* task_invariants_analysis_;
    isl::context* isl_ctx_;
};

class value_set_analysis_pass
{
public:
    using result_type = value_set_analysis_result;

    value_set_analysis_result run(const expression& root, analysis_manager& manager,
                                  pass_resource_manager& resource_manager) const;

    std::vector<analysis_id> required_analyses() const;
};
}

#endif


#include "LaserOdometry.h"
#include "common/feature_utils.h"
#include "common/math_utils.h"
#include "common/ros_utils.h"

#include "common/transform_utils.h"
#include <Eigen/Eigenvalues>
#include <Eigen/QR>
#include <pcl/common/eigen.h>
#include <pcl/filters/filter.h>

namespace lidar_slam {

using std::sin;
using std::cos;
using std::asin;
using std::atan2;
using std::sqrt;
using std::fabs;
using std::pow;

LaserOdometry::LaserOdometry()
    : _systemInited(false), _inputFrameCount(0), _maxIterations(25),
      _deltaTAbort(0.1), _deltaRAbort(0.1), _scale_rot_y(1.0),
      _scale_trans_z(1.05), _cornerPointsSharp(new CloudI()),
      _cornerPointsLessSharp(new CloudI()), _surfPointsFlat(new CloudI()),
      _surfPointsLessFlat(new CloudI()), _laserCloud(new CloudIN()),
      _lastCornerCloud(new CloudI()), _lastSurfaceCloud(new CloudI()),
      _laserCloudOri(new CloudI()), _coeffSel(new CloudI()) {
  cloudReceiveCount = 0;
  _Tsum = Eigen::Isometry3f::Identity();
}

bool LaserOdometry::setup(ros::NodeHandle &node, ros::NodeHandle &privateNode) {

  //imu_que.setup(node, privateNode);

  // fetch laser odometry params
  float fParam;
  int iParam;
  if (privateNode.getParam("scale_rot_y", fParam)) {
    _scale_rot_y = fParam;
    ROS_INFO("Set scale_rot_y: %f", fParam);
  }

  if (privateNode.getParam("scale_trans_z", fParam)) {
    _scale_trans_z = fParam;
    ROS_INFO("Set scale_trans_z: %f", fParam);
  }

  if (privateNode.getParam("maxIterations", iParam)) {
    if (iParam < 1) {
      ROS_ERROR("Invalid maxIterations parameter: %d (expected > 0)", iParam);
      return false;
    } else {
      _maxIterations = iParam;
      ROS_INFO("Set maxIterations: %d", iParam);
    }
  }

  if (privateNode.getParam("deltaTAbort", fParam)) {
    if (fParam <= 0) {
      ROS_ERROR("Invalid deltaTAbort parameter: %f (expected > 0)", fParam);
      return false;
    } else {
      _deltaTAbort = fParam;
      ROS_INFO("Set deltaTAbort: %g", fParam);
    }
  }

  if (privateNode.getParam("deltaRAbort", fParam)) {
    if (fParam <= 0) {
      ROS_ERROR("Invalid deltaRAbort parameter: %f (expected > 0)", fParam);
      return false;
    } else {
      _deltaRAbort = fParam;
      ROS_INFO("Set deltaRAbort: %g", fParam);
    }
  }

  privateNode.param("sendRegisteredCloud", _sendRegisteredCloud, true);
  privateNode.param("receiveFullCloud", _receiveFullCloud, true);

  // initialize odometry and odometry tf messages
  _laserOdometryMsg.header.frame_id = "/lidar_odom_init";
  _laserOdometryMsg.child_frame_id = "/laser_odom";

  _laserOdometryTrans.frame_id_ = "/lidar_odom_init";
  _laserOdometryTrans.child_frame_id_ = "/laser_odom";

  // advertise laser odometry topics
  _pubLaserCloudCornerLast =
      node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);
  _pubLaserCloudSurfLast =
      node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);
  _pubLaserCloudFullRes =
      node.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 2);
  _pubLaserOdometry =
      node.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 5);

  // subscribe to scan registration topics
  _subCornerPointsSharp = node.subscribe<sensor_msgs::PointCloud2>(
      "/laser_cloud_sharp", 2, &LaserOdometry::laserCloudSharpHandler, this);

  _subCornerPointsLessSharp = node.subscribe<sensor_msgs::PointCloud2>(
      "/laser_cloud_less_sharp", 2, &LaserOdometry::laserCloudLessSharpHandler,
      this);

  _subSurfPointsFlat = node.subscribe<sensor_msgs::PointCloud2>(
      "/laser_cloud_flat", 2, &LaserOdometry::laserCloudFlatHandler, this);

  _subSurfPointsLessFlat = node.subscribe<sensor_msgs::PointCloud2>(
      "/laser_cloud_less_flat", 2, &LaserOdometry::laserCloudLessFlatHandler,
      this);

  if(_receiveFullCloud){
    _subLaserCloudFullRes = node.subscribe<sensor_msgs::PointCloud2>(
        "/velodyne_cloud_2", 2, &LaserOdometry::laserCloudFullResHandler, this);

  }

  spin_thread = std::thread(&LaserOdometry::spin, this);

  return true;
}

LaserOdometry::~LaserOdometry() {
  ROS_INFO("[LaserOdometry] _inputFrameCount:%ld", _inputFrameCount);
  ROS_INFO("[LaserOdometry] cloudReceiveCount:%ld", cloudReceiveCount);
}

void LaserOdometry::transformToStart(const PointI &pi, PointI &po) {
  float s = 10 * (pi.intensity - int(pi.intensity));
  Twist t = _transform * s;
  Eigen::Isometry3f it;
  convertTransform(t, it);
  po.getVector3fMap() = it * pi.getVector3fMap();
  po.intensity = pi.intensity;
}

void LaserOdometry::transformToStart(const PointIN &pi, PointIN &po) {

  float s = 10 * (pi.curvature - int(pi.curvature));
  Twist t = _transform * s;
  Eigen::Isometry3f it;

  convertTransform(t, it);
  po.getVector3fMap() = it * pi.getVector3fMap();
  po.intensity = pi.intensity;
  po.curvature = pi.curvature;
}

size_t LaserOdometry::transformToEnd(CloudI::Ptr &cloud) {
  size_t cloudSize = cloud->points.size();
  Eigen::Isometry3f it, it_inv;
  convertTransform(_transform, it);
  it_inv = it.inverse();
  for (size_t i = 0; i < cloudSize; i++) {
    PointI &point = cloud->points[i];
    transformToStart(point, point);
    point.getVector3fMap() = it_inv * point.getVector3fMap();
  }

  return cloudSize;
}

size_t LaserOdometry::transformToEnd(CloudIN::Ptr &cloud) {
  size_t cloudSize = cloud->points.size();
  Eigen::Isometry3f it, it_inv;
  convertTransform(_transform, it);
  it_inv = it.inverse();

  for (size_t i = 0; i < cloudSize; i++) {
    PointIN &point = cloud->points[i];
    transformToStart(point, point);
    point.getVector3fMap() = it_inv * point.getVector3fMap();

    // swap to lidar frame
    /*
    float temp_x = point.x;
    point.x = point.z;
    point.z = point.y;
    point.y = temp_x;*/
  }

  return cloudSize;
}

void LaserOdometry::laserCloudSharpHandler(
    const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharpMsg) {
  _timeCornerPointsSharp = cornerPointsSharpMsg->header.stamp;

  _cornerPointsSharp->clear();
  pcl::fromROSMsg(*cornerPointsSharpMsg, *_cornerPointsSharp);

  _newCornerPointsSharp = true;
}

void LaserOdometry::laserCloudLessSharpHandler(
    const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharpMsg) {
  _timeCornerPointsLessSharp = cornerPointsLessSharpMsg->header.stamp;

  _cornerPointsLessSharp->clear();
  pcl::fromROSMsg(*cornerPointsLessSharpMsg, *_cornerPointsLessSharp);
  _newCornerPointsLessSharp = true;
}

void LaserOdometry::laserCloudFlatHandler(
    const sensor_msgs::PointCloud2ConstPtr &surfPointsFlatMsg) {
  _timeSurfPointsFlat = surfPointsFlatMsg->header.stamp;

  _surfPointsFlat->clear();
  pcl::fromROSMsg(*surfPointsFlatMsg, *_surfPointsFlat);

  _newSurfPointsFlat = true;
}

void LaserOdometry::laserCloudLessFlatHandler(
    const sensor_msgs::PointCloud2ConstPtr &surfPointsLessFlatMsg) {
  _timeSurfPointsLessFlat = surfPointsLessFlatMsg->header.stamp;

  _surfPointsLessFlat->clear();
  pcl::fromROSMsg(*surfPointsLessFlatMsg, *_surfPointsLessFlat);
  _newSurfPointsLessFlat = true;
}

void LaserOdometry::laserCloudFullResHandler(
    const sensor_msgs::PointCloud2ConstPtr &laserCloudFullResMsg) {
  _timeLaserCloudFullRes = laserCloudFullResMsg->header.stamp;

  _laserCloud->clear();
  pcl::fromROSMsg(*laserCloudFullResMsg, *_laserCloud);
  _newLaserCloudFullRes = true;
  cloudReceiveCount++;
}

void LaserOdometry::spin() {
  ros::Rate rate(500);
  bool status = ros::ok();

  while (status) {
    ros::spinOnce();
    process();
    status = ros::ok();
    rate.sleep();
  }
}

void LaserOdometry::reset() {
  _newCornerPointsSharp = false;
  _newCornerPointsLessSharp = false;
  _newSurfPointsFlat = false;
  _newSurfPointsLessFlat = false;
  _newLaserCloudFullRes = false;
}

bool LaserOdometry::hasNewData() {

  if(_receiveFullCloud){
    return _newCornerPointsSharp && _newCornerPointsLessSharp &&
          _newSurfPointsFlat && _newSurfPointsLessFlat &&
          _newLaserCloudFullRes &&
          fabs((_timeCornerPointsSharp - _timeSurfPointsLessFlat).toSec()) <
              0.005 &&
          fabs((_timeCornerPointsLessSharp - _timeSurfPointsLessFlat).toSec()) <
              0.005 &&
          fabs((_timeSurfPointsFlat - _timeSurfPointsLessFlat).toSec()) <
              0.005 &&
          fabs((_timeLaserCloudFullRes - _timeSurfPointsLessFlat).toSec()) <
              0.005;
  }else{
    return _newCornerPointsSharp && _newCornerPointsLessSharp &&
          _newSurfPointsFlat && _newSurfPointsLessFlat &&
          fabs((_timeCornerPointsSharp - _timeSurfPointsLessFlat).toSec()) <
              0.005 &&
          fabs((_timeCornerPointsLessSharp - _timeSurfPointsLessFlat).toSec()) <
              0.005 &&
          fabs((_timeSurfPointsFlat - _timeSurfPointsLessFlat).toSec()) <
              0.005;

  }

}

void LaserOdometry::process() {
  if (!hasNewData()) {
    return;
  }

  reset();

  if (!_systemInited) {
    _cornerPointsLessSharp.swap(_lastCornerCloud);
    _surfPointsLessFlat.swap(_lastSurfaceCloud);

    _lastCornerKDTree.setInputCloud(_lastCornerCloud);
    _lastSurfaceKDTree.setInputCloud(_lastSurfaceCloud);
    _systemInited = true;
    return;
  }
  Eigen::Isometry3f pre_pos, cor_pos;
  Eigen::Vector3f velocity;
  //imu_que.predict(_timeSurfPointsLessFlat, pre_pos);
  scanMatch();
  transformUpdate();
  //imu_que.correct(_Tsum, cor_pos, velocity);

  transformToEnd(_cornerPointsLessSharp);
  transformToEnd(_surfPointsLessFlat);

  _cornerPointsLessSharp.swap(_lastCornerCloud);
  _surfPointsLessFlat.swap(_lastSurfaceCloud);

  size_t lastCornerCloudSize = _lastCornerCloud->points.size();
  size_t lastSurfaceCloudSize = _lastSurfaceCloud->points.size();

  if (lastCornerCloudSize > 10 && lastSurfaceCloudSize > 100) {
    _lastCornerKDTree.setInputCloud(_lastCornerCloud);
    _lastSurfaceKDTree.setInputCloud(_lastSurfaceCloud);
  }

  publishResult();
}

void LaserOdometry::scanMatch() {
  PointI coeff;
  bool isDegenerate = false;
  Eigen::Matrix<float, 6, 6> matP;

  _inputFrameCount++;
  size_t lastCornerCloudSize = _lastCornerCloud->points.size();
  size_t lastSurfaceCloudSize = _lastSurfaceCloud->points.size();

  if (lastCornerCloudSize > 10 && lastSurfaceCloudSize > 100) {
    std::vector<int> pointSearchInd(1);
    std::vector<float> pointSearchSqDis(1);

    size_t cornerPointsSharpNum = _cornerPointsSharp->points.size();
    size_t surfPointsFlatNum = _surfPointsFlat->points.size();

    _pointSearchCornerInd1.resize(cornerPointsSharpNum);
    _pointSearchCornerInd2.resize(cornerPointsSharpNum);
    _pointSearchSurfInd1.resize(surfPointsFlatNum);
    _pointSearchSurfInd2.resize(surfPointsFlatNum);
    _pointSearchSurfInd3.resize(surfPointsFlatNum);

    for (size_t iterCount = 0; iterCount < _maxIterations; iterCount++) {
      PointI pointSel, pointProj, tripod1, tripod2, tripod3;
      _laserCloudOri->clear();
      _coeffSel->clear();

      for (int i = 0; i < cornerPointsSharpNum; i++) {
        transformToStart(_cornerPointsSharp->points[i], pointSel);

        if (iterCount % 5 == 0) {

          _lastCornerKDTree.nearestKSearch(pointSel, 1, pointSearchInd,
                                           pointSearchSqDis);

          int closestPointInd = -1, minPointInd2 = -1;
          if (pointSearchSqDis[0] < 25) {
            closestPointInd = pointSearchInd[0];
            int closestPointScan =
                int(_lastCornerCloud->points[closestPointInd].intensity);

            float pointSqDis, minPointSqDis2 = 25;
            for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) {
              if (int(_lastCornerCloud->points[j].intensity) >
                  closestPointScan + 2.5) {
                break;
              }

              pointSqDis =
                  calcSquaredDiff(_lastCornerCloud->points[j], pointSel);

              if (int(_lastCornerCloud->points[j].intensity) >
                  closestPointScan) {
                if (pointSqDis < minPointSqDis2) {
                  minPointSqDis2 = pointSqDis;
                  minPointInd2 = j;
                }
              }
            }
            for (int j = closestPointInd - 1; j >= 0; j--) {
              if (int(_lastCornerCloud->points[j].intensity) <
                  closestPointScan - 2.5) {
                break;
              }

              pointSqDis =
                  calcSquaredDiff(_lastCornerCloud->points[j], pointSel);

              if (int(_lastCornerCloud->points[j].intensity) <
                  closestPointScan) {
                if (pointSqDis < minPointSqDis2) {
                  minPointSqDis2 = pointSqDis;
                  minPointInd2 = j;
                }
              }
            }
          }

          _pointSearchCornerInd1[i] = closestPointInd;
          _pointSearchCornerInd2[i] = minPointInd2;
        }
        if (_pointSearchCornerInd2[i] >= 0) {
          const PointI &A = _lastCornerCloud->points[_pointSearchCornerInd1[i]];
          const PointI &B = _lastCornerCloud->points[_pointSearchCornerInd2[i]];
          PointI coefficients;
          if (getCornerFeatureCoefficients(A, B, pointSel, iterCount,
                                           coefficients)) {
            _laserCloudOri->push_back(_cornerPointsSharp->points[i]);
            _coeffSel->push_back(coefficients);
          }
        }
      }

      for (int i = 0; i < surfPointsFlatNum; i++) {
        transformToStart(_surfPointsFlat->points[i], pointSel);

        if (iterCount % 5 == 0) {
          _lastSurfaceKDTree.nearestKSearch(pointSel, 1, pointSearchInd,
                                            pointSearchSqDis);
          int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
          if (pointSearchSqDis[0] < 25) {
            closestPointInd = pointSearchInd[0];
            int closestPointScan =
                int(_lastSurfaceCloud->points[closestPointInd].intensity);

            float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25;
            for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++) {
              if (int(_lastSurfaceCloud->points[j].intensity) >
                  closestPointScan + 2.5) {
                break;
              }

              pointSqDis =
                  calcSquaredDiff(_lastSurfaceCloud->points[j], pointSel);

              if (int(_lastSurfaceCloud->points[j].intensity) <=
                  closestPointScan) {
                if (pointSqDis < minPointSqDis2) {
                  minPointSqDis2 = pointSqDis;
                  minPointInd2 = j;
                }
              } else {
                if (pointSqDis < minPointSqDis3) {
                  minPointSqDis3 = pointSqDis;
                  minPointInd3 = j;
                }
              }
            }
            for (int j = closestPointInd - 1; j >= 0; j--) {
              if (int(_lastSurfaceCloud->points[j].intensity) <
                  closestPointScan - 2.5) {
                break;
              }

              pointSqDis =
                  calcSquaredDiff(_lastSurfaceCloud->points[j], pointSel);

              if (int(_lastSurfaceCloud->points[j].intensity) >=
                  closestPointScan) {
                if (pointSqDis < minPointSqDis2) {
                  minPointSqDis2 = pointSqDis;
                  minPointInd2 = j;
                }
              } else {
                if (pointSqDis < minPointSqDis3) {
                  minPointSqDis3 = pointSqDis;
                  minPointInd3 = j;
                }
              }
            }
          }

          _pointSearchSurfInd1[i] = closestPointInd;
          _pointSearchSurfInd2[i] = minPointInd2;
          _pointSearchSurfInd3[i] = minPointInd3;
        }

        if (_pointSearchSurfInd2[i] >= 0 && _pointSearchSurfInd3[i] >= 0) {
          const PointI &A = _lastSurfaceCloud->points[_pointSearchSurfInd1[i]];
          const PointI &B = _lastSurfaceCloud->points[_pointSearchSurfInd2[i]];
          const PointI &C = _lastSurfaceCloud->points[_pointSearchSurfInd3[i]];

          PointI coefficients;
          if (getSurfaceFeatureCoefficients(A, B, C, pointSel, iterCount,
                                            coefficients)) {
            _laserCloudOri->push_back(_surfPointsFlat->points[i]);
            _coeffSel->push_back(coefficients);
          }
        }
      }

      int pointSelNum = _laserCloudOri->points.size();
      //cout << "iterCount,pointSelNum:" << iterCount << "," << pointSelNum << std::endl;
      if (pointSelNum < 10) {
        continue;
      }

      Eigen::Matrix<float, Eigen::Dynamic, 6> matA(pointSelNum, 6);
      Eigen::Matrix<float, 6, Eigen::Dynamic> matAt(6, pointSelNum);
      Eigen::Matrix<float, 6, 6> matAtA;
      Eigen::VectorXf matB(pointSelNum);
      Eigen::Matrix<float, 6, 1> matAtB;
      Eigen::Matrix<float, 6, 1> matX;

      float s = 1;
      /**/
      float srx = sin(s * _transform.rot_x.rad());
      float crx = cos(s * _transform.rot_x.rad());
      float sry = sin(s * _transform.rot_y.rad());
      float cry = cos(s * _transform.rot_y.rad());
      float srz = sin(s * _transform.rot_z.rad());
      float crz = cos(s * _transform.rot_z.rad());
      float tx = s * _transform.pos.x();
      float ty = s * _transform.pos.y();
      float tz = s * _transform.pos.z();

      for (int i = 0; i < pointSelNum; i++) {
        const PointI &pointOri = _laserCloudOri->points[i];
        coeff = _coeffSel->points[i];
        /*
        float arx = (crx * sry * srz * pointOri.x +
                     crx * crz * sry * pointOri.y - srx * sry * pointOri.z) *
                        coeff.x +
                    (-srx * srz * pointOri.x - crz * srx * pointOri.y -
                     crx * pointOri.z) *
                        coeff.y +
                    (crx * cry * srz * pointOri.x +
                     crx * cry * crz * pointOri.y - cry * srx * pointOri.z) *
                        coeff.z;

        float ary = ((cry * srx * srz - crz * sry) * pointOri.x +
                     (sry * srz + cry * crz * srx) * pointOri.y +
                     crx * cry * pointOri.z) *
                        coeff.x +
                    ((-cry * crz - srx * sry * srz) * pointOri.x +
                     (cry * srz - crz * srx * sry) * pointOri.y -
                     crx * sry * pointOri.z) *
                        coeff.z;

        float arz =
            ((crz * srx * sry - cry * srz) * pointOri.x +
             (-cry * crz - srx * sry * srz) * pointOri.y) *
                coeff.x +
            (crx * crz * pointOri.x - crx * srz * pointOri.y) * coeff.y +
            ((sry * srz + cry * crz * srx) * pointOri.x +
             (crz * sry - cry * srx * srz) * pointOri.y) *
                coeff.z;

        */
        float arx = ((crz*sry*crx + srz*srx)* pointOri.y +(srz*crx-crz*sry*srx)* pointOri.z)*coeff.x +
        ((srz*sry*crx-crz*srx)*pointOri.y -(srz*sry*srx+crz*crx)*pointOri.z)*coeff.y +
        (cry*crx*pointOri.y-cry*srx*pointOri.z)*coeff.z;

        float ary = (-crz*sry*pointOri.x+crz*cry*srx*pointOri.y+crz*cry*crx*pointOri.z)*coeff.x +
        (-srz*sry*pointOri.x+srz*cry*srx*pointOri.y +srz*cry*crx*pointOri.z)*coeff.y +
        (-cry*pointOri.x-sry*srx*pointOri.y-sry*crx*pointOri.z)*coeff.z;

        float arz = (-srz*cry*pointOri.x -(srz*sry*srx+crz*crx)*pointOri.y+(crz*srx-srz*sry*crx)*pointOri.z)*coeff.x+
        (crz*cry*pointOri.x+ (crz*sry*srx-srz*crx)*pointOri.y+crz*sry*crx+srz*srx*pointOri.z)*coeff.y+
        0*coeff.z;

        matA(i, 0) = arx;
        matA(i, 1) = ary;
        matA(i, 2) = arz;
        matA(i, 3) = coeff.x;
        matA(i, 4) = coeff.y;
        matA(i, 5) = coeff.z;
        matB(i, 0) = -0.05 * coeff.intensity;
      }
      matAt = matA.transpose();
      matAtA = matAt * matA;
      matAtB = matAt * matB;

      matX = matAtA.colPivHouseholderQr().solve(matAtB);

      if (iterCount == 0) {
        Eigen::Matrix<float, 1, 6> matE;
        Eigen::Matrix<float, 6, 6> matV;
        Eigen::Matrix<float, 6, 6> matV2;

        Eigen::SelfAdjointEigenSolver<Eigen::Matrix<float, 6, 6>> esolver(
            matAtA);
        matE = esolver.eigenvalues().real();
        matV = esolver.eigenvectors().real();

        matV2 = matV;

        isDegenerate = false;
        float eignThre[6] = {10, 10, 10, 10, 10, 10};
        for (int i = 0; i < 6; i++) {
          if (matE(0, i) < eignThre[i]) {
            for (int j = 0; j < 6; j++) {
              matV2(i, j) = 0;
            }
            isDegenerate = true;
          } else {
            break;
          }
        }
        matP = matV.inverse() * matV2;
      }

      if (isDegenerate) {
        Eigen::Matrix<float, 6, 1> matX2;
        matX2 = matX;
        matX = matP * matX2;
      }
      _transform.rot_x += matX(0, 0);
      _transform.rot_y += matX(1, 0);
      _transform.rot_z += matX(2, 0);
      _transform.pos.x() += matX(3, 0);
      _transform.pos.y() += matX(4, 0);
      _transform.pos.z() += matX(5, 0);

      if (!pcl_isfinite(_transform.rot_x.rad()))
        _transform.rot_x = Angle();
      if (!pcl_isfinite(_transform.rot_y.rad()))
        _transform.rot_y = Angle();
      if (!pcl_isfinite(_transform.rot_z.rad()))
        _transform.rot_z = Angle();

      if (!pcl_isfinite(_transform.pos.x()))
        _transform.pos.x() = 0.0;
      if (!pcl_isfinite(_transform.pos.y()))
        _transform.pos.y() = 0.0;
      if (!pcl_isfinite(_transform.pos.z()))
        _transform.pos.z() = 0.0;

      float deltaR =
          sqrt(pow(rad2deg(matX(0, 0)), 2) + pow(rad2deg(matX(1, 0)), 2) +
               pow(rad2deg(matX(2, 0)), 2));
      float deltaT = sqrt(pow(matX(3, 0) * 100, 2) + pow(matX(4, 0) * 100, 2) +
                          pow(matX(5, 0) * 100, 2));

      if (deltaR < _deltaRAbort && deltaT < _deltaTAbort) {
        break;
      }
    }
  }
}

void LaserOdometry::transformUpdate() {
  Eigen::Isometry3f update;
  convertTransform(_transform, update);
  _Tsum = _Tsum * update;
}

void LaserOdometry::publishResult() {

  Eigen::Quaternionf geoQuat(_Tsum.rotation());
  Eigen::Vector3f pos(_Tsum.translation());

  _laserOdometryMsg.header.stamp = _timeSurfPointsLessFlat;
  Isometry2Odom(_Tsum.cast<double>(), _laserOdometryMsg);
  Twist tw;
  convertTransform(_Tsum, tw);
  _laserOdometryMsg.twist.twist.angular.x = tw.rot_x.rad();
  _laserOdometryMsg.twist.twist.angular.y = tw.rot_y.rad();
  _laserOdometryMsg.twist.twist.angular.z = tw.rot_z.rad();

  _pubLaserOdometry.publish(_laserOdometryMsg);

  _laserOdometryTrans.stamp_ = _timeSurfPointsLessFlat;
  Isometry2TFtransform(_Tsum.cast<double>(), _laserOdometryTrans);
  _tfBroadcaster.sendTransform(_laserOdometryTrans);

  ros::Time sweepTime = _timeSurfPointsLessFlat;
  publishCloudMsg(_pubLaserCloudCornerLast, *_lastCornerCloud, sweepTime,
                  "/laser_odom");
  publishCloudMsg(_pubLaserCloudSurfLast, *_lastSurfaceCloud, sweepTime,
                  "/laser_odom");

  if (_receiveFullCloud && _sendRegisteredCloud) {
    transformToEnd(_laserCloud); // transform full resolution cloud to sweep end
                               // before sending it
    publishCloudMsg(_pubLaserCloudFullRes, *_laserCloud, sweepTime, "/laser_odom");

  }
}

} // end namespace lidar_slam


/*
(***********************************************************************)
(*                                                                     *)
(* The ACME project                                                    *)
(*                                                                     *)
(* Copyright (c) 2020-2021, Davide Stocco and Enrico Bertolazzi.       *)
(*                                                                     *)
(* The ACME project and its components are supplied under the terms of *)
(* the open source BSD 2-Clause License. The contents of the ACME      *)
(* project and its components may not be copied or disclosed except in *)
(* accordance with the terms of the BSD 2-Clause License.              *)
(*                                                                     *)
(* URL: https://opensource.org/licenses/BSD-2-Clause                   *)
(*                                                                     *)
(*    Davide Stocco                                                    *)
(*    Department of Industrial Engineering                             *)
(*    University of Trento                                             *)
(*    e-mail: davide.stocco@unitn.it                                   *)
(*                                                                     *)
(*    Enrico Bertolazzi                                                *)
(*    Department of Industrial Engineering                             *)
(*    University of Trento                                             *)
(*    e-mail: enrico.bertolazzi@unitn.it                               *)
(*                                                                     *)
(***********************************************************************)
*/

///
/// file: acme_point.hh
///

#pragma once

#ifndef INCLUDE_ACME_POINT
#define INCLUDE_ACME_POINT

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

#include "acme.hh"
#include "acme_entity.hh"
#include "acme_math.hh"

namespace acme
{

  /*\
   |               _       _
   |   _ __   ___ (_)_ __ | |_
   |  | '_ \ / _ \| | '_ \| __|
   |  | |_) | (_) | | | | | |_
   |  | .__/ \___/|_|_| |_|\__|
   |  |_|
  \*/

  //! Point class container
  /**
   * Specialization of Eigen::Matrix class
   */
  class point : public vec3, public entity
  {
  public:
    using vec3::Matrix;

    //! Point class destructor
    ~point(void);

    //! Point class constructor
    point(void);

    //! This constructor allows you to construct matrix from Eigen expressions
    template <typename derived>
    point(
      Eigen::MatrixBase<derived> const &other //!< Eigen matrix object
      ) : vec3(other)
    {
    }

    //! This method allows you to assign Eigen expressions to matrix
    template <typename derived>
    point &
    operator=(
      Eigen::MatrixBase<derived> const &other //!< Eigen matrix object
    )
    {
      this->vec3::operator=(other);
      return *this;
    }

    //! Translate point by vector
    void
    translate(
      vec3 const &vector_in //!< Input translation vector
      ) override;

    //! Transform point with affine transformation matrix
    void
    transform(
      affine const &affine_in //!< 4x4 affine transformation matrix
      ) override;

    //! Check if entity is degenerated
    bool
    isDegenerated(
      acme::real tolerance = EPSILON //!< Tolerance
    ) const override;

    //! Return object hierarchical level
    integer
    level(void) const override
    {
      return 1;
    }

    //! Return object type as string
    std::string
    type(void) const override
    {
      return "point";
    }

    //! Check whether the object is no entity
    bool
    isNone(void) const override
    {
      return false;
    }

    //! Check whether the object is a point
    bool
    isPoint(void) const override
    {
      return true;
    }

    //! Check whether the object is a line
    bool
    isLine(void) const override
    {
      return false;
    }

    //! Check whether the object is a ray
    bool
    isRay(void) const override
    {
      return false;
    }

    //! Check whether the object is a plane
    bool
    isPlane(void) const override
    {
      return false;
    }

    //! Check whether the object is a segment
    bool
    isSegment(void) const override
    {
      return false;
    }

    //! Check whether the object is a triangle
    bool
    isTriangle(void) const override
    {
      return false;
    }

    //! Check whether the object is a disk
    bool
    isDisk(void) const override
    {
      return false;
    }

    //! Check whether the object is a ball
    bool
    isBall(void) const override
    {
      return false;
    }

    //! Check whether in the point is clampable
    bool
    isClampable(void) const override
    {
      return true;
    }

    //! Check whether in the point is non-clampable
    bool
    isNonClampable(void) const override
    {
      return false;
    }

    //! Get minumum and maximum values along axes
    bool
    clamp(
      acme::vec3 &min, //!< Input minimum point
      acme::vec3 &max  //!< Input maximum point
    ) const override;

    //! Get minumum and maximum values along axes
    bool
    clamp(
      acme::real &min_x, //!< Input x value of minimum point
      acme::real &min_y, //!< Input y value of minimum point
      acme::real &min_z, //!< Input z value of minimum point
      acme::real &max_x, //!< Input x value of maximum point
      acme::real &max_y, //!< Input y value of maximum point
      acme::real &max_z  //!< Input z value of maximum point
    ) const override;

  }; // class point

  static point const NAN_POINT       = point::Constant(QUIET_NAN); //!< Not-a-Number static const point object
  static point       THROWAWAY_POINT = point(NAN_POINT);           //!< Throwaway static non-const point object

} // namespace acme

#endif

///
/// eof: acme_point.hh
///


/*
 * Copyright 2021 Huawei Technologies Co., Ltd. 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.
 */
#define BOOST_BIND_GLOBAL_PLACEHOLDERS
#include "FairmotMindsporePost.h"

#include <dirent.h>
#include <float.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <memory>
#include <string>
#include <boost/property_tree/json_parser.hpp>
#include <opencv2/core/hal/hal.hpp>
#include <opencv2/opencv.hpp>

#include "MxBase/CV/ObjectDetection/Nms/Nms.h"
#include "acl/acl.h"

namespace {
// Output Tensor
const int OUTPUT_TENSOR_SIZE = 2;
const int OUTPUT_ID_FEATURE_INDEX = 0;
const int OUTPUT_DETS_INDEX = 1;

const int OUTPUT_ID_FEATURE_SIZE = 2;
const int OUTPUT_DETS_SIZE = 3;
const float CONF_THRES = 0.3;
const int TRACK_BUFFER = 30;
const int K = 500;
const int TrackState_New = 0;
const int TrackState_Tracked = 1;
const int TrackState_Lost = 2;
const int TrackState_Removed = 3;
const int MIN_BOX_AREA = 100;
}  // namespace

namespace MxBase {
BaseTrack basetrack;
KalmanFilter kalmanfilter;
void FairmotMindsporePost::get_result(int *collist, int cols, cost *d, cost min,
                                      int rows, int &endofpath, row *colsol,
                                      cost **assigncost, cost *v, int *pred) {
  col low = 0, up = 0;  // columns in 0..low-1 are ready, now none.
  int k, j, i, j1;
  cost h, v2;
  boolean unassignedfound = FALSE;
  do {
    if (up == low) {
      min = d[collist[up++]];
      for (k = up; k < cols; k++) {
        j = collist[k];
        h = d[j];
        if (h <= min) {
          if (h < min) {
            up = low;  // restart list at index low.
            min = h;
          }
          collist[k] = collist[up];
          collist[up++] = j;
        }
      }
      for (k = low; k < up; k++)
        if (colsol[collist[k]] < 0) {
          endofpath = collist[k];
          unassignedfound = TRUE;
          break;
        }
    }
    if (!unassignedfound) {
      j1 = collist[low];
      low++;
      i = colsol[j1];
      if (i > rows) {
        i = 0;
      }
      h = assigncost[i][j1] - v[j1] - min;
      for (k = up; k < cols; k++) {
        j = collist[k];
        v2 = assigncost[i][j] - v[j] - h;
        if (v2 < d[j]) {
          pred[j] = i;
          if (v2 == min) {
            if (colsol[j] < 0) {
              endofpath = j;
              unassignedfound = TRUE;
              break;
            } else {
              collist[k] = collist[up];
              collist[up++] = j;
            }
          }
          d[j] = v2;
        }
      }
    }
  } while (!unassignedfound);
}
void FairmotMindsporePost::func(int &numfree, int *free, cost **assigncost,
                                row *colsol, cost *v, col *rowsol, int cols) {
  int j2, loopcnt = 0;  // do-loop to be done twice.
  do {
    loopcnt++;
    int k = 0;
    row prvnumfree;
    prvnumfree = numfree;
    numfree = 0;
    while (k < prvnumfree) {
      int i = free[k++];
      int umin = assigncost[i][0] - v[0];
      int j1 = 0;
      int usubmin = BIG;
      for (int j = 1; j < cols; j++) {
        int h = assigncost[i][j] - v[j];
        if (h < usubmin) {
          if (h >= umin) {
            usubmin = h;
            j2 = j;
          } else {
            usubmin = umin;
            umin = h;
            j2 = j1;
            j1 = j;
          }
        }
      }
      int i0 = colsol[j1];
      if (umin < usubmin) {
        v[j1] = v[j1] - (usubmin - umin);
      } else {
        if (i0 > -1) {
          j1 = j2;
          i0 = colsol[j2];
        }
      }
      rowsol[i] = j1;
      colsol[j1] = i;
      if (i0 > -1) {
        if (umin < usubmin) {
          free[--k] = i0;
        } else {
          free[numfree++] = i0;
        }
      }
    }
  } while (loopcnt < 2);  // repeat once.
}

void FairmotMindsporePost::lap(cost **assigncost, col *rowsol, row *colsol,
                               cost *u, cost *v, int rows, int cols) {
  int i, numfree = 0, f, *pred = new row[rows], *free = new row[rows], j, j1,
         endofpath, *collist = new col[cols], *matches = new col[cols];
  cost min, *d = new cost[rows];
  for (i = 0; i < rows; i++) matches[i] = 0;
  for (j = cols; j--;) {  // reverse order gives better results.
    row imin = 0;
    min = assigncost[0][j];
    for (i = 1; i < rows; i++)
      if (assigncost[i][j] < min) {
        min = assigncost[i][j];
        imin = i;
      }
    v[j] = min;
    if (++matches[imin] == 1) {
      rowsol[imin] = j;
      colsol[j] = imin;
    } else if (v[j] < v[rowsol[imin]]) {
      int j_1 = rowsol[imin];
      rowsol[imin] = j;
      colsol[j] = imin;
      colsol[j_1] = -1;
    } else {
      colsol[j] = -1;
    }
  }
  for (i = 0; i < rows; i++)
    if (matches[i] == 0) {
      free[numfree++] = i;
    } else if (matches[i] == 1) {
      j1 = rowsol[i];
      min = BIG;
      for (j = 0; j < cols; j++)
        if (j != j1)
          if (assigncost[i][j] - v[j] < min) min = assigncost[i][j] - v[j];
      v[j1] = v[j1] - min;
    }
  func(numfree, free, assigncost, colsol, v, rowsol, cols);
  for (f = 0; f < numfree; f++) {
    row freerow;
    freerow = free[f];  // start row of augmenting path.
    for (j = cols; j--;) {
      d[j] = assigncost[freerow][j] - v[j];
      pred[j] = freerow;
      collist[j] = j;  // init column list.
    }
    get_result(collist, cols, d, min, rows, endofpath, colsol, assigncost, v,
               pred);
    do {
      i = pred[endofpath];
      colsol[endofpath] = i;
      j1 = endofpath;
      endofpath = rowsol[i];
      rowsol[i] = j1;
    } while (i != freerow);
  }
  delete[] matches;
}

int BaseTrack::next_id() {
  this->count += 1;
  return this->count;
}

Results::Results(uint32_t frame_id, const std::vector<cv::Mat> &online_tlwhs,
                 const std::vector<int> &online_ids) {
  this->frame_id = frame_id;
  this->online_tlwhs = online_tlwhs;
  this->online_ids = online_ids;
}

JDETracker::JDETracker(uint32_t frame_rate) {
  this->det_thresh = CONF_THRES;
  this->buffer_size = static_cast<int>(frame_rate / 30.0 * TRACK_BUFFER);
  this->max_time_lost = this->buffer_size;
  this->max_per_image = K;
  KalmanFilter kalman_filter;
  this->kalman_filter = kalman_filter;
}
STack::STack() {}
STack::STack(cv::Mat tlwh, float score, cv::Mat temp_feat,
             uint32_t buffer_size) {
  tlwh.convertTo(this->tlwh, CV_64FC1);
  this->is_activated = false;
  this->score = score;
  this->tracklet_len = 0;
  this->update_features(temp_feat);
  this->alpha = 0.9;
}
cv::Mat STack::gettlwh() {
  if (this->mean.rows == 0) {
    return this->tlwh.clone();
  } else {
    cv::Mat ret =
        this->mean(cv::Range(0, this->mean.rows), cv::Range(0, 4)).clone();
    ret.at<double>(0, 2) *= ret.at<double>(0, 3);
    for (size_t i = 0; i < 2; i++) {
      ret.at<double>(0, i) -= ret.at<double>(0, i + 2) / 2;
    }
    return ret;
  }
}
cv::Mat STack::tlbr() {
  cv::Mat ret = this->gettlwh();
  for (size_t i = 0; i < 2; i++) {
    ret.at<double>(0, i + 2) += ret.at<double>(0, i);
  }
  return ret;
}
cv::Mat STack::tlwh_to_xyah(cv::Mat tlwh) {
  cv::Mat ret = tlwh;
  for (size_t i = 0; i < 2; i++) {
    ret.at<double>(0, i) += ret.at<double>(0, i + 2) / 2;
  }
  ret.at<double>(0, 2) /= ret.at<double>(0, 3);
  return ret;
}
void STack::activate(const KalmanFilter &kalman_filter, uint32_t frame_id) {
  this->kalman_filter = kalman_filter;
  this->track_id = basetrack.next_id();
  this->kalman_filter.initiate(this->tlwh_to_xyah(this->tlwh), this->mean,
                               this->covariance);
  this->tracklet_len = 0;
  this->state = TrackState_Tracked;
  this->is_activated = false;
  if (frame_id == 1) {
    this->is_activated = true;
  }
  this->frame_id = frame_id;
  this->start_frame = frame_id;
}
void STack::update_features(cv::Mat temp_feat) {
  cv::Mat feat;
  cv::normalize(temp_feat, feat);
  this->curr_feat = feat;
  if (this->smooth_feat.empty()) {
    this->smooth_feat = feat;
  } else {
    this->smooth_feat =
        this->alpha * this->smooth_feat + (1 - this->alpha) * feat;
  }
  this->features.push_back(feat);
  cv::normalize(this->smooth_feat, this->smooth_feat);
}

void STack::update(STack new_track, int frame_id, bool update_feature) {
  this->frame_id = frame_id;
  this->tracklet_len += 1;
  cv::Mat new_tlwh = new_track.gettlwh();
  this->kalman_filter.update(this->mean, this->covariance,
                             this->tlwh_to_xyah(new_tlwh));
  this->state = TrackState_Tracked;
  this->is_activated = true;
  this->score = new_track.score;
  if (update_feature == true) {
    this->update_features(new_track.curr_feat);
  }
}

void STack::re_activate(STack new_track, int frame_id, bool new_id) {
  this->kalman_filter.update(this->mean, this->covariance,
                             this->tlwh_to_xyah(new_track.gettlwh()));
  this->update_features(new_track.curr_feat);
  this->tracklet_len = 0;
  this->state = TrackState_Tracked;
  this->is_activated = true;
  this->frame_id = frame_id;
  if (new_id) {
    this->track_id = basetrack.next_id();
  }
}

void KalmanFilter::initiate(cv::Mat measurement, cv::Mat &mean,
                            cv::Mat &covariance) {
  cv::Mat mean_pos = measurement;
  cv::Mat mean_vel = cv::Mat::zeros(mean_pos.rows, mean_pos.cols, CV_64FC1);
  hconcat(mean_pos, mean_vel, mean);
  double tmp[1][8] = {
      2 * this->std_weight_position * measurement.at<double>(0, 3),
      2 * this->std_weight_position * measurement.at<double>(0, 3),
      1e-2,
      2 * this->std_weight_position * measurement.at<double>(0, 3),
      10 * this->std_weight_velocity * measurement.at<double>(0, 3),
      10 * this->std_weight_velocity * measurement.at<double>(0, 3),
      1e-5,
      10 * this->std_weight_velocity * measurement.at<double>(0, 3)};
  cv::Mat std = cv::Mat(1, 8, CV_64FC1, tmp);
  std = std.mul(std);
  covariance = cv::Mat::eye(std.cols, std.cols, CV_64FC1);
  for (size_t i = 0; i < std.cols; i++)
    covariance.at<double>(i, i) = std.at<double>(0, i);
}
void KalmanFilter::multi_predict(cv::Mat &mean,
                                 std::vector<cv::Mat> &covariance) {
  cv::Mat std_pos(4, mean.rows, CV_64FC1);
  cv::Mat std_vel(4, mean.rows, CV_64FC1);
  for (size_t i = 0; i < 4; i++)
    for (size_t j = 0; j < mean.rows; j++)
      if (i == 2) {
        std_pos.at<double>(i, j) = 1e-2;
        std_vel.at<double>(i, j) = 1e-5;
      } else {
        std_pos.at<double>(i, j) =
            this->std_weight_position * mean.at<double>(j, 3);
        std_vel.at<double>(i, j) =
            this->std_weight_velocity * mean.at<double>(j, 3);
      }
  cv::Mat sqr;
  vconcat(std_pos, std_vel, sqr);
  sqr = sqr.mul(sqr).t();
  std::vector<cv::Mat> motion_cov;
  for (size_t i = 0; i < mean.rows; i++) {
    cv::Mat diag = cv::Mat::eye(sqr.cols, sqr.cols, CV_64FC1);
    for (size_t j = 0; j < sqr.cols; j++) {
      diag.at<double>(j, j) = sqr.at<double>(i, j);
    }
    motion_cov.push_back(diag);
  }
  mean = mean * this->motion_mat.t();
  std::vector<cv::Mat> left;
  for (size_t i = 0; i < covariance.size(); i++) {
    left.push_back(this->motion_mat * covariance[i]);
  }
  for (size_t i = 0; i < covariance.size(); i++) {
    covariance[i] = left[i] * this->motion_mat.t() + motion_cov[i];
  }
}
KalmanFilter::KalmanFilter() {
  this->ndim = 4;
  this->dt = 1.0;
  this->motion_mat = cv::Mat::eye(2 * (this->ndim), 2 * (this->ndim), CV_64FC1);
  for (size_t i = 0; i < this->ndim; ++i) {
    this->motion_mat.at<double>(i, this->ndim + i) = this->dt;
  }
  this->update_mat = cv::Mat::eye(this->ndim, 2 * (this->ndim), CV_64FC1);
  this->std_weight_position = 1.0 / 20;
  this->std_weight_velocity = 1.0 / 160;
}
cv::Mat KalmanFilter::GatingDistance(cv::Mat mean, cv::Mat covariance,
                                     cv::Mat measurements, bool only_position,
                                     const std::string &metric) {
  this->project(mean, covariance);
  cv::Mat d(measurements.rows, measurements.cols, CV_64FC1);
  for (size_t i = 0; i < measurements.rows; i++) {
    d.row(i) = measurements.row(i) - mean;
  }
  cv::Mat cholesky_factor = cv::Mat::zeros(covariance.size(), CV_64F);
  for (int i = 0; i < covariance.rows; ++i) {
    int j;
    double sum;
    for (j = 0; j < i; ++j) {
      sum = 0;
      for (int k = 0; k < j; ++k) {
        sum +=
            cholesky_factor.at<double>(i, k) * cholesky_factor.at<double>(j, k);
      }
      cholesky_factor.at<double>(i, j) = (covariance.at<double>(i, j) - sum) /
                                         cholesky_factor.at<double>(j, j);
    }
    sum = 0;
    assert(i == j);
    for (int k = 0; k < j; ++k) {
      sum +=
          cholesky_factor.at<double>(j, k) * cholesky_factor.at<double>(j, k);
    }
    cholesky_factor.at<double>(j, j) = sqrt(covariance.at<double>(j, j) - sum);
  }
  cv::Mat z;
  cv::solve(cholesky_factor, d.t(), z);
  z = z.mul(z);
  cv::Mat squared_maha(1, z.cols, CV_64FC1);
  for (size_t i = 0; i < z.cols; i++) {
    double sum = 0;
    for (size_t t = 0; t < z.rows; t++) {
      sum += z.at<double>(t, i);
    }
    squared_maha.at<double>(0, i) = sum;
  }
  return squared_maha;
}

void KalmanFilter::project(cv::Mat &mean, cv::Mat &covariance) {
  cv::Mat std(1, 4, CV_64FC1);
  for (size_t i = 0; i < 4; i++)
    if (i == 2)
      std.at<double>(0, i) = 1e-1;
    else
      std.at<double>(0, i) = this->std_weight_position * mean.at<double>(0, 3);
  std = std.mul(std);
  cv::Mat innovation_cov = cv::Mat::eye(std.cols, std.cols, CV_64FC1);
  for (size_t j = 0; j < std.cols; j++) {
    innovation_cov.at<double>(j, j) = std.at<double>(0, j);
  }
  cv::Mat tmp(mean.rows, this->update_mat.rows, CV_64FC1);
  for (size_t i = 0; i < this->update_mat.rows; i++) {
    tmp.at<double>(0, i) = this->update_mat.row(i).dot(mean);
  }
  mean = tmp;
  covariance =
      this->update_mat * covariance * this->update_mat.t() + innovation_cov;
}

void KalmanFilter::update(cv::Mat &mean, cv::Mat &covariance,
                          cv::Mat measurement) {
  cv::Mat projected_mean = mean.clone();
  cv::Mat projected_cov = covariance.clone();
  this->project(projected_mean, projected_cov);
  cv::Mat chol_factor;
  this->cholesky_decomposition(projected_cov, chol_factor);
  cv::Mat b = covariance * this->update_mat.t();
  b = b.t();
  cv::Mat kalman_gain(chol_factor.rows, b.cols, CV_64FC1);
  for (size_t i = 0; i < b.cols; i++) {
    f64_vec_t x = {0, 0, 0, 0};
    f64_vec_t *f_x = &x;
    cv::Mat mat_b(b.rows, 1, CV_64FC1);
    mat_b = b.col(i);
    this->chol_subtitute(chol_factor, mat_b, f_x, chol_factor.rows);
    for (size_t j = 0; j < chol_factor.rows; j++) {
      if ((*f_x)[j] < 0.001)
        kalman_gain.at<double>(j, i) = 0;
      else
        kalman_gain.at<double>(j, i) = (*f_x)[j];
    }
  }
  kalman_gain = kalman_gain.t();
  cv::Mat innovation = measurement - projected_mean;
  mean += innovation * kalman_gain.t();
  covariance -= kalman_gain * projected_cov * kalman_gain.t();
}

void KalmanFilter::chol_subtitute(cv::Mat chol_factor, cv::Mat mat_b,
                                  f64_vec_t *f_x, int n) {
  f64_mat_t L;
  f64_vec_t b;
  for (int i = 0; i < n; ++i)
    for (int j = 0; j < n; ++j) L[i][j] = chol_factor.at<double>(i, j);
  for (int i = 0; i < n; ++i) b[i] = mat_b.at<double>(i, 0);
  f64_mat_t *f_L = &L;
  f64_vec_t *f_b = &b;
  int i, j;
  double f_sum;

  double *pX;
  double *pXj;
  const double *pL;
  const double *pB;
  /** @llr - This function shall solve the unknown vector f_x given a matrix f_L
   * and a vector f_b with the relation f_L*fL'*f_x = f_b.*/

  pX = &(*f_x)[0];
  pB = &(*f_b)[0];
  /* Copy f_b into f_x */
  for (i = 0u; i < n; i++) {
    (*pX) = (*pB);
    pX++;
    pB++;
  }
  /* Solve Ly = b  for y */
  pXj = &(*f_x)[0];
  for (i = 0u; i < n; i++) {
    double *pXi;
    double fLii;
    f_sum = (*f_x)[i];
    fLii = (*f_L)[i][i];
    pXi = &(*f_x)[0];
    pL = &(*f_L)[i][0];

    for (j = 0u; j < i; j++) {
      f_sum -= (*pL) * (*pXi);
      pL++;
      pXi++;
    }
    (*pXj) = f_sum / fLii;
    pXj++;
  }
  /* Solve L'x = y for x */
  for (i = 1u; i <= n; i++) {
    f_sum = (*f_x)[n - i];
    pXj = &(*f_x)[n - i + 1u];
    pL = &(*f_L)[n - i + 1u][n - i];
    for (j = n - i + 1u; j < n; j++) {
      f_sum -= (*pL) * (*pXj);
      pXj++;
      pL += n; /* PRQA S 0488 */
    }
    (*f_x)[n - i] = f_sum / (*f_L)[n - i][n - i];
  }
}

void KalmanFilter::cholesky_decomposition(const cv::Mat &A, cv::Mat &L) {
  L = cv::Mat::zeros(A.size(), CV_64F);
  int rows = A.rows;

  for (int i = 0; i < rows; ++i) {
    int j;
    float sum;

    for (j = 0; j < i; ++j) {
      sum = 0;
      for (int k = 0; k < j; ++k) {
        sum += L.at<double>(i, k) * L.at<double>(j, k);
      }
      L.at<double>(i, j) = (A.at<double>(i, j) - sum) / L.at<double>(j, j);
    }
    sum = 0;
    assert(i == j);
    for (int k = 0; k < j; ++k) {
      sum += L.at<double>(j, k) * L.at<double>(j, k);
    }
    L.at<double>(j, j) = sqrt(A.at<double>(j, j) - sum);
  }
}

FairmotMindsporePost &FairmotMindsporePost::operator=(
    const FairmotMindsporePost &other) {
  if (this == &other) {
    return *this;
  }
  PostProcessBase::operator=(other);
  return *this;
}

APP_ERROR FairmotMindsporePost::Init(
    const std::map<std::string, std::shared_ptr<void>> &postConfig) {
  LogInfo << "Begin to initialize FairmotMindsporePost.";
  APP_ERROR ret = PostProcessBase::Init(postConfig);
  if (ret != APP_ERR_OK) {
    LogError << GetError(ret) << "Fail to superinit  in PostProcessBase.";
    return ret;
  }

  LogInfo << "End to initialize FairmotMindsporePost.";
  return APP_ERR_OK;
}

APP_ERROR FairmotMindsporePost::DeInit() {
  LogInfo << "Begin to deinitialize FairmotMindsporePost.";
  LogInfo << "End to deinitialize FairmotMindsporePost.";
  return APP_ERR_OK;
}

bool FairmotMindsporePost::IsValidTensors(
    const std::vector<TensorBase> &tensors) const {
  if (tensors.size() < OUTPUT_TENSOR_SIZE) {
    LogError << "The number of tensor (" << tensors.size()
             << ") is less than required (" << OUTPUT_TENSOR_SIZE << ")";
    return false;
  }

  auto idFeatureShape = tensors[OUTPUT_ID_FEATURE_INDEX].GetShape();
  if (idFeatureShape.size() != OUTPUT_ID_FEATURE_SIZE) {
    LogError << "The number of tensor[" << OUTPUT_ID_FEATURE_INDEX
             << "] dimensions (" << idFeatureShape.size()
             << ") is not equal to (" << OUTPUT_ID_FEATURE_SIZE << ")";
    return false;
  }

  auto detsShape = tensors[OUTPUT_DETS_INDEX].GetShape();
  if (detsShape.size() != OUTPUT_DETS_SIZE) {
    LogError << "The number of tensor[" << OUTPUT_DETS_INDEX << "] dimensions ("
             << detsShape.size() << ") is not equal to ("
             << OUTPUT_ID_FEATURE_SIZE << ")";
    return false;
  }

  return true;
}

void FairmotMindsporePost::TransformPreds(const cv::Mat &coords,
                                          MxBase::JDETracker tracker,
                                          cv::Mat &target_coords) {
  target_coords = cv::Mat::zeros(coords.rows, coords.cols, CV_32FC1);
  float scale = tracker.s;
  uint32_t h = tracker.out_height;
  uint32_t w = tracker.out_width;
  float scale_value[1][2] = {scale, scale};
  cv::Mat scale_tmp = cv::Mat(1, 2, CV_32FC1, scale_value);
  float src_w = scale_tmp.at<float>(0, 0);
  uint32_t dst_w = w;
  uint32_t dst_h = h;
  float sn = 0.0;
  float cs = 1.0;
  uint32_t src_point_0 = 0;
  float src_point_1 = src_w * (-0.5);
  float src_dir_value[1][2] = {src_point_0 * cs - src_point_1 * sn,
                               src_point_0 * sn + src_point_1 * cs};
  cv::Mat src_dir = cv::Mat(1, 2, CV_32FC1, src_dir_value);
  float dst_dir_value[1][2] = {0, static_cast<float>(dst_w * (-0.5))};
  cv::Mat dst_dir = cv::Mat(1, 2, CV_32FC1, dst_dir_value);
  cv::Mat src = cv::Mat::zeros(3, 2, CV_32FC1);
  cv::Mat dst = cv::Mat::zeros(3, 2, CV_32FC1);
  float center_value[1][2] = {tracker.c[0], tracker.c[1]};
  cv::Mat center = cv::Mat(1, 2, CV_32FC1, center_value);
  cv::Mat shift = cv::Mat::zeros(1, 2, CV_32FC1);
  cv::Mat src_0 = scale_tmp.mul(shift) + center;
  cv::Mat src_1 = scale_tmp.mul(shift) + center + src_dir;
  cv::Mat direct = src_0 - src_1;
  float direct_tmp_value[1][2] = {-direct.at<float>(0, 1),
                                  direct.at<float>(0, 0)};
  cv::Mat direct_tmp = cv::Mat(1, 2, CV_32FC1, direct_tmp_value);
  cv::Mat src_2 = src_1 + direct_tmp;
  float dst_0_value[1][2] = {static_cast<float>(dst_w * (0.5)),
                             static_cast<float>(dst_h * (0.5))};
  cv::Mat dst_0 = cv::Mat(1, 2, CV_32FC1, dst_0_value);
  float dst_1_value[1][2] = {
      static_cast<float>(dst_w * (0.5)) + dst_dir.at<float>(0, 0),
      static_cast<float>(dst_h * (0.5)) + dst_dir.at<float>(0, 1)};
  cv::Mat dst_1 = cv::Mat(1, 2, CV_32FC1, dst_1_value);
  direct = dst_0 - dst_1;
  float direct_value[1][2] = {-direct.at<float>(0, 1), direct.at<float>(0, 0)};
  direct_tmp = cv::Mat(1, 2, CV_32FC1, direct_value);
  cv::Mat dst_2 = dst_1 + direct_tmp;
  for (size_t y = 0; y < src.cols; ++y) {
    src.at<float>(0, y) = src_0.at<float>(0, y);
    src.at<float>(1, y) = src_1.at<float>(0, y);
    src.at<float>(2, y) = src_2.at<float>(0, y);
    dst.at<float>(0, y) = dst_0.at<float>(0, y);
    dst.at<float>(1, y) = dst_1.at<float>(0, y);
    dst.at<float>(2, y) = dst_2.at<float>(0, y);
  }
  cv::Mat trans(2, 3, CV_32FC1);
  trans = cv::getAffineTransform(dst, src);
  trans.convertTo(trans, CV_32F);
  for (size_t x = 0; x < coords.rows; ++x) {
    float pt_value[3][1] = {coords.at<float>(x, 0), coords.at<float>(x, 1),
                            1.0};
    cv::Mat pt = cv::Mat(3, 1, CV_32FC1, pt_value);
    for (size_t y = 0; y < 2; ++y) {
      cv::Mat new_pt = trans * pt;
      target_coords.at<float>(x, y) = new_pt.at<float>(y);
    }
  }
}

void FairmotMindsporePost::PostProcess(cv::Mat &det,
                                       const MxBase::JDETracker &tracker) {
  cv::Mat coords_0 = det(cv::Range(0, det.rows), cv::Range(0, 2));
  cv::Mat coords_1 = det(cv::Range(0, det.rows), cv::Range(2, 4));
  cv::Mat target_coords_0;
  cv::Mat target_coords_1;
  TransformPreds(coords_0, tracker, target_coords_0);
  TransformPreds(coords_1, tracker, target_coords_1);
  for (size_t x = 0; x < det.rows; ++x) {
    for (size_t y = 0; y < 4; ++y) {
      if (y < 2) {
        det.at<float>(x, y) = target_coords_0.at<float>(x, y);
      } else {
        det.at<float>(x, y) = target_coords_1.at<float>(x, y - 2);
      }
    }
  }
  det = det(cv::Range(0, det.rows), cv::Range(0, 5));
}
void FairmotMindsporePost::TensorBaseToCVMat(cv::Mat &imageMat,
                                             const MxBase::TensorBase &tensor) {
  TensorBase Data = tensor;
  uint32_t outputModelWidth;
  uint32_t outputModelHeight;
  auto shape = Data.GetShape();
  if (shape.size() == 2) {
    outputModelWidth = shape[0];
    outputModelHeight = shape[1];
  } else {
    outputModelWidth = shape[1];
    outputModelHeight = shape[2];
  }
  auto *data = reinterpret_cast<float *>(GetBuffer(Data, 0));
  cv::Mat dataMat(outputModelWidth, outputModelHeight, CV_32FC1);
  for (size_t x = 0; x < outputModelWidth; ++x) {
    for (size_t y = 0; y < outputModelHeight; ++y) {
      dataMat.at<float>(x, y) = data[x * outputModelHeight + y];
    }
  }
  imageMat = dataMat.clone();
}

std::vector<cv::Mat> FairmotMindsporePost::get_lap(cost **assigncost,
                                                   col *rowsol, row *colsol,
                                                   cost *u, cost *v, int row,
                                                   int col, int dim) {
  std::vector<cv::Mat> results;
  lap(assigncost, rowsol, colsol, u, v, dim, dim);
  cv::Mat x(1, row, CV_32FC1), y(1, col, CV_32FC1);
  for (int i = 0; i < row; i++)
    x.at<float>(0, i) = rowsol[i] > (col - 1) ? (-1) : rowsol[i];
  for (int j = 0; j < col; j++)
    y.at<float>(0, j) = colsol[j] > (row - 1) ? (-1) : colsol[j];
  cv::Mat matches(0, 2, CV_32FC1);
  for (size_t i = 0; i < x.cols; i++) {
    if (x.at<float>(0, i) >= 0) {
      cv::Mat tmp(1, 2, CV_32FC1);
      tmp.at<float>(0, 0) = i;
      tmp.at<float>(0, 1) = x.at<float>(0, i);
      matches.push_back(tmp);
    }
  }
  std::vector<int> a, b;
  for (size_t i = 0; i < x.cols; i++)
    if (x.at<float>(0, i) < 0) a.push_back(i);
  for (size_t i = 0; i < y.cols; i++)
    if (y.at<float>(0, i) < 0) b.push_back(i);
  cv::Mat unmatched_a(1, a.size(), CV_32FC1),
      unmatched_b(1, b.size(), CV_32FC1);
  for (size_t i = 0; i < a.size(); i++) unmatched_a.at<float>(0, i) = a[i];
  for (size_t i = 0; i < b.size(); i++) unmatched_b.at<float>(0, i) = b[i];
  results.push_back(matches);
  results.push_back(unmatched_a);
  results.push_back(unmatched_b);
  return results;
}

std::vector<cv::Mat> FairmotMindsporePost::LinearAssignment(cv::Mat cost_matrix,
                                                            float thresh) {
  if (cost_matrix.rows == 0) {
    std::vector<cv::Mat> results;
    cv::Mat matches(0, 2, CV_32FC1), u_track,
        u_detection(1, cost_matrix.cols, CV_32FC1);
    for (size_t i = 0; i < cost_matrix.cols; ++i)
      u_detection.at<float>(0, i) = i;
    results.push_back(matches);
    results.push_back(u_track);
    results.push_back(u_detection);
    return results;
  } else {
    int row = cost_matrix.rows, col = cost_matrix.cols;
    int N = row > col ? row : col;
    cv::Mat cost_c_extended = cv::Mat::ones(2 * N, 2 * N, CV_64FC1);
    cost_c_extended *= thresh;
    if (row != col) {
      double min = 0, max = 0;
      double *minp = &min, *maxp = &max;
      cv::minMaxIdx(cost_matrix, minp, maxp);
      for (size_t i = 0; i < N; i++)
        for (size_t j = 0; j < N; j++)
          cost_c_extended.at<double>(i, j) = (*maxp) + thresh + 1;
    }
    for (size_t i = 0; i < row; i++)
      for (size_t j = 0; j < col; j++)
        cost_c_extended.at<double>(i, j) = cost_matrix.at<double>(i, j);
    cost_matrix = cost_c_extended;
    int dim = 2 * N, *rowsol = new int[dim], *colsol = new int[dim];
    double **costMatrix = new double *[dim], *u = new double[dim],
           *v = new double[dim];
    for (int i = 0; i < dim; i++) costMatrix[i] = new double[dim];
    for (int i = 0; i < dim; ++i)
      for (int j = 0; j < dim; ++j)
        costMatrix[i][j] = cost_matrix.at<double>(i, j);
    return get_lap(costMatrix, rowsol, colsol, u, v, row, col, dim);
  }
}

void FairmotMindsporePost::FuseMotion(MxBase::KalmanFilter &kf,
                                      cv::Mat &cost_matrix,
                                      std::vector<STack *> tracks,
                                      std::vector<STack *> detections,
                                      bool only_position, float lambda_) {
  if (cost_matrix.rows != 0) {
    int gating_dim;
    if (only_position = false)
      gating_dim = 2;
    else
      gating_dim = 4;
    float gating_threshold = kalmanfilter.chi2inv95[gating_dim];
    cv::Mat measurements(detections.size(), 4, CV_64FC1);
    for (size_t i = 0; i < detections.size(); i++)
      measurements.row(i) =
          (*detections[i]).tlwh_to_xyah((*detections[i]).gettlwh()) + 0;
    for (size_t i = 0; i < tracks.size(); i++) {
      cv::Mat gating_distance =
          kf.GatingDistance((*tracks[i]).mean, (*tracks[i]).covariance,
                            measurements, only_position);
      for (size_t t = 0; t < gating_distance.cols; t++)
        if (gating_distance.at<double>(0, t) > gating_threshold)
          cost_matrix.at<double>(i, t) = DBL_MAX;
      cost_matrix.row(i) =
          lambda_ * cost_matrix.row(i) + (1 - lambda_) * gating_distance;
    }
  }
}
std::vector<STack *> FairmotMindsporePost::JointStracks(
    std::vector<STack *> tlista, std::vector<STack *> tlistb) {
  std::vector<STack *> res;
  std::map<int, int> exists;
  for (size_t t = 0; t < tlista.size(); t++) {
    exists[(*tlista[t]).track_id] = 1;
    res.push_back(tlista[t]);
  }
  for (size_t t = 0; t < tlistb.size(); t++) {
    int tid = (*tlistb[t]).track_id;
    if (exists[tid] == 0) {
      exists[tid] = 1;
      res.push_back(tlistb[t]);
    }
  }
  return res;
}
void FairmotMindsporePost::RemoveDuplicateStracks(
    std::vector<STack *> &stracksa, std::vector<STack *> &stracksb) {
  cv::Mat pdist = IouDistance(stracksa, stracksb);
  std::vector<size_t> p, q, dupa, dupb;
  std::vector<STack *> resa;
  std::vector<STack *> resb;
  for (size_t i = 0; i < pdist.rows; i++)
    for (size_t j = 0; j < pdist.cols; j++)
      if (pdist.at<double>(i, j) < 0.15) {
        p.push_back(i);
        q.push_back(j);
      }
  for (size_t i = 0; i < p.size(); i++) {
    int timep = (*stracksa[p[i]]).frame_id - (*stracksa[p[i]]).start_frame;
    int timeq = (*stracksb[q[i]]).frame_id - (*stracksb[q[i]]).start_frame;
    if (timep > timeq) {
      dupb.push_back(q[i]);
    } else {
      dupa.push_back(p[i]);
    }
  }
  for (size_t i = 0; i < stracksa.size(); i++) {
    if (std::find(dupa.begin(), dupa.end(), i) == dupa.end()) {
      resa.push_back(stracksa[i]);
    }
  }
  for (size_t i = 0; i < stracksb.size(); i++) {
    if (std::find(dupb.begin(), dupb.end(), i) == dupb.end()) {
      resb.push_back(stracksb[i]);
    }
  }
  stracksa = resa;
  stracksb = resb;
}
std::vector<STack *> FairmotMindsporePost::SubStracks(
    std::vector<STack *> tlista, std::vector<STack *> tlistb) {
  std::vector<STack *> res;
  std::map<size_t, STack *> stracks;
  std::map<size_t, STack *>::iterator it;
  std::vector<size_t> key;
  std::vector<size_t> del_key;
  for (size_t t = 0; t < tlista.size(); t++) {
    key.push_back((*tlista[t]).track_id);
    stracks[(*tlista[t]).track_id] = tlista[t];
  }
  for (size_t t = 0; t < tlistb.size(); t++) {
    int tid = (*tlistb[t]).track_id;
    it = stracks.find(tid);
    if (it != stracks.end()) {
      del_key.push_back(tid);
      stracks.erase(it);
    }
  }
  for (size_t i = 0; i < key.size(); i++) {
    bool flag = false;
    for (size_t j = 0; j < del_key.size(); j++) {
      if (del_key[j] == key[i]) {
        flag = true;
      }
      if (flag == true) {
        break;
      }
    }
    if (flag == false) {
      res.push_back(stracks[key[i]]);
    }
  }
  return res;
}
cv::Mat FairmotMindsporePost::BboxOverlaps(std::vector<cv::Mat> boxes,
                                           std::vector<cv::Mat> query_boxes) {
  int N = boxes.size();
  int K = query_boxes.size();
  cv::Mat overlaps = cv::Mat::zeros(N, K, CV_64FC1);
  for (size_t k = 0; k < K; k++) {
    double box_area =
        (query_boxes[k].at<double>(0, 2) - query_boxes[k].at<double>(0, 0) +
         1) *
        (query_boxes[k].at<double>(0, 3) - query_boxes[k].at<double>(0, 1) + 1);
    for (size_t n = 0; n < N; n++) {
      double iw =
          std::min(boxes[n].at<double>(0, 2), query_boxes[k].at<double>(0, 2)) -
          std::max(boxes[n].at<double>(0, 0), query_boxes[k].at<double>(0, 0)) +
          1;
      if (iw > 0) {
        double ih = std::min(boxes[n].at<double>(0, 3),
                             query_boxes[k].at<double>(0, 3)) -
                    std::max(boxes[n].at<double>(0, 1),
                             query_boxes[k].at<double>(0, 1)) +
                    1;
        if (ih > 0) {
          double ua = static_cast<double>(
              (boxes[n].at<double>(0, 2) - boxes[n].at<double>(0, 0) + 1) *
                  (boxes[n].at<double>(0, 3) - boxes[n].at<double>(0, 1) + 1) +
              box_area - iw * ih);
          overlaps.at<double>(n, k) = iw * ih / ua;
        }
      }
    }
  }
  return overlaps;
}
cv::Mat FairmotMindsporePost::IouDistance(std::vector<STack *> atracks,
                                          std::vector<STack *> btracks) {
  std::vector<cv::Mat> atlbrs;
  std::vector<cv::Mat> btlbrs;
  cv::Mat cost_matrix;
  for (size_t i = 0; i < atracks.size(); i++) {
    atlbrs.push_back((*atracks[i]).tlbr());
  }
  for (size_t i = 0; i < btracks.size(); i++) {
    btlbrs.push_back((*btracks[i]).tlbr());
  }
  cv::Mat ious = cv::Mat::zeros(atlbrs.size(), btlbrs.size(), CV_64FC1);
  if (!ious.empty()) {
    ious = BboxOverlaps(atlbrs, btlbrs);
    cost_matrix = 1 - ious;
  } else {
    cost_matrix = cv::Mat::zeros(atlbrs.size(), btlbrs.size(), CV_64FC1);
  }
  return cost_matrix;
}
void FairmotMindsporePost::MultiPredict(std::vector<STack *> &stracks) {
  if (stracks.size() > 0) {
    cv::Mat multi_mean(stracks.size(), (*stracks[0]).mean.cols, CV_64FC1);
    std::vector<cv::Mat> multi_covariance;
    for (size_t i = 0; i < stracks.size(); i++) {
      multi_mean.row(i) = (*stracks[i]).mean.clone() + 0;
      multi_covariance.push_back((*stracks[i]).covariance);
    }
    for (size_t i = 0; i < stracks.size(); i++) {
      if ((*stracks[i]).state != TrackState_Tracked) {
        multi_mean.at<double>(i, 7) = 0;
      }
    }
    kalmanfilter.multi_predict(multi_mean, multi_covariance);
    for (size_t i = 0; i < multi_covariance.size(); i++) {
      (*stracks[i]).mean = multi_mean.row(i);
      (*stracks[i]).covariance = multi_covariance[i];
    }
  }
}

std::vector<STack *> FairmotMindsporePost::Get_output_stracks(
    MxBase::JDETracker &tracker, const std::vector<STack *> &activated_starcks,
    const std::vector<STack *> &refind_stracks,
    std::vector<STack *> lost_stracks, std::vector<STack *> removed_stracks) {
  std::vector<STack *> det_tmp;
  for (size_t i = 0; i < tracker.tracked_stracks.size(); i++) {
    if ((*tracker.tracked_stracks[i]).state == TrackState_Tracked) {
      det_tmp.push_back(tracker.tracked_stracks[i]);
    }
  }
  std::vector<STack *>().swap(tracker.tracked_stracks);
  tracker.tracked_stracks = det_tmp;
  std::vector<STack *>().swap(det_tmp);
  tracker.tracked_stracks =
      JointStracks(tracker.tracked_stracks, activated_starcks);
  tracker.tracked_stracks =
      JointStracks(tracker.tracked_stracks, refind_stracks);
  tracker.lost_stracks =
      SubStracks(tracker.lost_stracks, tracker.tracked_stracks);
  for (size_t i = 0; i < lost_stracks.size(); i++) {
    tracker.lost_stracks.push_back(lost_stracks[i]);
  }
  tracker.lost_stracks =
      SubStracks(tracker.lost_stracks, tracker.removed_stracks);
  for (size_t i = 0; i < removed_stracks.size(); i++) {
    tracker.removed_stracks.push_back(removed_stracks[i]);
  }
  std::vector<STack *> output_stracks;
  RemoveDuplicateStracks(tracker.tracked_stracks, tracker.lost_stracks);
  // get scores of lost tracks
  for (size_t i = 0; i < tracker.tracked_stracks.size(); i++) {
    if ((*tracker.tracked_stracks[i]).is_activated) {
      output_stracks.push_back(tracker.tracked_stracks[i]);
    }
  }
  return output_stracks;
}

void FairmotMindsporePost::Get_detections(cv::Mat det,
                                          std::vector<STack *> &detections,
                                          cv::Mat id_feature) {
  if (det.rows > 0) {
    cv::Mat det_tmp = det(cv::Range(0, det.rows), cv::Range(0, 5)).clone();
    for (size_t x = 0; x < det.rows; ++x) {
      cv::Mat tlbrs = det_tmp.row(x);
      cv::Mat f = id_feature.row(x);
      cv::Mat ret = tlbrs(cv::Range(0, tlbrs.rows), cv::Range(0, 4));
      for (size_t y = 0; y < 2; ++y) {
        ret.at<float>(0, y + 2) -= ret.at<float>(0, y);
      }
      STack *stack = new STack(ret, tlbrs.at<float>(0, 4), f, 30);
      detections.push_back(stack);
    }
  }
}

void FairmotMindsporePost::Get_dists(cv::Mat &dists,
                                     std::vector<STack *> &detections,
                                     std::vector<STack *> &strack_pool) {
  if (dists.rows != 0) {
    cv::Mat det_features(detections.size(), (*detections[0]).curr_feat.cols,
                         CV_32FC1);
    cv::Mat track_features(strack_pool.size(),
                           (*strack_pool[0]).smooth_feat.cols, CV_32FC1);
    for (size_t i = 0; i < detections.size(); i++)
      det_features.row(i) = (*detections[i]).curr_feat + 0;
    det_features.convertTo(det_features, CV_64F);
    for (size_t i = 0; i < strack_pool.size(); i++)
      track_features.row(i) = (*strack_pool[i]).smooth_feat + 0;
    track_features.convertTo(track_features, CV_64F);
    // cv::Mat cdist(track_features.rows, det_features.rows, CV_64FC1);
    for (size_t i = 0; i < dists.rows; i++)
      for (size_t j = 0; j < dists.cols; j++) {
        cv::normalize(det_features.row(j), det_features.row(j));
        cv::normalize(track_features.row(i), track_features.row(i));
        dists.at<double>(i, j) =
            1 - track_features.row(i).dot(det_features.row(j));
      }
  }
}

void FairmotMindsporePost::Update_Starcks(
    const std::vector<STack *> &strack_pool, std::vector<STack *> &detections,
    const cv::Mat &matches, std::vector<STack *> &activated_starcks,
    std::vector<STack *> &refind_stracks, const MxBase::JDETracker &tracker) {
  for (size_t i = 0; i < matches.rows; i++) {
    STack *track = strack_pool[matches.at<float>(i, 0)];
    STack *dets = detections[matches.at<float>(i, 1)];
    if ((*track).state == TrackState_Tracked) {
      (*track).update((*detections[matches.at<float>(i, 1)]), tracker.frame_id);
      activated_starcks.push_back(track);
    } else {
      (*track).re_activate(*dets, tracker.frame_id, false);
      refind_stracks.push_back(track);
    }
  }
}

std::vector<STack *> FairmotMindsporePost::ObjectDetectionOutput(
    const std::vector<TensorBase> &tensors, MxBase::JDETracker &tracker) {
  tracker.frame_id += 1;
  cv::Mat id_feature, det, matches, u_track, u_detection, u_unconfirmed, dists;
  std::vector<STack *> activated_starcks, refind_stracks, lost_stracks,
      removed_stracks, detections, unconfirmed, tracked_stracks, det_tmp,
      r_tracked_stracks, output_stracks, strack_pool;
  TensorBaseToCVMat(det, tensors[1]);
  TensorBaseToCVMat(id_feature, tensors[0]);
  PostProcess(det, tracker);
  cv::Mat scores = det(cv::Range(0, det.rows), cv::Range(4, 5));
  cv::Mat new_det(0, det.cols, CV_32FC1), new_id(0, id_feature.cols, CV_32FC1);
  for (size_t x = 0; x < det.rows; ++x) {
    if (det.at<float>(x, 4) > CONF_THRES) {
      new_det.push_back(det.row(x));
      new_id.push_back(id_feature.row(x));
    }
  }
  det = new_det;
  id_feature = new_id;
  Get_detections(det, detections, id_feature);
  for (size_t i = 0; i < tracker.tracked_stracks.size(); i++)
    if (!(*tracker.tracked_stracks[i]).is_activated)
      unconfirmed.push_back(tracker.tracked_stracks[i]);
    else
      tracked_stracks.push_back(tracker.tracked_stracks[i]);
  strack_pool = JointStracks(tracked_stracks, tracker.lost_stracks);
  MultiPredict(strack_pool);
  dists = cv::Mat::zeros(strack_pool.size(), detections.size(), CV_64FC1);
  Get_dists(dists, detections, strack_pool);
  FuseMotion(tracker.kalman_filter, dists, strack_pool, detections);
  std::vector<cv::Mat> results;
  results = LinearAssignment(dists, 0.4);
  matches = results[0];
  u_track = results[1];
  u_detection = results[2];
  Update_Starcks(strack_pool, detections, matches, activated_starcks,
                 refind_stracks, tracker);
  for (size_t i = 0; i < u_detection.cols; ++i)
    det_tmp.push_back(
        detections[static_cast<int>(u_detection.at<float>(0, i))]);
  detections = det_tmp;
  std::vector<STack *>().swap(det_tmp);
  for (size_t i = 0; i < u_track.cols; ++i)
    if ((*strack_pool[u_track.at<float>(0, i)]).state == TrackState_Tracked)
      r_tracked_stracks.push_back(strack_pool[u_track.at<float>(0, i)]);
  dists = IouDistance(r_tracked_stracks, detections);
  results = LinearAssignment(dists, 0.5);
  matches = results[0];
  u_track = results[1];
  u_detection = results[2];
  Update_Starcks(r_tracked_stracks, detections, matches, activated_starcks,
                 refind_stracks, tracker);
  for (size_t i = 0; i < u_track.cols; i++) {
    STack *track = r_tracked_stracks[u_track.at<float>(0, i)];
    if ((*track).state != TrackState_Lost) {
      (*track).state = TrackState_Lost;
      lost_stracks.push_back(track);
    }
  }
  for (size_t i = 0; i < u_detection.cols; ++i)
    det_tmp.push_back(
        detections[static_cast<int>(u_detection.at<float>(0, i))]);
  detections = det_tmp;
  std::vector<STack *>().swap(det_tmp);
  dists = IouDistance(unconfirmed, detections);
  results = LinearAssignment(dists, 0.7);
  matches = results[0];
  u_unconfirmed = results[1];
  u_detection = results[2];
  for (size_t i = 0; i < matches.rows; i++) {
    (*unconfirmed[matches.at<float>(i, 0)])
        .update((*detections[matches.at<float>(i, 1)]), tracker.frame_id);
    activated_starcks.push_back(unconfirmed[matches.at<float>(i, 0)]);
  }
  for (size_t i = 0; i < u_unconfirmed.cols; i++) {
    STack *track = unconfirmed[u_unconfirmed.at<float>(0, i)];
    (*track).state = TrackState_Removed;
    removed_stracks.push_back(track);
  }
  for (int j = 0; j < u_detection.cols; j++) {
    auto inew = u_detection.at<float>(0, j);
    STack *track = detections[inew];
    if ((*track).score < tracker.det_thresh) continue;
    (*track).activate(tracker.kalman_filter, tracker.frame_id);
    activated_starcks.push_back(track);
  }
  for (size_t i = 0; i < tracker.lost_stracks.size(); i++) {
    if (tracker.frame_id - (*tracker.lost_stracks[i]).frame_id >
        tracker.max_time_lost) {
      (*tracker.lost_stracks[i]).state = TrackState_Removed;
      removed_stracks.push_back(tracker.lost_stracks[i]);
    }
  }
  output_stracks =
      Get_output_stracks(tracker, activated_starcks, refind_stracks,
                         lost_stracks, removed_stracks);
  return output_stracks;
}

APP_ERROR FairmotMindsporePost::Process(const std::vector<TensorBase> &tensors,
                                        MxBase::JDETracker &tracker,
                                        MxBase::Files &file) {
  LogDebug << "Begin to process FairmotMindsporePost.";
  auto inputs = tensors;
  APP_ERROR ret = CheckAndMoveTensors(inputs);
  if (ret != APP_ERR_OK) {
    LogError << "CheckAndMoveTensors failed, ret=" << ret;
    return ret;
  }
  std::vector<STack *> online_targets;
  online_targets = ObjectDetectionOutput(inputs, tracker);
  std::vector<cv::Mat> online_tlwhs;
  std::vector<int> online_ids;
  for (size_t i = 0; i < online_targets.size(); i++) {
    cv::Mat tlwh = (*online_targets[i]).gettlwh();
    int tid = (*online_targets[i]).track_id;
    double tmp = tlwh.at<double>(0, 2) / tlwh.at<double>(0, 3);
    bool vertical = false;
    if (tmp > 1.6) {
      vertical = true;
    }
    if ((tlwh.at<double>(0, 2) * tlwh.at<double>(0, 3) > MIN_BOX_AREA) &&
        vertical == false) {
      online_tlwhs.push_back(tlwh);
      online_ids.push_back(tid);
    }
  }
  Results *result = new Results(file.frame_id + 1, online_tlwhs, online_ids);
  file.results.push_back(result);
  file.frame_id += 1;
  LogInfo << "End to process FairmotMindsporePost.";
  return APP_ERR_OK;
}

extern "C" {
std::shared_ptr<MxBase::FairmotMindsporePost> GetObjectInstance() {
  LogInfo << "Begin to get FairmotMindsporePost instance.";
  auto instance = std::make_shared<FairmotMindsporePost>();
  LogInfo << "End to get FairmotMindsporePost Instance";
  return instance;
}
}

}  // namespace MxBase


#include <boost/filesystem.hpp>
#include <MPILib/include/utilities/Log.hpp>
#include <fstream>
#include <iostream>
#include "GridAlgorithm.hpp"
#include "Stat.hpp"
#include "TwoDLibException.hpp"
#include "display.hpp"
#include "GridReport.hpp"
#include "MeshAlgorithmCode.hpp"

namespace TwoDLib {

	GridAlgorithm::GridAlgorithm
	(
		const std::string& model_name,
		const std::string& transform_matrix,
		MPILib::Time h,
		double start_v,
		double start_w,
		MPILib::Time tau_refractive,
		const std::string&  rate_method,
		const unsigned int num_objects
	):
	_model_name(model_name),
	_rate_method(rate_method),
	_rate(0.0),
	_t_cur(0.0),
	_root(CreateRootNode(model_name)),
	_vec_mesh(CreateMeshObject()),
	_vec_vec_rev(std::vector<std::vector<Redistribution> >{this->Mapping("Reversal")}),
	_vec_vec_res(std::vector<std::vector<Redistribution> >{this->Mapping("Reset")}),
	_vec_tau_refractive(std::vector<MPILib::Time>({tau_refractive})),
	_dt(_vec_mesh[0].TimeStep()),
	_vec_num_objects(CreateNumObjects(num_objects)),
	_sys(_vec_mesh, _vec_vec_rev, _vec_vec_res, _vec_tau_refractive, _vec_num_objects),
	_n_evolve(0),
	_n_steps(0),
	_sysfunction(rate_method == "AvgV" ? &TwoDLib::Ode2DSystemGroup::AvgV : &TwoDLib::Ode2DSystemGroup::F),
	_start_v(start_v),
	_start_w(start_w),
	_transform_matrix(transform_matrix)
	{
		_mass_swap = vector<double>(_sys._vec_mass.size());

		vector<Coordinates> coords = _vec_mesh[0].findPointInMeshSlow(Point(start_v, start_w));

		_sys.Initialize(0,coords[0][0],coords[0][1]);

	}

	GridAlgorithm::GridAlgorithm(const GridAlgorithm& rhs):
	_model_name(rhs._model_name),
	_rate_method(rhs._rate_method),
	_rate(rhs._rate),
	_t_cur(rhs._t_cur),
	_vec_mesh(rhs._vec_mesh),
	_root(rhs._root),
	_vec_vec_rev(rhs._vec_vec_rev),
	_vec_vec_res(rhs._vec_vec_res),
	_dt(_vec_mesh[0].TimeStep()),
	_vec_tau_refractive(rhs._vec_tau_refractive),
	_vec_num_objects(rhs._vec_num_objects),
	_sys(_vec_mesh,_vec_vec_rev,_vec_vec_res,_vec_tau_refractive, rhs._vec_num_objects),
	_n_evolve(0),
	_n_steps(0),
	_sysfunction(rhs._sysfunction),
	_start_v(rhs._start_v),
	_start_w(rhs._start_w),
	_transform_matrix(rhs._transform_matrix)
	{
		_mass_swap = vector<double>(_sys._vec_mass.size());

		vector<Coordinates> coords = _vec_mesh[0].findPointInMeshSlow(Point(_start_v, _start_w));

		_sys.Initialize(0,coords[0][0],coords[0][1]);

	}

	std::vector<MPILib::Index> GridAlgorithm::CreateNumObjects(MPILib::Index num) {
		std::vector<MPILib::Index> r = { num };
		return r;
	}

	GridAlgorithm* GridAlgorithm::clone() const
	{
	  return new GridAlgorithm(*this);
	}

	void GridAlgorithm::assignNodeId( MPILib::NodeId nid ) {
		_node_id = nid;
	}

	void GridAlgorithm::configure(const MPILib::SimulationRunParameter& par_run)
	{
		_transformMatrix = TransitionMatrix(_transform_matrix);
		_csr_transform = new CSRMatrix(_transformMatrix, _sys, 0);

		Display::getInstance()->addOdeSystem(_node_id, &_sys);
		GridReport<MPILib::CustomConnectionParameters>::getInstance()->registerObject(_node_id, this);

		_t_cur = par_run.getTBegin();
		_network_time_step     = par_run.getTStep();

		_sys.InitializeResetRefractive(_network_time_step);

		Quadrilateral q1 = _sys.MeshObjects()[0].Quad(1,0);
		Quadrilateral q2 = _sys.MeshObjects()[0].Quad(1,1);

		double cell_h_dist = std::fabs(q2.Centroid()[0] - q1.Centroid()[0]);
		double cell_v_dist = std::fabs(q2.Centroid()[1] - q1.Centroid()[1]);

		// one of these distances should be close to zero, so pick the other one
		// we do this because we don't know if this is a v- or h- efficacy

		double cell_width = std::max(cell_h_dist, cell_v_dist);

		setupMasterSolver(cell_width);
		// at this stage initialization must have taken place, either by default in (0,0),
		// or by the user calling Initialize if there is no strip 0

		double sum = _sys.P();
		if (sum == 0.)
			throw TwoDLib::TwoDLibException("No initialization of the mass array has taken place. Call Initialize before configure.");

	}

	void GridAlgorithm::setupMasterSolver(double cell_width){
		try {
			std::unique_ptr<MasterGrid> p_master(new MasterGrid(_sys,cell_width));
			_p_master = std::move(p_master);
		}
		// TODO: investigate the following
		// for some reason, the exception is usually not caught by the main program, which is why we write its message to cerr here.
		catch(TwoDLibException& e){
			std::cerr << e.what() << std::endl;
			throw e;
		}

	}

	std::vector<TwoDLib::Redistribution> GridAlgorithm::Mapping(const string& type)
	{
		Pred pred(type);
		pugi::xml_node rev_node = _root.find_child(pred);

		if (rev_node.name() != std::string("Mapping") ||
		    rev_node.attribute("type").value() != type)
			throw TwoDLibException("Couldn't find mapping in model file");

		std::ostringstream ostrev;
		rev_node.print(ostrev);
		std::istringstream istrev(ostrev.str());
		vector<TwoDLib::Redistribution> vec_rev = TwoDLib::ReMapping(istrev);
		return vec_rev;
	}

	std::vector<Mesh> GridAlgorithm::CreateMeshObject(){
		// mesh
		pugi::xml_node mesh_node = _root.first_child();

		if (mesh_node.name() != std::string("Mesh") )
		  throw TwoDLib::TwoDLibException("Couldn't find mesh node in model file");
		std::ostringstream ostmesh;
		mesh_node.print(ostmesh);
		std::istringstream istmesh(ostmesh.str());

		TwoDLib::Mesh mesh(istmesh);

		// MatrixGenerator should already have inserted the stationary bin and there is no need
		// to reexamine the stat file
		std::vector<TwoDLib::Mesh> vec_mesh{ mesh };
		return vec_mesh;
	}

	pugi::xml_node GridAlgorithm::CreateRootNode(const string& model_name){

		// document
		pugi::xml_parse_result result = _doc.load_file(model_name.c_str());
		pugi::xml_node  root = _doc.first_child();

		if (result.status != pugi::status_ok)
		  throw TwoDLib::TwoDLibException("Can't open .model file.");
		return root;
	}

	void GridAlgorithm::evolveNodeState
	(
		const std::vector<MPILib::Rate>& nodeVector,
		const std::vector<MPILib::CustomConnectionParameters>& weightVector,
		MPILib::Time time
	)
	{
	  // The network time step must be an integer multiple of the network time step; in principle
	  // we would expect this multiple to be one, but perhaps there are reasons to allow a population
	  // have a finer time resolution than others, so we allow larger multiples but write a warning in the log file.
		// determine number of steps and fix at the first step.
		if (_n_steps == 0){
		  // since n_steps == 0, time is the network time step
			double n = (time - _t_cur)/_dt;

			_n_steps = static_cast<MPILib::Number>(round(n));
			if (_n_steps == 0){

			  throw TwoDLibException("Network time step is smaller than this grid's time step.");
			}
			if (fabs(_n_steps - n) > 1e-6){
			  throw TwoDLibException("Mismatch of mesh time step and network time step. Network time step should be a multiple (mostly one) of network time step");
			}
			if (_n_steps > 1)
			  LOG(MPILib::utilities::logWARNING) << "Mesh runs at a time step which is a multiple of the network time step. Is this intended?";
			else
			  ; // else is fine
		}
	    // mass rotation
	    for (MPILib::Index i = 0; i < _n_steps; i++){

			_sys.EvolveWithoutMeshUpdate();

			if (_vec_num_objects[0] > 0) {
#pragma omp parallel for
				for (int id = 0; id < _sys._vec_objects_to_index.size(); id++) {
					if (_sys._vec_objects_refract_times[id] >= 0.0)
						continue;
					_sys._vec_objects_to_index[id] = _csr_transform->MVObject(_sys._vec_objects_to_index[id], 1);
				}
				_sys.updateVecCellsToObjects();
			}
			else {
#pragma omp parallel for
				for (int id = 0; id < _mass_swap.size(); id++)
					_mass_swap[id] = 0.;

				_csr_transform->MV(_mass_swap, _sys._vec_mass);

				_sys._vec_mass = _mass_swap;
			}
	    }

			// WARNING: originally reset goes afvirtual void applyMasterSolver();ter master but this way,
			// we can guarantee there's no mass above threshold when running
			// MVGrid in MasterGrid
			_sys.RedistributeProbability(_n_steps);

			std::vector<std::vector<MPILib::Rate>> vec_rates;
			for(unsigned int i=0; i<_vec_vec_delay_queues.size(); i++){
				std::vector<MPILib::Rate> rates;
				for(unsigned int j=0; j<_vec_vec_delay_queues[i].size(); j++){
					rates.push_back(_vec_vec_delay_queues[i][j].getCurrentRate());
				}
				vec_rates.push_back(rates);
			}

			applyMasterSolver(vec_rates[0]);

			_t_cur += _n_steps*_dt;

 	    _rate = (_sys.*_sysfunction)()[0];

 	    _n_evolve++;
	}

	void GridAlgorithm::applyMasterSolver(std::vector<MPILib::Rate> rates) {
		if (_vec_num_objects[0] > 0)
			_p_master->ApplyFinitePoisson(_n_steps * _dt, rates, _efficacy_map);
		else
			_p_master->Apply(_n_steps * _dt, rates, _efficacy_map);
	}

	void GridAlgorithm::FillMap(const std::vector<MPILib::CustomConnectionParameters>& vec_weights)
	{
		// this function will only be called once;
		_efficacy_map = std::vector<double>(vec_weights.size());

 		for(MPILib::Index i_weight = 0; i_weight < _efficacy_map.size(); i_weight++){
			_efficacy_map[i_weight] = std::stod(vec_weights[i_weight]._params.at("efficacy"));
		}

		_vec_vec_delay_queues = std::vector< std::vector<MPILib::DelayedConnectionQueue> >(0); // MeshAlgorithm really only uses the first array, i.e. the rates it receives in prepareEvole
 		_vec_vec_delay_queues.push_back( std::vector<MPILib::DelayedConnectionQueue>(vec_weights.size()));
		for(unsigned int q = 0; q < vec_weights.size(); q++){
			_vec_vec_delay_queues[0][q] = MPILib::DelayedConnectionQueue(_network_time_step, std::stod(vec_weights[q]._params.at("delay")));
		}
	}

	MPILib::AlgorithmGrid GridAlgorithm::getGrid(MPILib::NodeId id, bool b_state) const
	{
		// An empty grid will lead to crashes
		vector<double> array_interpretation {0.};
		vector<double> array_state {0.};

		return MPILib::AlgorithmGrid(array_state,array_interpretation);
	}

	void GridAlgorithm::reportDensity(MPILib::Time t) const
	{
		std::ostringstream ost;
		ost << _node_id  << "_" << t;
		ost << "_" << _sys.P();
		string fn("density_mesh_" + ost.str());

		std::string model_path = _model_name;
		boost::filesystem::path path(model_path);

		// MdK 27/01/2017. grid file is now created in the cwd of the program and
		// not in the directory where the mesh resides.
		const std::string dirname = path.filename().string() + "_mesh";

		if (! boost::filesystem::exists(dirname) ){
			boost::filesystem::create_directory(dirname);
		}
		std::ofstream ofst(dirname + "/" + fn);
		std::vector<std::ostream*> vec_str{&ofst};
		_sys.Dump(vec_str);
	}

	void GridAlgorithm::prepareEvolve
	(
		const std::vector<MPILib::Rate>& nodeVector,
		const std::vector<MPILib::CustomConnectionParameters>& weightVector,
		const std::vector<MPILib::NodeType>& typeVector
	)
	{
		if (_efficacy_map.size() == 0){
			FillMap(weightVector);
		}

		// take into account the number of connections

		assert(nodeVector.size() == weightVector.size());
		for (MPILib::Index i = 0; i < nodeVector.size(); i++){
			double offset = 0.0;
			if (weightVector[i]._params.find("avgv_offset") != weightVector[i]._params.end())
				offset = std::stod(weightVector[i]._params.at("avgv_offset"));

			_vec_vec_delay_queues[0][i].updateQueue((offset + nodeVector[i])*std::stod(weightVector[i]._params.at("num_connections")));
		}

	}

}