/***************************************************************************************************************************
 * collision_avoidance.cpp
 *
 * Author: Qyp
 *
 * Update Time: 2019.4.17
 *
 * 说明: 避障程序
 *
 *
 * 初版，需要遵循固定场景，具体场景设置请参看教程文件
 * 具体功能待完善
***************************************************************************************************************************/
//ROS 头文件
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <command_to_mavros.h>
//topic 头文件
#include <iostream>
#include <px4_command/command.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/Pose.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/PoseStamped.h>

/*
 * 主要功能:
 * 1.获取激光雷达数据
 * 2.根据距离判断是否启用避障策略
 * 3.如启用避障策略,产生控制指令
 *
 */

using namespace std;

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
enum Command
{
    Idle,
    Takeoff,
    Move_ENU,
    Move_Body,
    Hold,
    Land,
    Disarm,
    Failsafe_land,
};

#define RAD2DEG(x) ((x)*180./M_PI)
//--------------------------------------------输入--------------------------------------------------
// sensor_msgs::LaserScan Laser;                                   //激光雷达点云数据
// Eigen::Vector3d pos_drone;                                  //无人机当前位置
// float target_x;                                                 //期望位置_x
// float target_y;                                                 //期望位置_y

// int range_min;                                                //激光雷达探测范围 最小角度
// int range_max;                                                //激光雷达探测范围 最大角度

// //--------------------------------------------算法相关--------------------------------------------------
// float R_outside,R_inside;                                       //安全半径 [避障算法相关参数]
// float p_R;                                                      //大圈比例参数
// float p_r;                                                      //小圈比例参数

// float distance_c,angle_c;                                       //最近障碍物距离 角度
// float distance_cx,distance_cy;                                  //最近障碍物距离XY
// float vel_collision[2];                                         //躲避障碍部分速度
// float vel_collision_max;                                        //躲避障碍部分速度限幅

// float p_xy;                                                     //追踪部分位置环P
// float vel_track[2];                                             //追踪部分速度
// float vel_track_max;                                            //追踪部分速度限幅
 int flag_land;                                                  //降落标志位
// //--------------------------------------------输出--------------------------------------------------
// std_msgs::Bool flag_collision_avoidance;                       //是否进入避障模式标志位
// float vel_sp_body[2];                                           //总速度
// float vel_sp_max;                                               //总速度限幅
 px4_command::command Command_now;                               //发送给position_control.cpp的命令
// //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>声 明 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
// void cal_min_distance();
// float satfunc(float data, float Max);
// void printf();                                                                       //打印函数
// void printf_param();

//--------------------------------------------输入--------------------------------------------------
sensor_msgs::LaserScan Laser;                                   //激光雷达点云数据
Eigen::Vector3d pos_drone;                                      //无人机当前位置 (来自fcu)
float target_x;                                                 //期望位置_x
float target_y;                                                 //期望位置_y
float target_z;                                                 //期望位置_z
float target_temp_x;
float target_temp_y;
int xiangxian = 0;
//--------------------------------------------算法相关--------------------------------------------------
float p_xy;                                                     //追踪部分位置环P
float p_z;
float vel_track[2];                                             //追踪部分速度
//--------------------------------------------输出--------------------------------------------------
Eigen::Vector3d vel_sp_body;                                           //总速度
float vel_sp_max;                                               //总速度限幅
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>声 明 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
float satfunc(float data, float Max);
void printf();                                                                       //打印函数
void printf_param();                                                                 //打印各项参数以供检查
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回 调 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
float barriar_threshols = 2; //设置判断是否为障碍物距离
float pi = 3.1415926;
typedef struct
{
	int amount; 				//障碍物数量
	float distan_min;			//据某个障碍物最短距离
	float direc_start;			//障碍物起始方向
	float distan_start;			//障碍物起始距离
	float direc_stop;			//障碍物结束方向
	float distan_stop;			//障碍物结束距离
	float size; 				//障碍物大小
	float frame_x;				//障碍物相当飞机的坐标
	float frame_y;
	float world_x;				//世界坐标系ENU
	float world_y;
	float distan_min_vert_range; //某个障碍物最短距离点据最短路经的垂直距离
	float distan_min_away_target;//某个障碍物据目标点的距离
}S_BARRIER;
int barrier_num = 0;
int barrier_obj = 0;
float uav2targetDistan;
S_BARRIER s_barrier[10];

                                                             //打印各项参数以供检查
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回 调 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
//接收雷达的数据，并做相应处理,然后计算前后左右四向最小距离
// void lidar_cb(const sensor_msgs::LaserScan::ConstPtr& scan)
// {
//     Laser = *scan;

//     int count;    //count = 359 or 358
//     count = Laser.scan_time / Laser.time_increment;

//     //-179°到180°
//     //cout << "Angle_range : "<< RAD2DEG(Laser.angle_min) << " to " << RAD2DEG(Laser.angle_max) <<endl;

//     //剔除inf的情况
//     for(int i = 0; i <= count; i++)
//     {
//         //判断是否为inf
//         int a = isinf(Laser.ranges[i]);

//         //如果为inf，则赋值上一角度的值
//         if(a == 1)
//         {
//             if(i == 0)
//             {
//                 Laser.ranges[i] = Laser.ranges[count];
//             }
//             else
//             {
//                 Laser.ranges[i] = Laser.ranges[i-1];
//             }
//         }
//     }

//     //计算前后左右四向最小距离
//     cal_min_distance();

// }
void lidar_cb(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    Laser = *scan;
    int count = 359;
	float distan_min_direc = 0;
	for(int i=0; i<= barrier_num; i++)
	{
		s_barrier[i].distan_min = 0;
 		s_barrier[i].direc_start = 0;
		s_barrier[i].direc_stop = 0;
 		s_barrier[i].distan_start = 0;
		s_barrier[i].distan_stop = 0;
		s_barrier[i].frame_x = 0;
		s_barrier[i].frame_y = 0;
		s_barrier[i].world_x = 0;
		s_barrier[i].world_y = 0;
		s_barrier[i].distan_min_vert_range = 0;
		s_barrier[i].size = 0;

	}
	barrier_num = 0;
	/*检测障碍物*/
    for(int temp = 0; temp <= count; temp++)
    {
		if(isinf(Laser.ranges[temp]))
		{

		}
		else if(Laser.ranges[temp] <= barriar_threshols)
		{
				barrier_num++;
				s_barrier[barrier_num].direc_start = temp;
				s_barrier[barrier_num].distan_min = Laser.ranges[temp];
				s_barrier[barrier_num].distan_start = Laser.ranges[temp];
				distan_min_direc = temp;
			while(1)
			{
				//cout << "Laser.ranges[temp] : " << Laser.ranges[temp] << endl;
				temp++;
				if(s_barrier[barrier_num].distan_min > Laser.ranges[temp])
				{
					s_barrier[barrier_num].distan_min = Laser.ranges[temp];
					distan_min_direc = temp;
					//cout << "s_barrier[barrier_num].distan_min : " << s_barrier[barrier_num].distan_min << endl;
				}
				if(Laser.ranges[temp] > barriar_threshols || temp >= 359)
				{
					s_barrier[barrier_num].direc_stop = temp;
					s_barrier[barrier_num].distan_stop = Laser.ranges[temp-1];
					//余弦定理
					s_barrier[barrier_num].size =  sqrt(s_barrier[barrier_num].distan_start*s_barrier[barrier_num].distan_start+s_barrier[barrier_num].distan_stop*s_barrier[barrier_num].distan_stop-2*s_barrier[barrier_num].distan_stop*s_barrier[barrier_num].distan_start*cos((s_barrier[barrier_num].direc_stop- s_barrier[barrier_num].direc_start)*pi/180));
					s_barrier[barrier_num].frame_x = -s_barrier[barrier_num].distan_min * cos(distan_min_direc/180*3.1415926);
					s_barrier[barrier_num].frame_y = -s_barrier[barrier_num].distan_min * sin(distan_min_direc/180*3.1415926);
					s_barrier[barrier_num].world_x = s_barrier[barrier_num].frame_x + pos_drone[0];
					s_barrier[barrier_num].world_y = s_barrier[barrier_num].frame_y + pos_drone[1];
					//点到直线距离公式
					float k = (pos_drone[1] - target_y)/(pos_drone[0] - target_x);
					s_barrier[barrier_num].distan_min_vert_range = (s_barrier[barrier_num].world_y-k*s_barrier[barrier_num].world_x+k*target_x-target_y)/sqrt(1+k*k);
					//勾股定理
					s_barrier[barrier_num].distan_min_away_target = sqrt((s_barrier[barrier_num].world_x-target_x)*(s_barrier[barrier_num].world_x-target_x)+(s_barrier[barrier_num].world_y-target_y)*(s_barrier[barrier_num].world_y-target_y));
					break;
				}
			}
		}
		else
		{

		}

    }
	/*避障策略*/
	if(barrier_num >0)
	{
		//cout << "barrier_num : " << barrier_num  <<endl;
		float barrier_distan_min = 1;
		int barrier_temp = 0;
		for(int temp=1;temp<=barrier_num;temp++)
		{
			//cout << "barrier_num : " << barrier_num  <<endl;
			uav2targetDistan = sqrt((target_x - pos_drone[0])*(target_x - pos_drone[0]) + (target_y - pos_drone[1])*(target_y - pos_drone[1]));//飞机与目标点之间的距离
			if(s_barrier[temp].distan_min_vert_range < 1 && s_barrier[temp].distan_min_vert_range > -1 && s_barrier[temp].distan_min_away_target < uav2targetDistan)//找到满足条件的障碍物点
			{
				// cout << "temp : " << temp <<endl;
				if(barrier_distan_min > s_barrier[temp].distan_min)//寻找离飞机最近的障碍物，前提要障碍物据飞机与目标点直线距离不小于1m,而且障碍物在飞机的前方。
				{
					barrier_distan_min = s_barrier[temp].distan_min;
					barrier_temp = temp;
   					cout << "barrier_temp : " << barrier_temp <<endl;
				}
				if (xiangxian == 1 || xiangxian == 2 || xiangxian == 3 || xiangxian == 4)
				{
					if (s_barrier[temp].distan_min_vert_range > -0.2 && s_barrier[temp].distan_min_vert_range < 0.2 )
				    {
				 	   s_barrier[temp].distan_min_vert_range = abs(s_barrier[temp].distan_min_vert_range);
				    }
				}

			}

		}
		if(barrier_temp > 0 && xiangxian == 1)
		{
 			if ((s_barrier[barrier_temp].distan_min_vert_range > 0.2)&&(s_barrier[barrier_temp].distan_min_vert_range < 0.8))		//正负代表障碍物在uav到目标点的直线的哪一侧
			{
				target_temp_x =  s_barrier[barrier_temp].world_x+1/s_barrier[barrier_temp].distan_min;
				target_temp_y =  s_barrier[barrier_temp].world_y-1/s_barrier[barrier_temp].distan_min;
				vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
				vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
				cout <<"11111111111111111111111111111111" <<endl;
			}
			else if ((s_barrier[barrier_temp].distan_min_vert_range < -0.2)&&(s_barrier[barrier_temp].distan_min_vert_range > -0.8))
			{
				target_temp_x = s_barrier[barrier_temp].world_x-1/s_barrier[barrier_temp].distan_min;
				target_temp_y = s_barrier[barrier_temp].world_y+1/s_barrier[barrier_temp].distan_min;
				vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
				vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
				cout <<"2222222222222222222222222222222222" <<endl;
			}
			else if((s_barrier[barrier_temp].distan_min_vert_range < 0.2)&&((s_barrier[barrier_temp].distan_min_vert_range > 0)))
			{
				target_temp_x =  s_barrier[barrier_temp].world_x+1/s_barrier[barrier_temp].distan_min;
				target_temp_y =  s_barrier[barrier_temp].world_y-1/s_barrier[barrier_temp].distan_min;
				vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
				vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
				cout <<"333333333333333333333333333333" <<endl;
			}
			else if((s_barrier[barrier_temp].distan_min_vert_range > -0.2)&&((s_barrier[barrier_temp].distan_min_vert_range < 0)))
			{
				target_temp_x = s_barrier[barrier_temp].world_x-1/s_barrier[barrier_temp].distan_min;
				target_temp_y = s_barrier[barrier_temp].world_y+1/s_barrier[barrier_temp].distan_min;
				vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
				vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
				cout <<"4444444444444444444444444444444444" <<endl;
			}

			// vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
			// vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
			// cout <<"55555555555555555555555555555555555" <<endl;
			// cout << "target_temp_x : " << target_temp_x << " m "<<endl;
			// cout << "s_barrier[barrier_temp].world_x : " << s_barrier[barrier_temp].world_x << " m "<<endl;
    		// cout << "target_temp_y : " << target_temp_y << " m "<<endl;
			// cout << "s_barrier[barrier_temp].world_y : " << s_barrier[barrier_temp].world_y << " m "<<endl;
			// cout << "s_barrier[barrier_temp].distan_min_vert_range " << s_barrier[barrier_temp].distan_min_vert_range << " m "<<endl;
		}
		else if(barrier_temp > 0 && xiangxian == 2)
		{
 			if ((s_barrier[barrier_temp].distan_min_vert_range > 0.2)&&(s_barrier[barrier_temp].distan_min_vert_range < 0.8))		//正负代表障碍物在uav到目标点的直线的哪一侧
			{
				target_temp_x =  s_barrier[barrier_temp].world_x-1/s_barrier[barrier_temp].distan_min;
				target_temp_y =  s_barrier[barrier_temp].world_y-1/s_barrier[barrier_temp].distan_min;
				vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
				vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
				cout <<"11111111111111111111111111111111" <<endl;
			}
			else if ((s_barrier[barrier_temp].distan_min_vert_range < -0.2)&&(s_barrier[barrier_temp].distan_min_vert_range > -0.8))
			{
				target_temp_x = s_barrier[barrier_temp].world_x+1/s_barrier[barrier_temp].distan_min;
				target_temp_y = s_barrier[barrier_temp].world_y+1/s_barrier[barrier_temp].distan_min;
				vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
				vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
				cout <<"2222222222222222222222222222222222" <<endl;
			}
			else if((s_barrier[barrier_temp].distan_min_vert_range < 0.2)&&((s_barrier[barrier_temp].distan_min_vert_range > 0)))
			{
				target_temp_x =  s_barrier[barrier_temp].world_x+5/s_barrier[barrier_temp].distan_min;
				target_temp_y =  s_barrier[barrier_temp].world_y+5/s_barrier[barrier_temp].distan_min;
				vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
				vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
				cout <<"333333333333333333333333333333" <<endl;
			}
			else if((s_barrier[barrier_temp].distan_min_vert_range > -0.2)&&((s_barrier[barrier_temp].distan_min_vert_range < 0)))
			{
				target_temp_x = s_barrier[barrier_temp].world_x-5/s_barrier[barrier_temp].distan_min;
				target_temp_y = s_barrier[barrier_temp].world_y-5/s_barrier[barrier_temp].distan_min;
				vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
				vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
				cout <<"4444444444444444444444444444444444" <<endl;
			}
		}
		else if(barrier_temp > 0 && xiangxian == 3)
		{
 			if ((s_barrier[barrier_temp].distan_min_vert_range > 0.2)&&(s_barrier[barrier_temp].distan_min_vert_range < 0.8))		//正负代表障碍物在uav到目标点的直线的哪一侧
			{
				target_temp_x =  s_barrier[barrier_temp].world_x+1/s_barrier[barrier_temp].distan_min;
				target_temp_y =  s_barrier[barrier_temp].world_y-1/s_barrier[barrier_temp].distan_min;
				vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
				vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
				cout <<"11111111111111111111111111111111" <<endl;
			}
			else if ((s_barrier[barrier_temp].distan_min_vert_range < -0.2)&&(s_barrier[barrier_temp].distan_min_vert_range > -0.8))
			{
				target_temp_x = s_barrier[barrier_temp].world_x-1/s_barrier[barrier_temp].distan_min;
				target_temp_y = s_barrier[barrier_temp].world_y+1/s_barrier[barrier_temp].distan_min;
				vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
				vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
				cout <<"2222222222222222222222222222222222" <<endl;
			}
			else if((s_barrier[barrier_temp].distan_min_vert_range < 0.2)&&((s_barrier[barrier_temp].distan_min_vert_range > 0)))
			{
				target_temp_x =  s_barrier[barrier_temp].world_x-5/s_barrier[barrier_temp].distan_min;
				target_temp_y =  s_barrier[barrier_temp].world_y+5/s_barrier[barrier_temp].distan_min;
				vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
				vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
				cout <<"333333333333333333333333333333" <<endl;
			}
			else if((s_barrier[barrier_temp].distan_min_vert_range > -0.2)&&((s_barrier[barrier_temp].distan_min_vert_range < 0)))
			{
				target_temp_x = s_barrier[barrier_temp].world_x+5/s_barrier[barrier_temp].distan_min;
				target_temp_y = s_barrier[barrier_temp].world_y-5/s_barrier[barrier_temp].distan_min;
				vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
				vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
				cout <<"4444444444444444444444444444444444" <<endl;
			}
		}
		else if(barrier_temp > 0 && xiangxian == 4)
		{
 			if ((s_barrier[barrier_temp].distan_min_vert_range > 0.2)&&(s_barrier[barrier_temp].distan_min_vert_range < 0.8))		//正负代表障碍物在uav到目标点的直线的哪一侧
			{
				target_temp_x =  s_barrier[barrier_temp].world_x-1/s_barrier[barrier_temp].distan_min;
				target_temp_y =  s_barrier[barrier_temp].world_y-1/s_barrier[barrier_temp].distan_min;
				vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
				vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
				cout <<"11111111111111111111111111111111" <<endl;
			}
			else if ((s_barrier[barrier_temp].distan_min_vert_range < -0.2)&&(s_barrier[barrier_temp].distan_min_vert_range > -0.8))
			{
				target_temp_x = s_barrier[barrier_temp].world_x+1/s_barrier[barrier_temp].distan_min;
				target_temp_y = s_barrier[barrier_temp].world_y+1/s_barrier[barrier_temp].distan_min;
				vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
				vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
				cout <<"2222222222222222222222222222222222" <<endl;
			}
			else if((s_barrier[barrier_temp].distan_min_vert_range < 0.2)&&((s_barrier[barrier_temp].distan_min_vert_range > 0)))
			{
				target_temp_x =  s_barrier[barrier_temp].world_x-5/s_barrier[barrier_temp].distan_min;
				target_temp_y =  s_barrier[barrier_temp].world_y-5/s_barrier[barrier_temp].distan_min;
				vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
				vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
				cout <<"333333333333333333333333333333" <<endl;
			}
			else if((s_barrier[barrier_temp].distan_min_vert_range > -0.2)&&((s_barrier[barrier_temp].distan_min_vert_range < 0)))
			{
				target_temp_x = s_barrier[barrier_temp].world_x+5/s_barrier[barrier_temp].distan_min;
				target_temp_y = s_barrier[barrier_temp].world_y+5/s_barrier[barrier_temp].distan_min;
				vel_sp_body[0] = 0.7 * (target_temp_x - pos_drone[0]);
				vel_sp_body[1] = 0.7 * (target_temp_y - pos_drone[1]);
				cout <<"4444444444444444444444444444444444" <<endl;
			}
		}

		else
		{
			vel_sp_body[0] = p_xy * (target_x - pos_drone[0]);
			vel_sp_body[1] = p_xy * (target_y - pos_drone[1]);
			cout <<"66666666666666666666666666666666666666" <<endl;
		}
	}
	else
	{
		vel_sp_body[0] = p_xy * (target_x - pos_drone[0]);
		vel_sp_body[1] = p_xy * (target_y - pos_drone[1]);
		cout <<"77777777777777777777777" <<endl;
	}

	// cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>s_barrier<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
	if(barrier_num > 0)
	{
		for(int i=1;i<=barrier_num;i++)
		{
			cout << "s_barrier_number : " << i << endl;
			// cout <<	"s_barrier_distan_min : " << s_barrier[i].distan_min << endl;
			// cout <<	"s_barrier_direc_start : " << s_barrier[i].direc_start << endl;
			// cout <<	"s_barrier_direc_stop: " << s_barrier[i].direc_stop << endl;
			// cout <<	"s_barrier_distan_stop: " << s_barrier[i].distan_stop << endl;
			// cout <<	"s_barrier_distan_start: " << s_barrier[i].distan_start << endl;
			// cout <<	"s_barrier_size: " << s_barrier[i].size << endl;
			// cout <<	"s_barrier_frame_x: " << s_barrier[i].frame_x << endl;
			// cout <<	"s_barrier_frame_y: " << s_barrier[i].frame_y << endl;
			// cout <<	"s_barrier_world_x: " << s_barrier[i].world_x << endl;
			// cout <<	"s_barrier_world_y: " << s_barrier[i].world_y << endl;
			cout <<	"s_barrier_distan_min_vert_range: " << s_barrier[i].distan_min_vert_range << endl;
			cout <<	"xiangxian?: " << xiangxian << endl;
			// cout <<	"s_barrier_distan_min_away_target: " << s_barrier[i].distan_min_away_target << endl;
		}
	}

}

void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    // Read the Drone Position from the Mavros Package [Frame: ENU]
    Eigen::Vector3d pos_drone_fcu_enu(msg->pose.position.x,msg->pose.position.y,msg->pose.position.z);

    pos_drone = pos_drone_fcu_enu;
}

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

    // 频率 [20Hz]
    ros::Rate rate(20.0);

    //【订阅】Lidar数据
    ros::Subscriber lidar_sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 1000, lidar_cb);

    //【订阅】无人机当前位置 坐标系 NED系
    ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 100, pos_cb);

    // 【发布】发送给position_control.cpp的命令
    ros::Publisher command_pub = nh.advertise<px4_command::command>("/px4/command", 10);

    //读取参数表中的参数
    nh.param<float>("target_x", target_x, 1.0);
    nh.param<float>("target_y", target_y, 1.0);
    nh.param<float>("p_xy", p_xy, 0.5);
    nh.param<float>("vel_sp_max", vel_sp_max, 0.0);
    if(target_x>0 && target_y>0)
	{
		xiangxian = 1;
	}
	else if (target_x<0 && target_y>0)
	{
		xiangxian = 2;
	}
	else if (target_x<0 && target_y<0)
	{
		xiangxian = 3;
	}
	else if (target_x>0 && target_y<0)
	{
		xiangxian = 4;
	}
	else if (target_x>0 && target_y ==0)
	{
		xiangxian = 1;
	}
	else if (target_x==0 && target_y >0)
	{
		xiangxian = 2;
	}
	else if (target_x<0 && target_y ==0)
	{
		xiangxian = 3;
	}
	else if (target_x==0 && target_y <0)
	{
		xiangxian = 4;
	}

    // nh.param<float>("R_outside", R_outside, 2);
    // nh.param<float>("R_inside", R_inside, 1);
    // nh.param<float>("vel_track_max", vel_track_max, 0.5);
    // nh.param<float>("p_R", p_R, 0.0);
    // nh.param<float>("p_r", p_r, 0.0);
    // nh.param<float>("vel_collision_max", vel_collision_max, 0.0);
    // nh.param<int>("range_min", range_min, 0.0);
    // nh.param<int>("range_max", range_max, 0.0);

    //打印现实检查参数
    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;
    }

    //初值
    // vel_track[0]= 0;
    // vel_track[1]= 0;

    // vel_collision[0]= 0;
    // vel_collision[1]= 0;

    vel_sp_body[0]= 0;
    vel_sp_body[1]= 0;

    //四向最小距离 初值
    flag_land = 0;

    //输出指令初始化
    int comid = 1;

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Main Loop<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while(ros::ok())
    {
        //回调一次 更新传感器状态
        //1. 更新雷达点云数据，存储在Laser中,并计算四向最小距离
        ros::spinOnce();

        // //2. 根据最小距离判断：是否启用避障策略
        // if (distance_c >= R_outside )
        // {
        //     flag_collision_avoidance.data = false;
        // }
        // else
        // {
        //     flag_collision_avoidance.data = true;
        // }

        // //3. 计算追踪速度
        // vel_track[0] = p_xy * (target_x - pos_drone[0]);
        // vel_track[1] = p_xy * (target_y - pos_drone[1]);

        // //速度限幅
        // for (int i = 0; i < 2; i++)
        // {
        //     vel_track[i] = satfunc(vel_track[i],vel_track_max);
        // }
        // vel_collision[0]= 0;
        // vel_collision[1]= 0;

        // //4. 避障策略
        // if(flag_collision_avoidance.data == true)
        // {
        //     distance_cx = distance_c * cos(angle_c/180*3.1415926);
        //     distance_cy = distance_c * sin(angle_c/180*3.1415926);

        //     distance_cx = - distance_cx;

        //     float F_c;

        //     F_c = 0;

        //     if(distance_c > R_outside)
        //     {
        //         //对速度不做限制
        //         vel_collision[0] = vel_collision[0] + 0;
        //         vel_collision[1] = vel_collision[1] + 0;
        //         cout << " Forward Outside "<<endl;
        //     }

        //     //小幅度抑制移动速度
        //     if(distance_c > R_inside && distance_c <= R_outside)
        //     {
        //         F_c = p_R * (R_outside - distance_c);

        //     }

        //     //大幅度抑制移动速度
        //     if(distance_c <= R_inside )
        //     {
        //         F_c = p_R * (R_outside - R_inside) + p_r * (R_inside - distance_c);
        //     }

        //     if(distance_cx > 0)
        //     {
        //         vel_collision[0] = vel_collision[0] - F_c * distance_cx /distance_c;
        //     }else{
        //         vel_collision[0] = vel_collision[0] - F_c * distance_cx /distance_c;
        //     }

        //     if(distance_cy > 0)
        //     {
        //         vel_collision[1] = vel_collision[1] - F_c * distance_cy / distance_c;
        //     }else{
        //         vel_collision[1] = vel_collision[1] - F_c * distance_cy /distance_c;
        //     }

        //     //避障速度限幅
        //     for (int i = 0; i < 2; i++)
        //     {
        //         vel_collision[i] = satfunc(vel_collision[i],vel_collision_max);
        //     }
        // }

        // vel_sp_body[0] = vel_track[0] + vel_collision[0];
        // vel_sp_body[1] = vel_track[1] + vel_collision[1];

        for (int i = 0; i < 2; i++)
        {
            vel_sp_body[i] = satfunc(vel_sp_body[i],vel_sp_max);
        }

        //5. 发布Command指令给position_controller.cpp
        Command_now.command = Move_Body;     //机体系下移动
        Command_now.comid = comid;
        comid++;
        Command_now.sub_mode = 2; // xy 速度控制模式 z 位置控制模式
        Command_now.vel_sp[0] =  vel_sp_body[0];
        Command_now.vel_sp[1] =  vel_sp_body[1];  //ENU frame
        Command_now.pos_sp[2] =  0;
        Command_now.yaw_sp = 0 ;

        float abs_distance;
        abs_distance = sqrt((pos_drone[0] - target_x) * (pos_drone[0] - target_x) + (pos_drone[1] - target_y) * (pos_drone[1] - target_y));

        if(abs_distance < 0.3 || flag_land == 1)
        {
            Command_now.command = 3;     //Land
            flag_land = 1;
        }

        command_pub.publish(Command_now);

        //打印
        printf();

        rate.sleep();

    }

    return 0;

}

// //计算前后左右四向最小距离
// void cal_min_distance()
// {

//     distance_c = Laser.ranges[range_min];
//     angle_c = 0;
//     for (int i = range_min; i <= range_max; i++)
//     {
//         if(Laser.ranges[i] < distance_c)
//         {
//             distance_c = Laser.ranges[i];
//             angle_c = i;
//         }
//     }

// }

//饱和函数
float satfunc(float data, float Max)
{
    if(abs(data)>Max)
    {
        return ( data > 0 ) ? Max : -Max;
    }
    else
    {
        return data;
    }
}

void printf()
{

//    cout <<  "uav2targetDistan :" << uav2targetDistan << "m" <<endl;
//   cout << "vel_sp_x : " << vel_sp_body[0] << " [m/s] "<<endl;
//    cout << "vel_sp_y : " << vel_sp_body[1] << " [m/s] "<<endl;

}

void printf_param()
{
    cout <<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Parameter <<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    cout << "target_x : "<< target_x << endl;
    cout << "target_y : "<< target_y << endl;
	cout << "target_z : "<< target_z << endl;

    cout << "p_xy : "<< p_xy << endl;
	cout << "p_z : "<< p_z << endl;

    cout << "vel_sp_max : "<< vel_sp_max << endl;

}


/*
This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

Copyright (c) 2019 Panda Team
*/

#include <vector>
#include <iostream>
#include <chrono>
#include "assets/test_data.cpp"
#include "../../modules/distance.hpp"
#include "../../modules/space.hpp"
//#include <boost/gil/gil_all.hpp>
//#include <boost/gil/extension/io/jpeg_io.hpp>
#include <boost/gil.hpp>
#include <boost/gil/extension/io/jpeg.hpp>
#include <boost/gil/extension/numeric/sampler.hpp>
#include <boost/gil/extension/numeric/resample.hpp>

template <typename T>
void saveToCsv(std::string filename, const std::vector<std::vector<T>> &mat, const std::vector<std::string> &features)
{
	std::ofstream outputFile;

	// create and open the .csv file
	outputFile.open(filename);

	// write the file headers
	outputFile << ",";
	for (auto i = 0; i < features.size(); ++i)
	{
		outputFile << features[i];
		outputFile << ",";
	}
	//outputFile << "date";
	outputFile << std::endl;

	// last item in the mat is date
	for (auto i = 0; i < mat.size(); ++i)
	{
		outputFile << features[i] << ",";
		//outputFile << dates[i] << ",";
		for (auto j = 0; j < mat[i].size(); j++)
		{
			outputFile << mat[i][j] << ",";
		}
		outputFile << std::endl;
	}

	// close the output file
	outputFile.close();
}

int main()
{
	/******************** examples for Earth Mover Distance **************************/
	// example for picture

	std::cout << "EMD example have started" << std::endl;
	std::cout << "" << std::endl;

	int num_images = 20;
	typedef int edm_Type;

	std::vector<std::string> names(num_images);
	std::vector < boost::gil::rgb8_image_t > jpegs(num_images);
	std::vector < std::vector < edm_Type > > grayJpegs(num_images);
	for (size_t i = 0; i < num_images; ++i)
	{
		names[i] = "sample_" + std::to_string(i);
		boost::gil::read_image("assets/sample" + std::to_string(i) + ".jpg_resized.jpg", jpegs[i], boost::gil::jpeg_tag());
		//boost::gil::jpeg_read_image("assets/sample" + std::to_string(i) + ".jpg_resized.jpg", jpegs[i]);

		//boost::gil::rgb8_image_t squareScaled(50, 50);
		//boost::gil::resize_view(const_view(jpegs[i]), view(squareScaled), boost::gil::bilinear_sampler());

		//auto gray = boost::gil::color_converted_view<boost::gil::gray8_pixel_t>(const_view(jpegs[i]));
		auto gray = boost::gil::color_converted_view<boost::gil::gray8_pixel_t>(const_view(jpegs[i]));
		//for (size_t j = 0; j < 1000; ++j)
		for (size_t j = 0; j < gray.size(); ++j)
		{
			grayJpegs[i].push_back(gray[j]);
		}
	}
	std::cout << "Read complete, got an image " << jpegs[0].width()
		<< " by " << jpegs[0].height() << " pixels\n";
	std::cout << "And grayscaled and size scaled image ready " << grayJpegs[0].size() << " items\n";

	boost::gil::rgb8_pixel_t px = *const_view(jpegs[0]).at(5, 10);
	std::cout << "The pixel at 5,10 is "
		<< (int)px[0] << ','
		<< (int)px[1] << ','
		<< (int)px[2] << '\n';

	//size_t im1_R = img1.size() / 6;
	//size_t im1_C = img1[0].size() / 6;
	size_t im1_R = jpegs[0].width();
	size_t im1_C = jpegs[0].height();

	// serialize_mat2vec
	//std::vector<edm_Type> i1;
	//std::vector<edm_Type> i2;

	std::cout << "iterate through grayscale" << std::endl;
	for (size_t i = 0; i < im1_R * im1_C; ++i)
	{
		//i1.push_back(gray_img1[i]);
		//i2.push_back(gray_img2[i]);
	}

	//typedef int edm_Type;

	//size_t im1_R = img1.size() / 6;
	//size_t im1_C = img1[0].size() / 6;
	//std::cout << "Read complete, got an image " << img1.size()
	//	<< " by " << img1[0].size() << " pixels\n";

	//// serialize_mat2vec
	//std::vector<edm_Type> i1;
	//std::vector<edm_Type> i2;

	//for (size_t i = 0; i < im1_R; ++i)
	//{
	//	for (size_t j = 0; j < im1_C; ++j)
	//	{
	//		i1.push_back(img1[i][j]);
	//		i2.push_back(img2[i][j]);
	//	}
	//}

	std::vector < std::vector < edm_Type > > distance_matrix(num_images, std::vector < edm_Type >(num_images));

	std::cout << "create distance matrix" << std::endl;
	auto cost_mat = metric::EMD_details::ground_distance_matrix_of_2dgrid<edm_Type>(im1_C, im1_R);
	std::cout << "create max distance matrix" << std::endl;
	auto maxCost = metric::EMD_details::max_in_distance_matrix(cost_mat);

	std::cout << "we have started" << std::endl;
	std::cout << "" << std::endl;

	metric::EMD<edm_Type> distance(cost_mat, maxCost);

	for (size_t i = 0; i < num_images; ++i)
	{
		for (size_t j = i + 1; j < num_images; ++j)
		{
			// assumes that i1 and i2 are serialized vectors of the image matrices, and cost_mat contains a distance matrix that takes into account the original pixel locations.
			auto t1 = std::chrono::steady_clock::now();
			auto result1 = distance(grayJpegs[i], grayJpegs[j]);
			auto t2 = std::chrono::steady_clock::now();
			std::cout << "result for " << i << " <-> " << j << " is:" << result1 << " (Time = " << double(std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1).count()) / 1000000 << " s)" << std::endl;
			std::cout << "" << std::endl;

			distance_matrix[i][j] = result1;
			distance_matrix[j][i] = result1;
		}
	}

	saveToCsv("distances.csv", distance_matrix, names);

	/*** initialize the tree ***/
	std::cout << "tree for 5 pictures been creating" << std::endl;
	std::vector<std::vector<edm_Type>> grayJpegs5(grayJpegs.begin(), grayJpegs.begin() + 5);
	metric::Tree<std::vector < edm_Type >, metric::Euclidian<double>> cTree5(grayJpegs5);
	std::cout << "tree for 5 pictures has created" << std::endl;
	cTree5.print();

	std::vector<std::vector<edm_Type>> grayJpegs10(grayJpegs.begin(), grayJpegs.begin() + 10);
	metric::Tree<std::vector < edm_Type >, metric::Euclidian<double>> cTree10(grayJpegs10);
	std::cout << "tree for 10 pictures has created" << std::endl;
	cTree10.print();

	metric::Tree<std::vector < edm_Type >, metric::Euclidian<double>> cTree20(grayJpegs);
	std::cout << "tree for 20 pictures has created" << std::endl;
	cTree20.print();

	return 0;
}


#ifndef _RYSQ_CORE_HPP_
#define _RYSQ_CORE_HPP_

#include <vector>
#include <utility>
#include <boost/array.hpp>
#include <boost/utility.hpp>
#include <iostream>
#include <stdio.h>

#include "rysq/config.hpp"

static const int RYSQ_NORMALIZE = 1;

namespace rysq {

    static const double SQRT_4PI5 = 34.986836655249725;

    enum type { SP = -1, S, P, D, F, G, H, I };

    static const type types[] = { SP, S, P, D, F };

    extern const int LX[];
    extern const int LY[];
    extern const int LZ[];
    extern const double NORMALIZE[];

    struct shell {
	struct const_iterator {
	    typedef std::forward_iterator_tag iterator_category;
	    typedef int value_type;
	    typedef int reference;
	    typedef int difference_type;
	    typedef const int* pointer;
	    explicit const_iterator(int value = 0) : value(value) {}
	    const const_iterator& operator++() const { ++ value; return *this ; }
	    reference operator[](size_t i) const { return value + i; }
	    reference operator*() const { return value; }
	    operator int() const { return value ; }
	    // operator size_t() const { return size_t(value); }
	    mutable int value;
	};
	const rysq::type type;
	const int L;
	const int index;
	const int size;
	const int nc;
	const int K;
	// shell() : type(rysq::SP), L(0), index(0), size(0), nc(0), K(0) {}
	shell(int type, int K = 0)
	    : type(rysq:: type(type)),
	      L(std::abs(type)),
	      index(begin(type)),
	      size(end(type) - index),
	      nc(1 + (type < 0)*L),
	      K(K)
	{}
	operator rysq::type() const { return type; }
	bool is_hybrid() const { return nc > 1; }
	const_iterator begin() const { return const_iterator(index); }
	const_iterator end() const { return const_iterator(index + size); }
    private:
	static int begin(int a) { return (a >= 0)*((a*(a+1)*(a+2))/6); }
	static int end(int a) { return begin(std::abs(a)+1); }
    };

    namespace detail {

	template<class F, typename Quartet>
	typename F::value_type add(const Quartet &q) {
	    return (F::value(q[0])+F::value(q[1])+F::value(q[2])+F::value(q[3]));
	}

	template<class F, typename Quartet>
	typename F::value_type multiply(const Quartet &q) {
	    return (F::value(q[0])*F::value(q[1])*F::value(q[2])*F::value(q[3]));
	}

	struct L {
	    typedef size_t value_type;
	    static size_t value(const rysq::type &type) { return shell(type).L; }
	    static size_t value(const rysq::shell &shell) { return shell.L; }
	    template<typename Quartet>
	    static value_type add(const Quartet &q) {
		return detail::add<L>(q);
	    }
	};

	struct size {
	    typedef size_t value_type;
	    static size_t value(const rysq::type &type) { return shell(type).size; }
	    static size_t value(const rysq::shell &shell) { return shell.size; }
	    template<typename Quartet>
	    static value_type multiply(const Quartet &q) {
		return detail::multiply<size>(q);
	    }
	};

	struct K {
	    typedef size_t value_type;
	    static size_t value(const rysq::shell &shell) { return shell.K; }
	    template<typename Quartet>
	    static value_type multiply(const Quartet &q) {
		return detail::multiply<K>(q);
	    }
	};

    }

    class Shell : public shell, boost::noncopyable {
    public:
	Shell(rysq::type type, int K, const double *a, const double* C[])
	    : shell(type, K) { initialize(a, C); }
	~Shell() { if (this->data_) delete[] this->data_; }
	double& operator()(size_t i) const { return a_[i]; }
	double& operator()(size_t i, size_t k) const { return C_[k][i]; }
	const char* symbol() const {return symbol_; }
    private:
	char symbol_[4];
	double *a_, *C_[2];
	double *data_;
	void initialize(const double *a, const double* C[]);
    };
    inline std::ostream& operator<<(std::ostream &os, const Shell &shell) {
	return os <<  "{ "  << shell.L << " }";
    }

    template<typename C, typename T>
    boost::array<C,4> make_array(const T &t0, const T &t1, const T &t2, const T &t3) {
	boost::array<C,4> q = {{ C(t0), C(t1), C(t2), C(t3) }};
	return q;
    }

    struct State {
	State(const Shell &A, const Shell &B) : A(A), B(B) {}
	size_t K() const { return A.K*B.K; }
	size_t L() const { return A.L + B.L; }
	size_t size() const {return A.size*B.size; }
	size_t hybrid() const { return  A.is_hybrid() + B.is_hybrid(); }
	const Shell& operator[](bool i) const { return ((i == 0) ? A : B); }
    private:
	const Shell &A, &B;
    };

    template<class  C>
    struct quartet_base : boost::array<C,4> {
	typedef boost::array<C,4> base;
	typedef C c_type[4];
	quartet_base() {}
	template<typename T>
	quartet_base(const T &t0, const T &t1, const T &t2, const T &t3)
	    : base(make(t0, t1, t2, t3)) {}
	template<typename T>
	quartet_base(const boost::array<T,4> &q)
	    : base(make(q[0], q[1], q[2], q[3])) {}
	operator const c_type&() const { return this->elems; }
    protected:
	template<typename T>
	static base make(const T &t0, const T &t1, const T &t2, const T &t3) {
	    boost::array<C,4> array = {{ t0, t1, t2, t3 }};
	    return array;
	}
	size_t size() const;// { return base::size(); }
    };

    template<class C>
    struct Quartet : quartet_base<C> {
	typedef quartet_base<C> base;
	Quartet() {}
	template<typename T>
	Quartet(const T &t0, const T &t1, const T &t2, const T &t3)
	    : base(t0, t1, t2, t3) {}
	template<typename T>
	Quartet(const T (&q)[4]) : base(q[0], q[1], q[2], q[3]) {}
	template<typename T>
   	Quartet(const boost::array<T,4> &q) : base(q[0], q[1], q[2], q[3]) {}
    };

    template<>
    struct Quartet<rysq::type> : quartet_base<rysq::type> {
	typedef quartet_base<rysq::type> base;
	Quartet(const type &a, const type &b, const type &c, const type &d)
	    : base(a,b,c,d) {}
	template<typename T>
   	Quartet(const boost::array<T,4> &quartet) : base(quartet) {}
	size_t L() const { return detail::L::add(*this); }
	size_t size() const { return detail::size::multiply(*this); }
    };

    template<>
    struct Quartet<rysq::shell> : quartet_base<rysq::shell> {
	typedef quartet_base<rysq::shell> base;
	Quartet(const shell &a, const shell &b, const shell &c, const shell &d)
	    : base(a,b,c,d) {}
	template<typename T>
   	Quartet(const boost::array<T,4> &quartet) : base(quartet) {}
	size_t L() const { return detail::L::add(*this); }
	size_t size() const { return detail::size::multiply(*this); }
	size_t K() const { return detail::K::multiply(*this); }
    };

    template<>
    class Quartet<Shell> {
    public:
	typedef State Bra, Ket;

	static int size(int a, int b, int c, int d) {
	    return (shell(a).size*shell(b).size*shell(c).size*shell(d).size);
	}

	static size_t size(const rysq::type (&quartet)[4]) {
	    return size(quartet[0], quartet[1], quartet[2], quartet[3]);
	}

#ifdef __CUDACC__
	__host__ __device__
#endif
	static int symmetry(int i, int j, int k, int l) {
	    return 1 << ((k == l) + (i == j) + (i == k && j == l));
	}

	Quartet(Shell &S0, Shell &S1, Shell &S2, Shell &S3);
	~Quartet() {}
	int L() const { return L_; }
	int size() const { return size_; }
	int size(int i, int j) const { return (*this)[i].size*(*this)[j].size; }
	int hybrid() const { return hybrid_; }
	int nc() const { return nc_; }
	int K() const { return K_; }
	const Shell& operator[](size_t i) const { return *shells_[i]; }
	const Bra bra() const { return Bra((*this)[0], (*this)[1]); }
	const Ket ket() const { return Ket((*this)[2], (*this)[3]); }
	operator Quartet<rysq::type>() const { return cast<rysq::type>(); }
	operator Quartet<rysq::shell>() const  { return cast<rysq::shell>(); }
	boost::array<Shell*,4> data() const { return shells_; }
    private:
	boost::array<Shell*,4> shells_;
	int L_, size_, hybrid_, nc_, K_;
	template<typename T>
	Quartet<T> cast() const {
	    const Quartet &q = *this;
	    return Quartet<T>(T(q[0]), T(q[1]), T(q[2]), T(q[3]));
	}
    };

    template<typename T>
    std::ostream& operator<<(std::ostream &os, const Quartet<T> &quartet) {
	return (os << "{ " << quartet[0] << quartet[1] <<
		quartet[2] << quartet[3] << "}");
    }

    struct Transpose {
	enum { BRA = 1, KET = 2, BRAKET = 4 };
	bool bra, ket, braket;
	size_t value, index[4];
	Transpose() : bra(0), ket(0), braket(0), value(0) { initialize(); }
	Transpose(bool bra, bool ket, bool braket)
	    : bra(bra), ket(ket), braket(braket)
	{
	    this->value = ((int(bra) << 0) | (int(ket) << 1) | (int(braket) << 2));
	    using std::swap;
	    initialize();
	    if (bra) swap(index[0], index[1]);
	    if (ket) swap(index[2], index[3]);
	    if (braket) {
		swap(index[0], index[2]);
		swap(index[1], index[3]);
	    }
	}
	template<class Q> Q operator()(const Q &quartet);
    private:
	void initialize() {
	    for (int i = 0; i < 4; ++i) index[i] = i;
	}
    };

    class Parameters {
    public:
	int flags;
	double scale;
	double scale2;
	double cutoff;
	Parameters(int flags = 0, double scale = 1.0, double scale2 = 4.0,
		   double cutoff = 1.0e-10) :
	    flags(flags), scale(scale), scale2(scale2), cutoff(cutoff) {}
	static const Parameters Default;
    };

    int initialize();

    int finalize();

    typedef boost::array<double,3> Center;
    typedef boost::array<int,4> Int4;

    struct index_list {
	typedef size_t type[2];
	static const type list_[6];
	typedef type* iterator;
	typedef const type* const_iterator;
	static size_t size() { return 6; }
	static const type& at(size_t i) { return list_[i]; }
	static const_iterator begin() { return list_; }
	static const_iterator end() { return list_ + size(); }

#ifdef __CUDACC__
	__host__ __device__
#endif
	static inline size_t index1(size_t index){
	    // if ( index == 0)  return 0;
	    // if ( index == 1)  return 2;
	    // return (index > 3);
	    return (index == 1) ? 2 : (index > 3);
	}
#ifdef __CUDACC__
	__host__ __device__
#endif

	static inline size_t index2(size_t index){
	    // if ( index == 0) return 1;
	    // // if ( index == 5) return 3;
	    // return 2 + index%2;
	    return (index == 0) ? 1 : (2 + index%2);
	}
#ifdef __CUDACC__
	__host__ __device__
#endif

	static inline size_t index3(size_t I,size_t J) {
	    return ((I > 0) ? (0) :
		    ((J - I > 1) ? (I + 1) : (J + 1)));
	}
#ifdef __CUDACC__
	__host__ __device__
#endif
	static inline size_t index4(size_t I, size_t J) {
	    return 6 - (I +  J + index3(I, J));
	}

	template<typename T>
	static size_t find(const T (&index)[2]) {
	    return find(index[0], index[1]);
	}
	static size_t find(size_t i,size_t j) {
	    const size_t transpose = 6*(i > j);
	    size_t index;
	    if (i + j == 1) index =  0 + transpose;
	    else if (i + j == 5) index =  1 + transpose;
	    else if (transpose) index =  2*j + i + transpose;
	    else index =  2*i + j;
	    // std::cout << i << " " << j << " " << index << std::endl;
	    return index;
	}
    };

    template<typename T, class L = index_list>
    struct index_set {
	static L indices(){ return L(); }
	T& get(size_t i, size_t j) {
	    return data_[L::find(i,j)];
	}
	const T& get(size_t i, size_t j) const {
	    return data_[L::find(i,j)];
	}
	T& operator[](size_t i) { return data_[i]; }
	const T& operator[](size_t i) const { return data_[i]; }
    private:
	T data_[12];
    };

}

// #ifdef RYSQ_CUDA
// #include "rysq-cuda.hpp"
// #endif

#endif


#include <string>
#include <sstream>
#include <algorithm>
#include <iterator>
#include "mile_types.h"
#include "mile_crypto.h"
#include <boost/multiprecision/cpp_int.hpp>

using namespace std;
typedef boost::multiprecision::uint256_t uint256_t;

Seed::Seed(const std::string &phrase):Array() {
    Clear();
    sha3_256((const unsigned char*) phrase.c_str(), phrase.length(), Data.data());
}

string UInt256ToDecString(const uint256_t &ui)
{
    std::stringstream ss;
    ss << std::dec << ui;
    return ss.str();
}

string UInt256ToHexString(const uint256_t& ui)
{
    std::stringstream ss;
    ss << std::hex << ui;
    return ss.str();
}

bool StringToUInt256(const string& s, uint256_t& ui, bool hexString)
{
    std::stringstream ss;

    try
    {
        // note: boost 1.53 in CentOS doesn't support stream modifiers like std::hex for multiprecision types
        // but prefix '0x' for hex numbers supported in boost 1.53 and in boost 1.64 as well
        // ss << (hexString ? std::hex : std::dec) << s;
        // ss >> (hexString ? std::hex : std::dec) >> ui;
        ss << (hexString ? "0x" : "") << s;
        ss >> (hexString ? std::hex : std::dec) >> ui;
    }
    catch (const boost::exception& e)
    {
        return false;
    }
    // check that all symbols are processed e.g. 0xfz -> error
    if (ss.fail() || !ss.eof())
    {
        return false;
    }

    return true;
}

void UInt256ToVector(const uint256_t& ui, vector<unsigned char>& v, bool littleEndian)
{
    v.clear();
    v.reserve(sizeof(ui));

    size_t size = ui.backend().size();
    const boost::multiprecision::limb_type* p = ui.backend().limbs();
    size_t limbTypeSize = sizeof(boost::multiprecision::limb_type);

    for (size_t i = 0; i < size; ++i, ++p)
    {
        boost::multiprecision::limb_type t = *p;
        //
        for (size_t j = 0; j < limbTypeSize; j++)
        {
            v.push_back(t & 0xff);
            t >>= 8;
        }
    }

    // add padding with zeros
    v.resize(32);

    // if big endian specified
    // note: optimize later if needed!
    if (!littleEndian)
    {
        std::reverse(v.begin(), v.end());
    }
}

bool VectorToUInt256(const vector<unsigned char>& v, uint256_t& ui, bool littleEndian)
{
    if (v.size() != 32)
    {
        return false;
    }

    vector<unsigned char> inputVector(v);

    // if big endian specified
    // note: optimize later if needed!
    if (!littleEndian)
    {
        std::reverse(inputVector.begin(), inputVector.end());
    }

    ui = 0;

    // process inputVector in little endian format
    // note: optimize later
    for (size_t i = 0, offset = 0; i < inputVector.size(); i++, offset += 8)
    {
        ui |= static_cast<uint256_t>(inputVector[i]) << offset;
    }

    return true;
}

string UInt64ToDecString(const uint64_t& ui)
{
    std::stringstream ss;
    ss << std::dec << ui;
    return ss.str();
}

bool DecStringToUint(const string& decString, unsigned int& value)
{
    std::stringstream ss;

    if (decString.empty())
    {
        return false;
    }

    ss << std::dec << decString;
    ss >> value;

    if (ss.fail() || !ss.eof())
    {
        return false;
    }

    return true;
}

bool StringToUInt64(const string& s, uint64_t& ui, bool hexString)
{
    std::stringstream ss;

    try
    {
        // note: boost 1.53 in CentOS doesn't support stream modifiers like std::hex for multiprecision types
        // but prefix '0x' for hex numbers supported in boost 1.53 and in boost 1.64 as well
        // ss << (hexString ? std::hex : std::dec) << s;
        // ss >> (hexString ? std::hex : std::dec) >> ui;
        ss << (hexString ? "0x" : "") << s;
        ss >> (hexString ? std::hex : std::dec) >> ui;
    }
    catch (const std::exception& e)
    {
        return false;
    }
    // check that all symbols are processed e.g. 0xfz -> error
    if (ss.fail() || !ss.eof())
    {
        return false;
    }

    return true;
}

void UInt64ToVector(const uint64_t& ui, vector<unsigned char>& v, bool littleEndian)
{
    v.clear();
    v.resize(sizeof(uint64_t));

    if (littleEndian)
    {
        for (size_t i = 0; i < 8; i++)
        {
            v[i] = static_cast<unsigned char>((ui >> (i * 8)) & 0xff);
        }
    }
    else // big endian
    {
        for (size_t i = 0; i < 8; i++)
        {
            v[7 - i] = static_cast<unsigned char>((ui >> (i * 8)) & 0xff);
        }
    }
}

bool VectorToUInt64(const vector<unsigned char>& v, uint64_t& ui, bool littleEndian)
{
    if (v.size() != sizeof(uint64_t))
    {
        return false;
    }

    ui = 0;

    if (littleEndian)
    {
        for (size_t i = 0, offset = 0; i < v.size(); i++, offset += 8)
        {
            ui |= static_cast<uint64_t>(v[i]) << offset;
        }
    }
    else // big endian
    {
        for (size_t i = 0, offset = 0; i < v.size(); i++, offset += 8)
        {
            ui |= static_cast<uint64_t>(v[7 - i]) << offset;
        }
    }

    return true;
}


#include <UnitTest++/UnitTest++.h>
#include "../frame_registration.h"
#include "coela_core/src/pixel_array_routines.h"
#include "coela_core/src/image_utils.h"
#include <boost/filesystem.hpp>

#include <iostream>
#include <sstream>

using namespace coela;
using namespace std;
SUITE(frame_registration)
{

    string test_suite_output_dir= string(UnitTestSuite::GetSuiteName()) + "_tests/";

    TEST(Run_Standard_Suite_Setup) {
        cout << "*** \""<< UnitTestSuite::GetSuiteName() <<"\" unit tests running ***" <<endl;
        boost::filesystem::create_directories(test_suite_output_dir);
    }

    TEST(find_best_psf_match) {

        CcdImage<double> construction_image;
        construction_image.pix = PixelArray2d<double>(200,200,0.0);

        construction_image.initialize_CCD_grid_to_specific_offset_and_scale(
            CcdPosition(0,0), 0.25);

        psf_models::ReferencePsf ref_psf4x;

        bool output_to_screen=false;

        ref_psf4x.psf_image.pix =PixelArray2d<double>(40, 40, 0);
        ref_psf4x.exact_centre = PixelPosition(20.61,20.61);
        ref_psf4x.central_pixel = PixelPosition::pixel_containing_point(ref_psf4x.exact_centre);
        ref_psf4x.psf_image.initialize_CCD_grid_to_specific_offset_and_scale(CcdPosition(0,0),
                0.25);
        for (PixelIterator i(ref_psf4x.psf_image.pix.range()); i!=i.end; ++i) {
            ref_psf4x.psf_image.pix(i) =
                max(0.0,
                    10 - coord_distance(PixelPosition::centre_of_pixel(i), ref_psf4x.exact_centre)
                   );
        }

        PixelIndex output_offset(83,71);
        PixelRange output_range(ref_psf4x.psf_image.pix.range().low + output_offset,
                                ref_psf4x.psf_image.pix.range().high + output_offset);
        for (PixelIterator in(ref_psf4x.psf_image.pix.range()), out(output_range);
                in!=in.end && out!=out.end;
                ++in, ++out) {
            construction_image.pix(out)+=ref_psf4x.psf_image.pix(in);
        }

        construction_image.write_to_file(test_suite_output_dir+"construction_img.fits");
        ref_psf4x.psf_image.write_to_file(test_suite_output_dir+"ref_psf4x.fits");

        psf_models::ReferencePsf ref_psf1x = ref_psf4x;
        ref_psf1x.exact_centre.x/=4.0;  ref_psf1x.exact_centre.y/=4.0;
        ref_psf1x.central_pixel = PixelPosition::pixel_containing_point(ref_psf1x.exact_centre);
        ref_psf1x.psf_image = image_utils::bin_image(ref_psf4x.psf_image, 4);

//
        PixelPosition expected_img_centre = ((ref_psf4x.exact_centre - PixelPosition::origin)
                                             + PixelShift(output_offset.x, output_offset.y)) + PixelPosition::origin;

        CHECK_CLOSE(pixel_array_routines::centroid(construction_image.pix,
                    construction_image.pix.range()).x ,
                    expected_img_centre.x,
                    0.01);

        CcdPosition known_image_centre =
            construction_image.CCD_grid.corresponding_grid_Position(expected_img_centre);

        CcdImage<double> test_image = image_utils::bin_image(construction_image, 4);
        test_image.write_to_file(test_suite_output_dir+"binned_img.fits");

        CHECK_CLOSE(1.0, test_image.CCD_grid.pixel_width_, 1e-6);

        CcdBoxRegion search_region(5,5,45,45);

        frame_registration::GuideStarLock<CcdPosition> lock1x =
            frame_registration::find_best_psf_match(test_image,
                    search_region,
                    ref_psf1x,
                    1.0,
                    0.25
                                                   );

        CHECK_CLOSE(known_image_centre.x, lock1x.Position.x, 0.5);
        CHECK_CLOSE(known_image_centre.y, lock1x.Position.y, 0.5);

        if (output_to_screen) {
            cerr<<"1x error: " << coord_distance(known_image_centre  ,lock1x.Position)<<endl;
            cerr<<"1x error xy: " << known_image_centre  - lock1x.Position<<endl;
        }
        frame_registration::GuideStarLock<CcdPosition> lock1x_parabolic =
            frame_registration::find_best_psf_match(test_image,
                    search_region,
                    ref_psf1x,
                    1.0,
                    0.25,
                    true
                                                   );

        CHECK_CLOSE(known_image_centre.x, lock1x_parabolic.Position.x, 0.5);
        CHECK_CLOSE(known_image_centre.y, lock1x_parabolic.Position.y, 0.5);
        if (output_to_screen) {
            cerr<<"1x parabolic error: " << coord_distance(known_image_centre  ,
                    lock1x_parabolic.Position)<<endl;
            cerr<<"1x parabolic error xy: " << known_image_centre  - lock1x_parabolic.Position<<endl;
        }

        frame_registration::GuideStarLock<CcdPosition> lock4x =
            frame_registration::find_best_psf_match(test_image,
                    search_region,
                    ref_psf4x,
                    4,
                    0.25
                                                   );

        CHECK_CLOSE(known_image_centre.x, lock4x.Position.x, 0.13);
        CHECK_CLOSE(known_image_centre.y, lock4x.Position.y, 0.13);
        if (output_to_screen) {
            cerr<<"4x error: " << coord_distance(known_image_centre ,lock4x.Position)<<endl;
            cerr<<"4x error xy: " << known_image_centre - lock4x.Position<<endl;
        }
        frame_registration::GuideStarLock<CcdPosition> lock4x_parabolic =
            frame_registration::find_best_psf_match(test_image,
                    search_region,
                    ref_psf4x,
                    4,
                    0.25,
                    true
                                                   );
        if (output_to_screen) {
            cerr<<"4x parabolic error: " << coord_distance(known_image_centre ,
                    lock4x_parabolic.Position)<<endl;
            cerr<<"4x paraboloic error xy: " << known_image_centre - lock4x_parabolic.Position<<endl;
        }
//        CHECK_CLOSE(known_image_centre.x, lock4x_parabolic.Position.x, 0.05);
//        CHECK_CLOSE(known_image_centre.y, lock4x_parabolic.Position.y, 0.05);

    }

}


#include <boost/algorithm/string.hpp>
#include <fmt/core.h>
#include <string>
#include <vector>

#include "dat_file_writer.h"
#include "h5_file.h"

using namespace std;

/*
  Opens a dat file for writing, to log hits that we find.
  Some metadata for headers is copied out from the h5 file.
 */
DatFileWriter::DatFileWriter(const string& filename, const FilterbankFile& metadata,
                             double max_drift) : metadata(metadata) {
  hit_count = 0;
  file.open(filename);

  vector<string> parts;
  boost::split(parts, filename, boost::is_any_of("/"));
  file << "# -------------------------- o --------------------------\n";
  file << "# File ID: " << parts[parts.size() - 1] << " \n";
  file << "# -------------------------- o --------------------------\n";
  file << "# Source:" << metadata.source_name << "\n";
  file << fmt::format("# MJD: {:18.12f}\tRA: {:.6f}\tDEC: {:.6f}\n",
                      metadata.tstart, metadata.src_raj, metadata.src_dej);
  file << fmt::format("# DELTAT: {:10.6f}\tDELTAF(Hz): {:10.6f}\t"
                      "max_drift_rate: {:10.6f}\tobs_length: {:10.6f}\n",
                      metadata.tsamp, metadata.foff * 1000000.0, max_drift,
                      metadata.num_timesteps * metadata.tsamp);
  file << "# --------------------------\n"
    "# Top_Hit_# \t"
    "Drift_Rate \t"
    "SNR \t"
    "Uncorrected_Frequency \t"
    "Corrected_Frequency \t"
    "Index \t"
    "freq_start \t"
    "freq_end \t"
    "SEFD \t"
    "SEFD_freq \t"
    "Coarse_Channel_Number \t"
    "Full_number_of_hits \t"
    "\n"
    "# --------------------------\n";
  file << flush;
}

DatFileWriter::~DatFileWriter() {
  file.close();
}

void DatFileWriter::recordHit(int coarse_channel, int freq_index, int drift_bins,
                              double drift_rate, double snr, const float* input) {
  ++hit_count;

  int global_index = coarse_channel * metadata.coarse_channel_size + freq_index;
  double frequency = metadata.fch1 + global_index * metadata.foff;

  // Currently we just output one frequency for all frequency-type columns.
  // We also just output 1 instead of counting up pre-deduping hits because I suspect
  // that nobody wants it, and it's bothersome to calculate with our current algorithm.
  // TODO: see what the astronomers actually want here
  file << fmt::format("{}\t{:10.6f}\t{:10.6f}\t{:14.6f}\t{:14.6f}\t{}\t{:14.6f}\t{:14.6f}\t",
                      hit_count, drift_rate, snr, frequency, frequency, freq_index,
                      frequency, frequency);
  file << fmt::format("0.0\t0.000000\t{}\t1\t\n", coarse_channel);
  file << flush;
}


/*
 * @Description: 前端里程计算法
 * @Author: Ren Qian
 * @Date: 2020-02-04 18:52:45
 */
#ifndef LIDAR_LOCALIZATION_MAPPING_FRONT_END_FRONT_END_HPP_
#define LIDAR_LOCALIZATION_MAPPING_FRONT_END_FRONT_END_HPP_

#include <deque>
#include <Eigen/Dense>
#include <yaml-cpp/yaml.h>

#include "lidar_localization/sensor_data/cloud_data.hpp"
#include "lidar_localization/models/registration/registration_interface.hpp"
#include "lidar_localization/models/cloud_filter/cloud_filter_interface.hpp"

namespace lidar_localization {
class FrontEnd {
  public:
    struct Frame {
        Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
        CloudData cloud_data;
    };

  public:
    FrontEnd();

    bool Update(const CloudData& cloud_data, Eigen::Matrix4f& cloud_pose);
    bool SetInitPose(const Eigen::Matrix4f& init_pose);

  private:
    bool InitWithConfig();
    bool InitParam(const YAML::Node& config_node);
    bool InitRegistration(std::shared_ptr<RegistrationInterface>& registration_ptr, const YAML::Node& config_node);
    bool InitFilter(std::string filter_user, std::shared_ptr<CloudFilterInterface>& filter_ptr, const YAML::Node& config_node);
    bool UpdateWithNewFrame(const Frame& new_key_frame);

  private:
    std::string data_path_ = "";

    // scan filter:
    std::shared_ptr<CloudFilterInterface> frame_filter_ptr_;
    // local map filter:
    std::shared_ptr<CloudFilterInterface> local_map_filter_ptr_;
    // point cloud registrator:
    std::shared_ptr<RegistrationInterface> registration_ptr_;

    std::deque<Frame> local_map_frames_;

    CloudData::CLOUD_PTR local_map_ptr_;
    Frame current_frame_;

    Eigen::Matrix4f init_pose_ = Eigen::Matrix4f::Identity();

    float key_frame_distance_ = 2.0;
    int local_frame_num_ = 20;
};
}

#endif


#include <mlpack/core.hpp>
#include <NN_basis_function.hpp>
#include <armadillo>
#include <boost/program_options.hpp>
#include <iostream>
#include <robotics.hpp>
#include <HSMM.hpp>
#include <ProMPs_emission.hpp>

using namespace arma;
using namespace hsmm;
using namespace robotics;
using namespace std;
namespace po = boost::program_options;

field<mat> fromMatToField(const mat& obs) {
    field<mat> ret(obs.n_cols);
    for(int i = 0; i < obs.n_cols; i++)
        ret(i) = obs.col(i);
    return ret;
}

// Running the Viterbi algorithm.
void ViterbiAlgorithm(HSMM& promp_hsmm, const field<field<mat>>& seq_obs,
        string filename) {
    int nseq = seq_obs.n_elem;
    for(int s = 0; s < nseq; s++) {
        const field<mat>& obs = seq_obs(s);
        int nstates = promp_hsmm.nstates_;
        int nobs = obs.n_elem;
        imat psi_duration(nstates, nobs, fill::zeros);
        imat psi_state(nstates, nobs, fill::zeros);
        mat delta(nstates, nobs, fill::zeros);
        cout << "Before pdf" << endl;
        cube log_pdf = promp_hsmm.computeEmissionsLogLikelihood(obs);
        cout << "After pdf" << endl;
        Viterbi(promp_hsmm.transition_, promp_hsmm.pi_, promp_hsmm.duration_,
                log_pdf, delta, psi_duration, psi_state,
                promp_hsmm.min_duration_, nobs);
        cout << "Delta last column" << endl;
        cout << delta.col(nobs - 1) << endl;
        ivec viterbiStates, viterbiDurations;
        viterbiPath(psi_duration, psi_state, delta, viterbiStates,
                viterbiDurations);
        cout << "Viterbi states and durations" << endl;
        imat states_and_durations = join_horiz(viterbiStates, viterbiDurations);
        cout << states_and_durations << endl;
        cout << "Python states list representation" << endl;
        cout << "[";
        for(int i = 0; i < viterbiStates.n_elem; i++)
            cout << viterbiStates[i] <<
                    ((i + 1 == viterbiStates.n_elem)? "]" : ",");
        cout << endl;
        cout << "Python duration list representation" << endl;
        cout << "[";
        for(int i = 0; i < viterbiDurations.n_elem; i++)
            cout << viterbiDurations[i] <<
                    ((i + 1 == viterbiDurations.n_elem)? "]" : ",");
        cout << endl;

        // Saving the matrix of joint states and durations.
        string viterbi_filename(filename);
        if (nseq > 1)
            viterbi_filename += string(".") + to_string(s);
        states_and_durations.save(viterbi_filename, raw_ascii);
    }
}

mat join_mats(vector<mat> v) {
    int total_cols = 0;
    int total_rows = v.at(0).n_rows;
    for(auto& m: v)
        total_cols += m.n_cols;
    mat ret(total_rows, total_cols);
    int idx = 0;
    for(auto& m: v) {
        int d = m.n_cols;
        ret.cols(idx, idx + d - 1) = m;
        idx += d;
    }
    assert(idx == total_cols);
    return ret;
}

int main(int argc, char *argv[]) {
    mlpack::Log::Info.ignoreInput = false;  // Turning mlpack verbose output on.
    po::options_description desc("Options");
    desc.add_options()
        ("help,h", "Produce help message")
        ("input,i", po::value<string>(), "Path to the input obs")
        ("output,o", po::value<string>(), "Path to the json output params")
        ("viterbilabels,l", po::value<string>(), "Path to the input viterbi "
                "files (labels). Note that the format is viterbi (not labels)")
        ("viterbi,v", po::value<string>(), "Path to the output viterbi file")
        ("nstates,s", po::value<int>(), "Number of states (NNs)")
        ("mindur", po::value<int>(), "Minimum duration of a segment")
        ("ndur", po::value<int>(), "Number of different durations supported")
        ("hiddenunits,u", po::value<int>()->default_value(10),
                "Number of hidden units per hidden layer")
        ("nlayers", po::value<int>()->default_value(1),
                "Number of hidden layers")
        ("nfiles,n", po::value<int>()->default_value(1),
                "Number of input files to process")
        ("debug", "Flag for activating debug mode in HSMM")
        ("nodur", "Flag to deactivate the learning of durations")
        ("notrans", "Flag to deactivate the learning of transitions")
        ("nopi", "Flag to deactivate the learning of initial pmf")
        ("durmomentmatching", "Flag to active the Gaussian moment matching"
                " for the duration learning")
        ("polybasisfun", po::value<int>()->default_value(0), "Order of the "
                "poly basis functions")
        ("noselftransitions", "Flag to deactive self transitions")
        ("wpriorvar", po::value<double>(), "Prior variance for Sigma_w")
        ("alphadurprior", po::value<int>()->default_value(1),
                "Alpha for Dirichlet prior for the duration")
        ("trainingiter", po::value<int>()->default_value(10), "Training "
                "iterations")
        ("delta", po::value<double>(), "If this is given a value, the model "
                "switches to segment agnostic and the samples are generated "
                "according to the provided delta")
        ("leaveoneout", po::value<int>(), "Index of the sequence that will be"
                " left out for validation")
        ("ms", po::value<string>(), "File name where a "
                "matrix containing the state marginals will be stored.")
        ("mr", po::value<string>(), "File name where a "
                "matrix containing the run length marginals will be stored.")
        ("md", po::value<string>(), "File name where a "
                "matrix containing the duration marginals will be stored.")
        ("savebasisfunparams", po::value<string>(), "File where the NN weights"
                " will be saved after training");
    vector<string> required_fields = {"input", "output", "viterbilabels",
            "nfiles", "nstates", "mindur", "ndur", "viterbi"};
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);
    if (vm.count("help")) {
        cout << desc << endl;
        return 0;
    }
    for(auto s: required_fields) {
        if (!vm.count(s)) {
            cerr << "Error: You must provide the argument: " << s << endl;
            return 1;
        }
    }
    string input_filename = vm["input"].as<string>();
    string output_filename = vm["output"].as<string>();
    string viterbi_filename = vm["viterbilabels"].as<string>();
    int nseq = vm["nfiles"].as<int>();
    int nstates = vm["nstates"].as<int>();
    int hidden_units = vm["hiddenunits"].as<int>();
    int nlayers = vm["nlayers"].as<int>();
    vector<mat> obs_for_each_state[nstates];
    vector<mat> times_for_each_state[nstates];
    field<field<mat>> seq_obs(nseq);
    field<Labels> seq_labels(nseq);
    for(int i = 0; i < nseq; i++) {
        string iname = input_filename;
        string vname = viterbi_filename;
        if (nseq != 1) {
            iname += string(".") + to_string(i);
            vname += string(".") + to_string(i);
        }
        mat obs;
        obs.load(iname, raw_ascii);
        seq_obs(i) = fromMatToField(obs);
        imat vit;
        vit.load(vname, raw_ascii);
        ivec hs = vit.col(0);
        ivec dur = vit.col(1);
        int idx = 0;
        for(int j = 0; j < dur.n_rows; j++) {
            mat segment = obs.cols(idx, idx + dur(j) - 1);
            obs_for_each_state[hs(j)].push_back(segment);
            rowvec times = linspace<rowvec>(0, 1.0, dur(j));
            times_for_each_state[hs(j)].push_back(times);
            idx += dur(j);
            seq_labels(i).setLabel(idx - 1, dur(j), hs(j));
        }
    }

    // There should be at least one segment for the hidden state 0.
    int njoints = obs_for_each_state[0].at(0).n_rows;
    vector<shared_ptr<ScalarCombBasis>> basis;
    vector<vec> means;
    vector<nlohmann::json> basis_fun_params;
    for(int i = 0; i < nstates; i++) {
        mat inputs = join_mats(times_for_each_state[i]);
        mat outputs = join_mats(obs_for_each_state[i]);
        assert (outputs.n_rows == njoints);
        assert(outputs.n_cols == inputs.n_cols);

        // Defining the architecture of the NN.
        ivec hidden_units_per_layer = ones<ivec>(nlayers) * hidden_units;
        auto nn1 = make_shared<ScalarNNBasis>(hidden_units_per_layer, njoints);

        // Training the NN.
        nn1->getNeuralNet().Train(inputs, outputs);
        auto serialized = nn1->to_stream();
        basis_fun_params.push_back(serialized);

        // Testing the NN building from serialized parameters.
        auto nn = make_shared<ScalarNNBasis>(serialized);

        // Extracting the parameters from the output layer.
        pair<mat,vec> out_params = nn->getOutputLayerParams();
        mat joint_params = join_horiz(out_params.first, out_params.second);
        vec flattened_params = conv_to<vec>::from(vectorise(joint_params, 1));
        means.push_back(flattened_params);
        cout << "Weights " << endl << out_params.first << endl;
        cout << "Bias " << endl << out_params.second << endl;

        // Adding polynomial terms to the NN basis.
        auto poly = make_shared<ScalarPolyBasis>(vm["polybasisfun"].as<int>());
        auto comb = shared_ptr<ScalarCombBasis>(new ScalarCombBasis(
                    {nn, poly}));

        // Predicting.
        vec test_input = linspace<vec>(0,1,100);
        vector<mat> test_output;
        for(int j = 0; j < test_input.n_elem; j++) {
            vec output = nn->eval(test_input(j));
            vec output2 = nn1->eval(test_input(j));
            assert(approx_equal(output, output2, "reldiff", 1e-8));
            test_output.push_back(output);
        }
        mat mat_test_output = join_mats(test_output);
        //mat_test_output.save("prediction.txt." + to_string(i) , raw_ascii);
        basis.push_back(comb);
    }
    int n_basis_functions = basis.at(0)->dim();

    if (vm.count("savebasisfunparams")) {
        nlohmann::json basisfunparams = basis_fun_params;
        std::ofstream basis_fun_file(vm["savebasisfunparams"].as<string>());
        basis_fun_file << std::setw(4) << basisfunparams << std::endl;
        basis_fun_file.close();
    }

    // Instantiating as many ProMPs as hidden states.
    vector<FullProMP> promps;
    for(int i = 0; i < nstates; i++) {
        vec mu_w = means.at(i);
        assert(mu_w.n_elem == n_basis_functions * njoints);
        mat Sigma_w = eye<mat>(n_basis_functions * njoints,
                    n_basis_functions * njoints);
        mat Sigma_y = 0.001 * eye<mat>(njoints, njoints);
        ProMP promp(mu_w, Sigma_w, Sigma_y);
        FullProMP nn_promp(basis.at(i), promp, njoints);
        promps.push_back(nn_promp);
    }
    int min_duration = vm["mindur"].as<int>();
    int ndurations = vm["ndur"].as<int>();
    mat transition(nstates, nstates);
    transition.fill(1.0 / nstates );
    if (vm.count("noselftransitions")) {
        transition.fill(1.0 / (nstates - 1));
        transition.diag().zeros();
    }
    vec pi(nstates);
    pi.fill(1.0/nstates);
    mat durations(nstates, ndurations);
    durations.fill(1.0 / ndurations);

    // Creating the ProMP emission.
    shared_ptr<ProMPsEmission> ptr_emission(new ProMPsEmission(promps));

    // Creating a prior for Sigma_w.
    if (vm.count("wpriorvar")) {
        mat Phi = vm["wpriorvar"].as<double>() * eye<mat>(
                n_basis_functions * njoints, n_basis_functions * njoints);
        NormalInverseWishart iw_prior(Phi, Phi.n_rows + 2);
        ptr_emission->set_Sigma_w_Prior(iw_prior);
    }
    if (vm.count("delta"))
        ptr_emission->setDelta(vm["delta"].as<double>());

    OnlineHSMM promp_hsmm(std::static_pointer_cast<
            AbstractEmissionOnlineSetting>(ptr_emission),
            transition, pi, durations, min_duration);

    if (vm.count("nodur"))
        promp_hsmm.setDurationLearningChoice("nodur");
    if (vm.count("durmomentmatching"))
        promp_hsmm.setDurationLearningChoice("momentmatching");
    if (vm.count("alphadurprior")) {
        mat alphas = ones<mat>(nstates, ndurations) *
            vm["alphadurprior"].as<int>();
        promp_hsmm.setDurationDirichletPrior(alphas);
    }
    if (vm.count("notrans"))
        promp_hsmm.learning_transitions_ = false;
    if (vm.count("nopi"))
        promp_hsmm.learning_pi_ = false;
    if (vm.count("debug"))
        promp_hsmm.debug_ = true;

    // Saving the model in a json file.
    std::ofstream initial_params(output_filename);
    nlohmann::json initial_model = promp_hsmm.to_stream();
    initial_params << std::setw(4) << initial_model << std::endl;
    initial_params.close();

    // Leave one out.
    field<field<mat>> t_seq;
    field<Labels> t_labels;
    if (vm.count("leaveoneout")) {
        int omitted = vm["leaveoneout"].as<int>();
        field<field<mat>> left_one_out(seq_obs.n_elem - 1);
        field<Labels> left_labels(seq_labels.n_elem - 1);
        int idx = 0;
        for(int i = 0; i < seq_obs.n_elem; i++)
            if (i != omitted) {
                left_one_out(idx) = seq_obs(i);
                left_labels(idx) = seq_labels(i);
                idx++;
            }
        t_seq = left_one_out;
        t_labels = left_labels;
        cout << "Leaving one out of the training: " << omitted << endl;
    }
    else {
        t_seq = seq_obs;
        t_labels = seq_labels;
    }

    for(int i = 0; i < vm["trainingiter"].as<int>(); i++) {

        // Reading the current parameters.
        std::ifstream current_params_stream(output_filename);
        nlohmann::json current_params;
        current_params_stream >> current_params;
        promp_hsmm.from_stream(current_params);

        bool convergence_reached = promp_hsmm.fit(t_seq, t_labels, 5, 1e-5);

        // Saving again the parameters after one training iteration.
        std::ofstream output_params(output_filename);
        current_params = promp_hsmm.to_stream();
        if (vm.count("commitid"))
            current_params["git_commit_id"] = vm["commitid"].as<string>();
        output_params << std::setw(4) << current_params << std::endl;
        output_params.close();

        ViterbiAlgorithm(promp_hsmm, seq_obs, vm["viterbi"].as<string>());

        if (convergence_reached)
            break;
    }

    cout << "loglikelihood: " << promp_hsmm.loglikelihood(t_seq) << endl;
    if (vm.count("leaveoneout")) {
        int omitted = vm["leaveoneout"].as<int>();
        field<field<mat>> test = {seq_obs(omitted)};
        field<Labels> test_labels = {seq_labels(omitted)};
        cout << "loglikelihoodtest: " << promp_hsmm.loglikelihood(test) << endl;
    }

    if (vm.count("leaveoneout")) {
        int omitted = vm["leaveoneout"].as<int>();
        string test_filename = input_filename + "." + to_string(omitted);
        mat obs_for_cond;
        obs_for_cond.load(test_filename, raw_ascii);
        mat state_marginals_over_time(nstates, obs_for_cond.n_cols);
        mat runlength_marginals_over_time(min_duration + ndurations,
                obs_for_cond.n_cols);
        mat duration_marginals_over_time(ndurations, obs_for_cond.n_cols);
        for(int c = 0; c < obs_for_cond.n_cols; c++) {
            promp_hsmm.addNewObservation(obs_for_cond.col(c));
            if (vm.count("ms")) {
                vec s_marginal = promp_hsmm.getStateMarginal();
                state_marginals_over_time.col(c) = s_marginal;
            }
            if (vm.count("mr")) {
                vec r_marginal = promp_hsmm.getRunlengthMarginal();
                runlength_marginals_over_time.col(c) = r_marginal;
            }
            if (vm.count("md")) {
                vec d_marginal = promp_hsmm.getDurationMarginal();
                duration_marginals_over_time.col(c) = d_marginal;
            }
        }
        // Evaluation the likelihood of the left-out trajectory.
        field<field<mat>> field_obs = {seq_obs(omitted)};
        double onlinell = promp_hsmm.loglikelihood(field_obs);
        cout << "onlineloglikelihood: " << onlinell << endl;

        // Saving the marginals if required.
        if (vm.count("ms"))
            state_marginals_over_time.save(vm["ms"].as<string>(), raw_ascii);
        if (vm.count("mr"))
            runlength_marginals_over_time.save(vm["mr"].as<string>(), raw_ascii);
        if (vm.count("md"))
            duration_marginals_over_time.save(vm["md"].as<string>(), raw_ascii);
    }
    return 0;
}


// STD
#include <iostream>
#include <vector>

// Boost
#include <boost/iostreams/stream.hpp>
#include <boost/iostreams/device/null.hpp>

#include <fmt/format.h>

// jhmr
#include "jhmrProgOptUtils.h"
#include "jhmrPAOUtils.h"
//#include "jhmrTimer.h"
//#include "jhmrProjDataIO.h"
#include "jhmrHDF5.h"
#include "jhmrRegi2D3DDebugH5.h"
#include "jhmrRegi2D3DDebugIO.h"
#include "jhmrMultiObjMultiLevel2D3DRegi.h"
#include "jhmrFCSVUtils.h"
#include "jhmrProjPreProc.h"
#include "jhmrCIOSFusionDICOM.h"
#include "jhmrHounsfieldToLinearAttenuationFilter.h"
#include "jhmrGPUPrefsXML.h"
#include "jhmrRegi2D3DCMAES.h"
#include "jhmrRegi2D3DCMAES-Device-MultiConfig.h"
#include "jhmrRegi2D3DBOBYQA.h"
#include "jhmrPatchGradNCCImageSimilarityMetricGPU.h"
#include "jhmrNormalDist.h"
#include "jhmrFoldedNormalDist.h"
#include "jhmrRegi2D3DSE3EulerDecompPenaltyFn.h"
#include "jhmrRegi2D3DSE3MagPenaltyFn.h"
#include "jhmrCameraRayCastingGPU.h"
#include "jhmrAnatCoordFrames.h"
#include "jhmrLandmark2D3DRegi.h"
#include "jhmrRegi2D3DIntensityExhaustive.h"
#include "jhmrSampleUniformUnitVecs.h"

#include "CommonFEM.h"

#include "bigssMath.h"

using namespace jhmr;
using namespace jhmr::fem;

constexpr int kEXIT_VAL_SUCCESS = 0;
constexpr int kEXIT_VAL_BAD_USE = 1;
constexpr bool kSAVE_REGI_DEBUG = true;
using SimMetGPU = PatchGradNCCImageSimilarityMetricGPU;
using CMAESRegi           = Regi2D3DCMAES<MultiLevelMultiObjRegi::RayCaster,
MultiLevelMultiObjRegi::SimMetric>;
using CMAES_Device_MultiConfig_Regi = Regi2D3DCMAESdeviceMultiConfig<MultiLevelMultiObjRegi::RayCaster, MultiLevelMultiObjRegi::SimMetric>;

typedef RayCasterLineIntGPU::ImageVolumeType ImageVolumeType;

using size_type = std::size_t;

using Pt3         = Eigen::Matrix<CoordScalar,3,1>;
using Pt2         = Eigen::Matrix<CoordScalar,2,1>;

FrameTransform ConvertSlicerToITK(std::vector<float> slicer_vec){
  FrameTransform RAS2LPS;
  RAS2LPS(0, 0) = -1; RAS2LPS(0, 1) = 0; RAS2LPS(0, 2) = 0; RAS2LPS(0, 3) = 0;
  RAS2LPS(1, 0) = 0;  RAS2LPS(1, 1) = -1;RAS2LPS(1, 2) = 0; RAS2LPS(1, 3) = 0;
  RAS2LPS(2, 0) = 0;  RAS2LPS(2, 1) = 0; RAS2LPS(2, 2) = 1; RAS2LPS(2, 3) = 0;
  RAS2LPS(3, 0) = 0;  RAS2LPS(3, 1) = 0; RAS2LPS(3, 2) = 0; RAS2LPS(3, 3) = 1;

  FrameTransform ITK_xform;
  for(size_type idx=0; idx<4; ++idx)
  {
    for(size_type idy=0; idy<3; ++idy)
    {
      ITK_xform(idy, idx) = slicer_vec[idx*3+idy];
    }
    ITK_xform(3, idx) = 0.0;
  }
  ITK_xform(3, 3) = 1.0;

  ITK_xform= RAS2LPS * ITK_xform * RAS2LPS;

  float tmp1, tmp2, tmp3;
  tmp1 = ITK_xform(0,0)*ITK_xform(0,3) + ITK_xform(0,1)*ITK_xform(1,3) + ITK_xform(0,2)*ITK_xform(2,3);
  tmp2 = ITK_xform(1,0)*ITK_xform(0,3) + ITK_xform(1,1)*ITK_xform(1,3) + ITK_xform(1,2)*ITK_xform(2,3);
  tmp3 = ITK_xform(2,0)*ITK_xform(0,3) + ITK_xform(2,1)*ITK_xform(1,3) + ITK_xform(2,2)*ITK_xform(2,3);

  ITK_xform(0,3) = -tmp1;
  ITK_xform(1,3) = -tmp2;
  ITK_xform(2,3) = -tmp3;

  return ITK_xform;
}

int main(int argc, char* argv[])
{
  #ifdef JHMR_HAS_OPENCL
    typedef CameraRayCasterGPULineIntegral RayCaster;
  #else
    typedef CameraRayCasterCPULineIntegral<float> RayCaster;
  #endif
  typedef RayCaster::CameraModelType CamModel;

  ProgOpts po;

  jhmrPROG_OPTS_SET_COMPILE_DATE(po);

  po.set_help("Compute 3D Polaris Position of Snake Tip Jig");
  po.set_arg_usage("<Source Bayview H5 root path> <Bayview H5 Debug file root path> <Bayview slicer root path> <Experiment list file path> <Result file path> <Top landmark path> <result path>");
  po.set_min_num_pos_args(4);

  po.add("verbose", 'v', ProgOpts::kSTORE_TRUE, "verbose", "Verbose logging to stdout")
    << false;  // default to non-verbose mode

  try
  {
    po.parse(argc, argv);
  }
  catch (const ProgOpts::Exception& e)
  {
    std::cerr << "Error parsing command line arguments: " << e.what() << std::endl;
    po.print_usage(std::cerr);
    return kEXIT_VAL_BAD_USE;
  }

  if (po.help_set())
  {
    po.print_usage(std::cout);
    po.print_help(std::cout);
    return kEXIT_VAL_SUCCESS;
  }

  boost::iostreams::stream<boost::iostreams::null_sink> null_ostream((boost::iostreams::null_sink()));
  const bool verbose = po.get("verbose");
  std::ostream& vout = verbose ? std::cout : null_ostream;

  const std::string device_2d_fcsv_root_path  = po.pos_args()[0];  // 2D Landmark root path
  const std::string device_3d_fcsv_path       = po.pos_args()[1];  // 3D device landmarks path
  const std::string UR_kins_path              = po.pos_args()[2];  // UR kinematics path
  const std::string dicom_path                = po.pos_args()[3];  // Dicom image path
  const std::string output_path               = po.pos_args()[4];  // Output path

  std::vector<std::string> img_ID_list(po.pos_args().begin()+5, po.pos_args().end());

  const size_type num_views = img_ID_list.size();

  vout << "default GPU prefs..." << std::endl;
  GPUPrefsXML gpu_prefs;

  std::cout << "reading device BB landmarks from FCSV file..." << std::endl;
  auto device_3d_fcsv = ReadFCSVFileNamePtMap<Pt3>(device_3d_fcsv_path);
  FromMapConvertRASToLPS(device_3d_fcsv.begin(), device_3d_fcsv.end());

  const std::string handeye_X_path = "/Users/gaocong/Documents/Research/Femoroplasty/Phantom_Drilling/Handeye/AXYB_output/devicehandeye_X.h5";
  const std::string devicevol_path = "/Users/gaocong/Documents/Research/Femoroplasty/Phantom_Drilling/meta_data/Device_crop_CT.nii.gz";
  const std::string deviceseg_path = "/Users/gaocong/Documents/Research/Femoroplasty/Phantom_Drilling/meta_data/Device_crop_seg.nii.gz";

  FrameTransform device_rotcen_ref = FrameTransform::Identity();
  {
    auto deviceref_fcsv = device_3d_fcsv.find("RotCenter");
    Pt3 device_rotcen_pt;

    if (deviceref_fcsv != device_3d_fcsv.end()){
      device_rotcen_pt = deviceref_fcsv->second;
    }
    else{
      std::cout << "ERROR: NOT FOUND DEVICE ROT CENTER PT" << std::endl;
    }

    device_rotcen_ref.matrix()(0,3) = -device_rotcen_pt[0];
    device_rotcen_ref.matrix()(1,3) = -device_rotcen_pt[1];
    device_rotcen_ref.matrix()(2,3) = -device_rotcen_pt[2];
  }

  FrameTransform handeye_ref_xform = FrameTransform::Identity();
  {
    auto handeye_ref_fcsv = device_3d_fcsv.find("RotCenter");
    Pt3 handeye_ref_pt;

    if (handeye_ref_fcsv != device_3d_fcsv.end()){
      handeye_ref_pt = handeye_ref_fcsv->second;
    }
    else{
      std::cout << "ERROR: NOT FOUND DEVICE ROT CENTER PT" << std::endl;
    }

    handeye_ref_xform.matrix()(0,3) = -handeye_ref_pt[0];
    handeye_ref_xform.matrix()(1,3) = -handeye_ref_pt[1];
    handeye_ref_xform.matrix()(2,3) = -handeye_ref_pt[2];
  }

  LabelVolPtr device_seg = ReadITKImageFromDisk<LabelVol>(deviceseg_path);

  VolPtr devicevol_att;
  {
    vout << "reading device volume..." << std::endl; // We only use the needle metal part
    auto devicevol_hu = ReadITKImageFromDisk<Vol>(devicevol_path);

    vout << "  HU --> Att. ..." << std::endl;
    auto hu2att = HounsfieldToLinearAttenuationFilter<Vol>::New();
    hu2att->SetInput(devicevol_hu);
    hu2att->Update();

    devicevol_att = hu2att->GetOutput();
  }

  const LabelScalar device_label = 1;
  VolPtr device_vol = ApplyMaskToITKImage(devicevol_att.GetPointer(), device_seg.GetPointer(), device_label, PixelScalar(0), true);

  std::shared_ptr<MultiLevelMultiObjRegi::CamAlignRefFrameWithCurPose> device_singleview_regi_ref_frame;

  {
    vout << "setting up device ref. frame..." << std::endl;
    // setup camera aligned reference frame, use metal device volume center point as the origin

    itk::ContinuousIndex<double,3> center_idx;

    auto device_fcsv_rotc = device_3d_fcsv.find("RotCenter");
    Pt3 device_rotcenter;

    if (device_fcsv_rotc != device_3d_fcsv.end()){
      device_rotcenter = device_fcsv_rotc->second;
    }
    else{
      vout << "ERROR: NOT FOUND DRILL ROT CENTER" << std::endl;
    }

    device_singleview_regi_ref_frame = std::make_shared<MultiLevelMultiObjRegi::CamAlignRefFrameWithCurPose>();
    device_singleview_regi_ref_frame->vol_idx = 0;
    device_singleview_regi_ref_frame->center_of_rot_wrt_vol[0] = device_rotcenter[0];
    device_singleview_regi_ref_frame->center_of_rot_wrt_vol[1] = device_rotcenter[1];
    device_singleview_regi_ref_frame->center_of_rot_wrt_vol[2] = device_rotcenter[2];
  }

  const CamModel default_cam = NaiveCamModelFromCIOSFusion(
  MakeNaiveCIOSFusionMetaDR(), true).cast<CoordScalar>();

  bool is_first_view = true;

  ProjPreProc<PixelScalar> proj_pre_proc;
  proj_pre_proc.input_projs.resize(num_views);

  std::vector<CIOSFusionDICOMInfo> devicecios_metas(num_views);
  for(size_type view_idx = 0; view_idx < num_views; ++view_idx)
  {
    const std::string img_path = dicom_path + "/" + img_ID_list[view_idx];
    std::tie(proj_pre_proc.input_projs[view_idx].img, devicecios_metas[view_idx]) =
                                                    ReadCIOSFusionDICOM<PixelScalar>(img_path);
    proj_pre_proc.input_projs[view_idx].cam = NaiveCamModelFromCIOSFusion(
                                                    devicecios_metas[view_idx], true).cast<CoordScalar>();
  }

  proj_pre_proc();
  auto& projs_to_regi = proj_pre_proc.output_projs;

  {
    std::vector<SingleProjData<PixelScalar>> tmp_pd_list(1);
    {
      tmp_pd_list[0].img = projs_to_regi[0].img;
      tmp_pd_list[0].cam = projs_to_regi[0].cam;
    }
    WriteProjData(output_path+"/proj_dr_data" + img_ID_list[0] + ".xml", tmp_pd_list);
  }

  // ###################### Multi-configuration initialization ###############################
  Pt3 ld_3D_pt_init, ld2_3D_pt_init;
  std::vector<float> initref_vecX, initref_vecY, initref_vecZ;
  float initref_minX = 100000;
  float initref_maxX = -100000;
  float initref_minY = 100000;
  float initref_maxY = -100000;
  float initref_minZ = 100000;
  float initref_maxZ = -100000;
  std::vector<Pt3> ld_3D_pt_init_list;
  FrameTransform handeye_regi_X;
  ReadITKAffineTransformFromFile(handeye_X_path, &handeye_regi_X);

  FrameTransformList UReef_xform_list;
  for(size_type view_idx = 0; view_idx < num_views; ++view_idx)
  {
    FrameTransform regi_UReef_xform;
    {
      const std::string src_ureef_path          = UR_kins_path + "/" + img_ID_list[view_idx] + "/ur_eef.h5";
      H5::H5File h5_ureef(src_ureef_path, H5F_ACC_RDWR);
      H5::Group ureef_transform_group           = h5_ureef.openGroup("TransformGroup");
      H5::Group ureef_group0                    = ureef_transform_group.openGroup("0");
      std::vector<float> UReef_tracker          = ReadVectorH5<float>("TranformParameters", ureef_group0);
      regi_UReef_xform                         = ConvertSlicerToITK(UReef_tracker);
    }
    UReef_xform_list.push_back(regi_UReef_xform);
  }

  // Create 3D pnp landmark map
  LandMap3 pnp_vol_lands;
  LandMap2 pnp_img_lands;
  for(size_type view_idx = 0; view_idx < num_views; ++view_idx)
  {
    // Assuming first view device volume frame as reference frame;
    // ld_3D_pt_init is i-th view base landmark w.r.t. first view base seg volume frame
    for( const auto& n : device_3d_fcsv )
    {
      auto ld_3D_pt_init = handeye_ref_xform.inverse() * handeye_regi_X.inverse() * UReef_xform_list[0].inverse() * UReef_xform_list[view_idx] * handeye_regi_X * handeye_ref_xform * n.second;
      ld_3D_pt_init_list.push_back(ld_3D_pt_init);

      // Sum over all the points w.r.t. first view device volume frame
      initref_vecX.push_back( ld_3D_pt_init(0) );
      initref_vecY.push_back( ld_3D_pt_init(1) );
      initref_vecZ.push_back( ld_3D_pt_init(2) );

      if( ld_3D_pt_init(0) > initref_maxX )
        initref_maxX = ld_3D_pt_init(0);

      if( ld_3D_pt_init(1) > initref_maxY )
        initref_maxY = ld_3D_pt_init(1);

      if( ld_3D_pt_init(2) > initref_maxZ )
        initref_maxZ = ld_3D_pt_init(2);

      if( ld_3D_pt_init(0) < initref_minX )
        initref_minX = ld_3D_pt_init(0);

      if( ld_3D_pt_init(1) < initref_minY )
        initref_minY = ld_3D_pt_init(1);

      if( ld_3D_pt_init(2) < initref_minZ )
        initref_minZ = ld_3D_pt_init(2);
    }

    const std::string device_2d_fcsv_path = device_2d_fcsv_root_path + "/" + img_ID_list[view_idx] + ".fcsv";
    auto device_2d_fcsv = ReadFCSVFileNamePtMap<Pt3>(device_2d_fcsv_path);
    FromMapConvertRASToLPS(device_2d_fcsv.begin(), device_2d_fcsv.end());

//    jhmrASSERT(device_2d_fcsv.size() > 3);

    for( const auto& n : device_2d_fcsv )
    {
      std::pair<std::string, Pt2> pnp_ld(std::to_string(view_idx) + n.first, Pt2{(1536 - n.second[0]/0.194), (1536 - n.second[1]/0.194)});
      pnp_img_lands.insert(pnp_ld);
    }
  }

  // Average over all points

  // Define a refernece frame at the center of ld1_3D_pts
  FrameTransform initref_xform = FrameTransform::Identity();
  initref_xform(0,3) = -(initref_minX + initref_maxX)/2;
  initref_xform(1,3) = -(initref_minY + initref_maxY)/2;
  initref_xform(2,3) = -(initref_minZ + initref_maxZ)/2;

  for(size_type view_idx=0; view_idx < num_views; ++view_idx)
  {
    for( const auto& n : device_3d_fcsv )
    {
      auto ld_3D_pt_init = handeye_ref_xform.inverse() * handeye_regi_X.inverse() * UReef_xform_list[0].inverse() * UReef_xform_list[view_idx] * handeye_regi_X * handeye_ref_xform * n.second;
      auto ld_3D_pt_ref = initref_xform * ld_3D_pt_init;
      std::pair<std::string, Pt3> ld_3D(std::to_string(view_idx) + n.first, ld_3D_pt_ref);
      pnp_vol_lands.insert(ld_3D);
    }
  }

  const FrameTransform lands_cam_to_initref = EstCamToWorldBruteForcePOSITCMAESRefine(default_cam, pnp_img_lands, pnp_vol_lands);

  MultiLevelMultiObjRegi regi;

  regi.save_debug_info            = kSAVE_REGI_DEBUG;
  regi.debug_info.vol_path        = devicevol_path;
  regi.debug_info.label_vol_path  = deviceseg_path;
  regi.debug_info.labels_used = { };

  regi.vols = { };
  regi.vol_names = { };

  regi.ref_frames = { device_singleview_regi_ref_frame };

  regi.fixed_cam_imgs.resize(num_views);

  for (size_type view_idx = 0; view_idx < num_views; ++view_idx)
  {
    regi.debug_info.labels_used.push_back( 1 );
    regi.vols.push_back( device_vol );
    regi.vol_names.push_back( "Device" );
    regi.fixed_cam_imgs[view_idx].img = projs_to_regi[view_idx].img;
    regi.fixed_cam_imgs[view_idx].cam = projs_to_regi[view_idx].cam;
  }

  regi.levels.resize(1);

  regi.debug_info.regi_names = { {"Device"} };

  regi.init_cam_to_vols = { };

  for(size_type view_idx = 0; view_idx < num_views; ++view_idx)
  {
    FrameTransform init_cam_wrt_device = handeye_ref_xform.inverse() * handeye_regi_X.inverse() * UReef_xform_list[view_idx].inverse()
                            * UReef_xform_list[0] * handeye_regi_X * handeye_ref_xform * initref_xform.inverse() * lands_cam_to_initref;

    regi.init_cam_to_vols.push_back( init_cam_wrt_device );

    // ###################### Reprojection for initial pnp poses ###############################
    {
      RayCaster ray_caster;
      ray_caster.set_camera_model(default_cam);
      ray_caster.use_proj_store_replace_method();
      ray_caster.set_volume(device_vol);
      ray_caster.set_num_projs(1);
      ray_caster.allocate_resources();
      ray_caster.xform_cam_to_itk_phys(0) = init_cam_wrt_device;
      ray_caster.compute(0);

      WriteITKImageRemap8bpp(ray_caster.proj(0).GetPointer(), output_path + "/deice_init_reproj" + img_ID_list[view_idx] + ".png");
    }

  }

  device_singleview_regi_ref_frame->cam_extrins = regi.fixed_cam_imgs[0].cam.extrins;

  auto se3_vars = std::make_shared<SE3OptVarsLieAlg<double>>();

  auto& lvl = regi.levels[0];

  lvl.ds_factor = 0.125;

  auto rc = std::make_shared<RayCasterLineIntGPU>(gpu_prefs.ctx, gpu_prefs.queue);
  lvl.ray_caster = rc;
  {
    lvl.sim_metrics.resize(num_views);
    for (size_type view_idx = 0; view_idx < num_views; ++view_idx)
    {
      auto sm = std::make_shared<PatchGradNCCImageSimilarityMetricGPU>(gpu_prefs.ctx, gpu_prefs.queue);
      sm->set_setup_vienna_cl_ctx(view_idx == 0);
      sm->set_smooth_img_before_sobel_kernel_radius(5);
      sm->set_patch_radius(std::lround(lvl.ds_factor * 41));
      sm->set_patch_stride(1);
      lvl.sim_metrics[view_idx] = sm;
    }
  }

  lvl.regis.resize(1);

  {
    auto& lvl_regi_coarse = lvl.regis[0];
    lvl_regi_coarse.mov_vols = { };
    lvl_regi_coarse.init_mov_vol_poses = {};
    lvl_regi_coarse.ref_frames = { };
    for(size_type view_idx = 0; view_idx < num_views; ++view_idx)
    {
      lvl_regi_coarse.mov_vols.push_back( view_idx );

      auto init_guess = std::make_shared<MultiLevelMultiObjRegi::Level::SingleRegi::InitPosePrevPoseEst>();
      init_guess->vol_idx = view_idx;
      lvl_regi_coarse.init_mov_vol_poses.push_back( init_guess );
      lvl_regi_coarse.ref_frames.push_back( 0 );
    }
    lvl_regi_coarse.static_vols = { };

    {
      auto cmaes_regi = std::make_shared<CMAES_Device_MultiConfig_Regi>();
      cmaes_regi->set_opt_vars(se3_vars);
      cmaes_regi->set_opt_x_tol(0.01);
      cmaes_regi->set_opt_obj_fn_tol(0.01);

      cmaes_regi->set_UReef_xform_list(UReef_xform_list);
      cmaes_regi->set_initref_xform(initref_xform);
      cmaes_regi->set_handeye_ref_xform(handeye_ref_xform);
      cmaes_regi->set_handeyeX(handeye_regi_X);

      using NormPDF = NormalDist1D<CoordScalar>;

      auto pen_fn = std::make_shared<Regi2D3DSE3EulerDecompPenaltyFn<NormPDF>>();

      cmaes_regi->set_pop_size(50);
      cmaes_regi->set_sigma({ 5 * kDEG2RAD, 5 * kDEG2RAD, 5 * kDEG2RAD, 10, 10, 20 });

      pen_fn->rot_x_pdf   = NormPDF(0, 10 * kDEG2RAD);
      pen_fn->rot_y_pdf   = NormPDF(0, 10 * kDEG2RAD);
      pen_fn->rot_z_pdf   = NormPDF(0, 10 * kDEG2RAD);
      pen_fn->trans_x_pdf = NormPDF(0, 15);
      pen_fn->trans_y_pdf = NormPDF(0, 15);
      pen_fn->trans_z_pdf = NormPDF(0, 20);

      cmaes_regi->set_penalty_fn(pen_fn);
      cmaes_regi->set_img_sim_penalty_coefs(0.9, 1.0);

      lvl_regi_coarse.regi = cmaes_regi;
    }
    vout << std::endl << "Running registration ..." << std::endl;
    regi.run();
    regi.levels[0].regis[0].fns_to_call_right_before_regi_run.clear();

    if(kSAVE_REGI_DEBUG){
      size_type img_num = regi.fixed_cam_imgs.size();
      std::vector<SingleProjData<PixelScalar>> tmp_pd_list(img_num);
      for (size_type v = 0; v < img_num; ++v)
      {
        tmp_pd_list[v].img = regi.fixed_cam_imgs[v].img;
        tmp_pd_list[v].cam = regi.fixed_cam_imgs[v].cam;
      }
      WriteRegi2D3DMultiLevelDebug(regi.debug_info, output_path+"/device_regi_debug" + img_ID_list[0] + ".h5",
                                  std::make_tuple(VolPtr(), LabelVolPtr(), tmp_pd_list));
    }
  }

  const std::string device_3d_bb_fcsv_path = "/Users/gaocong/Documents/Research/Femoroplasty/Phantom_Drilling/meta_data/Device3Dbb.fcsv";

  std::cout << "reading device BB landmarks from FCSV file..." << std::endl;
  auto device_3d_bb_fcsv = ReadFCSVFileNamePtMap<Pt3>(device_3d_bb_fcsv_path);
  FromMapConvertRASToLPS(device_3d_bb_fcsv.begin(), device_3d_bb_fcsv.end());

  for(size_type view_idx = 0; view_idx < num_views; ++view_idx)
  {
    WriteITKAffineTransform(output_path + "/device_regi_xform" + img_ID_list[view_idx] + ".h5", regi.cur_cam_to_vols[view_idx]);

    RayCaster ray_caster;
    ray_caster.set_camera_model(default_cam);
    ray_caster.use_proj_store_replace_method();
    ray_caster.set_volume(device_vol);
    ray_caster.set_num_projs(1);
    ray_caster.allocate_resources();
    ray_caster.xform_cam_to_itk_phys(0) = regi.cur_cam_to_vols[view_idx];
    ray_caster.compute(0);

    WriteITKImageRemap8bpp(ray_caster.proj(0).GetPointer(), output_path + "/device_reproj" + img_ID_list[view_idx] + ".png");
    WriteITKImageRemap8bpp(proj_pre_proc.input_projs[view_idx].img.GetPointer(), output_path + "/real" + img_ID_list[view_idx] + ".png");

    LandMap3 reproj_bbs_fcsv;

    for( const auto& n : device_3d_bb_fcsv )
    {
      auto reproj_bb = default_cam.phys_pt_to_ind_pt(Pt3(regi.cur_cam_to_vols[view_idx].inverse() *  n.second));
      reproj_bb[0] = 0.194 * (reproj_bb[0] - 1536);
      reproj_bb[1] = 0.194 * (reproj_bb[1] - 1536);
      reproj_bb[2] = 0;
      std::pair<std::string, Pt3> ld2_3D(n.first, reproj_bb);
      reproj_bbs_fcsv.insert(ld2_3D);
    }

    WriteFCSVFileFromNamePtMap(output_path + "/reproj_bb" + img_ID_list[view_idx] + ".fcsv", reproj_bbs_fcsv.begin(), reproj_bbs_fcsv.end());
  }

  // ###################### Check reprojection using kinematics ###############################
  size_type reproj_idx = 1;
  {
    FrameTransform reproj_device_xform = device_rotcen_ref.inverse() * handeye_regi_X.inverse() * UReef_xform_list[reproj_idx].inverse() * UReef_xform_list[0] * handeye_regi_X * device_rotcen_ref * regi.cur_cam_to_vols[0];

    RayCaster ray_caster;
    ray_caster.set_camera_model(default_cam);
    ray_caster.use_proj_store_replace_method();
    ray_caster.set_volume(device_vol);
    ray_caster.set_num_projs(1);
    ray_caster.allocate_resources();
    ray_caster.xform_cam_to_itk_phys(0) = reproj_device_xform;
    ray_caster.compute(0);

    WriteITKImageRemap8bpp(ray_caster.proj(0).GetPointer(), output_path + "/deice_URkins_repos" + img_ID_list[reproj_idx] + ".png");
  }

  return 0;
}


#include "df_grid.hpp"
#include "datatypes.hpp"
#include <boost/python.hpp>

namespace gd {
using namespace boost::python;

void py_export_schw_df_grid() {
	class_<DFGrid, boost::noncopyable >("DFGrid", init<int, int, double, double, Galaxy*>())
		.def("output_grid", &DFGrid::output_grid)
		.def("n_dofs", &DFGrid::n_dofs)
		.def("refine_global", &DFGrid::refine_global)
		.def("shape_value", &DFGrid::shape_value)
		.def("print_dof_indices_per_face", &DFGrid::print_dof_indices_per_face)
		.def("print_dof_indices_per_face_on_level", &DFGrid::print_dof_indices_per_face_on_level)
		.def("print_vertices", &DFGrid::print_vertices)
			;
}

};