// std c
#include <stdio.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <iostream>
#include <string>
#include <sstream>
#include <ctime>

// opencv pcl
#include <opencv/cv.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/imgproc/imgproc.hpp"

// ros
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>

// Eigen
#include <Eigen/Dense>
#include <Eigen/Core>

// ours
#include "detect_3d_cuboid/matrix_utils.h"
#include "detect_3d_cuboid/object_3d_util.h"
#include "tictoc_profiler/profiler.hpp"

using namespace std;
// using namespace cv;
using namespace Eigen;

void detect_3d_cuboid::set_calibration(const Matrix3d& Kalib)
{
      cam_pose.Kalib = Kalib;
      cam_pose.invK = Kalib.inverse();
}

// BRIEF 相机位姿：将 Matrix4d 表示的相机位姿转换成自定义的 cam_pose_infos 位姿结构体.
void detect_3d_cuboid::set_cam_pose(const Matrix4d& transToWolrd)
{
      cam_pose.transToWolrd = transToWolrd;
      cam_pose.rotationToWorld = transToWolrd.topLeftCorner<3,3>();
      Vector3d euler_angles; quat_to_euler_zyx(Quaterniond(cam_pose.rotationToWorld),euler_angles(0),euler_angles(1),euler_angles(2));
      cam_pose.euler_angle = euler_angles;
      cam_pose.invR = cam_pose.rotationToWorld.inverse();
      cam_pose.projectionMatrix = cam_pose.Kalib*transToWolrd.inverse().topRows<3>();  // project world coordinate to camera
      cam_pose.KinvR = cam_pose.Kalib * cam_pose.invR;
      cam_pose.camera_yaw = cam_pose.euler_angle(2);
      //TODO relative measure? not good... then need to change transToWolrd.
}

// NOTE 立方体检测函数.
/* 输入：原始图像 rgb_img
		相机位姿 transToWolrd
		2D 检测框信息 obj_bbox_coors
		提取的边缘矩阵信息 all_lines_raw
   输出：立方体提案 all_object_cuboids
*/
void detect_3d_cuboid::detect_cuboid(	const cv::Mat& rgb_img,
										const Matrix4d& transToWolrd,
										const MatrixXd& obj_bbox_coors,
				     				 	MatrixXd all_lines_raw,
										std::vector<ObjectSet>& all_object_cuboids)
{
	cv::Mat merge_lines_img = rgb_img.clone();
	// 绘制上边缘采样点
	typedef cv::Point_<int> Point2i;
	std::vector<Point2i> simple_points;
	cv::Mat image_point = rgb_img;

	// 读取相机位姿，将 Matrix4d 表示的相机位姿转换成自定义的 cam_pose_infos 位姿结构体格式.
    set_cam_pose(transToWolrd);
    cam_pose_raw = cam_pose;	// 先保存一份位姿信息在 cam_pose_raw 中.

	// 转换成灰度图.
    cv::Mat gray_img;
    if (rgb_img.channels()==3)
	  	cv::cvtColor(rgb_img, gray_img, CV_BGR2GRAY);
    else
	  	gray_img = rgb_img;

	// 读取图像大小.
    int img_width = rgb_img.cols;
	int img_height = rgb_img.rows;

	// 读取检测框信息的行数，也即图像帧数.
    int num_2d_objs = obj_bbox_coors.rows();
    all_object_cuboids.resize(num_2d_objs);

	// @PARAM all_configs	每帧视图的模式：1.观察到三个面；2.观察到两个面.
    vector<bool> all_configs;
	all_configs.push_back(consider_config_1);
	all_configs.push_back(consider_config_2);

    // 误差计算的一些阈值.
    double vp12_edge_angle_thre = 15; 		// 消失点 1 2 与边的夹角阈值.
	double vp3_edge_angle_thre = 10;  		// 消失点 3 与边的夹角阈值.
    double shorted_edge_thre = 20;  		// 边的阈值.if box edge are too short. box might be too thin. most possibly wrong.
    bool reweight_edge_distance = true;  	// if want to compare with all configurations. we need to reweight

    // 归一化误差的权重.
    bool whether_normalize_two_errors = true; 	// 是否归一化角度和距离误差
	double weight_vp_angle = 0.8; 				// NOTE 角度对齐误差的权重，训练的经验值，论文中为 0.7.
	double weight_skew_error = 1.5; 			// NOTE 形状误差的权重，训练的经验值，论文中为 1.5.
    // NOTE ：if also consider config2, need to weight two erros, in order to compare two configurations

    // STEP 【1.确保边缘线段的两个端点是从左到右存储的】
    align_left_right_edges(all_lines_raw); // this should be guaranteed when detecting edges
	// 显示边缘检测的图.
    if(whether_plot_detail_images)
    {
		cv::Mat output_img;
		// NOTE plot_image_with_edges() 函数绘制线段.
		plot_image_with_edges(rgb_img, output_img, all_lines_raw, cv::Scalar(255,0,0));
		cvNamedWindow("Raw detected Edges");
		cvMoveWindow("Raw detected Edges", 20, 300);
		cv::imshow("Raw detected Edges", output_img);	 //cv::waitKey(0);
		cv::waitKey(0);
    }

	// STEP 【2.世界坐标系下的平面和相机坐标系下平面】
    // TODO find ground-wall boundary edges
	// 作为列向量处理，在 pop up 中，使用的是 [0 0 -1 0]，在这里，希望法线指向内部，朝向相机以匹配表面法线预测
    Vector4d ground_plane_world(0,0,1,0);  // treated as column vector % in my pop-up code, I use [0 0 -1 0]. here I want the normal pointing innerwards, towards the camera to match surface normal prediction
	Vector4d ground_plane_sensor = cam_pose.transToWolrd.transpose() * ground_plane_world;

	// 逐帧处理.
	//int object_id=1;
    for (int object_id = 0; object_id < num_2d_objs; object_id++)
    {
		//std::cout<<"object id  "<<object_id<<std::endl;
		// 计时？
		ca::Profiler::tictoc("One 3D object total time");

		// STEP 【3.2D 检测框大小，坐标和扩张】
		// 读取YOLO边界框的左上角点的坐标 x y 和长宽.
		int left_x_raw = obj_bbox_coors(object_id,0);
		int top_y_raw = obj_bbox_coors(object_id,1);
		int obj_width_raw = obj_bbox_coors(object_id,2);
		int obj_height_raw = obj_bbox_coors(object_id,3);
		// 计算右下角点的坐标.
		int right_x_raw = left_x_raw+obj_bbox_coors(object_id,2);
		int down_y_raw = top_y_raw + obj_height_raw;

		// NOTE 绘制检测框.
		rectangle(	rgb_img,
					cv::Point(left_x_raw, top_y_raw),
        			cv::Point(right_x_raw, down_y_raw),
					cv::Scalar(0,255,255),
					2,
					8);

		// @PARAM down_expand_sample_all
		std::vector<int> down_expand_sample_all;
		down_expand_sample_all.push_back(0);											// 0.
		// TODO：2D 目标检测可能不准确，是否采样物体高度？？ 如果不采样 down_expand_sample_all.size()=1，采样则为3.
		if (whether_sample_bbox_height)  // 2D object detection might not be accurate
		{
			int down_expand_sample_ranges = max(min(20, obj_height_raw-90),20);
			down_expand_sample_ranges = min(down_expand_sample_ranges, img_height - top_y_raw - obj_height_raw-1);  // should lie inside the image  -1 for c++ index
																	// 图像高度 - 边界框左上角y轴坐标 - 检测框高度 - 1
			// 如果扩大边界，则提供更多样本.
			if (down_expand_sample_ranges > 10)  // if expand large margin, give more samples.
				down_expand_sample_all.push_back(round(down_expand_sample_ranges/2));	// 10.
			down_expand_sample_all.push_back(down_expand_sample_ranges);			// 20.
		}
		// for(int i=0; i<down_expand_sample_all.size(); i++)
        //     std::cout << down_expand_sample_all[i] << " " ;
		// down_expand_sample_all: 0 10 20

		// STEP 【4.对偏航角进行采样】
		// NOTE later if in video, could use previous object yaw..., also reduce search range
		// @PARAM 【偏航角】初始化为面向相机，与相机光轴对齐.
		double yaw_init = cam_pose.camera_yaw - 90.0 / 180.0 * M_PI;  // yaw init is directly facing the camera, align with camera optical axis
		// NOTE 偏航物体的角采样 -45 °到45°，每隔6°采样一个值，共15个.
		std::vector<double> obj_yaw_samples;
		// BRIEF linespace()函数从 a 到 b 以步长 c 产生采样的 d.
		linespace<double>(yaw_init - 45.0/180.0*M_PI, yaw_init + 45.0/180.0*M_PI, 6.0/180.0*M_PI, obj_yaw_samples);
		// std::cout << "初始化偏航角：\n" << yaw_init << std::endl;
		// for(int i = 0; i < obj_yaw_samples.size(); i++)
		// 	std::cout << "采样偏航角：\n" << obj_yaw_samples[i] << std::endl;

		MatrixXd all_configs_errors(400,9);
		MatrixXd all_box_corners_2ds(800,8); 	// initialize a large eigen matrix
		int valid_config_number_all_height=0; 	// 高度样本的有效对象.all valid objects of all height samples

		// @PARAM 一系列立方体序列 raw_obj_proposals
		ObjectSet raw_obj_proposals;
		raw_obj_proposals.reserve(100);		// 序列长度为 100.

		// 循环 1 次或 3 次(采样高度).
		// int sample_down_expan_id=1;
		for (int sample_down_expan_id = 0; sample_down_expan_id < down_expand_sample_all.size(); sample_down_expan_id++)
		{
			int down_expand_sample = down_expand_sample_all[sample_down_expan_id]; 	// = 0 ,10 或 20
			// 高度检测框的高度.
			int obj_height_expan = obj_height_raw + down_expand_sample;
			int down_y_expan = top_y_raw + obj_height_expan;
			// 宽度没变，高度变高了，求取对角线的长度. // TODO 为什么只拓宽了高度没有拓宽宽度呢？后面扩大检测框的大小高宽都变大了
			double obj_diaglength_expan = sqrt(obj_width_raw * obj_width_raw + obj_height_expan * obj_height_expan);

			// STEP 【5. 上边缘采样点】
			// 【顶边上的采样点的x坐标】，如果边太大，则提供更多样本。为所有边缘提供至少10个样本。对于小物体，物体位姿会改变很多.
			// NOTE 从边界框的最左边 left_x_raw+5 到最右边 right_x_raw-5 每隔 top_sample_resolution（20像素）的距离采样一个点top_x_samples[i].
			int top_sample_resolution = round(min(20,obj_width_raw/10 )); //  25 pixels

			// NOTE 修复 bug，若 top_sample_resolution = 0 会出现 Floating point exception (core dumped) 的错误.
			if(top_sample_resolution < 1)
				break;

			std::vector<int> top_x_samples;
			linespace<int>(left_x_raw+5, right_x_raw-5, top_sample_resolution, top_x_samples);
			// std::cout << "左：" << left_x_raw+5 << std::endl;
			// std::cout << "右：" << right_x_raw-5 << std::endl;
			// std::cout << "top_x_samples 采样点数：" << top_x_samples.size() << std::endl;
			// for(int i=0; i<top_x_samples.size(); i++)
			// 	std::cout << top_x_samples[i] << " " ;
			// std::cout << std::endl;

			// 存储顶边采样的点
			MatrixXd sample_top_pts(2,top_x_samples.size());
			for (int ii = 0; ii < top_x_samples.size(); ii++)
			{
				sample_top_pts(0,ii)=top_x_samples[ii];
				sample_top_pts(1,ii)=top_y_raw;

				// NOTE 保存采样点的坐标并绘制
				simple_points.push_back(Point2i(top_x_samples[ii], top_y_raw));
				circle(image_point, simple_points[ii], 3, cv::Scalar(255,0,0),-1,8,0);
			}

		  	// STEP 【扩大边界范围】
			// TODO expand some small margin for distance map  [10 20]
			// @PARAM 拓宽检测框的边界.
			int distmap_expand_wid = min(max(min(20, obj_width_raw-100),10), max(min(20, obj_height_expan-100),10));	// 20.
			int left_x_expan_distmap = max(0,left_x_raw-distmap_expand_wid); 				// 检测框左边界 x 坐标往左扩大 20.
			int right_x_expan_distmap = min(img_width-1,right_x_raw+distmap_expand_wid);	// 检测框右边界 x 坐标往右扩大 20.
			int top_y_expan_distmap = max(0,top_y_raw-distmap_expand_wid); 					// 检测框上边界 y 坐标往上扩大 20.
			int down_y_expan_distmap = min(img_height-1,down_y_expan+distmap_expand_wid);	// 检测框下边界 y 坐标往上扩大 20.
			int height_expan_distmap = down_y_expan_distmap - top_y_expan_distmap; 			// 扩大后的高度.
			int width_expan_distmap = right_x_expan_distmap - left_x_expan_distmap;			// 扩大后的宽度.
			// std::cout << "distmap_expand_wid：" << distmap_expand_wid << std::endl;
			// std::cout << "left_x_expan_distmap：" << left_x_expan_distmap << std::endl;
			// std::cout << "right_x_expan_distmap：" << right_x_expan_distmap  << std::endl;
			// std::cout << "top_y_expan_distmap：" << top_y_expan_distmap << std::endl;
			// std::cout << "down_y_expan_distmap：" << down_y_expan_distmap << std::endl;
			// std::cout << "width_expan_distmap：" << width_expan_distmap << std::endl;
			Vector2d expan_distmap_lefttop = Vector2d(left_x_expan_distmap, top_y_expan_distmap);			// 左上角坐标.
			Vector2d expan_distmap_rightbottom = Vector2d(right_x_expan_distmap, down_y_expan_distmap);		// 右下角坐标.

			if(sample_down_expan_id == 0)
				// NOTE 绘制拓宽的边界.
				rectangle(	rgb_img,
							cv::Point(left_x_expan_distmap, top_y_expan_distmap),
							cv::Point(right_x_expan_distmap, down_y_expan_distmap),
							cv::Scalar(255,0,0),
							2,
							8);

			// STEP 【6.线段处理】
			// STEP 【6.1 找出在扩大后的边界框内的线段.】
	      	// find edges inside the object bounding box
			// @PARAM all_lines_inside_object：存储所有在扩大后的边界框内的线段.
			MatrixXd all_lines_inside_object(all_lines_raw.rows(),all_lines_raw.cols()); // first allocate a large matrix, then only use the toprows to avoid copy, alloc
			int inside_obj_edge_num = 0;
			// 遍历 all_lines_raw 中的每一条线
			for (int edge_id = 0; edge_id < all_lines_raw.rows(); edge_id++)
				// 判断 all_lines_raw 矩阵中第 edge_id 线段的一个端点.head<2> 是否在区域中.
				if (check_inside_box(all_lines_raw.row(edge_id).head<2>(), expan_distmap_lefttop, expan_distmap_rightbottom ))
					// 判断 all_lines_raw 矩阵中第 edge_id 线段的另一个端点.tail<2> 是否在区域中.
					if (check_inside_box(all_lines_raw.row(edge_id).tail<2>(),expan_distmap_lefttop, expan_distmap_rightbottom ))
					{
						// 存储所有在扩大后的边界框内的线段.
						all_lines_inside_object.row(inside_obj_edge_num) = all_lines_raw.row(edge_id);
						inside_obj_edge_num++;
					}

		  	// STEP 【6.2 在找到物体的边缘线之后合并边，并剔除短边，小区域的边缘合并应该更快.】
			// merge edges and remove short lines, after finding object edges.  edge merge in small regions should be faster than all.
			// @PARAM 线段合并与筛选参数.
			double pre_merge_dist_thre = 20;
			double pre_merge_angle_thre = 5;
			double edge_length_threshold = 30;
			MatrixXd all_lines_merge_inobj; 	// NOTE 合并之后的线段存储矩阵.
			merge_break_lines(	all_lines_inside_object.topRows(inside_obj_edge_num), /*输入all_lines_inside_object矩阵的前inside_obj_edge_num行*/
								all_lines_merge_inobj, 		/*输出的合并后的线段矩阵*/
								pre_merge_dist_thre,		/*两条线段的距离（水平）阈值 20 像素*/
							  	pre_merge_angle_thre, 		/*角度阈值 5°*/
								edge_length_threshold);		/*长度阈值 30 像素*/
			// 显示筛选之后的边缘线段.
			// cv::Mat output_img;
			// plot_image_with_edges(merge_lines_img, output_img, all_lines_merge_inobj, cv::Scalar(0,255,0));
			// cvNamedWindow("merge_lines_img");
			// cvMoveWindow("merge_lines_img",500, 300);
			// cv::imshow("merge_lines_img", output_img);	 //cv::waitKey(0);
			// cv::waitKey(0);

			// STEP 【7. 计算角度、中点，Canny 边缘检测，距离变换】
			// 计算每条边缘线段的角度和中点.
			// @PARAM lines_inobj_angles	线段角度.
			// @PARAM edge_mid_pts			线段的中点.
			VectorXd lines_inobj_angles(all_lines_merge_inobj.rows());
			MatrixXd edge_mid_pts(all_lines_merge_inobj.rows(),2);
			for (int i=0; i < all_lines_merge_inobj.rows(); i++)
			{
				lines_inobj_angles(i) = std::atan2(all_lines_merge_inobj(i,3)-all_lines_merge_inobj(i,1), all_lines_merge_inobj(i,2)-all_lines_merge_inobj(i,0));   // [-pi/2 -pi/2]
				edge_mid_pts.row(i).head<2>() = (all_lines_merge_inobj.row(i).head<2>()+all_lines_merge_inobj.row(i).tail<2>())/2;
			}

			// TODO could canny or distance map outside sampling height to speed up!!!!   Then only need to compute canny onces.
			// detect canny edges and compute distance transform  NOTE opencv canny maybe different from matlab. but roughly same
			// @PARAM		object_bbox		扩大后的检测框.
			cv::Rect object_bbox = cv::Rect(left_x_expan_distmap, top_y_expan_distmap, width_expan_distmap, height_expan_distmap);//

			// NOTE Canny 算子边缘检测.
			cv::Mat im_canny;
			cv::Canny(gray_img(object_bbox),im_canny,80,200); // low thre, high thre    im_canny 0 or 255   [80 200  40 100]
			// 距离变换.
			cv::Mat dist_map;
			cv::distanceTransform(255 - im_canny, dist_map, CV_DIST_L2, 3); // dist_map is float datatype

			// 是否显示处理细节.
			if (whether_plot_detail_images)
			{
				cv::imshow("im_canny",im_canny);
				cv::Mat dist_map_img;cv::normalize(dist_map, dist_map_img, 0.0, 1.0, cv::NORM_MINMAX);
				cv::imshow("normalized distance map", dist_map_img);cv::waitKey();
			}

			// STEP 【8.生成立方体】
			MatrixXd all_configs_error_one_objH(200,9);   		// 误差.
			MatrixXd all_box_corners_2d_one_objH(400,8); 		// 2D坐标
			int valid_config_number_one_objH = 0;				// 有效测量次数

			// STEP 【8.1 采样相机的 roll pitch 角】
		  	// 采样相机 roll pitch 角，在相机偏角的+-6°每隔3°采样一个值 或者 直接使用相机的roll pitch角.
	      	std::vector<double> cam_roll_samples;
			std::vector<double> cam_pitch_samples;
			if (whether_sample_cam_roll_pitch)
			{
				linespace<double>(cam_pose_raw.euler_angle(0)-6.0/180.0*M_PI, cam_pose_raw.euler_angle(0)+6.0/180.0*M_PI, 3.0/180.0*M_PI, cam_roll_samples);
				linespace<double>(cam_pose_raw.euler_angle(1)-6.0/180.0*M_PI, cam_pose_raw.euler_angle(1)+6.0/180.0*M_PI, 3.0/180.0*M_PI, cam_pitch_samples);
			}
			else
			{
				cam_roll_samples.push_back(cam_pose_raw.euler_angle(0));
				cam_pitch_samples.push_back(cam_pose_raw.euler_angle(1));
			}

			// different from matlab. first for loop yaw, then for configurations.
			// int obj_yaw_id = 8;
			// 分别遍历采样的 roll，pitch 和 yaw 角.
			for (int cam_roll_id = 0; cam_roll_id < cam_roll_samples.size(); cam_roll_id++)
			for (int cam_pitch_id = 0; cam_pitch_id < cam_pitch_samples.size(); cam_pitch_id++)
			for (int obj_yaw_id = 0; obj_yaw_id < obj_yaw_samples.size(); obj_yaw_id++)
			{
				std::cout << "第 " << cam_roll_id << " 个相机 roll 采样角" << std::endl;
				std::cout << "第 " << cam_pitch_id << " 个相机 pitch 采样角" << std::endl;
				std::cout << "第 " << obj_yaw_id << " 个物体 yaw 采样角" << std::endl;

				if (whether_sample_cam_roll_pitch)
				{
					Matrix4d transToWolrd_new = transToWolrd;
					// 将当前角度值转换成旋转矩阵.
					// NOTE 这里yaw为什么不用采样的值而用相机的值？：yaw 采样是真对物体的，并没有对相机的 yaw 值进行采样。
					transToWolrd_new.topLeftCorner<3,3>() = euler_zyx_to_rot<double>(cam_roll_samples[cam_roll_id], cam_pitch_samples[cam_pitch_id], cam_pose_raw.euler_angle(2));
					set_cam_pose(transToWolrd_new);
					// TODO 平面？？
					ground_plane_sensor = cam_pose.transToWolrd.transpose() * ground_plane_world;
				}

				// 读取采样的物体偏航角.
				double obj_yaw_esti = obj_yaw_samples[obj_yaw_id];

				// STEP 【8.2 计算三个消失点】
				// @PARAM	vp_1, vp_2, vp_3	三个消失点.
				Vector2d vp_1, vp_2, vp_3;
				getVanishingPoints(cam_pose.KinvR, obj_yaw_esti, vp_1, vp_2, vp_3); // for object x y z  axis
				// std::cout << "vp_1:\n" << vp_1 << std::endl;
				// std::cout << "vp_2:\n" << vp_2 << std::endl;
				// std::cout << "vp_3:\n" << vp_3 << std::endl;
				// circle(image_point, cv::Point(vp_1(0)/5, vp_1(1))/5, 3, cv::Scalar(255,0,0),-1,8,0);

				// @PARAM all_vps(3,2) 存储三个消失点.
				MatrixXd all_vps(3,2);
				all_vps.row(0) = vp_1;
				all_vps.row(1) = vp_2;
				all_vps.row(2) = vp_3;

				// 输出采样的偏航角.
				// std::cout<<"obj_yaw_esti  " << obj_yaw_esti << "  " << obj_yaw_id << std::endl;

				// STEP 【8.3 寻找形成消失点的两条边】
				MatrixXd all_vp_bound_edge_angles = VP_support_edge_infos(	all_vps, 				/* 消失点矩阵 3*2 */
																			edge_mid_pts,			/* 每条线段的中点 n×2 */
																			lines_inobj_angles,		/* 每条线段的偏角 n×1 */
																			Vector2d(vp12_edge_angle_thre, vp3_edge_angle_thre));	/* 消失点与各边的夹角阈值*/
				// int sample_top_pt_id=15;
				// STEP 【8.4.遍历上边缘的采样点，得到 8 个点的 2D 坐标.】
				for (int sample_top_pt_id = 0; sample_top_pt_id < sample_top_pts.cols(); sample_top_pt_id++)
				{
					std::cout << "第 " << sample_top_pt_id << " 个上边缘采样点" << std::endl;
					// std::cout << "sample_top_pt_id " << sample_top_pt_id << std::endl;
					// STEP 【8.4.1 采样得到立方体上边缘的第一个点.】
					// @PARAM corner_1_top 当前采样点.
					Vector2d corner_1_top = sample_top_pts.col(sample_top_pt_id);
					bool config_good = true;

					// cv::putText(rgb_img,
					// 			"1",
					// 			cv::Point(corner_1_top(0), corner_1_top(1)),
					// 			2,      // fontFace
					// 			0.5,    // fontScale
					// 			cv::Scalar(0, 255, 0),
					// 			1);     // 粗细

					// @PARAM vp_1_position	消失点1的位置，1 是左边，2 是右边.
					int vp_1_position = 0;  // 0 initial as fail,  1  on left   2 on right

					// STEP 【8.4.2 计算立方体上边缘的第二个点.】
					// NOTE 检查【消失点1-上边缘采样点的射线是否与右边边界有交集】.
					// @PARAM corner_2_top 消失点1-上边缘采样点射线与边界框左右边界的交点.
					Vector2d corner_2_top = seg_hit_boundary(	vp_1,				/* 消失点 */
																corner_1_top,		/* 上边缘的采样点 */
																Vector4d(right_x_raw, top_y_raw, right_x_raw, down_y_expan) /* 右边边缘 */);
					// NOTE 如果消失点1-上边缘采样点的连线与右边边缘没有交集，再检查【是否与左边边缘有交集】.
					if (corner_2_top(0) == -1)
					{  	// vp1-corner1 doesn't hit the right boundary. check whether hit left
						corner_2_top = seg_hit_boundary(	vp_1,
															corner_1_top,
															Vector4d(left_x_raw, top_y_raw, left_x_raw, down_y_expan));
						// 如果与左边边界有交集，说明消失点在右边（2）
						if (corner_2_top(0) != -1) // vp1-corner1 hit the left boundary   vp1 on the right
							vp_1_position = 2;
					}
					// 如果与右边边界有交集，说明消失点在左边（1）
					else    // vp1-corner1 hit the right boundary   vp1 on the left
						vp_1_position = 1;

			  		// 检查消失点与采样点配置.
					config_good = vp_1_position > 0;
					if (!config_good)
					{
						if (print_details)
							printf("Configuration fails at corner 2, outside segment\n");
						continue;
					}
					// 上边缘采样点与边界上的交集点（消失点1）的距离
					if ((corner_1_top - corner_2_top).norm() < shorted_edge_thre)
					{
						if (print_details)
							printf("Configuration fails at edge 1-2, too short\n");
						continue;
					}

					// 输出立方体上边缘第一二个点的位置.
					// cout << "上边缘采样点1和交点corner_1/2   " << corner_1_top.transpose() << "   " << corner_2_top.transpose() << endl;

					// int config_ind = 0; // have to consider config now.
					for (int config_id = 1; config_id < 3; config_id++)  // configuration one or two of matlab version
					{
						if (!all_configs[config_id-1])
							continue;

						// @PARAM corner_4_top	消失点2-上边缘采样点射线与左右边界的交点.
						// NOTE 代码中的 3 4 与论文中相反，代码中的第三个点对应论文中第四个点.
						Vector2d corner_3_top, corner_4_top;

						// STEP 【8.4.3 第一种情形下的顶部第 3, 4 个点】
						// NOTE 第一种情形，可以观察到物体的三个面.
						if (config_id == 1)
						{
							// STEP 【计算消失点 2 与左右边界的交点(第三个点).】
							// 如果消失点 1 在左边，则消失点vp2在右边，与左侧边界有交点 corner_4_top
							if (vp_1_position == 1)   // then vp2 hit the left boundary
								corner_4_top = seg_hit_boundary(vp_2, corner_1_top, Vector4d(left_x_raw, top_y_raw, left_x_raw, down_y_expan));
							// 如果消失点 1 在右边，则消失点vp2在左边，与右侧边界有交点 corner_4_top
							else  // or, then vp2 hit the right boundary
								corner_4_top = seg_hit_boundary(vp_2, corner_1_top, Vector4d(right_x_raw, top_y_raw, right_x_raw, down_y_expan));

							// 如果没有交点.
							if (corner_4_top(1) == -1)	// TODO 这里用的y坐标 corner_4_top(1)，前面用的x坐标corner_2_top(0)？？
							{
								config_good = false;
								if (print_details)
									printf("Configuration %d fails at corner 4, outside segment\n",config_id);
								continue;
							}

							// 上边缘采样点与边界上的交集点（消失点2）的距离
							if ((corner_1_top - corner_4_top).norm() < shorted_edge_thre)
							{
								if (print_details)
									printf("Configuration %d fails at edge 1-4, too short\n",config_id);
								continue;
							}

							// compute the last point in the top face
							// STEP 【计算立方体上边缘第四个点（最后一个点）】
							corner_3_top = lineSegmentIntersect(vp_2, corner_2_top, vp_1, corner_4_top, true);

							// 检查.
							if (!check_inside_box( corner_3_top, Vector2d(left_x_raw,top_y_raw), Vector2d(right_x_raw, down_y_expan)))
							{    // check inside boundary. otherwise edge visibility might be wrong
								config_good = false;
								if (print_details)
									printf("Configuration %d fails at corner 3, outside box\n",config_id);
								continue;
							}
							if ( ((corner_3_top-corner_4_top).norm()<shorted_edge_thre) || ((corner_3_top-corner_2_top).norm()<shorted_edge_thre) )
							{
								if (print_details)
									printf("Configuration %d fails at edge 3-4/3-2, too short\n",config_id);
								continue;
							}

							// 输出立方体上边缘第三四个点的位置.
							//cout<<"corner_3/4   "<<corner_3_top.transpose()<<"   "<<corner_4_top.transpose()<<endl;
						}

						// STEP 【8.4.4 第二种情形下的顶部第 3, 4 个点】
						// NOTE 第二种情形，可以观察到物体的两个面.
						if (config_id==2)
						{
							// STEP 计算消失点2——顶点2与左侧边界的交点（第三个点）.
							// 如果消失点1在左侧，则其交点在右侧
							if (vp_1_position==1)   // then vp2 hit the left boundary
								corner_3_top = seg_hit_boundary(vp_2, corner_2_top, Vector4d(left_x_raw, top_y_raw, left_x_raw, down_y_expan));
							else  // or, then vp2 hit the right boundary
								corner_3_top = seg_hit_boundary(vp_2, corner_2_top, Vector4d(right_x_raw, top_y_raw, right_x_raw, down_y_expan));

							// 检查.
							if (corner_3_top(1)==-1)
							{
								config_good = false;
								if (print_details)
									printf("Configuration %d fails at corner 3, outside segment\n",config_id);
								continue;
							}
							if ((corner_2_top-corner_3_top).norm()<shorted_edge_thre)
							{
								if (print_details)
									printf("Configuration %d fails at edge 2-3, too short\n",config_id);
								continue;
							}

							// STEP 计算立方体上边缘第四个点（最后一个点）
							corner_4_top = lineSegmentIntersect(vp_1, corner_3_top, vp_2, corner_1_top, true);

							// 检查.
							if (!check_inside_box( corner_4_top, Vector2d(left_x_raw,top_y_expan_distmap), Vector2d(right_x_raw, down_y_expan_distmap)))
							{
								config_good=false;
								if (print_details)
									printf("Configuration %d fails at corner 4, outside box\n",config_id);
								continue;
							}
							if ( ((corner_3_top-corner_4_top).norm()<shorted_edge_thre) || ((corner_4_top-corner_1_top).norm()<shorted_edge_thre) )
							{
								if (print_details)
									printf("Configuration %d fails at edge 3-4/4-1, too short\n",config_id);
								continue;
							}

							// 输出情形2中立方体上边缘第三四个点的位置.
							// cout<<"corner_3/4   "<<corner_3_top.transpose()<<"   "<<corner_4_top.transpose()<<endl;
						}

						// compute first bottom points    computing bottom points is the same for config 1,2
						// NOTE 计算底部点（情形 1 和 2 的计算方法一样）
						// STEP 【8.5 计算第五个点	vp3—4线段与边界框下边界的交点】
						Vector2d corner_5_down = seg_hit_boundary(vp_3, corner_3_top, Vector4d(left_x_raw, down_y_expan, right_x_raw, down_y_expan));
						// 检查.
						if (corner_5_down(1)==-1){
							config_good = false;
							if (print_details) printf("Configuration %d fails at corner 5, outside segment\n",config_id);
							continue;
						}
						if ((corner_3_top-corner_5_down).norm()<shorted_edge_thre){
							if (print_details) printf("Configuration %d fails at edge 3-5, too short\n",config_id);
							continue;
						}

						// STEP 【8.6 计算第 6 个点	vp2—5线段与vp3—2的交点.】
						Vector2d corner_6_down = lineSegmentIntersect(vp_2, corner_5_down, vp_3, corner_2_top, true);
						if (!check_inside_box( corner_6_down, expan_distmap_lefttop, expan_distmap_rightbottom)){
							config_good=false;
							if (print_details) printf("Configuration %d fails at corner 6, outside box\n",config_id);
							continue;
						}
						if ( ((corner_6_down-corner_2_top).norm()<shorted_edge_thre) || ((corner_6_down-corner_5_down).norm()<shorted_edge_thre) ){
							if (print_details) printf("Configuration %d fails at edge 6-5/6-2, too short\n",config_id);
							continue;
						}

						// STEP 【8.7 计算第 7 个点	vp1—6线段与vp3—1的交点.】
						Vector2d corner_7_down=lineSegmentIntersect(vp_1, corner_6_down, vp_3, corner_1_top, true);
						if (!check_inside_box( corner_7_down, expan_distmap_lefttop, expan_distmap_rightbottom)){// might be slightly different from matlab
							config_good=false;
							if (print_details) printf("Configuration %d fails at corner 7, outside box\n",config_id);
							continue;
						}
						if ( ((corner_7_down-corner_1_top).norm()<shorted_edge_thre) || ((corner_7_down-corner_6_down).norm()<shorted_edge_thre) ){
							if (print_details) printf("Configuration %d fails at edge 7-1/7-6, too short\n",config_id);
							continue;
						}

						// STEP 【8.8 计算第 8 个点	vp1—5线段与vp5—7的交点.】
						Vector2d corner_8_down=lineSegmentIntersect(vp_1,corner_5_down,vp_2, corner_7_down,true);
						if (!check_inside_box( corner_8_down, expan_distmap_lefttop, expan_distmap_rightbottom)){
							config_good=false;
							if (print_details) printf("Configuration %d fails at corner 8, outside box\n",config_id);
							continue;
						}
						if ( ((corner_8_down-corner_4_top).norm()<shorted_edge_thre) || ((corner_8_down-corner_5_down).norm()<shorted_edge_thre) || ((corner_8_down-corner_7_down).norm()<shorted_edge_thre)){
							if (print_details) printf("Configuration %d fails at edge 8-4/8-5/8-7, too short\n",config_id);
							continue;
						}

						// @PARAM box_corners_2d_float	存储物体的 8 个顶点（2D）.
						MatrixXd box_corners_2d_float(2,8);
						box_corners_2d_float << 	corner_1_top, corner_2_top, corner_3_top, corner_4_top,
													corner_5_down, corner_6_down, corner_7_down, corner_8_down;
						// std::cout<<"box_corners_2d_float \n "<<box_corners_2d_float<<std::endl;

						// @PARAM box_corners_2d_float_shift 8 个顶点x坐标距离左边边界框边界的距离和 y坐标距离上边边界的距离（以检测框左上边界为轴的坐标）.
						MatrixXd box_corners_2d_float_shift(2,8);
						box_corners_2d_float_shift.row(0) = box_corners_2d_float.row(0).array() - left_x_expan_distmap;
						box_corners_2d_float_shift.row(1) = box_corners_2d_float.row(1).array() - top_y_expan_distmap;

						MatrixXi visible_edge_pt_ids, vps_box_edge_pt_ids;
						double sum_dist;

						// STEP 【9.1 距离误差计算】，第一种情形.
						if (config_id == 1)
						{
							// 可以看到三个面，共9条边.
							visible_edge_pt_ids.resize(9,2);
							visible_edge_pt_ids << 1,2, 2,3, 3,4, 4,1, 2,6, 3,5, 4,8, 5,8, 5,6;

							// 计算三个消失点所需要的两条边
							vps_box_edge_pt_ids.resize(3,4);
							// 1_2 与 8_5 交点得到vp1	4_1 与 5_6 交点得到vp1
							vps_box_edge_pt_ids << 1,2,8,5, 4,1,5,6, 4,8,2,6; // six edges. each row represents two edges [e1_1 e1_2   e2_1 e2_2;...] of one VP

							visible_edge_pt_ids.array() -=1;
							vps_box_edge_pt_ids.array() -=1; //change to c++ index

							// TODO  计算距离	dist_map？？
							sum_dist = box_edge_sum_dists(dist_map, box_corners_2d_float_shift, visible_edge_pt_ids);
						}
						// 第二种情况.
						else
						{
							visible_edge_pt_ids.resize(7,2);
							visible_edge_pt_ids<<1,2, 2,3, 3,4, 4,1, 2,6, 3,5, 5,6;
							vps_box_edge_pt_ids.resize(3,4);
							vps_box_edge_pt_ids<<1,2,3,4, 4,1,5,6, 3,5,2,6; // six edges. each row represents two edges [e1_1 e1_2   e2_1 e2_2;...] of one VP
							visible_edge_pt_ids.array() -=1;
							vps_box_edge_pt_ids.array() -=1;
							sum_dist = box_edge_sum_dists(dist_map, box_corners_2d_float_shift, visible_edge_pt_ids, reweight_edge_distance);
						}

						// STEP 【9.2 计算角度误差.】
						double total_angle_diff = box_edge_alignment_angle_error(	all_vp_bound_edge_angles,	/* 消失点与边的两个角度 */
																					vps_box_edge_pt_ids,		/* 每个消失点来源的两条边 */
																					box_corners_2d_float);		/* 8 个顶点的 2D坐标 */

						// @PARAM 	all_configs_error_one_objH	存储所有的误差.
						all_configs_error_one_objH.row(valid_config_number_one_objH).head<4>() = Vector4d(	config_id, 			/* 模式 */
																											vp_1_position, 		/* vp1的位置 */
																											obj_yaw_esti, 		/* 偏航角采样 */
																											sample_top_pt_id);	/* 上边缘采样点 */
						all_configs_error_one_objH.row(valid_config_number_one_objH).segment<3>(4) = Vector3d(	sum_dist/obj_diaglength_expan, 	/* 平均距离误差？ */
																												total_angle_diff, 				/* 角度误差 */
																												down_expand_sample);			/* 高度？受是否采样了高度影响？ */
						// 是否采样相机的 roll 和 pitch 角.
						if (whether_sample_cam_roll_pitch)
							all_configs_error_one_objH.row(valid_config_number_one_objH).segment<2>(7) = Vector2d(	cam_roll_samples[cam_roll_id],		/* 采样相机 roll 角 */
																													cam_pitch_samples[cam_pitch_id]);	/* 采样相机 pitch 角 */
						else
							all_configs_error_one_objH.row(valid_config_number_one_objH).segment<2>(7) = Vector2d(	cam_pose_raw.euler_angle(0),
																													cam_pose_raw.euler_angle(1));

						// TODO 所有情况下的物体的八个顶点的 2D坐标？
						all_box_corners_2d_one_objH.block(2*valid_config_number_one_objH,0,2,8) = box_corners_2d_float;

						// 有效的测量次数？？
						valid_config_number_one_objH++;

						if (valid_config_number_one_objH >= all_configs_error_one_objH.rows())
						{
							all_configs_error_one_objH.conservativeResize(2*valid_config_number_one_objH,NoChange);
							all_box_corners_2d_one_objH.conservativeResize(4*valid_config_number_one_objH,NoChange);
						}
		      		} //end of config loop	两种情形.
		  		} //end of top id	上边缘采样点.
	      	} //end of yaw	采样的yaw角.

			// std::cout << "valid_config_number_one_hseight  " << valid_config_number_one_objH << std::endl;
			// std::cout << "all_configs_error_one_objH  \n" << all_configs_error_one_objH.topRows(valid_config_number_one_objH) << std::endl;
			// MatrixXd all_corners = all_box_corners_2d_one_objH.topRows(2*valid_config_number_one_objH);
			// std::cout<<"all corners   "<<all_corners<<std::endl;

			// STEP 计算距离-角度的综合得分：normalized_score，和有效的提案ID：good_proposal_ids.
			VectorXd normalized_score;
			vector<int> good_proposal_ids;
			fuse_normalize_scores_v2(	all_configs_error_one_objH.col(4).head(valid_config_number_one_objH), 	/* 距离误差 */
										all_configs_error_one_objH.col(5).head(valid_config_number_one_objH),	/* 角度误差 */
										normalized_score, 				/* 综合得分 */
										good_proposal_ids, 				/* 最终纳入计算的测量的ID */
										weight_vp_angle,				/* 角度误差的权重 */
										whether_normalize_two_errors);	/* 是否归一化两个误差 */

			// 遍历所有有效的提案.
			for (int box_id = 0; box_id < good_proposal_ids.size(); box_id++)
			{
				int raw_cube_ind = good_proposal_ids[box_id];
				// 对相机的 roll 和 pitch 角进行采样.
				if (whether_sample_cam_roll_pitch)
				{
					Matrix4d transToWolrd_new = transToWolrd;
					// 将当前旋转转换成角度值.
					transToWolrd_new.topLeftCorner<3,3>() = euler_zyx_to_rot<double>(all_configs_error_one_objH(raw_cube_ind,7),
																					 all_configs_error_one_objH(raw_cube_ind,8),
																					 cam_pose_raw.euler_angle(2));
					set_cam_pose(transToWolrd_new);
					// 相机系下的地平面
					ground_plane_sensor = cam_pose.transToWolrd.transpose()*ground_plane_world;
				}

				// NOTE 计算立方体对象sample_obj.
		  		cuboid* sample_obj = new cuboid();
		  		change_2d_corner_to_3d_object(	all_box_corners_2d_one_objH.block(2*raw_cube_ind,0,2,8), /* 8 个点的 2D 坐标*/
				  								all_configs_error_one_objH.row(raw_cube_ind).head<3>(),	 /* 模式，vp1的位置，偏航角*/
												ground_plane_sensor,  					/* 法平面？*/
												cam_pose.transToWolrd, 					/* 相机旋转 */
												cam_pose.invK, 							/* 相机内参的逆矩阵 */
												cam_pose.projectionMatrix,				/* 投影矩阵 */
												*sample_obj);							/* 3D提案 */
				// 输出提案的具体信息.
				// sample_obj->print_cuboid();
				/*
				printing cuboids info....
				【pos】     -1.58339 0.373187 0.300602
				【scale】   0.155737 0.436576 0.300602
				【rotY】    -2.90009
				【box_config_type】   1  1
				【box_corners_2d】
				503 279 213 430 559 261 174 459
				245 396 319 200  56 184 116  23
				【box_corners_3d_world】
				-1.6302   -1.83902   -1.53659   -1.32776    -1.6302   -1.83902   -1.53659   -1.32776
				-0.087966   0.759848    0.83434 -0.0134734  -0.087966   0.759848    0.83434 -0.0134734
						0          0          0          0   0.601204   0.601204   0.601204   0.601204
				*/

				// 保证尺度为正.
				if ((sample_obj->scale.array() < 0).any())
					continue;                        // scale should be positive

				// STEP 存储对象的信息.
				// 2D 检测框.
				sample_obj->rect_detect_2d =  Vector4d(left_x_raw, top_y_raw, obj_width_raw, obj_height_raw);
				// 边缘误差.
				sample_obj->edge_distance_error = all_configs_error_one_objH(raw_cube_ind,4); // record the original error
				// 角度误差.
				sample_obj->edge_angle_error = all_configs_error_one_objH(raw_cube_ind,5);
				// 归一化误差.
				sample_obj->normalized_error = normalized_score(box_id);
				// NOTE 歪斜比：长/宽.
				// head(2).maxCoeff()：尺度的前两项xy中较大者：长
				// head(2).minCoeff()：尺度的前两项xy中较小者：宽
				double skew_ratio = sample_obj->scale.head(2).maxCoeff()/sample_obj->scale.head(2).minCoeff();
				sample_obj->skew_ratio = skew_ratio;
				// 是否对高度进行了采样？ 0,10,20.
				sample_obj->down_expand_height = all_configs_error_one_objH(raw_cube_ind,6);

				// 如果对相机的 roll 和 pitch 角进行采样.用采样得到的角-相机的原始角度.（否则直接使用相机的角度，角度差为0）
				// 保存 roll 和 pitch 角的【角度差】.
				if (whether_sample_cam_roll_pitch)
				{
					sample_obj->camera_roll_delta = all_configs_error_one_objH(raw_cube_ind,7) - cam_pose_raw.euler_angle(0);
					sample_obj->camera_pitch_delta = all_configs_error_one_objH(raw_cube_ind,8) - cam_pose_raw.euler_angle(1);
				}
				else
				{
					sample_obj->camera_roll_delta = 0;
					sample_obj->camera_pitch_delta = 0; }
					raw_obj_proposals.push_back(sample_obj);
				}
	  		} // end of differnet object height sampling

			// STEP 最后对所有提案进行排名（归一化误差+歪斜比）
			// %finally rank all proposals. [normalized_error   skew_error]
			// 准确提案的数量 1 ？0？
			int actual_cuboid_num_small = std::min(max_cuboid_num, (int)raw_obj_proposals.size());

			VectorXd all_combined_score(raw_obj_proposals.size());

			// 计算总体误差
			for (int box_id = 0; box_id < raw_obj_proposals.size(); box_id++)
			{
				cuboid* sample_obj = raw_obj_proposals[box_id];
				// 计算歪斜比误差 skew_ratio - 1，要求大于0
				double skew_error = weight_skew_error*std::max(sample_obj->skew_ratio - nominal_skew_ratio,0.0);

				// TODO 若大于最大歪斜比 3 ，则误差为100
				if (sample_obj->skew_ratio > max_cut_skew)
					skew_error = 100;

				// NOTE 新的误差：距离+角度+歪斜比误差
				double new_combined_error = sample_obj->normalized_error + weight_skew_error*skew_error;	// TODO 这里还乘了一个权重？前面也乘了.
				all_combined_score(box_id) = new_combined_error;
			}

	  		// STEP 保留误差最小的提案.
	  		// 提案的索引（ID）并从 0 开始递增赋值.
			std::vector<int> sort_idx_small(all_combined_score.rows());
			iota(sort_idx_small.begin(), sort_idx_small.end(), 0);

			// 对 all_combined_score 的前 actual_cuboid_num_small（1） 项进行递增.
			sort_indexes(	all_combined_score, 		/* 检索比较的序列 */
							sort_idx_small,				/* 按照all_combined_score中误差从小到达排序误差对应的ID */
							actual_cuboid_num_small);

			for (int ii = 0; ii < actual_cuboid_num_small; ii++) // use sorted index
			{
				all_object_cuboids[object_id].push_back(raw_obj_proposals[sort_idx_small[ii]]);
				//std::cout << "sort_idx_small[ii]：" << sort_idx_small[ii] << std::endl;
			}
			ca::Profiler::tictoc("One 3D object total time");
	}// end of different objects

		// STEP 绘制最终图案，保存结果.
		if (whether_plot_final_images || whether_save_final_images)
		{
			cv::Mat frame_all_cubes_img = rgb_img.clone();
			for (int object_id = 0; object_id < all_object_cuboids.size(); object_id++)
			{	if ( all_object_cuboids[object_id].size()>0 )
				{
					// NOTE 绘制提案.
					plot_image_with_cuboid(frame_all_cubes_img, all_object_cuboids[object_id][0]);
				}
			}
			if (whether_save_final_images)
				cuboids_2d_img = frame_all_cubes_img;
			if (whether_plot_final_images)
			{
				cv::imshow("frame_all_cubes_img", frame_all_cubes_img);
				cv::waitKey(0);
			}
		}
}


// Copyright (c) 2014 The Bitcoin Core developers
// Copyright (c) 2014-2015 The Dash developers
// Copyright (c) 2015-2017 The PIVX developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "primitives/transaction.h"
#include "main.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(main_tests)

CAmount nMoneySupplyPoWEnd = 5200003 * COIN;

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    CAmount nSum = 0;
    for (int nHeight = 0; nHeight < 1; nHeight += 1) {
        /* premine in block 1 (5,200,000 LNI) */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 17500000 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 1; nHeight < 3000; nHeight += 1) {
        /* PoW end (5,200,003 LNI) */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 5200003 * COIN);
        nSum += nSubsidy;
    }

    BOOST_CHECK(nSum > 0 && nSum <= nMoneySupplyPoWEnd);

    for (int nHeight = 3000; nHeight < 30000; nHeight += 1) {
        /* PoS Phase 1 (swap period) */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 0.001 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 30001; nHeight <= 555602; nHeight += 1) {
        /* PoS Phase 2 */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 10 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 555603; nHeight <= 1081203; nHeight += 1) {
        /* PoS Phase 3 */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 9 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 1081204; nHeight <= 1606804; nHeight += 1) {
        /* PoS Phase 4 */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 8 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 1606805; nHeight <= 2132405; nHeight += 1) {
        /* PoS Phase 5 */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 7 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 2132406; nHeight <= 2658006; nHeight += 1) {
        /* PoS Phase 6 */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 6 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 2658007; nHeight <= 3183607; nHeight += 1) {
        /* PoS Phase 7 */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 5 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 3183608; nHeight <= 3709208; nHeight += 1) {
        /* PoS Phase 5 */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 4 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 3709209; nHeight <= 4000000; nHeight += 1) {
        /* PoS Phase 6 (Finale phase) */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 3 * COIN);
        nSum += nSubsidy;
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include <vector>
#include <list>
#include <map>
#include <set>

#include <queue>
#include <deque>
#include <stack>
#include <bitset>
#include <algorithm>
#include <functional>
#include <numeric>
#include <utility>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <cstdio>
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <cstring>
#include <assert.h>
#include <boost/lexical_cast.hpp>

#define INF 1023123123
#define EPS 1e-11
#define LSOne(S) (S & (-S))

#define FORN(X,Y) for (int (X) = 0;(X) < (Y);++(X))
#define FORB(X,Y) for (int (X) = (Y);(X) >= 0;--(X))
#define REP(X,Y,Z) for (int (X) = (Y);(X) < (Z);++(X))
#define REPB(X,Y,Z) for (int (X) = (Y);(X) >= (Z);--(X))

#define SZ(Z) ((int)(Z).size())
#define ALL(W) (W).begin(), (W).end()
#define PB push_back

#define MP make_pair
#define A first
#define B second

#define FORIT(X,Y) for(typeof((Y).begin()) X = (Y).begin();X!=(Y).end();X++)

using namespace std;

typedef long long ll;
typedef double db;
typedef vector<int> vi;
typedef pair<int, int> ii;
typedef vector<ii> vii;

void print_case(int i)
{
   cout << "Case #" << i + 1 << ": ";
}

double best(vector<double>& mem, vi& dist, vi& h_speeds, vi& h_dists, int horse, int city)
{
   if (city == dist.size())
      return 0;

   h_dists[city] -= dist[city];
   double o1 = mem[city];
   if (o1 <= 0)
      o1 = mem[city] = dist[city] / (double)h_speeds[city] + best(mem, dist, h_speeds, h_dists, city, city + 1);
   double o2 = 10e20;
   h_dists[city] += dist[city];
   if (h_dists[horse] > dist[city])
   {
      h_dists[horse] -= dist[city];
      o2 = dist[city] / (double)h_speeds[horse] + best(mem, dist, h_speeds, h_dists, horse, city + 1);
      h_dists[horse] += dist[city];
   }
   return min(o1, o2);
}

int main()
{
   cout.precision(12);
   int ntc;
   cin >> ntc;

   FORN(kk, ntc)
   {
      print_case(kk);
      int n, q;
      cin >> n >> q;
      vi speeds(n), distances(n);
      FORN(i, n)
      {
         cin >> distances[i] >> speeds[i];
      }
      vector<vector<ll>> mat(n, vector<ll>(n));
      FORN(i, n)
      {
         FORN(j, n)
         {
            cin >> mat[i][j];
            if (mat[i][j] == -1)
               mat[i][j] = (ll)10e13;
         }
      }
      vector<ii> qs(q);
      FORN(i, q)
      {
         cin >> qs[i].first >> qs[i].second;
      }

      FORN(k, n) FORN(i, n)
      {
         FORN(j, n)
         {
            mat[i][j] = min(mat[i][j], mat[i][k] + mat[k][j]);
         }
      }
      FORN(i, n) FORN(j, n)
      {
         if (mat[i][j] > distances[i])
            mat[i][j] = -1;
      }
      vector<vector<double>> mat2(n, vector<double>(n, -1));
      FORN(i, n) FORN(j, n)
      {
         mat2[i][j] = mat[i][j] != -1 ? mat[i][j] / (double)speeds[i] : 10e15;
      }

      FORN(k, n) FORN(i, n)
      {
         FORN(j, n)
         {
            mat2[i][j] = min(mat2[i][j], mat2[i][k] + mat2[k][j]);
         }
      }
      FORN(i, q)
      {
         double time = mat2[qs[i].first - 1][qs[i].second - 1];
         cout << time << ' ';
      }
      cout << endl;
   }
}


// __BEGIN_LICENSE__
// Copyright (C) 2006-2010 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

//
#include <vw/Camera/CAHVModel.h>
#include <boost/algorithm/string.hpp>

namespace vw {
namespace camera {

  // FIXME -- Double check anything related to PinholeModel
  CAHVModel CAHVModel::operator= (PinholeModel const& pin_model) {

    //  Pinhole model parameters (in pixel units)
    double fH, fV, Hc, Vc;
    pin_model.intrinsic_parameters(fH, fV, Hc, Vc);

    //  Unit vectors defining camera coordinate frame
    Vector3 u,v,w;
    pin_model.coordinate_frame(u,v,w);

    //  The true rotation between world and camera coordinate
    //  frames includes the rotation R --AND-- a rotation from
    //  specifying the directions of increasing u,v,w pixels
    Matrix<double,3,3> R = pin_model.camera_pose().rotation_matrix();

    //  Now create the components of the CAHV model...
    Vector3 Hvec = R*u;
    Vector3 Vvec = R*v;

    C = pin_model.camera_center();
    A = R*w;
    H = fH*Hvec + Hc*A;
    V = fV*Vvec + Vc*A;

    return *this;
  }

  /// This constructor takes a filename and reads in a camera model
  /// from the file.  The file may contain either CAHV parameters or
  /// pinhole camera parameters.
  CAHVModel::CAHVModel(std::string const& filename) {
    if (filename.empty())
      vw_throw( IOErr() << "CAHVModel: null file name passed to constructor." );

    if (boost::ends_with(filename, ".cahv"))
      read_cahv(filename);
    else if (boost::ends_with(filename, ".pin"))
      read_pinhole(filename);
    else
      vw_throw( IOErr() << "CAHVModel: Unknown camera file suffix." );
  }

  Vector2 CAHVModel::point_to_pixel(Vector3 const& point) const {
    double dDot = dot_prod(point-C, A);
    return Vector2( dot_prod(point-C, H) / dDot,
                    dot_prod(point-C, V) / dDot );
  }

  Vector3 CAHVModel::pixel_to_vector(Vector2 const& pix) const {
    Vector3 va, vb;

    // Vertical component in a plane perpendicular to the vector
    va = V + (-(pix.y()) * A);

    // Horizontal component in a plane perpendicular to the vector
    vb = H + (-(pix.x()) * A);

    // Find vector
    Vector3 vec = cross_prod(va, vb);

    // Normalize vector
    vec *= 1.0 / norm_2(vec);

    // The vector VxH should be pointing in the same directions as A,
    // if it isn't (because we have a left handed system), flip the
    // vector.
    Vector3 temp = cross_prod(V, H);
    if (dot_prod(temp, A) < 0.0){
      vec *= -1.0;
      //cout << "CAHV PixelToVector changed sign of vec" << endl;
    }
    return vec;
  }

  // --------------------------------------------------
  //                 Private Methods
  // --------------------------------------------------
  void CAHVModel::read_cahv(std::string const& filename) {

    FILE *cahvFP = fopen(filename.c_str(), "r");
    if (cahvFP == 0)
      vw_throw( IOErr() << "CAHVModel::read_cahv: Could not open file\n" );

    char line[4096];

    // Scan through comments
    fgets(line, sizeof(line), cahvFP);
    while(line[0] == '#')
      fgets(line, sizeof(line), cahvFP);

    if (sscanf(line,"C = %lf %lf %lf", &C(0), &C(1), &C(2)) != 3) {
      vw_throw( IOErr() << "CAHVModel::read_CAHV: Could not read C vector\n" );
      fclose(cahvFP);
    }

    fgets(line, sizeof(line), cahvFP);
    if (sscanf(line,"A = %lf %lf %lf", &A(0),&A(1), &A(2)) != 3) {
      vw_throw( IOErr() << "CAHVModel::read_CAHV: Could not read A vector\n" );
      fclose(cahvFP);
    }

    fgets(line, sizeof(line), cahvFP);
    if (sscanf(line,"H = %lf %lf %lf", &H(0), &H(1), &H(2)) != 3) {
      vw_throw( IOErr() << "CAHVModel::read_CAHV: Could not read H vector\n" );
      fclose(cahvFP);
    }

    fgets(line, sizeof(line), cahvFP);
    if (sscanf(line,"V = %lf %lf %lf", &V(0), &V(1), &V(2)) != 3) {
      vw_throw( IOErr() << "CAHVModel::read_CAHV: Could not read V vector\n" );
      fclose(cahvFP);
    }

    fclose(cahvFP);
  }

  void CAHVModel::read_pinhole(std::string const& filename) {
    FILE *camFP = fopen(filename.c_str(), "r");

    if (camFP == 0)
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not open file\n" );

    char line[2048];
    double f, fH, fV, Hc, Vc;
    Vector2 pixelSize;
    Vector3 Hvec, Vvec;

    // Read intrinsic parameters
    fgets(line, sizeof(line), camFP);
    if (sscanf(line,"f = %lf", &f) != 1) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read focal length\n" );
      fclose(camFP);
    }

    fgets(line, sizeof(line), camFP);
    if (sscanf(line,"SP = %lf %lf", &pixelSize.x(), &pixelSize.y()) != 2) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read pixel size\n" );
      fclose(camFP);
    }

    fgets(line, sizeof(line), camFP);
    if (sscanf(line,"IC = %lf %lf", &Hc, &Vc) != 2) {
      vw_throw( IOErr() << "CAHVModel::ReadPinhole: Could not read image center pos\n" );
      fclose(camFP);
    }

    // Read extrinsic parameters
    fgets(line, sizeof(line), camFP);
    if (sscanf(line,"C = %lf %lf %lf", &C(0), &C(1), &C(2)) != 3) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read C vector\n" );
      fclose(camFP);
    }

    fgets(line, sizeof(line), camFP);
    if (sscanf(line,"A = %lf %lf %lf", &A(0), &A(1), &A(2)) != 3) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read A vector\n" );
      fclose(camFP);
    }

    fgets(line, sizeof(line), camFP);
    if (sscanf(line,"Hv = %lf %lf %lf", &Hvec(0), &Hvec(1), &Hvec(2)) != 3) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read Hvec\n" );
      fclose(camFP);
    }

    fgets(line, sizeof(line), camFP);
    if (sscanf(line,"Vv = %lf %lf %lf", &Vvec(0), &Vvec(1), &Vvec(2)) != 3) {
      vw_throw( IOErr() << "CAHVModel::read_pinhole: Could not read Vvec\n" );
      fclose(camFP);
    }

    // In the future, we should also read in a view matrix -- LJE
    //     double dummy
    //     if (sscanf(line, "VM = %lf %lf %lf %f %lf %lf %lf %f "
    //        "%lf %lf %lf %f %lf %lf %lf %f ",
    //        &Hvec(0), &Hvec(1), &Hvec(2), &dummy,
    //        &Vvec(0), &Vvec(1), &Vvec(2), &dummy,
    //        &A(0), &A(1), &A(2), &dummy,
    //        &C(0), &C(1), &C(2), &dummy) != 16)
    //     {
    //       vw_throw( IOErr()
    //  << "CAHVModel::ReadPinhole: Could not read view matrix\n" );
    //       fclose(camFP);
    //     }

    fH = f/pixelSize.x();
    fV = f/pixelSize.y();

    H = fH*Hvec + Hc*A;
    V = fV*Vvec + Vc*A;

    fclose(camFP);
  }

  // This is a re-implementation of the Epipolar math that is easier
  // to read, however it disagrees with the other epipolar code below
  // slightly.  It needs another look to clean up this discrepancy,
  // which is due to the direction chosen for Hvec. -mbroxton

//   void epipolar(CAHVModel const src_camera0, CAHVModel const src_camera1,
//                 CAHVModel &dst_camera0, CAHVModel &dst_camera1) {

//     double fh, fv, hc, vc;
//     Vector3 f, g;
//     Vector3 A, H, V;

//     // Compute a common image center and scale for the two models
//     hc = dot_prod(src_camera0.H, src_camera0.A) / 2.0 + dot_prod(src_camera1.H, src_camera1.A) / 2.0;
//     vc = dot_prod(src_camera0.V, src_camera0.A) / 2.0 + dot_prod(src_camera1.V, src_camera1.A) / 2.0;

//     // Find the magnitude of the new H and V vectors (this will be the
//     // average of the focal lengths of these cameras).
//     f = cross_prod(src_camera0.A, src_camera0.H);
//     g = cross_prod(src_camera1.A, src_camera1.H);
//     fh = (norm_2(f) + norm_2(g)) / 2.0;

//     f = cross_prod(src_camera0.A, src_camera0.V);
//     g = cross_prod(src_camera1.A, src_camera1.V);
//     fv = (norm_2(f) + norm_2(g)) / 2.0;

//     // Use common center and scale to construct an average A vector
//     Vector3 A_avg  = 0.5 * (src_camera0.A + src_camera1.A);

//     // Then adjust A to be perpindicular to the baseline between the
//     // two imagers.  This will move the epipoles to infinity.
//     Vector3 Hvec = normalize(src_camera1.C - src_camera0.C);
//     Vector3 Vvec = normalize(cross_prod(Hvec,A_avg));
//     A = normalize(cross_prod(Vvec,Hvec));

//     std::cout << "Fh: " << fh << "   Fv: " << fv << "\n";
//     std::cout << "Ch: " << hc << "   Cv: " << vc << "\n";
//     std::cout << "Hvec: " << Hvec << "   Vvec: " << Vvec << "\n";

//     // Use the standard equations for the epipolar camera model to
//     // determine H and V given the camera intristics and the
//     // horizontal and vertical vectors.
//     H = fh*Hvec + hc*A;
//     V = fv*Vvec + vc*A;

//     dst_camera0.C = src_camera0.C;
//     dst_camera0.A = A;
//     dst_camera0.H = H;
//     dst_camera0.V = V;

//     dst_camera1.C = src_camera1.C;
//     dst_camera1.A = A;
//     dst_camera1.H = H;
//     dst_camera1.V = V;
//   }

  void epipolar(CAHVModel const src_camera0, CAHVModel const src_camera1,
                CAHVModel &dst_camera0, CAHVModel &dst_camera1) {

    double hs, hc, vs, vc;
    Vector3 f, g, hp, ap, app, vp;
    Vector3 a, h, v;

    // Compute a common image center and scale for the two models
    hc = dot_prod(src_camera0.H, src_camera0.A) / 2.0 + dot_prod(src_camera1.H, src_camera1.A) / 2.0;
    vc = dot_prod(src_camera0.V, src_camera0.A) / 2.0 + dot_prod(src_camera1.V, src_camera1.A) / 2.0;

    f = cross_prod(src_camera0.A, src_camera0.H);
    g = cross_prod(src_camera1.A, src_camera1.H);
    hs = (norm_2(f) + norm_2(g)) / 2.0;

    f = cross_prod(src_camera0.A, src_camera0.V);
    g = cross_prod(src_camera1.A, src_camera1.V);
    vs = (norm_2(f) + norm_2(g)) / 2.0;

    // Use common center and scale to construct common A, H, V
    app  = src_camera0.A + src_camera1.A;

    // Note the directionality of f here, for consistency later
    f = src_camera1.C - src_camera0.C;
    g = cross_prod(app, f); // alter f (CxCy) to be
    f = cross_prod(g, app); // perpendicular to average A

    if (dot_prod(f, src_camera0.H) > 0)
      hp = f * hs / (norm_2(f));
    else
      hp = -f * hs / (norm_2(f));

    app = 0.5 * app;
    g = hp * dot_prod(app,hp) / (hs * hs);
    ap = app -g;
    a = ap/norm_2(ap);
    f = cross_prod(a, hp);
    vp = f * vs / hs;
    f = hc * a;
    h = hp + f;

    f = vc * a;
    v = vp + f;

    dst_camera0.C = src_camera0.C;
    dst_camera0.A = a;
    dst_camera0.H = h;
    dst_camera0.V = v;

    dst_camera1.C = src_camera1.C;
    dst_camera1.A = a;
    dst_camera1.H = h;
    dst_camera1.V = v;
  }

}} // namespace vw::camera


/*
 * Implementation of EDM methods, including S-map and cross-mapping
 *
 * - Patrick Laub, Department of Management and Marketing,
 *   The University of Melbourne, patrick.laub@unimelb.edu.au
 * - Edoardo Tescari, Melbourne Data Analytics Platform,
 *  The University of Melbourne, e.tescari@unimelb.edu.au
 *
 */

#pragma warning(disable : 4018)

#include "edm.h"
#include "cpu.h"
#include "distances.h"
#include "library_prediction_split.h"
#include "stats.h" // for correlation and mean_absolute_error
#include "thread_pool.h"

#ifndef FMT_HEADER_ONLY
#define FMT_HEADER_ONLY
#endif

#define EIGEN_NO_DEBUG
#define EIGEN_DONT_PARALLELIZE
#include <Eigen/SVD>
#include <algorithm> // std::partial_sort
#include <chrono>
#include <cmath>

#if defined(DUMP_LOW_LEVEL_INPUTS) || defined(WITH_ARRAYFIRE)
#include <fstream>
#include <iostream>
#endif

#if defined(WITH_ARRAYFIRE)
#include <af/macros.h>
#include <arrayfire.h>
#if WITH_GPU_PROFILING
#include <nvtx3/nvToolsExt.h>
#endif
#endif

using MatrixXd = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
using MatrixXi = Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

PredictionResult edm_task(const std::shared_ptr<ManifoldGenerator> generator, Options opts, int E,
                          const std::vector<bool>& libraryRows, const std::vector<bool> predictionRows, IO* io,
                          bool keep_going(), void all_tasks_finished());

void make_prediction(int Mp_i, const Options& opts, const Manifold& M, const Manifold& Mp,
                     Eigen::Map<MatrixXd> predictionsView, Eigen::Map<MatrixXi> rcView, Eigen::Map<MatrixXd> coeffsView,
                     int* kUsed, bool keep_going());

DistanceIndexPairs k_nearest_neighbours(const DistanceIndexPairs& potentialNeighbours, int k);

void simplex_prediction(int Mp_i, int t, const Options& opts, const Manifold& M, const std::vector<double>& dists,
                        const std::vector<int>& kNNInds, Eigen::Map<MatrixXd> predictionsView,
                        Eigen::Map<MatrixXi> rcView);

void smap_prediction(int Mp_i, int t, const Options& opts, const Manifold& M, const Manifold& Mp,
                     const std::vector<double>& dists, const std::vector<int>& kNNInds,
                     Eigen::Map<MatrixXd> predictionsView, Eigen::Map<MatrixXd> coeffsView,
                     Eigen::Map<MatrixXi> rcView);

#if defined(WITH_ARRAYFIRE)
void af_make_prediction(const int numPredictions, const Options& opts, const Manifold& hostM, const Manifold& hostMp,
                        const ManifoldOnGPU& M, const ManifoldOnGPU& Mp, const af::array& metricOpts,
                        Eigen::Map<MatrixXd> ystar, Eigen::Map<MatrixXi> rc, Eigen::Map<MatrixXd> coeffs,
                        std::vector<int>& kUseds, bool keep_going());
#endif

std::atomic<int> totalNumPredictions = 0;
std::atomic<int> estimatedTotalNumPredictions = 0;

std::atomic<int> numPredictionsFinished = 0;
std::atomic<int> numTasksFinished = 0;

#ifdef _WIN32
// Must leak resource, because windows + R deadlock otherwise. Memory
// is released on shutdown.
ThreadPool* workerPoolPtr = new ThreadPool(0);
ThreadPool* taskRunnerPoolPtr = new ThreadPool(0);
#else
ThreadPool workerPool(0), taskRunnerPool(0);
ThreadPool* workerPoolPtr = &workerPool;
ThreadPool* taskRunnerPoolPtr = &taskRunnerPool;
#endif

std::vector<std::future<PredictionResult>> launch_task_group(
  const std::shared_ptr<ManifoldGenerator> generator, Options opts, const std::vector<int>& Es,
  const std::vector<int>& libraries, int k, int numReps, int crossfold, bool explore, bool full, bool shuffle,
  bool saveFinalPredictions, bool saveFinalCoPredictions, bool saveSMAPCoeffs, bool copredictMode,
  const std::vector<bool>& usable, const std::string& rngState, IO* io, bool keep_going(), void all_tasks_finished())
{
  static bool initOnce = [&]() {
#if defined(WITH_ARRAYFIRE)
    af::setMemStepSize(1024 * 1024 * 5);
    taskRunnerPoolPtr->set_num_workers(1); // Avoid oversubscribing to the GPU
#else
    taskRunnerPoolPtr->set_num_workers(1);
#endif
    return true;
  }();

  workerPoolPtr->set_num_workers(opts.nthreads);

  // Construct the instance which will (repeatedly) split the data
  // into either the library set or the prediction set.
  LibraryPredictionSetSplitter splitter(explore, full, shuffle, crossfold, usable, rngState);

  int numLibraries = (explore ? 1 : libraries.size());

  // Note: the 'numIters' either refers to the 'replicate' option
  // used for bootstrap resampling, or the 'crossfold' number of
  // cross-validation folds. Both options can't be used together.
  int numIters = numReps > crossfold ? numReps : crossfold;

  opts.explore = explore;
  opts.numTasks = numIters * Es.size() * numLibraries;
  opts.configNum = 0;
  opts.taskNum = 0;
  opts.saveKUsed = true;

  int maxE = Es[Es.size() - 1];

  std::vector<bool> cousable;

  if (copredictMode) {
    opts.numTasks *= 2;
    cousable = generator->generate_usable(maxE, true);
  }

  int E, kAdj, library, librarySize;

  std::vector<std::future<PredictionResult>> futures;

  bool newLibraryPredictionSplit = true;

  for (int iter = 1; iter <= numIters; iter++) {
    if (explore) {
      newLibraryPredictionSplit = true;
      librarySize = splitter.next_library_size(iter);
    }

    if (keep_going != nullptr && !keep_going()) {
      break;
    }

    for (int i = 0; i < Es.size(); i++) {
      E = Es[i];

      // 'libraries' is implicitly set to one value in explore mode
      // though in xmap mode it is a user-supplied list which we loop over.
      for (int l = 0; l == 0 || l < libraries.size(); l++) {
        if (!explore) {
          newLibraryPredictionSplit = true;
        }

        if (explore) {
          library = librarySize;
        } else {
          library = libraries[l];
        }

        // Set the number of neighbours to use
        if (k > 0) {
          kAdj = k;
        } else if (k < 0) {
          kAdj = -1; // Leave a sentinel value so we know to skip the nearest neighbours calculation
        } else if (k == 0) {
          bool isSMap = opts.algorithm == Algorithm::SMap;
          int defaultK = generator->E_actual(E) + 1 + isSMap;
          kAdj = defaultK < library ? defaultK : library;
        }

        bool lastConfig = (E == maxE) && (l + 1 == numLibraries);

        if (explore) {
          opts.savePrediction = saveFinalPredictions && ((iter == numReps) || (crossfold > 0)) && lastConfig;
        } else {
          opts.savePrediction = saveFinalPredictions && (iter == numReps) && lastConfig;
        }
        opts.saveSMAPCoeffs = saveSMAPCoeffs;

        if (newLibraryPredictionSplit) {
          splitter.update_library_prediction_split(library, iter);
          newLibraryPredictionSplit = false;
        }

        opts.copredict = false;
        opts.k = kAdj;
        opts.library = library;

        futures.emplace_back(
          taskRunnerPoolPtr->enqueue([generator, opts, E, splitter, io, keep_going, all_tasks_finished] {
            return edm_task(generator, opts, E, splitter.libraryRows(), splitter.predictionRows(), io, keep_going,
                            all_tasks_finished);
          }));

        opts.taskNum += 1;

        if (copredictMode) {
          opts.copredict = true;
          if (explore) {
            opts.savePrediction = saveFinalCoPredictions && ((iter == numReps) || (crossfold > 0)) && lastConfig;
          } else {
            opts.savePrediction = saveFinalCoPredictions && ((iter == numReps)) && lastConfig;
          }
          opts.saveSMAPCoeffs = false;

          futures.emplace_back(
            taskRunnerPoolPtr->enqueue([generator, opts, E, splitter, cousable, io, keep_going, all_tasks_finished] {
              return edm_task(generator, opts, E, splitter.libraryRows(), cousable, io, keep_going, all_tasks_finished);
            }));

          opts.taskNum += 1;
        }

        opts.configNum += opts.thetas.size();
      }
    }
  }

  return futures;
}

PredictionResult edm_task(const std::shared_ptr<ManifoldGenerator> generator, Options opts, int E,
                          const std::vector<bool>& libraryRows, const std::vector<bool> predictionRows, IO* io,
                          bool keep_going(), void all_tasks_finished())
{
  opts.metrics = expand_metrics(*generator, E, opts.distance, opts.metrics);

  if (opts.taskNum == 0) {
    numPredictionsFinished = 0;
    numTasksFinished = 0;

    totalNumPredictions = 0;
    estimatedTotalNumPredictions = 0;
  }

#ifdef DUMP_LOW_LEVEL_INPUTS
  // This hack is simply to dump some really low level data structures
  // purely for the purpose of generating microbenchmarks.
  if (io != nullptr && io->verbosity > 4) {
    json lowLevelInputDump;
    lowLevelInputDump["generator"] = *generator;
    lowLevelInputDump["opts"] = opts;
    lowLevelInputDump["E"] = E;
    lowLevelInputDump["libraryRows"] = libraryRows;
    lowLevelInputDump["predictionRows"] = predictionRows;

    std::ofstream o("lowLevelInputDump.json");
    o << lowLevelInputDump << std::endl;
  }
#endif

  Manifold M(generator, E, libraryRows, false, opts.copredict, opts.lowMemoryMode);
  Manifold Mp(generator, E, predictionRows, true, opts.copredict, opts.lowMemoryMode);

  bool multiThreaded = opts.nthreads > 1;

#if defined(WITH_ARRAYFIRE)
  af::setDevice(0); // TODO potentially can cycle through GPUS if > 1

  // Char is the internal representation of bool in ArrayFire
  std::vector<char> mopts;
  for (int j = 0; j < M.E_actual(); j++) {
    mopts.push_back(opts.metrics[j] == Metric::Diff);
  }

  af::array metricOpts(M.E_actual(), mopts.data());

  const ManifoldOnGPU gpuM = M.toGPU(false);
  const ManifoldOnGPU gpuMp = Mp.toGPU(false);

  constexpr bool useAF = true;
  multiThreaded = multiThreaded && !useAF;
#endif

  int numThetas = (int)opts.thetas.size();
  int numPredictions = Mp.numPoints();
  int numCoeffCols = M.E_actual() + 1;

  totalNumPredictions += numPredictions;
  estimatedTotalNumPredictions = (opts.numTasks / (1.0 + opts.taskNum)) * totalNumPredictions;

  auto predictions = std::make_unique<double[]>(numThetas * numPredictions);
  std::fill_n(predictions.get(), numThetas * numPredictions, MISSING_D);
  Eigen::Map<MatrixXd> predictionsView(predictions.get(), numThetas, numPredictions);

  // If we're saving the coefficients (i.e. in xmap mode), then we're not running with multiple 'theta' values.
  auto coeffs = std::make_unique<double[]>(numPredictions * numCoeffCols);
  std::fill_n(coeffs.get(), numPredictions * numCoeffCols, MISSING_D);
  Eigen::Map<MatrixXd> coeffsView(coeffs.get(), numPredictions, numCoeffCols);

  auto rc = std::make_unique<retcode[]>(numThetas * numPredictions);
  std::fill_n(rc.get(), numThetas * numPredictions, UNKNOWN_ERROR);
  Eigen::Map<MatrixXi> rcView(rc.get(), numThetas, numPredictions);

  std::vector<int> kUsed;
  for (int i = 0; i < numPredictions; i++) {
    kUsed.push_back(MISSING_I);
  }

  if (io != nullptr && opts.taskNum == 0) {
    io->progress_bar(0.0);
  }

  if (multiThreaded) {
    std::vector<std::future<void>> results(numPredictions);
#if WITH_GPU_PROFILING
    workerPoolPtr->sync();
    auto start = std::chrono::high_resolution_clock::now();
#endif
    {
      std::unique_lock<std::mutex> lock(workerPoolPtr->queue_mutex);

      for (int i = 0; i < numPredictions; i++) {
        if (keep_going != nullptr && !keep_going()) {
          break;
        }

        results[i] = workerPoolPtr->unsafe_enqueue(
          [&, i] { make_prediction(i, opts, M, Mp, predictionsView, rcView, coeffsView, &(kUsed[i]), keep_going); });
      }
    }

    for (int i = 0; i < numPredictions; i++) {
      results[i].get();
      if (io != nullptr) {
        numPredictionsFinished += 1;
        io->progress_bar(numPredictionsFinished / ((double)estimatedTotalNumPredictions));
      }
    }
#if WITH_GPU_PROFILING
    workerPoolPtr->sync();
    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> diff = end - start;
    printf("CPU(t=%d): Task(%lu) took %lf seconds for %d predictions \n", opts.nthreads, opts.taskNum, diff.count(),
           numPredictions);
#endif
  } else {
#if defined(WITH_ARRAYFIRE)
    if (useAF) {
#if WITH_GPU_PROFILING
      af::sync(0);
      auto start = std::chrono::high_resolution_clock::now();
#endif
      af_make_prediction(numPredictions, opts, M, Mp, gpuM, gpuMp, metricOpts, predictionsView, rcView, coeffsView,
                         kUsed, keep_going);
#if WITH_GPU_PROFILING
      af::sync(0);
      auto end = std::chrono::high_resolution_clock::now();
      std::chrono::duration<double> diff = end - start;
      printf("GPU: Task(%lu) took %lf seconds for %d predictions \n", opts.taskNum, diff.count(), numPredictions);
#endif
    } else {
#endif

      for (int i = 0; i < numPredictions; i++) {
        if (keep_going != nullptr && !keep_going()) {
          break;
        }
        make_prediction(i, opts, M, Mp, predictionsView, rcView, coeffsView, &(kUsed[i]), keep_going);

        if (io != nullptr) {
          numPredictionsFinished += 1;
          io->progress_bar(numPredictionsFinished / ((double)estimatedTotalNumPredictions));
        }
      }
#if defined(WITH_ARRAYFIRE)
    }
#endif
  }

  PredictionResult pred;

  pred.explore = opts.explore;

  // Store the results, so long as we weren't interrupted by a 'break'.
  if (keep_going == nullptr || keep_going()) {
    // Start by calculating the MAE & rho of prediction, if requested
    for (int t = 0; t < numThetas * opts.calcRhoMAE; t++) {
      PredictionStats stats;

      stats.library = opts.library; // Could store 'M.numPoints()' here for a more accurate version
      stats.E = M.E_actual();
      stats.theta = opts.thetas[t];

      // POTENTIAL SPEEDUP: if predictions and y exist on GPU this could potentially be faster on GPU
      std::vector<double> y1, y2;

      for (int i = 0; i < Mp.numTargets(); i++) {
        if (Mp.target(i) != MISSING_D && predictionsView(t, i) != MISSING_D) {
          y1.push_back(Mp.target(i));
          y2.push_back(predictionsView(t, i));
        }
      }

      if (!(y1.empty() || y2.empty())) {
        stats.mae = mean_absolute_error(y1, y2);
        stats.rho = correlation(y1, y2);
      } else {
        stats.mae = MISSING_D;
        stats.rho = MISSING_D;
      }

      pred.stats.push_back(stats);
    }

    pred.configNum = opts.configNum;

    // Check if any make_prediction call failed, and if so find the most serious error
    pred.rc = *std::max_element(rc.get(), rc.get() + numThetas * numPredictions);

    if (opts.saveManifolds) {
      pred.M = std::make_unique<Manifold>(generator, E, libraryRows, false, opts.copredict, false);
      pred.Mp = std::make_unique<Manifold>(generator, E, predictionRows, true, opts.copredict, false);
    } else {
      pred.M = nullptr;
      pred.Mp = nullptr;
    }

    // If we're storing the prediction and/or the S-map coefficients, put them
    // into the resulting PredictionResult struct. Otherwise, let them be deleted.
    if (opts.savePrediction) {
      // Take only the predictions for the largest theta value.
      if (numThetas == 1) {
        pred.predictions = std::move(predictions);
      } else {
        pred.predictions = std::make_unique<double[]>(numPredictions);
        for (int i = 0; i < numPredictions; i++) {
          pred.predictions[i] = predictionsView(numThetas - 1, i);
        }
      }
    } else {
      pred.predictions = nullptr;
    }

    if (opts.saveSMAPCoeffs) {
      pred.coeffs = std::move(coeffs);
    } else {
      pred.coeffs = nullptr;
    }

    if (opts.savePrediction || opts.saveSMAPCoeffs) {
      pred.predictionRows = std::move(predictionRows);
    }

    if (opts.saveKUsed) {
      auto cleanedKUsed = remove_value<int>(kUsed, MISSING_I);
      if (cleanedKUsed.size() > 0) {
        pred.kMin = *std::min_element(cleanedKUsed.begin(), cleanedKUsed.end());
        pred.kMax = *std::max_element(cleanedKUsed.begin(), cleanedKUsed.end());
      } else {
        pred.kMin = MISSING_I;
        pred.kMax = MISSING_I;
      }
    }

    pred.cmdLine = opts.cmdLine;
    pred.copredict = opts.copredict;

    pred.numThetas = numThetas;
    pred.numPredictions = numPredictions;
    pred.numCoeffCols = numCoeffCols;
  }

  numTasksFinished += 1;

  if (numTasksFinished == opts.numTasks) {

    if (io != nullptr) {
      io->progress_bar(1.0);
    }

    if (all_tasks_finished != nullptr) {
      all_tasks_finished();
    }
  }

  return pred;
}

// Use a library set 'M' to make a prediction about the prediction set 'Mp'.
// Specifically, predict the 'Mp_i'-th value of the prediction set 'Mp'.
//
// The predicted value is stored in 'predictionsView', along with any return codes in 'rcView'.
// Optionally, the user may ask to store some S-map intermediate values in 'coeffsView'.
//
// The 'opts' value specifies the kind of prediction to make (e.g. S-map, or simplex method).
// This function is usually run in a worker thread, and the 'keep_going' callback is frequently called to
// see whether the user still wants this result, or if they have given up & simply want the execution
// to terminate.
//
// We sometimes let 'M' and 'Mp' be the same set, so we train and predict using the same values.
// In this case, the algorithm may cheat by pulling out the identical trajectory from the library set
// and using this as the prediction. As such, we throw away any neighbours which have a distance of 0 from
// the target point.
void make_prediction(int Mp_i, const Options& opts, const Manifold& M, const Manifold& Mp,
                     Eigen::Map<MatrixXd> predictionsView, Eigen::Map<MatrixXi> rcView, Eigen::Map<MatrixXd> coeffsView,
                     int* kUsed, bool keep_going())
{
  // An impatient user may want to cancel a long-running EDM command, so we occasionally check using this
  // callback to see whether we ought to keep going with this EDM command. Of course, this adds a tiny inefficiency,
  // but there doesn't seem to be a simple way to easily kill running worker threads across all OSs.
  if (keep_going != nullptr && !keep_going()) {
    rcView(0, Mp_i) = BREAK_HIT;
    return;
  }

  DistanceIndexPairs potentialNN;
  if (opts.distance == Distance::Wasserstein) {
    potentialNN = wasserstein_distances(Mp_i, opts, M, Mp);
  } else {
    if (opts.lowMemoryMode) {
      potentialNN = lazy_lp_distances(Mp_i, opts, M, Mp);
    } else {
      potentialNN = eager_lp_distances(Mp_i, opts, M, Mp);
    }
  }

  if (keep_going != nullptr && !keep_going()) {
    rcView(0, Mp_i) = BREAK_HIT;
    return;
  }

  // Do we have enough distances to find k neighbours?
  int numValidDistances = potentialNN.inds.size();
  int k = opts.k;

  if (k > numValidDistances) {
    if (opts.forceCompute) {
      k = numValidDistances;
    } else {
      rcView(0, Mp_i) = INSUFFICIENT_UNIQUE;
      return;
    }
  }

  if (k == 0 || numValidDistances == 0) {
    // Whether we throw an error or just silently ignore this prediction
    // depends on whether we are in 'strict' mode or not.
    rcView(0, Mp_i) = opts.forceCompute ? SUCCESS : INSUFFICIENT_UNIQUE;
    return;
  }

  // If we asked for all of the neighbours to be considered (e.g. with k = -1), return this index vector directly.
  DistanceIndexPairs kNNs;
  if (k < 0 || k == potentialNN.inds.size()) {
    kNNs = potentialNN;
  } else {
    kNNs = k_nearest_neighbours(potentialNN, k);
  }

  *kUsed = kNNs.inds.size();

  if (keep_going != nullptr && !keep_going()) {
    rcView(0, Mp_i) = BREAK_HIT;
    return;
  }

  if (opts.algorithm == Algorithm::Simplex) {
    for (int t = 0; t < opts.thetas.size(); t++) {
      simplex_prediction(Mp_i, t, opts, M, kNNs.dists, kNNs.inds, predictionsView, rcView);
    }
  } else if (opts.algorithm == Algorithm::SMap) {
    for (int t = 0; t < opts.thetas.size(); t++) {
      smap_prediction(Mp_i, t, opts, M, Mp, kNNs.dists, kNNs.inds, predictionsView, coeffsView, rcView);
    }
  } else {
    rcView(0, Mp_i) = INVALID_ALGORITHM;
  }
}

// For a given point, find the k nearest neighbours of this point.
//
// If there are many potential neighbours with the exact same distances, we
// prefer the neighbours with the smallest index value. This corresponds
// to a stable sort in C++ STL terminology.
//
// In typical use-cases of 'edm explore' the value of 'k' is small, like 5-20.
// However for a typical 'edm xmap' the value of 'k' is set as large as possible.
// If 'k' is small, the partial_sort is efficient as it only finds the 'k' smallest
// distances. If 'k' is larger, then it is faster to simply sort the entire distance
// vector.
DistanceIndexPairs k_nearest_neighbours(const DistanceIndexPairs& potentialNeighbours, int k)
{
  std::vector<int> idx(potentialNeighbours.inds.size());
  std::iota(idx.begin(), idx.end(), 0);

  if (k >= (int)(idx.size() / 2)) {
    auto comparator = [&potentialNeighbours](int i1, int i2) {
      return potentialNeighbours.dists[i1] < potentialNeighbours.dists[i2];
    };
    std::stable_sort(idx.begin(), idx.end(), comparator);
  } else {
    auto stableComparator = [&potentialNeighbours](int i1, int i2) {
      if (potentialNeighbours.dists[i1] != potentialNeighbours.dists[i2])
        return potentialNeighbours.dists[i1] < potentialNeighbours.dists[i2];
      else
        return i1 < i2;
    };
    std::partial_sort(idx.begin(), idx.begin() + k, idx.end(), stableComparator);
  }

  std::vector<int> kNNInds(k);
  std::vector<double> kNNDists(k);

  for (int i = 0; i < k; i++) {
    kNNInds[i] = potentialNeighbours.inds[idx[i]];
    kNNDists[i] = potentialNeighbours.dists[idx[i]];
  }

  return { kNNInds, kNNDists };
}

// An alternative version of 'k_nearest_neighbours' which doesn't sort the neighbours.
// This version splits ties differently on different OS's, so it can't be used directly,
// though perhaps a platform-independent implementation of std::nth_element would solve this problem.
DistanceIndexPairs k_nearest_neighbours_unstable(const DistanceIndexPairs& potentialNeighbours, int k)
{
  std::vector<int> indsToPartition(potentialNeighbours.inds.size());
  std::iota(indsToPartition.begin(), indsToPartition.end(), 0);

  auto comparator = [&potentialNeighbours](int i1, int i2) {
    return potentialNeighbours.dists[i1] < potentialNeighbours.dists[i2];
  };
  std::nth_element(indsToPartition.begin(), indsToPartition.begin() + k, indsToPartition.end(), comparator);

  std::vector<int> kNNInds(k);
  std::vector<double> kNNDists(k);

  for (int i = 0; i < k; i++) {
    kNNInds[i] = potentialNeighbours.inds[indsToPartition[i]];
    kNNDists[i] = potentialNeighbours.dists[indsToPartition[i]];
  }

  return { kNNInds, kNNDists };
}

void simplex_prediction(int Mp_i, int t, const Options& opts, const Manifold& M, const std::vector<double>& dists,
                        const std::vector<int>& kNNInds, Eigen::Map<MatrixXd> predictionsView,
                        Eigen::Map<MatrixXi> rcView)
{
  int k = kNNInds.size();

  // Find the smallest distance (closest neighbour) among the supplied neighbours.
  double minDist = *std::min_element(dists.begin(), dists.end());

  // Calculate our weighting of each neighbour, and the total sum of these weights.
  std::vector<double> w(k);
  double sumw = 0.0;
  const double theta = opts.thetas[t];

  for (int j = 0; j < k; j++) {
    w[j] = exp(-theta * (dists[j] / minDist));
    sumw = sumw + w[j];
  }

  // Make the simplex projection/prediction.
  double r = 0.0;
  for (int j = 0; j < k; j++) {
    r = r + M.target(kNNInds[j]) * (w[j] / sumw);
  }

  // Store the results & return value.
  predictionsView(t, Mp_i) = r;
  rcView(t, Mp_i) = SUCCESS;
}

void smap_prediction(int Mp_i, int t, const Options& opts, const Manifold& M, const Manifold& Mp,
                     const std::vector<double>& dists, const std::vector<int>& kNNInds,
                     Eigen::Map<MatrixXd> predictionsView, Eigen::Map<MatrixXd> coeffsView, Eigen::Map<MatrixXi> rcView)
{
  int k = kNNInds.size();

  // Calculate the weight for each neighbour
  Eigen::Map<const Eigen::VectorXd> distsMap(&(dists[0]), dists.size());
  Eigen::VectorXd w = Eigen::exp(-opts.thetas[t] * (distsMap.array() / distsMap.mean()));

  // Pull out the nearest neighbours from the manifold, and
  // simultaneously prepend a column of ones in front of the manifold data.
  MatrixXd X_ls_cj(k, M.E_actual() + 1);

  if (opts.lowMemoryMode) {
    for (int i = 0; i < k; i++) {
      X_ls_cj(i, 0) = w[i];
      M.lazy_fill_in_point(kNNInds[i], &(X_ls_cj(i, 1)));
      for (int j = 1; j < M.E_actual() + 1; j++) {
        X_ls_cj(i, j) *= w[i];
      }
    }
  } else {
    for (int i = 0; i < k; i++) {
      X_ls_cj(i, 0) = w[i];
      for (int j = 1; j < M.E_actual() + 1; j++) {
        X_ls_cj(i, j) = w[i] * M(kNNInds[i], j - 1);
      }
    }
  }

  // Scale targets by our weights vector
  Eigen::VectorXd y_ls(k);
  for (int i = 0; i < k; i++) {
    y_ls[i] = w[i] * M.target(kNNInds[i]);
  }

  // The old way to solve this system:
  // Eigen::BDCSVD<MatrixXd> svd(X_ls_cj, Eigen::ComputeThinU | Eigen::ComputeThinV);
  //  Eigen::VectorXd ics = svd.solve(y_ls);

  // The pseudo-inverse of X can be calculated as (X^T * X)^(-1) * X^T
  // see https://scicomp.stackexchange.com/a/33375
  const int svdOpts = Eigen::ComputeThinU | Eigen::ComputeThinV; // 'ComputeFull*' would probably work identically here.
  Eigen::JacobiSVD<MatrixXd> svd(X_ls_cj.transpose() * X_ls_cj, svdOpts);
  Eigen::VectorXd ics = svd.solve(X_ls_cj.transpose() * y_ls);

  double r = ics(0);

  auto y = std::unique_ptr<double[]>(new double[M.E_actual()], std::default_delete<double[]>());

  if (opts.lowMemoryMode) {
    Mp.lazy_fill_in_point(Mp_i, y.get());
  } else {
    Mp.eager_fill_in_point(Mp_i, y.get());
  }

  for (int j = 0; j < M.E_actual(); j++) {
    if (y[j] != MISSING_D) {
      r += y[j] * ics(j + 1);
    }
  }

  // If the 'savesmap' option is given, save the 'ics' coefficients
  // for the largest value of theta.
  if (opts.saveSMAPCoeffs && t == opts.thetas.size() - 1) {
    for (int j = 0; j < M.E_actual() + 1; j++) {
      if (std::abs(ics(j)) < 1.0e-11) {
        coeffsView(Mp_i, j) = MISSING_D;
      } else {
        coeffsView(Mp_i, j) = ics(j);
      }
    }
  }

  predictionsView(t, Mp_i) = r;
  rcView(t, Mp_i) = SUCCESS;
}

/////////////////////////////////////////////////////////////// ArrayFire PORTED versions BEGIN HERE

#if defined(WITH_ARRAYFIRE)

// Returns b8 array of shape [numLibraryPoints numPredictions 1 1] when either of skip flags are true
//        otherwise of shape [numLibraryPoints 1 1 1]
af::array afPotentialNeighbourIndices(const int& numPredictions, const bool& skipOtherPanels,
                                      const bool& skipMissingData, const ManifoldOnGPU& M, const ManifoldOnGPU& Mp)
{
  using af::anyTrue;
  using af::array;
  using af::dim4;
  using af::iota;
  using af::seq;
  using af::tile;

#if WITH_GPU_PROFILING
  auto range = nvtxRangeStartA(__FUNCTION__);
#endif

  const dim_t numLibraryPoints = M.numPoints;

  array result;
  if (skipOtherPanels && skipMissingData) {
    array numPredictionsMp = Mp.panel(seq(numPredictions));
    array panelM = tile(M.panel, 1, numPredictions);
    array panelMp = tile(numPredictionsMp.T(), numLibraryPoints);
    array mssngM = (M.mdata == M.missing);
    array msngCols = anyTrue(mssngM, 0);
    array msngFlags = tile(msngCols.T(), 1, numPredictions);

    result = !(msngFlags || (panelM != panelMp));
  } else if (skipOtherPanels) {
    array numPredictionsMp = Mp.panel(seq(numPredictions));
    array panelM = tile(M.panel, 1, numPredictions);
    array panelMp = tile(numPredictionsMp.T(), numLibraryPoints);

    result = !(panelM != panelMp);
  } else if (skipMissingData) {
    result = tile(!(anyTrue(M.mdata == M.missing, 0).T()), 1, numPredictions);
  } else {
    result = af::constant(1.0, M.numPoints, numPredictions, b8);
  }
#if WITH_GPU_PROFILING
  nvtxRangeEnd(range);
#endif
  return result;
}

void afNearestNeighbours(af::array& pValids, af::array& sDists, af::array& yvecs, af::array& smData,
                         const af::array& vDists, const af::array& yvec, const af::array& mdata, const Algorithm algo,
                         const int eacts, const int numLibraryPoints, const int numPredictions, const int k)
{
  using af::array;
  using af::dim4;
  using af::iota;
  using af::moddims;
  using af::sort;
  using af::tile;

#if WITH_GPU_PROFILING
  auto searchRange = nvtxRangeStartA("sortData");
#endif
  array maxs = af::max(pValids * vDists, 0);
  array pDists = pValids * vDists + (1 - pValids) * tile(maxs + 100, numLibraryPoints);

  array indices;
  topk(sDists, indices, pDists, k, 0, AF_TOPK_MIN);

  yvecs = moddims(yvec(indices), k, numPredictions);

  array vIdx = indices + iota(dim4(1, numPredictions), dim4(k)) * numLibraryPoints;

  pValids = moddims(pValids(vIdx), k, numPredictions);

  // Manifold data also needs to be reorder for SMap prediction
  if (algo == Algorithm::SMap) {
    array tmdata = tile(mdata, 1, 1, numPredictions);
    array soffs = iota(dim4(1, 1, numPredictions), dim4(eacts, k)) * (eacts * numLibraryPoints);
    array d0offs = iota(dim4(eacts), dim4(1, k, numPredictions));

    indices = tile(moddims(indices, 1, k, numPredictions), eacts) * eacts;
    indices += (soffs + d0offs);

    smData = moddims(tmdata(indices), eacts, k, numPredictions);
  }

#if WITH_GPU_PROFILING
  nvtxRangeEnd(searchRange);
#endif
}

void afSimplexPrediction(af::array& retcodes, af::array& ystar, const int numPredictions, const Options& opts,
                         const af::array& yvecs, const DistanceIndexPairsOnGPU& pair, const af::array& thetas,
                         const bool isKNeg)
{
  using af::array;
  using af::sum;
  using af::tile;

#if WITH_GPU_PROFILING
  auto range = nvtxRangeStartA(__FUNCTION__);
#endif

  const array& valids = pair.valids;
  const array& dists = pair.dists;
  const int k = valids.dims(0);
  const int tcount = opts.thetas.size();
  const array thetasT = tile(thetas, k, numPredictions);

  array weights;
  {
    array minDist;
    if (isKNeg) {
      minDist = tile(min(dists, 0), k, 1, tcount);
    } else {
      minDist = tile(dists(0, af::span), k, 1, tcount);
    }
    array tadist = tile(dists, 1, 1, tcount);

    weights = tile(valids, 1, 1, tcount) * af::exp(-thetasT * (tadist / minDist));
  }
  array r4thetas = tile(yvecs, 1, (isKNeg ? numPredictions : 1), tcount) * (weights / tile(sum(weights, 0), k));

  ystar = moddims(sum(r4thetas, 0), numPredictions, tcount);
  retcodes = af::constant(SUCCESS, numPredictions, tcount, s32);

#if WITH_GPU_PROFILING
  nvtxRangeEnd(range);
#endif
}

template<typename T>
void afSMapPrediction(af::array& retcodes, af::array& ystar, af::array& coeffs, const int numPredictions,
                      const Options& opts, const ManifoldOnGPU& M, const ManifoldOnGPU& Mp,
                      const DistanceIndexPairsOnGPU& pair, const af::array& mdata, const af::array& yvecs,
                      const af::array& thetas, const bool useLoops)
{
  using af::array;
  using af::constant;
  using af::dim4;
  using af::end;
  using af::matmulTN;
  using af::mean;
  using af::moddims;
  using af::pinverse;
  using af::select;
  using af::seq;
  using af::span;
  using af::tile;

#if WITH_GPU_PROFILING
  auto range = nvtxRangeStartA(__FUNCTION__);
#endif

  const array& valids = pair.valids;
  const array& dists = pair.dists;
  const int k = valids.dims(0);
  const int tcount = opts.thetas.size();
  const int MEactualp1 = M.E_actual + 1;
  const af_dtype cType = M.mdata.type();

  if (useLoops) {
    array meanDists = tile((k * mean(valids * dists, 0) / count(valids, 0)), k);
    array mdValids = tile(moddims(valids, 1, k, numPredictions), M.E_actual);
    array Mp_i_j = Mp.mdata(span, seq(numPredictions));
    array scaleval = ((Mp_i_j != double(MISSING_D)) * Mp_i_j);

    // Allocate Output arrays
    ystar = array(tcount, numPredictions, cType);

    for (int t = 0; t < tcount; ++t) {
      double theta = opts.thetas[t];

      array weights = valids * af::exp(-theta * (dists / meanDists));
      array y_ls = weights * tile(yvecs, 1, numPredictions);

      array icsOuts = array(MEactualp1, numPredictions, cType);
      for (int p = 0; p < numPredictions; ++p) {
        array X_ls_cj = constant(1.0, dim4(MEactualp1, k), cType);

        X_ls_cj(seq(1, end), span) = mdValids(span, span, p) * mdata;

        X_ls_cj *= tile(moddims(weights(span, p), 1, k), MEactualp1);

        icsOuts(span, p) = matmulTN(pinverse(X_ls_cj, 1e-9), y_ls(span, p));
      }
      array r2d = icsOuts(seq(1, end), span) * scaleval;
      array r = icsOuts(0, span) + sum(r2d, 0);

      ystar(t, span) = r;

      if (t == tcount - 1) {
        if (opts.saveSMAPCoeffs) {
          coeffs = select(af::abs(icsOuts) < 1.0e-11, double(MISSING_D), icsOuts).T();
        }
      }
    }
  } else {
    array thetasT = tile(thetas, k, numPredictions);
    array weights, y_ls;
    {
      array meanDists = (k * mean(valids * dists, 0) / count(valids, 0));
      array meanDistsT = tile(meanDists, k, 1, tcount);
      array ptDists = tile(dists, 1, 1, tcount);
      array validsT = tile(valids, 1, 1, tcount);

      weights = validsT * af::exp(-thetasT * (ptDists / meanDistsT));
      y_ls = weights * tile(yvecs, 1, 1, tcount);
    }

    array mdValids = tile(moddims(valids, 1, k, numPredictions), M.E_actual);
    array X_ls_cj = constant(1.0, dim4(MEactualp1, k, numPredictions), cType);

    X_ls_cj(seq(1, end), span) = mdValids * mdata;

    array X_ls_cj_T = tile(X_ls_cj, 1, 1, 1, tcount);

    X_ls_cj_T *= tile(moddims(weights, 1, k, numPredictions, tcount), MEactualp1);

    array icsOuts = matmulTN(pinverse(X_ls_cj_T, 1e-9), moddims(y_ls, k, 1, numPredictions, tcount));

    icsOuts = moddims(icsOuts, MEactualp1, numPredictions, tcount);
    array Mp_i_j = tile(Mp.mdata(span, seq(numPredictions)), 1, 1, tcount);
    array r2d = icsOuts(seq(1, end), span, span) * ((Mp_i_j != double(MISSING_D)) * Mp_i_j);
    array r = icsOuts(0, span, span) + sum(r2d, 0);

    ystar = moddims(r, numPredictions, tcount).T();
    retcodes = constant(SUCCESS, numPredictions, tcount);
    if (opts.saveSMAPCoeffs) {
      array lastTheta = icsOuts(span, span, tcount - 1);

      coeffs = select(af::abs(lastTheta) < 1.0e-11, double(MISSING_D), lastTheta).T();
    }
  }

  retcodes = constant(SUCCESS, numPredictions, tcount);
#if WITH_GPU_PROFILING
  nvtxRangeEnd(range);
#endif
}

void af_make_prediction(const int numPredictions, const Options& opts, const Manifold& hostM, const Manifold& hostMp,
                        const ManifoldOnGPU& M, const ManifoldOnGPU& Mp, const af::array& metricOpts,
                        Eigen::Map<MatrixXd> ystar, Eigen::Map<MatrixXi> rc, Eigen::Map<MatrixXd> coeffs,
                        std::vector<int>& kUseds, bool keep_going())
{
  try {
    using af::array;
    using af::constant;
    using af::dim4;
    using af::iota;

#if WITH_GPU_PROFILING
    auto mpRange = nvtxRangeStartA(__FUNCTION__);
#endif

    const int numThetas = opts.thetas.size();
    const af_dtype cType = M.mdata.type();

    if (opts.algorithm != Algorithm::Simplex && opts.algorithm != Algorithm::SMap) {
      array retcodes = constant(INVALID_ALGORITHM, numPredictions, numThetas, s32);
      retcodes.host(rc.data());
      return;
    }
    using af::span;
    using af::tile;
    using af::where;

    const bool skipOtherPanels = opts.panelMode && (opts.idw < 0);
    const bool skipMissingData = (opts.algorithm == Algorithm::SMap);

    array thetas = array(1, 1, opts.thetas.size(), opts.thetas.data()).as(cType);

    auto pValids = afPotentialNeighbourIndices(numPredictions, skipOtherPanels, skipMissingData, M, Mp);

    auto validDistPair = afLPDistances(numPredictions, opts, M, Mp, metricOpts);

#if WITH_GPU_PROFILING
    auto kisRange = nvtxRangeStartA("kNearestSelection");
#endif
    // TODO add code path for wasserstein later
    pValids = pValids && validDistPair.valids;

    // smData is set only if algo is SMap
    array retcodes, kUsed, sDists, yvecs, smData;

    const int k = opts.k;
    bool isKNeg = k < 0;

    if (k == 0) {
      af::array retcodes = af::constant(SUCCESS, numPredictions, opts.thetas.size(), s32);
      retcodes.host(rc.data());
      return;
    }

    if (k > 0) {
      try {
        afNearestNeighbours(pValids, sDists, yvecs, smData, validDistPair.dists, M.targets, M.mdata, opts.algorithm,
                            M.E_actual, M.numPoints, numPredictions, k);
      } catch (const af::exception& e) {
        // When 'k' is too large, afNearestNeighbours will crash.
        // For now, just continue as if k=-1 was specified.
        isKNeg = true;
        sDists = af::select(pValids, validDistPair.dists, MISSING_D);
        yvecs = M.targets;
        smData = M.mdata;
      }
    } else {
      sDists = af::select(pValids, validDistPair.dists, MISSING_D);
      yvecs = M.targets;
      smData = M.mdata;
    }
#if WITH_GPU_PROFILING
    nvtxRangeEnd(kisRange);
#endif

    if (opts.saveKUsed) {
      kUsed = af::sum(pValids, 0);
    }

    array ystars, dcoeffs;
    if (opts.algorithm == Algorithm::Simplex) {
      afSimplexPrediction(retcodes, ystars, numPredictions, opts, yvecs, { pValids, sDists }, thetas, isKNeg);
    } else if (opts.algorithm == Algorithm::SMap) {
      if (cType == f32) {
        afSMapPrediction<float>(retcodes, ystars, dcoeffs, numPredictions, opts, M, Mp, { pValids, sDists }, smData,
                                yvecs, thetas, isKNeg);
      } else {
        afSMapPrediction<double>(retcodes, ystars, dcoeffs, numPredictions, opts, M, Mp, { pValids, sDists }, smData,
                                 yvecs, thetas, isKNeg);
      }
    }

#if WITH_GPU_PROFILING
    auto returnRange = nvtxRangeStartA("ReturnValues");
#endif
    if (opts.algorithm == Algorithm::Simplex) {
      ystars.as(f64).host(ystar.data());
    } else {
      ystars.T().as(f64).host(ystar.data());
    }

    retcodes.T().host(rc.data());
    if (opts.saveKUsed) {
      kUsed.host(kUseds.data());
    }
    if (opts.saveSMAPCoeffs) {
      dcoeffs.T().as(f64).host(coeffs.data());
    }
#if WITH_GPU_PROFILING
    nvtxRangeEnd(returnRange);
    nvtxRangeEnd(mpRange);
#endif
  } catch (af::exception& e) {
    std::cerr << "ArrayFire threw an exception with message: \n" << std::endl;
    std::cerr << e << std::endl;

    af::array retcodes = af::constant(UNKNOWN_ERROR, numPredictions, opts.thetas.size(), s32);
    retcodes.host(rc.data());
  }
}

#endif