#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>

#include <sensor_msgs/PointCloud2.h>

#include "std_msgs/Float32MultiArray.h"

#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

#include <stdio.h>
#include <time.h>
#include <iostream>

#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl_ros/io/pcd_io.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/ModelCoefficients.h>

using namespace Eigen;
class AutoCalibrator
{
private:
  ros::NodeHandle _nh;
  ros::Subscriber _sub_pcd;
  ros::Publisher  _pub_pcd      = _nh.advertise<sensor_msgs::PointCloud2> ("/output", 5);
  ros::Publisher  _pub_vertex   = _nh.advertise<std_msgs::Float32MultiArray> ("/vertex", 10);

  tf::TransformListener tfListener;
  tf::TransformBroadcaster tfBroadcaster;

  sensor_msgs::PointCloud2 output_cloud;

  double downSamplingResolution   = 0.2; //[m]
  double planeDetectionDistanceTh = 0.05; //[m]

public:
  AutoCalibrator()
  {
    // subscribe ROS topics
    _sub_pcd   = _nh.subscribe("/camera/depth/points", 1, &AutoCalibrator::processPCD, this);
    ROS_INFO("Listening for incoming data on topic /hsrb/head_rgbd_sensor/depth_registered/rectified_points ...");
  }
  ~AutoCalibrator() {}

  // get points
  void processPCD(const sensor_msgs::PointCloud2ConstPtr &input_cloud)
  {
    // Container for original & filtered data
    pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
    pcl::PCLPointCloud2ConstPtr cloudDS(cloud);
    pcl::PCLPointCloud2 cloud_filtered;

    // Convert to PCL data type
    pcl_conversions::toPCL(*input_cloud, *cloud);

    //Down sampling
    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
    sor.setInputCloud (cloudDS);
    sor.setLeafSize (downSamplingResolution, downSamplingResolution, downSamplingResolution);
    sor.filter (cloud_filtered);
    //pcl_conversions::fromPCL(cloud_filtered, output_cloud);

    //Plane detection
    pcl::PointCloud<pcl::PointXYZ> cloudPD;
    pcl::fromPCLPointCloud2(cloud_filtered, cloudPD);
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients (true);

    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold (planeDetectionDistanceTh);

    seg.setInputCloud (cloudPD.makeShared());
    seg.segment (*inliers, *coefficients);

    //Get projection area
    if(inliers->indices.size() == 0){
      //平面検出できなかったら終了
      std::cerr << "Could not estimate a planar model for the given data" << std::endl;
    }
    else
    {
      //平面検出できた場合、平面の方程式基づき投影するエリアの計算
      double planeCoefficients[4];
      for(int i=0;i<4;i++)
      {
        planeCoefficients[i] = coefficients->values[i];
      }
      this->getProjectionArea(planeCoefficients);
    }
  }

  //投影するエリアの取得から投影変換まで
  void getProjectionArea(double *planeCoefficients)
  {
    Vector3f c;//平面とカメラの光軸との交点
    c<<0,0,-planeCoefficients[3]/planeCoefficients[2];

    Vector3f n;//平面の法線ベクトル
    n << planeCoefficients[0],planeCoefficients[1],planeCoefficients[2];

    Vector3f ey;//平面上の一つのベクトル1
    VectorXf ez(3);//平面上の一つのベクトル2

    ey << 0,-n(2)/n(1)*sqrt(1/(1+pow(n(2)/n(1),2))),sqrt(1/(1+pow(n(2)/n(1),2)));

    Matrix3f rot;//回転行列
    rot = AngleAxisf(M_PI/2,ey);//法線軸周りで90°回転する回転行列
    ez = rot*n;

    //実平面上の投影する四角形の各頂点の座標の計算
    double  sideLength = 0.2; //一辺の長さ(0.20[m]))
    Vector3f V1;//四角形の各頂点
    Vector3f V2;
    Vector3f V3;
    Vector3f V4;
    V1 = c + sideLength*ey - sideLength/2*ez;
    V2 = c + sideLength*ey + sideLength/2*ez;
    V3 = c + sideLength/2*ez;
    V4 = c - sideLength/2*ez;

    //プロジェクター座標系に座標変換
    //動作周波数向上のため、変換にはTFを使用しない
    tf::StampedTransform projector_transform;
    try
    {
      tfListener.lookupTransform("/camera_depth_optical_frame", "/projector_frame", ros::Time(0), projector_transform);
    }
    catch (tf::TransformException ex){
      ROS_ERROR("%s",ex.what());
    }
    Vector3f projector_pos;
    projector_pos << projector_transform.getOrigin().x(),
                     projector_transform.getOrigin().y(),
                     projector_transform.getOrigin().z();

    Vector3f P1;//四角形の各頂点
    Vector3f P2;
    Vector3f P3;
    Vector3f P4;
    P1 = V1-projector_pos;
    P2 = V2-projector_pos;
    P3 = V3-projector_pos;
    P4 = V4-projector_pos;

    tf::Transform transform;
    transform.setOrigin( tf::Vector3(V1(0), V1(1), V1(2)) );
    tf::Quaternion quaternion;
    quaternion.setRPY(0, 0, 0);
    transform.setRotation(quaternion);

    transform.setOrigin( tf::Vector3(P1(0), P1(1), P1(2)) );
    tfBroadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "projector_frame", "P1"));

    transform.setOrigin( tf::Vector3(P2(0), P2(1), P2(2)) );
    tfBroadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "projector_frame", "P2"));

    transform.setOrigin( tf::Vector3(P3(0), P3(1), P3(2)) );
    tfBroadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "projector_frame", "P3"));

    transform.setOrigin( tf::Vector3(P4(0), P4(1), P4(2)) );
    tfBroadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "projector_frame", "P4"));

    //  topic /vertexで投影変換後の座標をpublish
    //アフィン変換以降はofxTransformImageのノードで行う
    double projection_distance=0.9;
    std_msgs::Float32MultiArray vertexs;
    vertexs.data.push_back(P1(0)/P1(2)*projection_distance);
    vertexs.data.push_back(P1(1)/P1(2)*projection_distance);
    vertexs.data.push_back(P2(0)/P2(2)*projection_distance);
    vertexs.data.push_back(P2(1)/P2(2)*projection_distance);
    vertexs.data.push_back(P3(0)/P3(2)*projection_distance);
    vertexs.data.push_back(P3(1)/P3(2)*projection_distance);
    vertexs.data.push_back(P4(0)/P4(2)*projection_distance);
    vertexs.data.push_back(P4(1)/P4(2)*projection_distance);
    _pub_vertex.publish(vertexs);
  }
};
int main(int argc, char **argv)
{
  ros::init(argc, argv, "auto_calibrator");

  AutoCalibrator autoCalibrator;

  ros::spin();
}


/* Main memory port of MADlib Linear Regression
 */

//#include <regress/linear.cpp>

// Note: If errors occur in the following include files, it could indicate that
// new macros have been added to mainmemQL header files.

#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET

#include <cstdio>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <dbconnector/dbconnector.hpp>

#include "modules/regress/linear.cpp"

using namespace madlib;
using namespace madlib::modules::regress;

typedef dbconnector::mainmem::MutableByteString LRModel;
typedef MutableNativeColumnVector LRVector;

/*! Transitions the UDA using the given example and label
 */
LRModel Transition(linregr_transition &step_udf, LRModel &m,
                   double* vec, size_t vec_len, double y) {
  TransparentHandle<double> th(vec);
  MappedColumnVector v(th, vec_len);

  AnyType t1;
  t1 << m;
  t1 << y;
  t1 << v;
  AnyType result = step_udf.run(t1);

  dbconnector::mainmem::MutableByteString modelr =
      result.getAs<dbconnector::mainmem::MutableByteString>();
  return modelr;
}

/*! Runs the Linear Regression final function on the givne model
 * Returns the solution as a vector.
 *
 * TODO not clear how the coef vector is allocated and how it has to be freed
 */
LRVector Final(LRModel &m) {
  linregr_final final;
  AnyType fin_args;
  fin_args << m;
  AnyType fin = final.run(fin_args);
  MutableNativeColumnVector coef = fin[0].getAs<MutableNativeColumnVector>();
  return coef;
}

int main( ) {
  size_t len = 1024*1024;
  char *bytea = new char[len];
  double *state = reinterpret_cast<double*>(bytea);
  dbconnector::mainmem::MutableByteString model(bytea, len);

  printf("model.size() = %lu\n", model.size());

  double ex1[2] = {1.0, 2.0};
  double ex2[2] = {2.0, 3.0};
  linregr_transition step_udf;

  LRModel modelr = Transition(step_udf, model, ex1, 2, 5);
  modelr = Transition(step_udf, modelr, ex2, 2, 8);

  LRVector coef = Final(modelr);
  printf("\ncoef:\n");
  for (int i = 0; i < coef.size(); i++){
    printf("[%02d] %f\n", i, coef[i]);
  }
}


#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <stdexcept>

#include <boost/program_options.hpp>
#include <gecode/driver.hh>
#include <gecode/int.hh>
#include <gecode/set.hh>
#include <gecode/minimodel.hh>
#include "cfg.hpp"
#include "cfgparser.hpp"

using namespace std;
namespace po = boost::program_options;

using namespace Gecode;

class Homomorph: public Script {
protected:
  SetVarArray g1_edges;
  //SetVarArray g2_edges;
  SetVarArray g1_reach;
  //SetVarArray g2_reach;

  //IntVarArray mapping;
public:
  Homomorph(const Options& opt) :
      g1_edges(*this, 6, IntSet::empty, IntSet(0, 5)),
  //    g2_edges(*this, 6, IntSet::empty, IntSet(0, 5)),
      g1_reach(*this, 6, IntSet::empty, IntSet(0, 5))
    //  g2_reach(*this, 6, IntSet::empty, IntSet(0, 5)),
     // mapping(*this, 5, 0, 5)
  {
      // define edges between nodes
      dom(*this, g1_edges[0], SRT_EQ, IntSet(1, 2));
      dom(*this, g1_edges[1],  SRT_EQ, 5 );
      dom(*this, g1_edges[2], SRT_EQ,  IntSet(3,4));
      dom(*this, g1_edges[3],  SRT_EQ, 5);
      dom(*this, g1_edges[4],  SRT_EQ, 5 );
      dom(*this, g1_edges[5],  SRT_EQ, IntSet::empty);
/*
      dom(*this, g2_edges[5], SRT_EQ, IntSet(3, 4));
      dom(*this, g2_edges[1],  SRT_EQ, 0 );
      dom(*this, g2_edges[2], SRT_EQ,  0);
      dom(*this, g2_edges[3],  SRT_EQ, IntSet(1,2));
      dom(*this, g2_edges[4],  SRT_EQ, 0 );
      dom(*this, g2_edges[0],  SRT_EQ, IntSet::empty);
*/
      // define reachability sets
      for(int x = 0; x < 6; ++x )
      {
          rel(*this, g1_reach[x] >= g1_edges[x]);
          element(*this, SOT_UNION, g1_reach, g1_reach[x], g1_reach[x]);
      }
      branch(*this, g1_reach, SET_VAR_NONE(), SET_VAL_MIN_INC());
  }

  virtual void
  print(std::ostream& os) const
  {
    os << "\t" << g1_edges << std::endl;
    os << "\t" << g1_reach << std::endl;
  }

  Homomorph(bool share, Homomorph& s) : Script(share,s) {
    g1_edges.update(*this, share, s.g1_edges);
  }

  virtual Space*
  copy(bool share) {
    return new Homomorph(share,*this);
  }
};

int
main(int argc, char* argv[]) {
  Options opt("SEND+MORE=MONEY");
  opt.solutions(0);
  opt.iterations(20000);
  opt.parse(argc,argv);
  Script::run<Homomorph,DFS,Options>(opt);
  return 0;
}


#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <sophus/se3.hpp>

#include <pangolin/pangolin.h>
#include <pangolin/compat/function.h>
#include <pangolin/glcuda.h>
#include <pangolin/glsl.h>

#include <SceneGraph/SceneGraph.h>
#include <SceneGraph/GLVbo.h>

#include <calibu/Calibu.h>

#include <kangaroo/extra/RpgCameraOpen.h>
#include <kangaroo/extra/ImageSelect.h>
#include <kangaroo/extra/BaseDisplayCuda.h>
#include <kangaroo/extra/BaselineFromCamModel.h>

#include <kangaroo/kangaroo.h>
#include <kangaroo/variational.h>

const int MAXD = 128;

int main( int argc, char* argv[] )
{
  // Initialise window
  pangolin::View& container = SetupPangoGLWithCuda(1024, 768);
  size_t cu_mem_start, cu_mem_end, cu_mem_total;
  cudaMemGetInfo( &cu_mem_start, &cu_mem_total );
  glClearColor(1,1,1,0);

  // Open video device
  hal::Camera video = OpenRpgCamera(argc,argv);

  // Capture first image
  std::shared_ptr<hal::ImageArray> images = hal::ImageArray::Create();

  // N cameras, each w*h in dimension, greyscale
  const size_t N = video.NumChannels();
  if( N < 2 ) {
    std::cerr << "Two images are required to run this program!" << std::endl;
    exit(1);
  }
  const size_t nw = video.Width();
  const size_t nh = video.Height();

  // Capture first image
  video.Capture(*images);

  // Downsample this image to process less pixels
  const int max_levels = 6;
  const int level = roo::GetLevelFromMaxPixels( nw, nh, 640*480 );
  //    const int level = 4;
  assert(level <= max_levels);

  // Find centered image crop which aligns to 16 pixels at given level
  const NppiRect roi = roo::GetCenteredAlignedRegion(nw,nh,16 << level,16 << level);

  // Load Camera intrinsics from file
  GetPot clArgs( argc, argv );
  bool export_time = clArgs.search("-export_time");
  const std::string filename = clArgs.follow("","-cmod");
  if( filename.empty() ) {
    std::cerr << "Camera models file is required!" << std::endl;
    exit(1);
  }
  std::shared_ptr<calibu::Rig<double>> rig = calibu::ReadXmlRig(filename);

  if( rig->NumCams() != 2 ) {
    std::cerr << "Two camera models are required to run this program!" << std::endl;
    exit(1);
  }

  Eigen::Matrix3f CamModel0 = rig->cameras_[0]->K().cast<float>();
  Eigen::Matrix3f CamModel1 = rig->cameras_[1]->K().cast<float>();

  roo::ImageIntrinsics camMod[] = {
    {CamModel0(0,0),CamModel0(1,1),CamModel0(0,2),CamModel0(1,2)},
    {CamModel1(0,0),CamModel1(1,1),CamModel1(0,2),CamModel1(1,2)}
  };

  for(int i=0; i<2; ++i ) {
    // Adjust to match camera image dimensions
    const double scale = nw / rig->cameras_[i]->Width();
    roo::ImageIntrinsics camModel = camMod[i].Scale( scale );

    // Adjust to match cropped aligned image
    camModel = camModel.CropToROI( roi );

    camMod[i] = camModel;
  }

  const unsigned int w = roi.width;
  const unsigned int h = roi.height;
  const unsigned int lw = w >> level;
  const unsigned int lh = h >> level;

  const Eigen::Matrix3d& K0 = camMod[0].Matrix();
  const Eigen::Matrix3d& Kl = camMod[0][level].Matrix();

  std::cout << "K Matrix: " << std::endl << K0 << std::endl;
  std::cout << "K Matrix - Level: " << std::endl << Kl << std::endl;

  std::cout << "Video stream dimensions: " << nw << "x" << nh << std::endl;
  std::cout << "Chosen Level: " << level << std::endl;
  std::cout << "Processing dimensions: " << lw << "x" << lh << std::endl;
  std::cout << "Offset: " << roi.x << "x" << roi.y << std::endl;

  // print selected camera model
  std::cout << "Camera Model used: " << std::endl << camMod[0][level].Matrix() << std::endl;

  Eigen::Matrix3d RDFvision;RDFvision<< 1,0,0,  0,1,0,  0,0,1;
  Eigen::Matrix3d RDFrobot; RDFrobot << 0,1,0,  0,0, 1,  1,0,0;
  Eigen::Matrix4d T_vis_ro = Eigen::Matrix4d::Identity();
  T_vis_ro.block<3,3>(0,0) = RDFvision.transpose() * RDFrobot;
  Eigen::Matrix4d T_ro_vis = Eigen::Matrix4d::Identity();
  T_ro_vis.block<3,3>(0,0) = RDFrobot.transpose() * RDFvision;

  const Sophus::SE3d T_rl_orig = T_rlFromCamModelRDF(rig->cameras_[0],
                                  rig->cameras_[1], RDFvision);

  // TODO(jmf): For now, assume cameras are rectified. Later allow unrectified cameras.
  /*
    double k1 = 0;
    double k2 = 0;

    if(cam[0].Type() == MVL_CAMERA_WARPED)
    {
        k1 = cam[0].GetModel()->warped.kappa1;
        k2 = cam[0].GetModel()->warped.kappa2;
    }
    */

  const bool rectify = false;
  if(!rectify) {
    std::cout << "Using pre-rectified images" << std::endl;
  }

  // Check we received at least two images
  if(images->Size() < 2) {
    std::cerr << "Failed to capture first stereo pair from camera" << std::endl;
    return -1;
  }

  // Define Camera Render Object (for view / scene browsing)
  pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrixRDF_TopLeft(w,h,K0(0,0),K0(1,1),K0(0,2),K0(1,2),0.1,10000),
        pangolin::IdentityMatrix(pangolin::GlModelViewStack)
        );

  pangolin::GlBufferCudaPtr vbo(pangolin::GlArrayBuffer, lw*lh,GL_FLOAT, 4, cudaGraphicsMapFlagsWriteDiscard, GL_STREAM_DRAW );
  pangolin::GlBufferCudaPtr cbo(pangolin::GlArrayBuffer, lw*lh,GL_UNSIGNED_BYTE, 4, cudaGraphicsMapFlagsWriteDiscard, GL_STREAM_DRAW );
  pangolin::GlBuffer ibo = pangolin::MakeTriangleStripIboForVbo(lw,lh);

  // Allocate Camera Images on device for processing
  roo::Image<unsigned char, roo::TargetHost, roo::DontManage> hCamImg[] = {{0,nw,nh},{0,nw,nh}};
  roo::Image<float2, roo::TargetDevice, roo::Manage> dLookup[] = {{w,h},{w,h}};

  roo::Image<unsigned char, roo::TargetDevice, roo::Manage> upload(w,h);
  roo::Pyramid<unsigned char, max_levels, roo::TargetDevice, roo::Manage> img_pyr[] = {{w,h},{w,h}};

  roo::Image<float, roo::TargetDevice, roo::Manage> img[] = {{lw,lh},{lw,lh}};
  roo::Volume<float, roo::TargetDevice, roo::Manage> vol[] = {{lw,lh,MAXD},{lw,lh,MAXD}};
  roo::Image<float, roo::TargetDevice, roo::Manage>  disp[] = {{lw,lh},{lw,lh}};
  roo::Image<float, roo::TargetDevice, roo::Manage> meanI(lw,lh);
  roo::Image<float, roo::TargetDevice, roo::Manage> varI(lw,lh);
  roo::Image<float, roo::TargetDevice, roo::Manage> temp[] = {{lw,lh},{lw,lh},{lw,lh},{lw,lh},{lw,lh}};

  roo::Image<float,roo::TargetDevice, roo::Manage>& imgd = disp[0];
  roo::Image<float,roo::TargetDevice, roo::Manage> depthmap(lw,lh);
  roo::Image<float,roo::TargetDevice, roo::Manage> imga(lw,lh);
  roo::Image<float2,roo::TargetDevice, roo::Manage> imgq(lw,lh);
  roo::Image<float,roo::TargetDevice, roo::Manage> imgw(lw,lh);

  roo::Image<float4, roo::TargetDevice, roo::Manage>  d3d(lw,lh);
  roo::Image<unsigned char, roo::TargetDevice,roo::Manage> Scratch(lw*sizeof(roo::LeastSquaresSystem<float,6>),lh);

  typedef ulong4 census_t;
  roo::Image<census_t, roo::TargetDevice, roo::Manage> census[] = {{lw,lh},{lw,lh}};

  // Stereo transformation (post-rectification)
  Sophus::SE3d T_rl = T_rl_orig;

  const double baseline = T_rl.translation().norm();
  std::cout << "Baseline: " << baseline << std::endl;

  cudaMemGetInfo( &cu_mem_end, &cu_mem_total );
  std::cout << "CuTotal: " << cu_mem_total/(1024*1024) << ", Available: " << cu_mem_end/(1024*1024) << ", Used: " << (cu_mem_start-cu_mem_end)/(1024*1024) << std::endl;

  pangolin::Var<bool> step("ui.step", false, false);
  pangolin::Var<bool> run("ui.run", false, true);
  pangolin::Var<bool> lockToCam("ui.Lock to cam", false, true);
  pangolin::Var<int> show_slice("ui.show slice",MAXD/2, 0, MAXD-1);

  pangolin::Var<int> maxdisp("ui.maxdisp",MAXD, 0, MAXD);
  pangolin::Var<bool> subpix("ui.subpix", true, true);

  pangolin::Var<bool> use_census("ui.use census", true, true);
  pangolin::Var<int> avg_rad("ui.avg_rad",0, 0, 100);

  pangolin::Var<bool> do_dtam("ui.do dtam", false, true);
  pangolin::Var<bool> dtam_reset("ui.reset", false, false);

  pangolin::Var<float> g_alpha("ui.g alpha", 14, 0,4);
  pangolin::Var<float> g_beta("ui.g beta", 2.5, 0,2);

  pangolin::Var<float> theta("ui.theta", 100, 0,100);
  pangolin::Var<float> lambda("ui.lambda", 20, 0,20);
  pangolin::Var<float> sigma_q("ui.sigma q", 0.7, 0, 1);
  pangolin::Var<float> sigma_d("ui.sigma d", 0.7, 0, 1);
  pangolin::Var<float> huber_alpha("ui.huber alpha", 0.002, 0, 0.01);
  pangolin::Var<float> beta("ui.beta", 0.00001, 0, 0.01);

  pangolin::Var<float> alpha("ui.alpha", 0.9, 0,1);
  pangolin::Var<float> r1("ui.r1", 100, 0,0.01);
  pangolin::Var<float> r2("ui.r2", 100, 0,0.01);

  pangolin::Var<bool> filter("ui.filter", false, true);
  pangolin::Var<float> eps("ui.eps",0.01*0.01, 0, 0.01);
  pangolin::Var<int> rad("ui.radius",9, 1, 20);

  pangolin::Var<bool> leftrightcheck("ui.left-right check", false, true);
  pangolin::Var<float> maxdispdiff("ui.maxdispdiff",1, 0, 5);

  pangolin::Var<int> domedits("ui.median its",1, 1, 10);
  pangolin::Var<bool> domed9x9("ui.median 9x9", false, true);
  pangolin::Var<bool> domed7x7("ui.median 7x7", false, true);
  pangolin::Var<bool> domed5x5("ui.median 5x5", false, true);
  pangolin::Var<int> medi("ui.medi",12, 0, 24);

  pangolin::Var<float> filtgradthresh("ui.filt grad thresh", 0, 0, 20);

  pangolin::Var<bool> save_depthmaps("ui.save_depthmaps", false, true);

  int jump_frames = 0;

  pangolin::RegisterKeyPressCallback(' ', [&run](){run = !run;} );
  pangolin::RegisterKeyPressCallback('l', [&lockToCam](){lockToCam = !lockToCam;} );
  pangolin::RegisterKeyPressCallback(pangolin::PANGO_SPECIAL + pangolin::PANGO_KEY_RIGHT, [&step](){step=true;} );
  pangolin::RegisterKeyPressCallback(']', [&jump_frames](){jump_frames=100;} );
  pangolin::RegisterKeyPressCallback('}', [&jump_frames](){jump_frames=1000;} );

  pangolin::Handler2dImageSelect handler2d(lw,lh,level);
  //    ActivateDrawPyramid<unsigned char,max_levels> adleft(img_pyr[0],GL_LUMINANCE8, false, true);
  //    ActivateDrawPyramid<unsigned char,max_levels> adright(img_pyr[1],GL_LUMINANCE8, false, true);
  pangolin::ActivateDrawImage<float> adleft(img[0],GL_LUMINANCE32F_ARB, false, true);
  pangolin::ActivateDrawImage<float> adright(img[1],GL_LUMINANCE32F_ARB, false, true);
  pangolin::ActivateDrawImage<float> adisp(disp[0],GL_LUMINANCE32F_ARB, false, true);
  pangolin::ActivateDrawImage<float> adw(imgw,GL_LUMINANCE32F_ARB, false, true);
  //    ActivateDrawImage<float> adCrossSection(dCrossSection,GL_RGBA_FLOAT32_APPLE, false, true);
  pangolin::ActivateDrawImage<float> adVol(vol[0].ImageXY(show_slice),GL_LUMINANCE32F_ARB, false, true);

  SceneGraph::GLSceneGraph graph;
  //    SceneGraph::GLVbo glvbo(&vbo,&ibo,&cbo);
  SceneGraph::GLVbo glvbo(&vbo,nullptr,&cbo);
  graph.AddChild(&glvbo);

  SetupContainer(container, 6, (float)w/h);
  container[0].SetDrawFunction(boostd::ref(adleft)).SetHandler(&handler2d);
  container[1].SetDrawFunction(boostd::ref(adright)).SetHandler(&handler2d);
  container[2].SetDrawFunction(boostd::ref(adisp)).SetHandler(&handler2d);
  container[3].SetDrawFunction(boostd::ref(adVol)).SetHandler(&handler2d);
  container[4].SetDrawFunction(SceneGraph::ActivateDrawFunctor(graph, s_cam))
      .SetHandler( new pangolin::Handler3D(s_cam, pangolin::AxisNone) );
  container[5].SetDrawFunction(boostd::ref(adw)).SetHandler(&handler2d);

  bool bFirstTime = true;

  for(unsigned long frame=0; !pangolin::ShouldQuit();)
  {
    bool go = frame==0 || jump_frames > 0 || run || Pushed(step);

    for(; jump_frames > 0; jump_frames--) {
      video.Capture(*images);
    }

    if(go) {
      if(frame>0) {
        if( video.Capture(*images) == false) {
          exit(1);
        }
      }

      frame++;

      /////////////////////////////////////////////////////////////
      // Upload images to device (Warp / Decimate if necessery)
      for(int i=0; i<2; ++i ) {
        hCamImg[i].ptr = (unsigned char*) images->at(i)->data();

        if(rectify) {
          upload.CopyFrom(hCamImg[i].SubImage(roi));
          Warp(img_pyr[i][0], upload, dLookup[i]);
        }else{
          img_pyr[i][0].CopyFrom(hCamImg[i].SubImage(roi));
        }

        roo::BoxReduce<unsigned char, max_levels, unsigned int>(img_pyr[i]);
      }
    }

    go |= avg_rad.GuiChanged() | use_census.GuiChanged();
    if( go ) {
      for(int i=0; i<2; ++i ) {
        roo::ElementwiseScaleBias<float,unsigned char,float>(img[i], img_pyr[i][level],1.0f/255.0f);
        if(avg_rad > 0 ) {
          roo::BoxFilter<float,float,float>(temp[0],img[i],Scratch,avg_rad);
          roo::ElementwiseAdd<float,float,float,float>(img[i], img[i], temp[0], 1, -1, 0.5);
        }
        if(use_census) {
          Census(census[i], img[i]);
        }
      }
    }

    if( go | g_alpha.GuiChanged() || g_beta.GuiChanged() ) {
      ExponentialEdgeWeight(imgw, img[0], g_alpha, g_beta);
    }

    go |= filter.GuiChanged() | leftrightcheck.GuiChanged() | rad.GuiChanged() | eps.GuiChanged() | alpha.GuiChanged() | r1.GuiChanged() | r2.GuiChanged();
    if(go) {
      if(use_census) {
        roo::CensusStereoVolume<float, census_t>(vol[0], census[0], census[1], maxdisp, -1);
        if(leftrightcheck) roo::CensusStereoVolume<float, census_t>(vol[1], census[1], census[0], maxdisp, +1);
      }else{
        CostVolumeFromStereoTruncatedAbsAndGrad(vol[0], img[0], img[1], -1, alpha, r1, r2);
        if(leftrightcheck) CostVolumeFromStereoTruncatedAbsAndGrad(vol[1], img[1], img[0], +1, alpha, r1, r2);
      }

      if(filter) {
        // Filter Cost volume
        for(int v=0; v<(leftrightcheck?2:1); ++v)
        {
          roo::Image<float, roo::TargetDevice, roo::Manage>& I = img[v];
          roo::ComputeMeanVarience<float,float,float>(varI, temp[0], meanI, I, Scratch, rad);

          for(int d=0; d<maxdisp; ++d)
          {
            roo::Image<float> P = vol[v].ImageXY(d);
            roo::ComputeCovariance(temp[0],temp[2],temp[1],P,meanI,I,Scratch,rad);
            GuidedFilter(P,temp[0],varI,temp[1],meanI,I,Scratch,temp[2],temp[3],temp[4],rad,eps);
          }
        }
      }
    }

    static int n = 0;
    //        static float theta = 0;
    //        go |= Pushed(dtam_reset);
    //        if(go )
    if(Pushed(dtam_reset))
    {
      n = 0;
      theta.Reset();

      // Initialise primal and auxillary variables
      CostVolMinimumSubpix(imgd,vol[0], maxdisp,-1);
      imga.CopyFrom(imgd);

      // Initialise dual variable
      imgq.Memset(0);
    }

    const double min_theta = 1E-4;
    if(do_dtam && theta > min_theta)
    {
      for(int i=0; i<5; ++i ) {

        // Dual Ascent
        roo::WeightedHuberGradU_DualAscentP(imgq, imgd, imgw, sigma_q, huber_alpha);

        // Primal Descent
        roo::WeightedL2_u_minus_g_PrimalDescent(imgd, imgq, imga, imgw, sigma_d, 1.0f / (theta) );

        // Auxillary exhaustive search
        CostVolMinimumSquarePenaltySubpix(imga, vol[0], imgd, maxdisp, -1, lambda, (theta) );

        theta= theta * (1-beta*n);
        ++n;
      }
      if( theta <= min_theta && save_depthmaps ) {
        cv::Mat dmap = cv::Mat( lh, lw, CV_32FC1 );
        // convert disparity to depth
        roo::Disp2Depth(imgd, depthmap, Kl(0,0), baseline );
        depthmap.MemcpyToHost( dmap.data );

        // save depth image
        const double    timestamp = images->SystemTime();
        int             frame_i = static_cast<int>(frame);
        char            index[30];

        if (export_time) {
          sprintf(index, "%015.10f", timestamp);
        } else {
          sprintf(index, "%05d", frame_i);
        }
        std::string DepthPrefix = "SDepth-";
        std::string DepthFile;
        DepthFile = DepthPrefix + index + ".pdm";
        std::cout << "Depth File: " << DepthFile << std::endl;
        std::ofstream pDFile( DepthFile.c_str(), std::ios::out | std::ios::binary );
        pDFile << "P7" << std::endl;
        pDFile << dmap.cols << " " << dmap.rows << std::endl;
        unsigned int Size = dmap.elemSize1() * dmap.rows * dmap.cols;
        pDFile << 4294967295 << std::endl;
        pDFile.write( (const char*)dmap.data, Size );
        pDFile.close();

        // save grey image
        std::string GreyPrefix = "Left-";
        std::string GreyFile;
        GreyFile = GreyPrefix + index + ".pgm";
        std::cout << "Grey File: " << GreyFile << std::endl;
        cv::Mat gimg = cv::Mat( lh, lw, CV_8UC1 );
        img_pyr[0][level].MemcpyToHost( gimg.data );
        cv::imwrite( GreyFile, gimg );

        // reset
        step = true;
        dtam_reset = true;
      }

      if (bFirstTime) {
        float* temp;
        std::cout<<"n = "<<n<<std::endl;
        temp = (float *) malloc(2*sizeof(float)*imgq.h*imgq.w);
        imgq.MemcpyToHost(temp);
        for(int jj = 0; jj < 5; jj++) {
          for(int ii = 0; ii < 5; ii++){
            int index = 2*(ii + jj*imgq.w);
            std::cout<<"("<< temp[index] <<","<< temp[index + 1] <<")\t";
          }
          std::cout<<std::endl;
        }
        free(temp);
        bFirstTime = false;
      }

    }

    go |= pangolin::GuiVarHasChanged();
    //        if(go) {
    //            if(subpix) {
    //                CostVolMinimumSubpix(disp[0],vol[0], maxdisp,-1);
    //                if(leftrightcheck) CostVolMinimumSubpix(disp[1],vol[1], maxdisp,+1);
    //            }else{
    //                CostVolMinimum<float,float>(disp[0],vol[0], maxdisp);
    //                if(leftrightcheck) CostVolMinimum<float,float>(disp[1],vol[1], maxdisp);
    //            }

    //        }

    if(go) {
      for(int di=0; di<(leftrightcheck?2:1); ++di) {
        for(int i=0; i < domedits; ++i ) {
          if(domed9x9) MedianFilterRejectNegative9x9(disp[di],disp[di], medi);
          if(domed7x7) MedianFilterRejectNegative7x7(disp[di],disp[di], medi);
          if(domed5x5) MedianFilterRejectNegative5x5(disp[di],disp[di], medi);
        }
      }

      if(leftrightcheck ) {
        LeftRightCheck(disp[1], disp[0], +1, maxdispdiff);
        LeftRightCheck(disp[0], disp[1], -1, maxdispdiff);
      }

      if(filtgradthresh > 0) {
        FilterDispGrad(disp[0], disp[0], filtgradthresh);
      }
    }

    //        if(go)
    {
      // Generate point cloud from disparity image
      DisparityImageToVbo(d3d, disp[0], baseline, Kl(0,0), Kl(1,1), Kl(0,2), Kl(1,2) );

      //            if(container[3].IsShown())
      {
        // Copy point cloud into VBO
        {
          pangolin::CudaScopedMappedPtr var(vbo);
          roo::Image<float4> dVbo((float4*)*var,lw,lh);
          dVbo.CopyFrom(d3d);
        }

        // Generate CBO
        {
          pangolin::CudaScopedMappedPtr var(cbo);
          roo::Image<uchar4> dCbo((uchar4*)*var,lw,lh);
          roo::ConvertImage<uchar4,unsigned char>(dCbo, img_pyr[0][level]);
        }
      }

      // Update texture views
      adisp.SetImageScale(1.0f/maxdisp);
      //            adleft.SetLevel(show_level);
      //            adright.SetLevel(show_level);
      adVol.SetImage(vol[0].ImageXY(show_slice));
    }

    /////////////////////////////////////////////////////////////
    // Draw

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glColor3f(1,1,1);

    pangolin::FinishFrame();
  }
}


#include "modules/neuralNetwork/layer/recurrent/recurrent.hpp"
#include "modules/neuralNetwork/neuralNetwork.hpp"

#ifdef _KORALI_USE_CUDNN
  #include "auxiliar/cudaUtils.hpp"
#endif

#ifdef _KORALI_USE_ONEDNN
  #include "auxiliar/dnnUtils.hpp"
using namespace dnnl;
#endif

#include <Eigen/Dense>
using namespace Eigen;

namespace korali
{
namespace neuralNetwork
{
namespace layer
{
;

void Recurrent::initialize()
{
  // Checking Layer size
  if (_outputChannels == 0) KORALI_LOG_ERROR("Node count for layer %lu should be larger than zero.\n", _index);

  // Checking position
  if (_index == 0) KORALI_LOG_ERROR("Recurrent layers cannot be the starting layer of the NN\n");
  if (_index == _nn->_layers.size() - 1) KORALI_LOG_ERROR("Recurrent layers cannot be the last layer of the NN\n");

  // If using depth > 1, the input layer channels must be consistent
  if (_depth != 1)
    if (_prevLayer->_outputChannels != _outputChannels) KORALI_LOG_ERROR("Node count for layer %lu should be the same as that of the previous layer, when depth > 1.\n", _index);

  if (_nn->_engine == "Korali")
    KORALI_LOG_ERROR("Recurrent layers are not yet supported by the Korali NN backend, use OneDNN or CuDNN.\n");
}

void Recurrent::createHyperparameterMemory()
{
  // Getting dimensions
  const size_t L = _depth;     // Physical Layers
  const size_t D = 1;          // Directions
  const size_t G = _gateCount; // Gates
  const size_t IC = _prevLayer->_outputChannels;
  const size_t OC = _outputChannels;

  // Setting hyperparameter count
  const size_t weightsInputCount = L * D * G * OC * IC;
  const size_t weightsRecurrentCount = L * D * G * OC * OC;
  const size_t biasCount = L * D * G * OC;
  _hyperparameterCount = weightsInputCount + weightsRecurrentCount + biasCount;

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Checking Layer sizes
    const memory::dim IC = _prevLayer->_outputChannels; // channels
    const memory::dim OC = _outputChannels;             // channels
    const memory::dim G = _gateCount;                   // gates
    const memory::dim L = _depth;                       // layers
    const memory::dim D = 1;                            // directions

    memory::dims weightInputDims = {L, D, IC, G, OC};
    auto weightInputMemDesc = memory::desc(weightInputDims, memory::data_type::f32, memory::format_tag::ldigo);

    _weightsLayerMem = memory(weightInputMemDesc, _nn->_dnnlEngine);

    memory::dims weightRecurrentDims = {L, D, OC, G, OC};
    auto weightRecurrentMemDesc = memory::desc(weightRecurrentDims, memory::data_type::f32, memory::format_tag::ldigo);

    _weightsRecurrentMem = memory(weightRecurrentMemDesc, _nn->_dnnlEngine);

    memory::dims bias_dims = {L, D, G, OC};
    auto biasMemDesc = memory::desc(bias_dims, memory::data_type::f32, memory::format_tag::ldgo);

    _biasMem = memory(biasMemDesc, _nn->_dnnlEngine);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    // Creating dropout operator and its memory
    size_t seed = _nn->_k->_randomSeed++; // Pick a seed.
    cudnnErrCheck(cudnnCreateDropoutDescriptor(&_dropoutDesc));
    cudnnErrCheck(cudnnSetDropoutDescriptor(_dropoutDesc,
                                            _nn->_cuDNNHandle,
                                            0.0f,
                                            NULL,
                                            0,
                                            seed));

    // Creating RNN operator
    cudnnErrCheck(cudnnCreateRNNDescriptor(&_rnnDesc));
    cudnnErrCheck(cudnnSetRNNDescriptor_v8(_rnnDesc,
                                           CUDNN_RNN_ALGO_STANDARD,
                                           _rnnMode,
                                           CUDNN_RNN_SINGLE_REC_BIAS,
                                           CUDNN_UNIDIRECTIONAL,
                                           CUDNN_LINEAR_INPUT,
                                           CUDNN_DATA_FLOAT,
                                           CUDNN_DATA_FLOAT,
                                           CUDNN_DEFAULT_MATH,
                                           _prevLayer->_outputChannels,
                                           _outputChannels,
                                           _outputChannels,
                                           L, // Pseudo Layer Count
                                           _dropoutDesc,
                                           CUDNN_RNN_PADDED_IO_DISABLED));

    // Allocating memory for hyperparameters
    cudnnErrCheck(cudnnGetRNNWeightSpaceSize(_nn->_cuDNNHandle, _rnnDesc, &_weightsSize));

    // The number of hyperparameters is then the workspace size divided by the size of a float
    if (_hyperparameterCount != _weightsSize / sizeof(float))
      KORALI_LOG_ERROR("CuDNN - Non-consistent weights size with expected hyperparameters. Given: %lu, Expected: %lu\n", _hyperparameterCount, _weightsSize / sizeof(float));

    // Creating memory for hyperparameters and their gradients
    cudaErrCheck(cudaMalloc((void **)&_weightsTensor, _weightsSize));

    // Allocating space to store pointers to hyperparameters and their sizes
    _gateWeightTensors.resize(_gateCount);
    _gateBiasTensors.resize(_gateCount);

    // Getting pointers and length of all of the RNN hyperparameters
    for (size_t gateId = 0; gateId < _gateCount; gateId++)
    {
      cudnnTensorDescriptor_t gateWeightDesc;
      cudnnTensorDescriptor_t gateBiasDesc;
      cudnnErrCheck(cudnnCreateTensorDescriptor(&gateWeightDesc));
      cudnnErrCheck(cudnnCreateTensorDescriptor(&gateBiasDesc));

      cudnnErrCheck(cudnnGetRNNWeightParams(_nn->_cuDNNHandle,
                                            _rnnDesc,
                                            0,
                                            _weightsSize,
                                            _weightsTensor,
                                            gateId,
                                            gateWeightDesc,
                                            &_gateWeightTensors[gateId],
                                            gateBiasDesc,
                                            &_gateBiasTensors[gateId]));

      cudnnErrCheck(cudnnDestroyTensorDescriptor(gateWeightDesc));
      cudnnErrCheck(cudnnDestroyTensorDescriptor(gateBiasDesc));
    }
  }
#endif
}

void Recurrent::copyHyperparameterPointers(Layer *dstLayer)
{
  Recurrent *dstPtr = dynamic_cast<Recurrent *>(dstLayer);
  dstPtr->_hyperparameterCount = _hyperparameterCount;

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    dstPtr->_weightsLayerMem = _weightsLayerMem;
    dstPtr->_weightsRecurrentMem = _weightsRecurrentMem;
    dstPtr->_biasMem = _biasMem;
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    dstPtr->_rnnDesc = _rnnDesc;
    dstPtr->_dropoutDesc = _dropoutDesc;
    dstPtr->_weightsSize = _weightsSize;
    dstPtr->_weightsTensor = _weightsTensor;
    dstPtr->_gateWeightTensors = _gateWeightTensors;
    dstPtr->_gateBiasTensors = _gateBiasTensors;
  }
#endif
}

std::vector<float> Recurrent::generateInitialHyperparameters()
{
  std::vector<float> hyperparameters;

  // Getting dimensions
  const size_t L = _depth;
  const size_t G = _gateCount;
  const size_t IC = _prevLayer->_outputChannels;
  const size_t OC = _outputChannels;

  // Calculate hyperparameters for weight and bias of all linear layers
  // Setting value for this layer's xavier constant
  float xavierConstant = (_weightScaling * sqrtf(6.0f)) / sqrt(_outputChannels + _prevLayer->_outputChannels);

  // Weights applied to the input layer(s)
  for (size_t layerId = 0; layerId < L; layerId++)
    for (size_t gateId = 0; gateId < G; gateId++)
      for (size_t i = 0; i < OC * IC; i++)
        hyperparameters.push_back(xavierConstant * _nn->_uniformGenerator->getRandomNumber());

  // Weights applied to the recurrent layer
  for (size_t layerId = 0; layerId < L; layerId++)
    for (size_t gateId = 0; gateId < G; gateId++)
      for (size_t i = 0; i < OC * OC; i++)
        hyperparameters.push_back(xavierConstant * _nn->_uniformGenerator->getRandomNumber());

  // Bias for the recurrent layer
  for (size_t layerId = 0; layerId < L; layerId++)
    for (size_t gateId = 0; gateId < G; gateId++)
      for (size_t i = 0; i < OC; i++)
        hyperparameters.push_back(0.0f);

  return hyperparameters;
}

void Recurrent::createBackwardPipeline()
{
  /*********************************************************************************
   *  Initializing memory objects and primitives for BACKWARD propagation
   *********************************************************************************/

  // Calling base layer function
  Layer::createBackwardPipeline();

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    _weightsLayerGradientMem = memory(_weightsLayerMem.get_desc(), _nn->_dnnlEngine);
    _weightsRecurrentGradientMem = memory(_weightsRecurrentMem.get_desc(), _nn->_dnnlEngine);
    _biasGradientMem = memory(_biasMem.get_desc(), _nn->_dnnlEngine);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    cudaErrCheck(cudaMalloc((void **)&_weightsGradientTensor, _weightsSize));
  }
#endif
}

void Recurrent::backwardHyperparameters(const size_t t)
{
  if (_nn->_mode == "Inference")
    KORALI_LOG_ERROR("Requesting Layer hyperparameter gradient propagation but NN was configured for inference only.\n");

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Nothing to do here, weights and bias gradients have been generated already by backwardData.
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    // We need to clear the gradients because they are additive in CUDNN
    cudaErrCheck(cudaMemset(_weightsGradientTensor, 0, _hyperparameterCount));

    cudnnErrCheck(cudnnRNNBackwardWeights_v8(
      _nn->_cuDNNHandle,                    // handle
      _rnnDesc,                             // rnnDesc
      CUDNN_WGRAD_MODE_ADD,                 // addGrad
      _devSequenceLengths,                  // devSeqLengths
      _inputRNNDataDesc,                    // xDesc
      _prevLayer->_outputTensor[t],         // x
      _hTensorDesc,                         // hDesc
      t == 0 ? NULL : _hStateTensor[t - 1], // hx
      _outputRNNDataDesc,                   // yDesc
      _outputTensor[t],                     // y
      _weightsSize,
      _weightsGradientTensor,
      _workSpaceSize,
      _workSpaceTensor[t],
      _reserveSpaceSize,
      _reserveSpaceTensor[t]));
  }
#endif
}

void Recurrent::setHyperparameters(float *hyperparameters)
{
#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Getting dimensions
    const size_t L = _depth;
    const size_t G = _gateCount;
    const size_t IC = _prevLayer->_outputChannels;
    const size_t OC = _outputChannels;

    write_to_dnnl_memory(&hyperparameters[0], _weightsLayerMem);
    write_to_dnnl_memory(&hyperparameters[L * G * OC * IC], _weightsRecurrentMem);
    write_to_dnnl_memory(&hyperparameters[L * G * OC * OC + L * G * OC * IC], _biasMem);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN") cudaErrCheck(cudaMemcpy(_weightsTensor, hyperparameters, _weightsSize, cudaMemcpyHostToDevice));
#endif
}

void Recurrent::getHyperparameters(float *hyperparameters)
{
#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Getting dimensions
    const size_t L = _depth;
    const size_t G = _gateCount;
    const size_t IC = _prevLayer->_outputChannels;
    const size_t OC = _outputChannels;

    read_from_dnnl_memory(&hyperparameters[0], _weightsLayerMem);
    read_from_dnnl_memory(&hyperparameters[L * G * OC * IC], _weightsRecurrentMem);
    read_from_dnnl_memory(&hyperparameters[L * G * OC * OC + L * G * OC * IC], _biasMem);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN") cudaErrCheck(cudaMemcpy(hyperparameters, _weightsTensor, _weightsSize, cudaMemcpyDeviceToHost));
#endif
}

void Recurrent::getHyperparameterGradients(float *gradient)
{
#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Getting dimensions
    const size_t L = _depth;
    const size_t G = _gateCount;
    const size_t IC = _prevLayer->_outputChannels;
    const size_t OC = _outputChannels;

    read_from_dnnl_memory(&gradient[0], _weightsLayerGradientMem);
    read_from_dnnl_memory(&gradient[L * G * OC * IC], _weightsRecurrentGradientMem);
    read_from_dnnl_memory(&gradient[L * G * OC * OC + L * G * OC * IC], _biasGradientMem);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN") cudaErrCheck(cudaMemcpy(gradient, _weightsGradientTensor, _weightsSize, cudaMemcpyDeviceToHost));
#endif
}

void Recurrent::setConfiguration(knlohmann::json& js)
{
 if (isDefined(js, "Results"))  eraseValue(js, "Results");

 if (isDefined(js, "Depth"))
 {
 try { _depth = js["Depth"].get<size_t>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ recurrent ] \n + Key:    ['Depth']\n%s", e.what()); }
   eraseValue(js, "Depth");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Depth'] required by recurrent.\n");

 Layer::setConfiguration(js);
 _type = "layer/recurrent";
 if(isDefined(js, "Type")) eraseValue(js, "Type");
 if(isEmpty(js) == false) KORALI_LOG_ERROR(" + Unrecognized settings for Korali module: recurrent: \n%s\n", js.dump(2).c_str());
}

void Recurrent::getConfiguration(knlohmann::json& js)
{

 js["Type"] = _type;
   js["Depth"] = _depth;
 Layer::getConfiguration(js);
}

void Recurrent::applyModuleDefaults(knlohmann::json& js)
{

 std::string defaultString = "{\"Depth\": 1}";
 knlohmann::json defaultJs = knlohmann::json::parse(defaultString);
 mergeJson(js, defaultJs);
 Layer::applyModuleDefaults(js);
}

void Recurrent::applyVariableDefaults()
{

 Layer::applyVariableDefaults();
}

;

} //layer
} //neuralNetwork
} //korali
;


#include <limits>
#include <numeric>
#include <algorithm>
#include <unordered_map>

#include "CorresBuilder.h"
#include "DataProvider.h"

#include <pcl/common/common.h>
#include <pcl/console/print.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/transformation_estimation_point_to_plane_lls.h>

#include <boost/filesystem.hpp>
#include <boost/thread.hpp>
#include <boost/math/special_functions.hpp>
#include <boost/random.hpp>

using namespace std;

float CorresBuilder::volumeOverlapRatio(const Eigen::Affine3f& trans)
{
    const int res = 20;
    double ul = 3. / res;
    int s = 0;
    for (int i = 0; i < res; i++) {
        for (int j = 0; j < res; j++) {
            for (int k = 0; k < res; k++) {
                Eigen::Vector3f pos((i + 0.5) * ul, (j + 0.5) * ul, (k + 0.5) * ul);
                Eigen::Vector3f ppos = trans * pos;
                if (ppos(0) >= 0 && ppos(0) <= 3. &&
                    ppos(1) >= 0 && ppos(1) <= 3. &&
                    ppos(2) >= 0 && ppos(2) <= 3.) {
                    s++;
                }
            }
        }
    }

    return (double)s / res / res / res;
}

void CorresBuilder::operator()(DataProvider& provider)
{
    assert(provider.size() >= 2);
    m_cloudPairs.clear();
    m_initCloudTransform.clear();
    m_pointClouds.clear();

    PCL_INFO("Start build correspondence...\n");

    initCloudAndTransform(provider);
    initCloudPair();
    if (m_params.cloudPairNeedAlignment)
        alignPairs();
    findCorresPoints();
}

void CorresBuilder::initCloudAndTransform(DataProvider& provider)
{
    m_pointClouds.resize(provider.size());

    for (uint32_t i = 0; i < provider.size(); ++i)
    {
        CloudTypePtr rawCloud = provider[i];
        CloudTypePtr cloud(new CloudType);
        for (int j = 0; j < rawCloud->points.size(); ++j)
        {
            if (!pcl_isnan(rawCloud->points[j].normal_x)) {
                cloud->push_back(rawCloud->points[j]);
            }
        }
        m_pointClouds[i] = cloud;
    }

    m_initCloudTransform.resize(provider.size());

    //CloudTransform base = provider.initTransformOfCloudAtIndex(0).inverse();
    for (uint32_t i = 0; i < provider.size(); ++i)
    {
        CloudTransform trans = provider.initTransformOfCloudAtIndex(i);
        m_initCloudTransform[i] = trans;
    }
}

void CorresBuilder::initCloudPair()
{
    uint32_t size = m_pointClouds.size();
    for (uint32_t i = 0; i < size - 1; ++i)
    {
        m_cloudPairs.push_back(CloudPair(i, i + 1, m_initCloudTransform[i].inverse() * m_initCloudTransform[i + 1]));
        for (uint32_t j = i + 2; j < size; ++j)
        {
            CloudTransform trans = m_initCloudTransform[i].inverse() * m_initCloudTransform[j];
            if (volumeOverlapRatio(trans) > 0.3f && centroidDistance(i, j) < 1.5) {
                m_cloudPairs.push_back(CloudPair(i, j, trans));
            }
        }
    }
    PCL_WARN("%d cloud pairs\n", m_cloudPairs.size());
}

void CorresBuilder::alignPairs()
{

    int cpu_cores = std::max(boost::thread::hardware_concurrency(), (unsigned)2);
    int count = 0;
    boost::thread_group group;
    BOOST_FOREACH(CloudPair &pair, m_cloudPairs) {
        if (count == cpu_cores) {
            count = 0;
            group.join_all();
        }
        group.create_thread(boost::bind(&CorresBuilder::alignEachPair, this, boost::ref(pair)));
        count++;
    }
    group.join_all();

    /*
    BOOST_FOREACH(CloudPair &pair, m_cloudPairs) {
        alignEachPair(pair);
    }
     */
}

void CorresBuilder::findCorresPoints()
{

    boost::thread_group group;
    int cores = std::max(boost::thread::hardware_concurrency(), (unsigned)2);
    int count = 0;
    for (size_t i = 0; i < m_cloudPairs.size(); ++i) {
        if (count == cores) {
            count = 0;
            group.join_all();
        }
        if (m_cloudPairs[i].corresIdx != std::make_pair(-1, -1)) {
            group.create_thread(boost::bind(&CorresBuilder::findCorres, this, boost::ref(m_cloudPairs[i])));
            count++;
        }
    }
    group.join_all();
    /*
    for (size_t i = 0; i < m_cloudPairs.size(); ++i) {
        findCorres(m_cloudPairs[i]);
    }*/
}

void CorresBuilder::gridFilterPointPair(CloudPair& pair, float gridSize, int Nc)
{
    PointType minPoint, maxPoint;
    minPoint.x = minPoint.y = minPoint.z = std::numeric_limits<float>::max();
    maxPoint.x = maxPoint.y = maxPoint.z = -std::numeric_limits<float>::max();

    std::vector<PointPair, Eigen::aligned_allocator<PointPair> > &pointCorres = pair.corresPointIdx;
    CloudTypePtr pointCloud = m_pointClouds[pair.corresIdx.first];

    // find min max
    for (size_t p = 0; p < pointCorres.size(); ++p) {
        PointType point = pointCloud->points[pointCorres[p].first];
        if (point.x < minPoint.x) {
            minPoint.x = point.x;
        }
        if (point.y < minPoint.y) {
            minPoint.y = point.y;
        }
        if (point.z < minPoint.z) {
            minPoint.z = point.z;
        }

        if (point.x > maxPoint.x) {
            maxPoint.x = point.x;
        }
        if (point.y > maxPoint.y) {
            maxPoint.y = point.y;
        }
        if (point.z > maxPoint.z) {
            maxPoint.z = point.z;
        }
    }

    uint32_t grid_size_x = static_cast<uint32_t>(ceilf((maxPoint.x - minPoint.x) / gridSize));
    uint32_t grid_size_y = static_cast<uint32_t>(ceilf((maxPoint.y - minPoint.y) / gridSize));
    uint32_t grid_size_z = static_cast<uint32_t>(ceilf((maxPoint.z - minPoint.z) / gridSize));

    // split into grid
    typedef std::unordered_map<uint32_t, std::vector<PointPair> > Bin;
    Bin pointGridBin;
    for (size_t p = 0; p < pointCorres.size(); ++p) {
        PointType point = pointCloud->points[pointCorres[p].first];
        uint32_t index_x = static_cast<uint32_t>(floorf((point.x - minPoint.x) / gridSize));
        uint32_t index_y = static_cast<uint32_t>(floorf((point.y - minPoint.y) / gridSize));
        uint32_t index_z = static_cast<uint32_t>(floorf((point.z - minPoint.z) / gridSize));
        uint32_t index = index_x + index_y * grid_size_x + index_z * grid_size_x * grid_size_y;
        pointGridBin[index].push_back(pointCorres[p]);
    }

    // sort by quality
    for (Bin::iterator it = pointGridBin.begin(); it != pointGridBin.end(); ++it)
    {
        if (it->second.size() <= Nc) {
            continue;
        }
        std::sort(it->second.begin(), it->second.end(), PointPair::PointPairComparer());
        it->second.resize(Nc);
    }

    // re-merge
    pointCorres.clear();
    for (Bin::iterator it = pointGridBin.begin(); it != pointGridBin.end(); ++it)
    {
        pointCorres.insert(pointCorres.end(), it->second.begin(), it->second.end());
    }
    std::sort(pointCorres.begin(), pointCorres.end(), PointPair::PointPairIndexComparer());
}

void CorresBuilder::findCorres(CloudPair& pair)
{
    std::vector<PointPair, Eigen::aligned_allocator<PointPair> > &pointCorrespondence = pair.corresPointIdx;
    pointCorrespondence.clear();

    const int K = 1;

    if (pair.corresIdx == std::make_pair(-1, -1))
    {
        return;
    }

    pcl::KdTreeFLANN<PointType> kdtree;
    std::vector<int> pointIndices(K);
    std::vector<float> pointDistances(K);

    PCL_INFO("Processing pair <%d, %d>\n", pair.corresIdx.first, pair.corresIdx.second);

    CloudTypePtr p0 = m_pointClouds[pair.corresIdx.first];
    CloudTypePtr p1 = m_pointClouds[pair.corresIdx.second];

    CloudTypePtr transformed(new CloudType);
    pcl::transformPointCloudWithNormals(*p1, *transformed, pair.relativeTrans);

    kdtree.setInputCloud(p0);
    const float normThres = cosf(m_params.corresPointNormThres * M_PI / 180);
    for (size_t n = 0; n < transformed->size(); ++n) {
        if (!m_params.useBetterCorrespondence) {
            if (kdtree.nearestKSearch(transformed->points[n], K, pointIndices, pointDistances) > 0) {
                const PointType &point0 = p0->points[pointIndices[0]];
                const PointType &point1 = transformed->points[n];
                float norm = Eigen::Map<Eigen::Vector3f>(point0.normal).dot(Eigen::Map<Eigen::Vector3f>(point1.normal));
                if (pointDistances[0] < m_params.corresPointDistThres * m_params.corresPointDistThres &&
                    fabsf(norm) > normThres) {
                    pointCorrespondence.push_back(PointPair(pointIndices[0], n, norm));
                }
            }
        }
        else {
            const float lamda = 10.f;

            if (kdtree.radiusSearch(transformed->points[n], m_params.corresPointDistThres, pointIndices, pointDistances, 50) > 0) {
                for (int i = 0; i < pointDistances.size(); ++i)
                {
                    Eigen::Vector3f Np = transformed->points[n].getNormalVector3fMap();
                    Eigen::Vector3f Nq = p0->points[pointIndices[i]].getNormalVector3fMap();
                    pointDistances[i] = 1.f - sqrtf(pointDistances[i]) / m_params.corresPointDistThres + lamda * Np.dot(Nq);
                }
                int index = std::max_element(pointDistances.begin(), pointDistances.end()) - pointDistances.begin();
                float norm = transformed->points[n].getNormalVector3fMap().dot(p0->points[pointIndices[index]].getNormalVector3fMap());
                if ( norm > normThres) {
                    pointCorrespondence.push_back(PointPair(pointIndices[index], n, pointDistances[index]));
                }
            }
        }
    }

    PCL_INFO("\t<%d, %d> : Corresponce point number %d / %d\n", pair.corresIdx.first, pair.corresIdx.second, pointCorrespondence.size(), pair.validCorresPointNumber);
    if (pair.validCorresPointNumber > 0 && (pointCorrespondence.size() < pair.validCorresPointNumber / 2)) {
        PCL_WARN("\tReduced too much!\n");
        pair.corresIdx = std::make_pair(-1, -1);
    }
    else {
        gridFilterPointPair(pair, 0.2f, 10);
        pair.validCorresPointNumber = pointCorrespondence.size();
    }

    if (m_params.saveCorresPointIndices && pair.corresIdx != std::make_pair(-1, -1)) {
        char file[1024] = { 0 };
        sprintf(file, "%s/corres_%d_%d.txt", m_params.corresPointSavePath.c_str(), pair.corresIdx.first, pair.corresIdx.second);
        ofstream fs;
        fs.open(file, ofstream::out);
        if (fs.is_open()) {
            for (size_t i = 0; i < pointCorrespondence.size(); ++i) {
                fs << pointCorrespondence[i].first << " " << pointCorrespondence[i].second << endl;
            }
            fs.close();
        }
        else {
            PCL_ERROR("fail to save %s !", file);
        }
    }
}

CloudTypePtr CorresBuilder::downsampledCloudWithNumberOfPoints(CloudTypePtr cloud, int points)
{
    if (cloud->size() <= points)
        return cloud;

    size_t size = cloud->size();
    CloudTypePtr downsampledCloud(new CloudType);

    boost::random::mt19937 rng;
    boost::random::uniform_int_distribution<> six(0, size - 1);
    for (int i = 0; i < points; ++i)
    {
        downsampledCloud->points.push_back(cloud->points[six(rng)]);
    }
    downsampledCloud->width = downsampledCloud->points.size();
    downsampledCloud->height = 1;
    downsampledCloud->is_dense = true;
    return downsampledCloud;
}

void CorresBuilder::alignEachPair(CloudPair &pair)
{
    const int K = 1;
    pcl::KdTreeFLANN<PointType> kdtree;
    std::vector<int> pointIndices(K);
    std::vector<float> pointDistances(K);

    PCL_INFO("Align pair <%d, %d>\n", pair.corresIdx.first, pair.corresIdx.second);

    int sample_count = m_params.randomSamplingLimitUsedToAlignment;
    CloudTypePtr p0 = downsampledCloudWithNumberOfPoints(m_pointClouds[pair.corresIdx.first], sample_count);
    CloudTypePtr p1 = downsampledCloudWithNumberOfPoints(m_pointClouds[pair.corresIdx.second], sample_count);

    CloudTypePtr transformed(new CloudType);
    pcl::transformPointCloudWithNormals(*p1, *transformed, pair.relativeTrans);

    kdtree.setInputCloud(p0);
    size_t count = 0;
#pragma unroll 8
    for (size_t n = 0; n < transformed->size(); ++n) {
        if (kdtree.nearestKSearch(transformed->points[n], K, pointIndices, pointDistances) > 0) {
            if (pointDistances[0] < m_params.acceptableICPPointDistThres * m_params.acceptableICPPointDistThres) {
                ++count;
            }
        }
    }

    double r1 = (double)count / (double)p0->size();
    double r2 = (double)count / (double)p1->size();
    PCL_INFO("    <%d, %d> : %d inliers with ratio %.2f(%d) and %.2f(%d) ...\n", pair.corresIdx.first, pair.corresIdx.second, count, r1, p0->size(), r2, p1->size());

    const int min_count = m_params.acceptableCorresPointNum;
    const float min_ratio = m_params.acceptableCorresPointRatio;
    bool accept = count >= min_count || (r1 > min_ratio && r2 > min_ratio);
    if (!accept) {
        PCL_INFO("reject\n");
        pair.corresIdx = std::make_pair(-1, -1);
        return;
    }

    PCL_INFO("accept\n");
    pair.validCorresPointNumber = count;

    pcl::IterativeClosestPoint<PointType, PointType> icp;
    typedef pcl::registration::TransformationEstimationPointToPlaneLLS<PointType, PointType> P2PlaneEstimation;
    boost::shared_ptr<P2PlaneEstimation> point_to_plane(new P2PlaneEstimation);

    icp.setInputCloud(downsampledCloudWithNumberOfPoints(p1, 20000));
    icp.setInputTarget(downsampledCloudWithNumberOfPoints(p0, 20000));
    icp.setMaxCorrespondenceDistance(m_params.acceptableICPPointDistThres);
    icp.setMaximumIterations(m_params.maxICPIteration);
    icp.setRANSACOutlierRejectionThreshold(0.05);
    icp.setRANSACIterations(20);
    icp.setTransformationEpsilon(1e-6);
    icp.setTransformationEstimation(point_to_plane);
    icp.align(*transformed, pair.relativeTrans.matrix());

    if (icp.hasConverged()) {
        boost::mutex::scoped_lock lock(m_outputMutex);

        PCL_INFO("\t<%d, %d> : ICP fitness score is %.6f\n", pair.corresIdx.first, pair.corresIdx.second, icp.getFitnessScore());
        PCL_INFO("\tTransform matrix from :\n");
        cout << pair.relativeTrans.matrix() << endl;
        PCL_INFO("\tto:\n");
        cout << icp.getFinalTransformation() << endl;

        Eigen::Affine3f changed;
        changed = pair.relativeTrans.matrix().inverse() * icp.getFinalTransformation();
        if (changed.translation().maxCoeff() > m_params.translationThres || changed.translation().minCoeff() < -m_params.translationThres) {
            PCL_WARN("Pair align translate too much!! Reject\n");
            pair.corresIdx = std::make_pair(-1, -1);
            return;
        }

        Eigen::AngleAxisf angle;
        angle.fromRotationMatrix(changed.linear());
        if (fabsf(angle.angle()) > m_params.rotationThres * M_PI / 180) {
            PCL_WARN("Pair align rotate too much!! Reject\n");
            pair.corresIdx = std::make_pair(-1, -1);
            return;
        }

        pair.relativeTrans = icp.getFinalTransformation();
    }
    else {
        PCL_WARN("\tCan't align pair <%d, %d>\n", pair.corresIdx.first, pair.corresIdx.second);
        pair.corresIdx = std::make_pair(-1, -1);
    }
}

float CorresBuilder::centroidDistance(int cloud0, int cloud1)
{
    Eigen::Vector4f c0, c1;
    pcl::compute3DCentroid(*m_pointClouds[cloud0], c0);
    pcl::compute3DCentroid(*m_pointClouds[cloud1], c1);

    return (c0 - c1).norm();
}


#include "active_3d_planning_core/module/sensor_model/iterative_ray_caster_lidar.h"

#include <algorithm>
#include <cmath>
#include <vector>

#include <Eigen/Dense>

namespace active_3d_planning {
namespace sensor_model {

ModuleFactoryRegistry::Registration<IterativeRayCasterLidar>
    IterativeRayCasterLidar::registration("IterativeRayCasterLidar");

IterativeRayCasterLidar::IterativeRayCasterLidar(PlannerI& planner)
    : LidarModel(planner) {}

void IterativeRayCasterLidar::setupFromParamMap(Module::ParamMap* param_map) {
  LidarModel::setupFromParamMap(param_map);
  setParam<double>(param_map, "ray_step", &p_ray_step_, map_->getVoxelSize());
  setParam<double>(param_map, "downsampling_factor", &p_downsampling_factor_,
                   1.0);

  // Downsample to voxel size resolution at max range
  c_res_x_ = std::min(static_cast<int>(std::ceil(
                          p_ray_length_ * p_fov_x_ /
                          (map_->getVoxelSize() * p_downsampling_factor_))),
                      p_resolution_x_);
  c_res_y_ = std::min(static_cast<int>(std::ceil(
                          p_ray_length_ * p_fov_y_ /
                          (map_->getVoxelSize() * p_downsampling_factor_))),
                      p_resolution_y_);

  // Determine number of splits + split distances
  c_n_sections_ =
      std::floor(static_cast<double>(std::log2(std::min(c_res_x_, c_res_y_))));
  c_split_widths_.push_back(0);
  for (int i = 0; i < c_n_sections_; ++i) {
    c_split_widths_.push_back(std::pow(2, i));
    c_split_distances_.push_back(p_ray_length_ /
                                 std::pow(2.0, static_cast<double>(i)));
  }
  c_split_distances_.push_back(0.0);
  std::reverse(c_split_distances_.begin(), c_split_distances_.end());
  std::reverse(c_split_widths_.begin(), c_split_widths_.end());
}

bool IterativeRayCasterLidar::getVisibleVoxels(
    std::vector<Eigen::Vector3d>* result, const Eigen::Vector3d& position,
    const Eigen::Quaterniond& orientation) {
  // Setup ray table (contains at which segment to start, -1 if occluded
  ray_table_ = Eigen::ArrayXXi::Zero(c_res_x_, c_res_y_);

  // Ray-casting
  Eigen::Vector3d camera_direction;
  Eigen::Vector3d direction;
  Eigen::Vector3d current_position;
  Eigen::Vector3d voxel_center;
  double distance;
  bool cast_ray;
  double map_distance;
  for (int i = 0; i < c_res_x_; ++i) {
    for (int j = 0; j < c_res_y_; ++j) {
      int current_segment = ray_table_(i, j);  // get ray starting segment
      if (current_segment < 0) {
        continue;  // already occluded ray
      }
      LidarModel::getDirectionVector(
          &camera_direction,
          static_cast<double>(i) / (static_cast<double>(c_res_x_) - 1.0),
          static_cast<double>(j) / (static_cast<double>(c_res_y_) - 1.0));
      direction = orientation * camera_direction;
      distance = c_split_distances_[current_segment];
      cast_ray = true;
      while (cast_ray) {
        // iterate through all splits (segments)
        while (distance < c_split_distances_[current_segment + 1]) {
          current_position = position + distance * direction;
          distance += p_ray_step_;

          // Add point (duplicates are handled in
          // CameraModel::getVisibleVoxelsFromTrajectory)
          map_->getVoxelCenter(&voxel_center, current_position);
          result->push_back(voxel_center);

          // Check voxel occupied
          if (map_->getVoxelState(current_position) ==
              map::OccupancyMap::OCCUPIED) {
            // Occlusion, mark neighboring rays as occluded
            markNeighboringRays(i, j, current_segment, -1);
            cast_ray = false;
            break;
          }
        }
        if (cast_ray) {
          current_segment++;
          if (current_segment >= c_n_sections_) {
            cast_ray = false;  // done
          } else {
            // update ray starts of neighboring rays
            markNeighboringRays(i, j, current_segment - 1, current_segment);
          }
        }
      }
    }
  }
  return true;
}

void IterativeRayCasterLidar::markNeighboringRays(int x, int y, int segment,
                                                  int value) {
  // Set all nearby (towards bottom right) ray starts, depending on the segment
  // depth, to a value.
  for (int i = x; i < std::min(c_res_x_, x + c_split_widths_[segment]); ++i) {
    for (int j = y; j < std::min(c_res_y_, y + c_split_widths_[segment]); ++j) {
      ray_table_(i, j) = value;
    }
  }
}

}  // namespace sensor_model
}  // namespace active_3d_planning


#define BOOST_TEST_MODULE SolutionTest

#include "solution.hpp"

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(SolutionSuite)

BOOST_AUTO_TEST_CASE(PlainTest1)
{
    vector<int> num1{1, 3};
    vector<int> num2{2};
    float results = Solution().findMedianSortedArrays(num1, num2);
    float expected = 2.0;

    BOOST_CHECK_EQUAL(results, expected);
}

BOOST_AUTO_TEST_CASE(PlainTest2)
{
    vector<int> num1{1, 2};
    vector<int> num2{3, 4};
    float results = Solution().findMedianSortedArrays(num1, num2);
    float expected = 2.5;

    BOOST_CHECK_EQUAL(results, expected);
}

BOOST_AUTO_TEST_SUITE_END()