/***************************************************************************************************************************
 * landpad_det.cpp
 * Author: Jario
 * Update Time: 2020.1.12
 *
 * 说明: 降落目标识别程序，降落板的尺寸为60cmX60cm
 *      1. 【订阅】图像话题 (默认来自web_cam)
 *         /prometheus/camera/rgb/image_raw
 *      2. 【发布】目标位置，发布话题见 Prometheus/Modules/msgs/msg/DetectionInfo.msg
 *         /prometheus/object_detection/landpad_det
 *      3. 【发布】检测结果的可视化图像话题
 *         /prometheus/camera/rgb/image_landpad_det
***************************************************************************************************************************/

#include <time.h>
#include <fstream>
#include <iostream>
#include <math.h>
#include <pthread.h>
#include <thread>
#include <chrono>
#include <boost/thread/mutex.hpp>
#include <boost/thread/shared_mutex.hpp>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

// ros头文件
#include <ros/ros.h>
#include <yaml-cpp/yaml.h>
#include <ros/package.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>

// opencv头文件
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/dictionary.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/calib3d.hpp>

// topic 头文件
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <std_msgs/Bool.h>
#include <prometheus_msgs/DetectionInfo.h>
#include <prometheus_msgs/Message.h>

#include "message_utils.h"

using namespace std;
using namespace cv;

double threshold_error=0.4;

//---------------------------variables---------------------------------------
//------------ROS TOPIC---------
//【订阅】无人机位置
ros::Subscriber drone_pose_sub;
//【订阅】小车位置
ros::Subscriber vehicle_pose_sub;
//【订阅】输入图像
image_transport::Subscriber image_subscriber;
//【订阅】输入开关量
ros::Subscriber switch_subscriber;
//【发布】无人机和小车相对位置
ros::Publisher position_pub;
//【发布】识别后的图像
image_transport::Publisher landpad_pub;
//【发布】调试消息
ros::Publisher message_pub;
std::string msg_node_name;

//-------------VISION-----------
Mat img;
prometheus_msgs::DetectionInfo pose_now;

//-------------TIME-------------
ros::Time begin_time;
float photo_time;
double calculation_time;

// 相机话题中的图像同步相关变量
int frame_width, frame_height;
std_msgs::Header image_header;
cv::Mat cam_image_copy;
boost::shared_mutex mutex_image_callback;
bool image_status = false;
boost::shared_mutex mutex_image_status;

// 无人机位姿message
geometry_msgs::Pose pos_drone_optitrack;
Eigen::Vector3d euler_drone_optitrack;
Eigen::Quaterniond q_drone;
// 小车位姿message
geometry_msgs::Pose pos_vehicle_optitrack;
Eigen::Vector3d euler_vehicle_optitrack;
Eigen::Quaterniond q_vehicle;

// 保存的上次观测的位置 用于cluster算法使用
Eigen::Vector3d last_position;
bool bool_last_position = false;
// 接收消息，允许暂停检测
bool is_suspanded = false;
bool local_print = false;
bool message_print = true;

void printf_result();

//-----------------利用Euler角进行三次旋转得到无人机相对目标的位置------------------
void CodeRotateByZ(double x, double y, double thetaz, double& outx, double& outy)
{
    double x1 = x;  // 将变量拷贝一次，保证&x == &outx这种情况下也能计算正确
    double y1 = y;
    double rz = thetaz * CV_PI / 180;
    outx = cos(rz) * x1 - sin(rz) * y1;
    outy = sin(rz) * x1 + cos(rz) * y1;
}
void CodeRotateByY(double x, double z, double thetay, double& outx, double& outz)
{
    double x1 = x;
    double z1 = z;
    double ry = thetay * CV_PI / 180;
    outx = cos(ry) * x1 + sin(ry) * z1;
    outz = cos(ry) * z1 - sin(ry) * x1;
}
void CodeRotateByX(double y, double z, double thetax, double& outy, double& outz)
{
    double y1 = y;  // 将变量拷贝一次，保证&y == &y这种情况下也能计算正确
    double z1 = z;
    double rx = thetax * CV_PI / 180;
    outy = cos(rx) * y1 - sin(rx) * z1;
    outz = cos(rx) * z1 + sin(rx) * y1;
}
// 四元数转Euler
// q0 q1 q2 q3
// w x y z
void quaternion_2_euler(Eigen::Quaterniond quat, Eigen::Vector3d &angle)
{
    angle(0) = atan2(2.0 * (quat.z() * quat.y() + quat.w() * quat.x()), 1.0 - 2.0 * (quat.x() * quat.x() + quat.y() * quat.y()));
    angle(1) = asin(2.0 * (quat.y() * quat.w() - quat.z() * quat.x()));
    // angle[2] = atan2(2.0 * (quat[3] * quat[0] + quat[1] * quat[2]), -1.0 + 2.0 * (quat[0] * quat[0] + quat[1] * quat[1]));
    angle(2) = atan2(2.0 * (quat.z() * quat.w() + quat.x() * quat.y()), 1.0 - 2.0 * (quat.y() * quat.y() + quat.z() * quat.z()));
}

//--------------------------利用optitrack获取真值-------------------------------

// 获取系统时间
float get_dt(ros::Time last)
{
    ros::Time time_now = ros::Time::now();
    float currTimeSec = time_now.sec-last.sec;
    float currTimenSec = time_now.nsec / 1e9 - last.nsec / 1e9;
    return (currTimeSec + currTimenSec);
}

// 图像接收回调函数，接收web_cam的话题，并将图像保存在cam_image_copy中
void cameraCallback(const sensor_msgs::ImageConstPtr& msg)
{
    if (local_print)
        ROS_DEBUG("[LandpadDetector] USB image received.");

    cv_bridge::CvImagePtr cam_image;

    try {
        cam_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        image_header = msg->header;
    } catch (cv_bridge::Exception& e) {
        if (local_print)
            ROS_ERROR("cv_bridge exception: %s", e.what());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::ERROR, msg_node_name, "cv_bridge exception");
        return;
    }

    if (cam_image) {
        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            cam_image_copy = cam_image->image.clone();
        }
        {
            boost::unique_lock<boost::shared_mutex> lockImageStatus(mutex_image_status);
            image_status = true;
        }
        frame_width = cam_image->image.size().width;
        frame_height = cam_image->image.size().height;
    }
    return;
}

// 用此函数查看是否收到图像话题
bool getImageStatus(void)
{
    boost::shared_lock<boost::shared_mutex> lock(mutex_image_status);
    return image_status;
}

void switchCallback(const std_msgs::Bool::ConstPtr& msg)
{
    is_suspanded = !(bool)msg->data;
    // cout << is_suspanded << endl;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "landpad_det");
    ros::NodeHandle nh("~");
    image_transport::ImageTransport it(nh);

    // 发布调试消息
    msg_node_name = "/prometheus/message/landpad_det";
    message_pub = nh.advertise<prometheus_msgs::Message>(msg_node_name, 10);

    std::string camera_topic, camera_info;
    if (nh.getParam("camera_topic", camera_topic)) {
        if (local_print)
            ROS_INFO("camera_topic is %s", camera_topic.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "camera_topic is" + camera_topic);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter camera_topic");
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter camera_topic");
        camera_topic = "/prometheus/camera/rgb/image_raw";
    }

    if (nh.getParam("camera_info", camera_info)) {
        if (local_print)
            ROS_INFO("camera_info is %s", camera_info.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "camera_info is" + camera_info);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter camera_info");
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter camera_info");
        camera_info = "camera_param.yaml";
    }

    position_pub = nh.advertise<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/landpad_det", 10);

    // 接收开关话题
    switch_subscriber = nh.subscribe("/prometheus/switch/landpad_det", 10, switchCallback);

    // 接收图像的话题
    image_subscriber = it.subscribe(camera_topic.c_str(), 1, cameraCallback);
    // 发布ArUco检测结果的话题
    landpad_pub = it.advertise("/prometheus/camera/rgb/image_landpad_det", 1);

    sensor_msgs::ImagePtr msg_ellipse;

    std::string ros_path = ros::package::getPath("prometheus_detection");

    if (local_print)
        cout << "DETECTION_PATH: " << ros_path << endl;
    if (message_print)
        pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "DETECTION_PATH: " + ros_path);

    // 读取参数文档camera_param.yaml中的参数值；
    YAML::Node camera_config = YAML::LoadFile(ros_path + "/config/" + camera_info);
    // 相机内部参数
    double fx = camera_config["fx"].as<double>();
    double fy = camera_config["fy"].as<double>();
    double cx = camera_config["x0"].as<double>();
    double cy = camera_config["y0"].as<double>();
    // 相机畸变系数
    double k1 = camera_config["k1"].as<double>();
    double k2 = camera_config["k2"].as<double>();
    double p1 = camera_config["p1"].as<double>();
    double p2 = camera_config["p2"].as<double>();
    double k3 = camera_config["k3"].as<double>();

    double landpad_det_len = camera_config["landpad_det_len"].as<double>();
    // DEBUG
    // cout << fx << " " << fy << " " << cx << " " << cy << " " << k1 << " " << k2 << " ";

    //--------------------------相机参数赋值---------------------
    // 相机内参
    Mat camera_matrix;
    camera_matrix =cv::Mat(3,3,CV_64FC1,cv::Scalar::all(0));
    camera_matrix.ptr<double>(0)[0] = fx;
    camera_matrix.ptr<double>(0)[2] = cx;
    camera_matrix.ptr<double>(1)[1] = fy;
    camera_matrix.ptr<double>(1)[2] = cy;
    camera_matrix.ptr<double>(2)[2] = 1.0f;
    // 相机畸变参数k1 k2 p1 p2 k3
    Mat distortion_coefficients;
    distortion_coefficients=cv::Mat(5,1,CV_64FC1,cv::Scalar::all(0));
    distortion_coefficients.ptr<double>(0)[0] = k1;
    distortion_coefficients.ptr<double>(1)[0] = k2;
    distortion_coefficients.ptr<double>(2)[0] = p1;
    distortion_coefficients.ptr<double>(3)[0] = p2;
    distortion_coefficients.ptr<double>(4)[0] = k3;

    // ArUco Marker字典选择以及旋转向量和评议向量初始化
    Ptr<cv::aruco::Dictionary> dictionary=cv::aruco::getPredefinedDictionary(10);
    vector<double> rv(3), tv(3);
    cv::Mat rvec(rv), tvec(tv);
    // cv::VideoCapture capture(0);
    float last_x(0), last_y(0), last_z(0), last_yaw(0);
    bool switch_state = is_suspanded;

    // 节点运行频率： 20hz 【视觉端解算频率大概为20HZ】
    ros::Rate loopRate(20);
    ros::Rate loopRate_1Hz(1);
    //----------------------------------------主循环------------------------------------
    // const auto wait_duration = std::chrono::milliseconds(2000);
    while (ros::ok())
    {
        while (!getImageStatus() && ros::ok())
        {
            if (local_print)
                cout << "Waiting for image." << endl;
            if (message_print)
                pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Waiting for image.");

            // std::this_thread::sleep_for(wait_duration);
            ros::spinOnce();
            loopRate_1Hz.sleep();
        }

        if (switch_state != is_suspanded)
        {
            switch_state = is_suspanded;
            if (!is_suspanded)
            {
                if (local_print)
                    cout << "Start Detection." << endl;
                if (message_print)
                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Start Detection.");
            }
            else
            {
                if (local_print)
                    cout << "Stop Detection." << endl;
                if (message_print)
                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Stop Detection.");
            }
        }

        if (!is_suspanded)
        {
        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            img = cam_image_copy.clone();
        }

        clock_t start=clock();
        // capture>>img;

        //------------------调用ArUco Marker库对图像进行识别--------------
        // markerids存储每个识别到二维码的编号  markerCorners每个二维码对应的四个角点的像素坐标
        std::vector<int> markerids;
        vector<vector<Point2f> > markerCorners,rejectedCandidate;
        Ptr<cv::aruco::DetectorParameters> parameters=cv::aruco::DetectorParameters::create();
        cv::aruco::detectMarkers(img,dictionary,markerCorners,markerids,parameters,rejectedCandidate);

        //-------------------多于一个目标被识别到，进入算法-----------------
        if (markerids.size() > 0)
        {
            // 未处理后的位置
            vector<cv::Point3f> vec_Position_OcInW;
            vector<double> vec_yaw;
            cv::Point3f A1_Sum_Position_OcInW(0,0,0);
            double A1_Sum_yaw = 0.0;
            double tx, ty, tz;
            int marker_count = 0;

            std::vector<int> markerids_sel;
            int t = -1;
            for(int tt=0; tt<markerids.size(); tt++)
            {
                if (19 == markerids[tt])
                    t = tt;
            }
            if (-1 == t)
            {
                for(int tt=0; tt<markerids.size(); tt++)
                {
                    if (43 == markerids[tt])
                        t = tt;
                }
            }
            if (-1 == t)
            {
                for(int tt=0; tt<markerids.size(); tt++)
                {
                    if (1 == markerids[tt])
                        t = tt;
                }
            }
            if (-1 == t)
            {
                for(int tt=0; tt<markerids.size(); tt++)
                {
                    if (2 == markerids[tt])
                        t = tt;
                }
            }
            if (-1 == t)
            {
                for(int tt=0; tt<markerids.size(); tt++)
                {
                    if (3 == markerids[tt])
                        t = tt;
                }
            }
            if (-1 == t)
            {
                for(int tt=0; tt<markerids.size(); tt++)
                {
                    if (4 == markerids[tt])
                        t = tt;
                }
            }
            if (-1 != t)
            {
                cv::Mat RoteM, TransM;
                // C2W代表 相机坐标系转换到世界坐标系  W2C代表 世界坐标系转换到相机坐标系 Theta为欧拉角
                cv::Point3f Theta_C2W;
                cv::Point3f Theta_W2C;
                cv::Point3f Position_OcInW;

                // 大二维码：19，小二维码：43
                //--------------对每一个Marker的相对位置进行解算----------------
                vector<vector<Point2f> > singMarkerCorner_19, singMarkerCorner_43;
                vector<vector<Point2f> > singMarkerCorner_1, singMarkerCorner_2, singMarkerCorner_3, singMarkerCorner_4;
                if (markerids[t] == 19)
                {
                    singMarkerCorner_19.push_back(markerCorners[t]);
                    cv::aruco::estimatePoseSingleMarkers(singMarkerCorner_19,landpad_det_len*0.666667,camera_matrix,distortion_coefficients,rvec,tvec);
                }
                else if (markerids[t] == 43)
                {
                    singMarkerCorner_43.push_back(markerCorners[t]);
                    cv::aruco::estimatePoseSingleMarkers(singMarkerCorner_43,landpad_det_len*0.066667,camera_matrix,distortion_coefficients,rvec,tvec);
                }
                else if (markerids[t] == 1)
                {
                    singMarkerCorner_1.push_back(markerCorners[t]);
                    cv::aruco::estimatePoseSingleMarkers(singMarkerCorner_1,landpad_det_len*0.133334,camera_matrix,distortion_coefficients,rvec,tvec);
                }
                else if (markerids[t] == 2)
                {
                    singMarkerCorner_2.push_back(markerCorners[t]);
                    cv::aruco::estimatePoseSingleMarkers(singMarkerCorner_2,landpad_det_len*0.133334,camera_matrix,distortion_coefficients,rvec,tvec);
                }
                else if (markerids[t] == 3)
                {
                    singMarkerCorner_3.push_back(markerCorners[t]);
                    cv::aruco::estimatePoseSingleMarkers(singMarkerCorner_3,landpad_det_len*0.133334,camera_matrix,distortion_coefficients,rvec,tvec);
                }
                else if (markerids[t] == 4)
                {
                    singMarkerCorner_4.push_back(markerCorners[t]);
                    cv::aruco::estimatePoseSingleMarkers(singMarkerCorner_4,landpad_det_len*0.133334,camera_matrix,distortion_coefficients,rvec,tvec);
                }
                else
                {
                    continue;
                }

                // 将解算的位置转化成旋转矩阵 并旋转计算无人机相对于目标的位置
                double rm[9];
                RoteM = cv::Mat(3, 3, CV_64FC1, rm);
                // 利用罗德里格斯公式将旋转向量转成旋转矩阵
                Rodrigues(rvec, RoteM);
                double r11 = RoteM.ptr<double>(0)[0];
                double r12 = RoteM.ptr<double>(0)[1];
                double r13 = RoteM.ptr<double>(0)[2];
                double r21 = RoteM.ptr<double>(1)[0];
                double r22 = RoteM.ptr<double>(1)[1];
                double r23 = RoteM.ptr<double>(1)[2];
                double r31 = RoteM.ptr<double>(2)[0];
                double r32 = RoteM.ptr<double>(2)[1];
                double r33 = RoteM.ptr<double>(2)[2];
                TransM = tvec;
                // 计算欧拉角
                double thetaz = atan2(r21, r11) / CV_PI * 180;
                double thetay = atan2(-1 * r31, sqrt(r32*r32 + r33*r33)) / CV_PI * 180;
                double thetax = atan2(r32, r33) / CV_PI * 180;

                Theta_C2W.z = thetaz;
                Theta_C2W.y = thetay;
                Theta_C2W.x = thetax;

                Theta_W2C.x = -1 * thetax;
                Theta_W2C.y = -1 * thetay;
                Theta_W2C.z = -1 * thetaz;
                // 偏移向量
                tx = tvec.ptr<double>(0)[0];
                ty = tvec.ptr<double>(0)[1];
                tz = tvec.ptr<double>(0)[2];

                Position_OcInW.x = tx;
                Position_OcInW.y = ty;
                Position_OcInW.z = tz;

                // 计算偏航角之差
                Eigen::Matrix3d rotateMatrix;
                rotateMatrix << r11,r12,r13,r21,r22,r23,r31,r32,r33;

                Eigen::Vector3d eulerVec;
                eulerVec(0) = (Theta_C2W.z) / 180 * CV_PI;
                vec_yaw.push_back(eulerVec(0));
                vec_Position_OcInW.push_back(Position_OcInW);

                A1_Sum_Position_OcInW += Position_OcInW;
                A1_Sum_yaw += eulerVec(0); // 待修改

                marker_count += 1;
            }
            if (-1 != t)
            {
                // 解算位置的平均值
                // cout << marker_count << endl;
                cv::Point3f A1_Position_OcInW(0,0,0);
                double A1_yaw = 0.0;
                A1_Position_OcInW = A1_Sum_Position_OcInW / marker_count;
                A1_yaw = A1_Sum_yaw / marker_count;

                // 将解算后的位置发给控制端
                pose_now.header.stamp = ros::Time::now();
                pose_now.detected = true;
                pose_now.frame = 0;
                pose_now.position[0] = tx;
                pose_now.position[1] = ty;
                pose_now.position[2] = tz;
                pose_now.sight_angle[0] = atan(tx / tz);
                pose_now.sight_angle[1] = atan(ty / tz);
                pose_now.yaw_error = A1_yaw;

                last_x = pose_now.position[0];
                last_y = pose_now.position[1];
                last_z = pose_now.position[2];
                last_yaw = pose_now.yaw_error;
            }
            else
            {
                pose_now.header.stamp = ros::Time::now();
                pose_now.detected = false;
                pose_now.frame = 0;
                pose_now.position[0] = last_x;
                pose_now.position[1] = last_y;
                pose_now.position[2] = last_z;
                pose_now.sight_angle[0] = atan(last_x / last_z);
                pose_now.sight_angle[1] = atan(last_y / last_z);
                pose_now.yaw_error = last_yaw;
            }
        }
        else
        {
            pose_now.header.stamp = ros::Time::now();
            pose_now.detected = false;
            pose_now.frame = 0;
            pose_now.position[0] = last_x;
            pose_now.position[1] = last_y;
            pose_now.position[2] = last_z;
            pose_now.sight_angle[0] = atan(last_x / last_z);
            pose_now.sight_angle[1] = atan(last_y / last_z);
            pose_now.yaw_error = last_yaw;
        }
        position_pub.publish(pose_now);

        // 计算算法运行时间
        clock_t finish=clock();
        calculation_time=(finish-start)/1000;

        // 打印
        // printf_result();

        // 画出识别到的二维码
        cv::aruco::drawDetectedMarkers(img, markerCorners, markerids);

        msg_ellipse = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
        landpad_pub.publish(msg_ellipse);
        }

        // cv::imshow("test",img);
        ros::spinOnce();
        cv::waitKey(1);
        loopRate.sleep();
    }
}

void printf_result()
{
    // 固定的浮点显示
    cout.setf(ios::fixed);
    // setprecision(n) 设显示小数精度为n位
    cout<<setprecision(4);
    // 左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    cout.setf(ios::showpos);

    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>Landpad Detection<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    if(pose_now.detected)
    {
        cout << "is_detected: ture" <<endl;
    }else
    {
        cout << "is_detected: false" <<endl;
    }
    cout << "pos_target: [X Y Z] : " << pose_now.position[0]  << " [m] "<< pose_now.position[1] <<" [m] " << pose_now.position[2] << " [m] "<<endl;
    cout << "pos_target: [Yaw] :   " << pose_now.yaw_error/3.1415926 *180    << " [du] "<<endl;
    cout << "calculation_time =    " << time << " [ms] " << endl;
}


/*
	Copyright 2005-2007 Adobe Systems Incorporated
	Distributed under the MIT License (see accompanying file LICENSE_1_0_0.txt
	or a copy at http://stlab.adobe.com/licenses.html)
*/

/*************************************************************************************************/

#include <adobe/config.hpp>

#include <functional>
#include <utility>

#include <boost/test/unit_test.hpp>

#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/range/size.hpp>
#include <adobe/functional.hpp>
#include <adobe/algorithm/equal_range.hpp>
#include <adobe/utility/pair.hpp>

void equal_range_test() {
    using std::pair;
    using adobe::less;
    using adobe::equal_range;
    using adobe::equal_range_n;
    using adobe::aggregate_pair;
    using boost::begin;
    using boost::end;
    using boost::size;

    aggregate_pair<int> a[] = { { 0, 0 }, { 0, 1 }, { 1, 2 }, { 1, 3 }, { 3, 4 }, { 3, 5 } };
    const aggregate_pair<int> c[] = { { 0, 0 }, { 0, 1 }, { 1, 2 }, { 1, 3 }, { 3, 4 }, { 3, 5 } };

    BOOST_CHECK_EQUAL(equal_range_n(begin(a), size(a), a[0]).first, &a[0]);
    BOOST_CHECK_EQUAL(equal_range_n(begin(a), size(a), a[1], less()).first, &a[1]);
    BOOST_CHECK_EQUAL(equal_range_n(begin(a), size(a), 1, less(), &aggregate_pair<int>::first).first, &a[2]);

    BOOST_CHECK_EQUAL(equal_range(begin(a), end(a), a[0]).first, &a[0]);
    BOOST_CHECK_EQUAL(adobe::equal_range(begin(a), end(a), a[1], less()).first, &a[1]);
    BOOST_CHECK_EQUAL(equal_range(begin(a), end(a), 1, less(), &aggregate_pair<int>::first).first, &a[2]);

    BOOST_CHECK_EQUAL(equal_range(a, a[2]).first, &a[2]);
    BOOST_CHECK_EQUAL(equal_range(c, c[3]).first, &c[3]);

    BOOST_CHECK_EQUAL(equal_range(a, a[4], less()).first, &a[4]);
    BOOST_CHECK_EQUAL(equal_range(c, c[5], less()).first, &c[5]);

    BOOST_CHECK_EQUAL(equal_range(a, 1, less(), &aggregate_pair<int>::first).first, &a[2]);
    BOOST_CHECK_EQUAL(equal_range(c, 1, less(), &aggregate_pair<int>::first).first, &c[2]);
}

using namespace boost::unit_test;

test_suite*
init_unit_test_suite( int , char* [] )
{
    framework::master_test_suite().
        add( BOOST_TEST_CASE( &equal_range_test ) );

    return 0;
}


#include <vector>
#include <boost/type_traits/is_same.hpp>
#include <stan/model/indexing/index.hpp>
#include <gtest/gtest.h>

using stan::model::index_uni;
using stan::model::index_multi;
using stan::model::index_omni;
using stan::model::index_min;
using stan::model::index_max;
using stan::model::index_min_max;

TEST(MathIndexingIndex, index_uni) {
  index_uni idx(17);
  EXPECT_EQ(17, idx.n_);
}

TEST(MathIndexingIndex, index_multi) {
  std::vector<int> ns;
  ns.push_back(3);
  ns.push_back(23);

  index_multi idx(ns);
  EXPECT_EQ(2, idx.ns_.size());
  for (size_t i = 0; i < ns.size(); ++i)
    EXPECT_EQ(ns[i], idx.ns_[i]);
}

TEST(MathIndexingIndex, index_omni) {
  index_omni idx;
  (void) idx;  // just to silence compiler griping about idx being unused
}

TEST(MathIndexingIndex, index_min) {
  index_min idx(3);
  EXPECT_EQ(3, idx.min_);
}

TEST(MathIndexingIndex, index_max) {
  index_max idx(912);
  EXPECT_EQ(912, idx.max_);
}

TEST(MathIndexingIndex, index_min_max) {
  index_min_max idx(401,912);
  EXPECT_EQ(401, idx.min_);
  EXPECT_EQ(912, idx.max_);
}


/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

/*
 * test_pose.cpp
 *
 *  Created on: Mar 15, 2012
 *      Author: Pablo Iñigo Blasco
 */

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <geometry_msgs/Quaternion.h>
#include <gtest/gtest.h>
#include <mrpt/math/CQuaternion.h>
#include <mrpt/poses/CPose3DPDFGaussian.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/ros1bridge/pose.h>

#include <Eigen/Dense>

using namespace std;

TEST(PoseConversions, copyMatrix3x3ToCMatrixDouble33)
{
	tf2::Matrix3x3 src(0, 1, 2, 3, 4, 5, 6, 7, 8);
	const mrpt::math::CMatrixDouble33 des = mrpt::ros1bridge::fromROS(src);

	for (int r = 0; r < 3; r++)
		for (int c = 0; c < 3; c++) EXPECT_FLOAT_EQ(des(r, c), src[r][c]);
}
TEST(PoseConversions, copyCMatrixDouble33ToMatrix3x3)
{
	mrpt::math::CMatrixDouble33 src;
	for (int r = 0, i = 0; r < 3; r++)
		for (int c = 0; c < 3; c++, i++) src(r, c) = i;

	const tf2::Matrix3x3 des = mrpt::ros1bridge::toROS(src);

	for (int r = 0; r < 3; r++)
		for (int c = 0; c < 3; c++) EXPECT_FLOAT_EQ(des[r][c], src(r, c));
}

// Declare a test
TEST(PoseConversions, reference_frame_change_with_rotations)
{
	geometry_msgs::PoseWithCovariance ros_msg_original_pose;

	ros_msg_original_pose.pose.position.x = 1;
	ros_msg_original_pose.pose.position.y = 0;
	ros_msg_original_pose.pose.position.z = 0;
	ros_msg_original_pose.pose.orientation.x = 0;
	ros_msg_original_pose.pose.orientation.y = 0;
	ros_msg_original_pose.pose.orientation.z = 0;
	ros_msg_original_pose.pose.orientation.w = 1;

	// to mrpt
	mrpt::poses::CPose3DPDFGaussian mrpt_original_pose =
		mrpt::ros1bridge::fromROS(ros_msg_original_pose);

	EXPECT_EQ(
		ros_msg_original_pose.pose.position.x, mrpt_original_pose.mean[0]);

#if 0
	// to tf
	tf::Pose tf_original_pose;
	tf::poseMsgToTF(ros_msg_original_pose.pose, tf_original_pose);

	// rotate yaw pi in MRPT
	mrpt::poses::CPose3D rotation_mrpt;
	double yaw = M_PI / 2.0;
	rotation_mrpt.setFromValues(0, 0, 0, yaw, 0, 0);
	mrpt::poses::CPose3D mrpt_result = rotation_mrpt + mrpt_original_pose.mean;
	EXPECT_NEAR(mrpt_result[1], 1.0, 0.01);

	// rotate yaw pi in TF
	tf::Quaternion rotation_tf;
	rotation_tf.setRPY(0, 0, yaw);
	tf::Pose rotation_pose_tf;
	rotation_pose_tf.setIdentity();
	rotation_pose_tf.setRotation(rotation_tf);
	tf::Pose tf_result = rotation_pose_tf * tf_original_pose;
	EXPECT_NEAR(tf_result.getOrigin()[1], 1.0, 0.01);

	geometry_msgs::Pose mrpt_ros_result;
	mrpt_bridge::convert(mrpt_result, mrpt_ros_result);

	EXPECT_NEAR(mrpt_ros_result.position.x, tf_result.getOrigin()[0], 0.01);
	EXPECT_NEAR(mrpt_ros_result.position.y, tf_result.getOrigin()[1], 0.01);
	EXPECT_NEAR(mrpt_ros_result.position.z, tf_result.getOrigin()[2], 0.01);
#endif
}

void check_CPose3D_tofrom_ROS(
	double x, double y, double z, double yaw, double pitch, double roll)
{
	const mrpt::poses::CPose3D p3D(x, y, z, yaw, pitch, roll);

	// Convert MRPT->ROS
	geometry_msgs::Pose ros_p3D = mrpt::ros1bridge::toROS_Pose(p3D);

	// Compare ROS quat vs. MRPT quat:
	mrpt::math::CQuaternionDouble q;
	p3D.getAsQuaternion(q);

	EXPECT_NEAR(ros_p3D.position.x, p3D.x(), 1e-4) << "p: " << p3D << endl;
	EXPECT_NEAR(ros_p3D.position.y, p3D.y(), 1e-4) << "p: " << p3D << endl;
	EXPECT_NEAR(ros_p3D.position.z, p3D.z(), 1e-4) << "p: " << p3D << endl;

	EXPECT_NEAR(ros_p3D.orientation.x, q.x(), 1e-4) << "p: " << p3D << endl;
	EXPECT_NEAR(ros_p3D.orientation.y, q.y(), 1e-4) << "p: " << p3D << endl;
	EXPECT_NEAR(ros_p3D.orientation.z, q.z(), 1e-4) << "p: " << p3D << endl;
	EXPECT_NEAR(ros_p3D.orientation.w, q.r(), 1e-4) << "p: " << p3D << endl;

	// Test the other path: ROS->MRPT
	mrpt::poses::CPose3D p_bis = mrpt::ros1bridge::fromROS(ros_p3D);

	// p_bis==p3D?
	EXPECT_NEAR(
		(p_bis.asVectorVal() - p3D.asVectorVal()).array().abs().maxCoeff(), 0,
		1e-4)
		<< "p_bis: " << p_bis << endl
		<< "p3D: " << p3D << endl;
}

// Declare a test
TEST(PoseConversions, check_CPose3D_tofrom_ROS)
{
	using mrpt::DEG2RAD;
	using mrpt::RAD2DEG;
	using namespace mrpt;  // for 0.0_deg

	check_CPose3D_tofrom_ROS(0, 0, 0, 0.0_deg, 0.0_deg, 0.0_deg);
	check_CPose3D_tofrom_ROS(1, 2, 3, 0.0_deg, 0.0_deg, 0.0_deg);

	check_CPose3D_tofrom_ROS(1, 2, 3, 30.0_deg, 0.0_deg, 0.0_deg);
	check_CPose3D_tofrom_ROS(1, 2, 3, 0.0_deg, 30.0_deg, 0.0_deg);
	check_CPose3D_tofrom_ROS(1, 2, 3, 0.0_deg, 0.0_deg, 30.0_deg);

	check_CPose3D_tofrom_ROS(1, 2, 3, -5.0_deg, 15.0_deg, -30.0_deg);
}

// Declare a test
TEST(PoseConversions, check_CPose2D_to_ROS)
{
	const mrpt::poses::CPose2D p2D(1, 2, 0.56);

	// Convert MRPT->ROS
	const geometry_msgs::Pose ros_p2D = mrpt::ros1bridge::toROS_Pose(p2D);

	// Compare vs. 3D pose:
	const mrpt::poses::CPose3D p3D = mrpt::poses::CPose3D(p2D);
	const mrpt::poses::CPose3D p3D_ros = mrpt::ros1bridge::fromROS(ros_p2D);

	// p3D_ros should equal p3D
	EXPECT_NEAR(
		(p3D_ros.asVectorVal() - p3D.asVectorVal()).array().abs().maxCoeff(), 0,
		1e-4)
		<< "p3D_ros: " << p3D_ros << endl
		<< "p3D: " << p3D << endl;
}


#include <vector>
#include <string>
#include <opencv2/opencv.hpp>
#include <boost/python.hpp>
#include <boost/numpy.hpp>

namespace py = boost::python;
namespace np = boost::numpy;

cv::Mat transform(
  const cv::Mat& img,
  const bool   & fliplr,
  const bool   & rotate,
  const int    & angle,
  const bool   & norm,
  const int    & out_h,
  const int    & out_w,
  const int    & channels) {
  cv::Mat img_mat(img.cols, img.rows, CV_64FC(channels));

  img.convertTo(img_mat, CV_64F);

  // Flipping L-R
  if (fliplr) cv::flip(img_mat, img_mat, 1);

  // Rotation with random arngle
  if (rotate) {
    cv::Point2f center(img_mat.cols / 2., img_mat.rows / 2.);
    cv::Mat     r = cv::getRotationMatrix2D(center, angle, 1.0);
    cv::warpAffine(img_mat, img_mat, r, cv::Size(img_mat.cols, img_mat.rows),
                   cv::INTER_NEAREST);
  }

  // Cropping
  cv::Rect roi(img_mat.cols / 2 - out_w / 2, img_mat.rows / 2 - out_h / 2,
               out_w, out_h);
  cv::Mat crop(out_w, out_h, CV_64FC(channels));
  img_mat(roi).copyTo(crop);

  // patch-wise mean subtraction
  if (norm && (img.channels() == 3)) {
    cv::Scalar mean, stddev;
    cv::meanStdDev(crop, mean, stddev);

    cv::Mat *slice = new cv::Mat[channels];
    cv::split(crop, slice);

    for (int c = 0; c < channels; ++c) {
      cv::subtract(slice[c], mean[c], slice[c]);
      slice[c] /= stddev[c] + 1E-5;
    }
    cv::merge(slice, channels, crop);
    delete[] slice;
  }

  return crop;
}

cv::Mat extract_cvmat(const np::ndarray& img, const int& n) {
  size_t rows         = img.shape(1);
  size_t cols         = img.shape(2);
  size_t channels     = img.shape(3);
  unsigned char *data = reinterpret_cast<unsigned char *>(img.get_data());

  data += n * rows * cols * channels;
  cv::Mat img_mat(rows, cols, CV_8UC(channels));
  unsigned char *out_data = reinterpret_cast<unsigned char *>(img_mat.data);
  std::memcpy(out_data, data, rows * cols * channels);

  return img_mat;
}

void set_values(
  const cv::Mat& mat,
  np::ndarray    img,
  const int    & n) {
  size_t rows     = mat.rows;
  size_t cols     = mat.cols;
  size_t channels = mat.channels();

  assert(rows == img.shape(1));
  assert(cols == img.shape(2));
  assert(channels == img.shape(3));

  double *mat_data = reinterpret_cast<double *>(mat.data);
  float  *img_data = reinterpret_cast<float *>(img.get_data());

  for (size_t y = 0; y < rows; ++y) {
    for (size_t x = 0; x < cols; ++x) {
      for (size_t c = 0; c < channels; ++c) {
        int bind = n * rows * cols * channels + y * cols * channels + x *
                   channels + c;
        int ind = y * cols * channels + x * channels + c;
        img_data[bind] = float(mat_data[ind]);
      }
    }
  }
}

void batch_transform(
  const np::ndarray& sat,
  const np::ndarray& map,
  np::ndarray      & sat_out,
  np::ndarray      & map_out,
  const bool       & fliplr,
  const bool       & rotate,
  const bool       & norm,
  const int        & sat_out_h,
  const int        & sat_out_w,
  const int        & sat_channels,
  const int        & map_out_h,
  const int        & map_out_w) {
  size_t num = sat.shape(0);

  for (size_t n = 0; n < num; ++n) {
    cv::Mat sat_img  = extract_cvmat(sat, n);
    cv::Mat map_img  = extract_cvmat(map, n);
    int     angle    = rand() % 360;
    cv::Mat sat_crop = transform(sat_img,
                                 fliplr,
                                 rotate,
                                 angle,
                                 norm,
                                 sat_out_h,
                                 sat_out_w,
                                 sat_channels);
    cv::Mat map_crop = transform(map_img,
                                 fliplr,
                                 rotate,
                                 angle,
                                 norm,
                                 map_out_h,
                                 map_out_w,
                                 1);
    set_values(sat_crop, sat_out, n);
    set_values(map_crop, map_out, n);
  }
}

BOOST_PYTHON_MODULE(transform) {
  np::initialize();
  py::def("batch_transform", batch_transform);
}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/constant/allbits.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/unary_minus.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/meta/hierarchy/logical.hpp>
#include <boost/simd/meta/as_arithmetic.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  // -----------------------------------------------------------------------------------------------
  // genmask from logical
  BOOST_DISPATCH_OVERLOAD ( genmask_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bs::logical_<A0> >
                          )
  {
    using result_t = as_arithmetic_t<A0>;

    BOOST_FORCEINLINE result_t operator()( A0 const& a0 ) const BOOST_NOEXCEPT
    {
      using itype = dispatch::as_integer_t<result_t>;
      return bitwise_cast<result_t>(unary_minus(itype(a0)));
    }
  };

  // -----------------------------------------------------------------------------------------------
  // genmask for bool
  BOOST_DISPATCH_OVERLOAD ( genmask_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::bool_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator()( A0 const& a0 ) const BOOST_NOEXCEPT
    {
      return a0;
    }
  };

  // -----------------------------------------------------------------------------------------------
  // genmask from other value
  BOOST_DISPATCH_OVERLOAD ( genmask_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::unspecified_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator()( A0 const& a0 ) const BOOST_NOEXCEPT
    {
      using itype = dispatch::as_integer_t<A0>;
      return bitwise_cast<A0>(unary_minus(itype(a0 != 0)));
    }
  };

} } }

#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    timeDSFvariants.cpp
 * @brief   Time different implementations of DSF
 * @author  Frank Dellaert
 * @date    Oct 26, 2013
 */

#include <gtsam/base/DSFVector.h>
#include <gtsam_unstable/base/DSF.h>
#include <gtsam_unstable/base/DSFMap.h>

#include <boost/random.hpp>
#include <boost/timer.hpp>
#include <boost/format.hpp>
#include <boost/assign/std/vector.hpp>

#include <iostream>
#include <fstream>
#include <vector>
#include <utility>

using namespace std;
using namespace gtsam;
using namespace boost::assign;
using boost::timer;
using boost::format;

int main(int argc, char* argv[]) {

  // Create CSV file for results
  ofstream os("dsf-timing.csv");
  os << "images,points,matches,Base,Map,BTree" << endl;

  // loop over number of images
  vector<size_t> ms;
  ms += 10, 20, 30, 40, 50, 100, 200, 300, 400, 500, 1000;
  for(size_t m: ms) {
    // We use volatile here to make these appear to the optimizing compiler as
    // if their values are only known at run-time.
    volatile size_t n = 500; // number of points per image
    volatile size_t N = m * n; // number of points per image

    volatile double fm = 0.1; // fraction of image pairs matched
    volatile size_t np = fm * m * m / 2; // number of image pairs
    volatile double fpm = 0.5; // fraction of points matched
    volatile size_t nm = fpm * n * np; // number of matches

    cout << format("\nTesting with %1% images, %2% points, %3% matches\n")
            % (int)m % (int)N % (int)nm;
    cout << "Generating " << nm << " matches" << endl;
    boost::variate_generator<boost::mt19937, boost::uniform_int<size_t> > rn(
        boost::mt19937(), boost::uniform_int<size_t>(0, N - 1));
    typedef pair<size_t, size_t> Match;
    vector<Match> matches;
    matches.reserve(nm);
    for (size_t k = 0; k < nm; k++)
      matches.push_back(Match(rn(), rn()));

    os << format("%1%,%2%,%3%,") % (int)m % (int)N % (int)nm;

    {
      // DSFBase version
      timer tim;
      DSFBase dsf(N); // Allow for N keys
      for(const Match& m: matches)
        dsf.merge(m.first, m.second);
      os << tim.elapsed() << ",";
      cout << format("DSFBase: %1% s") % tim.elapsed() << endl;
    }

    {
      // DSFMap version
      timer tim;
      DSFMap<size_t> dsf;
      for(const Match& m: matches)
        dsf.merge(m.first, m.second);
      os << tim.elapsed() << endl;
      cout << format("DSFMap: %1% s") % tim.elapsed() << endl;
    }

    if (false) {
      // DSF version, functional
      timer tim;
      DSF<size_t> dsf;
      for (size_t j = 0; j < N; j++)
        dsf = dsf.makeSet(j);
      for(const Match& m: matches)
        dsf = dsf.makeUnion(m.first, m.second);
      os << tim.elapsed() << endl;
      cout << format("DSF functional: %1% s") % tim.elapsed() << endl;
    }

    if (false) {
      // DSF version, in place - always slower - use functional !
      timer tim;
      DSF<size_t> dsf;
      for (size_t j = 0; j < N; j++)
        dsf.makeSetInPlace(j);
      for(const Match& m: matches)
        dsf.makeUnionInPlace(m.first, m.second);
      os << tim.elapsed() << ",";
      cout << format("DSF in-place: %1% s") % tim.elapsed() << endl;
    }

  }

  return 0;

}