#include <ros/ros.h>
#include <ros/console.h>
#include <math.h>
#include <iostream>
#include <Eigen/Eigen>
#include <random>
#include <tf/transform_broadcaster.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Vector3.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/PointCloud2.h>

#include "printf_utils.h"

using namespace std;
using namespace Eigen;

const int MAX_SWRAM_NUM = 41;

// 节点功能：生成随机地图，并模拟无人机检测，模拟得到局部地图与全局地图

int swarm_num;                                     // 集群数量
int uav_id[MAX_SWRAM_NUM];                         // 编号
int map_flag;                                      // 地图编号
double map_size_x, map_size_y, map_size_z;         // 地图尺寸
double map_x_min, map_x_max, map_y_min, map_y_max; // 地图xy范围
double map_z_limit;
double map_resolution;              // 地图分辨率
double sensing_range, sensing_rate; // 传感器感知半径与频率
double sensing_horizon;

double obs_min_dist;                     // 障碍物最小间距
int cylinder_num;                        // 圆柱体数量
double cylinder_radius, cylinder_height; // 圆柱体参数
int cuboid_num;                          // 立方体数量
double cuboid_size, cuboid_height;       // 立方体参数
double wall_length, wall_height;         // 墙参数
double line_height;
// 初始状态量
bool global_map_ok = false;
bool uav_odom_ok[MAX_SWRAM_NUM] = {false};
// 无人机初始位置
double uav_init_x, uav_init_y;
nav_msgs::Odometry uav_odom[MAX_SWRAM_NUM];

ros::Subscriber uav_odom_sub[MAX_SWRAM_NUM];
ros::Publisher local_map_pub[MAX_SWRAM_NUM];
ros::Timer pub_local_map_timer[MAX_SWRAM_NUM];
ros::Publisher global_map_pub;
ros::Timer pub_global_map_timer;

pcl::PointCloud<pcl::PointXYZ> global_map_pcl; // 全局点云地图 - pcl格式
sensor_msgs::PointCloud2 global_map_ros;       // 全局点云地图 - ros_msg格式
pcl::PointCloud<pcl::PointXYZ> local_map_pcl;  // 局部点云地图 - pcl格式
sensor_msgs::PointCloud2 local_map_ros;        // 局部点云地图 - ros_msg格式

random_device rd;
default_random_engine eng(rd());
uniform_real_distribution<double> rand_x;
uniform_real_distribution<double> rand_y;

pcl::KdTreeFLANN<pcl::PointXYZ> kdtreeLocalMap;
vector<int> pointIdxRadiusSearch;
vector<float> pointRadiusSquaredDistance;

// 生成圆柱体
// 圆柱体半径：cylinder_radius
// 圆柱体高度：cylinder_height
void generate_cylinder(double x, double y)
{
    // 必须在地图范围内
    if (x < map_x_min || x > map_x_max || y < map_y_min || y > map_y_max)
    {
        return;
    }

    pcl::PointXYZ pt_random;
    x = floor(x / map_resolution) * map_resolution;
    y = floor(y / map_resolution) * map_resolution;
    int widNum = ceil((cylinder_radius) / map_resolution);
    int heiNum = ceil(cylinder_height / map_resolution);
    for (int r = -widNum; r <= widNum; r++)
        for (int s = -widNum; s <= widNum; s++)
            for (int t = -1; t <= heiNum; t++)
            {
                double temp_x = x + r * map_resolution;
                double temp_y = y + s * map_resolution;
                double temp_z = t * map_resolution;
                if ((Eigen::Vector2d(temp_x, temp_y) - Eigen::Vector2d(x, y)).norm() < cylinder_radius + 1e-2)
                {
                    pt_random.x = temp_x;
                    pt_random.y = temp_y;
                    pt_random.z = temp_z;
                    global_map_pcl.points.push_back(pt_random);
                }
            }
}

// 生成长方体
// 长方体长宽：cuboid_height
// 长方体高度：cuboid_height
void generate_cuboid(double x, double y)
{
    // 必须在地图范围内
    if (x < map_x_min || x > map_x_max || y < map_y_min || y > map_y_max)
    {
        return;
    }

    pcl::PointXYZ pt_random;
    x = floor(x / map_resolution) * map_resolution;
    y = floor(y / map_resolution) * map_resolution;
    // z = floor(z / map_resolution) * map_resolution + map_resolution / 2.0;
    int widNum = ceil((cuboid_size / 2.0) / map_resolution);
    int heiNum = ceil(cuboid_height / map_resolution);
    int temp_wid = cuboid_size / 2.0 / map_resolution;
    int flag = 0;
    if (widNum != temp_wid)
        flag = 1;
    for (int r = -widNum; r <= widNum; r++)
        for (int s = -widNum; s <= widNum; s++)
            for (int t = -1; t <= heiNum; t++)
            {
                double temp_x = x + r * map_resolution + flag * 0.5 * map_resolution;
                double temp_y = y + s * map_resolution + flag * 0.5 * map_resolution;
                double temp_z = t * map_resolution;
                if ((Eigen::Vector2d(temp_x, temp_y) - Eigen::Vector2d(x, y)).norm() <= cuboid_size / 2.0 * sqrt(2) + 1e-1)
                {
                    pt_random.x = temp_x;
                    pt_random.y = temp_y;
                    pt_random.z = temp_z;
                    global_map_pcl.points.push_back(pt_random);
                }
            }
}

// 生成横墙，即与X轴平行的墙
// 横墙长度：wall_length
// 横墙高度：wall_height
void generate_row_wall(double x, double y)
{
    // 必须在地图范围内
    if (x < map_x_min || x > map_x_max || y < map_y_min || y > map_y_max)
    {
        return;
    }

    pcl::PointXYZ pt_random;
    int widNum = ceil((wall_length / 2.0) / map_resolution);
    int heiNum = ceil(wall_height / map_resolution);

    for (int r = -widNum; r <= widNum; r++)
        for (int t = -1; t <= heiNum; t++)
        {
            double temp_x = x + r * map_resolution;
            double temp_y = y;
            double temp_z = t * map_resolution;
            pt_random.x = temp_x;
            pt_random.y = temp_y;
            pt_random.z = temp_z;
            global_map_pcl.points.push_back(pt_random);
        }
}

// 生成竖墙，即与Y轴平行的墙
// 横墙长度：wall_length
// 横墙高度：wall_height
void generate_column_wall(double x, double y)
{
    // 必须在地图范围内
    if (x < map_x_min || x > map_x_max || y < map_y_min || y > map_y_max)
    {
        return;
    }

    pcl::PointXYZ pt_random;
    int widNum = ceil((wall_length / 2.0) / map_resolution);
    int heiNum = ceil(wall_height / map_resolution);

    for (int r = -widNum; r <= widNum; r++)
        for (int t = -1; t < heiNum; t++)
        {
            double temp_x = x;
            double temp_y = y + r * map_resolution;
            double temp_z = t * map_resolution;
            pt_random.x = temp_x;
            pt_random.y = temp_y;
            pt_random.z = temp_z;
            global_map_pcl.points.push_back(pt_random);
        }
}

// 生成一根竖直的线
// 长度：line_height
void generate_line(double x, double y)
{
    // 必须在地图范围内
    if (x < map_x_min || x > map_x_max || y < map_y_min || y > map_y_max)
    {
        return;
    }

    pcl::PointXYZ pt_random;
    int heiNum = ceil(line_height / map_resolution);

    for (int t = -1; t < heiNum; t++)
    {
        double temp_x = x;
        double temp_y = y;
        double temp_z = t * map_resolution;
        pt_random.x = temp_x;
        pt_random.y = temp_y;
        pt_random.z = temp_z;
        global_map_pcl.points.push_back(pt_random);
    }
}

void GenerateBorder()
{
    wall_height = 0.1;

    wall_length = map_size_x;
    generate_row_wall(0.0, map_y_min);
    generate_row_wall(0.0, map_y_max);
    wall_length = map_size_y;
    generate_column_wall(map_x_min, 0.0);
    generate_column_wall(map_x_max, 0.0);

    cout << GREEN << "[map_generator] Finished generate border." << TAIL << endl;
}

void GenerateMap1()
{
    generate_cylinder(1.0, 1.0);

    generate_cuboid(2.0, 2.0);

    generate_row_wall(10.0, 10.0);

    generate_column_wall(-10.0, -10.0);

    generate_line(0.0, 0.0);

    global_map_pcl.width = global_map_pcl.points.size();
    global_map_pcl.height = 1;
    global_map_pcl.is_dense = true;

    kdtreeLocalMap.setInputCloud(global_map_pcl.makeShared());

    global_map_ok = true;

    cout << GREEN << "[map_generator] Finished generate map 1." << TAIL << endl;
}

void GenerateMap3()
{
    // 第一排
    for (int t = -8; t <= 8; t=t+2)
    {
        generate_cylinder((double)8, (double)t);
    }

    // 第二排
    for (int t = -9; t <= 7; t=t+2)
    {
        generate_cylinder((double)6, (double)t);
    }

    for (int t = -8; t <= 8; t=t+2)
    {
        generate_cylinder((double)4, (double)t);
    }

    for (int t = -9; t <= 7; t=t+2)
    {
        generate_cylinder((double)2, (double)t);
    }

    for (int t = -8; t <= 8; t=t+2)
    {
        generate_cylinder((double)0, (double)t);
    }

    for (int t = -9; t <= 7; t=t+2)
    {
        generate_cylinder((double)-2, (double)t);
    }

    for (int t = -8; t <= 8; t=t+2)
    {
        generate_cylinder((double)-4, (double)t);
    }

    for (int t = -9; t <= 7; t=t+2)
    {
        generate_cylinder((double)-6, (double)t);
    }

    for (int t = -8; t <= 8; t=t+2)
    {
        generate_cylinder((double)-8, (double)t);
    }

    global_map_pcl.width = global_map_pcl.points.size();
    global_map_pcl.height = 1;
    global_map_pcl.is_dense = true;

    kdtreeLocalMap.setInputCloud(global_map_pcl.makeShared());

    global_map_ok = true;

    cout << GREEN << "[map_generator] Finished generate map 3." << TAIL << endl;
}

void GenerateRandomMap()
{
    // 待存入全局点云的点
    pcl::PointXYZ pt_random;
    // 障碍物位置容器
    vector<Eigen::Vector2d> obs_position;
    // 将无人机初始位置存入容器（防止在无人机初始位置周围生成障碍物）
    obs_position.push_back(Eigen::Vector2d(uav_init_x, uav_init_y));

    // 障碍物位置随机生成器，参数为地图尺寸
    rand_x = uniform_real_distribution<double>(map_x_min, map_x_max);
    rand_y = uniform_real_distribution<double>(map_y_min, map_y_max);

    // 生成圆柱体障碍物
    for (int i = 0; i < cylinder_num; i++)
    {
        double x, y;
        // 随机生成障碍物位置：[x,y]
        x = rand_x(eng);
        y = rand_y(eng);

        // 检查是否与已经生成的障碍物重叠：两个障碍物之间距离大于 _min_dist，否则生成失败
        bool flag_continue = false;
        for (auto p : obs_position)
            if ((Eigen::Vector2d(x, y) - p).norm() < obs_min_dist)
            {
                i--;
                flag_continue = true;
                break;
            }
        if (flag_continue)
            continue;

        // 将本次生成的障碍物位置推入容器
        obs_position.push_back(Eigen::Vector2d(x, y));

        generate_cylinder(x, y);
    }

    // 生成长方体障碍物
    for (int i = 0; i < cuboid_num; ++i)
    {
        double x, y, z;
        // 随机生成障碍物位置：[x,y]
        x = rand_x(eng);
        y = rand_y(eng);

        // 检查是否与已经生成的障碍物重叠：两个障碍物之间距离大于 _min_dist，否则生成失败
        bool flag_continue = false;
        for (auto p : obs_position)
            if ((Eigen::Vector2d(x, y) - p).norm() < obs_min_dist /*metres*/)
            {
                i--; // i--代表此次生成失败，重新生成障碍物
                flag_continue = true;
                break;
            }
        if (flag_continue)
            continue;

        // 将本次生成的障碍物位置推入容器
        obs_position.push_back(Eigen::Vector2d(x, y));

        generate_cuboid(x, y);
    }

    global_map_pcl.width = global_map_pcl.points.size();
    global_map_pcl.height = 1;
    global_map_pcl.is_dense = true;
    kdtreeLocalMap.setInputCloud(global_map_pcl.makeShared());
    global_map_ok = true;

    cout << GREEN << "[map_generator] Finished generate random map." << TAIL << endl;
}

void uav_odom_cb(const nav_msgs::Odometry::ConstPtr &odom, int uav_id)
{
    uav_odom_ok[uav_id] = true;

    uav_odom[uav_id] = *odom;
}

void pub_global_map_cb(const ros::TimerEvent &event)
{
    if (global_map_ok)
    {
        pcl::toROSMsg(global_map_pcl, global_map_ros);
        global_map_ros.header.frame_id = "world";
        global_map_pub.publish(global_map_ros);
    }
    else
    {
        return;
    }
}

void pub_local_map_cb(const ros::TimerEvent &event, int uav_id)
{
    if (!global_map_ok || !uav_odom_ok[uav_id])
    {
        // cout << RED << "[map_generator] fail to pub local map." << TAIL << endl;
        return;
    }

    // 读取无人机姿态
    Eigen::Quaterniond q;
    q.x() = uav_odom[uav_id].pose.pose.orientation.x;
    q.y() = uav_odom[uav_id].pose.pose.orientation.y;
    q.z() = uav_odom[uav_id].pose.pose.orientation.z;
    q.w() = uav_odom[uav_id].pose.pose.orientation.w;

    // 读取偏航角
    Eigen::Matrix3d rot;
    rot = q;
    Eigen::Vector3d yaw_vec = rot.col(0);

    local_map_pcl.points.clear();
    pcl::PointXYZ searchPoint(uav_odom[uav_id].pose.pose.position.x,
                              uav_odom[uav_id].pose.pose.position.y,
                              uav_odom[uav_id].pose.pose.position.z);
    pointIdxRadiusSearch.clear();
    pointRadiusSquaredDistance.clear();

    pcl::PointXYZ pt;
    if (kdtreeLocalMap.radiusSearch(searchPoint, sensing_horizon,
                                    pointIdxRadiusSearch,
                                    pointRadiusSquaredDistance) > 0)
    {
        for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
        {
            pt = global_map_pcl.points[pointIdxRadiusSearch[i]];

            if ((fabs(pt.z - uav_odom[uav_id].pose.pose.position.z) / sensing_horizon) >
                tan(M_PI / 6.0))
                continue;

            Vector3d pt_vec(pt.x - uav_odom[uav_id].pose.pose.position.x,
                            pt.y - uav_odom[uav_id].pose.pose.position.y,
                            pt.z - uav_odom[uav_id].pose.pose.position.z);

            // if (pt_vec.normalized().dot(yaw_vec) < 0.5) continue;

            local_map_pcl.points.push_back(pt);
        }
    }

    local_map_pcl.width = local_map_pcl.points.size();
    local_map_pcl.height = 1;
    local_map_pcl.is_dense = true;

    pcl::toROSMsg(local_map_pcl, local_map_ros);
    local_map_ros.header.frame_id = "/world";

    local_map_pub[uav_id].publish(local_map_ros);
}

//  主函数
int main(int argc, char **argv)
{
    ros::init(argc, argv, "map_generator");
    ros::NodeHandle nh("~");

    // 【参数】集群数量
    nh.param("swarm_num", swarm_num, 0);
    // 【参数】0代表随机生成地图，1使用1号地图，2使用2号地图
    nh.param("map_generator/map_flag", map_flag, 5);
    // 【参数】地图尺寸、分辨率
    nh.param("map_generator/x_size", map_size_x, 10.0);
    nh.param("map_generator/y_size", map_size_y, 10.0);
    nh.param("map_generator/z_size", map_size_z, 5.0);
    nh.param("map_generator/resolution", map_resolution, 0.2);
    // 【参数】传感器感知半径与感知频率
    nh.param("map_generator/sensing_range", sensing_range, 10.0);
    nh.param("map_generator/sensing_horizon", sensing_horizon, 5.0);
    nh.param("map_generator/sensing_rate", sensing_rate, 10.0);
    // 【参数】障碍物最小距离
    nh.param("map_generator/obs_min_dist", obs_min_dist, 1.0);
    // 【参数】障碍物数量 - 圆柱
    nh.param("map_generator/cylinder_num", cylinder_num, 30);
    // 【参数】障碍物参数 - 圆柱
    nh.param("map_generator/cylinder_radius", cylinder_radius, 0.2);
    nh.param("map_generator/cylinder_height", cylinder_height, 3.0);
    // 【参数】障碍物数量 - 立方体
    nh.param("map_generator/cuboid_num", cuboid_num, 10);
    // 【参数】障碍物参数 - 立方体
    nh.param("map_generator/cuboid_size", cuboid_size, 0.3);
    nh.param("map_generator/cuboid_height", cuboid_height, 0.3);
    // 【参数】障碍物参数 - 墙
    nh.param("map_generator/wall_length", wall_length, 10.0);
    nh.param("map_generator/wall_height", wall_height, 2.0);
    nh.param("map_generator/line_height", line_height, 2.0);
    // 【参数】初始位置
    nh.param("uav_init_x", uav_init_x, 0.0);
    nh.param("uav_init_y", uav_init_y, 0.0);

    // 地图坐标范围 [map_x_min,  map_x_max] - [map_y_min, map_y_max]
    map_x_min = -map_size_x / 2.0;
    map_x_max = +map_size_x / 2.0;
    map_y_min = -map_size_y / 2.0;
    map_y_max = +map_size_y / 2.0;
    map_z_limit = map_size_z;

    if ((cylinder_num + cuboid_num) > map_size_x * 8)
    {
        cout << RED << "[map_generator] The map can't put all the obstacles, remove some." << TAIL << endl;
        cylinder_num = map_size_x * 4;
        cuboid_num = map_size_x * 4;
    }

    for (int i = 1; i <= swarm_num; i++)
    {
        uav_id[i] = i;
        // 【订阅】里程计数据
        uav_odom_sub[i] = nh.subscribe<nav_msgs::Odometry>("/uav" + std::to_string(i) + "/odom", 1, boost::bind(&uav_odom_cb, _1, uav_id[i]));
        // 【发布】模拟的局部点云信息
        local_map_pub[i] = nh.advertise<sensor_msgs::PointCloud2>("/uav" + std::to_string(i) + "/map_generator/local_cloud", 1);
        // 【定时器】发布局部点云定时器
        pub_local_map_timer[i] = nh.createTimer(ros::Duration(1.0 / sensing_rate), boost::bind(&pub_local_map_cb, _1, uav_id[i]));
    }

    // 【发布】全局点云地图
    global_map_pub = nh.advertise<sensor_msgs::PointCloud2>("/map_generator/global_cloud", 1);
    // 【定时器】发布全局点云定时器
    pub_global_map_timer = nh.createTimer(ros::Duration(10.0 / sensing_rate), pub_global_map_cb);

    ros::Duration(1.0).sleep();

    //  随机函数初始化
    unsigned int seed = rd();
    // unsigned int seed = 2433201515;
    cout << "seed=" << seed << endl;
    eng.seed(seed);

    // 生成边界
    GenerateBorder();

    if (map_flag == 0)
    {
        GenerateRandomMap();
    }
    else if (map_flag == 1)
    {
        GenerateMap1();
    }
    else if (map_flag == 3)
    {
        GenerateMap3();
    }
    else
    {
        cout << RED << "[map_generator] wrong map_flag." << TAIL << endl;
    }

    ros::Rate loop_rate(100.0);

    while (ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
}


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

// Copyright (c) 2017 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_ENVELOPE_SEGMENT_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_ENVELOPE_SEGMENT_HPP

#include <boost/geometry/algorithms/detail/envelope/segment.hpp>
#include <boost/geometry/algorithms/detail/normalize.hpp>
#include <boost/geometry/strategies/envelope.hpp>
#include <boost/geometry/strategies/spherical/azimuth.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace envelope
{

template
<
    typename CalculationType = void
>
class spherical_segment
{
public :

    inline spherical_segment()
    {}

    template <typename Point1, typename Point2, typename Box>
    inline void
    apply(Point1 const& point1, Point2 const& point2, Box& box) const
    {
        Point1 p1_normalized = detail::return_normalized<Point1>(point1);
        Point2 p2_normalized = detail::return_normalized<Point2>(point2);

        geometry::strategy::azimuth::spherical<CalculationType> azimuth_spherical;

        typedef typename coordinate_system<Point1>::type::units units_type;

        geometry::detail::envelope::envelope_segment_impl<spherical_equatorial_tag>
                ::template apply<units_type>(geometry::get<0>(p1_normalized),
                                             geometry::get<1>(p1_normalized),
                                             geometry::get<0>(p2_normalized),
                                             geometry::get<1>(p2_normalized),
                                             box,
                                             azimuth_spherical);

  }
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <typename CalculationType>
struct default_strategy<spherical_equatorial_tag, CalculationType>
{
    typedef strategy::envelope::spherical_segment<CalculationType> type;
};

template <typename CalculationType>
struct default_strategy<spherical_polar_tag, CalculationType>
{
    typedef strategy::envelope::spherical_segment<CalculationType> type;
};

}

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::envelope

}} //namepsace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_ENVELOPE_SEGMENT_HPP


#include <pybind11/eigen.h>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>
#include <pybind11/pytypes.h>
#include <pybind11/stl.h>
#include <iostream>

#include <string>
#include <toppra/geometric_path.hpp>
#include <toppra/constraint.hpp>
#include <toppra/constraint/joint_torque.hpp>
#include <toppra/constraint/linear_joint_acceleration.hpp>
#include <toppra/constraint/linear_joint_velocity.hpp>

namespace py = pybind11;

#ifdef BUILD_WITH_PINOCCHIO
// Mind that pinocchio headers **must** be included before boost headers.
#include <toppra/constraint/joint_torque/pinocchio.hpp>
#ifdef PINOCCHIO_WITH_PYTHON_INTERFACE
#include <boost/python.hpp>
#endif // PINOCCHIO_WITH_PYTHON_INTERFACE
#endif // BUILD_WITH_PINOCCHIO

namespace toppra {
namespace python {

using namespace constraint;

void exposeConstraints(py::module m)
{
  py::enum_<DiscretizationType>(m, "DiscretizationType")
      .value("Collocation", DiscretizationType::Collocation)
      .value("Interpolation", DiscretizationType::Interpolation)
      .export_values();

  // Abstract class must be binded for derived classes to work
  py::class_<LinearConstraint, LinearConstraintPtr>(m, "LinearConstraint")
      .def_property_readonly("nbConstraints", &LinearConstraint::nbConstraints)
      .def_property_readonly("nbVariables", &LinearConstraint::nbVariables)
      .def_property_readonly("hasLinearInequalities",
                             &LinearConstraint::hasLinearInequalities)
      .def_property_readonly("hasUbounds", &LinearConstraint::hasUbounds)
      .def_property_readonly("hasXbounds", &LinearConstraint::hasXbounds)
      .def_property("discretizationType",
                    (DiscretizationType(LinearConstraint::*)() const) &
                        LinearConstraint::discretizationType,
                    (void (LinearConstraint::*)(DiscretizationType)) &
                        LinearConstraint::discretizationType)

      .def ("computeParams", [](LinearConstraint& constraint, const GeometricPath& path, const Vector& gridpoints) -> py::tuple {
            Vectors a, b, c, g;
            Matrices F;
            Bounds ub, xb;
            constraint.computeParams(path, gridpoints, a, b, c, F, g, ub, xb);
            py::list list;
            py::none none;
            if (constraint.hasLinearInequalities()) {
              list.append(a);
              list.append(b);
              list.append(c);
              list.append(F);
              list.append(g);
            } else
              for (int i = 0; i < 5; ++i) list.append(none);
            if (constraint.hasUbounds()) list.append(ub);
            else list.append(none);
            if (constraint.hasXbounds()) list.append(xb);
            else list.append(none);
            return list;
          }
        );
    ;
  py::class_<LinearJointVelocity,
    std::shared_ptr<LinearJointVelocity>,
    LinearConstraint>(
      m, "LinearJointVelocity")
      .def(py::init<const Vector &, const Vector &>())
      ;
  py::class_<LinearJointAcceleration,
    std::shared_ptr<LinearJointAcceleration>,
    LinearConstraint>(
      m, "LinearJointAcceleration")
      .def(py::init<const Vector &, const Vector &>())
      ;

  py::class_<JointTorque,
    std::shared_ptr<JointTorque>,
    LinearConstraint>(
      m, "JointTorque");

  {
    auto mod_jointTorque = m.def_submodule("jointTorque");
#ifdef BUILD_WITH_PINOCCHIO
    using jointTorque::Pinocchio;
#ifdef PINOCCHIO_WITH_PYTHON_INTERFACE
    py::module::import("pinocchio");
#endif
    py::class_<Pinocchio<>, std::shared_ptr<Pinocchio<> >, JointTorque>(
        mod_jointTorque, "Pinocchio")
        .def(py::init<const std::string&, const Vector&>(),
            py::arg("urdfFilename"), py::arg("frictionCoeffs") = Vector())
#ifdef PINOCCHIO_WITH_PYTHON_INTERFACE
        .def(py::init([](py::object model, const Vector& fc) -> Pinocchio<>* {
              return new Pinocchio<>(
                  boost::python::extract<const pinocchio::Model&>(model.ptr()), fc);
            }), py::keep_alive<1, 2>(), py::arg("model"), py::arg("frictionCoeffs") = Vector())
#endif
      ;
#endif
  }
}
}  // namespace python
}  // namespace toppra


#pragma once

// geometry.hpp
#include <mapbox/geometry.hpp>
#include <mapbox/geometry/algorithms/detail/boost_adapters.hpp>
// vtzero
#include <vtzero/builder.hpp>
#include <vtzero/property_mapper.hpp>
// boost
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/algorithms/intersects.hpp>
// stl
#include <algorithm>
//BOOST_GEOMETRY_REGISTER_POINT_2D(mapbox::geometry::point<int>, int, boost::geometry::cs::cartesian, x, y)
// ^ Uncomment to enable coordinate_type = int ^

namespace vtile {
namespace detail {

template <typename CoordinateType>
struct point_handler
{
    using geom_type = mapbox::geometry::multi_point<CoordinateType>;
    point_handler(geom_type& geom, std::uint32_t dx, std::uint32_t dy, std::uint32_t zoom_factor, mapbox::geometry::box<CoordinateType> const& bbox)
        : geom_(geom),
          bbox_(bbox),
          dx_(dx),
          dy_(dy),
          zoom_factor_(zoom_factor)
    {
    }

    void points_begin(std::uint32_t /*unused*/)
    {
    }

    void points_point(vtzero::point const& pt)
    {
        CoordinateType x = pt.x * static_cast<std::int32_t>(zoom_factor_) - static_cast<std::int32_t>(dx_);
        CoordinateType y = pt.y * static_cast<std::int32_t>(zoom_factor_) - static_cast<std::int32_t>(dy_);
        mapbox::geometry::point<CoordinateType> pt0{x, y};
        if (boost::geometry::covered_by(pt0, bbox_))
        {
            geom_.push_back(std::move(pt0));
        }
    }

    void points_end() {}

    geom_type& geom_;
    mapbox::geometry::box<CoordinateType> const& bbox_;
    std::uint32_t const dx_;
    std::uint32_t const dy_;
    std::uint32_t const zoom_factor_;
};

template <typename CoordinateType>
struct line_string_handler
{
    using geom_type = mapbox::geometry::multi_line_string<CoordinateType>;

    line_string_handler(geom_type& geom, std::uint32_t dx, std::uint32_t dy, std::uint32_t zoom_factor)
        : geom_(geom),
          dx_(dx),
          dy_(dy),
          zoom_factor_(zoom_factor)
    {
    }

    void linestring_begin(std::uint32_t count)
    {
        first_ = true;
        geom_.emplace_back();
        geom_.back().reserve(count);
    }

    void linestring_point(vtzero::point const& pt)
    {
        if (first_ || pt.x != cur_x_ || pt.y != cur_y_)
        {
            CoordinateType x = pt.x * static_cast<std::int32_t>(zoom_factor_) - static_cast<std::int32_t>(dx_);
            CoordinateType y = pt.y * static_cast<std::int32_t>(zoom_factor_) - static_cast<std::int32_t>(dy_);
            geom_.back().emplace_back(x, y);
            cur_x_ = pt.x;
            cur_y_ = pt.y;
            first_ = false;
        }
    }

    void linestring_end() {}

    geom_type& geom_;
    std::uint32_t const dx_;
    std::uint32_t const dy_;
    std::uint32_t const zoom_factor_;
    CoordinateType cur_x_ = 0;
    CoordinateType cur_y_ = 0;
    bool first_ = true;
};

template <typename CoordinateType>
using annotated_ring = std::pair<mapbox::geometry::linear_ring<CoordinateType>, vtzero::ring_type>;

template <typename CoordinateType>
struct polygon_handler
{
    using geom_type = std::vector<annotated_ring<CoordinateType>>;
    polygon_handler(geom_type& geom, std::uint32_t dx, std::uint32_t dy, std::uint32_t zoom_factor)
        : geom_(geom),
          dx_(dx),
          dy_(dy),
          zoom_factor_(zoom_factor) {}

    void ring_begin(std::uint32_t count)
    {
        first_ = true;
        geom_.emplace_back();
        geom_.back().first.reserve(count);
    }

    void ring_point(vtzero::point const& pt)
    {
        if (first_ || pt.x != cur_x_ || pt.y != cur_y_)
        {
            CoordinateType x = pt.x * static_cast<std::int32_t>(zoom_factor_) - static_cast<std::int32_t>(dx_);
            CoordinateType y = pt.y * static_cast<std::int32_t>(zoom_factor_) - static_cast<std::int32_t>(dy_);
            geom_.back().first.emplace_back(x, y);
            cur_x_ = pt.x;
            cur_y_ = pt.y;
            first_ = false;
        }
    }

    void ring_end(vtzero::ring_type type)
    {
        geom_.back().second = type;
    }

    geom_type& geom_;
    std::uint32_t const dx_;
    std::uint32_t const dy_;
    std::uint32_t const zoom_factor_;
    CoordinateType cur_x_ = 0;
    CoordinateType cur_y_ = 0;
    bool first_ = true;
};

} // namespace detail

template <typename CoordinateType>
struct overzoomed_feature_builder
{
    using coordinate_type = CoordinateType;
    overzoomed_feature_builder(vtzero::layer_builder& layer_builder,
                               vtzero::property_mapper& mapper,
                               mapbox::geometry::box<coordinate_type> const& bbox,
                               std::uint32_t dx, std::uint32_t dy, std::uint32_t zoom_factor)
        : layer_builder_{layer_builder},
          mapper_{mapper},
          bbox_{bbox},
          dx_{dx},
          dy_{dy},
          zoom_factor_{zoom_factor} {}

    template <typename FeatureBuilder>
    void finalize(FeatureBuilder& builder, vtzero::feature const& feature)
    {
        // add properties
        builder.copy_properties(feature, mapper_);
        builder.commit();
    }

    void apply_geometry_point(vtzero::feature const& feature)
    {
        mapbox::geometry::multi_point<CoordinateType> multi_point;
        vtzero::decode_point_geometry(feature.geometry(), detail::point_handler<coordinate_type>(multi_point, dx_, dy_, zoom_factor_, bbox_));
        if (!multi_point.empty())
        {
            vtzero::point_feature_builder feature_builder{layer_builder_};
            feature_builder.copy_id(feature);
            feature_builder.add_points_from_container(multi_point);
            finalize(feature_builder, feature);
        }
    }

    void apply_geometry_linestring(vtzero::feature const& feature)
    {
        mapbox::geometry::multi_line_string<CoordinateType> multi_line;
        vtzero::decode_linestring_geometry(feature.geometry(), detail::line_string_handler<coordinate_type>(multi_line, dx_, dy_, zoom_factor_));
        std::vector<mapbox::geometry::line_string<coordinate_type>> result;
        boost::geometry::intersection(multi_line, bbox_, result);
        if (!result.empty())
        {
            vtzero::linestring_feature_builder feature_builder{layer_builder_};
            feature_builder.copy_id(feature);
            bool valid = false;
            for (auto const& l : result)
            {
                if (l.size() > 1)
                {
                    feature_builder.add_linestring(static_cast<unsigned>(l.size()));
                    auto itr = l.cbegin();
                    auto last_pt = *itr++;
                    feature_builder.set_point(static_cast<int>(last_pt.x), static_cast<int>(last_pt.y));
                    for (auto const& end = l.end(); itr != end; ++itr)
                    {
                        if (*itr != last_pt)
                        {
                            valid = true;
                            feature_builder.set_point(static_cast<int>(itr->x), static_cast<int>(itr->y));
                            last_pt = *itr;
                        }
                    }
                }
            }
            if (valid)
            {
                finalize(feature_builder, feature);
            }
        }
    }
    void apply_geometry_polygon(vtzero::feature const& feature)
    {
        std::vector<detail::annotated_ring<CoordinateType>> rings;
        vtzero::decode_polygon_geometry(feature.geometry(), detail::polygon_handler<CoordinateType>(rings, dx_, dy_, zoom_factor_));
        std::vector<mapbox::geometry::polygon<CoordinateType>> polygons;
        bool process = false;
        for (auto const& r : rings)
        {
            if (r.second == vtzero::ring_type::outer)
            {
                auto extent = mapbox::geometry::envelope(r.first);
                process = boost::geometry::intersects(extent, bbox_);
                if (process)
                {
                    polygons.emplace_back(); // start new polygon
                }
            }
            if (process && r.first.size() > 3)
            {
                polygons.back().push_back(std::move(r.first));
            }
        }
        if (!polygons.empty())
        {
            vtzero::polygon_feature_builder feature_builder{layer_builder_};
            feature_builder.copy_id(feature);
            bool valid = false;
            for (auto const& poly : polygons)
            {
                std::vector<mapbox::geometry::polygon<coordinate_type>> result;
                boost::geometry::intersection(poly, bbox_, result);
                for (auto const& p : result)
                {
                    for (auto const& ring : p)
                    {
                        if (ring.size() > 3)
                        {
                            valid = true;
                            feature_builder.add_ring(static_cast<unsigned>(ring.size()));
                            std::for_each(ring.begin(), ring.end(),
                                          [&feature_builder](auto const& pt) { feature_builder.set_point(static_cast<int>(pt.x), static_cast<int>(pt.y)); });
                        }
                    }
                }
            }
            if (valid)
            {
                finalize(feature_builder, feature);
            }
        }
    }

    void apply(vtzero::feature const& feature)
    {
        switch (feature.geometry_type())
        {
        case vtzero::GeomType::POINT:
            apply_geometry_point(feature);
            break;
        case vtzero::GeomType::LINESTRING:
            apply_geometry_linestring(feature);
            break;
        case vtzero::GeomType::POLYGON:
            apply_geometry_polygon(feature);
            break;
        default:
            // LCOV_EXCL_START
            break;
            // LCOV_EXCL_STOP
        }
    }
    vtzero::layer_builder& layer_builder_;
    vtzero::property_mapper& mapper_;
    mapbox::geometry::box<coordinate_type> const& bbox_;
    std::uint32_t dx_;
    std::uint32_t dy_;
    std::uint32_t zoom_factor_;
};

} // namespace vtile


#include "FeatureAnnotation.h"
#include <iostream>
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include "ActiveLearning/AnnotatedFeatureRecord.h"
#include "ActiveLearning/DataView.h"
#include "ActiveLearning/ActiveLearningData.h"
#include "ActiveLearning/SmoothedL1.h"

using Eigen::VectorXd;
using Eigen::SparseVector;
using boost::lexical_cast;

FeatureAnnotationLoss::FeatureAnnotationLoss(InferenceDataView_ptr data,
	ActiveLearningData_ptr alData, double lossWeight, Loss lossFunc)
: _data(data), _alData(alData), _lossWeight(lossWeight), _lossFunc(lossFunc) {}

std::wstring FeatureAnnotationLoss::status() const {
	return L"feat_loss: " + lexical_cast<std::wstring>(_debug_loss);
}

void FeatureAnnotationLoss::snapshot() {
}

double FeatureAnnotationLoss::forInstance(int inst, SparseVector<double>& gradient) const {
	double ret = 0.0;
	BOOST_FOREACH(AnnotatedFeatureRecord_ptr ann_feature, _alData->annotatedFeatures()) {
		int idx = ann_feature->idx;

		if (_data->features(inst).coeff(idx) > 0) {
			double observed = _data->features(inst).coeff(idx) * _data->prediction(inst);
			bool violated = ((observed < ann_feature->expectation
				&& ann_feature->positive_annotation) ||
				(observed > ann_feature->expectation
				&& !ann_feature->positive_annotation));

			if (violated) {
				double diff = ann_feature->expectation - observed;
				/*double scale = ann_feature->expectation / observed
				-  (1.0 - ann_feature->expectation)
				/ (1.0 - observed);
				std::cout << "idx: " << ann_feature->idx << "; diff: " << diff
				<< "; scale: " << scale << std::endl;*/
				ret -= _lossWeight * diff * diff;
				double common_factors = _lossWeight * 2.0 * diff;

				SparseVector<double>::InnerIterator it(_data->features(inst));
				double factor = common_factors *
					_data->prediction(inst)*(1.0-_data->prediction(inst))
					/ _data->frequency(idx);
				for (; it; ++it) {
					gradient.coeffRef(it.index()) += factor * it.value();
				}
			}
		}
	}
	return ret;
}

double FeatureAnnotationLoss::operator()(VectorXd& gradient, size_t thread_idx,
		size_t n_threads) const
{
	// Calculate penalties for annotated features
	// see Druck, et. al. "j Annotation Effort using
	// Generalized Expectation Criteria", UMass Tech Report UM-CS-2007-62

	double loss = 0.0;
	double dbg = 0.0;

	BOOST_FOREACH(AnnotatedFeatureRecord_ptr ann_feature,
					_alData->annotatedFeatures())
	{
		int idx = ann_feature->idx;
		// first gather expectations by current model ("observed")
		double observed = 0.0;
		SparseVector<double>::InnerIterator instIt(_data->instancesWithFeature(idx));
		for (; instIt; ++instIt) {
			observed += _data->prediction(instIt.index());
		}

		// then calculate penalty
		observed /= _data->frequency(idx);
		bool violated = ((observed < ann_feature->expectation
									&& ann_feature->positive_annotation) ||
								(observed > ann_feature->expectation
									&& !ann_feature->positive_annotation));

		double scale = ann_feature->expectation / observed
										-  (1.0 - ann_feature->expectation)
										/ (1.0 - observed);

		if (_lossFunc == KL) {
			if (violated) {
				if (_lossFunc == KL) {
					loss -= _data->frequency(idx)*_lossWeight *
						(ann_feature->expectation
						* log(ann_feature->expectation/observed) +
						(1.0 - ann_feature->expectation)
						* log((1.0 - ann_feature->expectation)/(1.0-observed)));

					double common_factors = _lossWeight * scale;
					SparseVector<double>::InnerIterator instIt(_data->instancesWithFeature(idx));

					for (; instIt; ++instIt) {
						int inst = instIt.index();
						if (inst % n_threads == thread_idx) {
							double factor = common_factors * _data->prediction(inst)
								*(1.0-_data->prediction(inst));
							for (SparseVector<double>::InnerIterator it(_data->features(inst)); it; ++it) {
								gradient(it.index()) += factor*it.value();
							}
						}
					}
				}
			}
		} else { // _lossFunc == L1
				const double epsilon = SmoothedL1::DEFAULT_EPSILON;
				double diff = ann_feature->expectation - observed;

				if (!violated) {
					diff = 0;
				}

				double localLoss = sqrt(diff * diff + epsilon);
				loss -= _lossWeight * localLoss;

				if (violated) {
					double commonCoeff = _lossWeight * diff/localLoss;

					SparseVector<double>::InnerIterator instIt(_data->instancesWithFeature(idx));

					for (; instIt; ++instIt) {
						int inst = instIt.index();
						if (inst % n_threads == thread_idx) {
							double coeff = commonCoeff * _data->prediction(inst)
								*(1.0-_data->prediction(inst));
							for (SparseVector<double>::InnerIterator it(_data->features(inst)); it; ++it) {
								gradient(it.index()) += coeff*it.value();
							}
						}
					}
				}
			}
		}

	// warning: hack
	// processing of instances is parallelized but of features is not
	// since LL is calculated by features for this objective, we only want
	// to return the LL once, so we do it in thread 0
	_debug_loss = loss;
	return (thread_idx == 0)?loss:0.0;
}


//==================================================================================================
/*!
  @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_SIMD_SINHCOSH_INCLUDED
#define BOOST_SIMD_FUNCTION_SIMD_SINHCOSH_INCLUDED

#include <boost/simd/function/scalar/sinhcosh.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/generic/function/sinhcosh.hpp>

#endif


#include "DocumentRanker.h"
#include <boost/thread/thread.hpp>
#include <atomic>

DocumentRanker::DocumentRanker(TokenVectorCreator* token_vector_creator, TextProcessor* text_processor) :
        token_vector_creator(token_vector_creator), text_processor(text_processor) {}

vector<float> DocumentRanker::score_documents(const string &query, const vector<IndexedDocument*>& documents) const {
    auto query_tokens = text_processor->process_text(query);
    SparseVector query_vector = token_vector_creator->create_sparse_vector(query_tokens);
    vector<float> document_scores(documents.size());

    std::atomic<long> index(0);

    boost::thread_group workers;
    auto worker_lambda = [&]() {
        long current_index = index++;
        while ( current_index < document_scores.size()) {
            document_scores[current_index] = query_vector.angle(documents[current_index]->getTfIdfVector());
            current_index = index++;
        }
    };

    int number_of_workers = 4;
    for (int i = 0; i < number_of_workers; i++) {
        workers.create_thread(worker_lambda);
    }
    workers.join_all();
    return document_scores;
}


/*
 Copyright (C) 2017 Sascha Meiers
 Distributed under the MIT software license, see the accompanying
 file LICENSE.md or http://www.opensource.org/licenses/mit-license.php.
 */

#include <iostream>
#include <fstream>
#include <vector>
#include <unordered_map>
#include <tuple>

#include <boost/program_options/cmdline.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/tokenizer.hpp>
#include <boost/filesystem.hpp>
#include <boost/progress.hpp>
#include <boost/iostreams/stream.hpp>
#include <boost/iostreams/stream_buffer.hpp>
#include <boost/iostreams/device/file.hpp>
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/filter/zlib.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <htslib/sam.h>

#include "version.hpp"
#include "intervals.hpp"
#include "counter.hpp"
#include "distribution.hpp"
#include "hmm.hpp"
#include "iocounts.hpp"

using interval::Interval;
using count::TGenomeCounts;
using count::Counter;

std::vector<std::string> get_states(unsigned ploidy)
{
    assert(ploidy>0 && ploidy < 6);
    std::vector<std::string> states;
    for (unsigned len=1; len < ploidy+1; ++len) {
        for (unsigned w = 0; w <= len; ++w) {

            unsigned c = len - w;
            std::string s;
            for (unsigned i = 0; i < w; ++i)
                s.push_back('W');
            for (unsigned i = 0; i < c; ++i)
                s.push_back('C');
            states.push_back(std::move(s));
        }
    }
    return states;
}

void setup_HMM_emissions(hmm::HMM<unsigned, hmm::MultiVariate<hmm::NegativeBinomial>> & hmm,
                         unsigned ploidy,
                         double nb_p,
                         double nb_r,
                         double nb_a,
                         double prior,
                         unsigned max_ploidy = 4)
{
    assert(max_ploidy>0 && max_ploidy<6);
    std::vector<hmm::MultiVariate<hmm::NegativeBinomial>> emissions;

    double prior_others = (1-prior)/static_cast<double>(max_ploidy);

    //std::cout << "MultiVariate<NegativeBinomial>:" << std::endl;
    for (unsigned len=1; len < max_ploidy+1; ++len) {
        for (unsigned w = 0; w <= len; ++w) {
            // e.g. WWWW (with N=4) should be  NB[p,a] x NB[p,(1-a)r]
            // or   WWWC                       NB[p,3/4r] x NB[p,1/4r]
            // but  WW ??        I decide for  NB[p,2/4r] x NB[p,a]
            unsigned c = len - w;
            double fac_w = (w==0 ? nb_a : w/static_cast<double>(max_ploidy) );
            double fac_c = (c==0 ? nb_a : c/static_cast<double>(max_ploidy) );
            hmm::MultiVariate<hmm::NegativeBinomial> distribution
                ({  hmm::NegativeBinomial(nb_p, fac_w * nb_r),
                    hmm::NegativeBinomial(nb_p, fac_c * nb_r)
                }, (len == ploidy ? prior : prior_others));
            emissions.push_back(std::move(distribution));

            // DEBUG
            //std::cout << std::setprecision(3) << "\t" << "p = " << nb_p << "\tcell_mean = " << nb_r/nb_p*(1-nb_p) << "\tState = ";
            //for (unsigned x = 0; x < w; ++x) std::cout << "W";
            //for (unsigned x = 0; x < c; ++x) std::cout << "C";
            //std::cout << "\tFactors = [" << fac_w << "\t" << fac_c << "]\t means = [" << fac_w * nb_r/nb_p*(1-nb_p) << "\t" <<  fac_c * nb_r/nb_p*(1-nb_p) << "]" << std::endl;

        }
    }
    //std::cout << "-" << std::endl;
    hmm.set_emissions(emissions);
}

void setup_HMM_emissions(hmm::HMM<unsigned, hmm::CombinedNegBinAndBinomial> & hmm,
                         unsigned ploidy,
                         double nb_p,
                         double nb_r,
                         double nb_a,
                         double prior,
                         unsigned max_ploidy)
{
    assert(max_ploidy>0 && max_ploidy<6);
    std::vector<hmm::CombinedNegBinAndBinomial> emissions;

    double prior_others = (1-prior)/static_cast<double>(max_ploidy);

    //std::cout << "CombinedNegBinAndBinomial:" << std::endl;
    for (unsigned len=1; len < max_ploidy+1; ++len) {
        for (unsigned w = 0; w <= len; ++w) {

            unsigned c = len - w;
            double ratio = (c == 0 || c == len ? (c == 0 ? 0.02 : 0.98) : static_cast<double>(c)/static_cast<double>(len) );
            double r     = (len == 0 ? nb_a : nb_r * static_cast<double>(len)/static_cast<double>(max_ploidy));
            emissions.push_back(hmm::CombinedNegBinAndBinomial(nb_p, r, ratio, (len == ploidy ? prior : prior_others)));

            // DEBUG
            //std::cout << std::setprecision(3) << "\t" << "p = " << nb_p << "\tcell_mean = " << nb_r/nb_p*(1-nb_p) << "\tState = ";
            //for (unsigned x = 0; x < w; ++x) std::cout << "W";
            //for (unsigned x = 0; x < c; ++x) std::cout << "C";
            //std::cout << "\t NB mean = " << r/nb_p*(1-nb_p) << "\tBinomial ratio = " << ratio << std::endl;

        }
    }
    //std::cout << "-" << std::endl;
    hmm.set_emissions(emissions);

    std::cout << "NB params for ploidy " << ploidy << ": p = " << nb_p << "\tr = " << nb_r << "\tmean = " << nb_r/nb_p*(1-nb_p) << std::endl;
}

template <typename TDistribution>
typename hmm::HMM<unsigned, TDistribution> setup_HMM(unsigned max_ploidy, double p_trans)
{
    assert(max_ploidy>0 && max_ploidy<6);
    std::vector<std::string> states = get_states(max_ploidy);
    std::vector<double> initials(states.size(), 1/static_cast<double>(states.size()));
    std::vector<double> transitions(states.size() * states.size(), p_trans);
    for (unsigned i = 0; i < states.size(); ++i) {
        transitions[i * states.size() + i] = 1 - (states.size() - 1) * p_trans;
    }
    hmm::HMM<unsigned, TDistribution> hmm(states);
    hmm.set_initials(initials);
    hmm.set_transitions(transitions);
    return hmm;
}

template <typename TDistribution>
void run_generic_HMM(std::vector<TGenomeCounts> & counts,
                     std::vector<unsigned> const & good_cells,
                     std::vector<CellInfo>  & cells,
                     std::vector<unsigned> const & good_bins,
                     std::vector<int32_t> const & good_map,
                     std::unordered_map<std::string, SampleInfo> const & samples,
                     float p_trans,
                     double prior,
                     unsigned ploidy = 2,
                     unsigned max_ploidy = 4)
{
    // Set up and run HMM:
    hmm::HMM<unsigned, TDistribution> hmm = setup_HMM<TDistribution>(max_ploidy, p_trans);

    //std::cout << "HMM states: ";
    //for (auto x: hmm.state_labels) {std::cout << x << "\t";}
    //std::cout << std::endl;

    for (auto i = good_cells.begin(); i != good_cells.end(); ++i)
    {
        // set NB(n,p) parameters according to `p` of sample and mean of cell.
        float p = samples.at(cells[*i].sample_name).p;
        float r = (float)cells[*i].mean_bin_count * p / (1-p);
        float a = 0.05;

        setup_HMM_emissions(hmm, ploidy, p, r, a, prior, max_ploidy);
        run_HMM(hmm, counts[*i], good_bins, good_map);
    }

}

struct Conf_ploidy {
    std::vector<boost::filesystem::path> f_in;
    boost::filesystem::path f_out;
    boost::filesystem::path f_bins;
    boost::filesystem::path f_excl;
    boost::filesystem::path f_info;
    boost::filesystem::path f_segments;
    unsigned ploidy;
    unsigned max_ploidy;
    int minMapQual;
    unsigned int window;
    std::string model;
    double prior;
    Conf_ploidy() : max_ploidy(4)
    {}
};

int main_hmm(int argc, char **argv)
{

    // Command line options
    Conf_ploidy conf;
    boost::program_options::options_description generic("Generic options");
    generic.add_options()
    ("help,?", "show help message")
    ("verbose,v", "Be more verbose in the output")
    ("mapq,q", boost::program_options::value<int>(&conf.minMapQual)->default_value(10), "min mapping quality")
    ("window,w", boost::program_options::value<unsigned int>(&conf.window)->default_value(500000), "window size of fixed windows")
    ("out,o", boost::program_options::value<boost::filesystem::path>(&conf.f_out)->default_value("out.txt.gz"), "output file for counts + strand state (gz)")
    ("bins,b", boost::program_options::value<boost::filesystem::path>(&conf.f_bins), "BED file with manual bins (disables -w). See also 'makebins'")
    ("exclude,x", boost::program_options::value<boost::filesystem::path>(&conf.f_excl), "Exclude chromosomes and regions")
    ("info,i", boost::program_options::value<boost::filesystem::path>(&conf.f_info), "Write info about samples")
    ("ploidy,p", boost::program_options::value<unsigned>(&conf.ploidy)->default_value(2), "Assume cells have this ploidy level (max 4)")
    ("prior,P",  boost::program_options::value<double>(&conf.prior)->notifier(in_range(0,1,"prior")), "Prior probability for copy number <p> (penalize other copy number states. Default: All copy numbers are equally probable)")
    ("model,m", boost::program_options::value<std::string>(&conf.model)->default_value("multiNB"), "Models for HMM: multiNB, Binomial+NB")
    ;

    boost::program_options::options_description hidden("Hidden options");
    hidden.add_options()
    ("input-file", boost::program_options::value<std::vector<boost::filesystem::path> >(&conf.f_in), "input bam file(s)")
    ;

    boost::program_options::positional_options_description pos_args;
    pos_args.add("input-file", -1);

    boost::program_options::options_description cmdline_options;
    cmdline_options.add(generic).add(hidden);
    boost::program_options::options_description visible_options;
    visible_options.add(generic);
    boost::program_options::variables_map vm;

    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(cmdline_options).positional(pos_args).run(), vm);
    boost::program_options::notify(vm);

    // Check command line arguments
    if (!vm["window"].defaulted() && vm.count("bins")) {
        std::cerr << "[Error] -w and -b cannot be specified together" << std::endl << std::endl;
        goto print_usage_and_exit;
    }
    if (vm.count("bins") && vm.count("exclude")) {
        std::cerr << "[Error] Exclude chromosomes (-x) have no effect when -b is specified. Stop" << std::endl << std::endl;
        goto print_usage_and_exit;
    }
    if (conf.model != "multiNB" && conf.model != "Binomial+NB") {
        std::cerr << "[Error] Unknown --model for the HMM." << std::endl << std::endl;
        goto print_usage_and_exit;
    }
    if (vm.count("prior") && (conf.prior < 0 || conf.prior > 1)) {
        std::cerr << "[Error] Prior probability (for CN " << conf.ploidy << ") has to be between 0 and 1. By default all CNs will be equally likely" << std::endl << std::endl;
        goto print_usage_and_exit;
    }

    if (vm.count("help") || !vm.count("input-file"))
    {
    print_usage_and_exit:
        std::cout << std::endl;
        std::cout << "Mosaicatcher " << STRINGIFYMACRO(MOSAIC_VERSION_MAJOR);
        std::cout << "." << STRINGIFYMACRO(MOSAIC_VERSION_MINOR) << std::endl;
        std::cout << "> Count reads from Strand-seq BAM files..." << std::endl;
        std::cout << "  Now for different ploidy levels, too!" << std::endl;
        std::cout << std::endl;
        std::cout << "Usage:   " << argv[0] << " --ploidy N [OPTIONS] <cell1.bam> <cell2.bam> ..." << std::endl << std::endl;
        std::cout << visible_options << std::endl;
        std::cout << "Notes:" << std::endl;
        std::cout << "  * writes a table of bin counts and state classifcation as a gzip file (default: out.txt.gz)" << std::endl;
        std::cout << "  * Reads are counted by start position" << std::endl;
        std::cout << "  * One cell per BAM file, including SM tag in header" << std::endl;
        std::cout << "  * For paired-end data, only read 1 is counted" << std::endl;
        return vm.count("help") ? 0 : 1;
    }

    /////////////////////////////////////////////////////////// global variables
    /* leave one BAM header open to get chrom names & lengths */
    bam_hdr_t* hdr = NULL;

    /* regarding each cell */
    std::vector<CellInfo>       cells(conf.f_in.size());
    std::vector<TGenomeCounts>  counts(conf.f_in.size());
    std::vector<unsigned>       good_cells;

    /* regarding each sample */
    std::unordered_map<std::string, SampleInfo> samples;

    /* regarding bins */
    std::vector<Interval>       bins;
    std::vector<int32_t>        chrom_map;
    std::vector<unsigned>       good_bins;
    std::vector<int32_t>        good_map;
    ////////////////////////////////////////////////////////////////////////////

    //
    // Chapter: Binning & counting
    // ===========================
    //

    // Read sample names from headers.
    // Keep one header throughout the program.
    if (vm.count("verbose")) std::cout << "[Info] Exploring SAM headers..." << std::endl;
    for(unsigned i = 0; i < conf.f_in.size(); ++i)
    {
        cells[i].id = (int32_t)i;
        cells[i].bam_file = conf.f_in[i].string();
        samFile* samfile = sam_open(conf.f_in[i].string().c_str(), "r");
        if (samfile == NULL) {
            std::cerr << "[Error] Fail to open file " << conf.f_in[i].string() << std::endl;
            return 1;
        }
        hdr = sam_hdr_read(samfile);
        if (!get_RG_tag("SM", hdr->text, cells[i].sample_name)) {
            std::cerr << "[Error] Each BAM file has to have exactly one RG tag. Group cells " << std::endl;
            std::cerr << "        belonging to the same sample by the SM tag." << std::endl;
            std::cerr << "        Problematic file: " << conf.f_in[i].string() << std::endl << std::endl;
            goto print_usage_and_exit;
        }
        if (!get_RG_tag("ID", hdr->text, cells[i].cell_name, /*allow_multiple_matches = */ true)) {
            std::cerr << "[Error] Each BAM file has to have exactly one RG tag." << std::endl;
            std::cerr << "        Problematic file: " << conf.f_in[i].string() << std::endl;
            goto print_usage_and_exit;
        }
        sam_close(samfile);
    }

    // Bin the genome
    unsigned median_binsize;
    chrom_map = std::vector<int32_t>(hdr->n_targets, -1);
    if (vm.count("bins"))
    {
        if (!read_dynamic_bins(bins,
                               chrom_map,
                               conf.f_bins.string().c_str(),
                               hdr))
            return 1;
        TMedianAccumulator<unsigned> med_acc;
        for (Interval const & b : bins)
            med_acc(b.end - b.start);
        median_binsize = boost::accumulators::median(med_acc);
        if (vm.count("verbose")) std::cout << "[Info] Reading " << bins.size() << " variable-width bins with median bin size of " << round(median_binsize/1000) << "kb" << std::endl;
    }
    else
    {
        std::vector<Interval> exclude;
        if (vm.count("exclude")) {
            read_exclude_file(conf.f_excl.string(), hdr, exclude, vm.count("verbose"));
            sort(exclude.begin(), exclude.end(), interval::invt_less);
        }
        if (vm.count("verbose")) std::cout << "[Info] Creating " << round(conf.window/1000) << "kb bins with " << exclude.size() << " excluded regions" << std::endl;
        create_fixed_bins(bins,
                          chrom_map,
                          conf.window,
                          exclude,
                          hdr->n_targets,
                          hdr->target_len);
        median_binsize = conf.window;
    }
    // add last element for easy calculation of number of bins
    chrom_map.push_back((int32_t)bins.size());

    // Count in bins. If A bam file cannot be read, the cell is ignored and
    //     the respective entry in `counts` and `cells` will be erased.
    if (vm.count("verbose")) std::cout << "[Info] Reading " << conf.f_in.size() <<  " BAM files...";
    boost::progress_display show_progress1(conf.f_in.size());
    for (unsigned i = 0, i_f = 0; i_f < conf.f_in.size(); ++i, ++i_f)
    {
        if (!count_sorted_reads(conf.f_in[i_f].string(),
                                bins,
                                chrom_map,
                                hdr,
                                conf.minMapQual,
                                counts[i],
                                cells[i]))
        {
            std::cerr << "[Warning] Ignoring cell " << conf.f_in[i_f].string() << std::endl;
            counts.erase(counts.begin()+i);
            cells.erase(cells.begin()+i);
            --i;
        }
        ++show_progress1;
    }

    //
    // Chapter: Filter cells and bins and estimate NB parameter p
    // ==========================================================
    //

    // median per cell
    count::set_median_per_cell(counts, cells);

    // filter cells with low counts
    good_cells = count::get_good_cells(counts, cells);

    // filter bins with abnormal counts
    if (good_cells.size() < 5) {
        std::cerr << "[Warning] Only few cells with sufficient coverage. I will not filter bad bins" << std::endl;
        good_bins.resize(bins.size());
        std::iota(good_bins.begin(), good_bins.end(), 0); // fill with 0,1,2,...
    } else {
        good_bins = count::get_good_bins(counts, cells, good_cells);
        if (vm.count("verbose")) std::cout << "[Info] Filtered out " << bins.size() - good_bins.size() << " bad bins" << std::endl;
    }

    // build chrom_map for good bins
    good_map = std::vector<int32_t>(chrom_map.size() - 1, -1);
    int32_t pos = 0;
    for (int32_t chr = 0; chr < static_cast<int32_t>(good_map.size()); ++chr) {
        while (pos < good_bins.size() && bins[good_bins[pos]].chr < chr)
            ++pos;
        // now goodit is either at first occurence of chr, or at the end.
        if (pos >= good_bins.size()) good_map[chr] = (int32_t)good_bins.size();
        else good_map[chr] = pos;
    }
    // add last element for easy calculation of number of bins
    good_map.push_back((int32_t)good_bins.size());

    // calculate cell means and cell variances, grouped by sample (not cell)
    calculate_new_cell_mean(samples, cells, counts, good_cells, good_bins);

    // Estimation of parameter p per sample (should work even with one cell only)
    for (auto it = samples.begin(); it != samples.end(); ++it) {
        SampleInfo & s = it->second;
        s.p = std::inner_product(s.means.begin(), s.means.end(), s.means.begin(), 0.0f) \
        / std::inner_product(s.means.begin(), s.means.end(), s.vars.begin(), 0.0f);
    }

    //
    // Chapter: Run HMM
    // ================
    //
    double prior = (vm.count("prior") ? conf.prior : 1.0/static_cast<double>(conf.max_ploidy));
    std::cout << "[Info] Running HMM with model " << conf.model << " and expected ploidy ";
    std::cout << std::setprecision(3) << conf.ploidy << " (prior = " << prior << ")" << std::endl;
    if (conf.model == "multiNB")
        run_generic_HMM<hmm::MultiVariate<hmm::NegativeBinomial>>(
                    counts,
                    good_cells,
                    cells,
                    good_bins,
                    good_map,
                    samples,
                    10.0f / bins.size(),
                    prior,
                    conf.ploidy,
                    conf.max_ploidy);
    if (conf.model == "Binomial+NB")
        run_generic_HMM<hmm::CombinedNegBinAndBinomial>(
                    counts,
                    good_cells,
                    cells,
                    good_bins,
                    good_map,
                    samples,
                    10.0f / bins.size(),
                    prior,
                    conf.ploidy,
                    conf.max_ploidy);

    // Print cell information:
    if (vm.count("info")) {
        if (vm.count("verbose")) std::cout << "[Write] Cell summary: " << conf.f_info.string() << std::endl;
        write_cell_info(conf.f_info.string(), cells);
    }

    // Write final counts + classification
    std::cout << "[Write] count table: " << conf.f_out.string() << std::endl;
    {
        // TODO: why do I pass vector<pair>? I could make it two separate vectors. Just check where else the io function is called.
        struct sample_cell_name_wrapper {
            std::vector<CellInfo> const & cells;
            sample_cell_name_wrapper(std::vector<CellInfo> const & cells) : cells(cells)
            {}
            std::pair<std::string,std::string> operator[](size_t i) const {
                return std::make_pair(cells[i].sample_name, cells[i].cell_name);
            }
        };

        if (!io::write_counts_gzip(conf.f_out.string(),
                                   counts,
                                   bins,
                                   hdr->target_name,
                                   sample_cell_name_wrapper(cells)) )
            return 1;
    }

    return 0;
}


#include "block.hpp"
#include "TileMap.hpp"
#include <SFML/System/Clock.hpp>
#include <SFML/System/Vector2.hpp>
#include <SFML/Window/Event.hpp>
#include <SFML/Window/Keyboard.hpp>
#include <string>
#include <vector>
#include <time.h>
#include <stdlib.h>
#include <Eigen/Dense>
#include <SFML/Graphics.hpp>
#include <ostream>

using namespace Eigen;

bool updateMove(int move, int &x, const int &y, const TetrisBlock &block, const MatrixXf &grid){
  /* Try to move the block horizontaly and return a boolean indicating of the operation was a succes or not*/

  if (block_position_possible(x + move, y, block, grid)){
    x += move;
    return true;
  }
  return false;
}

bool checkRotation(const int &x, const int &y, TetrisBlock block, const MatrixXf &grid){
  /* Check if a rotation is possible, it doesn't do any side effect */

  block.rotate();
  return block_position_possible(x, y, block, grid);
}

bool updateFalling(sf::Clock &clock, int &x, int &y, const TetrisBlock &block, MatrixXf &grid, float FALLING_TIME, float falling_multiplicator){
  /* This fonction handle the folling of a piece. It return false if the block can move or not enought time is elapsed and true if the block can't be placed. It change the position of y.
   */

  sf::Time elapsed = clock.getElapsedTime();
  if (elapsed.asSeconds() < FALLING_TIME * falling_multiplicator){
    return false;
  }
  clock.restart();
  // std::cout << x << "," << y << std::endl;
  if (block_position_possible(x, y+1, block, grid)){
    y += 1;
    return false;
  }

  add_block_to_grid(x, y, block, grid);

  return true;

}

int main() {
  srand (time(NULL));

  // simple types init
  const float FALLING_TIME = 0.5;
  float falling_multiplicator = 1;
  //2 top padding, 3 bottom padding
  const int nrows = 25;
  const int ncols = 16;
  const int visualNrows = 20;
  const int visualNcols = 10;
  const int xspawn = (ncols / 2) - 1;
  const int yspawn = 0;
  const int visualSize = 25;
  const int tileSize = 100;
  int x = xspawn;
  int y = yspawn;
  int bottom_padding = 2;
  int col_padding = 3;
  bool block_down = false;
  int move = 0;
  int line_index;
  bool gameOver = false;
  std::string fname {"../img/spritesheet.png"};

  // complexe type init
  MatrixXf grid(nrows, ncols);
  MatrixXf visualGrid(visualNrows, visualNcols);
  sf::RenderWindow window(sf::VideoMode(visualSize * visualNcols, visualSize * visualNrows), "Tetris");
  TileMap map;
  std::vector<int> line_indexes;
  sf::Clock clock;
  //get_block_map if defined in block.cpp
  auto block_map = get_block_map();

  // fonctions

  auto randint = [](int maxValue) -> int {return rand() % (maxValue); };

  auto get_random_block = [block_map, randint]() -> TetrisBlock {
			    auto random_it = std::next(std::begin(block_map), randint(block_map.size()));
			    return (*random_it).second;};

  //We use padding to handle contact with the sides of the grid
  auto init_grid = [](MatrixXf &grid, int nrows, int ncols) -> void {
    grid = MatrixXf::Zero(nrows, ncols);
    // bottom padding
    grid.row(nrows-1) = VectorXf::Ones(ncols);
    grid.row(nrows-2) = VectorXf::Ones(ncols);

    // left padding
    grid.col(0) = VectorXf::Ones(nrows);
    grid.col(1) = VectorXf::Ones(nrows);
    grid.col(2) = VectorXf::Ones(nrows);

    // right padding
    grid.col(ncols-1) = VectorXf::Ones(nrows);
    grid.col(ncols-2) = VectorXf::Ones(nrows);
    grid.col(ncols-3) = VectorXf::Ones(nrows);
      };

  auto delete_all_lines = [bottom_padding, col_padding](MatrixXf &grid, std::vector<int> &line_indexes) -> void {
				  int index;
			     	  line_indexes = find_lines(grid, bottom_padding);
				  while (!line_indexes.empty()){
				    index = line_indexes.back();
				    delete_line(index, grid, col_padding);
				    line_indexes.pop_back();
				  }
				  line_indexes = find_lines(grid, bottom_padding);
				};

  init_grid(grid, nrows, ncols);
  TetrisBlock block = get_random_block();

    // run the program as long as the window is open
    while (window.isOpen())
    {
        // check all the window's events that were triggered since the last iteration of the loop
        sf::Event event;
        while (window.pollEvent(event))
        {
            // "close requested" event: we close the window
	  if (event.type == sf::Event::Closed)
                window.close();

	if (sf::Keyboard::isKeyPressed(sf::Keyboard::Left)){
	  move = -1;
	}

	if (sf::Keyboard::isKeyPressed(sf::Keyboard::Right)){
	  move = 1;
	}

	if (sf::Keyboard::isKeyPressed((sf::Keyboard::Up))){
	  move = 2;
	}

	if (sf::Keyboard::isKeyPressed((sf::Keyboard::Down))){
	  move = 3;
	}

      }

	falling_multiplicator = 1;

	if ((move == -1) || (move == 1)){
	  updateMove(move, x, y, block, grid);
	}

	if (move == 2){
	  if (checkRotation(x, y, block, grid)){
	    block.rotate();
	  }
	}

	if (move == 3){
	  falling_multiplicator = 0.15;
	}

	move = 0;

	block_down = updateFalling(clock, x, y, block, grid, FALLING_TIME, falling_multiplicator);

	if (!line_indexes.empty()){
	  delete_all_lines(grid, line_indexes);
	}

	if (block_down){
	  //create a new block
	  x = xspawn;
	  y = yspawn;
	  block = get_random_block();

	  if (!block_position_possible(x, y, block, grid)){
	    gameOver = true;
	  }
	  delete_all_lines(grid, line_indexes);
	}

	if (gameOver){
	    std::cout << "Game over !" << std::endl;
	    window.close();
	  }

	visualGrid = get_visualGrid(x, y, visualNrows, visualNcols,  block, grid, 3, 3);

	map.load(fname, sf::Vector2u(tileSize, tileSize), sf::Vector2u(visualSize, visualSize), visualGrid);

        // clear the window with black color
        window.clear(sf::Color::Black);

        // draw everything here...
    	window.draw(map);
        // end the current frame
        window.display();
    }

      return 0;

}


// Autogenerated from KST: please remove this line if doing any edits by hand!

#include <boost/test/unit_test.hpp>
#include "floating_points.h"
#include <iostream>
#include <fstream>
#include <vector>

BOOST_AUTO_TEST_CASE(test_floating_points) {
    std::ifstream ifs("src/floating_points.bin", std::ifstream::binary);
    kaitai::kstream ks(&ifs);
    floating_points_t* r = new floating_points_t(&ks);

    BOOST_CHECK_CLOSE(r->single_value(), static_cast<float>(0.5), 100 * (1e-6) / (static_cast<float>(0.5)));
    BOOST_CHECK_CLOSE(r->single_value_be(), static_cast<float>(0.5), 100 * (1e-6) / (static_cast<float>(0.5)));
    BOOST_CHECK_CLOSE(r->double_value(), 0.25, 100 * (1e-6) / (0.25));
    BOOST_CHECK_CLOSE(r->double_value_be(), 0.25, 100 * (1e-6) / (0.25));
    BOOST_CHECK_CLOSE(r->approximate_value(), 1.2345, 100 * (1e-6) / (1.2345));
    BOOST_CHECK_CLOSE(r->single_value_plus_int(), 1.5, 100 * (1e-6) / (1.5));
    BOOST_CHECK_CLOSE(r->single_value_plus_float(), 1.0, 100 * (1e-6) / (1.0));
    BOOST_CHECK_CLOSE(r->double_value_plus_float(), 0.3, 100 * (1e-6) / (0.3));

    delete r;
}


#pragma once

#include <prc/rank.hpp>
#include <prc/suit.hpp>
#include <prc/suitedness.hpp>

#include <iosfwd>

#include <boost/fusion/include/io.hpp>
#include <boost/variant.hpp>

namespace prc::parser::ast
{
struct card
{
  prc::rank rank;
  prc::suit suit;
};

struct combo
{
  ast::card first_card;
  ast::card second_card;
};

struct unpaired_hand
{
  prc::rank first_rank;
  prc::rank second_rank;
  prc::suitedness suitedness;
};

struct paired_hand
{
  prc::rank rank;
};

using hand = boost::variant<paired_hand, unpaired_hand>;

struct hand_range
{
  hand from;
  hand to;
};

using range_elem = boost::variant<combo, hand_range, hand>;

bool operator==(card const& lhs, card const& rhs);
bool operator!=(card const& lhs, card const& rhs);

bool operator==(combo const& lhs, combo const& rhs);
bool operator!=(combo const& lhs, combo const& rhs);

bool operator==(unpaired_hand const& lhs, unpaired_hand const& rhs);
bool operator!=(unpaired_hand const& lhs, unpaired_hand const& rhs);

bool operator==(paired_hand const& lhs, paired_hand const& rhs);
bool operator!=(paired_hand const& lhs, paired_hand const& rhs);

bool operator==(hand_range const& lhs, hand_range const& rhs);
bool operator!=(hand_range const& lhs, hand_range const& rhs);

using boost::fusion::operator<<;

std::ostream& operator<<(std::ostream&, card const&);
std::ostream& operator<<(std::ostream&, combo const&);
std::ostream& operator<<(std::ostream&, unpaired_hand const&);
std::ostream& operator<<(std::ostream&, paired_hand const&);
std::ostream& operator<<(std::ostream&, hand_range const&);

inline namespace literals
{
range_elem operator"" _ast_re(char const*, std::size_t);
}
}


#include <boost/mpl/modulus.hpp>


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/xray/scatterer.h>

#include <scitbx/array_family/boost_python/flex_wrapper.h>
#include <scitbx/array_family/boost_python/flex_pickle_double_buffered.h>
#include <scitbx/array_family/boost_python/ref_pickle_double_buffered.h>
#include <boost/python/args.hpp>
#include <boost/python/return_internal_reference.hpp>

namespace scitbx { namespace af { namespace boost_python {

namespace {

  struct to_string : pickle_double_buffered::to_string
  {
    using pickle_double_buffered::to_string::operator<<;

    to_string()
    {
      unsigned version = 3;
      *this << version;
    }

    to_string& operator<<(cctbx::xray::scatterer<> const& val)
    {
      *this << val.label
            << val.scattering_type
            << val.fp
            << val.fdp;
      *this << val.site.const_ref()
            << val.occupancy
            << val.u_iso;
      *this << val.u_star.const_ref()
            << val.multiplicity()
            << val.weight_without_occupancy();
      *this << val.flags.bits
            << val.flags.param;
      return *this;
    }
  };

  struct from_string : pickle_double_buffered::from_string
  {
    from_string(const char* str_ptr)
    : pickle_double_buffered::from_string(str_ptr)
    {
      *this >> version;
      CCTBX_ASSERT(version == 2 || version == 3);
    }

    using pickle_double_buffered::from_string::operator>>;

    from_string& operator>>(cctbx::xray::scatterer<>& val)
    {
      bool obsolete_anisotropic_flag;
      int multiplicity;
      cctbx::xray::scatterer<>::float_type weight_without_occupancy;
      *this >> val.label
            >> val.scattering_type
            >> val.fp
            >> val.fdp;
      *this >> val.site.ref()
            >> val.occupancy;
      if (version == 2) {
        *this >> obsolete_anisotropic_flag;
      }
      *this >> val.u_iso;
      *this >> val.u_star.ref()
            >> multiplicity
            >> weight_without_occupancy;
      *this >> val.flags.bits
            >> val.flags.param;
      val.setstate(multiplicity, weight_without_occupancy);
      return *this;
    }

    unsigned version;
  };

}}}} // namespace scitbx::af::boost_python::<anonymous>

namespace cctbx { namespace xray { namespace {

  af::shared<std::string>
  extract_labels(af::const_ref<scatterer<> > const& self)
  {
    af::shared<std::string> result(af::reserve(self.size()));
    for(std::size_t i=0;i<self.size();i++) {
      result.push_back(self[i].label);
    }
    return result;
  }

  af::shared<std::string>
  extract_scattering_types(af::const_ref<scatterer<> > const& self)
  {
    af::shared<std::string> result(af::reserve(self.size()));
    for(std::size_t i=0;i<self.size();i++) {
      result.push_back(self[i].scattering_type);
    }
    return result;
  }

  af::shared<scitbx::vec3<double> >
  extract_sites(af::const_ref<scatterer<> > const& scatterers)
  {
    af::shared<scitbx::vec3<double> >
      result(af::reserve(scatterers.size()));
    for(std::size_t i=0;i<scatterers.size();i++) {
      result.push_back(scatterers[i].site);
    }
    return result;
  }

  void
  set_sites(
    af::ref<scatterer<> > const& scatterers,
    af::const_ref<scitbx::vec3<double> > const& sites)
  {
    CCTBX_ASSERT(scatterers.size() == sites.size());
    for(std::size_t i=0;i<scatterers.size();i++) {
      scatterers[i].site = sites[i];
    }
  }

  af::shared<double>
  extract_occupancies(af::const_ref<scatterer<> > const& scatterers)
  {
    af::shared<double>
      result(af::reserve(scatterers.size()));
    for(std::size_t i=0;i<scatterers.size();i++) {
      result.push_back(scatterers[i].occupancy);
    }
    return result;
  }

  void
  set_occupancies(
    af::ref<scatterer<> > const& scatterers,
    af::const_ref<double> const& occupancies)
  {
    CCTBX_ASSERT(scatterers.size() == occupancies.size());
    for(std::size_t i=0;i<scatterers.size();i++) {
      scatterers[i].occupancy = occupancies[i];
    }
  }

  void
  set_occupancies(
    af::ref<scatterer<> > const& scatterers,
    af::const_ref<double> const& occupancies,
    af::const_ref<bool> const& selection)
  {
    CCTBX_ASSERT(scatterers.size() == occupancies.size());
    CCTBX_ASSERT(scatterers.size() == selection.size());
    for(std::size_t i=0;i<scatterers.size();i++) {
      if(selection[i]) {
         scatterers[i].occupancy = occupancies[i];
      }
    }
  }

  int
  n_grad_u_iso(
    af::const_ref<scatterer<> > const& self)
  {
    int result = 0;
    for(std::size_t i=0;i<self.size();i++) {
        if(self[i].flags.use_u_iso() && self[i].flags.grad_u_iso()) {
           result++;
        }
    }
    return result;
  }

  int
  n_grad_u_aniso(
    af::const_ref<scatterer<> > const& self)
  {
    int result = 0;
    for(std::size_t i=0;i<self.size();i++) {
        if(self[i].flags.use_u_aniso() && self[i].flags.grad_u_aniso()) {
           result++;
        }
    }
    return result;
  }

  af::shared<bool>
  extract_grad_u_iso(
    af::const_ref<scatterer<> > const& self)
  {
    af::shared<bool> result(af::reserve(self.size()));
    for(std::size_t i=0;i<self.size();i++) {
      result.push_back(self[i].flags.grad_u_iso());
    }
    return result;
  }

  af::shared<double>
  extract_u_iso(
    af::const_ref<scatterer<> > const& self)
  {
    af::shared<double> result(af::reserve(self.size()));
    for(std::size_t i=0;i<self.size();i++) {
      result.push_back(self[i].u_iso);
      //XXX Feature change: don't change & return. Instead, return whatever is
      //                    there.
      //if (!self[i].flags.use_u_iso()) {
      //  result.push_back(-1);
      //}
      //else {
      //  result.push_back(self[i].u_iso);
      //}
    }
    return result;
  }

  af::shared<double>
  extract_u_iso_or_u_equiv(
    af::const_ref<scatterer<> > const& self,
    uctbx::unit_cell const* unit_cell)
  {
    af::shared<double> result(af::reserve(self.size()));
    for(std::size_t i=0;i<self.size();i++) {
      result.push_back(self[i].u_iso_or_equiv(unit_cell));
    }
    return result;
  }

  af::shared<scitbx::sym_mat3<double> >
  extract_u_cart_plus_u_iso(
    af::const_ref<scatterer<> > const& self,
    uctbx::unit_cell const* unit_cell)
  {
    af::shared<scitbx::sym_mat3<double> > result(af::reserve(self.size()));
    for(std::size_t i=0;i<self.size();i++) {
      result.push_back(self[i].u_cart_plus_u_iso(unit_cell));
    }
    return result;
  }

  void
  scale_adps(
    af::ref<scatterer<> > const& self,
    double scale_factor)
  {
    CCTBX_ASSERT(scale_factor > 0);
    for (std::size_t i =0; i < self.size(); i++) {
      if (self[i].flags.use_u_iso()) {
        self[i].u_iso *= scale_factor;
      } else if (self[i].flags.use_u_aniso()) {
        self[i].u_star *= scale_factor;
      }
    }
  }

  af::shared<scitbx::vec3<double> >
  u_cart_eigenvalues(
    af::const_ref<scatterer<> > const& self,
    uctbx::unit_cell const& unit_cell)
  {
    af::shared<scitbx::vec3<double> > result(af::reserve(self.size()));
    for(std::size_t i=0;i<self.size();i++) {
        scitbx::sym_mat3<double> u_cart;
        if(self[i].u_star != scitbx::sym_mat3<double>(-1,-1,-1,-1,-1,-1)) {
           u_cart = adptbx::u_star_as_u_cart(unit_cell, self[i].u_star);
        }
        else {
           CCTBX_ASSERT(self[i].u_iso >= 0.);
           u_cart = adptbx::u_iso_as_u_cart(self[i].u_iso);
        }
        result.push_back( adptbx::eigenvalues(u_cart) );
    }
    return result;
  }

  af::shared<double>
  anisotropy(
    af::const_ref<scatterer<> > const& self,
    uctbx::unit_cell const& unit_cell)
  {
    af::shared<double> result(af::reserve(self.size()));
    for(std::size_t i=0;i<self.size();i++) {
        scitbx::sym_mat3<double> u_cart;
        if(self[i].u_star != scitbx::sym_mat3<double>(-1,-1,-1,-1,-1,-1)) {
           u_cart = adptbx::u_star_as_u_cart(unit_cell, self[i].u_star);
           scitbx::vec3<double> ev = adptbx::eigenvalues(u_cart);
           double ev_max = af::max(ev);
           double ev_min = af::min(ev);
           if(ev_max == ev_min) {
              result.push_back( 1.0 );
           }
           else {
              CCTBX_ASSERT(ev_max != 0.0);
              result.push_back( af::min(ev)/ev_max );
           }
        }
        else {
           result.push_back( 1.0 );
        }
    }
    return result;
  }

  void
  set_u_iso(
    af::ref<scatterer<> > const& self,
    af::const_ref<double> const& u_iso,
    af::const_ref<bool> const& selection,
    uctbx::unit_cell const& unit_cell)
  {
    CCTBX_ASSERT(self.size() == u_iso.size());
    CCTBX_ASSERT(self.size() == selection.size());
    for(std::size_t i=0;i<self.size();i++) {
      if(self[i].flags.use_u_iso() && selection[i]) {
         self[i].u_iso = u_iso[i];
      }
      if(self[i].flags.use_u_aniso() && selection[i]) {
         self[i].u_star = adptbx::u_cart_as_u_star(unit_cell,
           scitbx::sym_mat3<double>(u_iso[i],u_iso[i],u_iso[i],0,0,0));
      }
    }
  }

  void
  adjust_u_iso(
    af::ref<scatterer<> > const& self)
  {
    double u_mean = 0.0;
    double b_min  = 0.5;
    double b_max  = 500.0;
    double min_factor = 8.0;
    double max_factor = 6.0;
    int counter = 0;
    for(std::size_t i=0;i<self.size();i++) {
      if(self[i].flags.use() && self[i].u_iso != -1.0) {
         if(self[i].u_iso < 0.0) self[i].u_iso = 0.0;
         u_mean = u_mean + self[i].u_iso;
         counter = counter + 1;
      }
    }
    u_mean = u_mean / counter;
    double u_min = std::max(adptbx::b_as_u(b_min), u_mean / min_factor);
    u_mean = 0.0;
    counter = 0;
    for(std::size_t i=0;i<self.size();i++) {
      if(self[i].flags.use() && self[i].u_iso != -1.0) {
         if(self[i].u_iso < u_min) self[i].u_iso = u_min;
         u_mean = u_mean + self[i].u_iso;
         counter = counter + 1;
      }
    }
    u_mean = u_mean / counter;
    double u_max = std::min(adptbx::b_as_u(b_max), u_mean * max_factor);
    for(std::size_t i=0;i<self.size();i++) {
      if(self[i].flags.use() && self[i].u_iso != -1.0) {
         if(self[i].u_iso > u_max) self[i].u_iso = u_max;
      }
    }
  }

  af::shared<scitbx::sym_mat3<double> >
  extract_u_star(af::const_ref<scatterer<> > const& self)
  {
    af::shared<scitbx::sym_mat3<double> > result(af::reserve(self.size()));
    for(std::size_t i=0;i<self.size();i++) {
      if (self[i].flags.use_u_aniso()) {
        result.push_back(self[i].u_star);
      }
      else {
        result.push_back(scitbx::sym_mat3<double>(-1,-1,-1,-1,-1,-1));
      }
    }
    return result;
  }

  void
  set_u_star(
    af::ref<scatterer<> > const& self,
    af::const_ref<scitbx::sym_mat3<double> > const& u_star)
  {
    CCTBX_ASSERT(self.size() == u_star.size());
    for(std::size_t i=0;i<self.size();i++) {
      if (self[i].flags.use_u_aniso()) {
        self[i].u_star = u_star[i];
      }
    }
  }

  af::shared<scitbx::sym_mat3<double> >
  extract_u_cart(
    af::const_ref<scatterer<> > const& self,
    uctbx::unit_cell const& unit_cell)
  {
    af::shared<scitbx::sym_mat3<double> > result(af::reserve(self.size()));
    for(std::size_t i=0;i<self.size();i++) {
      if(self[i].u_star != scitbx::sym_mat3<double>(-1,-1,-1,-1,-1,-1)) {
        result.push_back(adptbx::u_star_as_u_cart(unit_cell, self[i].u_star));
      }
      else {
        result.push_back(scitbx::sym_mat3<double>(-1,-1,-1,-1,-1,-1));
      }
      //XXX Feature change: don't change & return. Instead, return whatever is
      //                    there.
      //if (self[i].flags.use_u_aniso()) {
      //  result.push_back(adptbx::u_star_as_u_cart(unit_cell, self[i].u_star));
      //}
      //else {
      //  result.push_back(scitbx::sym_mat3<double>(-1,-1,-1,-1,-1,-1));
      //}
    }
    return result;
  }

  af::shared<bool>
  extract_use_u_iso(
    af::const_ref<scatterer<> > const& self)
  {
    af::shared<bool> result(af::reserve(self.size()));
    for(std::size_t i=0;i<self.size();i++) {
      result.push_back(self[i].flags.use_u_iso());
    }
    return result;
  }

  af::shared<bool>
  extract_use_u_aniso(
    af::const_ref<scatterer<> > const& self)
  {
    af::shared<bool> result(af::reserve(self.size()));
    for(std::size_t i=0;i<self.size();i++) {
      result.push_back(self[i].flags.use_u_aniso());
    }
    return result;
  }

  void
  set_u_cart(
    af::ref<scatterer<> > const& self,
    uctbx::unit_cell const& unit_cell,
    af::const_ref<scitbx::sym_mat3<double> > const& u_cart)
  {
    CCTBX_ASSERT(self.size() == u_cart.size());
    for(std::size_t i=0;i<self.size();i++) {
      if (self[i].flags.use_u_aniso()) {
        self[i].u_star = adptbx::u_cart_as_u_star(unit_cell, u_cart[i]);
      }
    }
  }

  void
  set_u_cart(
    af::ref<scatterer<> > const& self,
    uctbx::unit_cell const& unit_cell,
    af::const_ref<scitbx::sym_mat3<double> > const& u_cart,
    af::const_ref<std::size_t> const& selection)
  {
    CCTBX_ASSERT(self.size() == u_cart.size());
    for(std::size_t j=0;j<selection.size();j++) {
      std::size_t i=selection[j];
      CCTBX_ASSERT(i<self.size());
      CCTBX_ASSERT(self[i].flags.use_u_aniso());
      self[i].u_star = adptbx::u_cart_as_u_star(unit_cell, u_cart[i]);
    }
  }

  void
  convert_to_isotropic(
    af::ref<scatterer<> > const& self,
    uctbx::unit_cell const& unit_cell)
  {
    for(std::size_t i=0;i<self.size();i++) {
      self[i].convert_to_isotropic(unit_cell);
    }
  }

  void
  convert_to_isotropic(
    af::ref<scatterer<> > const& self,
    uctbx::unit_cell const& unit_cell,
    af::const_ref<std::size_t> const& selection)
  {
    for(std::size_t j=0;j<selection.size();j++) {
      std::size_t i=selection[j];
      self[i].convert_to_isotropic(unit_cell);
    }
  }

  void
  convert_to_anisotropic(
    af::ref<scatterer<> > const& self,
    uctbx::unit_cell const& unit_cell)
  {
    for(std::size_t i=0;i<self.size();i++) {
      self[i].convert_to_anisotropic(unit_cell);
    }
  }

  void
  convert_to_anisotropic(
    af::ref<scatterer<> > const& self,
    uctbx::unit_cell const& unit_cell,
    af::const_ref<bool> const& selection)
  {
    for(std::size_t i=0;i<self.size();i++) {
      if(selection[i]) {
        self[i].convert_to_anisotropic(unit_cell);
      }
    }
  }

  void
  convert_to_anisotropic(
    af::ref<scatterer<> > const& self,
    uctbx::unit_cell const& unit_cell,
    af::const_ref<std::size_t> const& selection)
  {
    for(std::size_t j=0;j<selection.size();j++) {
      std::size_t i=selection[j];
      self[i].convert_to_anisotropic(unit_cell);
    }
  }

  std::size_t
  count_anisotropic(af::const_ref<scatterer<> > const& self)
  {
    std::size_t result = 0;
    for(std::size_t i=0;i<self.size();i++) {
      if (self[i].flags.use_u_aniso()) result++;
    }
    return result;
  }

  std::size_t
  count_anomalous(af::const_ref<scatterer<> > const& self)
  {
    std::size_t result = 0;
    for(std::size_t i=0;i<self.size();i++) {
      if (self[i].fdp != 0) result++;
    }
    return result;
  }

  af::shared<scatterer<> >
  sites_mod_positive(af::shared<scatterer<> > const& self)
  {
    af::shared<scatterer<> > result = self.deep_copy();
    scatterer<>* sc = result.begin();
    const scatterer<>* sc_end = result.end();
    for(scatterer<>* sc=result.begin();sc!=sc_end;sc++) {
      sc->site = sc->site.mod_positive();
    }
    return result;
  }

  af::shared<scatterer<> >
  sites_mod_short(af::shared<scatterer<> > const& self)
  {
    af::shared<scatterer<> > result = self.deep_copy();
    scatterer<>* sc = result.begin();
    const scatterer<>* sc_end = result.end();
    for(scatterer<>* sc=result.begin();sc!=sc_end;sc++) {
      sc->site = sc->site.mod_short();
    }
    return result;
  }

}}} // namespace cctbx::xray::<anonymous>

namespace scitbx { namespace af { namespace boost_python {

  template <>
  struct flex_default_element<cctbx::xray::scatterer<> >
  {
    static cctbx::xray::scatterer<>
    get()
    {
      return cctbx::xray::scatterer<>(
        "", cctbx::fractional<>(0,0,0), 0, 0, "", 0, 0);
    }
  };

  void wrap_flex_xray_scatterer()
  {
    using namespace cctbx;
    using namespace boost::python;
    using boost::python::arg;

    flex_wrapper<cctbx::xray::scatterer<>,
                 return_internal_reference<>
                >::plain("xray_scatterer")
      .def_pickle(flex_pickle_double_buffered<
        cctbx::xray::scatterer<>, to_string, from_string>())
      .def("extract_labels", cctbx::xray::extract_labels)
      .def("extract_scattering_types", cctbx::xray::extract_scattering_types)
      .def("extract_sites", cctbx::xray::extract_sites)
      .def("set_sites", cctbx::xray::set_sites,
        (arg("sites")))
      .def("extract_occupancies", cctbx::xray::extract_occupancies)
      .def("extract_grad_u_iso", cctbx::xray::extract_grad_u_iso)
      .def("set_occupancies", (void(*)(
              af::ref<cctbx::xray::scatterer<> > const&,
              af::const_ref<double> const&)) cctbx::xray::set_occupancies,
              (arg("occupancies")))
      .def("set_occupancies", (void(*)(
              af::ref<cctbx::xray::scatterer<> > const&,
              af::const_ref<double> const&,
              af::const_ref<bool> const&)) cctbx::xray::set_occupancies,
              (arg("occupancies"),arg("selection")))
      .def("adjust_u_iso", cctbx::xray::adjust_u_iso)
      .def("n_grad_u_iso", cctbx::xray::n_grad_u_iso)
      .def("n_grad_u_aniso", cctbx::xray::n_grad_u_aniso)
      .def("extract_u_iso", cctbx::xray::extract_u_iso)
      .def("extract_use_u_iso", cctbx::xray::extract_use_u_iso)
      .def("extract_use_u_aniso", cctbx::xray::extract_use_u_aniso)
      .def("extract_u_iso_or_u_equiv", cctbx::xray::extract_u_iso_or_u_equiv,
        (arg("unit_cell")))
      .def("extract_u_cart_plus_u_iso", cctbx::xray::extract_u_cart_plus_u_iso,
        (arg("unit_cell")))
      .def("scale_adps", cctbx::xray::scale_adps,
        (arg("scale_factor")))
      .def("u_cart_eigenvalues", cctbx::xray::u_cart_eigenvalues,
        (arg("unit_cell")))
      .def("anisotropy", cctbx::xray::anisotropy,
        (arg("unit_cell")))
      .def("set_u_iso", cctbx::xray::set_u_iso,
        (arg("u_iso"),arg("selection"),arg("unit_cell")))
      .def("extract_u_star", cctbx::xray::extract_u_star)
      .def("set_u_star", cctbx::xray::set_u_star,
        (arg("u_star")))
      .def("extract_u_cart", cctbx::xray::extract_u_cart,
        (arg("unit_cell")))
      .def("set_u_cart", (void(*)(
              af::ref<cctbx::xray::scatterer<> > const&,
              uctbx::unit_cell const&,
              af::const_ref<scitbx::sym_mat3<double> > const&)) cctbx::xray::set_u_cart,
              (arg("unit_cell"),arg("u_cart")))
      .def("set_u_cart", (void(*)(
              af::ref<cctbx::xray::scatterer<> > const&,
              uctbx::unit_cell const&,
              af::const_ref<scitbx::sym_mat3<double> > const&,
              af::const_ref<std::size_t> const&)) cctbx::xray::set_u_cart,
              (arg("unit_cell"),arg("u_cart"),arg("selection")))
      .def("convert_to_isotropic", (void(*)(
              af::ref<cctbx::xray::scatterer<> > const&,
              uctbx::unit_cell const&)) cctbx::xray::convert_to_isotropic,
              (arg("unit_cell")))
      .def("convert_to_isotropic", (void(*)(
              af::ref<cctbx::xray::scatterer<> > const&,
              uctbx::unit_cell const&,
              af::const_ref<std::size_t> const&)) cctbx::xray::convert_to_isotropic,
              (arg("unit_cell"),arg("selection")))
      .def("convert_to_anisotropic", (void(*)(
              af::ref<cctbx::xray::scatterer<> > const&,
              uctbx::unit_cell const&)) cctbx::xray::convert_to_anisotropic,
              (arg("unit_cell")))
      .def("convert_to_anisotropic", (void(*)(
              af::ref<cctbx::xray::scatterer<> > const&,
              uctbx::unit_cell const&,
              af::const_ref<bool> const&)) cctbx::xray::convert_to_anisotropic,
              (arg("unit_cell"),arg("selection")))
      .def("convert_to_anisotropic", (void(*)(
              af::ref<cctbx::xray::scatterer<> > const&,
              uctbx::unit_cell const&,
              af::const_ref<std::size_t> const&)) cctbx::xray::convert_to_anisotropic,
              (arg("unit_cell"),arg("selection")))
      .def("count_anisotropic", cctbx::xray::count_anisotropic)
      .def("count_anomalous", cctbx::xray::count_anomalous)
      .def("sites_mod_positive", cctbx::xray::sites_mod_positive)
      .def("sites_mod_short", cctbx::xray::sites_mod_short)
      .def("flags_set_grads",
        (void(*)(
          af::ref<cctbx::xray::scatterer<> > const&, bool))
            cctbx::xray::flags_set_grads, (
              arg("state")))
#define CCTBX_XRAY_SCATTERERS_SET_BPL_DEF(attr) \
      .def("flags_set_" #attr, \
        (void(*)( \
          af::ref<cctbx::xray::scatterer<> > const&, \
          af::const_ref<std::size_t> const&)) \
            cctbx::xray::flags_set_##attr, ( \
              arg("iselection")))
      CCTBX_XRAY_SCATTERERS_SET_BPL_DEF(grad_site)
      CCTBX_XRAY_SCATTERERS_SET_BPL_DEF(grad_u_iso)
      CCTBX_XRAY_SCATTERERS_SET_BPL_DEF(grad_u_aniso)
      CCTBX_XRAY_SCATTERERS_SET_BPL_DEF(grad_occupancy)
      CCTBX_XRAY_SCATTERERS_SET_BPL_DEF(grad_fp)
      CCTBX_XRAY_SCATTERERS_SET_BPL_DEF(grad_fdp)
    ;
  }

}}} // namespace scitbx::af::boost_python


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generate the Inf IEEE value (\f$\infty\f$) for floating types and maximum value
    for integer types

    @par Header <boost/simd/constant/inf.hpp>

    @par Semantic

    For any type @c T,

    @code
    T x = Inf<T>();
    @endcode

    return IEEE Inf for floating types otherwise the maximum representable value for this type.

    @return The largest value of the input type
  **/
  template<typename T> T Inf();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      @return The largest representable value of the input type
    **/
    Value Inf();
  }
} }
#endif

#include <boost/simd/constant/scalar/inf.hpp>
#include <boost/simd/constant/simd/inf.hpp>

#endif


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "ParserFlatbuffersFixture.hpp"
#include "../TfLiteParser.hpp"

#include <string>
#include <iostream>

using armnnTfLiteParser::TfLiteParser;

BOOST_AUTO_TEST_SUITE(TensorflowLiteParser)

struct ResizeBilinearFixture : public ParserFlatbuffersFixture
{
    explicit ResizeBilinearFixture(const std::string & inputShape,
                                   const std::string & outputShape,
                                   const std::string & sizeShape,
                                   const std::string & sizeData)
    {
        m_JsonString = R"(
            {
                "version": 3,
                "operator_codes": [ { "builtin_code": "RESIZE_BILINEAR" } ],
                "subgraphs": [ {
                    "tensors": [
                        {
                            "shape": )" + sizeShape + R"( ,
                            "type": "INT32",
                            "buffer": 0,
                            "name": "sizeTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.0 ],
                                "zero_point": [ 0 ],
                            }
                        },
                        {
                            "shape": )" + inputShape + R"(,
                            "type": "FLOAT32",
                            "buffer": 1,
                            "name": "InputTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.0 ],
                                "zero_point": [ 0 ],
                            }
                        },
                        {
                            "shape": )" + outputShape + R"( ,
                            "type": "FLOAT32",
                            "buffer": 2,
                            "name": "OutputTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.0 ],
                                "zero_point": [ 0 ],
                            }
                        }
                    ],
                "inputs": [ 1 ],
                "outputs": [ 2 ],
                "operators": [
                    {
                        "opcode_index": 0,
                        "inputs": [ 1, 0 ],
                        "outputs": [ 2 ],
                        "builtin_options_type": "ResizeBilinearOptions",
                        "builtin_options": {
                        },
                        "custom_options_format": "FLEXBUFFERS"
                    }
                ],
              } ],
              "buffers" : [
                  { "data": )" + sizeData + R"(, },
                  { },
                  { },
              ]
            }
      )";
      Setup();
    }
};

struct SimpleResizeBilinearFixture : ResizeBilinearFixture
{
    SimpleResizeBilinearFixture()
        : ResizeBilinearFixture("[ 1, 3, 3, 1 ]",         // inputShape
                                "[ 1, 5, 5, 1 ]",         // outputShape
                                "[ 2 ]",                  // sizeShape
                                "[  5,0,0,0, 5,0,0,0 ]")  // sizeData
    {}
};

BOOST_FIXTURE_TEST_CASE(ParseResizeBilinear, SimpleResizeBilinearFixture)
{
    RunTest<4, armnn::DataType::Float32>(
                0,
                {{"InputTensor", { 0.0f, 1.0f, 2.0f,
                                   3.0f, 4.0f, 5.0f,
                                   6.0f, 7.0f, 8.0f }}},
                {{"OutputTensor", { 0.0f, 0.6f, 1.2f, 1.8f, 2.0f,
                                    1.8f, 2.4f, 3.0f, 3.6f, 3.8f,
                                    3.6f, 4.2f, 4.8f, 5.4f, 5.6f,
                                    5.4f, 6.0f, 6.6f, 7.2f, 7.4f,
                                    6.0f, 6.6f, 7.2f, 7.8f, 8.0f }}}
                );
}

BOOST_AUTO_TEST_SUITE_END()


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

  @copyright 2017 NumScale SAS

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

#include <boost/simd/range/segmented_aligned_range.hpp>
#include <boost/simd/function/all.hpp>
#include <boost/simd/pack.hpp>
#include <boost/simd/detail/is_aligned.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <algorithm>

namespace boost { namespace simd
{
  /*!
    @ingroup group-algo

    Returns an iterator to the first element in the range [first,last)
    for which pred returns false.
    If no such element is found, the function returns last.

    @param first  Beginning of the range of elements
    @param last   End of the range of elements
    @param pred   Predicate to apply to each element in the range.

    @par Requirement

      - @c first, @c last and @c out must be pointer to Vectorizable type.

    @par Example:

      @snippet find_if_not.cpp find_if_not

    @par possible output:

      @snippet find_if_not.txt find_if_not
  **/
  template<typename T, typename Pred>
  T const* find_if_not(T const* first, T const* last, Pred const& pred)
  {
    auto pr = segmented_aligned_range(first,last);
    auto safe_pred = [&pred](T x){ return pred(x); };

    auto r = std::find_if_not(pr.head.begin(), pr.head.end(), safe_pred);
    if (r != pr.head.end()) return r;

    auto rv = std::find_if_not( pr.body.begin(), pr.body.end()
                              , [&pred](pack<T> const& x){return all(pred(x)); }
                              );
    if (rv != pr.body.end())
    {
      auto z =  std::find_if_not(rv->begin(), rv->end(), safe_pred);
      return  pr.head.end()
            + std::distance(pr.body.begin(), rv)*pack<T>::static_size
            + std::distance(rv->begin(), z);
    }

    return std::find_if_not(pr.tail.begin(), pr.tail.end(), safe_pred);
  }
} }

#endif