/*
 * Version:  2017.07.31
 * Authors:  Members of the Team NAIST-Panasonic at the Amazon Robotics Challenge 2017:
 *           Gustavo A. Garcia R. <garcia-g at is.naist.jp> (Captain),
 *           Lotfi El Hafi, Felix von Drigalski, Wataru Yamazaki, Viktor Hoerig, Arnaud Delmotte,
 *           Akishige Yuguchi, Marcus Gall, Chika Shiogama, Kenta Toyoshima, Pedro Uriguen,
 *           Rodrigo Elizalde, Masaki Yamamoto, Yasunao Okazaki, Kazuo Inoue, Katsuhiko Asai,
 *           Ryutaro Futakuchi, Seigo Okada, Yusuke Kato, and Pin-Chu Yang
 *********************
 * Copyright 2017 Team NAIST-Panasonic
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *     http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *********************
 */

#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <ros/ros.h>
#include "std_msgs/Int16.h"
#include "geometry_msgs/PoseStamped.h"
#include "std_msgs/Float64.h"

#include <Eigen/Core>

#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <librealsense/rs.h>
#include <librealsense/rs.hpp>

#include <chrono>
#include <iostream>

#include <opencv2/opencv.hpp>
#include <opencv2/xfeatures2d.hpp>

#define NOISY 3.5

cv::Mat color_image;
cv::Mat depth_image;

bool color_image_ishere;
bool color_info_ishere;
bool depth_image_ishere;
bool depth_info_ishere;

float color_info_fx;
float color_info_fy;
float color_info_ppx;
float color_info_ppy;
float color_info_tx;
float color_info_ty;
float color_info_tz;

float depth_info_fx;
float depth_info_fy;
float depth_info_ppx;
float depth_info_ppy;
float depth_info_tx;
float depth_info_ty;
float depth_info_tz;

cv::Mat depth_to_color_extrinsics;
cv::Mat color_K;
cv::Mat depth_K;
bool USE_HARDCODED_ROTATION = true;

std::vector<std::vector<cv::Mat>> listImg;
std::vector<std::vector<cv::Mat>> listRectifiedImg;
std::vector<std::vector<std::vector<cv::Point3f>>>listCorners;
std::vector<cv::Point3f> listSize;

// Variables for item pose and confidence
geometry_msgs::PoseStamped itemPose;
std_msgs::Float64 conf[5];

std::vector<std::vector<std::vector<cv::KeyPoint>>>keypoints1;
std::vector<std::vector<cv::Mat>> descriptors1;
cv::Ptr<cv::DescriptorMatcher> matcher;
cv::Ptr<cv::AKAZE> akaze;
cv::Ptr<cv::ORB> orb;

ros::Publisher pub_item_pose, pub_confidence, pub_image_id;
ros::ServiceServer getItemPosService;

int w, h, depth;
float scale;

int selectedImg = 0;

cv::Point3f multMatVec(cv::Mat mat, cv::Point3f p)
{
  cv::Point3f p2;
  cv::Mat vec(mat.cols, 1, mat.type());

  if (vec.type() == CV_32F)
  {
    vec.at<float>(0, 0) = p.x;
    vec.at<float>(1, 0) = p.y;
    vec.at<float>(2, 0) = p.z;
    if (mat.cols == 4)
      vec.at<float>(3, 0) = 1.0;
  }
  else
  {
    vec.at<double>(0, 0) = p.x;
    vec.at<double>(1, 0) = p.y;
    vec.at<double>(2, 0) = p.z;
    if (mat.cols == 4)
      vec.at<double>(3, 0) = 1.0;
  }

  cv::Mat res = mat * vec;
  if (res.rows == 4)
  {
    if (res.type() == CV_32F)
      res = res / res.at<float>(3, 0);
    else
      res = res / res.at<double>(3, 0);
  }

  if (res.type() == CV_32F)
  {
    p2.x = res.at<float>(0, 0);
    p2.y = res.at<float>(1, 0);
    p2.z = res.at<float>(2, 0);
  }
  else
  {
    p2.x = res.at<double>(0, 0);
    p2.y = res.at<double>(1, 0);
    p2.z = res.at<double>(2, 0);
  }
  return p2;
}

cv::Mat point2Mat(cv::Point3d p)
{
  cv::Mat m(3, 1, CV_64F);
  m.at<double>(0, 0) = p.x;
  m.at<double>(1, 0) = p.y;
  m.at<double>(2, 0) = p.z;
  return m;
}

cv::Mat RTVec2Mat(cv::Mat rvec, cv::Mat tvec)
{
  if (rvec.empty() || tvec.empty())
  {
    printf("Error RTVec2Mat empty mat\n");
    return cv::Mat::eye(4, 4, CV_64F);
  }
  cv::Mat R;
  cv::Rodrigues(rvec, R);
  cv::Mat Rmat = cv::Mat::eye(4, 4, CV_64F);
  cv::Mat Tmat = cv::Mat::eye(4, 4, CV_64F);
  R.copyTo(Rmat(cv::Rect(0, 0, 3, 3)));
  tvec.copyTo(Tmat(cv::Rect(3, 0, 1, 3)));

  return Tmat * Rmat;
}

cv::Mat getRVecFromMat(cv::Mat M)
{
  cv::Mat rvec;
  cv::Rodrigues(M(cv::Rect(0, 0, 3, 3)), rvec);
  return rvec;
}

cv::Mat getTVecFromMat(cv::Mat M)
{
  return M(cv::Rect(3, 0, 1, 3)).clone();
}

cv::Point3f pcl2cv(pcl::Normal n)
{
  return cv::Point3f(n.normal_x, n.normal_y, n.normal_z);
}

cv::Point2f rs2cv(rs::float2 a)
{
  return cv::Point2f(a.x, a.y);
}

cv::Point3f rs2cv(rs::float3 a)
{
  return cv::Point3f(a.x, a.y, a.z);
}

rs::float2 cv2rs(cv::Point2f a)
{
  return
  { a.x, a.y};
}

rs::float3 cv2rs(cv::Point3f a)
{
  return
  { a.x, a.y, a.z};
}

cv::Point3f transformByMat44(cv::Mat M, cv::Point3f p)
{
  cv::Point3f p2;
  float w = M.at<float>(3, 0) * p.x + M.at<float>(3, 1) * p.y + M.at<float>(3, 2) * p.z + M.at<float>(3, 3);
  p2.x = (M.at<float>(0, 0) * p.x + M.at<float>(0, 1) * p.y + M.at<float>(0, 2) * p.z + M.at<float>(0, 3)) / w;
  p2.y = (M.at<float>(1, 0) * p.x + M.at<float>(1, 1) * p.y + M.at<float>(1, 2) * p.z + M.at<float>(1, 3)) / w;
  p2.z = (M.at<float>(2, 0) * p.x + M.at<float>(2, 1) * p.y + M.at<float>(2, 2) * p.z + M.at<float>(2, 3)) / w;
  return p2;
}

cv::Mat findPoseWith2PointsAndNormal(cv::Point3f p1, cv::Point3f n1, cv::Point3f p2, cv::Point3f origP1,
                                     cv::Point3f origN1, cv::Point3f origP2)
{
  cv::Point3f x = n1;
  cv::Point3f y = p2 - p1;
  y -= y.dot(x) * x;
  y /= sqrt(y.dot(y));
  cv::Point3f z = x.cross(y);

  cv::Point3f origX = origN1;
  cv::Point3f origY = origP2 - origP1;
  origY -= origY.dot(origX) * origX;
  origY /= sqrt(origY.dot(origY));
  cv::Point3f origZ = origX.cross(origY);

  cv::Point3f center = p1 - origP1.dot(origX) * x - origP1.dot(origY) * y - origP1.dot(origZ) * z;
  cv::Mat poseMat = cv::Mat::eye(4, 4, CV_32F);
  poseMat.at<float>(0, 3) = center.x;
  poseMat.at<float>(1, 3) = center.y;
  poseMat.at<float>(2, 3) = center.z;

  cv::Mat R1(3, 3, CV_32F);
  R1.at<float>(0, 0) = x.x;
  R1.at<float>(0, 1) = y.x;
  R1.at<float>(0, 2) = z.x;
  R1.at<float>(1, 0) = x.y;
  R1.at<float>(1, 1) = y.y;
  R1.at<float>(1, 2) = z.y;
  R1.at<float>(2, 0) = x.z;
  R1.at<float>(2, 1) = y.z;
  R1.at<float>(2, 2) = z.z;

  cv::Mat R2(3, 3, CV_32F);
  R2.at<float>(0, 0) = origX.x;
  R2.at<float>(0, 1) = origY.x;
  R2.at<float>(0, 2) = origZ.x;
  R2.at<float>(1, 0) = origX.y;
  R2.at<float>(1, 1) = origY.y;
  R2.at<float>(1, 2) = origZ.y;
  R2.at<float>(2, 0) = origX.z;
  R2.at<float>(2, 1) = origY.z;
  R2.at<float>(2, 2) = origZ.z;

  cv::Mat R = R1 * R2.inv();
  R.copyTo(poseMat(cv::Rect(0, 0, 3, 3)));

  return poseMat;
}

class RgbdFrame
{
public:
  cv::Mat depth; // Depth camera, depth value CV_16UC1.
  cv::Mat img; // Color camera, RGB value CV_8UC3.
  cv::Mat imgDepth; // Color camera, depth value CV_32F.
  pcl::PointCloud<pcl::PointXYZ>::Ptr imgPoints; // Color camera, point cloud.
  pcl::PointCloud<pcl::Normal>::Ptr imgNormals; // Color camera, normals.
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr rs_cloud_ptr; // Depth camera, point cloud (with RGB from projection on color camera).

  rs::intrinsics depth_intrin;
  rs::extrinsics depth_to_color;
  rs::intrinsics color_intrin;
  float scale;

  RgbdFrame()
  {
    rs_cloud_ptr = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
    imgPoints = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    imgNormals = pcl::PointCloud<pcl::Normal>::Ptr(new pcl::PointCloud<pcl::Normal>);
  }

  // Project the 3D point to the 2D color image.
  cv::Point2f colorCameraProject(cv::Point3f p)
  {
    return rs2cv(color_intrin.project(cv2rs(p)));
  }

  cv::Point2f depthCameraProject(cv::Point3f p)
  {
    return rs2cv(depth_intrin.project(cv2rs(p)));
  }

  cv::Point3f depthCameraToColorCamera(cv::Point3f p)
  {
    return rs2cv(depth_to_color.transform(cv2rs(p)));
  }

  cv::Point3f depthCameraDeproject(cv::Point2f p, float distance_in_meter)
  {
    return rs2cv(depth_intrin.deproject(cv2rs(p), distance_in_meter));
  }
};

cv::Mat findPoseWithDepthCue(const std::vector<cv::Point3f> &obj, const std::vector<cv::Point3f> &objNormal,
                             const std::vector<cv::Point2f> &scene, RgbdFrame frame, cv::Mat &mask)
{
  bool planar_test = true;
  std::vector<cv::Point3f> newObj;
  std::vector<cv::Point3f> newObjNormal;
  std::vector<cv::Point2f> newScene;
  std::vector<cv::Point3f> newScenePoints;
  std::vector<cv::Point3f> newSceneNormal;
  mask = cv::Mat(obj.size(), 1, CV_8UC1);
  mask.setTo(cv::Scalar(0));
  std::vector<int> selectedId;
  for (int i = 0; i < scene.size(); i++)
  {
    int id = int(scene[i].y) * frame.imgPoints->width + int(scene[i].x);
    pcl::PointXYZ p1 = frame.imgPoints->points[id];
    cv::Point3f p(p1.x, p1.y, p1.z);
    if (sqrt(p.dot(p)) > 0)
    {
      newObj.push_back(obj[i]);
      newObjNormal.push_back(objNormal[i]);
      newScene.push_back(scene[i]);
      newScenePoints.push_back(p);
      newSceneNormal.push_back(pcl2cv(frame.imgNormals->points[id]));
      selectedId.push_back(i);
    }
  }
  if (newObj.size() < 4)
    return cv::Mat();
  std::vector<std::vector<int>> goodPairs(newObj.size());
  std::vector<std::vector<bool>> goodPairsTable(newObj.size());
  for (int i = 0; i < newObj.size(); i++)
  {
    goodPairsTable[i] = std::vector<bool>(newObj.size());
    for (int j = 0; j < newObj.size(); j++)
      goodPairsTable[i][j] = false;
  }
  for (int i = 0; i < newObj.size(); i++)
  {
    for (int j = i + 1; j < newObj.size(); j++)
    {
      float angleDiff = fabs(acos(newSceneNormal[i].dot(newSceneNormal[j])));
      float dist = sqrt((newObj[i] - newObj[j]).dot(newObj[i] - newObj[j]));
      float dist2 = sqrt((newScenePoints[i] - newScenePoints[j]).dot(newScenePoints[i] - newScenePoints[j]));
      if (dist > 0.95 * dist2 && dist < 1.05 * dist2 && (!planar_test || angleDiff < 30 * CV_PI / 180))
      {
        goodPairs[i].push_back(j);
        goodPairs[j].push_back(i);
        goodPairsTable[i][j] = true;
        goodPairsTable[j][i] = true;
      }
    }
  }
  cv::Mat bestPose;
  std::vector<int> bestValidPoints;
  for (int iter = 0; iter < 10000; iter++)
  {

    int id1 = rand() % newObj.size();
    if (goodPairs[id1].size() < 3)
      continue;
    int id2 = goodPairs[id1][rand() % goodPairs[id1].size()];

    cv::Mat pose = findPoseWith2PointsAndNormal(newScenePoints[id1], newSceneNormal[id1], newScenePoints[id2],
                                                newObj[id1], newObjNormal[id1], newObj[id2]);

    std::vector<int> validPoints;
    for (int i = 0; i < newObj.size(); i++)
    {
      cv::Point3f delta = newScenePoints[i] - transformByMat44(pose, newObj[i]);
      float dist = delta.dot(delta);
      if (dist < 0.001 * 0.001)
        validPoints.push_back(i);
    }
    if (validPoints.size() > bestValidPoints.size())
    {

      bestPose = pose;
      bestValidPoints = validPoints;
    }
  }
  for (int i = 0; i < bestValidPoints.size(); i++)
  {
    mask.at<unsigned char>(selectedId[bestValidPoints[i]], 0) = 1;
  }
  return bestPose;
}

RgbdFrame extractDataFromRos(cv::Mat colorK, cv::Mat colorImg, cv::Mat depthK, cv::Mat depthImg, cv::Mat Extrinsic)
{
  RgbdFrame frame;

  frame.depth_intrin = rs::intrinsics();
  frame.depth_intrin.width = depthImg.cols;
  frame.depth_intrin.height = depthImg.rows;
  frame.depth_intrin.ppx = depthK.at<double>(0, 2);
  frame.depth_intrin.ppy = depthK.at<double>(1, 2);
  frame.depth_intrin.fx = depthK.at<double>(0, 0);
  frame.depth_intrin.fy = depthK.at<double>(1, 1);
  ((rs_intrinsics *)&frame.depth_intrin)->model = (rs_distortion)rs::distortion::inverse_brown_conrady;
  for (int i = 0; i < 5; i++)
    frame.depth_intrin.coeffs[i] = 0; // Approximated to 0 for now.

  frame.color_intrin = rs::intrinsics();
  frame.color_intrin.width = colorImg.cols;
  frame.color_intrin.height = colorImg.rows;
  frame.color_intrin.ppx = colorK.at<double>(0, 2);
  frame.color_intrin.ppy = colorK.at<double>(1, 2);
  frame.color_intrin.fx = colorK.at<double>(0, 0);
  frame.color_intrin.fy = colorK.at<double>(1, 1);
  ((rs_intrinsics *)&frame.color_intrin)->model = (rs_distortion)rs::distortion::modified_brown_conrady;
  for (int i = 0; i < 5; i++)
    frame.color_intrin.coeffs[i] = 0; // Approximated to 0 for now.

  for (int i = 0; i < 3; i++)
  {
    frame.depth_to_color.translation[i] = Extrinsic.at<double>(i, 3);
    for (int j = 0; j < 3; j++)
      frame.depth_to_color.rotation[j * 3 + i] = Extrinsic.at<double>(i, j);
  }
  frame.scale = 0.001;

  frame.depth = depthImg.clone();
  frame.img = colorImg.clone();

  return frame;
}

bool rayTriangleIntersect(const cv::Point3f &orig, const cv::Point3f &dir, const cv::Point3f &v0, const cv::Point3f &v1,
                          const cv::Point3f &v2, float &t)
{
  // Compute plane's normal.
  cv::Point3f v0v1 = v1 - v0;
  cv::Point3f v0v2 = v2 - v0;
  // No need to normalize.
  cv::Point3f N = v0v1.cross(v0v2);
  float area2 = sqrt(N.dot(N));

  // Step 1: Finding P.

  // Check if the ray and the plane are parallel.
  float NdotRayDirection = N.dot(dir);
  if (fabs(NdotRayDirection) < 0.00001) // Almost 0.
    return false; // They are parallel so they don't intersect.

  // Compute d parameter using equation 2.
  float d = N.dot(v0);

  // Compute t using equation 3.
  t = (N.dot(orig) + d) / NdotRayDirection;
  // Check if the triangle is in behind the ray.
  if (t < 0)
    return false; // The triangle is behind.

  // Compute the intersection point using equation 1.
  cv::Point3f P = orig + t * dir;

  // Step 2: Inside-outside test.

  cv::Point3f C; // Vector perpendicular to the triangle's plane.

  // Edge 0:
  cv::Point3f edge0 = v1 - v0;
  cv::Point3f vp0 = P - v0;
  C = edge0.cross(vp0);
  if (N.dot(C) < 0)
    return false; // P is on the right side.

  // Edge 1:
  cv::Point3f edge1 = v2 - v1;
  cv::Point3f vp1 = P - v1;
  C = edge1.cross(vp1);
  if (N.dot(C) < 0)
    return false; // P is on the right side.

  // Edge 2:
  cv::Point3f edge2 = v0 - v2;
  cv::Point3f vp2 = P - v2;
  C = edge2.cross(vp2);
  if (N.dot(C) < 0)
    return false; // P is on the right side.

  return true; // This ray hits the triangle.
}

void Barycentric(cv::Point2f p, cv::Point2f a, cv::Point2f b, cv::Point2f c, float &u, float &v, float &w)
{
  cv::Point2f v0 = b - a, v1 = c - a, v2 = p - a;
  float d00 = v0.dot(v0);
  float d01 = v0.dot(v1);
  float d11 = v1.dot(v1);
  float d20 = v2.dot(v0);
  float d21 = v2.dot(v1);
  float denom = d00 * d11 - d01 * d01;
  v = (d11 * d20 - d01 * d21) / denom;
  w = (d00 * d21 - d01 * d20) / denom;
  u = 1.0f - v - w;
}

// Get the raw data and build the current frames cloud.
RgbdFrame generatePointCloudOrganized2(cv::Mat colorK, cv::Mat colorImg, cv::Mat depthK, cv::Mat depthImg,
                                       cv::Mat Extrinsic)
{

  RgbdFrame frame = extractDataFromRos(colorK, colorImg, depthK, depthImg, Extrinsic);
  frame.imgDepth = cv::Mat(frame.img.rows, frame.img.cols, CV_32F);
  frame.imgDepth.setTo(cv::Scalar(0.0));
  frame.imgPoints->clear();
  frame.imgPoints->resize(frame.img.rows * frame.img.cols);
  frame.imgPoints->is_dense = false;
  frame.imgPoints->width = frame.img.cols;
  frame.imgPoints->height = frame.img.rows;
  for (int i = 0; i < frame.imgPoints->height; i++)
    for (int j = 0; j < frame.imgPoints->width; j++)
    {
      int id = i * frame.imgPoints->width + j;
      frame.imgPoints->points[id].x = 0.0;
      frame.imgPoints->points[id].y = 0.0;
      frame.imgPoints->points[id].z = 0.0;
    }

  // Depth dimension helpers.
  int dw = 0;
  int dh = 0;
  int dwh = 0;

  dw = frame.imgDepth.cols;
  dh = frame.imgDepth.rows;

  dwh = dw * dh;

  // Set up the cloud to be used.
  frame.rs_cloud_ptr->clear();
  frame.rs_cloud_ptr->resize(dwh);
  frame.rs_cloud_ptr->is_dense = false;
  frame.rs_cloud_ptr->width = dw;
  frame.rs_cloud_ptr->height = dh;

  static const float nan = std::numeric_limits<float>::quiet_NaN();

  // Iterate the data space.

  // First, iterate across columns:
  for (int dy = 0; dy < dh; dy++)
  {
    // Second, iterate across rows:
    for (int dx = 0; dx < dw; dx++)
    {
      uint i = dy * dw + dx;
      uint16_t depth_value = frame.depth.at<uint16_t>(dy, dx);

      if (depth_value == 0)
      {
        frame.rs_cloud_ptr->points[i].x = (float)nan;
        frame.rs_cloud_ptr->points[i].y = (float)nan;
        frame.rs_cloud_ptr->points[i].z = (float)nan;
        continue;
      }

      cv::Point2f depth_pixel((float)dx, (float)dy);
      float depth_in_meters = depth_value * frame.scale;

      cv::Point3f depth_point = frame.depthCameraDeproject(depth_pixel, depth_in_meters);
      cv::Point3f color_point = frame.depthCameraToColorCamera(depth_point);
      cv::Point2f color_pixel = frame.colorCameraProject(color_point);

      const int cx = (int)std::round(color_pixel.x);
      const int cy = (int)std::round(color_pixel.y);

      // Set up logic to remove bad points.
      bool depth_fail = true;
      bool color_fail = true;

      depth_fail = (depth_point.z > NOISY);
      color_fail = (cx < 0 || cy < 0 || cx > frame.img.cols || cy > frame.img.rows);

      // Cloud input pointers.

      // XYZ input access to cloud.
      float *dp_x;
      float *dp_y;
      float *dp_z;

      dp_x = &(frame.rs_cloud_ptr->points[i].x);
      dp_y = &(frame.rs_cloud_ptr->points[i].y);
      dp_z = &(frame.rs_cloud_ptr->points[i].z);

      // RGB input access to cloud.
      uint8_t *cp_r;
      uint8_t *cp_g;
      uint8_t *cp_b;

      cp_r = &(frame.rs_cloud_ptr->points[i].r);
      cp_g = &(frame.rs_cloud_ptr->points[i].g);
      cp_b = &(frame.rs_cloud_ptr->points[i].b);

      // Cloud input data.

      // Set up depth point data.
      float real_x = 0;
      float real_y = 0;
      float real_z = 0;
      float adjusted_x = 0;
      float adjusted_y = 0;
      float adjusted_z = 0;

      real_x = depth_point.x;
      real_y = depth_point.y;
      real_z = depth_point.z;

      // Adjust point to coordinates.
      adjusted_x = -1 * real_x;
      adjusted_y = -1 * real_y;
      adjusted_z = real_z;

      // Set up color point data.
      uint8_t *offset = frame.img.ptr<uint8_t>(cy) + cx * 3;

      uint8_t raw_r = 0;
      uint8_t raw_g = 0;
      uint8_t raw_b = 0;
      uint8_t adjusted_r = 0;
      uint8_t adjusted_g = 0;
      uint8_t adjusted_b = 0;

      // Cloud point evaluation.

      // If bad point, remove and skip.
      if (depth_fail || color_fail)
      {
        *dp_x = *dp_y = *dp_z = (float)nan;
        *cp_r = *cp_g = *cp_b = 255;
        continue;
      }

      // If valid point, add data to cloud.
      else
      {
        raw_r = raw_r = *(offset);
        raw_g = raw_g = *(offset + 1);
        raw_b = raw_b = *(offset + 2);

        // Adjust color arbitrarily.
        adjusted_r = raw_r;
        adjusted_g = raw_g;
        adjusted_b = raw_b;

        // Fill in cloud depth.
        *dp_x = adjusted_x;
        *dp_y = adjusted_y;
        *dp_z = adjusted_z;

        // Fill in cloud color.
        *cp_r = adjusted_r;
        *cp_g = adjusted_g;
        *cp_b = adjusted_b;
      }
    }
  }

  for (int i = 1; i < frame.depth.rows; i++)
  {
    for (int j = 1; j < frame.depth.cols; j++)
    {
      int x[4] = {j - 1, j, j, j - 1}, y[4] = {i - 1, i - 1, i, i};
      uint16_t depth_value[4];
      float depth_in_meters[4];
      cv::Point3f color_point[4];
      cv::Point2f color_pixel[4];
      bool valid = true;
      for (int k = 0; k < 4; k++)
      {
        depth_value[k] = frame.depth.at<uint16_t>(y[k], x[k]);
        depth_in_meters[k] = depth_value[k] * frame.scale;
        cv::Point2f depth_pixel((float)x[k], (float)y[k]);
        cv::Point3f depth_point = frame.depthCameraDeproject(depth_pixel, depth_in_meters[k]);
        color_point[k] = frame.depthCameraToColorCamera(depth_point);
        color_pixel[k] = frame.colorCameraProject(color_point[k]);
        if (depth_value[k] == 0
            || (color_pixel[k].x < 0 || color_pixel[k].y < 0 || color_pixel[k].x > frame.img.cols
                || color_pixel[k].y > frame.img.rows))
          valid = false;
      }
      if (!valid)
        continue;
      for (int k = 0; k < 2; k++)
      {
        cv::Point3f color_pointTri[3] = {color_point[0], color_point[2], (k == 0 ? color_point[1] : color_point[3])};
        cv::Point2f color_pixelTri[3] = {color_pixel[0], color_pixel[2], (k == 0 ? color_pixel[1] : color_pixel[3])};
        cv::Point2f pixMin = {std::min(color_pixelTri[0].x, std::min(color_pixelTri[1].x, color_pixelTri[2].x)),
                              std::min(color_pixelTri[0].y, std::min(color_pixelTri[1].y, color_pixelTri[2].y))};
        cv::Point2f pixMax = {std::max(color_pixelTri[0].x, std::max(color_pixelTri[1].x, color_pixelTri[2].x)),
                              std::max(color_pixelTri[0].y, std::max(color_pixelTri[1].y, color_pixelTri[2].y))};
        for (int x = (int)floor(pixMin.x); x < (int)ceil(pixMax.x); x++)
          for (int y = (int)floor(pixMin.y); y < (int)ceil(pixMax.y); y++)
          {
            if (x < 0 || y < 0 || x > frame.img.cols || y > frame.img.rows)
              continue;
            rs::float2 p = {float(x), float(y)};
            float u, v, w;
            Barycentric(rs2cv(p), color_pixelTri[0], color_pixelTri[1], color_pixelTri[2], u, v, w);
            if (u < 0 || u > 1 || v < 0 || v > 1)
              continue;
            cv::Point3f res = u * color_pointTri[0] + v * color_pointTri[1] + w * color_pointTri[2];
            frame.imgDepth.ptr<float>(y)[x] = sqrt(res.dot(res));
            int id = y * frame.imgPoints->width + x;
            frame.imgPoints->points[id].x = res.x;
            frame.imgPoints->points[id].y = res.y;
            frame.imgPoints->points[id].z = res.z;
          }
      }
    }
  }

  pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  ne.setNormalEstimationMethod(ne.AVERAGE_3D_GRADIENT);
  ne.setMaxDepthChangeFactor(0.02f);
  ne.setNormalSmoothingSize(10.0f);
  ne.setInputCloud(frame.imgPoints);
  ne.compute(*(frame.imgNormals));

  return frame;
}

bool allThere()
{
  if (color_image_ishere and color_info_ishere and depth_image_ishere and depth_info_ishere)
  {
    RgbdFrame frame = generatePointCloudOrganized2(color_K, color_image, depth_K, depth_image,
                                                   depth_to_color_extrinsics);

    cv::Mat frameImg;
    frameImg = frame.img.clone();

    std::vector<cv::KeyPoint> keypoints2;
    cv::Mat descriptors2;
    akaze->detectAndCompute(frameImg, cv::Mat(), keypoints2, descriptors2);

    if (keypoints2.size() == 0)
      return true;

    std::vector<std::vector<cv::DMatch>> matches(listImg[selectedImg].size());
    std::vector<std::vector<cv::DMatch>> good_matches(listImg[selectedImg].size());
    for (int face = 0; face < listImg[selectedImg].size(); face++)
    {
      matcher->match(descriptors1[selectedImg][face], descriptors2, matches[face]);

      double max_dist = 0;
      double min_dist = 100;
      // Quick calculation of max and min distances between keypoints.
      for (int i = 0; i < matches[face].size(); i++)
      {
        double dist = matches[face][i].distance;
        if (dist < min_dist)
          min_dist = dist;
        if (dist > max_dist)
          max_dist = dist;
      }

      //printf("Max dist: %f \n", max_dist );
      //printf("Min dist: %f \n", min_dist );

      // Draw only good matches (whose distance is less than 3*min_dist)
      for (int i = 0; i < matches[face].size(); i++)
      {
        if (matches[face][i].distance < 3 * min_dist)
          good_matches[face].push_back(matches[face][i]);
      }
    }

    std::vector<cv::Point3f> obj;
    std::vector<cv::Point3f> normal;
    std::vector<cv::Point2f> scene;
    for (int face = 0; face < listImg[selectedImg].size(); face++)
      for (size_t i = 0; i < good_matches[face].size(); i++)
      {
        // Get the keypoints from the good matches.
        cv::Point2f p = keypoints1[selectedImg][face][good_matches[face][i].queryIdx].pt;
        if (listCorners[selectedImg][face][0].z == 1)
        {
          obj.push_back((0.001 / scale) * cv::Point3f(p.x, p.y, scale * listSize[selectedImg].z));
          normal.push_back(cv::Point3f(0, 0, 1));
        }
        else
        {
          obj.push_back((0.001 / scale) * cv::Point3f(p.x, p.y, 0));
          normal.push_back(cv::Point3f(0, 0, -1));
        }
        scene.push_back(keypoints2[good_matches[face][i].trainIdx].pt);
      }
    cv::Mat mask;
    cv::Mat P = findPoseWithDepthCue(obj, normal, scene, frame, mask);
    int startId = 0;
    for (int face = 0; face < listImg[selectedImg].size(); face++)
    {
      matches[face] = good_matches[face];
      good_matches[face].clear();
      for (int i = startId; i < startId + matches[face].size(); i++)
      {
        if (mask.at<unsigned char>(i, 0) > 0)
        {
          good_matches[face].push_back(matches[face][i - startId]);
        }
      }
      obj.clear();
      scene.clear();
      for (size_t i = 0; i < good_matches[face].size(); i++)
      {
        // Get the keypoints from the good matches.
        cv::Point2f p = keypoints1[selectedImg][face][good_matches[face][i].queryIdx].pt;
        obj.push_back((0.001 / scale) * cv::Point3f(p.x, p.y, 0));
        scene.push_back(keypoints2[good_matches[face][i].trainIdx].pt);
      }
      startId += matches[face].size();
    }
    std::vector<cv::Mat> img_matches(listImg[selectedImg].size());
    for (int face = 0; face < listImg[selectedImg].size(); face++)
      cv::drawMatches(listRectifiedImg[selectedImg][face], keypoints1[selectedImg][face], frameImg, keypoints2,
                      good_matches[face], img_matches[face]);

    for (int face = 0; face < listImg[selectedImg].size(); face++)
      if (P.cols > 0)
      {
        cv::Point3f tl = transformByMat44(P, (0.001 / scale) * cv::Point3f(0, 0, 0));
        cv::Point3f tr = transformByMat44(
            P, (0.001 / scale) * cv::Point3f(listRectifiedImg[selectedImg][face].cols, 0, 0));
        cv::Point3f br = transformByMat44(
            P,
            (0.001 / scale)
                * cv::Point3f(listRectifiedImg[selectedImg][face].cols, listRectifiedImg[selectedImg][face].rows, 0));
        cv::Point3f bl = transformByMat44(
            P, (0.001 / scale) * cv::Point3f(0, listRectifiedImg[selectedImg][face].rows, 0));

        cv::Point3f tlb = transformByMat44(P, (0.001 / scale) * cv::Point3f(0, 0, scale * listSize[selectedImg].z));
        cv::Point3f trb = transformByMat44(
            P,
            (0.001 / scale)
                * cv::Point3f(listRectifiedImg[selectedImg][face].cols, 0, scale * listSize[selectedImg].z));
        cv::Point3f brb = transformByMat44(
            P,
            (0.001 / scale)
                * cv::Point3f(listRectifiedImg[selectedImg][face].cols, listRectifiedImg[selectedImg][face].rows,
                              scale * listSize[selectedImg].z));
        cv::Point3f blb = transformByMat44(
            P,
            (0.001 / scale)
                * cv::Point3f(0, listRectifiedImg[selectedImg][face].rows, scale * listSize[selectedImg].z));

        cv::Point3f com = (tl + tr + br + bl + tlb + trb + brb + blb) / 8;

        // transform to camera coordinate system
        const cv::Mat ImgToCamera = RTVec2Mat(point2Mat(cv::Point3d(0, 0, -CV_PI / 2)),
                                              point2Mat(cv::Point3d(0, 0, 0))); // this may not be necessary as the camera is publishing also its frames
        com = multMatVec(ImgToCamera, com);

        //Publishing item pose into topic
        itemPose.pose.position.x = com.x;
        itemPose.pose.position.y = com.y;
        itemPose.pose.position.z = com.z;
        itemPose.pose.orientation.x = 0.0;
        itemPose.pose.orientation.y = 0.0;
        itemPose.pose.orientation.z = 0.0;
        itemPose.pose.orientation.w = 1.0;
        itemPose.header.stamp = ros::Time::now();
        itemPose.header.frame_id = "tnp_ee_camera_frame";
        pub_item_pose.publish(itemPose);

        cv::Point2f tl2 = rs2cv(frame.color_intrin.project(cv2rs(tl)));
        cv::Point2f tr2 = rs2cv(frame.color_intrin.project(cv2rs(tr)));
        cv::Point2f br2 = rs2cv(frame.color_intrin.project(cv2rs(br)));
        cv::Point2f bl2 = rs2cv(frame.color_intrin.project(cv2rs(bl)));

        cv::Point2f tl2b = rs2cv(frame.color_intrin.project(cv2rs(tlb)));
        cv::Point2f tr2b = rs2cv(frame.color_intrin.project(cv2rs(trb)));
        cv::Point2f br2b = rs2cv(frame.color_intrin.project(cv2rs(brb)));
        cv::Point2f bl2b = rs2cv(frame.color_intrin.project(cv2rs(blb)));

        conf[face].data = 1.0;
        cv::Scalar color(0, 255, 0);
        std::vector<cv::Point> listPoints;
        std::vector<std::vector<cv::Point>> hulls(1);
        for (int i = 0; i < good_matches[face].size(); i++)
        {
          listPoints.push_back(keypoints2[good_matches[face][i].trainIdx].pt);
        }
        cv::convexHull(cv::Mat(listPoints), hulls[0], false);
        cv::drawContours(
            img_matches[face](cv::Rect(listRectifiedImg[selectedImg][face].cols, 0, frameImg.cols, frameImg.rows)),
            hulls, 0, cv::Scalar(255, 255, 0), 1, 8, std::vector<cv::Vec4i>(), 0, cv::Point());
        double area = hulls[0].size() >= 3 ? cv::contourArea(hulls[0]) : 0;
        //printf("Area: %lf px, ratio %lf\n", area, area/(frameImg.cols * frameImg.rows));
        if (good_matches[face].size() < 5 || area < frameImg.cols * frameImg.rows / 400)
        {
          color = cv::Scalar(0, 0, 255);
          conf[face].data = 0.33;
        }
        else if (good_matches[face].size() < 10 || area < frameImg.cols * frameImg.rows / 20)
        {
          color = cv::Scalar(0, 255, 255);
          conf[face].data = 0.66;
        }

        char txt[255];
        sprintf(txt, "%lf, %lf, %lf", com.x, com.y, com.z);
        cv::putText(img_matches[face], txt, cv::Point(50, 50), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0), 2);
        //printf("COM in camera reference frame: %s\n", txt);

        cv::line(img_matches[face], tl2 + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0),
                 tr2 + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0), color, 4);
        cv::line(img_matches[face], tr2 + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0),
                 br2 + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0), color, 4);
        cv::line(img_matches[face], br2 + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0),
                 bl2 + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0), color, 4);
        cv::line(img_matches[face], bl2 + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0),
                 tl2 + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0), color, 4);

        cv::line(img_matches[face], tl2 + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0),
                 tl2b + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0), color, 4);
        cv::line(img_matches[face], tr2 + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0),
                 tr2b + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0), color, 4);
        cv::line(img_matches[face], br2 + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0),
                 br2b + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0), color, 4);
        cv::line(img_matches[face], bl2 + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0),
                 bl2b + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0), color, 4);

        cv::line(img_matches[face], tl2b + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0),
                 tr2b + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0), color, 4);
        cv::line(img_matches[face], tr2b + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0),
                 br2b + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0), color, 4);
        cv::line(img_matches[face], br2b + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0),
                 bl2b + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0), color, 4);
        cv::line(img_matches[face], bl2b + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0),
                 tl2b + cv::Point2f(listRectifiedImg[selectedImg][face].cols, 0), color, 4);
      }

    // Show detected matches.
    char filename[255];
    for (int face = 0; face < listImg[selectedImg].size(); face++)
    {
      sprintf(filename, "Good Matches & Object detection %d", face);
      cv::imshow(filename, img_matches[face]);
      if (face == 0)
      {
        conf[4].data = conf[face].data;
      }
      else
      {
        if (conf[4].data < conf[face].data)
        {
          conf[4].data = conf[face].data;
        }
      }
    }

    //Publish the highest confidence among the different faces
    pub_confidence.publish(conf[4]);

    cv::waitKey(1);

    color_image_ishere = false;
    depth_image_ishere = false;
  }
}

void colorImageCallback(const sensor_msgs::ImageConstPtr &msg)
{
  color_image = cv_bridge::toCvShare(msg, "bgr8")->image.clone();
  color_image_ishere = true;
  allThere();
}

void colorInfoCallback(const sensor_msgs::CameraInfoConstPtr &msg)
{
  color_K = cv::Mat(3, 3, CV_64F, (void *)&msg->K[0], cv::Mat::AUTO_STEP).clone();

  color_info_fx = msg->K[0];
  color_info_fy = msg->K[4];
  color_info_ppx = msg->K[2];
  color_info_ppy = msg->K[5];
  color_info_tx = msg->P[3];
  color_info_ty = msg->P[7];
  color_info_tz = msg->P[11];
  color_info_ishere = true;
}

void depthImageCallback(const sensor_msgs::ImageConstPtr &msg)
{
  depth_image = cv_bridge::toCvShare(msg, "16UC1")->image.clone();
  depth_image_ishere = true;
  allThere();
}

void depthInfoCallback(const sensor_msgs::CameraInfoConstPtr &msg)
{
  cv::Mat R = cv::Mat::eye(3, 3, CV_64F);
  if (USE_HARDCODED_ROTATION)
  {
    R.at<double>(0, 0) = 0.999987;
    R.at<double>(0, 1) = 0.004618;
    R.at<double>(0, 2) = 0.002142;
    R.at<double>(1, 0) = -0.004655;
    R.at<double>(1, 1) = 0.999834;
    R.at<double>(1, 2) = 0.017596;
    R.at<double>(2, 0) = -0.002060;
    R.at<double>(2, 1) = -0.017605;
    R.at<double>(2, 2) = 0.999843;
    R = R.t(); // Transpose because RS uses column-major, and OpenCV uses row-major.
  }
  depth_to_color_extrinsics = cv::Mat::eye(4, 4, CV_64F);
  depth_to_color_extrinsics.at<double>(0, 3) = msg->P[3];
  depth_to_color_extrinsics.at<double>(1, 3) = msg->P[7];
  depth_to_color_extrinsics.at<double>(2, 3) = msg->P[11];
  R.copyTo(depth_to_color_extrinsics(cv::Rect(0, 0, 3, 3)));

  depth_K = cv::Mat(3, 3, CV_64F, (void *)&msg->K[0], cv::Mat::AUTO_STEP).clone();

  depth_info_fx = msg->K[0];
  depth_info_fy = msg->K[4];
  depth_info_ppx = msg->K[2];
  depth_info_ppy = msg->K[5];
  depth_info_tx = msg->P[3];
  depth_info_ty = msg->P[7];
  depth_info_tz = msg->P[11];
  depth_info_ishere = true;
}

void itemIdCallback(const std_msgs::Int16::ConstPtr &msg)
{
  char filename[255];
  if (selectedImg != msg->data && msg->data <= listImg.size() - 1)
  {
    for (int face = 0; face < listImg[selectedImg].size(); face++)
    {
      sprintf(filename, "Good Matches & Object detection %d", face);
      cv::destroyWindow(filename);
    }

    selectedImg = msg->data;
  }
}

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

  scale = 2.0;

  cv::Mat image;
  image = cv::imread("crayola.jpg");

  if (!image.data)
  {
    ROS_ERROR("NO IMAGES WHERE FOUND ON /ROOT/.ROS DIRECTORY THE PROCESS CANNOT CONTINUE");
    return 0;
  }

  std::vector<cv::Mat> crayolaImg;
  std::vector<std::vector<cv::Point3f>> crayolaCorners(1);
  crayolaImg.push_back(cv::imread("crayola.jpg"));
  crayolaCorners[0].push_back(cv::Point3f(389, 115, 0));
  crayolaCorners[0].push_back(cv::Point3f(1018, 117, 0));
  crayolaCorners[0].push_back(cv::Point3f(978, 661, 0));
  crayolaCorners[0].push_back(cv::Point3f(395, 616, 0));
  listImg.push_back(crayolaImg);
  listCorners.push_back(crayolaCorners);
  listSize.push_back(cv::Point3f(145, 125, 37));

  std::vector<cv::Mat> expoImg;
  std::vector<std::vector<cv::Point3f>> expoCorners(2);
  expoImg.push_back(cv::imread("expo1.jpg"));
  expoCorners[0].push_back(cv::Point3f(3308, 2173, 0));
  expoCorners[0].push_back(cv::Point3f(1059, 2178, 0));
  expoCorners[0].push_back(cv::Point3f(994, 1264, 0));
  expoCorners[0].push_back(cv::Point3f(3383, 1239, 0));
  expoImg.push_back(cv::imread("expo2.jpg"));
  expoCorners[1].push_back(cv::Point3f(843, 2256, 1));
  expoCorners[1].push_back(cv::Point3f(3544, 2122, 1));
  expoCorners[1].push_back(cv::Point3f(3534, 950, 1));
  expoCorners[1].push_back(cv::Point3f(692, 1160, 1));
  listImg.push_back(expoImg);
  listCorners.push_back(expoCorners);
  listSize.push_back(cv::Point3f(130, 53, 34));

  std::vector<cv::Mat> robotsImg;
  std::vector<std::vector<cv::Point3f>> robotsCorners(2);
  robotsImg.push_back(cv::imread("robots1.jpg"));
  robotsCorners[0].push_back(cv::Point3f(246, 998, 0));
  robotsCorners[0].push_back(cv::Point3f(2644, 928, 0));
  robotsCorners[0].push_back(cv::Point3f(2728, 3960, 0));
  robotsCorners[0].push_back(cv::Point3f(240, 3934, 0));
  robotsImg.push_back(cv::imread("robots2.jpg"));
  if (true)
  {
    robotsCorners[1].push_back(cv::Point3f(2880, 400, 1));
    robotsCorners[1].push_back(cv::Point3f(84, 516, 1));
    robotsCorners[1].push_back(cv::Point3f(99, 3907, 1));
    robotsCorners[1].push_back(cv::Point3f(2982, 3934, 1));
  }
  else
  {
    robotsCorners[1].push_back(cv::Point3f(84, 516, 0));
    robotsCorners[1].push_back(cv::Point3f(2880, 400, 0));
    robotsCorners[1].push_back(cv::Point3f(2982, 3934, 0));
    robotsCorners[1].push_back(cv::Point3f(99, 3907, 0));
  }
  listImg.push_back(robotsImg);
  listCorners.push_back(robotsCorners);
  listSize.push_back(cv::Point3f(166, 201, 4));

  std::vector<cv::Mat> pencilImg;
  std::vector<std::vector<cv::Point3f>> pencilCorners(2);
  pencilImg.push_back(cv::imread("pencil1.jpg"));
  pencilCorners[0].push_back(cv::Point3f(3400, 772, 0));
  pencilCorners[0].push_back(cv::Point3f(3396, 2256, 0));
  pencilCorners[0].push_back(cv::Point3f(88, 2240, 0));
  pencilCorners[0].push_back(cv::Point3f(132, 644, 0));
  pencilImg.push_back(cv::imread("pencil2.jpg"));
  pencilCorners[1].push_back(cv::Point3f(532, 1068, 1));
  pencilCorners[1].push_back(cv::Point3f(556, 2612, 1));
  pencilCorners[1].push_back(cv::Point3f(3920, 2648, 1));
  pencilCorners[1].push_back(cv::Point3f(3964, 1048, 1));
  listImg.push_back(pencilImg);
  listCorners.push_back(pencilCorners);
  listSize.push_back(cv::Point3f(91, 193, 9));

  std::vector<cv::Mat> kleenexImg;
  std::vector<std::vector<cv::Point3f>> kleenexCorners(1);
  kleenexImg.push_back(cv::imread("kleenex.jpg"));
  kleenexCorners[0].push_back(cv::Point3f(3564, 156, 0));
  kleenexCorners[0].push_back(cv::Point3f(3636, 2928, 0));
  kleenexCorners[0].push_back(cv::Point3f(864, 2964, 0));
  kleenexCorners[0].push_back(cv::Point3f(872, 132, 0));
  listImg.push_back(kleenexImg);
  listCorners.push_back(kleenexCorners);
  listSize.push_back(cv::Point3f(111, 111, 127));

  cv::Mat img = cv::imread("robots1.jpg");

  for (int i = 0; i < listImg.size(); i++)
  {
    std::vector<cv::Mat> img2(listImg[i].size());
    for (int j = 0; j < listImg[i].size(); j++)
    {
      std::vector<cv::Point2f> corners(4);
      for (int k = 0; k < 4; k++)
        corners[k] = cv::Point2f(listCorners[i][j][k].x, listCorners[i][j][k].y);
      std::vector<cv::Point2f> corners2(4);
      corners2[0] = scale * cv::Point2f(0, 0);
      corners2[1] = scale * cv::Point2f(listSize[i].x, 0);
      corners2[2] = scale * cv::Point2f(listSize[i].x, listSize[i].y);
      corners2[3] = scale * cv::Point2f(0, listSize[i].y);

      cv::Mat H = cv::findHomography(corners, corners2);
      cv::warpPerspective(listImg[i][j], img2[j], H,
                          cv::Size((int)(scale * (listSize[i].x)), (int)(scale * (listSize[i].y))));
      cv::waitKey(500);
    }
    listRectifiedImg.push_back(img2);
  }

  akaze = cv::AKAZE::create();

  matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");

  for (int i = 0; i < listImg.size(); i++)
  {
    keypoints1.push_back(std::vector<std::vector<cv::KeyPoint>>(listRectifiedImg[i].size()));
    descriptors1.push_back(std::vector<cv::Mat>(listRectifiedImg[i].size()));
    for (int j = 0; j < listImg[i].size(); j++)
    {
      akaze->detectAndCompute(listRectifiedImg[i][j], cv::Mat(), keypoints1[i][j], descriptors1[i][j]);
    }
  }

  ros::init(argc, argv, "tnp_image_viewer");
  ros::init(argc, argv, "tnp_plane_matching");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub_color_image, sub_depth_image;
  ros::Subscriber sub_color_info, sub_depth_info, sub_image_id;

  sub_color_image = it.subscribe("/tnp_image_converter/color/image_raw", 1, colorImageCallback);
  sub_color_info = nh.subscribe("/camera/color/camera_info", 1, colorInfoCallback);

  sub_depth_image = it.subscribe("/tnp_image_converter/depth/image_raw", 1, depthImageCallback);
  sub_depth_info = nh.subscribe("/camera/depth/camera_info", 1, depthInfoCallback);

  //Subscriber to selectedImg for changing between different objects
  sub_image_id = nh.subscribe("/tnp_vision/feature_based/target_item_id", 1, itemIdCallback);

  //Publishers for Item Pose and Confidence
  pub_item_pose = nh.advertise<geometry_msgs::PoseStamped>("/tnp_vision/feature_based/item_pose", 1);
  pub_confidence = nh.advertise<std_msgs::Float64>("/tnp_vision/feature_based/confidence", 1);
  pub_image_id = nh.advertise<std_msgs::Int16>("/tnp_vision/feature_based/target_item_id", 1);

  ros::spin();

  return 0;
}


/*
 * @file
 * @author University of Warwick
 * @version 1.0
 *
 * @section LICENSE
 *
 * @section DESCRIPTION
 *
 * Unit Tests for the concrete methods of the ParticleSystemSimple class
 */

#define BOOST_TEST_MODULE ParticleSystemSimple
#include <boost/test/unit_test.hpp>
#include <boost/test/output_test_stream.hpp>
#include <stdexcept>

#include "ParticleEmitterSimple.h"
#include "EuclideanPoint.h"
#include "PartitionerConfig.h"
#include "PartitionerNaiveConfig.h"
#include "MeshSourceStructGenConfig.h"
#include "MeshConfig.h"
#include "CupCfdAoSMesh.h"
#include "Error.h"
#include "DistributionFixed.h"
#include "DistributionNormal.h"
#include "DistributionUniform.h"
#include <memory>
#include "ParticleSimple.h"
#include "ParticleSystemSimple.h"

namespace utf = boost::unit_test;
namespace euc = cupcfd::geometry::euclidean;
namespace meshgeo = cupcfd::geometry::mesh;
namespace dist = cupcfd::distributions;

using namespace cupcfd::particles;

// Setup
BOOST_AUTO_TEST_CASE(setup)
{
    int argc = boost::unit_test::framework::master_test_suite().argc;
    char ** argv = boost::unit_test::framework::master_test_suite().argv;

    MPI_Init(&argc, &argv);

    cupcfd::error::eCodes status;

	// Need to register point, vector MPI datatype since the particle MPI datatype depends on them
	cupcfd::geometry::euclidean::EuclideanPoint<double, 3> point;
	status = point.registerMPIType();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	cupcfd::geometry::euclidean::EuclideanVector<double,3> vector;
	status = vector.registerMPIType();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	ParticleSimple<int, double> particle;
	status = particle.registerMPIType();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
}

// === Constructor ===
// Test 1:
BOOST_AUTO_TEST_CASE(constructor_test1)
{
	cupcfd::error::eCodes status;
	cupcfd::comm::Communicator comm(MPI_COMM_SELF);

	// === Create a small test mesh ===
	// Setup the configurations
	cupcfd::partitioner::PartitionerNaiveConfig<int,int> partConfig;
	cupcfd::geometry::mesh::MeshSourceStructGenConfig<int, double> meshSourceConfig(5, 5, 5, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
	cupcfd::geometry::mesh::MeshConfig<int,double,int> meshConfig(partConfig, meshSourceConfig);

	// Build the mesh
	cupcfd::geometry::mesh::CupCfdAoSMesh<int,double,int> * mesh;
	status = meshConfig.buildUnstructuredMesh(&mesh, comm);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	std::shared_ptr<meshgeo::CupCfdAoSMesh<int,double,int>> meshPtr(mesh);

	// Create the particle system
	ParticleSystemSimple<meshgeo::CupCfdAoSMesh<int,double,int>, int, double, int> particleSystem(meshPtr);

	// ToDo: What to test?
	// Check mesh properties stored inside system are correct?
}

// === addParticle ===
BOOST_AUTO_TEST_CASE(addParticle_test1, * utf::tolerance(0.00001))
{
	cupcfd::error::eCodes status;
	cupcfd::comm::Communicator comm(MPI_COMM_SELF);

	// === Create a small test mesh ===
	// Setup the configurations
	cupcfd::partitioner::PartitionerNaiveConfig<int,int> partConfig;
	cupcfd::geometry::mesh::MeshSourceStructGenConfig<int, double> meshSourceConfig(5, 5, 5, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
	cupcfd::geometry::mesh::MeshConfig<int,double,int> meshConfig(partConfig, meshSourceConfig);

	// Build the mesh
	cupcfd::geometry::mesh::CupCfdAoSMesh<int,double,int> * mesh;
	status = meshConfig.buildUnstructuredMesh(&mesh, comm);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	std::shared_ptr<meshgeo::CupCfdAoSMesh<int,double,int>> meshPtr(mesh);

	// Create the particle system
	ParticleSystemSimple<meshgeo::CupCfdAoSMesh<int,double,int>, int, double, int> system(meshPtr);

	// Add a particle
	cupcfd::geometry::euclidean::EuclideanPoint<double,3> pos1(0.12, 0.11, 0.14);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> velocity1(1.0, 1.1, 1.2);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> acceleration1(0.0, 0.0, 0.0);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> jerk1(0.0, 0.0, 0.0);
	uint pID=0, cellID=0, rank=0;
	ParticleSimple<int,double> particle1(pos1, velocity1, acceleration1, jerk1, pID, cellID, rank, 1000.0, 0.0, 0.0);

	status = system.addParticle(particle1);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	BOOST_CHECK_EQUAL(system.getNActiveParticles(), 1);	// 1 Active Particle
	BOOST_CHECK_EQUAL(system.getNTravelParticles(), 0); // Travel time was set to 0, so no travelling particles

	// ToDo: Further Checks
}

// === addParticleEmitter ===
// Test 1:
BOOST_AUTO_TEST_CASE(addParticleEmitter_test1, * utf::tolerance(0.00001))
{
	cupcfd::error::eCodes status;
	cupcfd::comm::Communicator comm(MPI_COMM_SELF);

	// === Create a small test mesh ===
	// Setup the configurations
	cupcfd::partitioner::PartitionerNaiveConfig<int,int> partConfig;
	cupcfd::geometry::mesh::MeshSourceStructGenConfig<int, double> meshSourceConfig(5, 5, 5, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
	cupcfd::geometry::mesh::MeshConfig<int,double,int> meshConfig(partConfig, meshSourceConfig);

	// Build the mesh
	cupcfd::geometry::mesh::CupCfdAoSMesh<int,double,int> * mesh;
	status = meshConfig.buildUnstructuredMesh(&mesh, comm);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	std::shared_ptr<meshgeo::CupCfdAoSMesh<int,double,int>> meshPtr(mesh);

	// Create the particle system
	ParticleSystemSimple<meshgeo::CupCfdAoSMesh<int,double,int>, int, double, int> system(meshPtr);

	// Add an emitter to the system
	cupcfd::geometry::euclidean::EuclideanPoint<double, 3> position(2.0, 3.0, 4.0);
    dist::DistributionFixed<int,double> rate(2.3);
    dist::DistributionFixed<int,double> angleXY(78);
    dist::DistributionFixed<int,double> angleRotation(62);
    dist::DistributionFixed<int,double> speed(0.052);
    dist::DistributionFixed<int,double> accelerationX(0.012);
    dist::DistributionFixed<int,double> accelerationY(0.012);
    dist::DistributionFixed<int,double> accelerationZ(0.012);
    dist::DistributionFixed<int,double> jerkX(0.01);
    dist::DistributionFixed<int,double> jerkY(0.01);
    dist::DistributionFixed<int,double> jerkZ(0.01);
    dist::DistributionFixed<int,double> decayRate(0.1);
    dist::DistributionFixed<int,double> decayThreshold(10);

    uint emitterID=0;
    ParticleEmitterSimple<int,double> emitter(0, 10, comm.rank, emitterID, position, &rate, &angleXY, &angleRotation, &speed,
			  &accelerationX, &accelerationY, &accelerationZ,
			  &jerkX, &jerkY, &jerkZ,
			  &decayRate, &decayThreshold);

    status = system.addParticleEmitter(emitter);
    BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
}

// === setParticleInactive

// === removeInactiveParticles ===
BOOST_AUTO_TEST_CASE(removeInactiveParticles_test1, * utf::tolerance(0.00001))
{
	cupcfd::error::eCodes status;
	cupcfd::comm::Communicator comm(MPI_COMM_SELF);

	// === Create a small test mesh ===
	// Setup the configurations
	cupcfd::partitioner::PartitionerNaiveConfig<int,int> partConfig;
	cupcfd::geometry::mesh::MeshSourceStructGenConfig<int, double> meshSourceConfig(5, 5, 5, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
	cupcfd::geometry::mesh::MeshConfig<int,double,int> meshConfig(partConfig, meshSourceConfig);

	// Build the mesh
	cupcfd::geometry::mesh::CupCfdAoSMesh<int,double,int> * mesh;
	status = meshConfig.buildUnstructuredMesh(&mesh, comm);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	std::shared_ptr<meshgeo::CupCfdAoSMesh<int,double,int>> meshPtr(mesh);

	// Create the particle system
	ParticleSystemSimple<meshgeo::CupCfdAoSMesh<int,double,int>, int, double, int> system(meshPtr);

	// Add a particle
	cupcfd::geometry::euclidean::EuclideanPoint<double,3> pos1(0.12, 0.11, 0.14);
	cupcfd::geometry::euclidean::EuclideanPoint<double,3> pos2(0.13, 0.11, 0.14);
	cupcfd::geometry::euclidean::EuclideanPoint<double,3> pos3(0.14, 0.11, 0.14);
	cupcfd::geometry::euclidean::EuclideanPoint<double,3> pos4(0.15, 0.11, 0.14);
	cupcfd::geometry::euclidean::EuclideanPoint<double,3> pos5(0.16, 0.11, 0.14);

	cupcfd::geometry::euclidean::EuclideanVector<double,3> velocity1(1.0, 1.1, 1.2);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> acceleration1(0.0, 0.0, 0.0);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> jerk1(0.0, 0.0, 0.0);

	uint pID=0, cellID=0, rank=0;
	ParticleSimple<int,double> particle1(pos1, velocity1, acceleration1, jerk1, pID, cellID, rank, 1000.0, 0.0, 0.0);
	ParticleSimple<int,double> particle2(pos2, velocity1, acceleration1, jerk1, pID, cellID, rank, 1000.0, 0.0, 0.0);
	ParticleSimple<int,double> particle3(pos3, velocity1, acceleration1, jerk1, pID, cellID, rank, 1000.0, 0.0, 0.0);
	ParticleSimple<int,double> particle4(pos4, velocity1, acceleration1, jerk1, pID, cellID, rank, 1000.0, 0.0, 0.0);
	ParticleSimple<int,double> particle5(pos5, velocity1, acceleration1, jerk1, pID, cellID, rank, 1000.0, 0.0, 0.0);

	status = system.addParticle(particle1);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	status = system.addParticle(particle2);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	status = system.addParticle(particle3);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	status = system.addParticle(particle4);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	status = system.addParticle(particle5);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	// Currently 5 'active'
	// Set two to inactive and remove them
	status = system.setParticleInactive(2);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	status = system.setParticleInactive(3);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	BOOST_CHECK_EQUAL(system.getNParticles(), 5);	// 3 active, 2 inactive
	status = system.removeInactiveParticles();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	BOOST_CHECK_EQUAL(system.getNParticles(), 3);	// 3 active, 0 inactive

	// Check system properties
	BOOST_CHECK_EQUAL(system.getNActiveParticles(), 3);
	BOOST_CHECK_EQUAL(system.getNTravelParticles(), 0);
	BOOST_TEST(system.particles[0].pos.cmp[0] == 0.12);
	BOOST_TEST(system.particles[1].pos.cmp[0] == 0.13);
	BOOST_TEST(system.particles[2].pos.cmp[0] == 0.16);
}

// === generateEmitterParticles ===

// === exchangeParticles ===
BOOST_AUTO_TEST_CASE(exchangeParticles_test1)
{
	cupcfd::error::eCodes status;
	cupcfd::comm::Communicator comm(MPI_COMM_WORLD);

	// === Create a small test mesh ===
	// Setup the configurations
	cupcfd::partitioner::PartitionerNaiveConfig<int,int> partConfig;
	cupcfd::geometry::mesh::MeshSourceStructGenConfig<int, double> meshSourceConfig(5, 5, 5, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
	cupcfd::geometry::mesh::MeshConfig<int,double,int> meshConfig(partConfig, meshSourceConfig);

	// Build the mesh
	cupcfd::geometry::mesh::CupCfdAoSMesh<int,double,int> * mesh;
	status = meshConfig.buildUnstructuredMesh(&mesh, comm);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	std::shared_ptr<meshgeo::CupCfdAoSMesh<int,double,int>> meshPtr(mesh);

	// Create the particle system
	ParticleSystemSimple<meshgeo::CupCfdAoSMesh<int,double,int>, int, double, int> system(meshPtr);

	// Add a particle
	cupcfd::geometry::euclidean::EuclideanPoint<double,3> pos1(0.12, 0.11, 0.14);
	cupcfd::geometry::euclidean::EuclideanPoint<double,3> pos2(0.13, 0.11, 0.14);
	cupcfd::geometry::euclidean::EuclideanPoint<double,3> pos3(0.14, 0.11, 0.14);
	cupcfd::geometry::euclidean::EuclideanPoint<double,3> pos4(0.15, 0.11, 0.14);
	cupcfd::geometry::euclidean::EuclideanPoint<double,3> pos5(0.16, 0.11, 0.14);

	cupcfd::geometry::euclidean::EuclideanVector<double,3> velocity1(1.0, 1.1, 1.2);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> acceleration1(0.0, 0.0, 0.0);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> jerk1(0.0, 0.0, 0.0);

	uint cellID=0;
	if(comm.rank == 0)
	{
		// We will add these exclusively to Rank 0, but set their destination ranks to something different
		// Note: Technically the Cell IDs are incorrect for the destination ranks, but if we are just
		// testing the exchange it shouldn't be an issue
		// Note: For this test the process IDs have to be carefully selected, since the ranks must
		// be mesh neighbours if they are to send to one another
		ParticleSimple<int,double> particle2(pos2, velocity1, acceleration1, jerk1, 0, cellID, 0, 1000.0, 0.0, 0.0);
		ParticleSimple<int,double> particle3(pos3, velocity1, acceleration1, jerk1, 1, cellID, 1, 1000.0, 0.0, 0.0);
		status = system.addParticle(particle2);
		BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
		status = system.addParticle(particle3);
		BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	}
	else if(comm.rank == 1)
	{
		ParticleSimple<int,double> particle4(pos4, velocity1, acceleration1, jerk1, 2, cellID, 2, 1000.0, 0.0, 13.3);
		status = system.addParticle(particle4);
		BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	}
	else if(comm.rank == 2)
	{
		ParticleSimple<int,double> particle1(pos1, velocity1, acceleration1, jerk1, 3, cellID, 3, 1000.0, 0.0, 5.7);
		ParticleSimple<int,double> particle5(pos5, velocity1, acceleration1, jerk1, 4, cellID, 2, 1000.0, 0.0, 0.0);

		status = system.addParticle(particle1);
		BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
		status = system.addParticle(particle5);
		BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	}

	// Test and Check
	status = system.exchangeParticles();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	status = system.removeInactiveParticles(); // The exchange process should have marked sent particles as inactive
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	if(comm.rank == 0)
	{
		BOOST_CHECK_EQUAL(system.getNActiveParticles(), 1);
		BOOST_CHECK_EQUAL(system.getNTravelParticles(), 0);
		BOOST_TEST(system.particles[0].pos.cmp[0] == 0.13);
	}
	else if(comm.rank == 1)
	{
		BOOST_CHECK_EQUAL(system.getNActiveParticles(), 1);
		BOOST_CHECK_EQUAL(system.getNTravelParticles(), 0);
		BOOST_TEST(system.particles[0].pos.cmp[0] == 0.14);
	}
	else if(comm.rank == 2)
	{
		BOOST_CHECK_EQUAL(system.getNActiveParticles(), 2);
		BOOST_CHECK_EQUAL(system.getNTravelParticles(), 1);
		BOOST_TEST(system.particles[0].pos.cmp[0] == 0.16);
		BOOST_TEST(system.particles[1].pos.cmp[0] == 0.15);
	}
	else if(comm.rank == 3)
	{
		BOOST_CHECK_EQUAL(system.getNActiveParticles(), 1);
		BOOST_CHECK_EQUAL(system.getNTravelParticles(), 1);
		BOOST_TEST(system.particles[0].pos.cmp[0] == 0.12);
	}
}

// === updateSystem ===
// Test 1: Test correct movement of particles around system in a serial mesh
BOOST_AUTO_TEST_CASE(updateSystem_test1, * utf::tolerance(0.00001))
{
	cupcfd::error::eCodes status;
	cupcfd::comm::Communicator comm(MPI_COMM_SELF);

	// === Create a small test mesh ===
	// Setup the configurations
	cupcfd::partitioner::PartitionerNaiveConfig<int,int> partConfig;
	cupcfd::geometry::mesh::MeshSourceStructGenConfig<int, double> meshSourceConfig(5, 5, 5, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
	cupcfd::geometry::mesh::MeshConfig<int,double,int> meshConfig(partConfig, meshSourceConfig);

	// Build the mesh
	cupcfd::geometry::mesh::CupCfdAoSMesh<int,double,int> * mesh;
	status = meshConfig.buildUnstructuredMesh(&mesh, comm);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	std::shared_ptr<meshgeo::CupCfdAoSMesh<int,double,int>> meshPtr(mesh);

	// Create the particle system
	ParticleSystemSimple<meshgeo::CupCfdAoSMesh<int,double,int>, int, double, int> system(meshPtr);

	// Add a particle
	cupcfd::geometry::euclidean::EuclideanPoint<double,3> pos1(0.12, 0.11, 0.14);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> velocity1(1.0, 1.1, 1.2);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> acceleration1(0.0, 0.0, 0.0);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> jerk1(0.0, 0.0, 0.0);
	uint pID=0, cellID=0, rank=0;
	ParticleSimple<int,double> particle1(pos1, velocity1, acceleration1, jerk1, pID, cellID, rank, 1000.0, 0.0, 0.0);

	status = system.addParticle(particle1);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	// Advance system by 119 seconds/time units
	// This should lead to it bouncing off
	status = system.updateSystem(19);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	// Check that the particle has ended up in the correct position (no acceleration/jerk to make this easier
	// to manually compute)
	BOOST_TEST(system.particles[0].pos.cmp[0] == 0.88);
	BOOST_TEST(system.particles[0].pos.cmp[1] == 0.99);
	BOOST_TEST(system.particles[0].pos.cmp[2] == 0.94);
}

// Test 2: Test correct movement of particles around system, including across ranks
BOOST_AUTO_TEST_CASE(updateSystem_test2, * utf::tolerance(0.00001))
{
	cupcfd::error::eCodes status;
	cupcfd::comm::Communicator comm(MPI_COMM_WORLD);

	// === Create a small test mesh ===
	// Setup the configurations
	cupcfd::partitioner::PartitionerNaiveConfig<int,int> partConfig;
	cupcfd::geometry::mesh::MeshSourceStructGenConfig<int, double> meshSourceConfig(5, 5, 5, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
	cupcfd::geometry::mesh::MeshConfig<int,double,int> meshConfig(partConfig, meshSourceConfig);

	// Build the mesh
	cupcfd::geometry::mesh::CupCfdAoSMesh<int,double,int> * mesh;
	status = meshConfig.buildUnstructuredMesh(&mesh, comm);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	std::shared_ptr<meshgeo::CupCfdAoSMesh<int,double,int>> meshPtr(mesh);

	// Create the particle system
	ParticleSystemSimple<meshgeo::CupCfdAoSMesh<int,double,int>, int, double, int> system(meshPtr);

	// Add a particle
	cupcfd::geometry::euclidean::EuclideanPoint<double,3> pos1(0.12, 0.11, 0.14);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> velocity1(1.0, 1.1, 1.2);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> acceleration1(0.0, 0.0, 0.0);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> jerk1(0.0, 0.0, 0.0);
	uint pID=0, cellID=0, rank=0;
	ParticleSimple<int,double> particle1(pos1, velocity1, acceleration1, jerk1, pID, cellID, rank, 1000.0, 0.0, 0.0);

	if(comm.rank == 0)
	{
		status = system.addParticle(particle1);
		BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	}

	// Advance system by 19 seconds/time units
	// This should lead to it bouncing around the system
	status = system.updateSystem(19);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	// Check that the particle has ended up in the correct position (no acceleration/jerk to make this easier
	// to manually compute)
	if(comm.rank == 0)
	{
		BOOST_CHECK_EQUAL(system.particles.size(), 0);
	}
	else if(comm.rank == 1)
	{
		BOOST_CHECK_EQUAL(system.particles.size(), 0);
	}
	else if(comm.rank == 2)
	{
		BOOST_CHECK_EQUAL(system.particles.size(), 0);
	}
	else if(comm.rank == 3)
	{
		BOOST_CHECK_EQUAL(system.particles.size(), 1);
		BOOST_TEST(system.particles[0].pos.cmp[0] == 0.88);
		BOOST_TEST(system.particles[0].pos.cmp[1] == 0.99);
		BOOST_TEST(system.particles[0].pos.cmp[2] == 0.94);
	}
}

// Test 3: Test correct movement of particles when there is also an emitter (so new particles should be generated too)
BOOST_AUTO_TEST_CASE(updateSystem_test3, * utf::tolerance(0.00001))
{
	cupcfd::error::eCodes status;
	cupcfd::comm::Communicator comm(MPI_COMM_WORLD);

	// === Create a small test mesh ===
	// Setup the configurations
	cupcfd::partitioner::PartitionerNaiveConfig<int,int> partConfig;
	cupcfd::geometry::mesh::MeshSourceStructGenConfig<int, double> meshSourceConfig(5, 5, 5, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0);
	cupcfd::geometry::mesh::MeshConfig<int,double,int> meshConfig(partConfig, meshSourceConfig);

	// Build the mesh
	cupcfd::geometry::mesh::CupCfdAoSMesh<int,double,int> * mesh;
	status = meshConfig.buildUnstructuredMesh(&mesh, comm);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	std::shared_ptr<meshgeo::CupCfdAoSMesh<int,double,int>> meshPtr(mesh);

	// Create the particle system
	ParticleSystemSimple<meshgeo::CupCfdAoSMesh<int,double,int>, int, double, int> system(meshPtr);

	// Add an emitter to the system
	cupcfd::geometry::euclidean::EuclideanPoint<double, 3> position(0.07, 0.12, 0.08);
    dist::DistributionFixed<int,double> rate(2.3);
    dist::DistributionFixed<int,double> angleXY(1.2);
    dist::DistributionFixed<int,double> angleRotation(1.3);
    dist::DistributionFixed<int,double> speed(0.052);
    dist::DistributionFixed<int,double> accelerationX(0.0);
    dist::DistributionFixed<int,double> accelerationY(0.0);
    dist::DistributionFixed<int,double> accelerationZ(0.0);
    dist::DistributionFixed<int,double> jerkX(0.0);
    dist::DistributionFixed<int,double> jerkY(0.0);
    dist::DistributionFixed<int,double> jerkZ(0.0);
    dist::DistributionFixed<int,double> decayRate(0.0);
    dist::DistributionFixed<int,double> decayThreshold(10);

    if(comm.rank == 0)
    {
    	uint emitterID=0;
    	ParticleEmitterSimple<int,double> emitter(0, 0, comm.rank, emitterID, position, &rate, &angleXY, &angleRotation, &speed,
				  &accelerationX, &accelerationY, &accelerationZ,
				  &jerkX, &jerkY, &jerkZ,
				  &decayRate, &decayThreshold);
        status = system.addParticleEmitter(emitter);
        BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
    }

	// Add a particle
	cupcfd::geometry::euclidean::EuclideanPoint<double,3> pos1(0.12, 0.11, 0.14);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> velocity1(1.0, 1.1, 1.2);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> acceleration1(0.0, 0.0, 0.0);
	cupcfd::geometry::euclidean::EuclideanVector<double,3> jerk1(0.0, 0.0, 0.0);
	uint pID=0, cellID=0, rank=0;
	ParticleSimple<int,double> particle1(pos1, velocity1, acceleration1, jerk1, pID, cellID, rank, 1000.0, 0.0, 0.0);

	if(comm.rank == 0)
	{
		status = system.addParticle(particle1);
		BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	}

	// Advance system by 2.31 seconds/time units
	// This should lead to a second particle being created and mover ever so slightly
	status = system.updateSystem(2.31);
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

	// Check that the particle has ended up in the correct position (no acceleration/jerk to make this easier
	// to manually compute)
	if(comm.rank == 0)
	{
		// Note this particle was generated right at the end of the time period and had little time to advance
		BOOST_CHECK_EQUAL(system.getNParticles(), 1);
		BOOST_CHECK_EQUAL(system.particles[0].getCellGlobalID(), 0);
		BOOST_TEST(system.particles[0].pos.cmp[0] == 0.0700504);
		BOOST_TEST(system.particles[0].pos.cmp[1] == 0.1204846);
		BOOST_TEST(system.particles[0].pos.cmp[2] == 0.07981844);

		// Next particle generation time should be 2.29 into the next dt period
		BOOST_TEST(system.emitters[0].nextParticleTime == 2.29);
	}
	else if(comm.rank == 1)
	{
		BOOST_CHECK_EQUAL(system.getNParticles(), 0);
	}
	else if(comm.rank == 2)
	{
		BOOST_CHECK_EQUAL(system.getNParticles(), 0);
	}
	else if(comm.rank == 3)
	{
		BOOST_CHECK_EQUAL(system.getNParticles(), 1);
		BOOST_CHECK_EQUAL(system.particles[0].getCellGlobalID(), 117);
		BOOST_TEST(system.particles[0].pos.cmp[0] == 0.43);
		BOOST_TEST(system.particles[0].pos.cmp[1] == 0.651);
		BOOST_TEST(system.particles[0].pos.cmp[2] == 0.912);
	}
}

// Cleanup
BOOST_AUTO_TEST_CASE(cleanup)
{
	// Cleanup these MPI datatypes
	cupcfd::geometry::euclidean::EuclideanPoint<double, 3> point;
	cupcfd::geometry::euclidean::EuclideanVector<double,3> vector;
	ParticleSimple<int, double> particle;
	cupcfd::error::eCodes status;

	status = particle.deregisterMPIType();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	status = point.deregisterMPIType();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);
	status = vector.deregisterMPIType();
	BOOST_CHECK_EQUAL(status, cupcfd::error::E_SUCCESS);

    MPI_Finalize();
}


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

#include <pow/aserti32d.h>

#include <chain.h>
#include <chainparams.h>
#include <config.h>
#include <consensus/activation.h>
#include <pow/pow.h>

#include <test/util/setup_common.h>

#include <boost/test/unit_test.hpp>

#include <cmath>

BOOST_FIXTURE_TEST_SUITE(aserti32d_tests, BasicTestingSetup)

static CBlockIndex GetBlockIndex(CBlockIndex *pindexPrev, int64_t nTimeInterval,
                                 uint32_t nBits) {
    CBlockIndex block;
    block.pprev = pindexPrev;
    block.nHeight = pindexPrev->nHeight + 1;
    block.nTime = pindexPrev->nTime + nTimeInterval;
    block.nBits = nBits;

    block.BuildSkip();
    block.nChainWork = pindexPrev->nChainWork + GetBlockProof(block);
    return block;
}

static double TargetFromBits(const uint32_t nBits) {
    return (nBits & 0xff'ff'ff) * pow(256, (nBits >> 24) - 3);
}

static double GetASERTApproximationError(const CBlockIndex *pindexPrev,
                                         const uint32_t finalBits,
                                         const CBlockIndex *pindexAnchorBlock) {
    const int64_t nHeightDiff =
        pindexPrev->nHeight - pindexAnchorBlock->nHeight;
    const int64_t nTimeDiff =
        pindexPrev->GetBlockTime() - pindexAnchorBlock->pprev->GetBlockTime();
    const uint32_t initialBits = pindexAnchorBlock->nBits;

    BOOST_CHECK(nHeightDiff >= 0);
    double dInitialPow = TargetFromBits(initialBits);
    double dFinalPow = TargetFromBits(finalBits);

    double dExponent =
        double(nTimeDiff - (nHeightDiff + 1) * 600) / double(2 * 24 * 3600);
    double dTarget = dInitialPow * pow(2, dExponent);

    return (dFinalPow - dTarget) / dTarget;
}

BOOST_AUTO_TEST_CASE(asert_difficulty_test) {
    DummyConfig config(CBaseChainParams::MAIN);

    std::vector<CBlockIndex> blocks(3000 + 2 * 24 * 3600);

    const Consensus::Params &params = config.GetChainParams().GetConsensus();
    const arith_uint256 powLimit = UintToArith256(params.powLimit);
    arith_uint256 currentPow = powLimit >> 3;
    uint32_t initialBits = currentPow.GetCompact();
    double dMaxErr = 0.0001166792656486;

    // Genesis block, and parent of ASERT anchor block in this test case.
    blocks[0] = CBlockIndex();
    blocks[0].nHeight = 0;
    blocks[0].nTime = 1269211443;
    // The pre-anchor block's nBits should never be used, so we set it to a
    // nonsense value in order to trigger an error if it is ever accessed
    blocks[0].nBits = 0x0dedbeef;

    blocks[0].nChainWork = GetBlockProof(blocks[0]);

    // Block counter.
    size_t i = 1;

    // ASERT anchor block. We give this one a solvetime of 150 seconds to ensure
    // that the solvetime between the pre-anchor and the anchor blocks is
    // actually used.
    blocks[1] = GetBlockIndex(&blocks[0], 150, initialBits);
    // The nBits for the next block should not be equal to the anchor block's
    // nBits
    CBlockHeader blkHeaderDummy;
    uint32_t nBits = GetNextASERTWorkRequired(&blocks[i++], &blkHeaderDummy,
                                              params, &blocks[1]);
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[1])) < dMaxErr);
    BOOST_CHECK(nBits != initialBits);

    // If we add another block at 1050 seconds, we should return to the anchor
    // block's nBits
    blocks[i] = GetBlockIndex(&blocks[i - 1], 1050, nBits);
    nBits = GetNextASERTWorkRequired(&blocks[i++], &blkHeaderDummy, params,
                                     &blocks[1]);
    BOOST_CHECK(nBits == initialBits);
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[1])) < dMaxErr);

    currentPow = arith_uint256().SetCompact(nBits);
    // Before we do anything else, check that timestamps *before* the anchor
    // block work fine. Jumping 2 days into the past will give a timestamp
    // before the achnor, and should halve the target
    blocks[i] = GetBlockIndex(&blocks[i - 1], 600 - 172800, nBits);
    nBits = GetNextASERTWorkRequired(&blocks[i++], &blkHeaderDummy, params,
                                     &blocks[1]);
    currentPow = arith_uint256().SetCompact(nBits);
    // Because nBits truncates target, we don't end up with exactly 1/2 the
    // target
    BOOST_CHECK(currentPow <= arith_uint256().SetCompact(initialBits) / 2);
    BOOST_CHECK(currentPow >= arith_uint256().SetCompact(initialBits - 1) / 2);
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[1])) < dMaxErr);

    // Jumping forward 2 days should return the target to the initial value
    blocks[i] = GetBlockIndex(&blocks[i - 1], 600 + 172800, nBits);
    nBits = GetNextASERTWorkRequired(&blocks[i++], &blkHeaderDummy, params,
                                     &blocks[1]);
    currentPow = arith_uint256().SetCompact(nBits);
    BOOST_CHECK(nBits == initialBits);
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[1])) < dMaxErr);

    // Pile up some blocks every 10 mins to establish some history.
    for (; i < 150; i++) {
        blocks[i] = GetBlockIndex(&blocks[i - 1], 600, nBits);
        BOOST_CHECK_EQUAL(blocks[i].nBits, nBits);
    }

    nBits = GetNextASERTWorkRequired(&blocks[i - 1], &blkHeaderDummy, params,
                                     &blocks[1]);

    BOOST_CHECK_EQUAL(nBits, initialBits);

    // Difficulty stays the same as long as we produce a block every 10 mins.
    for (size_t j = 0; j < 10; i++, j++) {
        blocks[i] = GetBlockIndex(&blocks[i - 1], 600, nBits);
        BOOST_CHECK_EQUAL(GetNextASERTWorkRequired(&blocks[i], &blkHeaderDummy,
                                                   params, &blocks[1]),
                          nBits);
    }

    // If we add a two blocks whose solvetimes together add up to 1200s,
    // then the next block's target should be the same as the one before these
    // blocks (at this point, equal to initialBits).
    blocks[i] = GetBlockIndex(&blocks[i - 1], 300, nBits);
    nBits = GetNextASERTWorkRequired(&blocks[i++], &blkHeaderDummy, params,
                                     &blocks[1]);
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[1])) < dMaxErr);
    // relative
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[i - 2])) < dMaxErr);
    blocks[i] = GetBlockIndex(&blocks[i - 1], 900, nBits);
    nBits = GetNextASERTWorkRequired(&blocks[i++], &blkHeaderDummy, params,
                                     &blocks[1]);
    // absolute
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[1])) < dMaxErr);
    // relative
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[i - 2])) < dMaxErr);
    BOOST_CHECK_EQUAL(nBits, initialBits);
    BOOST_CHECK(nBits != blocks[i - 1].nBits);

    // Same in reverse - this time slower block first, followed by faster block.
    blocks[i] = GetBlockIndex(&blocks[i - 1], 900, nBits);
    nBits = GetNextASERTWorkRequired(&blocks[i++], &blkHeaderDummy, params,
                                     &blocks[1]);
    // absolute
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[1])) < dMaxErr);
    // relative
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[i - 2])) < dMaxErr);
    blocks[i] = GetBlockIndex(&blocks[i - 1], 300, nBits);
    nBits = GetNextASERTWorkRequired(&blocks[i++], &blkHeaderDummy, params,
                                     &blocks[1]);
    // absolute
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[1])) < dMaxErr);
    // relative
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[i - 2])) < dMaxErr);
    BOOST_CHECK_EQUAL(nBits, initialBits);
    BOOST_CHECK(nBits != blocks[i - 1].nBits);

    // Jumping forward 2 days should double the target (halve the difficulty)
    blocks[i] = GetBlockIndex(&blocks[i - 1], 600 + 2 * 24 * 3600, nBits);
    nBits = GetNextASERTWorkRequired(&blocks[i++], &blkHeaderDummy, params,
                                     &blocks[1]);
    // absolute
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[1])) < dMaxErr);
    // relative
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[i - 2])) < dMaxErr);
    currentPow = arith_uint256().SetCompact(nBits) / 2;
    BOOST_CHECK_EQUAL(currentPow.GetCompact(), initialBits);

    // Jumping backward 2 days should bring target back to where we started
    blocks[i] = GetBlockIndex(&blocks[i - 1], 600 - 2 * 24 * 3600, nBits);
    nBits = GetNextASERTWorkRequired(&blocks[i++], &blkHeaderDummy, params,
                                     &blocks[1]);
    BOOST_CHECK(fabs(GetASERTApproximationError(
                    &blocks[i - 1], nBits, &blocks[1])) < dMaxErr); // absolute
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[i - 2])) <
                dMaxErr); // relative
    BOOST_CHECK_EQUAL(nBits, initialBits);

    // Jumping backward 2 days should halve the target (double the difficulty)
    blocks[i] = GetBlockIndex(&blocks[i - 1], 600 - 2 * 24 * 3600, nBits);
    nBits = GetNextASERTWorkRequired(&blocks[i++], &blkHeaderDummy, params,
                                     &blocks[1]);
    // absolute
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[1])) < dMaxErr);
    // relative
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[i - 2])) < dMaxErr);
    currentPow = arith_uint256().SetCompact(nBits);
    // Because nBits truncates target, we don't end up with exactly 1/2 the
    // target
    BOOST_CHECK(currentPow <= arith_uint256().SetCompact(initialBits) / 2);
    BOOST_CHECK(currentPow >= arith_uint256().SetCompact(initialBits - 1) / 2);

    // And forward again
    blocks[i] = GetBlockIndex(&blocks[i - 1], 600 + 2 * 24 * 3600, nBits);
    nBits = GetNextASERTWorkRequired(&blocks[i++], &blkHeaderDummy, params,
                                     &blocks[1]);
    // absolute
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[1])) < dMaxErr);
    // relative
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[i - 2])) < dMaxErr);
    BOOST_CHECK_EQUAL(nBits, initialBits);
    blocks[i] = GetBlockIndex(&blocks[i - 1], 600 + 2 * 24 * 3600, nBits);
    nBits = GetNextASERTWorkRequired(&blocks[i++], &blkHeaderDummy, params,
                                     &blocks[1]);
    // absolute
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[1])) < dMaxErr);
    // relative
    BOOST_CHECK(fabs(GetASERTApproximationError(&blocks[i - 1], nBits,
                                                &blocks[i - 2])) < dMaxErr);
    currentPow = arith_uint256().SetCompact(nBits) / 2;
    BOOST_CHECK_EQUAL(currentPow.GetCompact(), initialBits);

    // Iterate over the entire -2*24*3600..+2*24*3600 range to check that our
    // integer approximation:
    //   1. Should be monotonic.
    //   2. Should change target at least once every 8 seconds (worst-case:
    //      15-bit precision on nBits)
    //   3. Should never change target by more than XXXX per 1-second step.
    //   4. Never exceeds dMaxError in absolute error vs a double float
    //      calculation.
    //   5. Has almost exactly the dMax and dMin errors we expect for the
    //      formula.
    double dMin = 0;
    double dMax = 0;
    double dErr;
    double dRelMin = 0;
    double dRelMax = 0;
    double dRelErr;
    double dMaxStep = 0;
    uint32_t nBitsRingBuffer[8];
    double dStep = 0;
    blocks[i] = GetBlockIndex(&blocks[i - 1], -2 * 24 * 3600 - 30, nBits);
    for (size_t j = 0; j < 4 * 24 * 3600 + 660; j++) {
        blocks[i].nTime++;
        nBits = GetNextASERTWorkRequired(&blocks[i], &blkHeaderDummy, params,
                                         &blocks[1]);

        if (j > 8) {
            // 1: Monotonic
            BOOST_CHECK(
                arith_uint256().SetCompact(nBits) >=
                arith_uint256().SetCompact(nBitsRingBuffer[(j - 1) % 8]));
            // 2: Changes at least once every 8 seconds (worst case: nBits =
            // 1d008000 to 1d008001)
            BOOST_CHECK(arith_uint256().SetCompact(nBits) >
                        arith_uint256().SetCompact(nBitsRingBuffer[j % 8]));
            // 3: Check 1-sec step size
            dStep = (TargetFromBits(nBits) -
                     TargetFromBits(nBitsRingBuffer[(j - 1) % 8])) /
                    TargetFromBits(nBits);
            dMaxStep = std::max(dMaxStep, dStep);
            // from nBits = 1d008000 to 1d008001
            BOOST_CHECK(dStep < 0.0000314812106363);
        }
        nBitsRingBuffer[j % 8] = nBits;

        // 4 and 5: check error vs double precision float calculation
        dErr = GetASERTApproximationError(&blocks[i], nBits, &blocks[1]);
        dRelErr = GetASERTApproximationError(&blocks[i], nBits, &blocks[i - 1]);
        dMin = std::min(dMin, dErr);
        dMax = std::max(dMax, dErr);
        dRelMin = std::min(dRelMin, dRelErr);
        dRelMax = std::max(dRelMax, dRelErr);
        BOOST_CHECK_MESSAGE(
            fabs(dErr) < dMaxErr,
            strprintf(
                "solveTime: %d\tStep size: %.8f%%\tdErr: %.8f%%\tnBits: %0x\n",
                int64_t(blocks[i].nTime) - blocks[i - 1].nTime, dStep * 100,
                dErr * 100, nBits));
        BOOST_CHECK_MESSAGE(
            fabs(dRelErr) < dMaxErr,
            strprintf("solveTime: %d\tStep size: %.8f%%\tdRelErr: "
                      "%.8f%%\tnBits: %0x\n",
                      int64_t(blocks[i].nTime) - blocks[i - 1].nTime,
                      dStep * 100, dRelErr * 100, nBits));
    }
    auto failMsg = strprintf(
        "Min error: %16.14f%%\tMax error: %16.14f%%\tMax step: %16.14f%%\n",
        dMin * 100, dMax * 100, dMaxStep * 100);
    BOOST_CHECK_MESSAGE(
        dMin < -0.0001013168981059 && dMin > -0.0001013168981060 &&
            dMax > 0.0001166792656485 && dMax < 0.0001166792656486,
        failMsg);
    failMsg = strprintf("Min relError: %16.14f%%\tMax relError: %16.14f%%\n",
                        dRelMin * 100, dRelMax * 100);
    BOOST_CHECK_MESSAGE(
        dRelMin < -0.0001013168981059 && dRelMin > -0.0001013168981060 &&
            dRelMax > 0.0001166792656485 && dRelMax < 0.0001166792656486,
        failMsg);

    // Difficulty increases as long as we produce fast blocks
    for (size_t j = 0; j < 100; i++, j++) {
        uint32_t nextBits;
        arith_uint256 currentTarget;
        currentTarget.SetCompact(nBits);

        blocks[i] = GetBlockIndex(&blocks[i - 1], 500, nBits);
        nextBits = GetNextASERTWorkRequired(&blocks[i], &blkHeaderDummy, params,
                                            &blocks[1]);
        arith_uint256 nextTarget;
        nextTarget.SetCompact(nextBits);

        // Make sure that target is decreased
        BOOST_CHECK(nextTarget <= currentTarget);

        nBits = nextBits;
    }
}

static std::string StrPrintCalcArgs(const arith_uint256 refTarget,
                                    const int64_t targetSpacing,
                                    const int64_t timeDiff,
                                    const int64_t heightDiff,
                                    const arith_uint256 expectedTarget,
                                    const uint32_t expectednBits) {
    return strprintf("\n"
                     "ref=         %s\n"
                     "spacing=     %d\n"
                     "timeDiff=    %d\n"
                     "heightDiff=  %d\n"
                     "expTarget=   %s\n"
                     "exp nBits=   0x%08x\n",
                     refTarget.ToString(), targetSpacing, timeDiff, heightDiff,
                     expectedTarget.ToString(), expectednBits);
}

// Tests of the CalculateASERT function.
BOOST_AUTO_TEST_CASE(calculate_asert_test) {
    DummyConfig config(CBaseChainParams::MAIN);
    const Consensus::Params &params = config.GetChainParams().GetConsensus();
    const int64_t nHalfLife = params.nDAAHalfLife;

    const arith_uint256 powLimit = UintToArith256(params.powLimit);
    arith_uint256 initialTarget = powLimit >> 4;
    int64_t height = 0;

    // The CalculateASERT function uses the absolute ASERT formulation
    // and adds +1 to the height difference that it receives.
    // The time difference passed to it must factor in the difference
    // to the *parent* of the reference block.
    // We assume the parent is ideally spaced in time before the reference
    // block.
    static const int64_t parent_time_diff = 600;

    // Steady
    arith_uint256 nextTarget = CalculateASERT(
        initialTarget, params.nPowTargetSpacing,
        parent_time_diff + 600 /* nTimeDiff */, ++height, powLimit, nHalfLife);
    BOOST_CHECK(nextTarget == initialTarget);

    // A block that arrives in half the expected time
    nextTarget = CalculateASERT(initialTarget, params.nPowTargetSpacing,
                                parent_time_diff + 600 + 300, ++height,
                                powLimit, nHalfLife);
    BOOST_CHECK(nextTarget < initialTarget);

    // A block that makes up for the shortfall of the previous one, restores the
    // target to initial
    arith_uint256 prevTarget = nextTarget;
    nextTarget = CalculateASERT(initialTarget, params.nPowTargetSpacing,
                                parent_time_diff + 600 + 300 + 900, ++height,
                                powLimit, nHalfLife);
    BOOST_CHECK(nextTarget > prevTarget);
    BOOST_CHECK(nextTarget == initialTarget);

    // Two days ahead of schedule should double the target (halve the
    // difficulty)
    prevTarget = nextTarget;
    nextTarget =
        CalculateASERT(prevTarget, params.nPowTargetSpacing,
                       parent_time_diff + 288 * 1200, 288, powLimit, nHalfLife);
    BOOST_CHECK(nextTarget == prevTarget * 2);

    // Two days behind schedule should halve the target (double the difficulty)
    prevTarget = nextTarget;
    nextTarget =
        CalculateASERT(prevTarget, params.nPowTargetSpacing,
                       parent_time_diff + 288 * 0, 288, powLimit, nHalfLife);
    BOOST_CHECK(nextTarget == prevTarget / 2);
    BOOST_CHECK(nextTarget == initialTarget);

    // Ramp up from initialTarget to PowLimit - should only take 4 doublings...
    uint32_t powLimit_nBits = powLimit.GetCompact();
    uint32_t next_nBits;
    for (size_t k = 0; k < 3; k++) {
        prevTarget = nextTarget;
        nextTarget = CalculateASERT(prevTarget, params.nPowTargetSpacing,
                                    parent_time_diff + 288 * 1200, 288,
                                    powLimit, nHalfLife);
        BOOST_CHECK(nextTarget == prevTarget * 2);
        BOOST_CHECK(nextTarget < powLimit);
        next_nBits = nextTarget.GetCompact();
        BOOST_CHECK(next_nBits != powLimit_nBits);
    }

    prevTarget = nextTarget;
    nextTarget =
        CalculateASERT(prevTarget, params.nPowTargetSpacing,
                       parent_time_diff + 288 * 1200, 288, powLimit, nHalfLife);
    next_nBits = nextTarget.GetCompact();
    BOOST_CHECK(nextTarget == prevTarget * 2);
    BOOST_CHECK(next_nBits == powLimit_nBits);

    // Fast periods now cannot increase target beyond POW limit, even if we try
    // to overflow nextTarget. prevTarget is a uint256, so 256*2 = 512 days
    // would overflow nextTarget unless CalculateASERT correctly detects this
    // error
    nextTarget = CalculateASERT(prevTarget, params.nPowTargetSpacing,
                                parent_time_diff + 512 * 144 * 600, 0, powLimit,
                                nHalfLife);
    next_nBits = nextTarget.GetCompact();
    BOOST_CHECK(next_nBits == powLimit_nBits);

    // We also need to watch for underflows on nextTarget. We need to withstand
    // an extra ~446 days worth of blocks. This should bring down a powLimit
    // target to the a minimum target of 1.
    nextTarget = CalculateASERT(powLimit, params.nPowTargetSpacing, 0,
                                2 * (256 - 33) * 144, powLimit, nHalfLife);
    next_nBits = nextTarget.GetCompact();
    BOOST_CHECK_EQUAL(next_nBits, arith_uint256(1).GetCompact());

    // Define a structure holding parameters to pass to CalculateASERT.
    // We are going to check some expected results  against a vector of
    // possible arguments.
    struct calc_params {
        arith_uint256 refTarget;
        int64_t targetSpacing;
        int64_t timeDiff;
        int64_t heightDiff;
        arith_uint256 expectedTarget;
        uint32_t expectednBits;
    };

    // Define some named input argument values
    const arith_uint256 SINGLE_300_TARGET{
        "00000000ffb1ffffffffffffffffffffffffffffffffffffffffffffffffffff"};
    const arith_uint256 FUNNY_REF_TARGET{
        "000000008000000000000000000fffffffffffffffffffffffffffffffffffff"};

    // Define our expected input and output values.
    // The timeDiff entries exclude the `parent_time_diff` - this is
    // added in the call to CalculateASERT in the test loop.
    const std::vector<calc_params> calculate_args = {

        /* refTarget, targetSpacing, timeDiff, heightDiff, expectedTarget,
           expectednBits */

        {powLimit, 600, 0, 2 * 144, powLimit >> 1, 0x1c7fffff},
        {powLimit, 600, 0, 4 * 144, powLimit >> 2, 0x1c3fffff},
        {powLimit >> 1, 600, 0, 2 * 144, powLimit >> 2, 0x1c3fffff},
        {powLimit >> 2, 600, 0, 2 * 144, powLimit >> 3, 0x1c1fffff},
        {powLimit >> 3, 600, 0, 2 * 144, powLimit >> 4, 0x1c0fffff},
        {powLimit, 600, 0, 2 * (256 - 34) * 144, 3, 0x01030000},
        {powLimit, 600, 0, 2 * (256 - 34) * 144 + 119, 3, 0x01030000},
        {powLimit, 600, 0, 2 * (256 - 34) * 144 + 120, 2, 0x01020000},
        {powLimit, 600, 0, 2 * (256 - 33) * 144 - 1, 2, 0x01020000},
        // 1 bit less since we do not need to shift to 0
        {powLimit, 600, 0, 2 * (256 - 33) * 144, 1, 0x01010000},
        // more will not decrease below 1
        {powLimit, 600, 0, 2 * (256 - 32) * 144, 1, 0x01010000},
        {1, 600, 0, 2 * (256 - 32) * 144, 1, 0x01010000},
        {powLimit, 600, 2 * (512 - 32) * 144, 0, powLimit, powLimit_nBits},
        {1, 600, (512 - 64) * 144 * 600, 0, powLimit, powLimit_nBits},
        // clamps to powLimit
        {powLimit, 600, 300, 1, SINGLE_300_TARGET, 0x1d00ffb1},
        // confuses any attempt to detect overflow by inspecting result
        {FUNNY_REF_TARGET, 600, 600 * 2 * 33 * 144, 0, powLimit,
         powLimit_nBits},
        // overflow to exactly 2^256
        {1, 600, 600 * 2 * 256 * 144, 0, powLimit, powLimit_nBits},
        // just under powlimit (not clamped) yet over powlimit_nbits
        {1, 600, 600 * 2 * 224 * 144 - 1, 0, arith_uint256(0xffff8) << 204,
         powLimit_nBits},
    };

    for (auto &v : calculate_args) {
        nextTarget = CalculateASERT(v.refTarget, v.targetSpacing,
                                    parent_time_diff + v.timeDiff, v.heightDiff,
                                    powLimit, nHalfLife);
        next_nBits = nextTarget.GetCompact();
        const auto failMsg =
            StrPrintCalcArgs(v.refTarget, v.targetSpacing,
                             parent_time_diff + v.timeDiff, v.heightDiff,
                             v.expectedTarget, v.expectednBits) +
            strprintf("nextTarget=  %s\nnext nBits=  0x%08x\n",
                      nextTarget.ToString(), next_nBits);
        BOOST_CHECK_MESSAGE(nextTarget == v.expectedTarget &&
                                next_nBits == v.expectednBits,
                            failMsg);
    }
}

class ChainParamsWithDAAActivation : public CChainParams {
public:
    ChainParamsWithDAAActivation(const CChainParams &chainParams, int daaHeight)
        : CChainParams(chainParams) {
        consensus.daaHeight = daaHeight;
    }
};

/**
 * Test transition of cw144 to ASERT algorithm, which involves the selection
 * of an anchor block.
 */
BOOST_AUTO_TEST_CASE(asert_activation_anchor_test) {
    // Make a custom chain params based on mainnet, activating the cw144 DAA
    // at a lower height than usual, so we don't need to waste time making a
    // 504000-long chain.
    const auto mainChainParams = CreateChainParams(CBaseChainParams::MAIN);
    const ChainParamsWithDAAActivation chainParams(*mainChainParams, 2016);
    const Consensus::Params &params = chainParams.GetConsensus();

    const int64_t activationTime =
        gArgs.GetArg("-axionactivationtime", params.axionActivationTime);
    CBlockHeader blkHeaderDummy;

    // an arbitrary compact target for our chain (based on BCH chain ~ Aug 10
    // 2020).
    uint32_t initialBits = 0x1802a842;

    // Block store for anonymous blocks; needs to be big enough to fit all
    // generated blocks in this test.
    std::vector<CBlockIndex> blocks(10000);
    int bidx = 1;

    // Genesis block.
    blocks[0].nHeight = 0;
    blocks[0].nTime = 1269211443;
    blocks[0].nBits = initialBits;
    blocks[0].nChainWork = GetBlockProof(blocks[0]);

    // Pile up a random number of blocks to establish some history of random
    // height. cw144 DAA requires us to have height at least 2016, dunno why
    // that much.
    const int initialBlockCount = 2000 + int(InsecureRandRange(1000));
    for (int i = 1; i < initialBlockCount; i++) {
        blocks[bidx] = GetBlockIndex(&blocks[bidx - 1], 600, initialBits);
        bidx++;
        BOOST_REQUIRE(bidx < int(blocks.size()));
    }

    // Start making blocks prior to activation. First, make a block about 1 day
    // before activation. Then put down 145 more blocks with 500 second
    // solvetime each, such that the MTP on the final block is 1 second short of
    // activationTime.
    {
        blocks[bidx] = GetBlockIndex(&blocks[bidx - 1], 600, initialBits);
        blocks[bidx].nTime = activationTime - 140 * 500 - 1;
        bidx++;
    }
    for (int i = 0; i < 145; i++) {
        BOOST_REQUIRE(bidx < int(blocks.size()));
        blocks[bidx] = GetBlockIndex(&blocks[bidx - 1], 500, initialBits);
        bidx++;
    }
    CBlockIndex *pindexPreActivation = &blocks[bidx - 1];
    BOOST_CHECK_EQUAL(pindexPreActivation->nTime, activationTime + 5 * 500 - 1);
    BOOST_CHECK_EQUAL(pindexPreActivation->GetMedianTimePast(),
                      activationTime - 1);
    BOOST_CHECK(IsDAAEnabled(params, pindexPreActivation));

    // If we consult DAA, then it uses cw144 which returns a significantly lower
    // target because we have been mining too fast by a ratio 600/500 for a
    // whole day.
    BOOST_CHECK(!IsAxionEnabled(params, pindexPreActivation));
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(pindexPreActivation, &blkHeaderDummy, chainParams),
        0x180236e1);

    // ASERT has never run yet, so cache is unpopulated.
    BOOST_CHECK_EQUAL(GetASERTAnchorBlockCache(), nullptr);

    /**
     * Now we'll try adding on blocks to activate ASERT. The activation block
     * is going to be our anchor block. We will make several distinct anchor
     * blocks.
     */

    // Create an activating block with expected solvetime, taking the cw144
    // difficulty we just saw. Since solvetime is expected the next target is
    // unchanged.
    CBlockIndex indexActivation0 =
        GetBlockIndex(pindexPreActivation, 600, 0x180236e1);
    BOOST_CHECK(IsAxionEnabled(params, &indexActivation0));
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(&indexActivation0, &blkHeaderDummy, chainParams),
        0x180236e1);
    // second call will have used anchor cache, shouldn't change anything
    BOOST_CHECK_EQUAL(GetASERTAnchorBlockCache(), &indexActivation0);
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(&indexActivation0, &blkHeaderDummy, chainParams),
        0x180236e1);

    // Now we'll generate some more activations/anchors, using unique targets
    // for each one (if the algo gets confused between different anchors, we
    // will know).

    // Create an activating block with 0 solvetime, which will drop target by
    // ~415/416.
    CBlockIndex indexActivation1 =
        GetBlockIndex(pindexPreActivation, 0, 0x18023456);
    BOOST_CHECK(IsAxionEnabled(params, &indexActivation1));
    // cache will be stale here, and we should get the right result regardless:
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(&indexActivation1, &blkHeaderDummy, chainParams),
        0x180232fd);
    // second call will have used anchor cache, shouldn't change anything
    BOOST_CHECK_EQUAL(GetASERTAnchorBlockCache(), &indexActivation1);
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(&indexActivation1, &blkHeaderDummy, chainParams),
        0x180232fd);
    // for good measure, try again with wiped cache
    ResetASERTAnchorBlockCache();
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(&indexActivation1, &blkHeaderDummy, chainParams),
        0x180232fd);
    BOOST_CHECK_EQUAL(GetASERTAnchorBlockCache(), &indexActivation1);

    // Try activation with expected solvetime, which will keep target the same.
    uint32_t anchorBits2 = 0x180210fe;
    CBlockIndex indexActivation2 =
        GetBlockIndex(pindexPreActivation, 600, anchorBits2);
    BOOST_CHECK(IsAxionEnabled(params, &indexActivation2));
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(&indexActivation2, &blkHeaderDummy, chainParams),
        anchorBits2);
    BOOST_CHECK_EQUAL(GetASERTAnchorBlockCache(), &indexActivation2);

    // Try a three-month solvetime which will cause us to hit powLimit.
    uint32_t anchorBits3 = 0x18034567;
    CBlockIndex indexActivation3 =
        GetBlockIndex(pindexPreActivation, 86400 * 90, anchorBits3);
    BOOST_CHECK(IsAxionEnabled(params, &indexActivation2));
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(&indexActivation3, &blkHeaderDummy, chainParams),
        0x1d00ffff);
    // If the next block jumps back in time, we get back our original difficulty
    // level.
    CBlockIndex indexActivation3_return =
        GetBlockIndex(&indexActivation3, -86400 * 90 + 2 * 600, anchorBits3);
    BOOST_CHECK_EQUAL(GetNextWorkRequired(&indexActivation3_return,
                                          &blkHeaderDummy, chainParams),
                      anchorBits3);
    // Retry for cache
    BOOST_CHECK_EQUAL(GetNextWorkRequired(&indexActivation3_return,
                                          &blkHeaderDummy, chainParams),
                      anchorBits3);
    BOOST_CHECK_EQUAL(GetASERTAnchorBlockCache(), &indexActivation3);

    // Make an activation with MTP == activation exactly. This is a backwards
    // timestamp jump so the resulting target is 1.2% lower.
    CBlockIndex indexActivation4 =
        GetBlockIndex(pindexPreActivation, 0, 0x18011111);
    indexActivation4.nTime = activationTime;
    BOOST_CHECK_EQUAL(indexActivation4.GetMedianTimePast(), activationTime);
    BOOST_CHECK(IsAxionEnabled(params, &indexActivation4));
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(&indexActivation4, &blkHeaderDummy, chainParams),
        0x18010db3);
    BOOST_CHECK_EQUAL(GetASERTAnchorBlockCache(), &indexActivation4);

    // Finally create a random chain on top of our second activation, using
    // ASERT targets all the way. Erase cache so that this will do a fresh
    // search for anchor at every step (fortauntely this is not too slow, due to
    // the skiplist traversal)
    CBlockIndex *pindexChain2 = &indexActivation2;
    for (int i = 1; i < 1000; i++) {
        BOOST_REQUIRE(bidx < int(blocks.size()));
        ResetASERTAnchorBlockCache();
        uint32_t nextBits =
            GetNextWorkRequired(pindexChain2, &blkHeaderDummy, chainParams);
        BOOST_CHECK_EQUAL(GetASERTAnchorBlockCache(), &indexActivation2);
        blocks[bidx] =
            GetBlockIndex(pindexChain2, InsecureRandRange(1200), nextBits);
        pindexChain2 = &blocks[bidx++];
    }
    // Scan back down to make sure all targets are same when we keep cached
    // anchor.
    for (CBlockIndex *pindex = pindexChain2; pindex != &indexActivation2;
         pindex = pindex->pprev) {
        uint32_t nextBits =
            GetNextWorkRequired(pindex->pprev, &blkHeaderDummy, chainParams);
        BOOST_CHECK_EQUAL(nextBits, pindex->nBits);
        BOOST_CHECK_EQUAL(GetASERTAnchorBlockCache(), &indexActivation2);
    }
}

BOOST_AUTO_TEST_SUITE_END()