/*
 *  Created by Robin Raymond.
 *  Copyright 2009-2011. Robin Raymond. All rights reserved.
 *
 * This file is part of zsLib.
 *
 * zsLib is free software; you can redistribute it and/or modify it under the
 * terms of the GNU Lesser General Public License (LGPL) as published by the
 * Free Software Foundation; either version 3 of the License, or (at your
 * option) any later version.
 *
 * zsLib is distributed in the hope that it will be useful, but WITHOUT ANY
 * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 * FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for
 * more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with zsLib; if not, write to the Free Software Foundation, Inc., 51
 * Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA
 *
 */

#include <zsLib/Stringize.h>
#include <zsLib/Exception.h>

#include <boost/shared_array.hpp>

namespace zsLib { ZS_DECLARE_SUBSYSTEM(zsLib) }

namespace zsLib
{
  namespace internal
  {
    static char gDigits[10+26] =
    {
      '0','1','2','3','4','5','6','7','8','9',
      'a','b','c','d','e','f','g','h','i','j','k','l','m','n','o','p','q','r','s','t','u','v','w','x','y','z'
    };

    String convert(ULONGLONG value, size_t base)
    {
      ZS_THROW_INVALID_USAGE_IF((base < 2) || (base > (10+26)))

      boost::shared_array<char> buffer(new char[sizeof(ULONGLONG)*8+2]);
      memset(buffer.get(), 0, sizeof(char)*((sizeof(ULONGLONG)*8)+2));

      STR end = (buffer.get() + (sizeof(ULONGLONG)*8));
      STR dest = end;
      *dest = 0;
      while (value > 0)
      {
        --dest;
        *dest = gDigits[value % base];
        value /= base;
      }

      if (dest == end)
        *dest = '0';
      return String(dest);
    }
  }
}


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file substitute_test.cpp
/// \brief Test for the pbes rewriters.

#include "mcrl2/data/substitutions/variable_substitution.h"
#include "mcrl2/pbes/detail/parse.h"
#include "mcrl2/pbes/replace.h"
#include "mcrl2/pbes/txt2pbes.h"

#include <boost/test/included/unit_test_framework.hpp>

using namespace mcrl2;
using namespace mcrl2::pbes_system;

BOOST_AUTO_TEST_CASE(test_substitution)
{
  std::string text1 =
    "pbes nu X(n: Nat) = X(n + 1);\n"
    "init X(0);                   \n"
    ;
  pbes p1 = txt2pbes(text1);

  std::string text2 =
    "pbes nu X(n: Nat) = X(4 + 1);\n"
    "init X(0);                   \n"
    ;
  pbes p2 = txt2pbes(text2);

  data::mutable_map_substitution<> sigma;
  data::variable n("n", data::sort_nat::nat());
  sigma[n] = data::parse_data_expression("4");

  pbes p = p1;
  pbes_system::replace_free_variables(p.equations(), sigma);
  BOOST_CHECK(p == p1);

  std::cout << "--- p =\n" << pbes_system::pp(p) << std::endl;
  std::cout << "--- p1 =\n" << pbes_system::pp(p1) << std::endl;

  pbes_system::replace_variables(p, sigma);

  std::cout << "--- p =\n" << pbes_system::pp(p) << std::endl;
  std::cout << "--- p2 =\n" << pbes_system::pp(p2) << std::endl;

  // compare textual representations, to avoid conflicts between types
  BOOST_CHECK(pbes_system::pp(p) == pbes_system::pp(p2));
}

BOOST_AUTO_TEST_CASE(test_propositional_variable_substitution)
{
  std::string text1 =
    "pbes                     \n"
    "nu X(m: Nat) = Y(m + 1); \n"
    "nu Y(n: Nat) = X(n + 2); \n"
    "init X(0);               \n"
    ;
  pbes p1 = txt2pbes(text1);

  std::string text2 =
    "pbes                                     \n"
    "nu X(m: Nat) = Y(m + 1);                 \n"
    "nu Y(n: Nat) = X(n + 2 + 1) && Y(n + 2); \n"
    "init X(0);                               \n"
    ;
  pbes p2 = txt2pbes(text2);

  pbes p = p1;
  propositional_variable X = p.equations().front().variable();
  pbes_expression phi = parse_pbes_expression("X(m + 1) && Y(m)", "datavar m: Nat; \npredvar X: Nat; Y: Nat");
  propositional_variable_substitution sigma(X, phi);
  pbes_system::replace_propositional_variables(p, sigma);
  std::cout << pbes_system::pp(p) << std::endl;
  std::cout << pbes_system::pp(p2) << std::endl;

  // compare textual representations, to avoid conflicts between types
  BOOST_CHECK(pbes_system::pp(p) == pbes_system::pp(p2));
}

BOOST_AUTO_TEST_CASE(test_replace_propositional_variables)
{
  std::string text1 =
    "pbes                     \n"
    "nu X(m: Nat) = Y(m + 1); \n"
    "nu Y(n: Nat) = X(n + 2); \n"
    "init X(0);               \n"
    ;
  pbes p1 = txt2pbes(text1);

  std::string text2 =
    "pbes                     \n"
    "nu X(m: Nat) = Y(m + 1); \n"
    "nu Y(n: Nat) = Y(n + 2); \n"
    "init X(0);               \n"
    ;
  pbes p2 = txt2pbes(text2);

  pbes p = p1;
  replace_propositional_variables(p, [](const propositional_variable_instantiation& x) { return propositional_variable_instantiation(core::identifier_string("Y"), x.parameters()); });
  std::cout << pbes_system::pp(p) << std::endl;
  std::cout << pbes_system::pp(p2) << std::endl;
  BOOST_CHECK(pbes_system::pp(p) == pbes_system::pp(p2));
}

inline
pbes_expression sigma(const pbes_expression& x)
{
  std::string var_decl = "datavar b: Bool; \npredvar X: Bool;";
  pbes_expression x1 = parse_pbes_expression("X(false)", var_decl);
  pbes_expression x2 = parse_pbes_expression("X(true) || true", var_decl);
  return x == x1 ? x2 : x;
}

BOOST_AUTO_TEST_CASE(test_replace_pbes_expressions)
{
  std::string var_decl = "datavar b: Bool; \npredvar X: Bool;";
  pbes_expression x = parse_pbes_expression("X(b) || X(false)", var_decl);
  pbes_expression result = replace_pbes_expressions(x, sigma);
  pbes_expression expected_result = parse_pbes_expression("X(b) || X(true) || true", var_decl);
  if (!(result == expected_result))
  {
    std::cout << "error: " << pbes_system::pp(result) << " != " << pbes_system::pp(expected_result) << std::endl;
  }
  BOOST_CHECK(result == expected_result);
}

BOOST_AUTO_TEST_CASE(test_replace_variables)
{
  pbes_expression x = parse_pbes_expression("forall n: Nat. exists m: Nat. val(m > n)");
  pbes_expression expected_result = parse_pbes_expression("forall n: Nat. exists m: Nat. val(n > n)");
  data::mutable_map_substitution<> sigma;
  data::variable m("m", data::sort_nat::nat());
  data::variable n("n", data::sort_nat::nat());
  sigma[m] = n;
  pbes_expression result = pbes_system::replace_variables(x, sigma);
  if (!(result == expected_result))
  {
    std::cout << "error: " << pbes_system::pp(result) << " != " << pbes_system::pp(expected_result) << std::endl;
  }
  BOOST_CHECK(result == expected_result);
}

BOOST_AUTO_TEST_CASE(test_variable_assignment)
{
  pbes_expression x = parse_pbes_expression("forall n: Nat. exists m: Nat. val(m > n)");
  pbes_expression expected_result = parse_pbes_expression("forall n: Nat. exists m: Nat. val(n > n)");
  data::variable m("m", data::sort_nat::nat());
  data::variable n("n", data::sort_nat::nat());
  data::variable_substitution sigma(m, n);
  pbes_expression result = pbes_system::replace_variables(x, sigma);
  if (!(result == expected_result))
  {
    std::cout << "error: " << pbes_system::pp(result) << " != " << pbes_system::pp(expected_result) << std::endl;
  }
  BOOST_CHECK(result == expected_result);
}

boost::unit_test::test_suite* init_unit_test_suite(int argc, char* argv[])
{
  return nullptr;
}


/***************************************************************************************************************************
*
* Author: bingo
* Email: 1554459957@qq.com
* Time: 2019.10.14
* Description: lidar collision vfh v1.0
*
***************************************************************************************************************************/

//ROS 头文件
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <cmath>
#include <algorithm>
#include <vector>

//topic 头文件
#include <iostream>
#include <std_msgs/Bool.h>
#include <mavros_msgs/State.h>
#include <geometry_msgs/Pose.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/PositionTarget.h>
#include <eigen_conversions/eigen_msg.h>
#include <mavros/frame_tf.h>
#include <mavros_msgs/WaypointList.h>
#include <std_msgs/Float64.h>
#include <mavros_msgs/HomePosition.h>
#include <GeographicLib/Geocentric.hpp>
#include <sensor_msgs/NavSatFix.h>
using namespace std;

#define PI 3.1415926

struct Point2D
{
	float x;
	float y;
	float z;
};
float scan_distance_max;
float scan_distance_min;
float angle_resolution;
float heading;
float sector_value;
float sector_scale;
Point2D Uavp;
vector<float> map_cv;
vector<double> ranges;

float desire_z = 1.0; //期望高度

uint32_t init_mask = 0;
Eigen::Vector3d vel_sp_body;

void SetUavPosition(Point2D& uav) {
	Uavp.x = uav.x;
	Uavp.y = uav.y;
	Uavp.z = uav.z;
}

void SetUavHeading(float hd) {
	heading = hd;
	heading += 270;
	heading = (int)heading % 360;
	heading = 360 - heading;
}

void ComputeMV(vector<float> r) {
	float dist[360] = { 0 };
	ranges.clear();
	map_cv.clear();
	int range_size = r.size(); //有多少条激光

	for (size_t i = 0; i < range_size; i++)
	{
		//A non-zero value (true) if x is a NaN value; and zero (false) otherwise.

		//isinf A non-zero value (true) if x is an infinity; and zero (false) otherwise.
		if (!std::isnan(r[i]) && !std::isinf(r[i]))//取出有效值
		{
			float scan_distance = r[i];
			int sector_index = std::floor((i*angle_resolution) / sector_value);//(i*1)/30 sector_index:[1 12]
			if (scan_distance >= scan_distance_max || scan_distance < scan_distance_min)
				scan_distance = 0;
			else
				scan_distance = scan_distance_max - scan_distance;

			dist[sector_index] += scan_distance;
		}
		ranges.push_back(r[i]);
	}

	for (int j = 0; j < (int)(360 / sector_value); j++)//把uav四周分为12个sector，每个sector的值越小，代表越安全。
	{
		map_cv.push_back(dist[j]);
//		printf("dist[%d]=%f-",j,dist[j]);
	}
//	printf("##############################################################################################\n");
}

/*检测前方[340 360],[0 20]范围内是否安全*/
bool IsFrontSafety()
{
	float goal_sector = (int)(0 - (sector_value - sector_scale) + 360) % 360;//(0-(30-10)+360)%360 = 340,goal_sector = 340
	int start_index = goal_sector / angle_resolution;//start_index = 340
	float scan_distance = 0;
	for (int i = 0; i < (sector_value - sector_scale) * 2 / angle_resolution; i++)//for(i=0;i<40;i++)
	{
		int real_index = (start_index + i) % (int)(360 / angle_resolution);
		if (!std::isnan(ranges[real_index]) && !std::isinf(ranges[real_index]))
		{
			if (ranges[real_index] < scan_distance_max && ranges[real_index] >= scan_distance_min)
				scan_distance = scan_distance_max - ranges[real_index] + scan_distance;
		}
	}
	if (scan_distance < 0.1)
	{
		return true;
	}

	return false;
}

/*输入期望坐标点，输出期望机头方向*/
float CalculDirection(Point2D& goal) {
	float ori;
	//Compute arc tangent with two parameters
	//return Principal arc tangent of y/x, in the interval [-pi,+pi] radians.
	//One radian is equivalent to 180/PI degrees.
	float G_theta = atan2((goal.y - Uavp.y), (goal.x - Uavp.x));
	float goal_ori = G_theta * 180 / PI; //目标点相对uav的方向信息，即与y轴的夹角，正值代表在第一三现象，负值代表在二四现象。ENU坐标系下
	if (goal_ori < 0)
	{
		goal_ori += 360;
	}
	goal_ori -= heading;
	goal_ori += 360;
	goal_ori = (int)goal_ori % 360; //把目标点方向转化成机体坐标系下的方向

	float goal_sector = (int)(goal_ori - sector_value + 360) % 360;
	int start_index = goal_sector / angle_resolution;
	float scan_distance = 0;
	for (int i = 0; i < sector_value * 2 / angle_resolution; i++)
	{
		int real_index = (start_index + i) % (int)(360 / angle_resolution);
		if (!std::isnan(ranges[real_index]) && !std::isinf(ranges[real_index]))
		{
			if (ranges[real_index] < scan_distance_max && ranges[real_index] >= scan_distance_min)
				scan_distance = scan_distance_max - ranges[real_index] + scan_distance;
		}
	}
	if (scan_distance < 0.1)//判断目标方向处是否安全
	{
		ori = goal_ori;
		ori += heading;
		ori = (int)ori % 360;

		return ori;//若安全，则把机头指向目标处
	}

	vector<int> mesh;
	for (int i = 0; i < map_cv.size(); i++) //确定栅格中CV值 共有12个栅格，CV值越大，存在障碍物可能性越大
	{
		if (map_cv[i] < 0.1)
			mesh.push_back(0);
		else if (map_cv[i] >= 0.1 && map_cv[i] < 0.3)
			mesh.push_back(2);
		else
			mesh.push_back(4);
	}

	vector<float> cand_dir;
	for (int j = 0; j < mesh.size(); j++)
	{
		if (j == mesh.size() - 1) //if(j == 11)
		{
			if (mesh[0] + mesh[mesh.size() - 1] == 0)
				cand_dir.push_back(0.0);
		}
		else
		{
			if (mesh[j] + mesh[j + 1] == 0)
				cand_dir.push_back((j + 1)*sector_value);//寻找安全角度，机体坐标系下；即确定波谷
		}
	}

	if (cand_dir.size() != 0) {
		vector<float> delta;
		for (auto &dir_ite : cand_dir) {
			float delte_theta1 = fabs(dir_ite - goal_ori);
			float delte_theta2 = 360 - delte_theta1;
			float delte_theta = delte_theta1 < delte_theta2 ? delte_theta1 : delte_theta2;
			delta.push_back(delte_theta);
		}//寻找接近目标区域的波谷
		int min_index = min_element(delta.begin(), delta.end()) - delta.begin();
		ori = cand_dir.at(min_index);

		ori += heading;
		ori = (int)ori % 360;

		return ori;//确定机头方向
	}

	return -1;
}

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg) {
	init_mask |= 1 << 1;
	current_state = *msg;
}

mavros_msgs::HomePosition home_pos;
void home_pos_cb(const mavros_msgs::HomePosition::ConstPtr& msg) {
	init_mask |= 1 << 2;
	home_pos = *msg;
}

geometry_msgs::PoseStamped local_pos;
Eigen::Vector3d current_local_pos;
bool local_pos_updated = false;
void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
	init_mask |= 1 << 3;
	current_local_pos = mavros::ftf::to_eigen(msg->pose.position);
	local_pos = *msg;
	Point2D pt2d;
	pt2d.x = current_local_pos.x();
	pt2d.y = current_local_pos.y();
	SetUavPosition(pt2d);
	local_pos_updated = true;
}

mavros_msgs::WaypointList waypoints;
void waypoints_cb(const mavros_msgs::WaypointList::ConstPtr& msg) {
	init_mask |= 1 << 4;
	waypoints = *msg;
}

bool scan_updated = false;
void scan_cb(const sensor_msgs::LaserScan::ConstPtr &msg)
{
	//激光雷达数据回调函数
	init_mask |= 1 << 5;
	ComputeMV(msg->ranges);
	scan_updated = true;
}

bool heading_updated = false;
void heading_cb(const std_msgs::Float64::ConstPtr &msg)
{
	init_mask |= 1 << 6;
	SetUavHeading(msg->data);
	heading_updated = true;
}
Eigen::Vector3d current_gps;
void gps_cb(const sensor_msgs::NavSatFix::ConstPtr &msg)
{
	init_mask |= 1;
	current_gps = { msg->latitude, msg->longitude, msg->altitude };
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "collision_avoidance_vfh");
    ros::NodeHandle nh("~");
	scan_distance_max = 4.1;
	scan_distance_min = 0.2;
	angle_resolution = 1.0;
    heading = 90;
	sector_value = 30;
	sector_scale = 10;
	Uavp.x = 0;
	Uavp.y = 0;

    ros::Subscriber gps_sub = nh.subscribe("/mavros/global_position/global",100,gps_cb);
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/mavros/state", 100, state_cb);
    ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 100, local_pos_cb);
    ros::Subscriber lidar_sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 100, scan_cb);
    ros::Subscriber waypoint_sub = nh.subscribe<mavros_msgs::WaypointList>("/mavros/mission/waypoints", 100, waypoints_cb);
    ros::Subscriber homePos_sub = nh.subscribe<mavros_msgs::HomePosition>("/mavros/home_position/home", 100, home_pos_cb);
    ros::Subscriber head_sub = nh.subscribe<std_msgs::Float64>("/mavros/global_position/compass_hdg", 100, heading_cb);

ros::Publisher local_pos_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 100);
    // 频率 [20Hz]
    ros::Rate rate(20.0);
	while (ros::ok() && !current_state.connected) {
		ros::spinOnce();
		rate.sleep();
	}

  printf("Please set the waypoint in QGC before running this program.\n");
	printf("wait a moment\n");

	while (ros::ok())
	{
		//7F 0111 1111
		if (init_mask == 0x7f)
			break;

		ros::spinOnce();
		rate.sleep();
	}
	printf("init ok!\n");

while (ros::ok())
	{
		while (ros::ok() && !current_state.guided )
		{
			ros::spinOnce();
			rate.sleep();
		}
		printf("guild ok\n");
		//**********************************************pose vextor容器***********************************************
		std::vector<geometry_msgs::PoseStamped> pose;
//		printf("wp size=%d\n", waypoints.waypoints.size());
		for (int index = 0; index < waypoints.waypoints.size(); index++)//把gps下的航点信息转成ENU下的期望位置信息
		{
			geometry_msgs::PoseStamped p;
			GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());

			Eigen::Vector3d goal_gps(waypoints.waypoints[index].x_lat, waypoints.waypoints[index].y_long, 0);

			Eigen::Vector3d current_ecef;

			earth.Forward(current_gps.x(), current_gps.y(), current_gps.z(),

				current_ecef.x(), current_ecef.y(), current_ecef.z());

			Eigen::Vector3d goal_ecef;

			earth.Forward(goal_gps.x(), goal_gps.y(), goal_gps.z(),

				goal_ecef.x(), goal_ecef.y(), goal_ecef.z());

			Eigen::Vector3d ecef_offset = goal_ecef - current_ecef;

			Eigen::Vector3d enu_offset = mavros::ftf::transform_frame_ecef_enu(ecef_offset, current_gps);

			Eigen::Affine3d sp;

			Eigen::Quaterniond q;

			q = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX())

				* Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY())

				* Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ());

			sp.translation() = current_local_pos + enu_offset;

			sp.linear() = q.toRotationMatrix();
			//*******************************往vector容器存数据****************************************
			Eigen::Vector3d testv(sp.translation());
			p.pose.position.x = testv[0];
			p.pose.position.y = testv[1];
			printf("%f %f\n", testv[0], testv[1]);
			pose.push_back(p);
		}

		for (int i = 0; i < pose.size(); i++)
		{
			while (ros::ok()) {
				local_pos_updated = false;
				scan_updated = false;
				heading_updated = false;

				while (ros::ok())
				{
					ros::spinOnce();//等待订阅更新完成
					if (local_pos_updated && scan_updated && heading_updated)
						break;
					rate.sleep();
				}

				if (!current_state.guided)
					break;

				if (fabs(local_pos.pose.position.x - pose[i].pose.position.x) < 1.0 &&
					fabs(local_pos.pose.position.y - pose[i].pose.position.y) < 1.0)
				{
					break;//当前位置与期望航点距离相差不到1m,则退出此次航点任务
				}

				Point2D goal;
				goal.x = pose[i].pose.position.x;//读取期望位置
				goal.y = pose[i].pose.position.y;
				float direction = CalculDirection(goal);//根据障碍物的分布与期望位置，得到期望的航向

				if (direction >= -0.5)
				{
					if (direction > 180)
					{
						direction -= 360;
					}
					float arc = 3.1415 / 180 * direction;

					mavros_msgs::PositionTarget pos_target;
					pos_target.coordinate_frame = 1;
					pos_target.type_mask = 1 + 2 + /*4 + 8 + 16 + 32 +*/ 64 + 128 + 256 + 512 + 1024 + 2048;
					pos_target.velocity.x = 0.5* cos(arc);
					pos_target.velocity.y = 0.5* sin(arc);
					pos_target.position.z = desire_z;
					local_pos_pub.publish(pos_target);

					ros::Time last_request = ros::Time::now();
					while (ros::ok()) {
						local_pos_updated = false;
						scan_updated = false;
						heading_updated = false;

						while (ros::ok())
						{
							ros::spinOnce();
							if (local_pos_updated && scan_updated && heading_updated)
								break;
							rate.sleep();
						}

						if (!current_state.guided)
							break;

						if (fabs(local_pos.pose.position.x - pose[i].pose.position.x) < 1.0 &&
							fabs(local_pos.pose.position.y - pose[i].pose.position.y) < 1.0)
						{
							break;
						}

						if (ros::Time::now() - last_request > ros::Duration(4.0))
							break;

						if (IsFrontSafety() == false)
							break;

						local_pos_pub.publish(pos_target);
					}
				}
				else
				{
					mavros_msgs::PositionTarget pos_target;
					pos_target.coordinate_frame = 1;
					pos_target.type_mask = 1 + 2 +/* 4 + 8 + 16 + 32 +*/ 64 + 128 + 256 + 512 + 1024 + 2048;
					pos_target.velocity.x = 0;
					pos_target.velocity.y = 0;
					pos_target.position.z = desire_z;
 					local_pos_pub.publish(pos_target);
				}
			}
		}

		mavros_msgs::PositionTarget pos_target;
		pos_target.coordinate_frame = 1;
		pos_target.type_mask = 1 + 2 + /*4 + 8 + 16 + 32 +*/ 64 + 128 + 256 + 512 + 1024 + 2048;
		pos_target.velocity.x = 0;
		pos_target.velocity.y = 0;
		pos_target.position.z = desire_z;
		local_pos_pub.publish(pos_target);

		printf("task over\n");
		while (ros::ok() && current_state.guided)
		{
			ros::spinOnce();
			rate.sleep();
		}
	}

    return 0;
}


#include <map-optimization-legacy/landmark-covariance-estimation.h>

#include <Eigen/Core>
#include <ceres/ceres.h>
#include <gflags/gflags.h>
#include <maplab-common/threading-helpers.h>
#include <vi-map/landmark-quality-metrics.h>

DEFINE_uint64(
    cov_estimation_min_landmark_observer_count, 4,
    "Minimum required number of observers to estimate landmark"
    " covariance.");
DEFINE_uint64(
    cov_estimation_min_observations_per_frame, 5,
    "Minimum required number of observations per frame.");

namespace map_optimization_legacy {

LandmarkCovarianceEstimation::LandmarkCovarianceEstimation(vi_map::VIMap* map)
    : GraphBaOptimizer(CHECK_NOTNULL(map)) {}

LandmarkCovarianceEstimation::~LandmarkCovarianceEstimation() {}

void LandmarkCovarianceEstimation::assignCovarianceToLandmarks(
    const std::unordered_set<pose_graph::VertexId>& fixed_vertices) {
  addErrorTerms(fixed_vertices);

  ceres::Solver::Options options = getDefaultSolverOptions();
  // We don't need to iterate, but just build the problem.
  options.max_num_iterations = 0;
  static constexpr bool kCopyDataFromSolverBackToMap = true;

  ceres::Problem problem(ceres_error_terms::getDefaultProblemOptions());
  buildProblem(&problem);
  ceres::Solver::Summary summary;
  solve(kCopyDataFromSolverBackToMap, options, &problem, &summary);
  calculateCovariance(&problem);
}

void LandmarkCovarianceEstimation::addErrorTerms(
    const pose_graph::VertexIdSet& fixed_vertices) {
  bool kFixIntrinsics = true;
  bool kFixExtrinsicsRotation = true;
  bool kFixExtrinsicsTranslation = true;
  bool kFixLandmarkPosition = false;
  bool kFixAccelBias = false;
  bool kFixGyroBias = false;
  bool kFixVelocity = false;
  bool kUseGivenEdges = false;
  bool kStoreCachedImuCovariances = false;
  bool kIncludeOnlyMergedLandmarks = false;
  pose_graph::EdgeIdList edges;

  removeLandmarksBehindCamera();
  const size_t kMinNumOfObserverMissions = 0u;
  addVisualResidualBlocks(
      kFixIntrinsics, kFixExtrinsicsRotation, kFixExtrinsicsTranslation,
      kFixLandmarkPosition, kIncludeOnlyMergedLandmarks,
      FLAGS_cov_estimation_min_landmark_observer_count,
      FLAGS_cov_estimation_min_observations_per_frame,
      kMinNumOfObserverMissions);
  BaOptimizationOptions options;
  addInertialResidualBlocks(
      kFixGyroBias, kFixAccelBias, kFixVelocity, kUseGivenEdges, edges,
      kStoreCachedImuCovariances, options.gravity_magnitude, nullptr);

  addPosePriorResidualBlocks(
      fixed_vertices, options.prior_position_std_dev_meters,
      options.prior_orientation_std_dev_radians);
}

void LandmarkCovarianceEstimation::calculateCovariance(
    ceres::Problem* problem) {
  ceres::Covariance::Options covariance_options;
  covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;
  covariance_options.num_threads = common::getNumHardwareThreads();
  covariance_options.min_reciprocal_condition_number = 1e-32;
  covariance_options.apply_loss_function = true;
  ceres::Covariance covariance(covariance_options);

  std::vector<std::pair<const double*, const double*> > covariance_blocks;

  pose_graph::VertexIdList all_vertices;
  const_map_.getAllVertexIds(&all_vertices);

  vi_map::LandmarkIdSet added_landmarks;

  LOG(INFO) << "Adding covariance blocks";
  for (const pose_graph::VertexId& vertex_id : all_vertices) {
    vi_map::Vertex& ba_vertex = map_.getVertex(vertex_id);

    const unsigned int num_frames = ba_vertex.numFrames();
    for (unsigned int frame_idx = 0; frame_idx < num_frames; ++frame_idx) {
      const Eigen::Matrix2Xd& image_points_distorted =
          ba_vertex.getVisualFrame(frame_idx).getKeypointMeasurements();
      for (int i = 0; i < image_points_distorted.cols(); ++i) {
        vi_map::LandmarkId landmark_id =
            ba_vertex.getObservedLandmarkId(frame_idx, i);
        if (landmark_id.isValid()) {
          vi_map::Landmark& landmark = map_.getLandmark(landmark_id);
          CHECK_EQ(landmark.id(), landmark_id);

          if (added_landmarks.count(landmark.id()) == 0) {
            if (landmark.numberOfObservations() >
                    FLAGS_cov_estimation_min_landmark_observer_count &&
                vi_map::isLandmarkWellConstrained(map_, landmark)) {
              // Check if the position block was really added to the problem.
              // If not, there must be some inconsistency in choosing the
              // well-constrained landmarks.
              CHECK(problem->HasParameterBlock(landmark.get_p_B_Mutable()));
              covariance_blocks.push_back(
                  std::make_pair(
                      landmark.get_p_B_Mutable(), landmark.get_p_B_Mutable()));

              added_landmarks.insert(landmark.id());
            } else {
              // Not enough landmarks, let's put something big to covariance
              // diagonal.
              Eigen::Matrix3d some_large_covariance =
                  Eigen::Matrix3d::Identity();
              some_large_covariance *= 100;
              landmark.set_p_B_Covariance(some_large_covariance);
            }
          }
        }
      }
    }  // Loop over all frames in a vertex.
  }    // Loop over all vertices.

  LOG(INFO) << "Calculating covariance.";
  CHECK(covariance.Compute(covariance_blocks, problem));

  LOG(INFO) << "Storing covariance in landmark objects.";
  for (const vi_map::LandmarkId& landmark_id : added_landmarks) {
    vi_map::Landmark& landmark = map_.getLandmark(landmark_id);
    Eigen::Matrix<double, 3, 3, Eigen::RowMajor> position_covariance;

    covariance.GetCovarianceBlock(
        landmark.get_p_B_Mutable(), landmark.get_p_B_Mutable(),
        position_covariance.data());
    landmark.set_p_B_Covariance(position_covariance);
  }
}

}  // namespace map_optimization_legacy


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_CORE_FUNCTIONS_SIMD_INNER_FOLD_HPP_INCLUDED
#define NT2_CORE_FUNCTIONS_SIMD_INNER_FOLD_HPP_INCLUDED

#include <nt2/core/functions/inner_fold.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/meta/is_vectorizable.hpp>

#ifndef BOOST_SIMD_NO_SIMD
namespace nt2 { namespace ext
{
  //============================================================================
  // Generates inner_fold
  //============================================================================
  NT2_FUNCTOR_IMPLEMENTATION_IF( nt2::tag::inner_fold_, boost::simd::tag::simd_, (A0)(S0)(A1)(A2)(A3)(A4)
                               , (boost::simd::meta::is_vectorizable<typename A0::value_type, BOOST_SIMD_DEFAULT_EXTENSION>)
                               , ((expr_< table_< unspecified_<A0>, S0 >
                                        , nt2::tag::terminal_
                                        , boost::mpl::long_<0>
                                        >
                                 ))
                                 ((ast_< A1, nt2::container::domain>))
                                 (unspecified_<A2>)
                                 (unspecified_<A3>)
                                 (unspecified_<A4>)
                               )
  {
    typedef void                                                              result_type;
    typedef typename A0::value_type                                           value_type;
    typedef typename A1::extent_type                                          extent_type;
    typedef boost::simd::native<value_type,BOOST_SIMD_DEFAULT_EXTENSION>      target_type;

    BOOST_FORCEINLINE result_type operator()(A0& out, A1& in, A2 const& neutral, A3 const& bop, A4 const& uop ) const
    {
      extent_type ext = in.extent();
      static const std::size_t N = boost::simd::meta::cardinal_of<target_type>::value;
      std::size_t bound  = boost::fusion::at_c<0>(ext);
      std::size_t ibound = (boost::fusion::at_c<0>(ext)/N) * N;
      std::size_t obound = nt2::numel(boost::fusion::pop_front(ext));
      target_type vec_out;

      for(std::size_t j = 0, k = 0; j != obound; ++j, k+=bound)
      {
        vec_out = neutral(nt2::meta::as_<target_type>());

        for(std::size_t i = 0; i != ibound; i+=N)
          vec_out = bop(vec_out, nt2::run(in, i+k, meta::as_<target_type>()));

        value_type s_out = uop(vec_out);

        for(std::size_t i = ibound; i != bound; ++i)
          s_out = bop(s_out, nt2::run(in, i+k, meta::as_<value_type>()));

        nt2::run(out, j, s_out);

      }
    }

  };

  } }

#endif
#endif


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

#include <boost/multi_array.hpp>

namespace poplibs_test {
namespace embedding {

void multiSlice(const boost::multi_array<double, 2> &embeddingMatrix,
                const std::vector<unsigned> &indices,
                boost::multi_array<double, 2> &result);

void multiUpdateAdd(const boost::multi_array<double, 2> &deltas,
                    const std::vector<unsigned> &indices, const double scale,
                    boost::multi_array<double, 2> &embeddingMatrix);

} // namespace embedding
} // namespace poplibs_test


#ifndef PYTHONIC_TYPES_NUMPY_IEXPR_HPP
#define PYTHONIC_TYPES_NUMPY_IEXPR_HPP

#include "pythonic/types/nditerator.hpp"
#include "pythonic/types/tuple.hpp"

#ifdef USE_BOOST_SIMD
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/include/functions/store.hpp>
#endif

#include <numeric>

namespace pythonic {

    namespace types {

        template<class Arg, class... S>
            struct numpy_gexpr;
        template<class Arg, class F>
            struct numpy_fexpr;

        /* Expression template for numpy expressions - indexing
         */
        template<class T, size_t N>
            struct numpy_iexpr_helper;

        template<class Arg> // Arg often is a reference, e.g. for something as simple as a[i]
            struct numpy_iexpr {
                // wrapper around another numpy expression to skip first dimension using a given value.
                static constexpr size_t value = std::remove_reference<Arg>::type::value - 1;
                static const bool is_vectorizable = std::remove_reference<Arg>::type::is_vectorizable;
                typedef typename std::remove_reference<Arg>::type::dtype dtype;
                typedef typename std::remove_reference<decltype(numpy_iexpr_helper<numpy_iexpr, value>::get(std::declval<numpy_iexpr>(), 0L))>::type value_type;

                static constexpr bool is_strided = std::remove_reference<Arg>::type::is_strided;

                typedef typename std::conditional<
                  is_strided or value != 1,
                  nditerator<numpy_iexpr>,
                  dtype*
                >::type iterator;
                typedef typename std::conditional<
                  is_strided or value != 1,
                  const_nditerator<numpy_iexpr>,
                  dtype const*
                >::type const_iterator;

                Arg arg;
                dtype* buffer;
                array<long, value> shape;

                numpy_iexpr() {}
                numpy_iexpr(numpy_iexpr const &) = default;
                numpy_iexpr(numpy_iexpr&&) = default;

                template<class Argp> // not using the default one, to make it possible to accept reference and non reference version of Argp
                    numpy_iexpr(numpy_iexpr<Argp> const & other) :
                        arg(other.arg),
                        buffer(other.buffer),
                        shape(other.shape)
                {
                }

                numpy_iexpr(Arg const & arg, long index) : arg(arg), buffer(arg.buffer)
                {
                    buffer += buffer_offset(index, utils::int_<value>());
                }
                // force the move. Using universal reference here does not work (because of reference collapsing ?)
                numpy_iexpr(typename std::remove_reference<Arg>::type &&arg, long index) : arg(std::move(arg)), buffer(arg.buffer)
                {
                    buffer += buffer_offset(index, utils::int_<value>());
                }

                template<class E>
                numpy_iexpr& operator=(E const& expr) {
                    return utils::broadcast_copy<numpy_iexpr&, E, value, value - utils::dim_of<E>::value, false/*NIY*/>(*this, expr);
                }
                numpy_iexpr& operator=(numpy_iexpr const& expr) {
                    return utils::broadcast_copy<numpy_iexpr&, numpy_iexpr const &, value, value - utils::dim_of<numpy_iexpr>::value, false/*NIY*/>(*this, expr);
                }
                template<class E>
                numpy_iexpr& operator+=(E const& expr) {
                    return (*this) = (*this) + expr;
                }
                numpy_iexpr& operator+=(numpy_iexpr const& expr) {
                    return (*this) = (*this) + expr;
                }
                template<class E>
                numpy_iexpr& operator-=(E const& expr) {
                    return (*this) = (*this) - expr;
                }
                numpy_iexpr& operator-=(numpy_iexpr const& expr) {
                    return (*this) = (*this) - expr;
                }
                template<class E>
                numpy_iexpr& operator*=(E const& expr) {
                    return (*this) = (*this) * expr;
                }
                numpy_iexpr& operator*=(numpy_iexpr const& expr) {
                    return (*this) = (*this) * expr;
                }
                template<class E>
                numpy_iexpr& operator/=(E const& expr) {
                    return (*this) = (*this) / expr;
                }
                numpy_iexpr& operator/=(numpy_iexpr const& expr) {
                    return (*this) = (*this) / expr;
                }

                const_iterator begin() const { return make_const_nditerator<is_strided or value != 1>()(*this, 0); }
                const_iterator end() const { return make_const_nditerator<is_strided or value != 1>()(*this, shape[0]); }

                iterator begin() { return make_nditerator<is_strided or value != 1>()(*this, 0); }
                iterator end() { return make_nditerator<is_strided or value != 1>()(*this, shape[0]); }

                dtype const * fbegin() const { return buffer; }
                dtype const * fend() const { return buffer + flat_size(); }

                dtype * fbegin() { return buffer; }
                dtype const * fend() { return buffer + flat_size(); }

                /* There are three kind of indexing operator: fast(long), [long] and (long):
                 * - fast does not perform automatic bound wrapping
                 * - [] performs automatic bound wrapping, hen forwards to fast
                 * - () is an alias to [] and directly forwards to []
                 *
                 * For each indexing operator, we have three variant: &, const& and &&:
                 * - & means the numpy_iexpr has been bound to a non-const value, as in ``b=a[i] ; print b[j]``
                 *   in that case the return type if the dim of a is 2 is a reference, to allow ``b[j] = 1``
                 * - const & means the numpy_iexpr has been bound to a const value, as in ``np.copy(a[i])``
                 *   in that case the return type if the dim of a is 2 is a value (or const ref)
                 * - && means the numpy_iexpr is a r-value, which happens a lot, as in ``a[i][j]``
                 *   in that case the return type if the dim of a is 2 is a reference.
                 *   It is a bit weird because we return a refrence from a rvalue, but the reference is bound to
                 *   the buffer of ``a`` that is not temp.
                 */
                auto fast(long i) const &-> decltype(numpy_iexpr_helper<numpy_iexpr, value>::get(*this, i)) {
                    return numpy_iexpr_helper<numpy_iexpr, value>::get(*this, i);
                }
                auto fast(long i) &-> decltype(numpy_iexpr_helper<numpy_iexpr, value>::get(*this, i)) {
                    return numpy_iexpr_helper<numpy_iexpr, value>::get(*this, i);
                }
                auto fast(long i) &&-> decltype(numpy_iexpr_helper<numpy_iexpr, value>::get(std::move(*this), i)) {
                    return numpy_iexpr_helper<numpy_iexpr, value>::get(std::move(*this), i);
                }
#ifdef USE_BOOST_SIMD
                template<class I>
                auto load(I i) const
                -> decltype(boost::simd::load<boost::simd::native<dtype, BOOST_SIMD_DEFAULT_EXTENSION>>(this->buffer, i))
                {
                  typedef dtype T;
                  typedef typename boost::simd::native<T, BOOST_SIMD_DEFAULT_EXTENSION> vT;
                  return boost::simd::load<vT>(buffer, i);
                }
                template<class V>
                void store(V &&v, long i) {
                  boost::simd::store(v, buffer, i);
                }
#endif
                auto operator[](long i) const &-> decltype(this->fast(i)) {
                    if(i<0) i += shape[0];
                    return fast(i);
                }
                auto operator[](long i) &-> decltype(this->fast(i)) {
                    if(i<0) i += shape[0];
                    return fast(i);
                }
                auto operator[](long i) && -> decltype(std::move(*this).fast(i)) {
                    if(i<0) i += shape[0];
                    return std::move(*this).fast(i);
                }
                auto operator()(long i) const &-> decltype((*this)[i]) {
                    return (*this)[i];
                }
                auto operator()(long i) &-> decltype((*this)[i]) {
                    return (*this)[i];
                }
                auto operator()(long i) &&-> decltype(std::move(*this)[i]) {
                    return std::move(*this)[i];
                }
                numpy_gexpr<numpy_iexpr, slice> operator()(slice const& s0) const
                {
                    return make_gexpr(*this, s0);
                }
                numpy_gexpr<numpy_iexpr, slice> operator[](slice const& s0) const
                {
                    return make_gexpr(*this, s0);
                }
                numpy_gexpr<numpy_iexpr, contiguous_slice> operator()(contiguous_slice const& s0) const
                {
                    return make_gexpr(*this, s0);
                }
                numpy_gexpr<numpy_iexpr, contiguous_slice> operator[](contiguous_slice const& s0) const
                {
                    return make_gexpr(*this, s0);
                }
                template<class ...S>
                    numpy_gexpr<numpy_iexpr, slice, S...> operator()(slice const& s0, S const&... s) const
                    {
                        return make_gexpr(*this, s0, s...);
                    }
                template<class ...S>
                    numpy_gexpr<numpy_iexpr, contiguous_slice, S...> operator()(contiguous_slice const& s0, S const&... s) const
                    {
                        return make_gexpr(*this, s0, s...);
                    }
                template<class ...S>
                    auto operator()(long s0, S const&... s) const -> decltype(std::declval<numpy_iexpr<numpy_iexpr>>()(s...))
                    {
                        return (*this)[s0](s...);
                    }
                template<class F>
                    typename std::enable_if<is_numexpr_arg<F>::value, numpy_fexpr<numpy_iexpr, F>>::type
                    fast(F const& filter) const {
                        return numpy_fexpr<numpy_iexpr, F>(*this, filter);
                    }
                template<class F>
                    typename std::enable_if<is_numexpr_arg<F>::value, numpy_fexpr<numpy_iexpr, F>>::type
                    operator[](F const& filter) const {
                        return fast(filter);
                    }

                dtype const &operator[](array<long, value> const& indices) const
                {
                    size_t offset = indices[value-1];
                    long mult = shape[value-1];
                    for(size_t i = value - 2; i > 0; --i) {
                        offset +=  indices[i] * mult;
                        mult *= shape[i];
                    }
                    return buffer[offset + indices[0] * mult];
                }
                dtype& operator[](array<long, value> const& indices)
                {
                    return const_cast<dtype&>(const_cast<numpy_iexpr const&>(*this)[indices]);
                }

                long flat_size() const { return std::accumulate(shape.begin() + 1, shape.end(), *shape.begin(), std::multiplies<long>()); }

                private:

                /* compute the buffer offset, returning the offset between the
                 * first element of the iexpr and the start of the buffer.
                 * This used to be a plain loop, but g++ fails to unroll it, while it unrolls it with the template version...
                 */
                long buffer_offset(long index, utils::int_<0>) { return index;
                }

                template<size_t N>
                long buffer_offset(long index, utils::int_<N>) {
                    shape[value - N] = arg.shape[value - N + 1];
                    return buffer_offset(index * arg.shape[value - N + 1], utils::int_<N - 1>());
                }

            };

        // Indexing an numpy_iexpr that has a dimension greater than one yields a new numpy_iexpr
        template <class T, size_t N>
            struct numpy_iexpr_helper {
                static numpy_iexpr<T> get(T const& e, long i) {
                    return numpy_iexpr<T>(e, i);
                }
            };

        // Indexing an iexpr that has a dimension of one yields a qualified scalar. The qualifier is either:
        // - a reference if the numpy_iexpr is a ref itself, as in ``b = a[i] ; b[i] = 1``
        // - a reference if the numpy_iexpr is a r-value, as in ``a[i][j] = 1``
        // - a value if the numpy_iexpr is a const ref, as in ``b = a[i] ; c = b[i]``
        template <class T>
            struct numpy_iexpr_helper<T, 1> {
                static typename T::dtype get(T const & e, long i)
                {
                    return e.buffer[i];
                }
                static typename T::dtype & get(T && e, long i)
                {
                    return e.buffer[i];
                }
                static typename T::dtype & get(T & e, long i)
                {
                    return e.buffer[i];
                }
            };

    }

    template<class Arg>
        struct assignable<types::numpy_iexpr<Arg>>
        {
            typedef types::numpy_iexpr<typename assignable<Arg>::type> type;
        };
    template<class T, size_t N>
        struct assignable<types::numpy_iexpr<types::ndarray<T,N> &>>
        {
            typedef types::numpy_iexpr<types::ndarray<T,N> &> type;
        };
    template<class T, size_t N>
        struct assignable<types::numpy_iexpr<types::ndarray<T,N> const &>>
        {
            typedef types::numpy_iexpr<types::ndarray<T,N> const &> type;
        };

    template<class Arg>
        struct returnable<types::numpy_iexpr<Arg>>
        {
            typedef types::numpy_iexpr<typename returnable<Arg>::type> type;
        };
    template<class Arg>
        struct lazy<types::numpy_iexpr<Arg>>
        {
            typedef types::numpy_iexpr<typename lazy<Arg>::type> type;
        };
    template<class Arg>
        struct lazy<types::numpy_iexpr<Arg const &>>
        {
            typedef types::numpy_iexpr<typename lazy<Arg>::type const &> type;
        };

}
/* type inference stuff  {*/
#include "pythonic/types/combined.hpp"
template<class E, class K>
struct __combined<pythonic::types::numpy_iexpr<E>, K> {
    typedef pythonic::types::numpy_iexpr<E> type;
};

#endif


#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf_conversions/tf_eigen.h>
#include <std_srvs/Empty.h>
#include <geometry_msgs/Twist.h>

#include <Eigen/Dense>

#include <crazyflie_controller/ExecuteTrajectoryAction.h>
#include <actionlib/server/simple_action_server.h>

#include <crazyflie_driver/GenericLogData.h>

#include "pid.hpp"

double get(
    const ros::NodeHandle& n,
    const std::string& name) {
    double value;
    n.getParam(name, value);
    return value;
}

class Controller
{
public:

    Controller(
        const std::string& worldFrame,
        const std::string& frame,
        const ros::NodeHandle& n)
        : m_worldFrame(worldFrame)
        , m_frame(frame)
        , m_pubNav()
        , m_listener()
        , m_pidYaw(
            get(n, "PIDs/Yaw/kp"),
            get(n, "PIDs/Yaw/kd"),
            get(n, "PIDs/Yaw/ki"),
            get(n, "PIDs/Yaw/minOutput"),
            get(n, "PIDs/Yaw/maxOutput"),
            get(n, "PIDs/Yaw/integratorMin"),
            get(n, "PIDs/Yaw/integratorMax"),
            "yaw",
            true)
        , m_state(Idle)
        , m_subscribeStabilizer()
        // , m_goal()
        // , m_subscribeGoal()
        , m_serviceTakeoff()
        , m_serviceLand()
        , m_actionServerExecuteTrajectory(nullptr)
        , m_thrust(0)
        , m_kp(get(n, "PIDs/Body/kp"))
        , m_kd(get(n, "PIDs/Body/kd"))
        , m_ki(get(n, "PIDs/Body/ki"))
        , m_oldPosition(0,0,0)
        , m_current_r_error_integration(0,0,0)
        , m_massThrust(get(n, "MassThrust"))
        , m_maxAngle(get(n, "MaxAngle"))
        , m_mass(get(n, "mass"))
        , m_roll(0)
        , m_pitch(0)
        , m_yaw(0)
    {
        ros::NodeHandle nh;
        m_pubNav = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
        // m_subscribeGoal = nh.subscribe("goal", 1, &Controller::goalChanged, this);
        // ToDo switch to actions!
        m_serviceTakeoff = nh.advertiseService("takeoff", &Controller::takeoff, this);
        m_serviceLand = nh.advertiseService("land", &Controller::land, this);
        // m_serviceGoTo = nh.advertiseService("go_to", &Controller::go_to, this);

        m_subscribeStabilizer = nh.subscribe("/crazyflie/stabilizer", 1, &Controller::stabilizerChanged, this);

        m_actionServerExecuteTrajectory = new actionlib::SimpleActionServer<crazyflie_controller::ExecuteTrajectoryAction>(
            nh, "execute_trajectory",
            std::bind(&Controller::executeTrajectory, this, std::placeholders::_1),
            true);
    }

    ~Controller()
    {
        delete m_actionServerExecuteTrajectory;
    }

    void run(double frequency)
    {
        ros::NodeHandle node;
        ros::Timer timer = node.createTimer(ros::Duration(1.0/frequency), &Controller::iteration, this);
        ros::spin();
    }

private:
    // void goalChanged(
    //     const geometry_msgs::PoseStamped::ConstPtr& msg)
    // {
    //     m_goal = *msg;
    // }

    void stabilizerChanged(
        const crazyflie_driver::GenericLogData::ConstPtr& msg)
    {
        m_roll = -msg->values[0] / 180 * M_PI;
        m_pitch = msg->values[1] / 180 * M_PI;
        m_yaw = msg->values[2] / 180 * M_PI;
    }

    bool takeoff(
        std_srvs::Empty::Request& req,
        std_srvs::Empty::Response& res)
    {
        ROS_INFO("Takeoff requested!");
        m_state = TakingOff;

        return true;
    }

    bool land(
        std_srvs::Empty::Request& req,
        std_srvs::Empty::Response& res)
    {
        ROS_INFO("Landing requested!");
        m_state = Landing;

        return true;
    }

    bool executeTrajectory(
        const crazyflie_controller::ExecuteTrajectoryGoalConstPtr& goal)
    {
        ROS_INFO("execute trajectory requested!");

        m_trajectory = goal->trajectory;
        // m_startTime = ros::Time::now();

        // m_actionServerExecuteTrajectory->setSucceeded();
    }

    void getCurrentTrajectoryPoint(
        crazyflie_controller::QuadcopterTrajectoryPoint& result)
    {
        // TODO: we could also linearely interpolate here!
        ros::Duration d = ros::Time::now() - m_trajectory.header.stamp;
        for (auto& pt : m_trajectory.points) {
            if (pt.time_from_start > d) {
                result = pt;
                return;
            }
        }
        result = m_trajectory.points.back();
    }

    void getTransform(
        const std::string& sourceFrame,
        const std::string& targetFrame,
        tf::StampedTransform& result)
    {
        m_listener.lookupTransform(sourceFrame, targetFrame, ros::Time(0), result);
    }

    void pidReset()
    {
        m_current_r_error_integration = Eigen::Vector3d(0,0,0);
        m_pidYaw.reset();
    }

    void iteration(const ros::TimerEvent& e)
    {
        float dt = e.current_real.toSec() - e.last_real.toSec();

        switch(m_state)
        {
        case TakingOff:
            {
                tf::StampedTransform transform;
                m_listener.lookupTransform(m_worldFrame, m_frame, ros::Time(0), transform);
                if (transform.getOrigin().z() > 0.05 || m_thrust > 50000)
                {
                    pidReset();
                    m_state = Automatic;
                    m_thrust = 0;
                    m_trajectory.header.stamp = ros::Time::now();
                    m_trajectory.points.clear();
                    crazyflie_controller::QuadcopterTrajectoryPoint pt;
                    pt.position.z = 0.5;
                    m_trajectory.points.push_back(pt);
                }
                else
                {
                    m_thrust += 10000 * dt;
                    geometry_msgs::Twist msg;
                    msg.linear.z = m_thrust;
                    m_pubNav.publish(msg);
                }

            }
            break;
        case Landing:
            {
                // m_goal.pose.position.z = 0.05;
                m_trajectory.header.stamp = ros::Time::now();
                m_trajectory.points.clear();
                crazyflie_controller::QuadcopterTrajectoryPoint pt;
                pt.position.z = 0.05;
                m_trajectory.points.push_back(pt);

                tf::StampedTransform transform;
                m_listener.lookupTransform(m_worldFrame, m_frame, ros::Time(0), transform);
                if (transform.getOrigin().z() <= 0.05) {
                    m_state = Idle;
                    geometry_msgs::Twist msg;
                    m_pubNav.publish(msg);
                }
            }
            // intentional fall-thru
        case Automatic:
            {
                tf::StampedTransform tf_transform;
                m_listener.lookupTransform(m_worldFrame, m_frame, ros::Time(0), tf_transform);

                // CURRENT STATES
                Eigen::Affine3d transform;
                tf::transformTFToEigen(tf_transform, transform);

                // Current position
                Eigen::Vector3d current_position = transform.translation();

                // Current velocity
                Eigen::Vector3d current_velocity = (current_position - m_oldPosition) / dt;
                m_oldPosition = current_position;

                // Current Orientation
                // see m_roll, m_pitch, m_yaw

                // Current angular velocity
                //Eigen::Vector3d current_angular_velocity(
                //    m_imu.angular_velocity.x,
                //    m_imu.angular_velocity.y,
                //    m_imu.angular_velocity.z
                //);

                //DESIRED STATES
                crazyflie_controller::QuadcopterTrajectoryPoint trajectoryPoint;
                getCurrentTrajectoryPoint(trajectoryPoint);

                // Desired position
                Eigen::Vector3d target_position(
                    trajectoryPoint.position.x,
                    trajectoryPoint.position.y,
                    trajectoryPoint.position.z);

                //Desired velocity
                Eigen::Vector3d target_velocity(
                    trajectoryPoint.velocity.x,
                    trajectoryPoint.velocity.y,
                    trajectoryPoint.velocity.z);

                //Desired acceleration
                Eigen::Vector3d target_acceleration(
                    trajectoryPoint.acceleration.x,
                    trajectoryPoint.acceleration.y,
                    trajectoryPoint.acceleration.z);

                //Desired yaw
                double target_yaw = trajectoryPoint.yaw;

                // set this to 0 because we don't want to rotate during the flight
                Eigen::Vector3d target_angular_velocity(0, 0, 0);

                //CALCULATE THRUST

                // Position Error
                Eigen::Vector3d current_r_error = target_position - current_position;
                // Velocity Error
                Eigen::Vector3d current_v_error = target_velocity - current_velocity;
                // Desired thrust
                Eigen::Vector3d target_thrust = m_kp*current_r_error + m_kd*current_v_error + m_mass * target_acceleration + m_mass * Eigen::Vector3d(0,0,9.81);
                // Current z_axis
                Eigen::Vector3d current_z_axis( -sin(m_pitch)*cos(m_roll),
                                            sin(m_roll),
                                            cos(m_pitch)*cos(m_roll));
                // Current thrust
                double current_thrust = target_thrust.dot(current_z_axis) * m_massThrust;
                ROS_INFO("%f", current_thrust);

                // CALCULATE AXIS
                // // Desired z_axis
                Eigen::Vector3d z_axis_desired = target_thrust/target_thrust.norm();
                // // Desired x_center_axis
                // Eigen::Vector3d x_center_axis_desired = Eigen::Vector3d(sin(target_yaw), cos(target_yaw), 0);
                // // Desired y_axis
                // Eigen::Vector3d y_axis_desired = z_axis_desired.cross(x_center_axis_desired);
                // // Desired x_axis
                // Eigen::Vector3d x_axis_desired = y_axis_desired.cross(z_axis_desired);

                Eigen::Vector3d x_axis_desired = z_axis_desired.cross(Eigen::Vector3d(sin(target_yaw), cos(target_yaw), 0));
                //x_axis_desired.normalize();
                Eigen::Vector3d y_axis_desired = z_axis_desired.cross(x_axis_desired);

                // CONTROL

                tfScalar current_euler_roll, current_euler_pitch, current_euler_yaw;
                tf::Matrix3x3(tf_transform.getRotation()).getRPY(
                    current_euler_roll,
                    current_euler_pitch,
                    current_euler_yaw);

                double thrust = current_thrust;//z_axis_desired.dot(current_z_axis);
                if (thrust < 0) {
                    thrust = 0;
                }
                if (thrust > 65536) {
                    thrust = 65536;
                }

                double pitch_angle = asin(x_axis_desired[2]) * 180.0 / M_PI;
                double yaw_angle = target_yaw;
                // double yaw_angle = atan2(x_axis_desired.getY(), x_axis_desired.getX());
                double roll_angle = atan2(y_axis_desired[2], z_axis_desired[2]) * 180.0 / M_PI;

                geometry_msgs::Twist msg;
                msg.linear.x = pitch_angle;
                msg.linear.y = -roll_angle;
                msg.linear.z = thrust;
                msg.angular.z = m_pidYaw.update(current_euler_yaw, yaw_angle);
                m_pubNav.publish(msg);

            }
            break;
        case Idle:
            {
                geometry_msgs::Twist msg;
                m_pubNav.publish(msg);
            }
            break;
        }
    }

private:

    enum State
    {
        Idle = 0,
        Automatic = 1,
        TakingOff = 2,
        Landing = 3,
    };

private:
    std::string m_worldFrame;
    std::string m_frame;
    ros::Publisher m_pubNav;
    tf::TransformListener m_listener;
    PID m_pidYaw;
    State m_state;
    // geometry_msgs::PoseStamped m_goal;
    // ros::Subscriber m_subscribeGoal;
    ros::Subscriber m_subscribeStabilizer;
    ros::ServiceServer m_serviceTakeoff;
    ros::ServiceServer m_serviceLand;
    actionlib::SimpleActionServer<crazyflie_controller::ExecuteTrajectoryAction>* m_actionServerExecuteTrajectory;
    float m_thrust;

    double m_kp;
    double m_kd;
    double m_ki;
    Eigen::Vector3d m_oldPosition;
    Eigen::Vector3d m_current_r_error_integration;
    double m_massThrust;
    double m_maxAngle;
    double m_mass;

    crazyflie_controller::QuadcopterTrajectory m_trajectory;

    double m_roll;
    double m_pitch;
    double m_yaw;

    // ros::time m_startTime;
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "controller");

  // Read parameters
  ros::NodeHandle n("~");
  std::string worldFrame;
  n.param<std::string>("worldFrame", worldFrame, "/world");
  std::string frame;
  n.getParam("frame", frame);
  double frequency;
  n.param("frequency", frequency, 50.0);

  Controller controller(worldFrame, frame, n);
  controller.run(frequency);

  return 0;
}


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

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_GELSD_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_GELSD_HPP

#include <algorithm>

#include <boost/numeric/bindings/traits/type.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>
#include <boost/numeric/bindings/traits/detail/utils.hpp>
#include <boost/numeric/bindings/lapack/ilaenv.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

    namespace lapack {

        namespace detail {

            inline void gelsd(const integer_t m, const integer_t n, const integer_t nrhs,
                              float *a, const integer_t lda, float *b, const integer_t ldb,
                              float *s, const float rcond, integer_t *rank, float *work,
                              const integer_t lwork, integer_t *iwork, integer_t *info)
            {
                LAPACK_SGELSD(&m, &n, &nrhs, a, &lda, b, &ldb, s,
                              &rcond, rank, work, &lwork, iwork, info);
            }

            inline void gelsd(const integer_t m, const integer_t n, const integer_t nrhs,
                              double *a, const integer_t lda, double *b, const integer_t ldb,
                              double *s, const double rcond, integer_t *rank, double *work,
                              const integer_t lwork, integer_t *iwork, integer_t *info)
            {
                LAPACK_DGELSD(&m, &n, &nrhs, a, &lda, b, &ldb, s,
                              &rcond, rank, work, &lwork, iwork, info);
            }

            inline void gelsd(const integer_t m, const integer_t n, const integer_t nrhs,
                              traits::complex_f *a, const integer_t lda, traits::complex_f *b,
                              const integer_t ldb, float *s, const float rcond, integer_t *rank,
                              traits::complex_f *work, const integer_t lwork, float *rwork,
                              integer_t *iwork, integer_t *info)
            {
                LAPACK_CGELSD(&m, &n, &nrhs, traits::complex_ptr(a),
                              &lda, traits::complex_ptr(b), &ldb, s,
                              &rcond, rank, traits::complex_ptr(work),
                              &lwork, rwork, iwork, info);
            }

            inline void gelsd(const integer_t m, const integer_t n, const integer_t nrhs,
                              traits::complex_d *a, const integer_t lda, traits::complex_d *b,
                              const integer_t ldb, double *s, const double rcond, integer_t *rank,
                              traits::complex_d *work, const integer_t lwork, double *rwork,
                              integer_t *iwork, integer_t *info)
            {
                LAPACK_ZGELSD(&m, &n, &nrhs, traits::complex_ptr(a),
                              &lda, traits::complex_ptr(b), &ldb, s,
                              &rcond, rank, traits::complex_ptr(work),
                              &lwork, rwork, iwork, info);
            }

            // gelsd for real type
            template <typename MatrA, typename MatrB, typename VecS, typename Work>
            int gelsd(MatrA& A, MatrB& B, VecS& s, Work& work)
            {
                typedef typename MatrA::value_type val_t;
                typedef typename traits::type_traits<val_t>::real_type real_t;

                const std::ptrdiff_t m = traits::matrix_size1(A);
                const std::ptrdiff_t n = traits::matrix_size2(A);
                const std::ptrdiff_t nrhs = traits::matrix_size2(B);
                const std::ptrdiff_t maxmn = std::max(m, n);
                const std::ptrdiff_t minmn = std::min(m, n);

                // sanity checks
                assert(m >= 0 && n >= 0);
                assert(nrhs >= 0);
                assert(traits::leading_dimension(A) >= std::max<std::ptrdiff_t>(1, m));
                assert(traits::leading_dimension(B) >= std::max<std::ptrdiff_t>(1, maxmn));
                assert(traits::vector_size(work) >= 1);
                assert(traits::vector_size(s) >= std::max<std::ptrdiff_t>(1, minmn));

                integer_t info;
                const real_t rcond = -1;    // use machine precision
                integer_t rank;

                // query for maximum size of subproblems
                const integer_t smlsiz = ilaenv(9, "GELSD", "");
                const integer_t nlvl = static_cast<integer_t>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

                traits::detail::array<integer_t> iwork(3*minmn*nlvl + 11*minmn);

                detail::gelsd(traits::matrix_size1(A),
                              traits::matrix_size2(A),
                              traits::matrix_size2(B),
                              traits::matrix_storage(A),
                              traits::leading_dimension(A),
                              traits::matrix_storage(B),
                              traits::leading_dimension(B),
                              traits::vector_storage(s),
                              rcond,
                              &rank,
                              traits::vector_storage(work),
                              traits::vector_size(work),
                              traits::vector_storage(iwork),
                              &info);

                return info;
            }

            // gelsd for complex type
            template <typename MatrA, typename MatrB, typename VecS,
                        typename Work, typename RWork>
            int gelsd(MatrA& A, MatrB& B, VecS& s, Work& work, RWork& rwork)
            {
                typedef typename MatrA::value_type val_t;
                typedef typename traits::type_traits<val_t>::real_type real_t;

                const std::ptrdiff_t m = traits::matrix_size1(A);
                const std::ptrdiff_t n = traits::matrix_size2(A);
                const std::ptrdiff_t nrhs = traits::matrix_size2(B);
                const std::ptrdiff_t maxmn = std::max(m, n);
                const std::ptrdiff_t minmn = std::min(m, n);

                // sanity checks
                assert(m >= 0 && n >= 0);
                assert(nrhs >= 0);
                assert(traits::leading_dimension(A) >= std::max<std::ptrdiff_t>(1, m));
                assert(traits::leading_dimension(B) >= std::max<std::ptrdiff_t>(1, maxmn));
                assert(traits::vector_size(work) >= 1);
                assert(traits::vector_size(s) >= std::max<std::ptrdiff_t>(1, minmn));

                integer_t info;
                const real_t rcond = -1;    // use machine precision
                integer_t rank;

                // query for maximum size of subproblems
                const integer_t smlsiz = ilaenv(9, "GELSD", "");
                const integer_t nlvl = static_cast<integer_t>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

                traits::detail::array<integer_t> iwork(3*minmn*nlvl + 11*minmn);

                detail::gelsd(traits::matrix_size1(A),
                    traits::matrix_size2(A),
                    traits::matrix_size2(B),
                    traits::matrix_storage(A),
                    traits::leading_dimension(A),
                    traits::matrix_storage(B),
                    traits::leading_dimension(B),
                    traits::vector_storage(s),
                    rcond,
                    &rank,
                    traits::vector_storage(work),
                    traits::vector_size(work),
                    traits::vector_storage(rwork),
                    traits::vector_storage(iwork),
                    &info);

                return info;
            }

            template <int N>
            struct Gelsd { };

            // specialization for gelsd real flavors (sgelsd, dgelsd)
            template <>
            struct Gelsd<1>
            {
                template <typename MatrA, typename MatrB, typename VecS>
                int operator() (MatrA& A, MatrB& B, VecS& s, minimal_workspace) const
                {
                    typedef typename MatrA::value_type val_t;

                    const std::ptrdiff_t m = traits::matrix_size1(A);
                    const std::ptrdiff_t n = traits::matrix_size2(A);
                    const std::ptrdiff_t nrhs = traits::matrix_size2(B);

                    const std::ptrdiff_t minmn = std::min(m, n);           // minmn = m < n ? m : n
                    //const std::ptrdiff_t maxmn = std::max(m, n);           // maxmn = m > n ? m : n
                    //const std::ptrdiff_t maxmnr = std::max(maxmn, nrhs);   // maxmnr = maxmn > nrhs ? maxmn : nrhs

                    // query for maximum size of subproblems
                    const integer_t smlsiz = ilaenv(9, "GELSD", "");
                    const integer_t nlvl = static_cast<integer_t>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

                    const integer_t lwork = 12*minmn + 2*minmn*smlsiz + 8*minmn*nlvl +
                                      minmn*nrhs + (smlsiz+1)*(smlsiz+1);

                    traits::detail::array<val_t> work(lwork);

                    return gelsd(A, B, s, work);
                }

                template <typename MatrA, typename MatrB, typename VecS>
                int operator() (MatrA& A, MatrB& B, VecS& s, optimal_workspace) const
                {
                    typedef typename MatrA::value_type val_t;
                    typedef typename traits::type_traits<val_t>::real_type real_t;

                    //const std::ptrdiff_t m = traits::matrix_size1(A);
                    //const std::ptrdiff_t n = traits::matrix_size2(A);
                    //const std::ptrdiff_t nrhs = traits::matrix_size2(B);

                    //const std::ptrdiff_t minmn = std::min(m, n);           // minmn = m < n ? m : n
                    //const std::ptrdiff_t maxmn = std::max(m, n);           // maxmn = m > n ? m : n
                    //const std::ptrdiff_t maxmnr = std::max(maxmn, nrhs);   // maxmnr = maxmn > nrhs ? maxmn : nrhs

                    val_t temp_work;
                    integer_t temp_iwork;

                    const real_t rcond = -1;
                    integer_t rank;
                    integer_t info;

                    // query for optimal workspace size
                    detail::gelsd(traits::matrix_size1(A),
                                  traits::matrix_size2(A),
                                  traits::matrix_size2(B),
                                  traits::matrix_storage(A),
                                  traits::leading_dimension(A),
                                  traits::matrix_storage(B),
                                  traits::leading_dimension(B),
                                  traits::vector_storage(s),
                                  rcond,
                                  &rank,
                                  &temp_work,   //traits::vector_storage(work),
                                  -1,           //traits::vector_size(work),
                                  &temp_iwork,
                                  &info);

                    assert(info == 0);

                    const integer_t lwork = traits::detail::to_int(temp_work);

                    traits::detail::array<val_t> work(lwork);

                    return gelsd(A, B, s, work);
                }

                template <typename MatrA, typename MatrB, typename VecS, typename Work>
                int operator() (MatrA& A, MatrB& B, VecS& s, detail::workspace1<Work> workspace) const
                {
                    //const std::ptrdiff_t m = traits::matrix_size1(A);
                    //const std::ptrdiff_t n = traits::matrix_size2(A);
                    //const std::ptrdiff_t nrhs = traits::matrix_size2(B);

                    //const std::ptrdiff_t minmn = std::min(m, n);           // minmn = m < n ? m : n
                    //const std::ptrdiff_t maxmn = std::max(m, n);           // maxmn = m > n ? m : n
                    //const std::ptrdiff_t maxmnr = std::max(maxmn, nrhs);   // maxmnr = maxmn > nrhs ? maxmn : nrhs

                    typedef typename traits::vector_traits<VecS>::value_type value_type ;
                    return gelsd(A, B, s, workspace.select(value_type()));
                }
            };

            // specialization for gelsd (cgelsd, zgelsd)
            template <>
            struct Gelsd<2>
            {
                template <typename MatrA, typename MatrB, typename VecS>
                int operator() (MatrA& A, MatrB& B, VecS& s, minimal_workspace) const
                {
                    typedef typename MatrA::value_type val_t;
                    typedef typename traits::type_traits<val_t>::real_type real_t;

                    const std::ptrdiff_t m = traits::matrix_size1(A);
                    const std::ptrdiff_t n = traits::matrix_size2(A);
                    const std::ptrdiff_t nrhs = traits::matrix_size2(B);

                    const std::ptrdiff_t minmn = std::min(m, n);           // minmn = m < n ? m : n
                    //const std::ptrdiff_t maxmn = std::max(m, n);           // maxmn = m > n ? m : n
                    //const std::ptrdiff_t maxmnr = std::max(maxmn, nrhs);   // maxmnr = maxmn > nrhs ? maxmn : nrhs

                    // query for maximum size of subproblems
                    const integer_t smlsiz = ilaenv(9, "GELSD", "");
                    const integer_t nlvl = static_cast<integer_t>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

                    traits::detail::array<val_t> work(2*minmn + minmn*nrhs);

                    const std::ptrdiff_t rwork_size = 10*minmn + 2*minmn*smlsiz + 8*minmn*nlvl +
                                           3*smlsiz*nrhs + (smlsiz+1)*(smlsiz+1);

                    traits::detail::array<real_t> rwork(std::max<std::ptrdiff_t>(1, rwork_size));

                    return gelsd(A, B, s, work, rwork);
                }

                template <typename MatrA, typename MatrB, typename VecS>
                int operator() (MatrA& A, MatrB& B, VecS& s, optimal_workspace) const
                {
                    typedef typename MatrA::value_type val_t;
                    typedef typename traits::type_traits<val_t>::real_type real_t;

                    const std::ptrdiff_t m = traits::matrix_size1(A);
                    const std::ptrdiff_t n = traits::matrix_size2(A);
                    const std::ptrdiff_t nrhs = traits::matrix_size2(B);

                    const std::ptrdiff_t minmn = std::min(m, n);           // minmn = m < n ? m : n
                    //const std::ptrdiff_t maxmn = std::max(m, n);           // maxmn = m > n ? m : n
                    //const std::ptrdiff_t maxmnr = std::max(maxmn, nrhs);   // maxmnr = maxmn > nrhs ? maxmn : nrhs

                    val_t temp_work;
                    real_t temp_rwork;
                    integer_t temp_iwork;

                    const real_t rcond = -1;
                    integer_t rank;
                    integer_t info;

                    // query for optimal workspace size
                    detail::gelsd(traits::matrix_size1(A),
                                  traits::matrix_size2(A),
                                  traits::matrix_size2(B),
                                  traits::matrix_storage(A),
                                  traits::leading_dimension(A),
                                  traits::matrix_storage(B),
                                  traits::leading_dimension(B),
                                  traits::vector_storage(s),
                                  rcond,
                                  &rank,
                                  &temp_work,   //traits::vector_storage(work),
                                  -1,           //traits::vector_size(work),
                                  &temp_rwork,
                                  &temp_iwork,
                                  &info);

                    assert(info == 0);

                    const integer_t lwork = traits::detail::to_int(temp_work);

                    traits::detail::array<val_t> work(lwork);

                    // query for maximum size of subproblems
                    const integer_t smlsiz = ilaenv(9, "GELSD", "");
                    const integer_t nlvl = static_cast<integer_t>(((std::log(static_cast<float>(minmn))/std::log(2.f))/ (smlsiz+1)) + 1);

                    const std::ptrdiff_t rwork_size = 10*minmn + 2*minmn*smlsiz + 8*minmn*nlvl +
                                            3*smlsiz*nrhs + (smlsiz+1)*(smlsiz+1);

                    traits::detail::array<real_t> rwork(std::max<std::ptrdiff_t>(1, rwork_size));

                    return gelsd(A, B, s, work, rwork);
                }

                template <typename MatrA, typename MatrB, typename VecS, typename Work, typename RWork>
                int operator() (MatrA& A, MatrB& B, VecS& s, detail::workspace2<Work, RWork> workspace) const
                {
                    //const std::ptrdiff_t m = traits::matrix_size1(A);
                    //const std::ptrdiff_t n = traits::matrix_size2(A);
                    //const std::ptrdiff_t nrhs = traits::matrix_size2(B);

                    //const std::ptrdiff_t minmn = std::min(m, n);           // minmn = m < n ? m : n
                    //const std::ptrdiff_t maxmn = std::max(m, n);           // maxmn = m > n ? m : n
                    //const std::ptrdiff_t maxmnr = std::max(maxmn, nrhs);   // maxmnr = maxmn > nrhs ? maxmn : nrhs

                    typedef typename traits::vector_traits<VecS>::value_type    value_type ;
                    typedef typename traits::type_traits<value_type>::real_type real_type ;
                    return gelsd(A, B, s, workspace.select(value_type()), workspace.select(real_type()));
                }
            };

        } // detail

        // gelsd
        // Parameters:
        //  A:          matrix of coefficients
        //  B:          matrix of solutions (stored column-wise)
        //  s:          vector to store singular values on output, length >= max(1, min(m,n))
        //  workspace:  either optimal, minimal, or user supplied
        //
        template <typename MatrA, typename MatrB, typename VecS, typename Work>
        int gelsd(MatrA& A, MatrB& B, VecS& s, Work workspace)
        {
            typedef typename MatrA::value_type val_t;

            return detail::Gelsd<n_workspace_args<val_t>::value>() (A, B, s, workspace);
        }

        // gelsd, no singular values are returned
        // Parameters:
        //  A:          matrix of coefficients
        //  B:          matrix of solutions (stored column-wise)
        //  workspace:  either optimal, minimal, or user supplied
        //
        template <typename MatrA, typename MatrB, typename Work>
        int gelsd(MatrA& A, MatrB& B, Work workspace)
        {
            typedef typename MatrA::value_type val_t;
            typedef typename traits::type_traits<val_t>::real_type real_t;

            const std::ptrdiff_t m = traits::matrix_size1(A);
            const std::ptrdiff_t n = traits::matrix_size2(A);

            const std::ptrdiff_t s_size = std::max<std::ptrdiff_t>(1, std::min(m,n));
            traits::detail::array<real_t> s(s_size);

            return detail::Gelsd<n_workspace_args<val_t>::value>() (A, B, s, workspace);
        }

    } // lapack

}}}

#endif


#include <pokerstove/peval/Card.h>
#include <pokerstove/peval/CardSet.h>
#include <pokerstove/peval/CardSetGenerators.h>
#include <pokerstove/peval/PokerHandEvaluator.h>
#include <pokerstove/util/combinations.h>
#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <boost/shared_ptr.hpp>
#include <iostream>
#include <string>
#include <vector>

using namespace std;
namespace po = boost::program_options;
using namespace pokerstove;

int main(int argc, char** argv) {
  try {
    // set up the program options, handle the help case, and extract the
    // values
    po::options_description desc("create a lookup table for poker hands\n");
    // clang-format off
    desc.add_options()
        ("help,?", "produce help message")
        ("pocket-count,p", po::value<size_t>()->default_value(2),
         "number of pocket cards to use")
        ("board-count,b", po::value<size_t>()->default_value(3),
         "number of board cards to use")
        ("game,g", po::value<string>()->default_value("O"),
         "game to use for evaluation")
        ("ranks", "print the set of rank values");
    // clang-format on

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv)
                  .style(po::command_line_style::unix_style)
                  .options(desc)
                  .run(),
              vm);
    po::notify(vm);

    // check for help
    if (vm.count("help") || argc == 1) {
      cout << desc << endl;
      return 1;
    }

    // extract the options
    size_t pocketCount = vm["pocket-count"].as<size_t>();
    size_t boardCount = vm["board-count"].as<size_t>();
    string game = vm["game"].as<string>();

    bool ranks = vm.count("ranks") > 0;
    // make the sets
    Card::Grouping grouping = Card::SUIT_CANONICAL;
    if (ranks)
      grouping = Card::RANK;
    set<CardSet> pockets = createCardSet(pocketCount, grouping);
    set<CardSet> boards = createCardSet(boardCount, grouping);

    auto evaluator = PokerHandEvaluator::alloc(game);
    for (auto pit = pockets.begin(); pit != pockets.end(); pit++)
      for (auto bit = boards.begin(); bit != boards.end(); bit++) {
        if (ranks) {
          PokerEvaluation eval = evaluator->evaluateRanks(*pit, *bit);
          // cout << boost::format("%s, %s\n") % pit->rankstr() %
          // bit->rankstr();

          // clang-format off
          cout << boost::format("%s, %s: [%4d,%4d] -> %s [%9d]\n") %
                      pit->str() %
                      bit->str() %
                      pit->rankColex() %
                      bit->rankColex() %
                      eval.str() %
                      eval.code();
          // clang-format on
        } else {
          bit->canonize(*pit);
          if (pit->intersects(*bit))
            continue;
          PokerHandEvaluation eval = evaluator->evaluate(*pit, *bit);
          // clang-format off
          cout << boost::format("%s, %s: [%4d,%4d] -> %s [%9d]\n") %
                      pit->str() %
                      bit->str() %
                      pit->colex() %
                      bit->colex() %
                      eval.str() %
                      eval.high().code();
          // clang-format on
        }
      }
  } catch (std::exception& e) {
    cerr << "-- caught exception--\n" << e.what() << "\n";
    return 1;
  } catch (...) {
    cerr << "Exception of unknown type!\n";
    return 1;
  }
  return 0;
}


//Link to Boost
 #define BOOST_TEST_DYN_LINK

//VERY IMPORTANT - include this last
#include <boost/test/unit_test.hpp>
#include <boost/timer.hpp>

#include <vector>
#include <iostream>
#include <cmath>
#include "test.h"
#include "../Scalar4ptFunction.h"
using namespace std;

struct Scalar4ptFunctionTestData
{
    CftConfig Config;

    int Op1;
    int Op2;
    int Op3;
    int Op4;

    int OpExchange;

    cpx_t Z;
};

std::ostream& operator<< (std::ostream& out, const Scalar4ptFunctionTestData& data)
{
    out << '{';
    out << "Config=" << data.Config << ", Op1=" << data.Op1 << ", Op2=" << data.Op2;
    out << ", Op3=" << data.Op3 << ", Op4=" << data.Op4 << ", OpExchange=" << data.OpExchange;
    out << ", Z=" << data.Z;
    out << '}';
    return out;
}

Scalar4ptFunctionTestData CreateSingleScalar4ptFunctionTestData(CftConfig* config, bool useExternalAsExchangeOp)
{
    Scalar4ptFunctionTestData data;
    data.Config = *config;
    data.Op1 = randomint(1, config->OperatorNumbers[0]);
    data.Op2 = randomint(1, config->OperatorNumbers[0]);
    data.Op3 = randomint(1, config->OperatorNumbers[0]);
    data.Op4 = randomint(1, config->OperatorNumbers[0]);

    // if useExternalAsExchangeOp, OpExchange will be one of the external operators.
    if (useExternalAsExchangeOp) {
        int ops[4] = {data.Op1, data.Op2, data.Op3, data.Op4};
        data.OpExchange = ops[randomint(0,3)];
    } else {
        data.OpExchange = randomint(0, Sum(config->OperatorNumbers));
        if (data.OpExchange == data.Op1 || data.OpExchange == data.Op2
            || data.OpExchange == data.Op3 || data.OpExchange == data.Op4) {
            data.OpExchange = randomint(config->OperatorNumbers[0] + 1, Sum(config->OperatorNumbers));
        }

        // if OpExchange is stress tensor, some derivative functions will blow up.
        // so we pick another operator if OpExchange is stress tensor.
        if (data.OpExchange == config->OperatorNumbers[0] + config->OperatorNumbers[1] + 1) {
            data.OpExchange = data.OpExchange + 1;
        }
    }

    data.Z = RandomComplex(0.3) + 0.5;
    float_type r1 = Z2r(data.Z);
    float_type r2 = Z2r(-data.Z + 1.0);

    if (r1 >= 0.25 || r2 >= 0.25) {
        //std::cout << "Z=" << data.Z << ", r1=" << r1 << ", r2=" << r2 << endl;
    }

    return data;
}

vector<Scalar4ptFunctionTestData> CreateScalar4ptFunctionTestData(int size, bool useExternalAsExchangeOp)
{
    vector<CftConfig> configs = CreateCfgConfigTestData(size);
    vector<Scalar4ptFunctionTestData> ret;

    for (uint i = 0; i < configs.size(); i++) {
        ret.push_back(CreateSingleScalar4ptFunctionTestData(&configs[i], useExternalAsExchangeOp));
    }

    return ret;
}

// test suite begin
BOOST_FIXTURE_TEST_SUITE(Scalar4ptFunction_NotExternalAsExchangeOp_suite, SimpleTestFixture, * utf::label("s4pt1"))

BOOST_DATA_TEST_CASE(Evaluate_test, CreateScalar4ptFunctionTestData(TCNumber, false), sd)
{
    CftData cftData(sd.Config);
    Scalar4ptFunction s4f(&cftData, sd.Op1, sd.Op2, sd.Op3, sd.Op4);

    float_type actual = s4f.CrossingDiff(sd.Z);
    float_type expect = s4f.EvaluateS(sd.Z) - s4f.EvaluateT(sd.Z);
    MY_FLOAT_EQUAL(actual, expect, tol);
}

BOOST_DATA_TEST_CASE(CrossingDerDelta_test, CreateScalar4ptFunctionTestData(TCNumber, false), sd)
{
    CftData cftData(sd.Config);
    Scalar4ptFunction s4f(&cftData, sd.Op1, sd.Op2, sd.Op3, sd.Op4);

    float_type actual = s4f.CrossingDerDelta(sd.Z, sd.OpExchange);

    float_type dim = cftData.GetPrimaryDim(sd.OpExchange);
    cftData.SetPrimaryDim(sd.OpExchange, dim - inc);
    s4f.UpdateCftData(&cftData);
    float_type value1 = s4f.CrossingDiff(sd.Z);

    cftData.SetPrimaryDim(sd.OpExchange, dim + inc);
    s4f.UpdateCftData(&cftData);
    float_type value2 = s4f.CrossingDiff(sd.Z);
    float_type expect = (value2 - value1) / (2 * inc);

    BOOST_TEST_INFO("OpExchange: dlt=" << dim << ", l=" << cftData.GetPrimarySpin(sd.OpExchange) << ", value1=" << value1 << ", value2=" << value2);
    MY_FLOAT_EQUAL(actual, expect, tol);
}

BOOST_DATA_TEST_CASE(CrossingDerOpeCoefficient_op_test, CreateScalar4ptFunctionTestData(TCNumber, false), sd)
{
    CftData cftData(sd.Config);
    Scalar4ptFunction s4f(&cftData, sd.Op1, sd.Op2, sd.Op3, sd.Op4);

    OpeCoefficientKey key(sd.Op1, sd.Op2, sd.OpExchange);
    float_type actual = s4f.CrossingDerOpeCoefficient(sd.Z, key, sd.OpExchange);

    float_type coef = cftData.GetOpeCoefficient(key.Op1, key.Op2, key.Op3);

    cftData.SetOpeCoefficient(key.Op1, key.Op2, key.Op3, coef - inc);
    s4f.UpdateCftData(&cftData);
    float_type value1 = s4f.CrossingDiff(sd.Z, sd.OpExchange);

    cftData.SetOpeCoefficient(key.Op1, key.Op2, key.Op3, coef + inc);
    s4f.UpdateCftData(&cftData);
    float_type value2 = s4f.CrossingDiff(sd.Z, sd.OpExchange);
    float_type expect = (value2 - value1) / (2 * inc);

    MY_FLOAT_EQUAL(actual, expect, tol);
}

BOOST_DATA_TEST_CASE(CrossingDerOpeCoefficient_test, CreateScalar4ptFunctionTestData(TCNumber, false), sd)
{
    CftData cftData(sd.Config);
    Scalar4ptFunction s4f(&cftData, sd.Op1, sd.Op2, sd.Op3, sd.Op4);

    OpeCoefficientKey key(sd.Op3, sd.Op2, sd.OpExchange);
    float_type actual = s4f.CrossingDerOpeCoefficient(sd.Z, key);

    float_type coef = cftData.GetOpeCoefficient(key.Op1, key.Op2, key.Op3);

    cftData.SetOpeCoefficient(key.Op1, key.Op2, key.Op3, coef - inc);
    s4f.UpdateCftData(&cftData);
    float_type value1 = s4f.CrossingDiff(sd.Z);

    cftData.SetOpeCoefficient(key.Op1, key.Op2, key.Op3, coef + inc);
    s4f.UpdateCftData(&cftData);
    float_type value2 = s4f.CrossingDiff(sd.Z);
    float_type expect = (value2 - value1) / (2 * inc);

    MY_FLOAT_EQUAL(actual, expect, tol);
}

// test suite end
BOOST_AUTO_TEST_SUITE_END()

// test suite begin
BOOST_FIXTURE_TEST_SUITE(Scalar4ptFunction_ExternalAsExchangeOp_suite, SimpleTestFixture, * utf::label("s4pt2"))

BOOST_DATA_TEST_CASE(Evaluate_test, CreateScalar4ptFunctionTestData(TCNumber, true), sd)
{
    CftData cftData(sd.Config);
    Scalar4ptFunction s4f(&cftData, sd.Op1, sd.Op2, sd.Op3, sd.Op4);

    float_type actual = s4f.CrossingDiff(sd.Z);
    float_type expect = s4f.EvaluateS(sd.Z) - s4f.EvaluateT(sd.Z);
    MY_FLOAT_EQUAL(actual, expect, tol);
}

BOOST_DATA_TEST_CASE(CrossingDerDelta_test, CreateScalar4ptFunctionTestData(TCNumber, true), sd)
{
    CftData cftData(sd.Config);
    Scalar4ptFunction s4f(&cftData, sd.Op1, sd.Op2, sd.Op3, sd.Op4);

    float_type actual = s4f.CrossingDerDelta(sd.Z, sd.OpExchange);

    float_type dim = cftData.GetPrimaryDim(sd.OpExchange);
    cftData.SetPrimaryDim(sd.OpExchange, dim - inc);
    s4f.UpdateCftData(&cftData);
    float_type value1 = s4f.CrossingDiff(sd.Z);

    cftData.SetPrimaryDim(sd.OpExchange, dim + inc);
    s4f.UpdateCftData(&cftData);
    float_type value2 = s4f.CrossingDiff(sd.Z);
    float_type expect = (value2 - value1) / (2 * inc);

    BOOST_TEST_INFO("OpExchange: dlt=" << dim << ", l=" << cftData.GetPrimarySpin(sd.OpExchange) << ", value1=" << value1 << ", value2=" << value2);
    MY_FLOAT_EQUAL(actual, expect, tol);
}

BOOST_DATA_TEST_CASE(CrossingDerOpeCoefficient_op_test, CreateScalar4ptFunctionTestData(TCNumber, true), sd)
{
    CftData cftData(sd.Config);
    Scalar4ptFunction s4f(&cftData, sd.Op1, sd.Op2, sd.Op3, sd.Op4);

    OpeCoefficientKey key(sd.Op1, sd.Op2, sd.OpExchange);
    float_type actual = s4f.CrossingDerOpeCoefficient(sd.Z, key, sd.OpExchange);

    float_type coef = cftData.GetOpeCoefficient(key.Op1, key.Op2, key.Op3);

    cftData.SetOpeCoefficient(key.Op1, key.Op2, key.Op3, coef - inc);
    s4f.UpdateCftData(&cftData);
    float_type value1 = s4f.CrossingDiff(sd.Z, sd.OpExchange);

    cftData.SetOpeCoefficient(key.Op1, key.Op2, key.Op3, coef + inc);
    s4f.UpdateCftData(&cftData);
    float_type value2 = s4f.CrossingDiff(sd.Z, sd.OpExchange);
    float_type expect = (value2 - value1) / (2 * inc);

    MY_FLOAT_EQUAL(actual, expect, tol);
}

BOOST_DATA_TEST_CASE(CrossingDerOpeCoefficient_test, CreateScalar4ptFunctionTestData(TCNumber, true), sd)
{
    CftData cftData(sd.Config);
    Scalar4ptFunction s4f(&cftData, sd.Op1, sd.Op2, sd.Op3, sd.Op4);

    OpeCoefficientKey key(sd.Op3, sd.Op2, sd.OpExchange);
    float_type actual = s4f.CrossingDerOpeCoefficient(sd.Z, key);

    float_type coef = cftData.GetOpeCoefficient(key.Op1, key.Op2, key.Op3);

    cftData.SetOpeCoefficient(key.Op1, key.Op2, key.Op3, coef - inc);
    s4f.UpdateCftData(&cftData);
    float_type value1 = s4f.CrossingDiff(sd.Z);

    cftData.SetOpeCoefficient(key.Op1, key.Op2, key.Op3, coef + inc);
    s4f.UpdateCftData(&cftData);
    float_type value2 = s4f.CrossingDiff(sd.Z);
    float_type expect = (value2 - value1) / (2 * inc);

    MY_FLOAT_EQUAL(actual, expect, tol);
}

// test suite end
BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_CASE(Scalar4ptFunction_performance_test1, * utf::label("perf"))
{
    CftConfig config(3);
    config.OperatorNumbers = vector<int>({8,6,6,5,4,3,2,1});
    vector<Scalar4ptFunctionTestData> dataList;
    for (int i = 0; i < TCNumber; i++) {
        dataList.push_back(CreateSingleScalar4ptFunctionTestData(&config, true));
    }

    float_type value;
    boost::timer stopwatch;

    for (uint i = 0; i < dataList.size(); i++) {
        Scalar4ptFunctionTestData sd = dataList[i];
        CftData cftData(sd.Config);
        Scalar4ptFunction s4f(&cftData, sd.Op1, sd.Op2, sd.Op3, sd.Op4);

        value = s4f.CrossingDiff(sd.Z);
    }

    BOOST_TEST_MESSAGE("Run Scalar4ptFunction.CrossingDiff " << dataList.size() << " times takes " << stopwatch.elapsed() << " seconds.");

    stopwatch.restart();

    for (uint i = 0; i < dataList.size(); i++) {
        Scalar4ptFunctionTestData sd = dataList[i];
        CftData cftData(sd.Config);
        Scalar4ptFunction s4f(&cftData, sd.Op1, sd.Op2, sd.Op3, sd.Op4);

        value = s4f.CrossingDerDelta(sd.Z, sd.OpExchange);
    }

    BOOST_TEST_MESSAGE("Run Scalar4ptFunction.CrossingDerDelta " << dataList.size() << " times takes " << stopwatch.elapsed() << " seconds.");

    stopwatch.restart();

    for (uint i = 0; i < dataList.size(); i++) {
        Scalar4ptFunctionTestData sd = dataList[i];
        CftData cftData(sd.Config);
        Scalar4ptFunction s4f(&cftData, sd.Op1, sd.Op2, sd.Op3, sd.Op4);

        OpeCoefficientKey coef(sd.Op3, sd.Op2, sd.OpExchange);
        value = s4f.CrossingDerOpeCoefficient(sd.Z, coef);
    }

    BOOST_TEST_MESSAGE("Run Scalar4ptFunction.CrossingDerOpeCoefficient " << dataList.size() << " times takes " << stopwatch.elapsed() << " seconds.");
}

BOOST_AUTO_TEST_CASE(Scalar4ptFunction_performance_test2, * utf::label("perf"))
{
    CftConfig config(3);
    config.OperatorNumbers = vector<int>({8,6,6,5,4,3,2,1});
    vector<Scalar4ptFunctionTestData> dataList;
    for (int i = 0; i < TCNumber; i++) {
        dataList.push_back(CreateSingleScalar4ptFunctionTestData(&config, false));
    }

    float_type value;
    boost::timer stopwatch;

    for (uint i = 0; i < dataList.size(); i++) {
        Scalar4ptFunctionTestData sd = dataList[i];
        CftData cftData(sd.Config);
        Scalar4ptFunction s4f(&cftData, sd.Op1, sd.Op2, sd.Op3, sd.Op4);

        value = s4f.CrossingDiff(sd.Z);
    }

    BOOST_TEST_MESSAGE("Run Scalar4ptFunction.CrossingDiff " << dataList.size() << " times takes " << stopwatch.elapsed() << " seconds.");

    stopwatch.restart();

    for (uint i = 0; i < dataList.size(); i++) {
        Scalar4ptFunctionTestData sd = dataList[i];
        CftData cftData(sd.Config);
        Scalar4ptFunction s4f(&cftData, sd.Op1, sd.Op2, sd.Op3, sd.Op4);

        value = s4f.CrossingDerDelta(sd.Z, sd.OpExchange);
    }

    BOOST_TEST_MESSAGE("Run Scalar4ptFunction.CrossingDerDelta " << dataList.size() << " times takes " << stopwatch.elapsed() << " seconds.");

    stopwatch.restart();

    for (uint i = 0; i < dataList.size(); i++) {
        Scalar4ptFunctionTestData sd = dataList[i];
        CftData cftData(sd.Config);
        Scalar4ptFunction s4f(&cftData, sd.Op1, sd.Op2, sd.Op3, sd.Op4);

        OpeCoefficientKey coef(sd.Op3, sd.Op2, sd.OpExchange);
        value = s4f.CrossingDerOpeCoefficient(sd.Z, coef);
    }

    BOOST_TEST_MESSAGE("Run Scalar4ptFunction.CrossingDerOpeCoefficient " << dataList.size() << " times takes " << stopwatch.elapsed() << " seconds.");
}

BOOST_AUTO_TEST_CASE(Scalar4ptFunction_Debug, * utf::label("debug"))
{
    vector<float_type> dims = vector<float_type>({0.509292, 1.37473, 2.23385, 3.20297});
    cpx_t z(0.41671,0.0386725);
//    cpx_t z(0.598759,0.00609649);

    CftConfig cftConfig(3);
    cftConfig.OperatorNumbers = vector<int>({4, 0, 1});
    CftData cftData(cftConfig);
    for (uint i = 0; i < dims.size(); i++) {
        cftData.SetPrimaryDim(i + 1, dims[i]);
    }

    Scalar4ptFunction s4f(&cftData, 1, 1, 4, 4);
    //cout << "S channel=" << s4f.EvaluateS(z) << ", T channel=" << s4f.EvaluateT(z) << endl;
    cout << "T cb=" << s4f.ConformalBlock_T(z, 1) << endl;
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/minlog.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check minlog behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::minlog;
  using boost::simd::Minlog;
  T ref = T(0);
  STF_TYPE_IS(decltype(Minlog<T>()), T);
  STF_EQUAL(Minlog<T>(), ref);
  STF_EQUAL(minlog( as(T{}) ),ref);
}

STF_CASE_TPL( "Check minlog behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::minlog;
  using boost::simd::Minlog;
  T ref = T( -708.3964185322641);

  STF_TYPE_IS(decltype(Minlog<T>()), T);
  STF_IEEE_EQUAL(Minlog<T>(), ref);
  STF_IEEE_EQUAL(minlog( as(T{}) ), ref);
}

STF_CASE_TPL( "Check minlog behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::minlog;
  using boost::simd::Minlog;
  T ref = T(-88.3762626647949);
  STF_TYPE_IS(decltype(Minlog<T>()), T);
  STF_IEEE_EQUAL(Minlog<T>(), ref);
  STF_IEEE_EQUAL(minlog( as(T{}) ), ref);
}


#define PY_ARRAY_UNIQUE_SYMBOL superimg_PyArray_API
#define NO_IMPORT_ARRAY

#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <vigra/numpy_array.hxx>
#include <vigra/numpy_array_converters.hxx>

#include "seglib/cgp2d/cgp2d.hxx"
#include "seglib/cgp2d/cgp2d_python.hxx"
#include "py_cell_visitor.hxx"

namespace cgp2d {

namespace python = boost::python;

void export_cell1()
{
    using namespace python;

    docstring_options doc_options(true, true, false);

    ////////////////////////////////////////
    // Region Graph
    ////////////////////////////////////////
    // basic types
    // tgrid and input image type
    typedef Cgp<CoordinateType,LabelType> CgpType;
    typedef CgpType::TopologicalGridType TopologicalGridType;

    typedef  vigra::NumpyArray<2 ,vigra::Singleband < LabelType > > InputLabelImageType;
    // cgp type and cell types
    typedef CgpType::PointType PointType;
    // bound vector
    typedef std::vector<float> FloatVectorType;
    typedef std::vector<LabelType> LabelVectorType;
    // point vector
    typedef std::vector<PointType> PointVectorType;
    // geo cells
    typedef CgpType::Cell1 Cell1Type;
    typedef CgpType::Cells1 Cell1VectorType;

    // cells
    python::class_<Cell1Type>("Cell1",python::init<>())
        .def(CellTypeSuite<Cell1Type>())
    ;
}

} // namespace vigra


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing bitfloating capabilities

    Transform a pattern of bits stored in an integer value
    in a floating point with different formulas according to
    the integer @ref bitofsign (converse of @ref bitinteger)

    @par Semantic:

    @code
    as_floating<T> r = bitfloating(x);
    @endcode

    is similar to:

    @code
    as_floating<T> r =  bitwise_cast<as_floating<T>>(x >=0 ? x : Signmask<T>()-x);
    @endcode

    @par Note:
    This is an utility function related to the computation of
    floating successors or predecessors.

    @see next, prev, successor,  predecessor,  nextafter

  **/
  const boost::dispatch::functor<tag::bitfloating_> bitfloating = {};
} }
#endif

#include <boost/simd/function/scalar/bitfloating.hpp>
#include <boost/simd/function/simd/bitfloating.hpp>

#endif