/***************************************************************************************************************************
* px4_pos_controller.cpp
*
* Author: Qyp
*
* Update Time: 2019.5.9
*
* Introduction:  PX4 Position Controller using cascade PID method or PD+UDE or passivity
*         1. Subscribe command.msg from upper nodes (e.g. Target_tracking.cpp)
*         2. Calculate the accel_sp using pos_controller_PID.h(pos_controller_UDE.h pos_controller_passivity.h)
*         3. Send command to mavros package using command_to_mavros.h (mavros package will send the message to PX4 as Mavlink msg)
*         4. PX4 firmware will recieve the Mavlink msg by mavlink_receiver.cpp in mavlink module.
***************************************************************************************************************************/
#include <ros/ros.h>

#include <command_to_mavros.h>
#include <px4_command/command.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <pos_controller_PID.h>
#include <pos_controller_UDE.h>
#include <pos_controller_Passivity.h>
#include <pos_controller_NE.h>

#include <fstream>

#include <Eigen/Eigen>

using namespace std;

using namespace namespace_command_to_mavros;
using namespace namespace_PID;
using namespace namespace_UDE;
using namespace namespace_passivity;
using namespace namespace_NE;

//注意：代码中，参与运算的角度均是以rad为单位，但是涉及到显示时或者需要手动输入时均以deg为单位。
//自定义的Command变量
//相应的命令分别为 待机,起飞，移动(惯性系ENU)，移动(机体系)，悬停，降落，上锁，紧急降落
enum Command
{
    Idle,
    Takeoff,
    Move_ENU,
    Move_Body,
    Hold,
    Land,
    Disarm,
    Failsafe_land,
};
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>变量声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
Eigen::Vector3d pos_sp(0,0,0);
Eigen::Vector3d vel_sp(0,0,0);
Eigen::Vector3d accel_sp(0,0,0);
Eigen::Quaterniond q_sp;
Eigen::Vector3d att_sp(0,0,0);
float thrust_sp;
double yaw_sp = 0;

int flag_att_sp;

float Takeoff_height;
float Disarm_height;

Eigen::Vector3d Takeoff_position = Eigen::Vector3d(0.0,0.0,0.0);

px4_command::command Command_Now;                      //无人机当前执行命令
px4_command::command Command_Last;                     //无人机上一条执行命令

Eigen::Vector3d pos_drone_mocap;                       //无人机当前位置 (vicon)
float thrust_read;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
float get_ros_time(ros::Time begin);
void prinft_command_state();
void rotation_yaw(float yaw_angle, float input[2], float output[2]);
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void Command_cb(const px4_command::command::ConstPtr& msg)
{
    Command_Now = *msg;
}
/*
void setpoint_attitude_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg)
{
    thrust_read = msg->thrust;
}
void optitrack_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    //位置 -- optitrack系 到 ENU系
    int optitrack_frame = 0; //Frame convention 0: Z-up -- 1: Y-up
    // Read the Drone Position from the Vrpn Package [Frame: Vicon]  (Vicon to ENU frame)
    Eigen::Vector3d pos_drone_mocap_enu(-msg->pose.position.x,msg->pose.position.z,msg->pose.position.y);

    pos_drone_mocap = pos_drone_mocap_enu;
}
*/
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "px4_pos_controller");
    ros::NodeHandle nh("~");

    // 【订阅】指令
    //  本话题来自根据需求自定义的上层模块，比如track_land.cpp 比如move.cpp
    ros::Subscriber Command_sub = nh.subscribe<px4_command::command>("/px4/command", 10, Command_cb);

    //ros::Subscriber Thrust_sub = nh.subscribe<mavros_msgs::AttitudeTarget>("/mavros/setpoint_raw/attitude", 10, setpoint_attitude_cb);

    //ros::Subscriber optitrack_sub = nh.subscribe<geometry_msgs::PoseStamped>("/vrpn_client_node/UAV/pose", 1000, optitrack_cb);

    // 参数读取
    nh.param<float>("Takeoff_height", Takeoff_height, 1.0);
    nh.param<float>("Disarm_height", Disarm_height, 0.15);
    nh.param<int>("flag_att_sp", flag_att_sp, 0);

    cout << "Takeoff_height: "<< Takeoff_height<<" [m] "<<endl;
    cout << "Disarm_height : "<< Disarm_height <<" [m] "<<endl;
    cout << "flag_att_sp : "<< flag_att_sp <<"  "<<endl;

    ros::Rate rate(50.0);
    //用于与mavros通讯的类
    command_to_mavros pos_controller;

    pos_controller.printf_param();

    //位置控制类 - 根据switch_ude选择其中一个使用，默认为PID
    pos_controller_PID pos_controller_pid;
    pos_controller_UDE pos_controller_ude;
    pos_controller_passivity pos_controller_ps;
    pos_controller_NE pos_controller_ne;

    // 选择控制率
    int switch_ude;
    cout << "Please choose the controller: 0 for PID,1 for UDE,2 for passivity, 3 for NE: "<<endl;
    cin >> switch_ude;

    if(switch_ude == 0)
    {
        pos_controller_pid.printf_param();
    }else if(switch_ude == 1)
    {
        pos_controller_ude.printf_param();
    }else if(switch_ude == 2)
    {
        pos_controller_ps.printf_param();
    }else if(switch_ude == 3)
    {
        pos_controller_ne.printf_param();
    }

    int check_flag;
    // 这一步是为了程序运行前检查一下参数是否正确
    // 输入1,继续，其他，退出程序
    cout << "Please check the parameter and setting，1 for go on， else for quit: "<<endl;
    cin >> check_flag;

    if(check_flag != 1)
    {
        return -1;
    }

    // 等待和飞控的连接
    while(ros::ok() && pos_controller.current_state.connected)
    {
        ros::spinOnce();
        rate.sleep();
        ROS_INFO("Not Connected");
    }

    // 连接成功
    ROS_INFO("Connected!!");

    // 先读取一些飞控的数据
    int i =0;
    for(i=0;i<50;i++)
    {
        ros::spinOnce();
        rate.sleep();
    }

    //Set the takeoff position
    Takeoff_position = pos_controller.pos_drone_fcu;

    //NE需要设置起飞初始值
    if(switch_ude == 3)
    {
        pos_controller_ne.set_initial_pos(pos_controller.pos_drone_fcu);
    }

    //初始化命令-
    // 默认设置：Idle模式 电机怠速旋转 等待来自上层的控制指令
    Command_Now.comid = 0;
    Command_Now.command = Takeoff;
    Command_Now.sub_mode = 0;
    Command_Now.pos_sp[0] = pos_controller.pos_drone_fcu[0];
    Command_Now.pos_sp[1] = pos_controller.pos_drone_fcu[1];
    Command_Now.pos_sp[2] = pos_controller.pos_drone_fcu[2];
    Command_Now.vel_sp[0] = 0;
    Command_Now.vel_sp[1] = 0;
    Command_Now.vel_sp[2] = 0;
    Command_Now.yaw_sp = 0;

    // 记录启控时间
    ros::Time begin_time = ros::Time::now();
    float last_time = get_ros_time(begin_time);
    float dt = 0;

    // write into files
    ofstream ofile;
    ofile.open("/home/amov/fly_logs/command_fuzzy.txt");

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主  循  环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while(ros::ok())
    {
        //执行回调函数
        ros::spinOnce();

        // 当前时间
        float cur_time = get_ros_time(begin_time);
        dt = cur_time  - last_time;

        dt = constrain_function2(dt, 0.005, 0.03);

        last_time = cur_time;

        //Check for geo fence: If drone is out of the geo fence, it will land now.
        if(pos_controller.check_failsafe() == 1)
        {
            Command_Now.command = Land;
        }

        //Printf the drone state
        //pos_controller.prinft_drone_state_full(cur_time);
        pos_controller.prinft_drone_state(cur_time);

        //Printf the command state
        prinft_command_state();

        if(switch_ude == 0)
        {
            pos_controller_pid.printf_result();
        }else if(switch_ude == 1)
        {
            pos_controller_ude.printf_result();
        }else if(switch_ude == 2)
        {
            pos_controller_ps.printf_result();
        }else if(switch_ude == 3)
        {
            pos_controller_ne.printf_result();
        }

        //无人机一旦接受到Land指令，则会屏蔽其他指令
        if(Command_Last.command == Land)
        {
            Command_Now.command = Land;
        }

        switch (Command_Now.command)
        {
        // 【Idle】 怠速旋转，此时可以切入offboard模式，但不会起飞。
        case Idle:
            pos_controller.idle();
            break;

        // 【Takeoff】 从摆放初始位置原地起飞至指定高度，偏航角也保持当前角度
        case Takeoff:
            pos_sp = Eigen::Vector3d(Takeoff_position[0],Takeoff_position[1],Takeoff_position[2]+Takeoff_height);
            vel_sp = Eigen::Vector3d(0.0,0.0,0.0);
            yaw_sp = pos_controller.euler_fcu[2]; //rad

            if(switch_ude == 0)
            {
                accel_sp = pos_controller_pid.pos_controller(pos_controller.pos_drone_fcu, pos_controller.vel_drone_fcu, pos_sp, vel_sp, Command_Now.sub_mode, dt);
            }else if(switch_ude == 1)
            {
                accel_sp = pos_controller_ude.pos_controller(pos_controller.pos_drone_fcu, pos_controller.vel_drone_fcu, pos_sp, dt);
            }else if(switch_ude == 2)
            {
                accel_sp = pos_controller_ps.pos_controller(pos_drone_mocap, pos_controller.vel_drone_fcu, pos_sp, dt);
            }else if(switch_ude == 3)
            {
                accel_sp = pos_controller_ps.pos_controller(pos_drone_mocap, pos_controller.vel_drone_fcu, pos_sp, dt);
            }

            if(flag_att_sp == 0)
            {
                pos_controller.send_accel_setpoint(accel_sp, yaw_sp);
            }else
            {
                q_sp =  thrustToAttitude(accel_sp, yaw_sp);

                //att_sp =  quaternion_to_euler(q_sp);

                thrust_sp = accel_sp.norm();

                //cout << "thrust_sp: " << thrust_sp <<endl;

                //cout << "Attitude_sp0 [R P Y] : " << att_sp[0] * 180/M_PI <<" [deg] "<<att_sp[1] * 180/M_PI << " [deg] "<< att_sp[2] * 180/M_PI<<" [deg] "<<endl;

                pos_controller.send_attitude_setpoint(q_sp, thrust_sp);
            }

            break;

        // 【Move_ENU】 ENU系移动。只有PID算法中才有追踪速度的选项，其他控制只能追踪位置
        case Move_ENU:
            pos_sp = Eigen::Vector3d(Command_Now.pos_sp[0],Command_Now.pos_sp[1],Command_Now.pos_sp[2]);
            vel_sp = Eigen::Vector3d(Command_Now.vel_sp[0],Command_Now.vel_sp[1],Command_Now.vel_sp[2]);
            yaw_sp = Command_Now.yaw_sp;

            if(switch_ude == 0)
            {
                accel_sp = pos_controller_pid.pos_controller(pos_controller.pos_drone_fcu, pos_controller.vel_drone_fcu, pos_sp, vel_sp, Command_Now.sub_mode, dt);
            }else if(switch_ude == 1)
            {
                accel_sp = pos_controller_ude.pos_controller(pos_controller.pos_drone_fcu, pos_controller.vel_drone_fcu, pos_sp, dt);
            }else if(switch_ude == 2)
            {
                accel_sp = pos_controller_ps.pos_controller(pos_drone_mocap, pos_controller.vel_drone_fcu, pos_sp, dt);
                //accel_sp = pos_controller_ps.pos_controller(pos_controller.pos_drone_fcu, pos_controller.vel_drone_fcu, pos_sp, dt);
            }else if(switch_ude == 3)
            {
                accel_sp = pos_controller_ne.pos_controller(pos_drone_mocap, pos_controller.vel_drone_fcu, pos_sp, dt);
                //accel_sp = pos_controller_ps.pos_controller(pos_controller.pos_drone_fcu, pos_controller.vel_drone_fcu, pos_sp, dt);
            }

            if(flag_att_sp == 0)
            {
                pos_controller.send_accel_setpoint(accel_sp, yaw_sp);
            }else
            {
                q_sp =  thrustToAttitude(accel_sp, yaw_sp);

                //att_sp =  quaternion_to_euler(q_sp);

                thrust_sp = accel_sp.norm();
		ofile << cur_time << "	" << accel_sp[0] << "	" << accel_sp[1] << endl;

                //cout << "thrust_sp: " << thrust_sp <<endl;

                //cout << "Attitude_sp0 [R P Y] : " << att_sp[0] * 180/M_PI <<" [deg] "<<att_sp[1] * 180/M_PI << " [deg] "<< att_sp[2] * 180/M_PI<<" [deg] "<<endl;

                pos_controller.send_attitude_setpoint(q_sp, thrust_sp);
            }

            break;

        // 【Move_Body】 机体系移动。只有PID算法中才有追踪速度的选项，其他控制只能追踪位置
        case Move_Body:
            //只有在comid增加时才会进入解算
            if( Command_Now.comid  >  Command_Last.comid )
            {
                //xy velocity mode
                if( Command_Now.sub_mode & 0b10 )
                {
                    float d_vel_body[2] = {Command_Now.vel_sp[0], Command_Now.vel_sp[1]};         //the desired xy velocity in Body Frame
                    float d_vel_enu[2];                                                           //the desired xy velocity in NED Frame

                    rotation_yaw(pos_controller.euler_fcu[2], d_vel_body, d_vel_enu);
                    vel_sp[0] = d_vel_enu[0];
                    vel_sp[1] = d_vel_enu[1];
                }
                //xy position mode
                else
                {
                    float d_pos_body[2] = {Command_Now.pos_sp[0], Command_Now.pos_sp[1]};         //the desired xy position in Body Frame
                    float d_pos_enu[2];                                                           //the desired xy position in enu Frame (The origin point is the drone)
                    rotation_yaw(pos_controller.euler_fcu[2], d_pos_body, d_pos_enu);

                    pos_sp[0] = pos_controller.pos_drone_fcu[0] + d_pos_enu[0];
                    pos_sp[1] = pos_controller.pos_drone_fcu[1] + d_pos_enu[1];
                }

                //z velocity mode
                if( Command_Now.sub_mode & 0b01 )
                {
                    vel_sp[2] = Command_Now.vel_sp[2];
                }
                //z posiiton mode
                {
                    pos_sp[2] = pos_controller.pos_drone_fcu[2] + Command_Now.pos_sp[2];
                }

                yaw_sp = pos_controller.euler_fcu[2] + Command_Now.yaw_sp;

            }

            if(switch_ude == 0)
            {
                accel_sp = pos_controller_pid.pos_controller(pos_controller.pos_drone_fcu, pos_controller.vel_drone_fcu, pos_sp, vel_sp, Command_Now.sub_mode, dt);
            }else if(switch_ude == 1)
            {
                accel_sp = pos_controller_ude.pos_controller(pos_controller.pos_drone_fcu, pos_controller.vel_drone_fcu, pos_sp, dt);
            }else if(switch_ude == 2)
            {
                accel_sp = pos_controller_ps.pos_controller(pos_drone_mocap, pos_controller.vel_drone_fcu, pos_sp, dt);
            }else if(switch_ude == 3)
            {
                accel_sp = pos_controller_ne.pos_controller(pos_drone_mocap, pos_controller.vel_drone_fcu, pos_sp, dt);
            }

            if(flag_att_sp == 0)
            {
                pos_controller.send_accel_setpoint(accel_sp, yaw_sp);
            }else
            {
                q_sp =  thrustToAttitude(accel_sp, yaw_sp);

                //att_sp =  quaternion_to_euler(q_sp);

                thrust_sp = accel_sp.norm();

                //cout << "thrust_sp: " << thrust_sp <<endl;

                //cout << "Attitude_sp0 [R P Y] : " << att_sp[0] * 180/M_PI <<" [deg] "<<att_sp[1] * 180/M_PI << " [deg] "<< att_sp[2] * 180/M_PI<<" [deg] "<<endl;

                pos_controller.send_attitude_setpoint(q_sp, thrust_sp);
            }

            break;

        // 【Hold】 悬停。当前位置悬停
        case Hold:
            if (Command_Last.command != Hold)
            {
                pos_sp = Eigen::Vector3d(pos_controller.pos_drone_fcu[0],pos_controller.pos_drone_fcu[1],pos_controller.pos_drone_fcu[2]);
                yaw_sp = pos_controller.euler_fcu[2];
            }

            if(switch_ude == 0)
            {
                accel_sp = pos_controller_pid.pos_controller(pos_controller.pos_drone_fcu, pos_controller.vel_drone_fcu, pos_sp, vel_sp, Command_Now.sub_mode, dt);
            }else if(switch_ude == 1)
            {
                accel_sp = pos_controller_ude.pos_controller(pos_controller.pos_drone_fcu, pos_controller.vel_drone_fcu, pos_sp, dt);
            }else if(switch_ude == 2)
            {
                accel_sp = pos_controller_ps.pos_controller(pos_drone_mocap, pos_controller.vel_drone_fcu, pos_sp, dt);
            }else if(switch_ude == 3)
            {
                accel_sp = pos_controller_ne.pos_controller(pos_drone_mocap, pos_controller.vel_drone_fcu, pos_sp, dt);
            }

            if(flag_att_sp == 0)
            {
                pos_controller.send_accel_setpoint(accel_sp, yaw_sp);
            }else
            {
                q_sp =  thrustToAttitude(accel_sp, yaw_sp);

                //att_sp =  quaternion_to_euler(q_sp);

                thrust_sp = accel_sp.norm();

                //cout << "thrust_sp: " << thrust_sp <<endl;

                //cout << "Attitude_sp0 [R P Y] : " << att_sp[0] * 180/M_PI <<" [deg] "<<att_sp[1] * 180/M_PI << " [deg] "<< att_sp[2] * 180/M_PI<<" [deg] "<<endl;

                pos_controller.send_attitude_setpoint(q_sp, thrust_sp);
            }

            break;

        // 【Land】 降落。当前位置原地降落，降落后会自动上锁，且切换为mannual模式
        case Land:
            if (Command_Last.command != Land)
            {
                pos_sp = Eigen::Vector3d(pos_controller.pos_drone_fcu[0],pos_controller.pos_drone_fcu[1],Takeoff_position[2]);
                yaw_sp = pos_controller.euler_fcu[2];
            }

            //如果距离起飞高度小于10厘米，则直接上锁并切换为手动模式；
            if(abs(pos_controller.pos_drone_fcu[2] - 0.2) < Disarm_height)
            {
                if(pos_controller.current_state.mode == "OFFBOARD")
                {
                    pos_controller.mode_cmd.request.custom_mode = "MANUAL";
                    pos_controller.set_mode_client.call(pos_controller.mode_cmd);
                }

                if(pos_controller.current_state.armed)
                {
                    pos_controller.arm_cmd.request.value = false;
                    pos_controller.arming_client.call(pos_controller.arm_cmd);

                }

                if (pos_controller.arm_cmd.response.success)
                {
                    cout<<"Disarm successfully!"<<endl;
                }
            }else
            {

                if(switch_ude == 0)
                {
                    accel_sp = pos_controller_pid.pos_controller(pos_controller.pos_drone_fcu, pos_controller.vel_drone_fcu, pos_sp, vel_sp, 0b00, dt);
                }else if(switch_ude == 1)
                {
                    accel_sp = pos_controller_ude.pos_controller(pos_controller.pos_drone_fcu, pos_controller.vel_drone_fcu, pos_sp, dt);
                }else if(switch_ude == 2)
                {
                    accel_sp = pos_controller_ps.pos_controller(pos_drone_mocap, pos_controller.vel_drone_fcu, pos_sp, dt);
                }else if(switch_ude == 3)
                {
                    accel_sp = pos_controller_ne.pos_controller(pos_drone_mocap, pos_controller.vel_drone_fcu, pos_sp, dt);
                }

                if(flag_att_sp == 0)
                {
                    pos_controller.send_accel_setpoint(accel_sp, yaw_sp);
                }else
                {
                    q_sp =  thrustToAttitude(accel_sp, yaw_sp);

                    //att_sp =  quaternion_to_euler(q_sp);

                    thrust_sp = accel_sp.norm();

                    //cout << "thrust_sp: " << thrust_sp <<endl;

                    //cout << "Attitude_sp0 [R P Y] : " << att_sp[0] * 180/M_PI <<" [deg] "<<att_sp[1] * 180/M_PI << " [deg] "<< att_sp[2] * 180/M_PI<<" [deg] "<<endl;

                    pos_controller.send_attitude_setpoint(q_sp, thrust_sp);
                }
            }

            break;

        // 【Disarm】 紧急上锁。直接上锁，不建议使用，危险。
        case Disarm:
            if(pos_controller.current_state.mode == "OFFBOARD")
            {
                pos_controller.mode_cmd.request.custom_mode = "MANUAL";
                pos_controller.set_mode_client.call(pos_controller.mode_cmd);
            }

            if(pos_controller.current_state.armed)
            {
                pos_controller.arm_cmd.request.value = false;
                pos_controller.arming_client.call(pos_controller.arm_cmd);

            }

            if (pos_controller.arm_cmd.response.success)
            {
                cout<<"Disarm successfully!"<<endl;
            }

            break;

        // 【Failsafe_land】 暂空。可进行自定义
        case Failsafe_land:
            break;

        }

        Command_Last = Command_Now;

        rate.sleep();
    }

    ofile.close();
    return 0;

}

// 【获取当前时间函数】 单位：秒
float get_ros_time(ros::Time begin)
{
    ros::Time time_now = ros::Time::now();
    float currTimeSec = time_now.sec-begin.sec;
    float currTimenSec = time_now.nsec / 1e9 - begin.nsec / 1e9;
    return (currTimeSec + currTimenSec);
}
// 【打印控制指令函数】
void prinft_command_state()
{
    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Command State<<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;

    int sub_mode;
    sub_mode = Command_Now.sub_mode;

    switch(Command_Now.command)
    {
    case Move_ENU:
        cout << "Command: [ Move_ENU ] " <<endl;

        if((sub_mode & 0b10) == 0) //xy channel
        {
            cout << "Submode: xy position control "<<endl;
            cout << "X_setpoint   : " << Command_Now.pos_sp[0] << " [ m ]"  << "  Y_setpoint : "<< Command_Now.pos_sp[1] << " [ m ]"<<endl;
        }
        else{
            cout << "Submode: xy velocity control "<<endl;
            cout << "X_setpoint   : " << Command_Now.vel_sp[0] << " [m/s]" << "  Y_setpoint : "<< Command_Now.vel_sp[1] << " [m/s]" <<endl;
        }

        if((sub_mode & 0b01) == 0) //z channel
        {
            cout << "Submode:  z position control "<<endl;
            cout << "Z_setpoint   : "<< Command_Now.pos_sp[2] << " [ m ]" << endl;
        }
        else
        {
            cout << "Submode:  z velocity control "<<endl;
            cout << "Z_setpoint   : "<< Command_Now.vel_sp[2] << " [m/s]" <<endl;
        }

        cout << "Yaw_setpoint : "  << Command_Now.yaw_sp* 180/M_PI << " [deg] " <<endl;

        break;
    case Move_Body:
        cout << "Command: [ Move_Body ] " <<endl;

        if((sub_mode & 0b10) == 0) //xy channel
        {
            cout << "Submode: xy position control "<<endl;
            cout << "X_setpoint   : " << Command_Now.pos_sp[0] << " [ m ]"  << "  Y_setpoint : "<< Command_Now.pos_sp[1] << " [ m ]"<<endl;
        }
        else{
            cout << "Submode: xy velocity control "<<endl;
            cout << "X_setpoint   : " << Command_Now.vel_sp[0] << " [m/s]" << "  Y_setpoint : "<< Command_Now.vel_sp[1] << " [m/s]" <<endl;
        }

        if((sub_mode & 0b01) == 0) //z channel
        {
            cout << "Submode:  z position control "<<endl;
            cout << "Z_setpoint   : "<< Command_Now.pos_sp[2] << " [ m ]" << endl;
        }
        else
        {
            cout << "Submode:  z velocity control "<<endl;
            cout << "Z_setpoint   : "<< Command_Now.vel_sp[2] << " [m/s]" <<endl;
        }

        cout << "Yaw_setpoint : "  << Command_Now.yaw_sp * 180/M_PI<< " [deg] " <<endl;

        break;

    case Hold:
        cout << "Command: [ Hold ] " <<endl;
        cout << "Hold Position [X Y Z] : " << pos_sp[0] << " [ m ] "<< pos_sp[1]<<" [ m ] "<< pos_sp[2]<<" [ m ] "<<endl;
        cout << "Yaw_setpoint : "  << yaw_sp* 180/M_PI << " [deg] " <<endl;
        break;

    case Land:
        cout << "Command: [ Land ] " <<endl;
        cout << "Land Position [X Y Z] : " << pos_sp[0] << " [ m ] "<< pos_sp[1]<<" [ m ] "<< pos_sp[2]<<" [ m ] "<<endl;
        cout << "Yaw_setpoint : "  << yaw_sp* 180/M_PI << " [deg] " <<endl;
        break;

    case Disarm:
        cout << "Command: [ Disarm ] " <<endl;
        break;

    case Failsafe_land:
        cout << "Command: [ Failsafe_land ] " <<endl;
        break;

    case Idle:
        cout << "Command: [ Idle ] " <<endl;
        break;

    case Takeoff:
        cout << "Command: [ Takeoff ] " <<endl;
        cout << "Takeoff Position [X Y Z] : " << pos_sp[0] << " [ m ] "<< pos_sp[1]<<" [ m ] "<< pos_sp[2]<<" [ m ] "<<endl;
        cout << "Yaw_setpoint : "  << yaw_sp* 180/M_PI << " [deg] " <<endl;
        break;
    }

}
// 【坐标系旋转函数】- 机体系到enu系
// input是机体系,output是惯性系，yaw_angle是当前偏航角
void rotation_yaw(float yaw_angle, float input[2], float output[2])
{
    output[0] = input[0] * cos(yaw_angle) - input[1] * sin(yaw_angle);
    output[1] = input[0] * sin(yaw_angle) + input[1] * cos(yaw_angle);
}


//
// Copyright (c) 2019 Vinnie Falco (vinnie.falco@gmail.com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/cppalliance/json
//

#ifndef BOOST_JSON_DETAIL_NUMBER_CAST_HPP
#define BOOST_JSON_DETAIL_NUMBER_CAST_HPP

#include <boost/json/value.hpp>
#include <boost/json/error.hpp>
#include <limits>
#include <type_traits>

BOOST_JSON_NS_BEGIN
namespace detail {

template<class T>
using is_signed_integral =
    std::integral_constant<bool,
        std::is_signed<T>::value &&
        ! std::is_floating_point<T>::value>;

template<class T>
using is_unsigned_integral =
    std::integral_constant<bool,
        std::is_unsigned<T>::value &&
        ! std::is_same<T, bool>::value>;

template<class T>
T
number_cast(
    value const& jv,
    error_code& ec,
    typename std::enable_if<
        is_signed_integral<T>::value
            >::type* = 0) noexcept
{
    T result{};
    if(jv.kind() == kind::int64)
    {
        auto const i = jv.get_int64();
        if( i > (std::numeric_limits<T>::max)() ||
            i < (std::numeric_limits<T>::min)())
        {
            ec = error::not_exact;
        }
        else
        {
            result = static_cast<T>(i);
        }
    }
    else if(jv.kind() == kind::uint64)
    {
        auto const u = jv.get_uint64();
        if(u > static_cast<std::uint64_t>((
            std::numeric_limits<T>::max)()))
        {
            ec = error::not_exact;
        }
        else
        {
            result = static_cast<T>(u);
        }
    }
    else
    {
        BOOST_ASSERT(jv.kind() == kind::double_);
        auto const d = jv.get_double();
        if( d > (std::numeric_limits<T>::max)() ||
            d < (std::numeric_limits<T>::min)() ||
            static_cast<T>(d) != d)
        {
            ec = error::not_exact;
        }
        else
        {
            result = static_cast<T>(d);
        }
    }
    return result;
}

template<class T>
T
number_cast(
    value const& jv,
    error_code& ec,
    typename std::enable_if<
        is_unsigned_integral<T>::value
            >::type* = 0
    ) noexcept
{
    T result{};
    if(jv.kind() == kind::int64)
    {
        auto const i = jv.get_int64();
        if( i < 0 || static_cast<std::uint64_t>(i) >
            (std::numeric_limits<T>::max)())
        {
            ec = error::not_exact;
        }
        else
        {
            result = static_cast<T>(i);
        }
    }
    else if(jv.kind() == kind::uint64)
    {
        auto const u = jv.get_uint64();
        if(u > (std::numeric_limits<T>::max)())
        {
            ec = error::not_exact;
        }
        else
        {
            result = static_cast<T>(u);
        }
    }
    else
    {
        BOOST_ASSERT(jv.kind() == kind::double_);
        auto const d = jv.get_double();
        if( d < 0 ||
            d > (std::numeric_limits<T>::max)() ||
            static_cast<T>(d) != d)
        {
            ec = error::not_exact;
        }
        else
        {
            result = static_cast<T>(d);
        }
    }
    return result;
}

template<class T>
T
number_cast(
    value const& jv,
    error_code&,
    typename std::enable_if<
        std::is_floating_point<T>::value
            >::type* = 0
    ) noexcept
{
    if(jv.kind() == kind::int64)
        return static_cast<T>(jv.get_int64());

    if(jv.kind() == kind::uint64)
        return static_cast<T>(jv.get_uint64());

    BOOST_ASSERT(jv.kind() == kind::double_);
    return static_cast<T>(jv.get_double());
}

} // detail
BOOST_JSON_NS_END

#endif


#ifndef _RSA_FACE_DETECTED_HPP_
#define _RSA_FACE_DETECTED_HPP_

#include <vector>
#include <string>
#include <boost/smart_ptr.hpp>
#include <boost/make_shared.hpp>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "caffe/caffe.hpp"
#include "config.hpp"
#include <glog/logging.h>

#include "inf_rsa_face_detection_api.hh"

class RsaFaceDetector {
    public:
        explicit RsaFaceDetector(unsigned int gpuId, \
                    const std::string & sfnNet, const std::string & sfnWeight, \
                    const std::string & rsaNet, const std::string & rsaWeight, \
                    const std::string & lrnNet, const std::string & lrnWeight);
        ~RsaFaceDetector() {};
        imgsFaceLandmarkList detect_(std::vector<cv::Mat> iamges);
        facesLandmarkPerImg detect_(cv::Mat image);
        void sfnProcess_(const std::vector<cv::Mat> images);
        void sfnProcess_(const cv::Mat & img);
        void rsaProcess_(void);
        void lrnProcess_(imgsFaceLandmarkList & faceResult);
        void lrnProcess_(facesLandmarkPerImg & faceResult);

    private:
        std::string sfnNetDef_;
        std::string sfnNetWeight_;
        std::string rsaNetDef_;
        std::string rsaNetWeight_;
        std::string lrnNetDef_;
        std::string lrnNetWeight_;
        unsigned int gpuId_;

        std::vector<std::shared_ptr<caffe::Blob<float>>> transFeatMaps_;
        caffe::Blob<float> *sfnNetOutput_;
        caffe::Blob<float> *inputLayer_;
        caffe::Blob<float> *rsaInputLayer_;
        caffe::Blob<float> *lrnInputLayer_;
        std::vector<cv::Mat> inputChannels_;

        double resizeFactor_;
        std::vector<float> anchorBoxLen_;
        double threshScore_;
        double stride_;
        double anchorCenter_;
        std::vector<int> scale_;
        std::shared_ptr<caffe::Net<float>> sfnNet_;
        std::shared_ptr<caffe::Net<float>> rsaNet_;
        std::shared_ptr<caffe::Net<float>> lrnNet_;
};

#endif


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Thu 10 May 2018 14:56:07

/**
 * @file CMSSMNoFV_two_scale_ewsb_solver.hpp
 *
 * @brief contains class for solving EWSB when two-scale algorithm is used
 *
 * This file was generated at Thu 10 May 2018 14:56:07 with FlexibleSUSY
 * 2.0.1 (git commit: unknown) and SARAH 4.12.2 .
 */

#ifndef CMSSMNoFV_TWO_SCALE_EWSB_SOLVER_H
#define CMSSMNoFV_TWO_SCALE_EWSB_SOLVER_H

#include "CMSSMNoFV_ewsb_solver.hpp"
#include "CMSSMNoFV_ewsb_solver_interface.hpp"
#include "error.hpp"

#include <Eigen/Core>

namespace flexiblesusy {

class EWSB_solver;
class Two_scale;

class CMSSMNoFV_mass_eigenstates;

template<>
class CMSSMNoFV_ewsb_solver<Two_scale> : public CMSSMNoFV_ewsb_solver_interface {
public:
   CMSSMNoFV_ewsb_solver() = default;
   CMSSMNoFV_ewsb_solver(const CMSSMNoFV_ewsb_solver&) = default;
   CMSSMNoFV_ewsb_solver(CMSSMNoFV_ewsb_solver&&) = default;
   virtual ~CMSSMNoFV_ewsb_solver() {}
   CMSSMNoFV_ewsb_solver& operator=(const CMSSMNoFV_ewsb_solver&) = default;
   CMSSMNoFV_ewsb_solver& operator=(CMSSMNoFV_ewsb_solver&&) = default;

   virtual void set_loop_order(int l) override { loop_order = l; }
   virtual void set_number_of_iterations(int n) override { number_of_iterations = n; }
   virtual void set_precision(double p) override { precision = p; }

   virtual int get_loop_order() const override { return loop_order; }
   virtual int get_number_of_iterations() const override { return number_of_iterations; }
   virtual double get_precision() const override { return precision; }

   virtual int solve(CMSSMNoFV_mass_eigenstates&) override;
private:
   static const int number_of_ewsb_equations = 2;
   using EWSB_vector_t = Eigen::Matrix<double,number_of_ewsb_equations,1>;

   class EEWSBStepFailed : public Error {
   public:
      virtual ~EEWSBStepFailed() {}
      virtual std::string what() const { return "Could not perform EWSB step."; }
   };

   int number_of_iterations{100}; ///< maximum number of iterations
   int loop_order{2};             ///< loop order to solve EWSB at
   double precision{1.e-5};       ///< precision goal

   void set_ewsb_solution(CMSSMNoFV_mass_eigenstates&, const EWSB_solver*);
   template <typename It> void set_best_ewsb_solution(CMSSMNoFV_mass_eigenstates&, It, It);

   int solve_tree_level(CMSSMNoFV_mass_eigenstates&);
   int solve_iteratively(CMSSMNoFV_mass_eigenstates&);
   int solve_iteratively_at(CMSSMNoFV_mass_eigenstates&, int);
   int solve_iteratively_with(CMSSMNoFV_mass_eigenstates&, EWSB_solver*, const EWSB_vector_t&);

   EWSB_vector_t initial_guess(const CMSSMNoFV_mass_eigenstates&) const;
   EWSB_vector_t tadpole_equations(const CMSSMNoFV_mass_eigenstates&) const;
   EWSB_vector_t ewsb_step(const CMSSMNoFV_mass_eigenstates&) const;
};

} // namespace flexiblesusy

#endif


//Tencent is pleased to support the open source community by making FeatherCNN available.

//Copyright (C) 2019 THL A29 Limited, a Tencent company. All rights reserved.

//Licensed under the BSD 3-Clause License (the "License"); you may not use this file except
//in compliance with the License. You may obtain a copy of the License at
//
//https://opensource.org/licenses/BSD-3-Clause
//
//Unless required by applicable law or agreed to in writing, software distributed
//under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
//CONDITIONS OF ANY KIND, either express or implied. See the License for the
//specific language governing permissions and limitations under the License.

#include <booster/depthwise.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <assert.h>

//#include <arm_neon.h>

#ifdef __APPLE__
#else
#include <omp.h>
#endif

template <bool fuseBias, bool fuseRelu>
void globalDwConv(float *output, const float *input, int input_channels, int inw, int inh, float *kernel, int group, int nThreads, float *bias_arr)
{
    assert(group > 0 || input_channels % group == 0);
    int step = inw * inh;
    int block = input_channels / group;
    int groupKernelSize = inw * inh * group;

    for (int i = 0; i < input_channels; i++)
    {
        int k = i / group, u = i % group;
        output[i] = 0;
        for (int j = 0; j < step; j++)
        {
            output[i] += input[i * step + j] * kernel[k * groupKernelSize + u * step + j];
        }
        if (fuseBias)
        {
            output[i] += bias_arr[i];
        }
        if (fuseRelu)
        {
            output[i] = (output[i] > 0.f) ? output[i] : 0.f;
        }
    }

    /*
    int kw = inw, kh = inh;
    int width = kw * kh;
    int widthAligned = width & 0xFFFFFFFC;
    int widthRem = width & 0x03; // int widthRem = width & 0x11;
    int height = group;
    int heightAligned = group & 0xFFFFFFFC;
    int heightRem = height & 0x03; // int heightRem = height & 0x11;
    float ext[8];
    for(int i = 0; i < heightAligned; i += 4)
    {
        float32x4_t sum = vdupq_n_f32(0.f);
        float* p0 = const_cast<float *>(input) + width * i;
        float* p1 = p0 + width;
        float* p2 = p1 + width;
        float* p3 = p2 + width;
        float* k0 = kernel + width * i;
        float* k1 = k0 + width;
        float* k2 = k1 + width;
        float* k3 = k2 + width;

        for(int j = 0; j < widthAligned; j += 4)
        {
            float32x4_t v0 = vld1q_f32(p0);
            p0 += 4;
            float32x4_t v1 = vld1q_f32(p1);
            p1 += 4;
            float32x4_t v2 = vld1q_f32(p2);
            p2 += 4;
            float32x4_t v3 = vld1q_f32(p3);
            p3 += 4;

            float32x4_t r0 = vld1q_f32(k0);
            k0 += 4;
            float32x4_t r1 = vld1q_f32(k1);
            k1 += 4;
            float32x4_t r2 = vld1q_f32(k2);
            k2 += 4;
            float32x4_t r3 = vld1q_f32(k3);
            k3 += 4;

            float32x4x2_t row01 = vtrnq_f32(v0, v1);
            float32x4x2_t row23 = vtrnq_f32(v2, v3);

    //           * row0 = ( x00 x10 x20 x30 )
    //           * row1 = ( x01 x11 x21 x31 )
    //           * row2 = ( x02 x12 x22 x32 )
    //           * row3 = ( x03 x13 x23 x33 )

            v0 = vcombine_f32(vget_low_f32(row01.val[0]), vget_low_f32(row23.val[0]));
            v1 = vcombine_f32(vget_low_f32(row01.val[1]), vget_low_f32(row23.val[1]));
            v2 = vcombine_f32(vget_high_f32(row01.val[0]), vget_high_f32(row23.val[0]));
            v3 = vcombine_f32(vget_high_f32(row01.val[1]), vget_high_f32(row23.val[1]));
            row01 = vtrnq_f32(r0, r1);
            row23 = vtrnq_f32(r2, r3);
            r0 = vcombine_f32(vget_low_f32(row01.val[0]), vget_low_f32(row23.val[0]));
            r1 = vcombine_f32(vget_low_f32(row01.val[1]), vget_low_f32(row23.val[1]));
            r2 = vcombine_f32(vget_high_f32(row01.val[0]), vget_high_f32(row23.val[0]));
            r3 = vcombine_f32(vget_high_f32(row01.val[1]), vget_high_f32(row23.val[1]));
    #ifdef __aarch64__
            sum = vfmaq_f32(sum, v0, r0);
            sum = vfmaq_f32(sum, v1, r1);
            sum = vfmaq_f32(sum, v2, r2);
            sum = vfmaq_f32(sum, v3, r3);
    #else
            sum = vmlaq_f32(sum, v0, r0);
            sum = vmlaq_f32(sum, v1, r1);
            sum = vmlaq_f32(sum, v2, r2);
            sum = vmlaq_f32(sum, v3, r3);
    #endif
        }
        if(widthRem){
            for(int j = 0; j < widthRem; ++j)
            {
                ext[0] = p0[j];
                ext[1] = p1[j];
                ext[2] = p2[j];
                ext[3] = p3[j];
                ext[4] = k0[j];
                ext[5] = k1[j];
                ext[6] = k2[j];
                ext[7] = k3[j];
    #ifdef __aarch64__
                sum = vfmaq_f32(sum, vld1q_f32(ext + 4), vld1q_f32(ext));
    #else
                sum = vmlaq_f32(sum, vld1q_f32(ext + 4), vld1q_f32(ext));
    #endif
            }
        }
        vst1q_f32(output + i, sum);
    }
    for(int i = heightAligned; i < height; ++i)
    {
        float* p = const_cast<float *>(input) + i * width;
        float* k = kernel + i * width;
        float sum = 0.f;
        for(int j = 0; j < width; ++j)
        {
            sum += p[j] * k[j];
        }
        output[i] = sum; // output[heightAligned + i] = sum;
    }
    */
}

template <bool fuseBias, bool fuseRelu>
void dwConv_template(float *output, float *input, int input_channels, int inw, int inh, int stridew, int strideh, float *kernel, int kw, int kh, int group, int nThreads, float *bias_arr)
{
    if ((kw == inw) && (kh == inh))
    {
        globalDwConv<fuseBias, fuseRelu>(output, input, input_channels, inw, inh, kernel, group, nThreads, bias_arr);
    }
    else
    {
        int outw = (inw - kw) / stridew + 1; //for strided case in odd dimensions, should take the floor value as output dim.
        int outh = (inh - kh) / strideh + 1;

// #pragma omp parallel for num_threads(nThreads) schedule(static)
        //printf("dw param %d kernel %d %d stride %d %d input %d %d %d output %d %d\n", group, kh, kw, strideh, stridew, input_channels, inh, inw, outh, outw);
        for (int g = 0; g < group; ++g)
        {
            float *kp = kernel + kw * kh * g;
            float *outg = output + g * outw * outh;
            float *ing = input + g * inw * inh;
            for (int i = 0; i < outh; ++i)
            {
                for (int j = 0; j < outw; ++j)
                {
                    float *inp = ing + inw * (i * stridew) + (j * strideh);
                    float convSum = 0.f;
                    for (int m = 0; m < kh; m++)
                    {
                        for (int n = 0; n < kw; n++)
                        {
                            convSum += inp[m * inw + n] * kp[m * kw + n];
                        }
                    }
                    if (fuseBias)
                    {
                        convSum += bias_arr[g];
                    }
                    if (fuseRelu)
                    {
                        convSum = (convSum > 0.f) ? convSum : 0.f;
                    }
                    outg[j] = convSum;
                }
                outg += outw;
            }
        }
    }
}

template void dwConv_template<false, false>(float *, float *, int, int, int, int, int, float *, int, int, int, int, float *);
template void dwConv_template<false,  true>(float *, float *, int, int, int, int, int, float *, int, int, int, int, float *);
template void dwConv_template<true,  false>(float *, float *, int, int, int, int, int, float *, int, int, int, int, float *);
template void dwConv_template<true,   true>(float *, float *, int, int, int, int, int, float *, int, int, int, int, float *);


#include <algorithm>
#include <cassert>
#include <cmath>
#include <set>
#include <stdexcept>
#include <utility>
#include <boost/scoped_ptr.hpp>
#include <opengm/inference/lpcplex.hxx>
#include <opengm/datastructures/marray/marray.hxx>

#include "pgmlink/pgm.h"
#include "pgmlink/hypotheses.h"
#include "pgmlink/log.h"
#include "pgmlink/pgm_chaingraph.h"
#include "pgmlink/traxels.h"

//#include <ostream>

using namespace std;

namespace pgmlink {
  namespace pgm {
    namespace chaingraph {
    ////
    //// class chaingraph::Model
    ////
    Model::Model() : opengm_model( new OpengmModel() ) {
      init();
    }

    Model::Model(shared_ptr<OpengmModel> m,
				     const node_var_map& node_var,
				     const arc_var_map& arc_var
				     )
      : opengm_model(m) {
      node_var_.left = node_var;
      arc_var_.left = arc_var;
      init();
      }

    const Model::node_var_map& Model::var_of_node() const {
      return node_var_.left;
    }

    const Model::var_node_map& Model::node_of_var() const {
      return node_var_.right;
    }

    const Model::arc_var_map& Model::var_of_arc() const {
      return arc_var_.left;
    }

    const Model::var_arc_map& Model::arc_of_var() const
    {
      return arc_var_.right;
    }

    Model::var_t Model::var_of_node(node_t e) const {
      node_var_map::const_iterator it = var_of_node().find(e);
      if(it!=var_of_node().end()) {
	return it->second;
      } else {
	throw std::out_of_range("chaingraph::Model::var_of_node(): key does not exist");
      }
    }

    Model::var_t Model::var_of_arc(arc_t e) const {
      arc_var_map::const_iterator it = var_of_arc().find(e);
      if(it!=var_of_arc().end()) {
	return it->second;
      } else {
	throw std::out_of_range("ChaingraphModel::var_of_arc(): key does not exist");
      }
    }

    Model::node_t Model::node_of_var(var_t e) const {
      var_node_map::const_iterator it = node_of_var().find(e);
      if(it!=node_of_var().end()) {
	return it->second;
      } else {
	throw std::out_of_range("ChaingraphModel::node_of_var(): key does not exist");
      }
    }

    Model::arc_t Model::arc_of_var(var_t e) const {
      var_arc_map::const_iterator it = arc_of_var().find(e);
      if(it!=arc_of_var().end()) {
	return it->second;
      } else {
	throw std::out_of_range("ChaingraphModel::arc_of_var(): key does not exist");
      }
    }

    Model::VarCategory Model::var_category(var_t e) const {
      if(arc_of_var().count(e)) {
	return Model::arc_var;
      } else if(node_of_var().count(e)) {
	return Model::node_var;
      } else {
	throw std::out_of_range("ChaingraphModel::var_category(): key does not exist");
      }
    }

    void Model::init() {
      weight_map[det_weight] = vector<OpengmModel::IndexType>();
      weight_map[mov_weight] = vector<OpengmModel::IndexType>();
      weight_map[div_weight] = vector<OpengmModel::IndexType>();
      weight_map[app_weight] = vector<OpengmModel::IndexType>();
      weight_map[dis_weight] = vector<OpengmModel::IndexType>();
      weight_map[opp_weight] = vector<OpengmModel::IndexType>();
    }

    ////
    //// class ModelBuilder
    ////
    ModelBuilder& ModelBuilder::appearance( function<double (const Traxel&)> f ) {
      if(!f) {
	throw invalid_argument("ChaingraphModelBuilder::appearance(): empty function");
      }
      appearance_ = f;
      return *this;
    }

    ModelBuilder& ModelBuilder::disappearance( function<double (const Traxel&)> f ) {
      if(!f) {
	throw invalid_argument("ChaingraphModelBuilder::disappearance(): empty function");
      }
      disappearance_ = f;
      return *this;
    }

    ModelBuilder& ModelBuilder::move( function<double (const Traxel&,const Traxel&)> f ) {
      if(!f) {
	throw invalid_argument("ChaingraphModelBuilder::move(): empty function");
      }
      move_ = f;
      return *this;
    }

    ModelBuilder& ModelBuilder::with_detection_vars( function<double (const Traxel&)> detection,
									 function<double (const Traxel&)> non_detection) {
      if(!(detection && non_detection)) {
	throw invalid_argument("ChaingraphModelBuilder::with_detection_vars(): empty function");
      }

      with_detection_vars_ = true;
      detection_ = detection;
      non_detection_ = non_detection;
      return *this;
    }

    ModelBuilder& ModelBuilder::without_detection_vars() {
      with_detection_vars_ = false;
      detection_ = NULL;
      non_detection_ = NULL;
      return *this;
    }

    ModelBuilder& ModelBuilder::with_divisions( function<double (const Traxel&,const Traxel&,const Traxel&)> division) {
      if(!division) {
	throw invalid_argument("ChaingraphModelBuilder::division(): empty function");
      }
      with_divisions_ = true;
      division_ = division;
      return *this;
    }

    ModelBuilder& ModelBuilder::without_divisions() {
      with_divisions_ = false;
      division_ = NULL;
      return *this;
    }

      namespace {
	inline size_t cplex_id(size_t opengm_id) {
	  return 2*opengm_id + 1;
	}
      }
      void ModelBuilder::add_hard_constraints( const Model& m, const HypothesesGraph& hypotheses, OpengmLPCplex& cplex ) {
	LOG(logDEBUG) << "Chaingraph::add_constraints: entered";
	////
	//// outgoing transitions
	////
	LOG(logDEBUG) << "Chaingraph::add_constraints: outgoing transitions";
	for(HypothesesGraph::NodeIt n(hypotheses); n!=lemon::INVALID; ++n) {
	  // couple detection and transitions
	  if(has_detection_vars()) {
	    for(HypothesesGraph::OutArcIt a(hypotheses, n); a!=lemon::INVALID; ++a) {
	      couple(m, n, a, cplex);
	    }
	  }

	  // couple assignments
	  vector<size_t> cplex_idxs;
	  for(HypothesesGraph::OutArcIt a(hypotheses, n); a!=lemon::INVALID; ++a) {
	    cplex_idxs.push_back(cplex_id(m.var_of_arc(a)));
	  }
	  if( cplex_idxs.size() > 0 ) {
	    vector<int> coeffs(cplex_idxs.size(), 1);
	    // 0 <= 1*transition + ... + 1*transition <= 2 [div] or 1 [no div]
	    const size_t max_on = has_divisions() ? 2 : 1;
	    cplex.addConstraint(cplex_idxs.begin(), cplex_idxs.end(), coeffs.begin(), 0, max_on);
	  }
	}

	////
	//// incoming transitions
	////
	LOG(logDEBUG) << "Chaingraph::add_constraints: incoming transitions";
	for(HypothesesGraph::NodeIt n(hypotheses); n!=lemon::INVALID; ++n) {
	  // couple detection and transitions
	  if(has_detection_vars()) {
	    for(HypothesesGraph::InArcIt a(hypotheses, n); a!=lemon::INVALID; ++a) {
	      couple(m, n, a, cplex);
	    }
	  }

	  // couple transitions
	  vector<size_t> cplex_idxs;
	  for(HypothesesGraph::InArcIt a(hypotheses, n); a!=lemon::INVALID; ++a) {
	    cplex_idxs.push_back(cplex_id(m.var_of_arc(a)));
	  }
	  if(cplex_idxs.size() > 0) {
	    vector<int> coeffs(cplex_idxs.size(), 1);
	    // 0 <= 1*transition + ... + 1*transition <= 1
	    cplex.addConstraint(cplex_idxs.begin(), cplex_idxs.end(), coeffs.begin(), 0, 1);
	  }
	}
      }

      void ModelBuilder::fix_detections( const Model& m, const HypothesesGraph& g, OpengmLPCplex& cplex ) {
	if(!has_detection_vars()) {
	  throw std::runtime_error("chaingraph::ModelBuilder::fix_detections(): called without has_detection_vars()");
	}
	for(HypothesesGraph::NodeIt n(g); n!=lemon::INVALID; ++n) {
	  vector<size_t> cplex_idxs;
	  cplex_idxs.push_back(cplex_id(m.var_of_node(n)));
	  vector<int> coeffs;
	  coeffs.push_back(1);
	  // 1 <= 1*detection <= 1
	  cplex.addConstraint(cplex_idxs.begin(), cplex_idxs.end(), coeffs.begin() , 1, 1);
	}
      }

      inline void ModelBuilder::add_detection_vars( const HypothesesGraph& hypotheses, Model& m ) const {
	if(!has_detection_vars()) {
	  throw std::runtime_error("chaingraph::ModelBuilder::add_detection_vars(): called without has_detection_vars()");
	}

	for(HypothesesGraph::NodeIt n(hypotheses); n!=lemon::INVALID; ++n) {
	  m.opengm_model->addVariable(2);
	  m.node_var_.left.insert(Model::node_var_map::value_type(n, m.opengm_model->numberOfVariables() - 1));
	}
      }

      inline void ModelBuilder::add_assignment_vars( const HypothesesGraph& hypotheses, Model& m ) const {
	for(HypothesesGraph::ArcIt a(hypotheses); a!=lemon::INVALID; ++a) {
	  m.opengm_model->addVariable(2);
	  m.arc_var_.left.insert(Model::arc_var_map::value_type(a, m.opengm_model->numberOfVariables() - 1));
	}
      }

      vector<OpengmModel::IndexType> ModelBuilder::vars_for_outgoing_factor( const HypothesesGraph& hypotheses,
									     const Model& m,
									     const HypothesesGraph::Node& n) const {
	vector<OpengmModel::IndexType> vi; // opengm variable indices; can be empty if there are no det vars
	if(has_detection_vars()) {
	  vi.push_back(m.var_of_node(n)); // first detection node, remaining will be assignment vars
	}
	for(HypothesesGraph::OutArcIt a(hypotheses, n); a != lemon::INVALID; ++a) {
	  vi.push_back(m.var_of_arc(a));
	}
	return vi;
      }

      vector<OpengmModel::IndexType> ModelBuilder::vars_for_incoming_factor( const HypothesesGraph& hypotheses,
									     const Model& m,
									     const HypothesesGraph::Node& n) const {
	vector<OpengmModel::IndexType> vi; // opengm variable indices; can be empty if there are no det vars
	if(has_detection_vars()) {
	  vi.push_back(m.var_of_node(n)); // detection var, remaining will be assignment vars
	}
	for(HypothesesGraph::InArcIt a(hypotheses, n); a != lemon::INVALID; ++a) {
	  vi.push_back(m.var_of_arc(a));
	}
	std::reverse(vi.begin(), vi.end()); // det var should be the first index to be consistent with vars_for_outgoing_factor()
	return vi;
      }

      void ModelBuilder::couple(const Model& m, const HypothesesGraph::Node& n, const HypothesesGraph::Arc& a, OpengmLPCplex& cplex ) {
	vector<size_t> cplex_idxs;
	cplex_idxs.push_back(cplex_id(m.var_of_node(n)));
	cplex_idxs.push_back(cplex_id(m.var_of_arc(a)));
	vector<int> coeffs;
	coeffs.push_back(1);
	coeffs.push_back(-1);
	// 0 <= 1*detection - 1*transition <= 1
	cplex.addConstraint(cplex_idxs.begin(), cplex_idxs.end(), coeffs.begin() , 0, 1);
      }

    ////
    //// class TrainableChaingraphModelBuilder
    ////
    TrainableModelBuilder* TrainableModelBuilder::clone() const {
      return new TrainableModelBuilder(*this);
    }

    Model* TrainableModelBuilder::build(const HypothesesGraph& hypotheses) const {
      //// setup the model
      Model* model( new Model() );

      // assign the weight ids to event types
      assert(model->opengm_model->numberOfWeights() == 0);
      model->opengm_model->increaseNumberOfWeights(3);
      model->weight_map[Model::mov_weight].push_back(0);
      model->weight_map[Model::app_weight].push_back(1);
      model->weight_map[Model::dis_weight].push_back(2);

      if(has_divisions()) {
	model->opengm_model->increaseNumberOfWeights(1);
	model->weight_map[Model::div_weight].push_back(3);
      }
      if(has_detection_vars()) {
	model->opengm_model->increaseNumberOfWeights(2);
	model->weight_map[Model::det_weight].push_back(4);
	model->weight_map[Model::opp_weight].push_back(5);
      }

      if( has_detection_vars() ) {
	add_detection_vars( hypotheses, *model );
      }
      add_assignment_vars( hypotheses, *model );

      if( has_detection_vars() ) {
      	for(HypothesesGraph::NodeIt n(hypotheses); n!=lemon::INVALID; ++n) {
      	  add_detection_factor( hypotheses, *model, n );
      	}
      }

      for(HypothesesGraph::NodeIt n(hypotheses); n!=lemon::INVALID; ++n) {
      	add_outgoing_factor( hypotheses, *model, n );
      	add_incoming_factor( hypotheses, *model, n );
      }

      return model;
    }

    void TrainableModelBuilder::add_detection_factor( const HypothesesGraph& hypotheses, Model& m, const HypothesesGraph::Node& n ) const {
      property_map<node_traxel, HypothesesGraph::base_graph>::type& traxel_map = hypotheses.get(node_traxel());
      std::vector<size_t> var_indices;
      var_indices.push_back(m.var_of_node(n));
      size_t shape[] = {2};

      size_t indicate[] = {0};
      OpengmWeightedFeature<OpengmModel::ValueType>(var_indices, shape, shape+1, indicate, non_detection()(traxel_map[n]) )
      	.add_as_feature_to( *(m.opengm_model), m.weight_map[Model::det_weight].front() );

      indicate[0] = 1;
      OpengmWeightedFeature<OpengmModel::ValueType>(var_indices, shape, shape+1, indicate, detection()(traxel_map[n]) )
      	.add_as_feature_to( *(m.opengm_model), m.weight_map[Model::det_weight].front() );
    }

    namespace {
      std::vector<size_t> DecToBin(size_t number)
      {
	if ( number == 0 ) return std::vector<size_t>(1,0);
	if ( number == 1 ) return std::vector<size_t>(1,1);

	if ( number % 2 == 0 ) {
	  std::vector<size_t> ret = DecToBin(number / 2);
	  ret.push_back(0);
	  return ret;
	}
	else {
	  std::vector<size_t> ret = DecToBin(number / 2);
	  ret.push_back(1);
	  return ret;
	}
      }

      int BinToDec(std::vector<size_t> number)
      {
	int result = 0, pow = 1;
	for ( int i = number.size() - 1; i >= 0; --i, pow <<= 1 )
	  result += number[i] * pow;
	return result;
      }
    }

    inline void TrainableModelBuilder::add_outgoing_factor( const HypothesesGraph& hypotheses,
							    Model& m,
							    const HypothesesGraph::Node& n) const {
      using namespace std;
      property_map<node_traxel, HypothesesGraph::base_graph>::type& traxel_map = hypotheses.get(node_traxel());

      LOG(logDEBUG) << "TrainableModelBuilder::add_outgoing_factor(): entered";
      // setup node and arc var indices
      const vector<size_t> vi = vars_for_outgoing_factor(hypotheses, m, n); // one or zero det vars and zero or more assignment vars
      if(vi.size() == 0) {
	// nothing to do here, get out!
	// happens in case of no det vars and no outgoing arcs
	return;
      }

      // collect outgoing arcs for use in the feature functions further down
      vector<HypothesesGraph::Arc> arcs;
      for(HypothesesGraph::OutArcIt a(hypotheses, n); a != lemon::INVALID; ++a) {
	arcs.push_back(a);
      }

      // construct factor
      const size_t table_dim = vi.size();
      assert(table_dim > 0);
      const std::vector<size_t> shape(table_dim, 2);
      std::vector<size_t> coords;

      std::set<size_t > entries;
      for(size_t i = 0; i < static_cast<size_t>(std::pow(2., static_cast<int>(table_dim))); ++i) {
	entries.insert( entries.end(), i );
      }

      // opportunity configuration; only in case of detection vars
      if(has_detection_vars()) {
	coords = std::vector<size_t>(table_dim, 0); // (0,0,...,0)
	size_t check = entries.erase(BinToDec(coords));
	assert(check == 1);
	OpengmWeightedFeature<OpengmModel::ValueType>(vi, shape.begin(), shape.end(), coords.begin(), opportunity_cost() )
	  .add_as_feature_to( *(m.opengm_model), m.weight_map[Model::opp_weight].front() );
      }

      // disappearance configuration
      {
	coords = std::vector<size_t>(table_dim, 0);
	if(has_detection_vars()){
	  coords[0] = 1; // (1,0,...,0)
	}
	size_t check = entries.erase(BinToDec(coords));
	assert(check == 1);
	OpengmWeightedFeature<OpengmModel::ValueType>(vi, shape.begin(), shape.end(), coords.begin(), disappearance()(traxel_map[n]) )
	  .add_as_feature_to( *(m.opengm_model), m.weight_map[Model::dis_weight].front() );
      }

      // move configurations
      {
	coords = std::vector<size_t>(table_dim, 0);
	size_t assignment_begin = 0;
	if(has_detection_vars()) {
	  coords[0] = 1;
	  assignment_begin = 1;
	}
	// (1  ,0,0,0,1,0,0)
	for(size_t i = assignment_begin; i < table_dim; ++i) {
	  coords[i] = 1;
	  size_t check = entries.erase(BinToDec(coords));
	  assert(check == 1);
	  OpengmWeightedFeature<OpengmModel::ValueType>(vi, shape.begin(), shape.end(), coords.begin(), move()(traxel_map[n], traxel_map[hypotheses.target(arcs[i-assignment_begin])]) )
	    .add_as_feature_to( *(m.opengm_model), m.weight_map[Model::mov_weight].front() );
	  coords[i] = 0; // reset coords
	}
      }

      // division configurations
      if(has_divisions()) {
	coords = std::vector<size_t>(table_dim, 0);
	size_t assignment_begin = 0;
	if(has_detection_vars()) {
	  coords[0] = 1;
	  assignment_begin = 1;
	}

	// (1   ,0,0,1,0,1,0,0)
	for(unsigned int i = assignment_begin; i < table_dim - 1; ++i) {
	  for(unsigned int j = i+1; j < table_dim; ++j) {
	    coords[i] = 1;
	    coords[j] = 1;
	    size_t check = entries.erase(BinToDec(coords));
	    assert(check == 1);
	    OpengmModel::ValueType value = division()(traxel_map[n],
							traxel_map[hypotheses.target(arcs[i-assignment_begin])],
							traxel_map[hypotheses.target(arcs[j-assignment_begin])]);
	    OpengmWeightedFeature<OpengmModel::ValueType>(vi, shape.begin(), shape.end(), coords.begin(), value)
	      .add_as_feature_to( *(m.opengm_model), m.weight_map[Model::div_weight].front() );
	    // reset
	    coords[i] = 0;
	    coords[j] = 0;
	  }
	}
      }

      // forbidden configurations
      {
	for(std::set<size_t>::iterator it = entries.begin(); it != entries.end(); ++it) {
	  coords = DecToBin(*it);
	  // pad with zeros up to coordinate size
	  if(coords.size() < table_dim ) {
	    coords.insert(coords.begin(), table_dim-coords.size(), 0);
	  }
	  assert( coords.size() == table_dim );
	  OpengmWeightedFeature<OpengmModel::ValueType>(vi, shape.begin(), shape.end(), coords.begin(), forbidden_cost())
	    .add_to( *(m.opengm_model) );
	}
      }

      LOG(logDEBUG) << "TrainableChaingraphModelBuilder::add_outgoing_factor(): leaving";
    }

    inline void TrainableModelBuilder::add_incoming_factor( const HypothesesGraph& hypotheses,
								      Model& m,
								      const HypothesesGraph::Node& n ) const {
      using namespace std;
      property_map<node_traxel, HypothesesGraph::base_graph>::type& traxel_map = hypotheses.get(node_traxel());

      LOG(logDEBUG) << "TrainableModelBuilder::add_incoming_factor(): entered";
      // collect and count incoming arcs
      const vector<size_t> vi = vars_for_incoming_factor(hypotheses, m, n); // one or zero det vars and zero or more assignment vars
      if(vi.size() == 0) {
	// nothing to do here, get out!
	// happens in case of no det vars and no incoming arcs
	return;
      }

      //// construct factor
      const size_t table_dim = vi.size();
      const std::vector<size_t> shape(table_dim, 2);
      std::vector<size_t> coords;

      std::set<size_t > entries;
      for(size_t i = 0; i < static_cast<size_t>(std::pow(2., static_cast<int>(table_dim))); ++i) {
	entries.insert( entries.end(), i );
      }

      // allow opportunity configuration
      // (0,0,...,0)
      if(has_detection_vars()) {
	coords = std::vector<size_t>(table_dim, 0);
	size_t check = entries.erase(BinToDec(coords));
	assert(check == 1);
      }

      // appearance configuration
      coords = std::vector<size_t>(table_dim, 0);
      if(has_detection_vars()) {
	coords[0] = 1; // (1,0,...,0)
      }
      size_t check = entries.erase(BinToDec(coords));
      assert(check == 1);
      OpengmWeightedFeature<OpengmModel::ValueType>(vi, shape.begin(), shape.end(), coords.begin(), appearance()(traxel_map[n]) )
	.add_as_feature_to( *(m.opengm_model), m.weight_map[Model::app_weight].front() );

      // allow move configurations
      // (1,0,0,0,1,0,0)
      coords = std::vector<size_t>(table_dim, 0);
      size_t assignment_begin = 0;
      if(has_detection_vars()) {
	coords[0] = 1;
	assignment_begin = 1;
      }
      for(size_t i = assignment_begin; i < table_dim; ++i) {
	coords[i] = 1;
	size_t check = entries.erase(BinToDec(coords));
	assert(check == 1);
	coords[i] = 0; // reset coords
      }

      // forbidden configurations
      for(std::set<size_t>::iterator it = entries.begin(); it != entries.end(); ++it) {
	coords = DecToBin(*it);
	// zero padding up to table dim
	if(coords.size() < table_dim ) {
	  coords.insert(coords.begin(), table_dim-coords.size(), 0);
	}
	assert( coords.size() == table_dim );
	OpengmWeightedFeature<OpengmModel::ValueType>(vi, shape.begin(), shape.end(), coords.begin(), forbidden_cost())
	  .add_to( *(m.opengm_model) );
      }

      LOG(logDEBUG) << "TrainableModelBuilder::add_incoming_factor(): leaving";
    }

    ////
    //// class ECCV12ModelBuilder
    ////
    ECCV12ModelBuilder* ECCV12ModelBuilder::clone() const {
      return new ECCV12ModelBuilder(*this);
    }

    Model* ECCV12ModelBuilder::build(const HypothesesGraph& hypotheses) const {
      using boost::shared_ptr;
      using std::map;

      LOG(logDEBUG) << "ECCV12ModelBuilder::build: entered";
      if( !has_detection_vars() ) {
	throw std::runtime_error("ECCV12ModelBuilder::build(): option without detection vars not yet implemented");
      }
//      if( !has_divisions() ) {
//	throw std::runtime_error("ECCV12ModelBuilder::build(): option without divisions not yet implemented");
//      }

      Model* model( new Model() );

      if( has_detection_vars() ) {
	add_detection_vars( hypotheses, *model );
      }
      add_assignment_vars( hypotheses, *model );

      if( has_detection_vars() ) {
	for(HypothesesGraph::NodeIt n(hypotheses); n!=lemon::INVALID; ++n) {
	  add_detection_factor( hypotheses, *model, n );
	}
      }

      for(HypothesesGraph::NodeIt n(hypotheses); n!=lemon::INVALID; ++n) {
	add_outgoing_factor( hypotheses, *model, n );
 	add_incoming_factor( hypotheses, *model, n );
      }

      return model;
    }

    void ECCV12ModelBuilder::add_detection_factor( const HypothesesGraph& hypotheses, Model& m, const HypothesesGraph::Node& n) const {
      property_map<node_traxel, HypothesesGraph::base_graph>::type& traxel_map = hypotheses.get(node_traxel());

      size_t vi[] = {m.var_of_node(n)};
      vector<size_t> coords(1,0);
      OpengmExplicitFactor<double> table( vi, vi+1 );

      coords[0] = 0;
      table.set_value( coords, non_detection()(traxel_map[n]) );

      coords[0] = 1;
      table.set_value( coords, detection()(traxel_map[n]) );

      table.add_to( *(m.opengm_model) );
    }

    inline void ECCV12ModelBuilder::add_outgoing_factor( const HypothesesGraph& hypotheses,
								   Model& m,
								   const HypothesesGraph::Node& n
								   ) const {
      using namespace std;
      property_map<node_traxel, HypothesesGraph::base_graph>::type& traxel_map = hypotheses.get(node_traxel());

      LOG(logDEBUG) << "ECCV12ModelBuilder::add_outgoing_factor(): entered";
      // collect and count outgoing arcs
      vector<HypothesesGraph::Arc> arcs;
      vector<size_t> vi; 		// opengm variable indeces
      vi.push_back(m.var_of_node(n)); // first detection node, remaining will be transition nodes
      int count = 0;
      for(HypothesesGraph::OutArcIt a(hypotheses, n); a != lemon::INVALID; ++a) {
	arcs.push_back(a);
	vi.push_back(m.var_of_arc(a));
	++count;
      }

      // construct factor
      if(count == 0) {
	// build value table
	size_t table_dim = 1; 		// only one detection var
	std::vector<size_t> coords;
	OpengmExplicitFactor<double> table( vi );

	// opportunity
	coords = std::vector<size_t>(table_dim, 0); 		// (0)
	table.set_value( coords, opportunity_cost() );

	// disappearance
	coords = std::vector<size_t>(table_dim, 0);
	coords[0] = 1; 						// (1)
	table.set_value( coords, disappearance()(traxel_map[n]) );

	table.add_to( *m.opengm_model );

      } else if(count == 1) {
	// no division possible
	size_t table_dim = 2; 		// detection var + 1 * transition var
	std::vector<size_t> coords;
	OpengmExplicitFactor<double> table( vi, forbidden_cost() );

	// opportunity configuration
	coords = std::vector<size_t>(table_dim, 0); // (0,0)
	table.set_value( coords, opportunity_cost() );

	// disappearance configuration
	coords = std::vector<size_t>(table_dim, 0);
	coords[0] = 1; // (1,0)
	table.set_value( coords, disappearance()(traxel_map[n]) );

	// move configurations
	coords = std::vector<size_t>(table_dim, 1);
	// (1,1)
	table.set_value( coords, move()(traxel_map[n], traxel_map[hypotheses.target(arcs[0])]) );

	table.add_to( *m.opengm_model );

      } else {
	// build value table
	size_t table_dim = count + 1; 		// detection var + n * transition var
	std::vector<size_t> coords;
	OpengmExplicitFactor<double> table( vi, forbidden_cost() );

	// opportunity configuration
	coords = std::vector<size_t>(table_dim, 0); // (0,0,...,0)
	table.set_value( coords, opportunity_cost() );

	// disappearance configuration
	coords = std::vector<size_t>(table_dim, 0);
	coords[0] = 1; // (1,0,...,0)
	table.set_value( coords, disappearance()(traxel_map[n]) );

	// move configurations
	coords = std::vector<size_t>(table_dim, 0);
	coords[0] = 1;
	// (1,0,0,0,1,0,0)
	for(size_t i = 1; i < table_dim; ++i) {
	  coords[i] = 1;
	  table.set_value( coords, move()(traxel_map[n], traxel_map[hypotheses.target(arcs[i-1])]) );
	  coords[i] = 0; // reset coords
	}

	// division configurations
	if (has_divisions()) {
		coords = std::vector<size_t>(table_dim, 0);
		coords[0] = 1;
		// (1,0,0,1,0,1,0,0)
		for(unsigned int i = 1; i < table_dim - 1; ++i) {
		  for(unsigned int j = i+1; j < table_dim; ++j) {
			coords[i] = 1;
			coords[j] = 1;
			table.set_value(coords, division()(traxel_map[n],
							  traxel_map[hypotheses.target(arcs[i-1])],
							  traxel_map[hypotheses.target(arcs[j-1])]
							  ));

			// reset
			coords[i] = 0;
			coords[j] = 0;
		  }
		}
	}

	table.add_to( *m.opengm_model );

      }
      LOG(logDEBUG) << "ChaingraphECCV12ModelBuilder::add_outgoing_factor(): leaving";
    }

    inline void ECCV12ModelBuilder::add_incoming_factor( const HypothesesGraph& hypotheses,
								   Model& m,
								   const HypothesesGraph::Node& n) const {
      using namespace std;
      property_map<node_traxel, HypothesesGraph::base_graph>::type& traxel_map = hypotheses.get(node_traxel());

      LOG(logDEBUG) << "ECCV12ModelBuilder::add_incoming_factor(): entered";
      // collect and count incoming arcs
      vector<size_t> vi; // opengm variable indeces
      int count = 0;
      for(HypothesesGraph::InArcIt a(hypotheses, n); a != lemon::INVALID; ++a) {
	vi.push_back(m.var_of_arc(a));
	++count;
      }
      vi.push_back(m.var_of_node(n));
      std::reverse(vi.begin(), vi.end());

      //// construct factor
      // build value table
      size_t table_dim = count + 1; // detection var + n * transition var
      OpengmExplicitFactor<double> table( vi, forbidden_cost() );
      std::vector<size_t> coords;

      // allow opportunity configuration
      // (0,0,...,0)
      coords = std::vector<size_t>(table_dim, 0);
      table.set_value( coords, 0 );

      // appearance configuration
      coords = std::vector<size_t>(table_dim, 0);
      coords[0] = 1; // (1,0,...,0)
      table.set_value( coords, appearance()(traxel_map[n]) );
      assert(table.get_value( coords ) == appearance()(traxel_map[n]));

      // allow move configurations
      coords = std::vector<size_t>(table_dim, 0);
      coords[0] = 1;
      // (1,0,0,0,1,0,0)
      for(size_t i = 1; i < table_dim; ++i) {
	coords[i] = 1;
	table.set_value( coords, 0 );
	coords[i] = 0; // reset coords
      }

      table.add_to( *m.opengm_model );
      LOG(logDEBUG) << "ECCV12ModelBuilder::add_incoming_factor(): leaving";
    }

    } /* namespace chaingraph */
  } /* namespace pgm */
} /* namespace pgmlink */


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

#include <pcl/gpu/kinfu_large_scale/tsdf_volume.h>
#include "internal.h"
#include <algorithm>
#include <Eigen/Core>

#include <iostream>

using namespace pcl;
using namespace pcl::gpu;
using namespace Eigen;
using pcl::device::kinfuLS::device_cast;

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

pcl::gpu::kinfuLS::TsdfVolume::TsdfVolume(const Vector3i& resolution) : resolution_(resolution), volume_host_ (new std::vector<float>), weights_host_ (new std::vector<short>)
{
  int volume_x = resolution_(0);
  int volume_y = resolution_(1);
  int volume_z = resolution_(2);

  volume_.create (volume_y * volume_z, volume_x);

  const Vector3f default_volume_size = Vector3f::Constant (3.f); //meters
  const float    default_tranc_dist  = 0.03f; //meters

  setSize(default_volume_size);
  setTsdfTruncDist(default_tranc_dist);

  reset();

}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void
pcl::gpu::kinfuLS::TsdfVolume::setSize(const Vector3f& size)
{
  size_ = size;
  setTsdfTruncDist(tranc_dist_);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void
pcl::gpu::kinfuLS::TsdfVolume::setTsdfTruncDist (float distance)
{
  float cx = size_(0) / resolution_(0);
  float cy = size_(1) / resolution_(1);
  float cz = size_(2) / resolution_(2);

  tranc_dist_ = std::max (distance, 2.1f * std::max (cx, std::max (cy, cz)));
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

pcl::gpu::DeviceArray2D<int>
pcl::gpu::kinfuLS::TsdfVolume::data() const
{
  return volume_;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

const Eigen::Vector3f&
pcl::gpu::kinfuLS::TsdfVolume::getSize() const
{
    return size_;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

const Eigen::Vector3i&
pcl::gpu::kinfuLS::TsdfVolume::getResolution() const
{
  return resolution_;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

const Eigen::Vector3f
pcl::gpu::kinfuLS::TsdfVolume::getVoxelSize() const
{
  return size_.array () / resolution_.array().cast<float>();
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

float
pcl::gpu::kinfuLS::TsdfVolume::getTsdfTruncDist () const
{
  return tranc_dist_;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void
pcl::gpu::kinfuLS::TsdfVolume::reset()
{
  pcl::device::kinfuLS::initVolume(volume_);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void
pcl::gpu::kinfuLS::TsdfVolume::fetchCloudHost (PointCloud<PointXYZI>& cloud, bool connected26) const
{
  PointCloud<PointXYZ>::Ptr cloud_ptr_ = PointCloud<PointXYZ>::Ptr (new PointCloud<PointXYZ>);
  PointCloud<PointIntensity>::Ptr cloud_i_ptr_ = PointCloud<PointIntensity>::Ptr (new PointCloud<PointIntensity>);
  fetchCloudHost(*cloud_ptr_);
  pcl::concatenateFields (*cloud_ptr_, *cloud_i_ptr_, cloud);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void
pcl::gpu::kinfuLS::TsdfVolume::fetchCloudHost (PointCloud<PointType>& cloud, bool connected26) const
{
  int volume_x = resolution_(0);
  int volume_y = resolution_(1);
  int volume_z = resolution_(2);

  int cols;
  std::vector<int> volume_host;
  volume_.download (volume_host, cols);

  cloud.points.clear ();
  cloud.points.reserve (10000);

  const int DIVISOR = pcl::device::kinfuLS::DIVISOR; // SHRT_MAX;

#define FETCH(x, y, z) volume_host[(x) + (y) * volume_x + (z) * volume_y * volume_x]

  Array3f cell_size = getVoxelSize();

  for (int x = 1; x < volume_x-1; ++x)
  {
    for (int y = 1; y < volume_y-1; ++y)
    {
      for (int z = 0; z < volume_z-1; ++z)
      {
        int tmp = FETCH (x, y, z);
        int W = reinterpret_cast<short2*>(&tmp)->y;
        int F = reinterpret_cast<short2*>(&tmp)->x;

        if (W == 0 || F == DIVISOR)
          continue;

        Vector3f V = ((Array3f(x, y, z) + 0.5f) * cell_size).matrix ();

        if (connected26)
        {
          int dz = 1;
          for (int dy = -1; dy < 2; ++dy)
            for (int dx = -1; dx < 2; ++dx)
            {
              int tmp = FETCH (x+dx, y+dy, z+dz);

              int Wn = reinterpret_cast<short2*>(&tmp)->y;
              int Fn = reinterpret_cast<short2*>(&tmp)->x;
              if (Wn == 0 || Fn == DIVISOR)
                continue;

              if ((F > 0 && Fn < 0) || (F < 0 && Fn > 0))
              {
                Vector3f Vn = ((Array3f (x+dx, y+dy, z+dz) + 0.5f) * cell_size).matrix ();
                Vector3f point = (V * abs (Fn) + Vn * abs (F)) / (abs (F) + abs (Fn));

                pcl::PointXYZ xyz;
                xyz.x = point (0);
                xyz.y = point (1);
                xyz.z = point (2);

                cloud.points.push_back (xyz);
              }
            }
          dz = 0;
          for (int dy = 0; dy < 2; ++dy)
            for (int dx = -1; dx < dy * 2; ++dx)
            {
              int tmp = FETCH (x+dx, y+dy, z+dz);

              int Wn = reinterpret_cast<short2*>(&tmp)->y;
              int Fn = reinterpret_cast<short2*>(&tmp)->x;
              if (Wn == 0 || Fn == DIVISOR)
                continue;

              if ((F > 0 && Fn < 0) || (F < 0 && Fn > 0))
              {
                Vector3f Vn = ((Array3f (x+dx, y+dy, z+dz) + 0.5f) * cell_size).matrix ();
                Vector3f point = (V * abs(Fn) + Vn * abs(F))/(abs(F) + abs (Fn));

                pcl::PointXYZ xyz;
                xyz.x = point (0);
                xyz.y = point (1);
                xyz.z = point (2);

                cloud.points.push_back (xyz);
              }
            }
        }
        else /* if (connected26) */
        {
          for (int i = 0; i < 3; ++i)
          {
            int ds[] = {0, 0, 0};
            ds[i] = 1;

            int dx = ds[0];
            int dy = ds[1];
            int dz = ds[2];

            int tmp = FETCH (x+dx, y+dy, z+dz);

            int Wn = reinterpret_cast<short2*>(&tmp)->y;
            int Fn = reinterpret_cast<short2*>(&tmp)->x;
            if (Wn == 0 || Fn == DIVISOR)
              continue;

            if ((F > 0 && Fn < 0) || (F < 0 && Fn > 0))
            {
              Vector3f Vn = ((Array3f (x+dx, y+dy, z+dz) + 0.5f) * cell_size).matrix ();
              Vector3f point = (V * abs (Fn) + Vn * abs (F)) / (abs (F) + abs (Fn));

              pcl::PointXYZ xyz;
              xyz.x = point (0);
              xyz.y = point (1);
              xyz.z = point (2);

              cloud.points.push_back (xyz);
            }
          }
        } /* if (connected26) */
      }
    }
  }
#undef FETCH
  cloud.width  = (int)cloud.points.size ();
  cloud.height = 1;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
pcl::gpu::DeviceArray<pcl::gpu::kinfuLS::TsdfVolume::PointType>
pcl::gpu::kinfuLS::TsdfVolume::fetchCloud (DeviceArray<PointType>& cloud_buffer) const
{
  if (cloud_buffer.empty ())
    cloud_buffer.create (DEFAULT_CLOUD_BUFFER_SIZE);

  float3 device_volume_size = device_cast<const float3> (size_);
  size_t size = pcl::device::kinfuLS::extractCloud (volume_, device_volume_size, cloud_buffer);
  return (DeviceArray<PointType> (cloud_buffer.ptr (), size));
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void
pcl::gpu::kinfuLS::TsdfVolume::fetchNormals (const DeviceArray<PointType>& cloud, DeviceArray<PointType>& normals) const
{
  normals.create (cloud.size ());
  const float3 device_volume_size = device_cast<const float3> (size_);
  pcl::device::kinfuLS::extractNormals (volume_, device_volume_size, cloud, (pcl::device::kinfuLS::PointType*)normals.ptr ());
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void
pcl::gpu::kinfuLS::TsdfVolume::pushSlice (PointCloud<PointXYZI>::Ptr existing_data_cloud, const pcl::gpu::kinfuLS::tsdf_buffer* buffer) const
{
  size_t gpu_array_size = existing_data_cloud->points.size ();

  if(gpu_array_size == 0)
  {
    //std::cout << "[KinfuTracker](pushSlice) Existing data cloud has no points\n";//CREATE AS PCL MESSAGE
    return;
  }

  const pcl::PointXYZI *first_point_ptr = &(existing_data_cloud->points[0]);

  pcl::gpu::DeviceArray<pcl::PointXYZI> cloud_gpu;
  cloud_gpu.upload (first_point_ptr, gpu_array_size);

  DeviceArray<float4>& cloud_cast = (DeviceArray<float4>&) cloud_gpu;
  //volume().pushCloudAsSlice (cloud_cast, &buffer_);
  pcl::device::kinfuLS::pushCloudAsSliceGPU (volume_, cloud_cast, buffer);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

size_t
pcl::gpu::kinfuLS::TsdfVolume::fetchSliceAsCloud (DeviceArray<PointType>& cloud_buffer_xyz, DeviceArray<float>& cloud_buffer_intensity, const pcl::gpu::kinfuLS::tsdf_buffer* buffer, int shiftX, int shiftY, int shiftZ ) const
{
  if (cloud_buffer_xyz.empty ())
    cloud_buffer_xyz.create (DEFAULT_CLOUD_BUFFER_SIZE/2);

  if (cloud_buffer_intensity.empty ()) {
    cloud_buffer_intensity.create (DEFAULT_CLOUD_BUFFER_SIZE/2);
  }

  float3 device_volume_size = device_cast<const float3> (size_);

  size_t size = pcl::device::kinfuLS::extractSliceAsCloud (volume_, device_volume_size, buffer, shiftX, shiftY, shiftZ, cloud_buffer_xyz, cloud_buffer_intensity);

  std::cout << " SIZE IS " << size << std::endl;

  return (size);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void
pcl::gpu::kinfuLS::TsdfVolume::fetchNormals (const DeviceArray<PointType>& cloud, DeviceArray<NormalType>& normals) const
{
  normals.create (cloud.size ());
  const float3 device_volume_size = device_cast<const float3> (size_);
  pcl::device::kinfuLS::extractNormals (volume_, device_volume_size, cloud, (pcl::device::kinfuLS::float8*)normals.ptr ());
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void
pcl::gpu::kinfuLS::TsdfVolume::convertToTsdfCloud ( pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud,
                                                    const unsigned step) const
{
  int sx = header_.resolution(0);
  int sy = header_.resolution(1);
  int sz = header_.resolution(2);

  const int cloud_size = static_cast<int> (header_.getVolumeSize() / (step*step*step));

  cloud->clear();
  cloud->reserve (std::min (cloud_size/10, 500000));

  int volume_idx = 0, cloud_idx = 0;
  // #pragma omp parallel for // if used, increment over idx not possible! use index calculation
  for (int z = 0; z < sz; z+=step)
    for (int y = 0; y < sy; y+=step)
      for (int x = 0; x < sx; x+=step, ++cloud_idx)
      {
        volume_idx = sx*sy*z + sx*y + x;
        // pcl::PointXYZI &point = cloud->points[cloud_idx];

        if (weights_host_->at(volume_idx) == 0 || volume_host_->at(volume_idx) > 0.98 )
          continue;

        pcl::PointXYZI point;
        point.x = x; point.y = y; point.z = z;//*64;
        point.intensity = volume_host_->at(volume_idx);
        cloud->push_back (point);
      }
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void
pcl::gpu::kinfuLS::TsdfVolume::downloadTsdf (std::vector<float>& tsdf) const
{
  tsdf.resize (volume_.cols() * volume_.rows());
  volume_.download(&tsdf[0], volume_.cols() * sizeof(int));

#pragma omp parallel for
  for(int i = 0; i < (int) tsdf.size(); ++i)
  {
    float tmp = reinterpret_cast<short2*>(&tsdf[i])->x;
    tsdf[i] = tmp/pcl::device::kinfuLS::DIVISOR;
  }
}

void
pcl::gpu::kinfuLS::TsdfVolume::downloadTsdfLocal () const
{
  pcl::gpu::kinfuLS::TsdfVolume::downloadTsdf (*volume_host_);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void
pcl::gpu::kinfuLS::TsdfVolume::downloadTsdfAndWeights (std::vector<float>& tsdf, std::vector<short>& weights) const
{
  int volumeSize = volume_.cols() * volume_.rows();
  tsdf.resize (volumeSize);
  weights.resize (volumeSize);
  volume_.download(&tsdf[0], volume_.cols() * sizeof(int));

  #pragma omp parallel for
  for(int i = 0; i < (int) tsdf.size(); ++i)
  {
    short2 elem = *reinterpret_cast<short2*>(&tsdf[i]);
    tsdf[i] = (float)(elem.x)/pcl::device::kinfuLS::DIVISOR;
    weights[i] = (short)(elem.y);
  }
}

void
pcl::gpu::kinfuLS::TsdfVolume::downloadTsdfAndWeightsLocal () const
{
  pcl::gpu::kinfuLS::TsdfVolume::downloadTsdfAndWeights (*volume_host_, *weights_host_);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////

bool
pcl::gpu::kinfuLS::TsdfVolume::save (const std::string &filename, bool binary) const
{
  pcl::console::print_info ("Saving TSDF volume to "); pcl::console::print_value ("%s ... ", filename.c_str());
  std::cout << std::flush;

  std::ofstream file (filename.c_str(), binary ? std::ios_base::binary : std::ios_base::out);

  if (file.is_open())
  {
    if (binary)
    {
      // HEADER
      // write resolution and size of volume
      file.write ((char*) &header_, sizeof (Header));
      /* file.write ((char*) &header_.resolution, sizeof(Eigen::Vector3i));
      file.write ((char*) &header_.volume_size, sizeof(Eigen::Vector3f));
      // write  element size
      int volume_element_size = sizeof(VolumeT);
      file.write ((char*) &volume_element_size, sizeof(int));
      int weights_element_size = sizeof(WeightT);
      file.write ((char*) &weights_element_size, sizeof(int)); */

      // DATA
      // write data
      file.write ((char*) &(volume_host_->at(0)), volume_host_->size()*sizeof(float));
      file.write ((char*) &(weights_host_->at(0)), weights_host_->size()*sizeof(short));
    }
    else
    {
      // write resolution and size of volume and element size
      file << header_.resolution(0) << " " << header_.resolution(1) << " " << header_.resolution(2) << std::endl;
      file << header_.volume_size(0) << " " << header_.volume_size(1) << " " << header_.volume_size(2) << std::endl;
      file << sizeof (float) << " " << sizeof(short) << std::endl;

      // write data
      for (std::vector<float>::const_iterator iter = volume_host_->begin(); iter != volume_host_->end(); ++iter)
        file << *iter << std::endl;
    }

    file.close();
  }
  else
  {
    pcl::console::print_error ("[saveTsdfVolume] Error: Couldn't open file %s.\n", filename.c_str());
    return false;
  }

  pcl::console::print_info ("done [%d voxels]\n", this->size ());

  return true;
}

bool
pcl::gpu::kinfuLS::TsdfVolume::load (const std::string &filename, bool binary)
{
  pcl::console::print_info ("Loading TSDF volume from "); pcl::console::print_value ("%s ... ", filename.c_str());
  std::cout << std::flush;

  std::ifstream file (filename.c_str());

  if (file.is_open())
  {
    if (binary)
    {
      // read HEADER
      file.read ((char*) &header_, sizeof (Header));
      /* file.read (&header_.resolution, sizeof(Eigen::Array3i));
      file.read (&header_.volume_size, sizeof(Eigen::Vector3f));
      file.read (&header_.volume_element_size, sizeof(int));
      file.read (&header_.weights_element_size, sizeof(int)); */

      // check if element size fits to data
      if (header_.volume_element_size != sizeof(float))
      {
        pcl::console::print_error ("[TSDFVolume::load] Error: Given volume element size (%d) doesn't fit data (%d)", sizeof(float), header_.volume_element_size);
        return false;
      }
      if ( header_.weights_element_size != sizeof(short))
      {
        pcl::console::print_error ("[TSDFVolume::load] Error: Given weights element size (%d) doesn't fit data (%d)", sizeof(short), header_.weights_element_size);
        return false;
      }

      // read DATA
      int num_elements = header_.getVolumeSize();
      volume_host_->resize (num_elements);
      weights_host_->resize (num_elements);
      file.read ((char*) &(*volume_host_)[0], num_elements * sizeof(float));
      file.read ((char*) &(*weights_host_)[0], num_elements * sizeof(short));
    }
    else
    {
      pcl::console::print_error ("[TSDFVolume::load] Error: ASCII loading not implemented.\n");
    }

    file.close ();
  }
  else
  {
    pcl::console::print_error ("[TSDFVolume::load] Error: Cloudn't read file %s.\n", filename.c_str());
    return false;
  }

  const Eigen::Vector3i &res = this->gridResolution();
  pcl::console::print_info ("done [%d voxels, res %dx%dx%d]\n", this->size(), res[0], res[1], res[2]);

  return true;
}