//Use an image number A and A+5 to calculate the error
//30~39, 126~135, 236~245, 322~331, 426~435, 531~540, 620~629, 731~740, 825~834

//Using synthetic dataset to test the property of image NID

#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <vector>
#include <iostream>
#include <memory>

#include "g2o/g2o/core/block_solver.h"
#include "g2o/g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/g2o/solvers/linear_solver_eigen.h"
#include "g2o/g2o/types/types_six_dof_expmap.h"
#include "g2o/g2o/core/robust_kernel_impl.h"
#include "g2o/g2o/solvers/linear_solver_dense.h"

#include "CudaPoints3d.cuh"
#include "CudaComputeHref.cuh"

int cell = 16;
int bin_num = 10;//if you modify the bin number, modify the knots in the types_six_dof_expmap.h
int bs_degree = 3;

struct Intrinsics{
    double fx;
    double fy;
    double cx;
    double cy;
};

void Get3dPointAndIntensity(int cell_size, int cell_row_id, int cell_col_id, const cv::Mat& image, const cv::Mat& depth, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >& points_3d, Eigen::VectorXd& intensity, const Eigen::Matrix4d& T_wc, Intrinsics in, double* point3dcuda, std::vector<int>& location);

std::vector<cv::Mat> ReadGroundtruth(std::string add, std::string dataset);

g2o::SE3Quat toSE3Quat(const cv::Mat &cvT)
{
    Eigen::Matrix<double,3,3> R;
    R << cvT.at<double>(0,0), cvT.at<double>(0,1), cvT.at<double>(0,2),
         cvT.at<double>(1,0), cvT.at<double>(1,1), cvT.at<double>(1,2),
         cvT.at<double>(2,0), cvT.at<double>(2,1), cvT.at<double>(2,2);

    Eigen::Matrix<double,3,1> t(cvT.at<double>(0,3), cvT.at<double>(1,3), cvT.at<double>(2,3));

    return g2o::SE3Quat(R,t);
}

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

    cudaFree(0);

    if(argc!=2){
        std::cout<<"usage './program path_to_config.yaml', image1's timestamp should be smaller than image2"<<std::endl;
        exit(0);
    }
    std::vector<std::vector<std::vector<double> > > bs;
    std::vector<std::vector<std::vector<double> > > bs_der;

    std::ofstream of("nid_error.csv", std::ofstream::out | std::ofstream::app);

    std::string config_add = argv[1];
    cv::FileStorage fc;
    fc.open(config_add, cv::FileStorage::READ);
    std::string type0 = fc["image0_type"] , type1 = fc["image1_type"], id0 = fc["image0_id"], id1 = fc["image1_id"], use_gt = fc["use_groundtruth"], dataset = fc["dataset"], im_add = fc["im_address"];
    double depth_factor = 1.0/(int)fc["depth_factor"], fx = fc["fx"], fy = fc["fy"], cx = fc["cx"], cy = fc["cy"];
    bool use_gpu = static_cast<int>(fc["use_gpu"]) != 0;

    int pose_id0 = stoi(id0);
    int pose_id1 = stoi(id1);
    int interval = pose_id1 - pose_id0;

    for(int ti = 0; ti<1; ti++){
      std::cout<<"optimize relative pose between "<<pose_id0<<" and "<<pose_id1<<", in string "<<id0.c_str()<<","<<id1.c_str()<<std::endl;
      cv::Mat im0,im1;
      std::string rgb0_add, rgb1_add;
      if(dataset == "eth_cvg"){
        rgb0_add = im_add + type0 + "/" + id0 + ".png";
        rgb1_add = im_add + type1 + "/" + id1 + ".png";
      }

      std::cout<<"dataset address "<<rgb0_add<<std::endl;

      cv::Mat im_rgb0 = cv::imread(rgb0_add,CV_LOAD_IMAGE_UNCHANGED);//use cv::IMREAD_GRAYSCALE, the result will be different
      im0 = im_rgb0;
      cvtColor(im0,im0,CV_RGB2GRAY);

      cv::Mat im_rgb1 = cv::imread(rgb1_add,CV_LOAD_IMAGE_UNCHANGED);
      im1 = im_rgb1;
      cvtColor(im1,im1,CV_RGB2GRAY);

      std::cout<<"image size "<<im0.size()<<","<<im1.size()<<std::endl;

      //read corresponding depth
      cv::Mat depth0;
      if(dataset == "eth_cvg"){
        std::string depth_add = im_add + "depth/" + id0 + ".png";
        depth0 = cv::imread(depth_add, CV_LOAD_IMAGE_UNCHANGED);
        depth0.convertTo(depth0,CV_64F,depth_factor);
      }

      std::string pose_gt_add;
      if(dataset == "eth_cvg"){
        pose_gt_add = im_add + "groundtruth.txt";
      }
      std::vector<cv::Mat> gt = ReadGroundtruth(pose_gt_add, dataset);

      std::cout<<"id of two image is "<<pose_id0<<","<<pose_id1<<std::endl;

      cv::Mat T_wc0_cv,T_wc1_cv, T_cw0_cv, T_cw1_cv;

      //pose id output from slam needs to corresponds to the id input of this program
      cv::FileStorage fp0, fp1;
      if(use_gt == "0"){
        if(fp0.open(std::to_string(pose_id0) + ".xml", cv::FileStorage::READ) && fp1.open(std::to_string(pose_id1) + ".xml", cv::FileStorage::READ)){
          std::cout<<"use the pose from etimation"<<std::endl;
          fp0["pose"]>>T_cw0_cv;
          fp1["pose"]>>T_cw1_cv;
          T_cw0_cv.convertTo(T_cw0_cv, CV_64F);
          T_cw1_cv.convertTo(T_cw1_cv, CV_64F);
          T_wc0_cv = T_cw0_cv.inv();
          T_wc1_cv = T_cw1_cv.inv();
        }
        else{
          std::cout<<"no correspoding pose, exit "<<std::endl;
          exit(0);
        }
      }
      else if(use_gt == "1"){
          std::cout<<"use groundtruth pose "<<std::endl;
          T_wc0_cv = gt[pose_id0];
          T_wc1_cv = gt[pose_id1];
      }
      else{
        std::cout<<"you must have pose provided by groundtruth or SLAM"<<std::endl;
      }

      Intrinsics in;
      in.fx = fx;
      in.fy = fy;
      in.cx = cx;
      in.cy = cy;

      Eigen::Matrix4d T_wc0, T_wc1;
      T_wc0<<T_wc0_cv.ptr<double>(0)[0], T_wc0_cv.ptr<double>(0)[1], T_wc0_cv.ptr<double>(0)[2], T_wc0_cv.ptr<double>(0)[3],
            T_wc0_cv.ptr<double>(1)[0], T_wc0_cv.ptr<double>(1)[1], T_wc0_cv.ptr<double>(1)[2], T_wc0_cv.ptr<double>(1)[3],
            T_wc0_cv.ptr<double>(2)[0], T_wc0_cv.ptr<double>(2)[1], T_wc0_cv.ptr<double>(2)[2], T_wc0_cv.ptr<double>(2)[3],
            0                              , 0                              , 0                              , 1;
      T_wc1<<T_wc1_cv.ptr<double>(0)[0], T_wc1_cv.ptr<double>(0)[1], T_wc1_cv.ptr<double>(0)[2], T_wc1_cv.ptr<double>(0)[3],
            T_wc1_cv.ptr<double>(1)[0], T_wc1_cv.ptr<double>(1)[1], T_wc1_cv.ptr<double>(1)[2], T_wc1_cv.ptr<double>(1)[3],
            T_wc1_cv.ptr<double>(2)[0], T_wc1_cv.ptr<double>(2)[1], T_wc1_cv.ptr<double>(2)[2], T_wc1_cv.ptr<double>(2)[3],
            0                              , 0                              , 0                              , 1;

      double time_use = (double)cv::getTickCount();
      /*set g2o relates */
      g2o::SparseOptimizer optimizer;
      g2o::BlockSolver_6_X::LinearSolverType * linearSolver;

      linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_X::PoseMatrixType>();

      g2o::BlockSolver_6_X * solver_ptr = new g2o::BlockSolver_6_X(linearSolver);

      g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);

      optimizer.setAlgorithm(solver);

      optimizer.setVerbose(true);

      g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();

      std::cout<<"before add disturbance the T_wc1 inverse is \n"<<T_wc1.inverse()<<std::endl;

      g2o::SE3Quat T_cw1_g2o = toSE3Quat(T_wc1_cv.inv());
      g2o::Vector6d min_vec_gt = T_cw1_g2o.toMinimalVector();

      const Eigen::Matrix4d SE3_ori= T_wc1;
      Eigen::Vector3d t_dist;
      double t_offset = 0.02;
      double r_offset = 0.005 ;//0.005 has good result, 0.007 also shows a lot improvements

      t_dist<<0.5*t_offset, -t_offset, -t_offset;

      Eigen::Matrix3d rotation_dist; //= Eigen::Matrix3d::Identity();
      rotation_dist = Eigen::AngleAxisd(r_offset*M_PI, Eigen::Vector3d::UnitX())
      * Eigen::AngleAxisd(r_offset*M_PI,  Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(r_offset*M_PI, Eigen::Vector3d::UnitZ());

      Eigen::Matrix4d dist = Eigen::Matrix4d::Zero();
      dist.block<3,3>(0,0) = rotation_dist;
      dist(3,3) = 1.0;

      Eigen::Matrix3d r_cw1;
      Eigen::Vector3d t_cw1;
      r_cw1 = T_wc1.block<3,3>(0,0).transpose();
      t_cw1 = -r_cw1 * T_wc1.block<3,1>(0,3);

      r_cw1 = rotation_dist * r_cw1;
      t_cw1 = t_cw1 + t_dist;

      //std::cout<<"inv t_fcw 1 \n"<<r_cw1<<"\n"<<t_cw1.transpose()<<std::endl;

      vSE3->setEstimate(g2o::SE3Quat(r_cw1,t_cw1));//g2o::SE3Quat(r_cw1,t_cw1)//g2o::SE3Quat(Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero())
      vSE3->setId(0);
      vSE3->setFixed(false);
      optimizer.addVertex(vSE3);
      std::cout<<"original matrix to be optimized \n"<<vSE3->estimate().to_homogeneous_matrix()<<std::endl;
      g2o::Vector6d to_be_estimated_pose = vSE3->estimate().toMinimalVector();
      g2o::Vector6d error_ori = min_vec_gt - to_be_estimated_pose;
      std::cout<<"the error to be minimized is (6d minimal form) \n"<<error_ori.transpose()<<std::endl;

      int cell_size = cell * cell;
      std::vector<g2o::EdgeSE3ProjectIntensityOnlyPoseNID*> vpEdges;
      std::vector<size_t> vnIndexEdge;
      std::vector<bool> inlier;
      inlier.reserve(cell_size);
      vpEdges.reserve(cell_size);
      vnIndexEdge.reserve(cell_size);

      double* points_3d_all;
      double* im0_data;
      double* im1_data;
      double* intrinscis = (double*)malloc(5*sizeof(double));
      intrinscis[0] = fx; intrinscis[1] = fy; intrinscis[2] = cx;intrinscis[3] = cy; intrinscis[4] = depth_factor;
      double *bs_value = (double *)malloc(sizeof(double) * im0.cols * im0.rows * 4);
      int* bin_index = (int *)malloc(sizeof(int) * im0.cols * im0.rows);
      int* bs_counter = (int*)malloc(cell*cell*sizeof(int));
      double* Href = (double*)malloc(cell*cell*sizeof(double));
      memset(Href,0,cell*cell*sizeof(double));

      cudaMallocManaged(&points_3d_all, 3*im0.cols*im0.rows*sizeof(double));
      cudaMallocManaged(&im0_data, im0.cols*im0.rows*sizeof(double));
      cudaMallocManaged(&im1_data, im1.cols*im1.rows*sizeof(double));

      //seems we have to use this naive way to initialze the value
      for(int i = 0; i<im0.rows * im0.cols; i++){
        im0_data[i] = (double)im0.data[i];
      }

      for(int i = 0; i<im1.rows * im1.cols; i++){
        im1_data[i] = (double)im1.data[i];
      }

      Calculate3Dpoint(depth0.ptr<double>(0), T_wc0.data(), points_3d_all, intrinscis, im0.rows, im0.cols);

      double tt = 0.0;
      tt = (double)cv::getTickCount();
      CudaComputeHref(im0_data, points_3d_all, vSE3->estimate().to_homogeneous_matrix().data(), intrinscis, bin_num, bs_degree, cell, im0.rows, im0.cols, bs_value, bin_index, bs_counter, Href);
      tt = ((double)cv::getTickCount() - tt)/cv::getTickFrequency();
      //std::cout<<"use "<<tt<<" compute Href related"<<std::endl;

      //Assign value to the optimizer so that it can compute staff like H, Href and H_joint
      optimizer.im0_ = im0_data;
      optimizer.im1_ = im1_data;
      optimizer.points3d_ = points_3d_all;
      optimizer.rows_ = im0.rows;
      optimizer.cols_ = im0.cols;
      optimizer.camera_intrincis_ = intrinscis;
      optimizer.bin_num_ = bin_num;
      optimizer.bs_degree_ = bs_degree;
      optimizer.cell_num_ = cell;
      optimizer.bs_counter_ = bs_counter;
      optimizer.bs_value_ref_ = bs_value;
      optimizer.bs_index_ref_ = bin_index;
      optimizer.Href_ = Href;

      const double deltaNID = sqrt(0.95);//0.8 for 1st trial
      //calculate entropy of each cell
      double total_nid = 0.0;
      double t0;
      for(int i = 0; i<cell; i++){
          for(int j = 0; j<cell; j++){
              std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > points_3d;
              Eigen::VectorXd intensity;
              std::vector<int> pixel_location;

              if(!use_gpu)
                Get3dPointAndIntensity(cell, i, j, im0, depth0, points_3d, intensity, T_wc0, in, points_3d_all,pixel_location);

              g2o::EdgeSE3ProjectIntensityOnlyPoseNID* e = new g2o::EdgeSE3ProjectIntensityOnlyPoseNID();

              e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));

              e->use_CPU_ = !use_gpu;

              if(!use_gpu){
                e->image1_ = im1;
                e->fx_ = in.fx;
                e->fy_ = in.fy;
                e->cx_ = in.cx;
                e->cy_ = in.cy;
                e->x_world_set_ = points_3d;
                e->pixel_location_ = pixel_location;
                e->setMeasurement(intensity);
              }

              //must first set Measurement then set b spline relates
              e->set_bspline_relates(bs_degree, bin_num);

              g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
              e->setRobustKernel(rk);
              rk->setDelta(deltaNID);

              Eigen::MatrixXd info = Eigen::MatrixXd::Identity(1,1);
              e->setInformation(info);

              if(!use_gpu){
                e->computeHref();
              }

              if(isnan(Href[j+cell*i])){
                e->setLevel(1);
              }
              else{
                e->set_href(Href[j+cell*i]);
              }

              optimizer.addEdge(e);
              vpEdges.push_back(e);
              vnIndexEdge.push_back(j+cell*i);
              inlier.push_back(true);

          }
          //break;
      }

      const float chi2threshold[4]={1.0, 1.0, 1.0, 1.0};
      const int its[4]={10,10,10,10};

      int nGood=0;

      for(size_t it=0; it<1; it++)
      {
          std::cout<<"enter optimization ............. "<<it<<std::endl;
          vSE3->setEstimate(g2o::SE3Quat(r_cw1,t_cw1));

          optimizer.initializeOptimization(0);
          optimizer.optimize(its[it]);
      }

      // Recover optimized pose and return number of inliers
      g2o::VertexSE3Expmap* vSE3_recov = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0));
      g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();
      g2o::Vector6d min_vec_optimized =  SE3quat_recov.toMinimalVector();
      auto pose = SE3quat_recov.to_homogeneous_matrix();

      g2o::Vector6d error = min_vec_gt - min_vec_optimized;
      std::cout<<"the final error is \n"<<error.transpose()<<std::endl;
      of<<error(0)<<","<<error(1)<<","<<error(2)<<","<<error(3)<<","<<error(4)<<","<<error(5)<<","<<pose_id0<<","<<pose_id1<<std::endl;

      std::cout<<"pose optimized \n"<<pose<<std::endl;

      time_use = ((double)cv::getTickCount() - time_use)/cv::getTickFrequency();
      std::cout<<"use "<<time_use<<" in release mode"<<std::endl;

      //for next iteration
      pose_id0++;
      pose_id1++;
      id0 = std::to_string(pose_id0);
      id1 = std::to_string(pose_id1);
      if(dataset == "eth_cvg"){
        if(id0.length()==1)
          id0 = "000"+id0;
        if(id0.length()==2)
          id0 = "00"+id0;
        if(id0.length()==3)
          id0 = "0"+id0;
        if(id1.length()==1)
          id1 = "000"+id1;
        if(id1.length()==2)
          id1 = "00"+id1;
        if(id1.length()==3)
          id1 = "0"+id1;
      }

      cudaFree(points_3d_all);
      cudaFree(im0_data);
      cudaFree(im1_data);
      free(intrinscis);
      free(bs_value);
      free(bin_index);
      free(bs_counter);
      free(Href);
    }
    return 0;

}

void Get3dPointAndIntensity(int cell_size, int cell_row_id, int cell_col_id, const cv::Mat& image, const cv::Mat& depth, std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >& points_3d, Eigen::VectorXd& intensity, const Eigen::Matrix4d& T_wc, Intrinsics in, double* point3dcuda, std::vector<int>& location){
    int row_block = image.rows / cell_size;
    int col_block = image.cols / cell_size;
    int row_start = row_block * cell_row_id;
    int col_start = col_block * cell_col_id;
    int row_end = row_block * (cell_row_id + 1);
    int col_end = col_block * (cell_col_id + 1);
    int counter = 0;

    std::vector<double> intensity_v;
    for(int i = row_start; i < row_end; i++)
        for(int j = col_start; j < col_end; j++){
            double z_p = depth.ptr<double>(i)[j];
            counter++;
            if(z_p <0.01 || z_p > 100)
                continue;

            double x_p = z_p * (j - in.cx) / in.fx;
            double y_p = z_p * (i - in.cy) / in.fy;

            Eigen::Vector3d p_world = (T_wc*Eigen::Vector4d(x_p,y_p,z_p,1)).head(3);

            points_3d.push_back(p_world);
            intensity_v.push_back(image.ptr<uchar>(i)[j]);
            location.push_back(i*image.cols + j);
        }

    intensity = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(intensity_v.data(), intensity_v.size());

    return;
};

std::vector<cv::Mat> ReadGroundtruth(std::string add, std::string dataset){
    std::vector<cv::Mat> all_gt;

    std::ifstream groundtruth_file(add.c_str());
    if(!groundtruth_file){
        printf("cannot find the file that contains groundtruth \n");
        return all_gt;
    }

    int counter = 0;
    std::string one_row_gt;

    while(getline(groundtruth_file,one_row_gt)){
        std::istringstream temp_one_row_gt(one_row_gt);
        std::string string_gt;
        Eigen::Matrix3d rm;
        double px,py,pz;
        int sequence = 0;
        if (dataset == "eth_cvg"){
            double qx,qy,qz,qw;
            while(getline(temp_one_row_gt, string_gt, ' ')){
                switch(sequence){
                    case 0:{
                        int msg_timestamp = atoi(string_gt.c_str());
                        sequence++;
                    }
                        break;
                    case 1:{
                        //the case 1,2,3 is position x,y,z
                        px = (double)atof(string_gt.c_str());
                        sequence++;
                    }
                        break;
                    case 2:{
                        py = (double)atof(string_gt.c_str());
                        sequence++;
                    }
                        break;
                    case 3:{
                        pz = (double)atof(string_gt.c_str());
                        sequence++;
                    }
                        break;
                    case 4:{
                        //the case 4,5,6,7 is quternion x,y,z,w
                        qx = (double)atof(string_gt.c_str());
                        sequence++;
                    }
                        break;
                    case 5:{
                        qy = (double)atof(string_gt.c_str());
                        sequence++;
                    }
                        break;
                    case 6:{
                        qz = (double)atof(string_gt.c_str());
                        sequence++;
                    }
                        break;
                    case 7:{
                        qw = (double)atof(string_gt.c_str());
                        sequence++;
                    }
                        break;
                    default:
                        break;
                }
            }
            Eigen::Quaterniond qt(qw,qx,qy,qz);
            rm = qt.toRotationMatrix();
        }

        cv::Mat one_gt(4,4,CV_64FC1);
        one_gt.ptr<double>(0)[0] = rm(0,0);
        one_gt.ptr<double>(0)[1] = rm(0,1);
        one_gt.ptr<double>(0)[2] = rm(0,2);
        one_gt.ptr<double>(1)[0] = rm(1,0);
        one_gt.ptr<double>(1)[1] = rm(1,1);
        one_gt.ptr<double>(1)[2] = rm(1,2);
        one_gt.ptr<double>(2)[0] = rm(2,0);
        one_gt.ptr<double>(2)[1] = rm(2,1);
        one_gt.ptr<double>(2)[2] = rm(2,2);
        one_gt.ptr<double>(3)[0] = 0.0;
        one_gt.ptr<double>(3)[1] = 0.0;
        one_gt.ptr<double>(3)[2] = 0.0;

        one_gt.ptr<double>(0)[3] = px;
        one_gt.ptr<double>(1)[3] = py;
        one_gt.ptr<double>(2)[3] = pz;
        one_gt.ptr<double>(3)[3] = 1.0;

        all_gt.push_back(one_gt);

  }

  return all_gt;
};


// -*-indent-tabs-mode:t;c-basic-offset:4;tab-width:4-*-
// © Alfredo A. Correa 2018-2021

#define BOOST_TEST_MODULE "C++ Unit Tests for Multi element access"
#define BOOST_TEST_DYN_LINK
#include<boost/test/unit_test.hpp>
//#include <boost/test/execution_monitor.hpp>  // for boost::execution_exception

#include "../array.hpp"

namespace multi = boost::multi;

template<class T> void what(T&&) = delete;

BOOST_AUTO_TEST_CASE(empty_intersection){
	multi::array<double, 1> arr(10);
	multi::array<double, 1> arr2;

	auto const is = intersection(arr.extension(), arr2.extension());
	BOOST_REQUIRE( arr(is).is_empty() );
	arr2(is) = arr(is);

	BOOST_REQUIRE( arr2(is) == arr(is) );
}

BOOST_AUTO_TEST_CASE(multi_tests_element_access_with_tuple){
	multi::array<double, 2> m({3, 3}, 44.);
	std::array<int, 2> p = {1, 2};

	BOOST_REQUIRE( m[p[0]][p[1]] == m(1, 2) );
	BOOST_REQUIRE( &m(p[0], p[1]) == &m[p[0]][p[1]] );

#if 0
	using std::
#if not defined(__cpp_lib_apply) or not(__cpp_lib_apply>=201603)
		experimental::
#endif
		apply;
	BOOST_REQUIRE( &m[p[0]][p[1]] == &apply(m, p) );
#endif

	BOOST_REQUIRE( &m[p[0]][p[1]] == &m(p[0], p[1]) );
	BOOST_REQUIRE( &m(p[0], p[1]) == &m.apply(p) );
}

BOOST_AUTO_TEST_CASE(multi_tests_extension_with_tuple){
	std::tuple<int, int> t = {3, 3};
	multi::array<double, 2> m1(t, 44.);
	BOOST_REQUIRE( size(m1) == 3 );

	std::array<int, 3> a = {3, 3};
	multi::array<double, 2> m2(a, 55.);
}

#if 1
BOOST_AUTO_TEST_CASE(multi_test_constness_reference){
	multi::array<double, 2> const m({10, 10}, 99.);

	BOOST_REQUIRE( size( m(1, {0, 3}) ) == 3 );

	BOOST_REQUIRE( m(1, {0, 3})[1] == 99. );
	BOOST_REQUIRE( m({0, 3}, 1).dimensionality == 1 );
	BOOST_REQUIRE( size(m.sliced(0, 3)) == 3 );

	BOOST_REQUIRE( m.range({0, 3}).rotated()[1].unrotated().size() == 3 );

//	BOOST_TEST_REQUIRE( size(m.paren(multi::index_range{0, 3}, 1)) == 3 );
//range(a).rotated().paren(as...).unrotated()
	BOOST_REQUIRE( m({0, 3}, {0, 3})[1][1] == 99. );

	static_assert(not std::is_assignable<decltype(m(1, {0, 3})[1]), double>{}, "!");
// none of these lines should compile because m is read-only
//	m(1, {0, 3})[1] = 88.;
//	m({0, 3}, 1)[1] = 77.;
//	m({0, 3}, {0, 3})[1][1] = 66.;
}

BOOST_AUTO_TEST_CASE(multi_test_non_constness_reference){
	multi::array<double, 2> m({10, 10}, 99.);

	BOOST_REQUIRE( size( m(1, {0, 3}) ) == 3 );
	static_assert(std::is_assignable<decltype(m(1, {0, 3})[1]), double>{}, "!");

	BOOST_REQUIRE( m(1, {0, 3})[1] == 99. );
	BOOST_REQUIRE( m({0, 3}, 1).dimensionality == 1 );
	BOOST_REQUIRE( size(m.sliced(0, 3)) == 3 );

	BOOST_REQUIRE( size(m.range({0, 3}).rotated().paren(1L).unrotated()) == 3 );
	BOOST_REQUIRE( size(m(multi::index_range{0, 3}, 1)) == 3 );

//	BOOST_REQUIRE( size(m({0, 3}, 1)) == 3 );

	BOOST_REQUIRE( m({0, 3}, {0, 3})[1][1] == 99. );

	m(1, {0, 3})[1] = 88.;
	BOOST_REQUIRE( m(1, {0, 3})[1] == 88. );

//	m({0, 3}, 1)[1] = 77.;
//	BOOST_REQUIRE( m({0, 3}, 1)[1] == 77. );

//	m({0, 3}, {0, 3})[1][1] = 66.;
//	BOOST_REQUIRE( m({0, 3}, 1)[1] == 66. );
}

BOOST_AUTO_TEST_CASE(multi_test_stencil){

	multi::array<std::string, 2> A =
		{{"a", "b", "c", "d", "e"},
		 {"f", "g", "h", "f", "g"},
		 {"h", "i", "j", "k", "l"}}
	;

	BOOST_REQUIRE(      size(A) == 3                                           );
	BOOST_REQUIRE(           A.num_elements() == 3*5                           );
	BOOST_REQUIRE(           A[1][2] == "h"                                    );

	BOOST_REQUIRE(      size(A          ({1, 3}, {2, 5})) == 2                 );
	BOOST_REQUIRE( extension(A          ({1, 3}, {2, 5})).start() == 0         );
	BOOST_REQUIRE(           A          ({1, 3}, {2, 5}).num_elements() == 2*3 );
	BOOST_REQUIRE(           A          ({1, 3}, {2, 5}).num_elements() == 2*3 );
	BOOST_REQUIRE(           A          ({1, 3}, {2, 5})[0][0] == "h"          );
	BOOST_REQUIRE(          &A          ({1, 3}, {2, 5})[0][0] == &A[1][2]     );

	BOOST_REQUIRE(      size(A.stenciled({1, 3}, {2, 5})) == 2                 );
	BOOST_REQUIRE( extension(A.stenciled({1, 3}, {2, 5})).start() == 1         );
	BOOST_REQUIRE(           A.stenciled({1, 3}, {2, 5}).num_elements() == 2*3 );
	BOOST_REQUIRE(           A.stenciled({1, 3}, {2, 5}) [1][2] == "h"         );
	BOOST_REQUIRE(          &A.stenciled({1, 3}, {2, 5}) [1][2] == &A[1][2]    );

}

BOOST_AUTO_TEST_CASE(multi_extension_intersection){
	multi::array<double, 1> A = {{2., 2., 2.}};
	multi::array<double, 1> B = {{3., 3., 3., 3.}};

	BOOST_REQUIRE( intersection( extension(A), extension(B) ).size() == 3 );
	for(auto i : intersection( extension(A), extension(B) ) ){
		B[i] += A[i];
	}

	BOOST_REQUIRE( B[2] == 5. );
	BOOST_REQUIRE( B[3] == 3. );
}

BOOST_AUTO_TEST_CASE(multi_extensions_intersection){
	multi::array<double, 2> A = {{2., 2., 2.}, {2., 2., 2.}};
	multi::array<double, 2> B = {{3., 3., 3., 3.}, {3., 3., 3., 3.}, {3., 3., 3., 3.}} ;

	BOOST_REQUIRE( extensions(A).num_elements() ==  6 );
	BOOST_REQUIRE( extensions(B).num_elements() == 12 );

	auto is = intersection( extensions(A), extensions(B) );
	BOOST_REQUIRE( is.num_elements() == 6 );

	multi::array<double, 2> C(is);
	C(std::get<0>(is), std::get<1>(is)) = A;
	BOOST_REQUIRE( C == A );
}

BOOST_AUTO_TEST_CASE(multi_extensions_intersection_2){
	multi::array<double, 2> A({80, 20}, 4.);
	multi::array<double, 2> B({30, 70}, 8.);

	BOOST_REQUIRE( extensions(A).num_elements() == 80*20 );
	BOOST_REQUIRE( extensions(B).num_elements() == 30*70 );

	auto is = intersection( extensions(A), extensions(B) );
	BOOST_REQUIRE( is.num_elements() == 30*20 );

	multi::array<double, 2> C(is);
	C(std::get<0>(is), std::get<1>(is)) = A(std::get<0>(is), std::get<1>(is));
	BOOST_REQUIRE( C[16][17] == 4. );

	C(std::get<0>(is), std::get<1>(is)) = B(std::get<0>(is), std::get<1>(is));
	BOOST_REQUIRE( C[16][17] == 8. );
}

#endif


#include <iostream>
#include <assert.h>

#ifdef WITH_BULLET
    #include "Magnum/BulletIntegration/Integration.h"

    void with_bullet() {
        std::cout << "With Bullet\n";
        Magnum::Math::Vector3<btScalar> a{btScalar(1.0), btScalar(2.0), btScalar(3.0)};
        btVector3 b{btScalar(1.0), btScalar(2.0), btScalar(3.0)};
        assert(Magnum::Math::Vector3<btScalar>{b} == a);
    }
#endif

#ifdef WITH_EIGEN
    #include <Eigen/Geometry>
    #include "Magnum/EigenIntegration/Integration.h"

    void with_eigen() {
        std::cout << "With Eigen\n";
        Magnum::Math::Quaternion<float> q{};
        Eigen::Quaternion<float> eq{q.scalar(), q.vector().x(), q.vector().y(), q.vector().z()};
    }
#endif

#ifdef WITH_GLM
    #include "Magnum/Magnum.h"
    #include "Magnum/Math/Matrix3.h"
    #include "Magnum/Math/Matrix4.h"
    #include "Magnum/GlmIntegration/Integration.h"

    void with_glm() {
        std::cout << "With GLM\n";
        Magnum::Math::BoolVector<2> a{0x6};
        glm::bvec2 b{false, true};
        assert(glm::bvec2(a) == b);
    }
#endif

#ifdef WITH_IMGUI
    #include <Magnum/Magnum.h>
    #include <Magnum/Math/Color.h>
    #include "Magnum/ImGuiIntegration/Integration.h"

    void with_imgui() {
        std::cout << "With ImGui\n";
        ImVec2 imVec2{1.1f, 1.2f};
        Magnum::Vector2 vec2(1.1f, 1.2f);
        assert(Magnum::Vector2{imVec2} == vec2);
    }
#endif

int main() {
    #ifdef WITH_BULLET
        with_bullet();
    #endif

    #ifdef WITH_EIGEN
        with_eigen();
    #endif

    #ifdef WITH_GLM
        with_glm();
    #endif

    #ifdef WITH_IMGUI
        with_imgui();
    #endif

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/arithmetic/include/functions/scalar/rec.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/mzero.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL( rec_,  NT2_REAL_TYPES)
{
  using nt2::rec;
  using nt2::tag::rec_;

  // return type conformity test
  NT2_TEST_TYPE_IS( typename nt2::meta::call<rec_(T)>::type , T );

  // specific values tests
  NT2_TEST_ULP_EQUAL(rec(nt2::Inf<T>()), nt2::Zero<T>(), 0);
  NT2_TEST_ULP_EQUAL(rec(nt2::Minf<T>()), nt2::Zero<T>(), 0);
  NT2_TEST_ULP_EQUAL(rec(nt2::Mone<T>()), nt2::Mone<T>(), 0);
  NT2_TEST_ULP_EQUAL(rec(nt2::Mzero<T>()), nt2::Minf<T>(), 0);
  NT2_TEST_ULP_EQUAL(rec(nt2::Nan<T>()), nt2::Nan<T>(), 0);
  NT2_TEST_ULP_EQUAL(rec(nt2::One<T>()), nt2::One<T>(), 0);
  NT2_TEST_ULP_EQUAL(rec(nt2::Zero<T>()), nt2::Inf<T>(), 0);
} // end of test for floating_


//
//  Copyright Markus Rickert 2008
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/std.hpp>
#include <boost/numeric/bindings/ublas.hpp>

int
main(int argc, char** argv)
{
  // matrix
  boost::numeric::ublas::matrix<double> a(3, 4);
  for(std::size_t i = 0; i < a.size1(); ++i) for(std::size_t j = 0; j < a.size2(); ++j) a(i, j) = i * a.size2() + j;
  std::cout << "A=";
  std::cout << boost::numeric::bindings::stride1(a) << ",";
  std::cout << boost::numeric::bindings::stride2(a) << std::endl;
  std::cout << a << std::endl;

  // matrix matrix_range
  boost::numeric::ublas::matrix_range<
  boost::numeric::ublas::matrix<double>
  > a2 = boost::numeric::ublas::subrange(a, 1, 3, 0, 2);
  std::cout << "A2=";
  std::cout << boost::numeric::bindings::stride1(a2) << ",";
  std::cout << boost::numeric::bindings::stride2(a2) << std::endl;
  std::cout << a2 << std::endl;

  // matrix column_major
  boost::numeric::ublas::matrix<double, boost::numeric::ublas::column_major> b(3, 4);
  std::cout << "B=";
  std::cout << boost::numeric::bindings::stride1(b) << ",";
  std::cout << boost::numeric::bindings::stride2(b) << std::endl;

  // bounded_matrix
  boost::numeric::ublas::bounded_matrix<double, 3, 4> c;
  std::cout << "C=";
  std::cout << boost::numeric::bindings::stride1(c) << ",";
  std::cout << boost::numeric::bindings::stride2(c) << std::endl;

  // bounded_matrix matrix_range
  boost::numeric::ublas::matrix_range<
  boost::numeric::ublas::bounded_matrix<double, 3, 4>
  > c2 = boost::numeric::ublas::subrange(c, 0, 1, 0, 2);
  std::cout << "C2=";
  std::cout << boost::numeric::bindings::stride1(c2) << ",";
  std::cout << boost::numeric::bindings::stride2(c2) << std::endl;

  // bounded_matrix column_major
  boost::numeric::ublas::bounded_matrix<double, 3, 4, boost::numeric::ublas::column_major> d;
  std::cout << "D=";
  std::cout << boost::numeric::bindings::stride1(d) << ",";
  std::cout << boost::numeric::bindings::stride2(d) << std::endl;

  // vector
  boost::numeric::ublas::vector<double> e(3);
  std::cout << "e=";
  std::cout << boost::numeric::bindings::stride1(e) << ",";
  std::cout << boost::numeric::bindings::stride2(e) << std::endl;

  // vector vector_range
  boost::numeric::ublas::vector_range<
  boost::numeric::ublas::vector<double>
  > f = boost::numeric::ublas::subrange(e, 0, 2);
  std::cout << "f=";
  std::cout << boost::numeric::bindings::stride1(f) << ",";
  std::cout << boost::numeric::bindings::stride2(f) << std::endl;

  // bounded_vector
  boost::numeric::ublas::bounded_vector<double, 3> h;
  std::cout << "h=";
  std::cout << boost::numeric::bindings::stride1(h) << ",";
  std::cout << boost::numeric::bindings::stride2(h) << std::endl;

  // bounded_vector vector_range
  boost::numeric::ublas::vector_range<
  boost::numeric::ublas::bounded_vector<double, 3>
  > i = boost::numeric::ublas::subrange(h, 0, 2);
  std::cout << "i=";
  std::cout << boost::numeric::bindings::stride1(i) << ",";
  std::cout << boost::numeric::bindings::stride2(i) << std::endl;

  // std::vector
  std::vector<double> k(3);
  std::cout << "k=";
  std::cout << boost::numeric::bindings::stride1(k) << ",";
  std::cout << boost::numeric::bindings::stride2(k) << std::endl;

  // std::valarray
  std::valarray<double> l(3);
  std::cout << "l=";
  std::cout << boost::numeric::bindings::stride1(l) << ",";
  std::cout << boost::numeric::bindings::stride2(l) << std::endl;

  // array
  double m[3];
  std::cout << "m=";
  std::cout << boost::numeric::bindings::stride1(m) << ",";
  std::cout << boost::numeric::bindings::stride2(m) << std::endl;

  // matrix
  boost::numeric::ublas::matrix<double> n(3, 1);
  std::cout << "N=";
  std::cout << boost::numeric::bindings::stride1(n) << ",";
  std::cout << boost::numeric::bindings::stride2(n) << std::endl;
  std::cout << boost::numeric::bindings::stride_row(n) << ",";
  std::cout << boost::numeric::bindings::stride_column(n) << std::endl;

  return 0;
}


#include "catch.hpp"

#include "io.h"
#include "valve_graph.h"
#include <Eigen/Eigenvalues>

// Check mesh class
TEST_CASE("Valve Graph is checked", "[Valve Graph]") {
  // Tolerance
  const double tolerance = 1.e-12;

  // create junctions
  std::vector<std::string> junction_names{"1", "2", "3", "4"};
  std::vector<double> elevations{5, 4, 3, 2};
  std::vector<double> demands{1, 2, 3, 4};
  std::vector<double> leak_diameters{0, 0, 0, 0.2};

  std::vector<pipenetwork::JunctionProp> junc_props;
  for (int i = 0; i < elevations.size(); ++i) {
    pipenetwork::JunctionProp junc_prop;
    junc_prop.name = junction_names[i];
    junc_prop.elevation = elevations[i];
    junc_prop.demand = demands[i];
    junc_prop.leak_diameter = leak_diameters[i];
    junc_props.emplace_back(junc_prop);
  }

  std::vector<pipenetwork::ReservoirProp> res_props;
  std::vector<std::string> pipe_names{"p1", "p2", "p3", "p4"};

  std::vector<std::pair<std::string, std::string>> node_names{
      std::make_pair("1", "2"), std::make_pair("2", "4"),
      std::make_pair("2", "3"), std::make_pair("3", "4")};
  const std::vector<double> length{100, 200, 300, 400};
  const std::vector<double> diameter{3, 4, 5, 6};
  const std::vector<double> roughness{0.2, .6, .9, 2};

  std::vector<pipenetwork::PipeProp> pipe_props;
  for (int i = 0; i < pipe_names.size(); ++i) {
    pipenetwork::PipeProp pipe_prop;
    pipe_prop.name = pipe_names[i];
    pipe_prop.length = length[i];
    pipe_prop.diameter = diameter[i];
    pipe_prop.roughness = roughness[i];
    pipe_prop.node1_name = node_names[i].first;
    pipe_prop.node2_name = node_names[i].second;
    pipe_props.emplace_back(pipe_prop);
  }

  std::vector<pipenetwork::PumpProp> pump_props;
  std::vector<pipenetwork::ValveProp> valve_props;

  std::vector<pipenetwork::isolation::ISOVProp> iso_valve_props;
  pipenetwork::isolation::ISOVProp v1_prop;
  v1_prop.on_node = "1";
  v1_prop.on_pipe = "p1";
  v1_prop.name = "v1";
  iso_valve_props.emplace_back(v1_prop);

  pipenetwork::isolation::ISOVProp v2_prop;
  v2_prop.on_node = "2";
  v2_prop.on_pipe = "p1";
  v2_prop.name = "v2";
  iso_valve_props.emplace_back(v2_prop);
  // assume broken

  pipenetwork::isolation::ISOVProp v3_prop;
  v3_prop.on_node = "2";
  v3_prop.on_pipe = "p3";
  v3_prop.name = "v3";
  iso_valve_props.emplace_back(v3_prop);

  pipenetwork::isolation::ISOVProp v4_prop;
  v4_prop.on_node = "3";
  v4_prop.on_pipe = "p3";
  v4_prop.name = "v4";
  iso_valve_props.emplace_back(v4_prop);

  pipenetwork::isolation::ISOVProp v5_prop;
  v5_prop.on_node = "4";
  v5_prop.on_pipe = "p4";
  v5_prop.name = "v5";
  iso_valve_props.emplace_back(v5_prop);

  auto mesh_nodes =
      std::make_shared<pipenetwork::MeshNodes>(junc_props, res_props);
  auto mesh_links = std::make_shared<pipenetwork::MeshLinks>(
      pipe_props, pump_props, valve_props, *mesh_nodes);
  auto valve_graph =
      pipenetwork::ValveGraph(mesh_nodes, mesh_links, iso_valve_props);

  SECTION("CHECK The NODE PIPE MATRIX") {
    auto node_pipe_matrix = valve_graph.node_pipe_mtx();
    REQUIRE(node_pipe_matrix.coeff(0, 0) == 1);
    REQUIRE(node_pipe_matrix.coeff(1, 0) == 1);
    REQUIRE(node_pipe_matrix.coeff(1, 1) == 1);
    REQUIRE(node_pipe_matrix.coeff(3, 1) == 1);
    REQUIRE(node_pipe_matrix.coeff(1, 2) == 1);
    REQUIRE(node_pipe_matrix.coeff(2, 2) == 1);
    REQUIRE(node_pipe_matrix.coeff(5, 2) == 0);
  }
  SECTION("CHECK The Valve Location Matrix") {
    auto valve_loc_matrix = valve_graph.valve_loc_mtx();
    //    std::cout << valve_loc_matrix << std::endl;
    REQUIRE(valve_loc_matrix.coeff(0, 0) == 1);
    REQUIRE(valve_loc_matrix.coeff(1, 0) == 1);
    REQUIRE(valve_loc_matrix.coeff(1, 1) == 0);
    REQUIRE(valve_loc_matrix.coeff(3, 1) == 0);
    REQUIRE(valve_loc_matrix.coeff(1, 2) == 1);
    REQUIRE(valve_loc_matrix.coeff(2, 2) == 1);
    REQUIRE(valve_loc_matrix.coeff(3, 2) == 0);
    REQUIRE(valve_loc_matrix.coeff(3, 3) == 1);
  }
  SECTION("CHECK isolation segment search algorithm") {
    auto& initial_segments = valve_graph.segments();

    auto nsegs = initial_segments.nsegs();
    REQUIRE(nsegs == 4);

    pipenetwork::Index pid = 0;
    auto seg0 = initial_segments.pid2seg(pid);

    REQUIRE(seg0.pids.size() == 1);
    REQUIRE(seg0.nids.size() == 0);
    REQUIRE(seg0.vids.size() == 2);

    auto seg3 = initial_segments.pid2seg(3);
    REQUIRE(seg3.pids.size() == 1);
    REQUIRE(seg3.nids.size() == 1);
    REQUIRE(seg3.vids.size() == 2);
  }
  SECTION("Check the Segment Valves Matrix") {
    auto& initial_segments = valve_graph.segments();
    auto seg_valve_mtx = initial_segments.seg_valve_mtx();
    std::cout << seg_valve_mtx << std::endl;
    REQUIRE(seg_valve_mtx.innerSize() == 4);
    REQUIRE(seg_valve_mtx.outerSize() == 5);

    REQUIRE(seg_valve_mtx.coeff(0, 0) == 1);
    REQUIRE(seg_valve_mtx.coeff(0, 1) == 1);
    REQUIRE(seg_valve_mtx.coeff(0, 2) == 0);
    REQUIRE(seg_valve_mtx.coeff(1, 0) == 0);
    REQUIRE(seg_valve_mtx.coeff(1, 1) == 1);
    REQUIRE(seg_valve_mtx.coeff(1, 2) == 1);
    REQUIRE(seg_valve_mtx.coeff(1, 4) == 1);
    REQUIRE(seg_valve_mtx.coeff(2, 0) == 0);
    REQUIRE(seg_valve_mtx.coeff(2, 2) == 1);
    REQUIRE(seg_valve_mtx.coeff(2, 3) == 1);
    REQUIRE(seg_valve_mtx.coeff(3, 1) == 0);
    REQUIRE(seg_valve_mtx.coeff(3, 3) == 1);
    REQUIRE(seg_valve_mtx.coeff(3, 4) == 1);

    SECTION("Check the shrink matrix tool") {
      std::vector<pipenetwork::Index> rows_to_remove{0, 2};
      std::vector<pipenetwork::Index> cols_to_remove{1, 3};
      auto shrinked_mtx = pipenetwork::isolation::IsoSegHelper::shrink_mtx(
          seg_valve_mtx, rows_to_remove, cols_to_remove);
      REQUIRE(shrinked_mtx.innerSize() == 2);
      REQUIRE(shrinked_mtx.outerSize() == 3);
      REQUIRE(shrinked_mtx.coeff(0, 0) == 0);
      REQUIRE(shrinked_mtx.coeff(0, 1) == 1);
      REQUIRE(shrinked_mtx.coeff(0, 2) == 1);
      REQUIRE(shrinked_mtx.coeff(1, 0) == 0);
      REQUIRE(shrinked_mtx.coeff(1, 1) == 0);
      REQUIRE(shrinked_mtx.coeff(1, 2) == 1);
    }
  }
  SECTION("Check the Segment Valves ADJ Matrix") {
    auto& initial_segments = valve_graph.segments();
    auto seg_valve_adj_mtx = initial_segments.seg_valve_adj_mtx();
    //      std::cout << seg_valve_adj_mtx << std::endl;
    REQUIRE(seg_valve_adj_mtx.coeff(0, 0) == 0);
    REQUIRE(seg_valve_adj_mtx.coeff(0, 1) == 1);
    REQUIRE(seg_valve_adj_mtx.coeff(1, 0) == 1);
    REQUIRE(seg_valve_adj_mtx.coeff(1, 1) == 0);
  }
  SECTION("Check components finding algorithm") {
    auto& segments = valve_graph.segments();
    std::set<pipenetwork::Index> iso_sid{1};
    auto components = segments.get_segment_components(iso_sid);
    REQUIRE(components.size() == 3);
    REQUIRE(components[0].size() == 1);
    REQUIRE(components[1].size() == 1);
    REQUIRE(components[2].size() == 2);
  }
  SECTION("Check merge segments by remove valves") {
    auto& segments = valve_graph.segments();
    segments.merge_segments(1);
    auto seg_valve_mtx = segments.seg_valve_mtx();
    std::cout << seg_valve_mtx << std::endl;
    REQUIRE(seg_valve_mtx.innerSize() == 3);
    REQUIRE(seg_valve_mtx.coeff(0, 0) == 1);
    REQUIRE(seg_valve_mtx.coeff(0, 1) == 1);
    REQUIRE(seg_valve_mtx.coeff(0, 2) == 1);
    REQUIRE(seg_valve_mtx.coeff(0, 4) == 1);
    REQUIRE(seg_valve_mtx.coeff(1, 0) == 0);
    REQUIRE(seg_valve_mtx.coeff(1, 1) == 0);
    REQUIRE(seg_valve_mtx.coeff(1, 2) == 1);
    REQUIRE(seg_valve_mtx.coeff(1, 4) == 0);
    REQUIRE(seg_valve_mtx.coeff(2, 0) == 0);
    REQUIRE(seg_valve_mtx.coeff(2, 1) == 0);
    REQUIRE(seg_valve_mtx.coeff(2, 2) == 0);
    REQUIRE(seg_valve_mtx.coeff(2, 4) == 1);

    auto segment = segments.pid2seg(1);
    REQUIRE(segment.sid == 0);
    REQUIRE(segment.pids.size() == 2);

    SECTION("Check components finding algorithm after merging") {
      std::set<pipenetwork::Index> iso_sid{2};
      auto components = segments.get_segment_components(iso_sid);
      REQUIRE(components.size() == 2);
      REQUIRE(components[0].size() == 2);
      REQUIRE(components[1].size() == 1);
    }
    SECTION("Continous failure case") {
      segments.merge_segments(4);
      auto seg_valve_mtx = segments.seg_valve_mtx();
      std::cout << seg_valve_mtx << std::endl;
      REQUIRE(seg_valve_mtx.innerSize() == 2);

      auto seg_valve_adj_mtx = segments.seg_valve_adj_mtx();
      std::cout << seg_valve_adj_mtx << std::endl;

      auto components = segments.get_segment_components({1});
      REQUIRE(components.size() == 2);
      REQUIRE(components[0].size() == 1);
      REQUIRE(components[1].size() == 1);
    }
  }

  SECTION("Check wrappers in the valve graph class") {
    valve_graph.initialize_iso_segs();  // reinitialize
    REQUIRE(valve_graph.nsegs() == 4);

    auto seg0 = valve_graph.pname2segment("p1");
    REQUIRE(seg0.pids.size() == 1);
    REQUIRE(seg0.nids.size() == 0);
    REQUIRE(seg0.vids.size() == 2);

    auto seg_components_init = valve_graph.segment_components({"p1", "p2"});
    REQUIRE(seg_components_init.size() == 3);

    valve_graph.fail_valves({"v2", "v5"});
    REQUIRE(valve_graph.nsegs() == 2);

    auto seg_components_after = valve_graph.segment_components({"p1", "p2"});
    REQUIRE(seg_components_after.size() == 2);

    auto seg3 = valve_graph.pname2segment("p3");
    REQUIRE(seg3.pids.size() == 1);
    REQUIRE(seg3.nids.size() == 0);
    REQUIRE(seg3.vids.size() == 2);

    auto seg_union = valve_graph.pname2segment("p4");
    REQUIRE(seg_union.pids.size() == 3);
    REQUIRE(seg_union.nids.size() == 3);
    REQUIRE(seg_union.vids.size() == 3);
  }
}


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      100910    J. Melman         First creation of code.
 *      110111    J. Melman         Adapted to the offical Tudat standards.
 *      110124    J. Melman         Further adapted to the offical Tudat standards.
 *      110201    J. Melman         Made the tests for obliquity and astronomical unit more
 *                                  accurate.
 *      120127    D. Dirkx          Moved to Tudat core.
 *      120127    K. Kumar          Transferred unit tests over to Boost unit test framework.
 *      120128    K. Kumar          Changed BOOST_CHECK to BOOST_CHECK_CLOSE_FRACTION for unit test
 *                                  comparisons.
 *      130111    D. Dirkx          Added unit tests for Planck, Boltzmann, and Stefan-Boltzmann
 *                                  constants.
 *
 *    References
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_physical_constants )

//! Test if the physical constants have the correct relations (ratios/offsets).
BOOST_AUTO_TEST_CASE( testRelationsBetweenPhysicalConstant )
{
    using namespace physical_constants;

    // Test for the number of seconds in a year.
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_YEAR, JULIAN_DAY * JULIAN_YEAR_IN_DAYS,
                                std::numeric_limits< double >::epsilon( ) );

    // Test for the number of seconds in a year.
    BOOST_CHECK_CLOSE_FRACTION( SIDEREAL_YEAR, JULIAN_DAY * SIDEREAL_YEAR_IN_DAYS,
                                std::numeric_limits< double >::epsilon( ) );

    // Test pre-computed powers of speed of light.
    BOOST_CHECK_CLOSE_FRACTION( std::pow( SPEED_OF_LIGHT, -2.0 ), INVERSE_SQUARE_SPEED_OF_LIGHT,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( std::pow( SPEED_OF_LIGHT, -3.0 ), INVERSE_CUBIC_SPEED_OF_LIGHT,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( std::pow( SPEED_OF_LIGHT, -4.0 ), INVERSE_QUARTIC_SPEED_OF_LIGHT,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( std::pow( SPEED_OF_LIGHT, -5.0 ), INVERSE_QUINTIC_SPEED_OF_LIGHT,
                                std::numeric_limits< double >::epsilon( ) );
}

//! Test if physical constants have the expected value.
BOOST_AUTO_TEST_CASE( testOtherConstants )
{
    using namespace physical_constants;

    // Test for gravitational constant.
    BOOST_CHECK_CLOSE_FRACTION( GRAVITATIONAL_CONSTANT, 6.67259e-11,
                                std::numeric_limits< double >::epsilon( ) );

    // Test for speed of light.
    BOOST_CHECK_CLOSE_FRACTION( SPEED_OF_LIGHT, 299792458.0,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( SPEED_OF_LIGHT_LONG, 299792458.0L,
                                std::numeric_limits< long double >::epsilon( ) );

    // Test for astronomical unit.
    BOOST_CHECK_CLOSE_FRACTION( ASTRONOMICAL_UNIT, 1.49597870691e11,
                                std::numeric_limits< double >::epsilon( ) );

    // Test for Planck constant.
    BOOST_CHECK_CLOSE_FRACTION( PLANCK_CONSTANT, 6.62606957E-34,
                                std::numeric_limits< double >::epsilon( ) );

    // Test for Boltzmann constant.
    BOOST_CHECK_CLOSE_FRACTION( BOLTZMANN_CONSTANT, 1.3806488E-23,
                                std::numeric_limits< double >::epsilon( ) );

    // Test permittivity/permeability fo vacuum
    BOOST_CHECK_CLOSE_FRACTION( VACUUM_PERMEABILITY, 4.0 * mathematical_constants::PI * 1.0E-7,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( VACUUM_PERMITTIVITY, 1.0 /
                                ( 4.0 * mathematical_constants::PI * 1.0E-7 * std::pow( SPEED_OF_LIGHT, 2.0 ) ),
                                  std::numeric_limits< double >::epsilon( ) );

            // Test for Stefan-Boltzmann constant relation (derived from Planck and Boltzmann constants).
            BOOST_CHECK_CLOSE_FRACTION( STEFAN_BOLTZMANN_CONSTANT, 2.0 * std::pow(
                                            mathematical_constants::PI, 5.0 ) *
                                        std::pow( BOLTZMANN_CONSTANT, 4.0 ) /
                                        ( 15.0 * SPEED_OF_LIGHT * SPEED_OF_LIGHT *
                                          PLANCK_CONSTANT * PLANCK_CONSTANT * PLANCK_CONSTANT ),
                                        std::numeric_limits< double >::epsilon( ) );

    // Test for Stefan-boltzmann constant value (NIST, 2013)
    BOOST_CHECK_CLOSE_FRACTION( STEFAN_BOLTZMANN_CONSTANT, 5.670373E-8, 1.0E-7 );

    // Test time scale rate difference factors
    BOOST_CHECK_CLOSE_FRACTION( LG_TIME_RATE_TERM, 6.969290134E-10, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( LB_TIME_RATE_TERM, 1.550519768E-8, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( LG_TIME_RATE_TERM_LONG, 6.969290134E-10L, std::numeric_limits< long double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( LB_TIME_RATE_TERM_LONG, 1.550519768E-8L, std::numeric_limits< long  double >::epsilon( ) );

}

//! Check if the time constants have the expected values.
BOOST_AUTO_TEST_CASE( testTimeConstants )
{
    using namespace physical_constants;

    // Test for the number of Julian days in a year.
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_YEAR_IN_DAYS, 365.25,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_YEAR_IN_DAYS_LONG, 365.25L,
                                std::numeric_limits< long double >::epsilon( ) );

    // Test for the number of sidereal days in a year.
    BOOST_CHECK_CLOSE_FRACTION( SIDEREAL_YEAR_IN_DAYS, 365.25636,
                                std::numeric_limits< double >::epsilon( ) );

    // Test for the Julian day length.
    BOOST_CHECK_CLOSE_FRACTION( SIDEREAL_DAY, 86164.09054,
                                std::numeric_limits< double >::epsilon( ) );

    // Test for the sidereal day length.
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_DAY, 86400.0,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_DAY_LONG, 86400.0L,
                                std::numeric_limits< long double >::epsilon( ) );
}

//! Test templated get physical constant functions
BOOST_AUTO_TEST_CASE( testTemplatedConstantFunctions )
{
    using namespace physical_constants;

    // Test for the number of Julian days in a year.
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_YEAR_IN_DAYS, getJulianYearInDays< double >( ),
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_YEAR_IN_DAYS_LONG, getJulianYearInDays< long double >( ),
                                std::numeric_limits< long double >::epsilon( ) );

    // Test for the sidereal day length.
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_DAY, getJulianDay< double >( ),
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_DAY_LONG, getJulianDay< long double >( ),
                                std::numeric_limits< long double >::epsilon( ) );

    // Test for speed of light.
    BOOST_CHECK_CLOSE_FRACTION( SPEED_OF_LIGHT, getSpeedOfLight< double >( ),
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( SPEED_OF_LIGHT_LONG, getSpeedOfLight< long double >( ),
                                std::numeric_limits< long double >::epsilon( ) );

    // Test time scale rate difference factors
    BOOST_CHECK_CLOSE_FRACTION( LG_TIME_RATE_TERM, getLgTimeRateTerm< double >( ),
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( LB_TIME_RATE_TERM, getLbTimeRateTerm< double >( ),
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( LG_TIME_RATE_TERM_LONG, getLgTimeRateTerm< long double >( ),
                                std::numeric_limits< long double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( LB_TIME_RATE_TERM_LONG, getLbTimeRateTerm< long double >( ),
                                std::numeric_limits< long  double >::epsilon( ) );

}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/*
 * MIT License
 *
 * Copyright (c) 2020 Robert Grupp
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all
 * copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include "xregOpenCLSpatial.h"

#include <boost/compute/utility/source.hpp>

const char* xreg::kOPENCL_SPATIAL_SRC = BOOST_COMPUTE_STRINGIZE_SOURCE(

float2 xregLineSegmentRectIntersect(const float3 min_rect_corner, const float3 max_rect_corner,
                                    const float3 line_start_pt, const float3 line_vec)
{
  float2 t = (float2) (0.0f, 1.0f);

  const float3 line_vec_inv = 1.0f / line_vec;

  const float3 t0 = (min_rect_corner - line_start_pt) * line_vec_inv;
  const float3 t1 = (max_rect_corner - line_start_pt) * line_vec_inv;

  t.x = fmax(t.x, fmax(fmax(fmin(t0.x,t1.x), fmin(t0.y,t1.y)), fmin(t0.z,t1.z)));

  t.y = fmin(t.y, fmin(fmin(fmax(t0.x,t1.x), fmax(t0.y,t1.y)), fmax(t0.z,t1.z)));

  t = (t.x <= t.y) ? t : (float2)(0.0f,0.0f);

  // checks for the case of a line being parallel to volume when at least one line/vector component is zero
  const int3 can_intersect_parallel = (int3) ( (fabs(line_vec.x) > 1.0e-8) ? 1 : (((line_start_pt.x >= min_rect_corner.x) && (line_start_pt.x <= max_rect_corner.x)) ? 1 : 0),
                                               (fabs(line_vec.y) > 1.0e-8) ? 1 : (((line_start_pt.y >= min_rect_corner.y) && (line_start_pt.y <= max_rect_corner.y)) ? 1 : 0),
                                               (fabs(line_vec.z) > 1.0e-8) ? 1 : (((line_start_pt.z >= min_rect_corner.z) && (line_start_pt.z <= max_rect_corner.z)) ? 1 : 0)
                                             );

  t = (can_intersect_parallel.x && can_intersect_parallel.y && can_intersect_parallel.z) ? t : (float2)(0.0f,0.0f);

  return t;
}

float2 xregRayRectIntersect(const float3 min_rect_corner, const float3 max_rect_corner,
                            const float3 line_start_pt, const float3 line_vec)
{
  float2 t = (float2) (0.0f, INFINITY);

  const float3 line_vec_inv = 1.0f / line_vec;

  const float3 t0 = (min_rect_corner - line_start_pt) * line_vec_inv;
  const float3 t1 = (max_rect_corner - line_start_pt) * line_vec_inv;

  t.x = fmax(t.x, fmax(fmax(fmin(t0.x,t1.x), fmin(t0.y,t1.y)), fmin(t0.z,t1.z)));

  t.y = fmin(t.y, fmin(fmin(fmax(t0.x,t1.x), fmax(t0.y,t1.y)), fmax(t0.z,t1.z)));

  t = (t.x <= t.y) ? t : (float2)(0.0f,0.0f);

  // checks for the case of a line being parallel to volume when at least one line/vector component is zero
  const int3 can_intersect_parallel = (int3) ( (fabs(line_vec.x) > 1.0e-8) ? 1 : (((line_start_pt.x >= min_rect_corner.x) && (line_start_pt.x <= max_rect_corner.x)) ? 1 : 0),
                                               (fabs(line_vec.y) > 1.0e-8) ? 1 : (((line_start_pt.y >= min_rect_corner.y) && (line_start_pt.y <= max_rect_corner.y)) ? 1 : 0),
                                               (fabs(line_vec.z) > 1.0e-8) ? 1 : (((line_start_pt.z >= min_rect_corner.z) && (line_start_pt.z <= max_rect_corner.z)) ? 1 : 0)
                                             );

  t = (can_intersect_parallel.x && can_intersect_parallel.y && can_intersect_parallel.z) ? t : (float2)(0.0f,0.0f);

  return t;
}

);


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
/*!
 * \file
**/
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_EPS_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_EPS_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

/*!
 * \ingroup boost_simd_ieee
 * \defgroup boost_simd_ieee_eps eps
 *
 * \par Description
 * Returns 2 times the Machine epsilon
 * \par
 * The Machine epsilon gives an upper bound on the relative
 * error due to rounding in floating point arithmetic. Typically $2^{-53}$
 * for floating point doubles and $2^{-24}$ for
 * floats. MATLAB(TM) language defines the value eps
 * as the distance from 1.0 to the next larger double
 * which is two times the machine epsilon...
 * Returns one for integer types.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/eps.hpp>
 * \endcode
 *
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class A0>
 *     meta::call<tag::eps_(A0)>::type
 *     eps(const A0 & a0);
 * }
 * \endcode
 *
 * \param a0 the unique parameter of eps
 *
 * \return a value of the same type as the parameter
 *
 * \par Notes
 * In SIMD mode, this function acts elementwise on the inputs vectors elements
 * \par
 *
**/

namespace boost { namespace simd { namespace tag
  {
    /*!
     * \brief Define the tag eps_ of functor eps
     *        in namespace boost::simd::tag for toolbox boost.simd.ieee
    **/
    struct eps_ : ext::elementwise_<eps_> { typedef ext::elementwise_<eps_> parent; };
  }
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::eps_, eps, 1)
} }

#endif

// modified by jt the 25/12/2010


/*
 * Copyright 2017 Will Mitchell
 *
 * 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 <algorithm>
using std::remove_if;
#include <cctype>
using std::isalpha;
#include <cmath>
using std::sqrt;
#include <iostream>
using std::cout;
using std::endl;
#include <fstream>
using std::getline;
using std::ifstream;
#include <random>
using std::mt19937;
using std::uniform_int_distribution;
#include <string>
using std::stoul;
using std::string;
#include <utility>
using std::pair;
#include <vector>
using std::vector;

#include <boost/algorithm/string.hpp>

template <typename T>
string randomstring(string alphabet, unsigned length,
                    uniform_int_distribution<string::size_type> &dist,
                    T &rand_source);

template <typename T>
string randomstring(string alphabet, unsigned length,
                    uniform_int_distribution<string::size_type> &dist,
                    T &rand_source) {
  string ret;

  for (unsigned i = 0; i < length; ++i)
    ret += alphabet[dist(rand_source)];

  return ret;
}

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

  //string alphabet = "1234567890";
  string alphabet = "abcdefghijklmnopqrstuvwxyz";
  unsigned long count = 10000;
  unsigned length = 9;

  // Parse command line
  if (argc > 1) {
    try {
      count = stoul(argv[1]);
    }
    catch (std::invalid_argument e) {
      cout << "Invalid number of samples '" << argv[1] << "'" << endl;
    }
  }
  if (argc > 2) {
    try {
      length = static_cast<unsigned>(stoul(argv[2]));
    }
    catch (std::invalid_argument e) {
      cout << "Invalid sample length of '" << argv[2] << "'" << endl;
    }
  }
  if (argc > 3)
    alphabet = argv[3];

  // Set up RNG
  // Use a set seed
  mt19937 mt(12345);
  // Use a good seed, if possible
  // std::random_device seed;
  // mt19937 mt(seed);

  uniform_int_distribution<string::size_type> uniform_dist(0,
                                                           alphabet.size() - 1);

  // cout << "Alphabet: " << alphabet << "\n"
  //      << "Length: " << length << "\n"
  //      << "Number of trials: " << count << endl;

  for (unsigned long i = 0; i < count; ++i) {
    string rand = randomstring(alphabet, length, uniform_dist, mt);
    cout << rand << "\n";
  }

  return 0;
}


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [range_t]
#include <boost/simd/range/range.hpp>
#include <boost/simd/literal.hpp>
#include <iostream>

int main()
{
  using namespace boost::simd::literal;

  std::int32_t data[16] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16};

  boost::simd::range_t<std::int32_t*,8> r = boost::simd::range(&data[0], &data[0]+16, 8_c);

  std::cout << *r.begin() << "\n";
  std::cout << *(r.begin()+1) << "\n";

  return 0;
}
//! [range_t]


// Copyright (c) 2018 by University Paris-Est Marne-la-Vallee
// GeometricExplicit.hpp
// This file is part of the Garamon for project_namespace.
// Authors: Stephane Breuils and Vincent Nozick
// Conctat: vincent.nozick@u-pem.fr
//
// Licence MIT
// A a copy of the MIT License is given along with this program

/// \file GeometricExplicit.hpp
/// \author Stephane Breuils, Vincent Nozick
/// \brief Explicit precomputed per grades geometric products of project_namespace.

#ifndef project_inclusion_guard
#define project_inclusion_guard
#pragma once

#include <Eigen/Core>

#include "project_namespace/Mvec.hpp"
#include "project_namespace/Constants.hpp"

/*!
 * @namespace project_namespace
 */
namespace project_namespace {
    template<typename T> class Mvec;

    project_explicit_geometric_functions
    project_explicit_geometric_pointer_functions

}/// End of Namespace

#endif // project_inclusion_guard


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/decs.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/mtwo.hpp>

STF_CASE_TPL (" DECS SIGNED_INT",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::decs;

  using r_t = decltype(decs(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(decs(bs::Mone<T>()), bs::Mtwo<T>());
  STF_EQUAL(decs(bs::One<T>()), bs::Zero<T>());
  STF_EQUAL(decs(bs::Valmin<T>()), bs::Valmin<T>());
  STF_EQUAL(decs(bs::Zero<T>()), bs::Mone<T>());
}

STF_CASE_TPL (" decs unsigned_uint",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::decs;

  using r_t = decltype(decs(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(decs(bs::One<T>()), bs::Zero<T>());
  STF_EQUAL(decs(bs::Two<T>()), bs::One<T>());
  STF_EQUAL(decs(bs::Zero<T>()), bs::Zero<T>());
}

STF_CASE_TPL(" decs floating", STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::decs;

  using r_t = decltype(decs(T()));

  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(decs(bs::Inf<T>()), bs::Inf<T>());
  STF_IEEE_EQUAL(decs(bs::Nan<T>()), bs::Nan<T>());
  STF_EQUAL(decs(bs::Minf<T>()), bs::Minf<T>());
#endif
  STF_EQUAL(decs(bs::One<T>()), bs::Zero<T>());
  STF_EQUAL(decs(bs::Two<T>()), bs::One<T>());
  STF_EQUAL(decs(bs::Zero<T>()), bs::Mone<T>());
}


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_FMAX_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FMAX_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing fmax capabilities

    Returns the maximum value, ignoring nans.

    This is a convenient alias of @ref maxnum
  **/
  const boost::dispatch::functor<tag::fmax_> fmax = {};
} }
#endif

#include <boost/simd/function/scalar/maxnum.hpp>
#include <boost/simd/function/simd/fmax.hpp>

#endif


//
// Copyright (c) 2015-2018 CNRS
//

/*
 * Test the CRBA algorithm. The code validates both the computation times and
 * the value by comparing the results of the CRBA with the reconstruction of
 * the mass matrix using the RNEA.
 * For a strong timing benchmark, see benchmark/timings.
 *
 */

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/utils/timer.hpp"

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

template<typename JointModel>
static void addJointAndBody(pinocchio::Model & model,
                            const pinocchio::JointModelBase<JointModel> & joint,
                            const std::string & parent_name,
                            const std::string & name,
                            const pinocchio::SE3 placement = pinocchio::SE3::Random(),
                            bool setRandomLimits = true)
{
  using namespace pinocchio;
  typedef typename JointModel::ConfigVector_t CV;
  typedef typename JointModel::TangentVector_t TV;

  Model::JointIndex idx;

  if (setRandomLimits)
    idx = model.addJoint(model.getJointId(parent_name),joint,
                         SE3::Random(),
                         name + "_joint",
                         TV::Random() + TV::Constant(1),
                         TV::Random() + TV::Constant(1),
                         CV::Random() - CV::Constant(1),
                         CV::Random() + CV::Constant(1)
                         );
    else
      idx = model.addJoint(model.getJointId(parent_name),joint,
                           placement, name + "_joint");

      model.addJointFrame(idx);

      model.appendBodyToJoint(idx,Inertia::Random(),SE3::Identity());
      model.addBodyFrame(name + "_body", idx);
      }

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_crba )
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model);

  #ifdef NDEBUG
    #ifdef _INTENSE_TESTING_
      const size_t NBT = 1000*1000;
    #else
      const size_t NBT = 10;
    #endif

    Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nq);

    PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
    SMOOTH(NBT)
      {
        crba(model,data,q);
      }
    timer.toc(std::cout,NBT);

  #else
    const size_t NBT = 1;
    using namespace Eigen;
    using namespace pinocchio;

    Eigen::MatrixXd M(model.nv,model.nv);
    Eigen::VectorXd q = Eigen::VectorXd::Ones(model.nq);
    q.segment <4> (3).normalize();
    Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
    Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
    data.M.fill(0);  crba(model,data,q);
    data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();

    /* Joint inertia from iterative crba. */
    const Eigen::VectorXd bias = rnea(model,data,q,v,a);
    for(int i=0;i<model.nv;++i)
    {
      M.col(i) = rnea(model,data,q,v,Eigen::VectorXd::Unit(model.nv,i)) - bias;
    }

    // std::cout << "Mcrb = [ " << data.M << "  ];" << std::endl;
    // std::cout << "Mrne = [  " << M << " ]; " << std::endl;
    BOOST_CHECK(M.isApprox(data.M,1e-12));

    std::cout << "(the time score in debug mode is not relevant)  " ;

    q = Eigen::VectorXd::Zero(model.nq);

    PinocchioTicToc timer(PinocchioTicToc::US); timer.tic();
    SMOOTH(NBT)
    {
      crba(model,data,q);
    }
    timer.toc(std::cout,NBT);

  #endif // ifndef NDEBUG

}

BOOST_AUTO_TEST_CASE(test_minimal_crba)
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model), data_ref(model);

  model.lowerPositionLimit.head<7>().fill(-1.);
  model.upperPositionLimit.head<7>().fill(1.);

  Eigen::VectorXd q = randomConfiguration(model,model.lowerPositionLimit,model.upperPositionLimit);
  Eigen::VectorXd v(Eigen::VectorXd::Random(model.nv));

  crba(model,data_ref,q);
  data_ref.M.triangularView<Eigen::StrictlyLower>() = data_ref.M.transpose().triangularView<Eigen::StrictlyLower>();

  crbaMinimal(model,data,q);
  data.M.triangularView<Eigen::StrictlyLower>() = data.M.transpose().triangularView<Eigen::StrictlyLower>();

  BOOST_CHECK(data.M.isApprox(data_ref.M));

  ccrba(model,data_ref,q,v);
  computeJointJacobians(model,data_ref,q);
  BOOST_CHECK(data.Ag.isApprox(data_ref.Ag));
  BOOST_CHECK(data.J.isApprox(data_ref.J));

}

BOOST_AUTO_TEST_SUITE_END ()


// Copyright Robert Eisele 2017
#include <iostream>
#include <fstream>
#include <limits>
#include <string>
#include <Eigen/Dense>
#include "readCSV_lastline.h"

std::istream& ignoreline(std::ifstream& in, std::ifstream::pos_type& pos)
{
  pos = in.tellg();
  return in.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
}

std::string getLastLine(std::ifstream& in)
{
  std::ifstream::pos_type pos = in.tellg();

  std::ifstream::pos_type lastPos;
  while (in >> std::ws && ignoreline(in, lastPos))
    pos = lastPos;

  in.clear();
  in.seekg(pos);

  std::string line;
  std::getline(in, line);
  return line;
}

Eigen::VectorXd readCSV_lastline(std::string file, int cols) {

  std::ifstream in(file);

  std::string line;

  int col = 0;

  Eigen::VectorXd res = Eigen::VectorXd(cols);

  if (in.is_open()) {

    line = getLastLine(in);
    char *ptr = (char *) line.c_str();
    int len = line.length();

    char *start = ptr;
    for (int i = 0; i < len; i++) {

      if (ptr[i] == ',') {
        res(col++) = atof(start);
        start = ptr + i + 1;
      }
    }
    res(col) = atof(start);

    in.close();
  }
  return res;
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/range.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
using namespace boost::hana;

int main() {
    BOOST_HANA_CONSTANT_ASSERT( equal(range(int_<0>, int_<0>), range(int_<0>, int_<0>)));
    BOOST_HANA_CONSTANT_ASSERT(!equal(range(int_<0>, int_<0>), range(int_<0>, int_<1>)));
    BOOST_HANA_CONSTANT_ASSERT(!equal(range(int_<0>, int_<1>), range(int_<0>, int_<0>)));
    BOOST_HANA_CONSTANT_ASSERT( equal(range(int_<0>, int_<1>), range(int_<0>, int_<1>)));
    BOOST_HANA_CONSTANT_ASSERT(!equal(range(int_<0>, int_<2>), range(int_<0>, int_<1>)));
    BOOST_HANA_CONSTANT_ASSERT( equal(range(int_<0>, int_<2>), range(int_<0>, int_<2>)));
    BOOST_HANA_CONSTANT_ASSERT( equal(range(int_<0>, int_<0>), range(int_<2>, int_<2>)));

    BOOST_HANA_CONSTANT_ASSERT(equal(range(int_<2>, int_<4>), range(int_<2>, int_<4>)));
    BOOST_HANA_CONSTANT_ASSERT(equal(range(int_<-4>, int_<-3>), range(int_<-4>, int_<-3>)));
    BOOST_HANA_CONSTANT_ASSERT(equal(range(int_<-4>, int_<2>), range(int_<-4>, int_<2>)));
}


// Copyright 2019 coderkarl. Subject to the BSD license.

#include "avoid_obstacles/AvoidObs.h"
#include "avoid_obstacles/AvoidObsCommon.h"
#include <geometry_msgs/PointStamped.h>
#include <math.h>
#include <boost/math/special_functions/round.hpp>
#include <algorithm>

/**********************************************************************
* Obstacle Avoidance using a nav_msgs/OccupacyGrid and A* path planning
*
* Subscribe to /scan and Publish OccupacyGrid /costmap, Publish Path /path
*
* The leddar.launch file launches this node as well as a static transform between base_link and laser
**********************************************************************/

//Constructor
AvoidObs::AvoidObs()
{
    //Topics you want to publish
    costmap_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("costmap", 1);
    cmd_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel",10);

    pf_obs_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("pfObs", 1);

    obs_cone_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("obs_cone_pose",1);

    //Topic you want to subscribe
    scan_sub_ = nh_.subscribe("scan", 50, &AvoidObs::scanCallback, this); //receive laser scan
    odom_sub_ = nh_.subscribe("odom", 10, &AvoidObs::odomCallback, this);
    goal_sub_ = nh_.subscribe("/move_base_simple/goal", 1, &AvoidObs::goalCallback, this);
    wp_cone_sub_ = nh_.subscribe("wp_cone_pose", 1, &AvoidObs::coneCallback, this);
    found_cone_sub_ = nh_.subscribe("found_cone",1, &AvoidObs::foundConeCallback, this);
    known_obstacle_sub_ = nh_.subscribe("known_obstacle",1,&AvoidObs::knownObstacleCallback, this);
    hill_wp_sub_ = nh_.subscribe("hill_wp",1,&AvoidObs::hillWaypointCallback, this);
    nh_p  = ros::NodeHandle("~");
    nh_p.param("plan_rate_hz", plan_rate_, 1.0); //set in avoid_obs.launch
    nh_p.param("map_res_m", map_res_, 0.5);
    nh_p.param("map_size", n_width_, 200);
    nh_p.param("map_size", n_height_, 200);
    nh_p.param("max_range", max_range_, 40.0);
    nh_p.param("min_hill_range", min_hill_range_, 1.0);
    nh_p.param("plan_range_m",plan_range_, 40.0);
    nh_p.param("clear_decrement",clear_decrement_,-5);
    nh_p.param("fill_increment",fill_increment_,10);
    nh_p.param("adjacent_cost_offset", adjacent_cost_offset, 2.0);
    nh_p.param("adjacent_cost_slope", adjacent_cost_slope, 1.0);
    nh_p.param("inflation_factor", inflation_factor_, 2);
    nh_p.param("reinflate_radius", reinflate_radius_, 2.5);
    nh_p.param("cone_search_radius", cone_search_radius_, 1.0);
    nh_p.param("reinflate_cost_thresh", reinflate_cost_thresh_, 30);
    nh_p.param("use_PotFields", use_PotFields_,false);
    nh_p.param("cone_obs_thresh", cone_obs_thresh_, 20);
    nh_p.param("max_num_known_obstacles", max_num_known_obstacles_, 20);
    nh_p.param("known_obstacle_time_limit", known_obstacle_time_limit_, 30.0);

    scan_range = max_range_;

    ROS_INFO("map_size (n cells): %d", n_width_);

    reinflate_n_cells_ = boost::math::iround(reinflate_radius_/map_res_);
    cone_search_n_cells_ = boost::math::iround(cone_search_radius_/map_res_);

    //listener.setExtrapolationLimit(ros::Duration(0.1));
    listener.waitForTransform("laser", "odom", ros::Time(0), ros::Duration(10.0));

    num_obs_cells = 0; //number of obstacle cells

    map_pose.position.x = -(n_width_/2)*map_res_-map_res_/2;//1.0 -(n_width_/2)*map_res_ + map_res_/2; // I believe this is zero by default, check by echoing costmap
    map_pose.position.y = -(n_height_/2)*map_res_-map_res_/2;//1.0 -(n_height_/2)*map_res_ + map_res_/2; // will need to update x and y as we move
    ROS_INFO("map_pose x,y: %0.2f, %0.2f",map_pose.position.x, map_pose.position.y);
    map_pose.orientation.w = 1.0;

    costmap.header.stamp = ros::Time::now();
    costmap.header.frame_id = "odom";
    costmap.info.resolution = map_res_;
    costmap.info.width = n_width_;
    costmap.info.height = n_height_;
    costmap.info.origin = map_pose;
    // Fill costmap with zeros
    // cost(ix,iy) = costmap.data[ix*n_height + iy], x-RIGHT, y-UP, 0,0 is bottom left
    costmap.data.resize(n_width_*n_height_);

    goal_pose.orientation.w = 1.0;
    bot_pose.orientation.w = 1.0;
    bot_yaw = 0.0;

    //Potential Fields Obstacle Map for debugging
    pfObs.header = costmap.header;
    pfObs.info = costmap.info;
    pfObs.data.resize(n_width_*n_height_);

    camera_cone_poseStamped.header.frame_id = "odom";
    obs_cone_poseStamped.header.frame_id = "odom";
    camera_cone_poseStamped.pose.orientation.w = 1.0;
    obs_cone_poseStamped.pose.orientation.w = 1.0;
}

AvoidObs::~AvoidObs(){}

void AvoidObs::knownObstacleCallback(const geometry_msgs::PoseStamped& obs_pose)
{
  // TODO: transform known obstacle to odom frame if not already
  knownObstacleDeq.push_back(obs_pose);
  //TODO: Consider using boost circular buffer
  if(knownObstacleDeq.size() > max_num_known_obstacles_)
  {
    knownObstacleDeq.pop_front();
  }
}

void AvoidObs::hillWaypointCallback(const std_msgs::Int16& msg)
{
  if(msg.data == 1)
  {
    scan_range = min_hill_range_;
  }
  else
  {
    scan_range = max_range_;
  }
}

void AvoidObs::coneCallback(const geometry_msgs::PoseStamped& data)
{
    camera_cone_poseStamped = data;
}

void AvoidObs::foundConeCallback(const std_msgs::Int16& msg)
{
  if(msg.data == 0) // need to reset the camera_cone_poseStamped so the just bumped cone now becomes an obstacle again
  {
    // TODO: add a bool flag when we bump a cone and update a waypoint to next cone, clear the flag in next coneCallback
    //  do not check_for_cone_obstacle if this flag is set
    camera_cone_poseStamped.pose.position.x = 0;
    camera_cone_poseStamped.pose.position.y = 0;
  }
}

bool AvoidObs::check_for_cone_obstacle()
{
    double sec_since_cone = (ros::Time::now()-camera_cone_poseStamped.header.stamp).toSec();
    unsigned min_cell_dist = 4*cone_search_n_cells_;
    if(sec_since_cone < 2.0)
    {
        int cost, max_nearby_cost = cone_obs_thresh_;
        int cx, cy;
        get_map_indices(camera_cone_poseStamped.pose.position.x, camera_cone_poseStamped.pose.position.y, cx, cy);
        for(int dx = -cone_search_n_cells_; dx <= cone_search_n_cells_; ++dx)
        {
            for(int dy = -cone_search_n_cells_; dy <= cone_search_n_cells_; ++dy)
            {
                cost = get_cost(cx+dx,cy+dy);
                if(cost > cone_obs_thresh_ and cost <= 100)
                {
                    costmap.data[(cy+dy) * n_width_ + (cx+dx)] = 0;
                    unsigned cell_dist = abs(dx)+abs(dy);
                    //if(cell_dist <= min_cell_dist)
                    if(cost >= max_nearby_cost)
                    {
                        max_nearby_cost = cost;
                        min_cell_dist = cell_dist;
                        obs_cone_poseStamped.header.stamp = ros::Time::now();
                        obs_cone_poseStamped.pose.position.x = map_pose.position.x + (cx+dx)*map_res_ + map_res_/2;
                        obs_cone_poseStamped.pose.position.y = map_pose.position.y + (cy+dy)*map_res_ + map_res_/2;
                    }
                }
            }
        }
        if(max_nearby_cost > cone_obs_thresh_)
        {
            obs_cone_pub_.publish(obs_cone_poseStamped);
            return true;
        }
    }
    return false;
}

double AvoidObs::get_plan_rate()
{
    return plan_rate_;
}

void AvoidObs::update_cell(float x, float y, int val)
{
    int ix, iy;
    get_map_indices(x, y, ix, iy);
    if (inflation_factor_ <= ix && ix < n_width_-inflation_factor_ && inflation_factor_ <= iy && iy < n_height_-inflation_factor_)
    {
        int cur_val = costmap.data[iy * n_width_ + ix];
        if (val > 0 && cur_val == 0) //ignore first hits
        {
            costmap.data[iy * n_width_ + ix] = 1;
        }
        else
        {
            if(cur_val + val > 100)
                cur_val = 100;
            else if (cur_val + val < 0)
                cur_val = 0;
            else
                cur_val += val;
            costmap.data[iy*n_width_ + ix] = cur_val;

            //adjacent cell inflation
            if(val > 0)
            {
                for(int dx = -inflation_factor_; dx <= inflation_factor_; ++dx)
                {
                    for(int dy = -inflation_factor_; dy <= inflation_factor_; ++dy)
                    {
                        if((dx != 0 or dy != 0))
                        {
                            int adj_val = costmap.data[(iy+dy)*n_width_ + ix+dx];
                            int new_val = double(cur_val)/(adjacent_cost_offset+double(abs(dx)+abs(dy))*adjacent_cost_slope);
                            if(adj_val < new_val)
                                costmap.data[(iy+dy)*n_width_ + ix+dx] = new_val;
                        }
                    }
                }
            }
        }
    }
}

bool AvoidObs::update_plan()
{
	/*path.poses.clear();
	geometry_msgs::PoseStamped wp;
	wp.pose.position.x = 5.0;
	wp.pose.position.y = 5.0;
	path.poses.push_back(wp);
	*/

	geometry_msgs::Pose start, temp_goal = goal_pose;

	float dx = goal_pose.position.x - bot_pose.position.x;
	float dy = goal_pose.position.y - bot_pose.position.y;
	float goal_dist_sqd = dx*dx + dy*dy;
	if(goal_dist_sqd > plan_range_*plan_range_)
	{
		float dir_rad;
		if(dx == 0 && dy == 0)
			dir_rad = 0.0;
		else
			dir_rad = atan2(dy,dx);
		temp_goal.position.x = bot_pose.position.x+plan_range_*cos(dir_rad);
		temp_goal.position.y = bot_pose.position.y+plan_range_*sin(dir_rad);
	}

	if(use_PotFields_)
	{
		//Potential Fields Test
		// pfObs is an odom grid map, same info as costmap
		pfObs.header.stamp = ros::Time::now();
		pfObs.data.clear();
		pfObs.data.resize(n_width_*n_height_);

		pf.obs_list.clear();
		int bot_ix, bot_iy;
		get_map_indices(pf.bot.x, pf.bot.y, bot_ix, bot_iy);

		for(int ix = bot_ix - 5; ix < bot_ix+5; ++ix)
		{
			for(int iy = bot_iy - 5; iy < bot_iy+5; ++iy)
			{
				if(get_cost(ix,iy) > 30)
				{
					pfObs.data[iy*n_width_ + ix] = 95;
					PotentialFields::Obstacle obs;
					obs.x = map_pose.position.x + ix*map_res_;
					obs.y = map_pose.position.y + iy*map_res_;
					//ROS_INFO("Added pfObs x,y: %0.0f, %0.0f",obs.x, obs.y);
					pf.obs_list.push_back(obs);
				}
			}
		}

		//testing hard coded obstacle
		// revealed need to use round for x,y to ix,iy and also offset map by res/2
		//pf.obs_list.clear();
		PotentialFields::Obstacle obs;
		float x = 7.0, y=1.0;
		int ix, iy;
		for(int k=0; k<=0; ++k)
		{
			get_map_indices(x, y+k, ix, iy);
			pfObs.data[iy*n_width_ + ix] = 50;
			obs.x = x;
			obs.y = y+k;
			pf.obs_list.push_back(obs);
		}
		obs.x = 3; obs.y = 3;
		get_map_indices(obs.x, obs.y, ix, iy);
		pfObs.data[iy*n_width_ + ix] = 50;
		pf.obs_list.push_back(obs);
		obs.x = 1; obs.y = 1;
        get_map_indices(obs.x, obs.y, ix, iy);
        pfObs.data[iy*n_width_ + ix] = 50;
        pf.obs_list.push_back(obs);
		// end testing hard coded obstacle list
		bot_yaw = get_yaw(bot_pose);
		geometry_msgs::Twist cmd = pf.update_cmd(bot_yaw);
		//ROS_INFO("bot_yaw: %0.2f", bot_yaw);

		cmd_pub_.publish(cmd);
		pf_obs_pub_.publish(pfObs);
	}
}

bool AvoidObs::get_map_indices(float x, float y, int& ix, int& iy)
{
	ix = boost::math::iround((x-map_pose.position.x)/map_res_ - map_res_);
	iy = boost::math::iround((y-map_pose.position.y)/map_res_ - map_res_);
	return true;
}

int AvoidObs::get_cost(int ix, int iy)
{
	if(0 > ix || ix >= n_width_ || 0 > iy || iy >= n_height_)
		return 200;
	return costmap.data[iy*n_width_ + ix];
}

void AvoidObs::odomCallback(const nav_msgs::Odometry& odom)
{
	bot_pose.position = odom.pose.pose.position;
	bot_pose.orientation = odom.pose.pose.orientation;

	pf.bot.x = bot_pose.position.x;
	pf.bot.y = bot_pose.position.y;
}

void AvoidObs::goalCallback(const geometry_msgs::PoseStamped& data)
{
	goal_pose = data.pose;
	pf.goal.x = goal_pose.position.x;
	pf.goal.y = goal_pose.position.y;
}

void AvoidObs::scanCallback(const sensor_msgs::LaserScan& scan) //use a point cloud instead, use laser2pc.launch
{
    //ROS_INFO("NEW SCAN");
	// Transform scan to map frame, clear and fill costmap
    listener.waitForTransform("laser", "odom", scan.header.stamp, ros::Duration(10.0)); // ros::Time(0) causes exceptions

	geometry_msgs::PointStamped laser_point, odom_point;
	laser_point.header.frame_id = "laser";
	laser_point.header.stamp = scan.header.stamp;//ros::Time();
	laser_point.point.z = 0;

	for (int i = 0; i < scan.ranges.size();i++)
	{
	    float range = scan.ranges[i];
	    float angle  = scan.angle_min +(float(i) * scan.angle_increment);

	    //clear map cells
	    // only clear at range >= 0.5 meters
        for (double r = 0.5; r < (range - map_res_*2.0); r += map_res_)
        {
            double angle_step = map_res_ / r;
            //clearing as we pass obstacles, try angle_increment/3 vs /2 (reduce clearing fov per laser)
            for (double a = (angle - scan.angle_increment / 2); a < (angle + scan.angle_increment / 2); a += angle_step)
            {
                laser_point.point.x = r * cos(a);
                laser_point.point.y = r * sin(a);
                try
                {
                    listener.transformPoint("odom", laser_point,
                            odom_point);
                    update_cell(odom_point.point.x, odom_point.point.y,
                            clear_decrement_); //CLEAR_VAL_DECREASE
                }
                catch (tf::TransformException& ex)
                {
                    int xa;
                    ROS_ERROR("AvoidObs clear Received an exception trying to transform a point : %s", ex.what());
                }

            }
        }

	    // fill obstacle cells
	    if(range < scan_range)
	    {
			laser_point.point.x = range*cos(angle) ;
			laser_point.point.y = range*sin(angle) ;
			int count = 0;
			while(count < 3)
			{
				++count;
				try{
					listener.transformPoint("odom", laser_point, odom_point);
					update_cell(odom_point.point.x, odom_point.point.y, fill_increment_);
					break;
				}
				catch(tf::TransformException& ex){
					int xa;
					ROS_ERROR("AvoidObs fill Received an exception trying to transform a point : %s", ex.what());
				}
			}
	    }
	}

	// Re-apply inflation to all obstacles within a radius of the bot, this covers outside the scan FOV
	int radius_ind = reinflate_n_cells_;
	int bot_ix, bot_iy;
    get_map_indices(bot_pose.position.x, bot_pose.position.y, bot_ix, bot_iy);

    for(int ix = bot_ix - radius_ind; ix < bot_ix+radius_ind; ++ix)
    {
        for(int iy = bot_iy - radius_ind; iy < bot_iy+radius_ind; ++iy)
        {
            if(get_cost(ix,iy) > reinflate_cost_thresh_)
            {
                int cur_val = costmap.data[iy * n_width_ + ix];
                //adjacent cell inflation
                for(int dx = -1; dx <= 1; ++dx)
                {
                    for(int dy = -1; dy <= 1; ++dy)
                    {
                        if((dx != 0 or dy != 0))
                        {
                            int iix = ix+dx;
                            int iiy = iy+dy;
                            if (0 <= iix && iix < n_width_ && 0 <= iiy && iiy < n_height_)
                            {
                                int adj_val = costmap.data[(iiy)*n_width_ + iix];
                                int new_val = double(cur_val)/(adjacent_cost_offset+double(abs(dx)+abs(dy))*adjacent_cost_slope);
                                if(adj_val < new_val)
                                    costmap.data[(iiy)*n_width_ + iix] = new_val;
                            }
                        }
                    }
                }
            }
        }
    }
    // END Re-apply inflation

	//odom_point.point.x = 10.0;
	//odom_point.point.y = -5.0;
	//update_cell(odom_point.point.x, odom_point.point.y, 100.0);

    check_for_cone_obstacle(); //clears obstacles near cone pose estimated from camera

    // ensure costmap has known obstacles
    check_known_obstacles();

    costmap.header.stamp = scan.header.stamp;
    costmap_pub_.publish(costmap);
}

void AvoidObs::check_known_obstacles()
{
  if(knownObstacleDeq.size() > 0)
  {
    // check if oldest obstacle has expired
    if( (ros::Time::now() - knownObstacleDeq[0].header.stamp).toSec() > known_obstacle_time_limit_)
    {
      knownObstacleDeq.pop_front();
    }

    // insert known obstacles into costmap
    for(unsigned k=0; k<knownObstacleDeq.size(); ++k)
    {
      const geometry_msgs::PoseStamped& obs_pose = knownObstacleDeq.at(k);
      update_cell(obs_pose.pose.position.x, obs_pose.pose.position.y, 100);
    }
  }
}

int main(int argc, char **argv)
{
    //Initiate ROS
    ros::init(argc, argv, "avoid_obs");

    AvoidObs avoid_obs;
    ROS_INFO("Starting Obstacle Avoidance");
    int loop_hz = avoid_obs.get_plan_rate();
    ros::Rate rate(loop_hz);

    while(ros::ok())
    {
        ros::spinOnce();
        avoid_obs.update_plan();
        rate.sleep();
    }

    return 0;
}


#define BOOST_TEST_MODULE "test_dimension"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include "../LinearAlgebra.hpp"
constexpr static std::size_t N = 3;
constexpr static std::size_t M = 6;

BOOST_AUTO_TEST_CASE(dimension_static_vector)
{
    const ax::Vector<double, N> vec1;
    const ax::Vector<double, M> vec2(1e0);

    BOOST_CHECK_EQUAL(dimension(vec1), N);
    BOOST_CHECK_EQUAL(dimension(vec2), M);
}

BOOST_AUTO_TEST_CASE(dimension_dynamic_vector)
{
    const ax::Vector<double, ax::DYNAMIC> vec1(N);
          ax::Vector<double, ax::DYNAMIC> vec2(M);

    BOOST_CHECK_EQUAL(dimension(vec1), N);
    BOOST_CHECK_EQUAL(dimension(vec2), M);

    vec2.append(1.0);
    BOOST_CHECK_EQUAL(dimension(vec2), M+1);
}

BOOST_AUTO_TEST_CASE(dimension_expression)
{
    const ax::Vector<double, N> vec1(1.0);
    const ax::Vector<double, N> vec2(2.0);

    // static + static
    const auto expr1 = vec1 + vec2;
    BOOST_CHECK_EQUAL(dimension(expr1), N);

    const auto expr2 = vec1 * 2.0;
    BOOST_CHECK_EQUAL(dimension(expr2), N);

    // dynamic + dynamic
    const ax::Vector<double, ax::DYNAMIC> vec3(N);
    const ax::Vector<double, ax::DYNAMIC> vec4(N);

    const auto expr3 = vec3 + vec4;
    BOOST_CHECK_EQUAL(dimension(expr3), N);

    const auto expr4 = vec3 * 2.0;
    BOOST_CHECK_EQUAL(dimension(expr4), N);

    // mixture
    const auto expr5 = vec1 + vec3;
    BOOST_CHECK_EQUAL(dimension(expr5), N);

    const auto expr6 = vec3 + vec1;
    BOOST_CHECK_EQUAL(dimension(expr6), N);
}

BOOST_AUTO_TEST_CASE(dimension_static_matrix_column)
{
    const ax::Matrix<double, N, M> mat1(1e0);
    const ax::Matrix<double, M, N> mat2(1e0);

    BOOST_CHECK_EQUAL(dimension_col(mat1), M);
    BOOST_CHECK_EQUAL(dimension_col(mat2), N);
}

BOOST_AUTO_TEST_CASE(dimension_static_matrix_row)
{
    const ax::Matrix<double, N, M> mat1(1e0);
    const ax::Matrix<double, M, N> mat2(1e0);

    BOOST_CHECK_EQUAL(dimension_row(mat1), N);
    BOOST_CHECK_EQUAL(dimension_row(mat2), M);
}

BOOST_AUTO_TEST_CASE(dimension_dynamic_matrix_column)
{
    const ax::Matrix<double, ax::DYNAMIC, ax::DYNAMIC> mat1(N, M);
    const ax::Matrix<double, ax::DYNAMIC, ax::DYNAMIC> mat2(M, N);

    BOOST_CHECK_EQUAL(dimension_col(mat1), M);
    BOOST_CHECK_EQUAL(dimension_col(mat2), N);
}

BOOST_AUTO_TEST_CASE(dimension_dynamic_matrix_row)
{
    const ax::Matrix<double, ax::DYNAMIC, ax::DYNAMIC> mat1(N, M);
    const ax::Matrix<double, ax::DYNAMIC, ax::DYNAMIC> mat2(M, N);

    BOOST_CHECK_EQUAL(dimension_row(mat1), N);
    BOOST_CHECK_EQUAL(dimension_row(mat2), M);
}


#include <sparse.h>
#include <sparse_fill.h>

#include <narrow.h>

#include <prox_rigid_body.h>
#include <prox_get_velocity_vector.h>
#include <prox_set_velocity_vector.h>

#include <prox_math_policy.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/test_tools.hpp>

typedef prox::MathPolicy<float>      math_policy;
typedef math_policy::tiny_types      tiny_types;
typedef math_policy::real_type       real_type;
typedef math_policy::vector3_type    vector3_type;
typedef math_policy::vector6_type    vector6_type;
typedef math_policy::matrix3x3_type  matrix3x3_type;

BOOST_AUTO_TEST_SUITE(velocity_vector);
BOOST_AUTO_TEST_CASE(get_velocity_vector_test)
{

  // Initialize geometry
  narrow::System<tiny_types> narrow;

  size_t gid = narrow.create_geometry();

  narrow::Geometry< tiny_types >::sphere_type sphere;
  sphere.radius() = 1.0f;

  narrow::Geometry<tiny_types > & moving_geometry = narrow.get_geometry(gid);
  moving_geometry.add_shape( sphere );

  // Initialize objects
  std::vector< prox::RigidBody< math_policy > > bodies;
  bodies.resize( 4u );

  bodies[0].set_geometry_idx( gid );
  bodies[0].set_velocity( vector3_type::make( 5.0,7.0,0.0) );

  bodies[1].set_geometry_idx( gid );
  bodies[1].set_velocity( vector3_type::make( 0.0,1.0,0.0) );

  bodies[2].set_geometry_idx( gid );
  bodies[2].set_velocity( vector3_type::make( 0.0,3.0,0.0) );

  bodies[3].set_geometry_idx( gid );
  bodies[3].set_velocity( vector3_type::make( 2.0,5.0,0.0) );

  vector6_type u;

  prox::get_velocity_vector(bodies.begin(),bodies.end(),u, math_policy());

  BOOST_CHECK_EQUAL(u(0)(0), 5);
  BOOST_CHECK_EQUAL(u(0)(1), 7);
  BOOST_CHECK_EQUAL(u(0)(2), 0);

  BOOST_CHECK_EQUAL(u(1)(0), 0);
  BOOST_CHECK_EQUAL(u(1)(1), 1);
  BOOST_CHECK_EQUAL(u(1)(2), 0);

  BOOST_CHECK_EQUAL(u(2)(0), 0);
  BOOST_CHECK_EQUAL(u(2)(1), 3);
  BOOST_CHECK_EQUAL(u(2)(2), 0);

  BOOST_CHECK_EQUAL(u(3)(0), 2);
  BOOST_CHECK_EQUAL(u(3)(1), 5);
  BOOST_CHECK_EQUAL(u(3)(2), 0);

  bodies[2].set_velocity( vector3_type::make( 8.0,3.0,0.5) );

  prox::get_velocity_vector(bodies.begin(),bodies.end(),u, math_policy());

  BOOST_CHECK_EQUAL(u(0)(0), 5);
  BOOST_CHECK_EQUAL(u(0)(1), 7);
  BOOST_CHECK_EQUAL(u(0)(2), 0);

  BOOST_CHECK_EQUAL(u(1)(0), 0);
  BOOST_CHECK_EQUAL(u(1)(1), 1);
  BOOST_CHECK_EQUAL(u(1)(2), 0);

  BOOST_CHECK_EQUAL(u(2)(0), 8);
  BOOST_CHECK_EQUAL(u(2)(1), 3);
  BOOST_CHECK_EQUAL(u(2)(2), 0.5);

  BOOST_CHECK_EQUAL(u(3)(0), 2);
  BOOST_CHECK_EQUAL(u(3)(1), 5);
  BOOST_CHECK_EQUAL(u(3)(2), 0);
}

BOOST_AUTO_TEST_CASE(set_velocity_vector_test)
{

  // Initialize geometry
  narrow::System<tiny_types> narrow;

  size_t gid = narrow.create_geometry();

  narrow::Geometry< tiny_types >::sphere_type sphere;
  sphere.radius() = 1.0f;

  narrow::Geometry< tiny_types > & moving_geometry = narrow.get_geometry(gid);
  moving_geometry.add_shape( sphere );

  // Initialize objects
  std::vector< prox::RigidBody< math_policy > > bodies;
  bodies.resize( 4u );

  bodies[0].set_geometry_idx( gid );
  bodies[1].set_geometry_idx( gid );
  bodies[2].set_geometry_idx( gid );
  bodies[3].set_geometry_idx( gid );

  vector6_type u(4);

  sparse::fill(u(0));// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'
  sparse::fill(u(1));// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'
  sparse::fill(u(2));// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'
  sparse::fill(u(3));// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'

  prox::set_velocity_vector(bodies.begin(),bodies.end(),u, math_policy());

  BOOST_CHECK_EQUAL(bodies[0].get_velocity()(0), 0);
  BOOST_CHECK_EQUAL(bodies[0].get_velocity()(1), 1);
  BOOST_CHECK_EQUAL(bodies[0].get_velocity()(2), 2);

  BOOST_CHECK_EQUAL(bodies[1].get_velocity()(0), 0);
  BOOST_CHECK_EQUAL(bodies[1].get_velocity()(1), 1);
  BOOST_CHECK_EQUAL(bodies[1].get_velocity()(2), 2);

  BOOST_CHECK_EQUAL(bodies[2].get_velocity()(0), 0);
  BOOST_CHECK_EQUAL(bodies[2].get_velocity()(1), 1);
  BOOST_CHECK_EQUAL(bodies[2].get_velocity()(2), 2);

  BOOST_CHECK_EQUAL(bodies[3].get_velocity()(0), 0);
  BOOST_CHECK_EQUAL(bodies[3].get_velocity()(1), 1);
  BOOST_CHECK_EQUAL(bodies[3].get_velocity()(2), 2);

  sparse::fill(u(2),4);// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'
  sparse::fill(u(3),7);// 2009-09-20 Kenny:  error:`fill' is not a member of 'sparse'

  prox::set_velocity_vector(bodies.begin(),bodies.end(),u, math_policy());

  BOOST_CHECK_EQUAL(bodies[0].get_velocity()(0), 0);
  BOOST_CHECK_EQUAL(bodies[0].get_velocity()(1), 1);
  BOOST_CHECK_EQUAL(bodies[0].get_velocity()(2), 2);

  BOOST_CHECK_EQUAL(bodies[1].get_velocity()(0), 0);
  BOOST_CHECK_EQUAL(bodies[1].get_velocity()(1), 1);
  BOOST_CHECK_EQUAL(bodies[1].get_velocity()(2), 2);

  BOOST_CHECK_EQUAL(bodies[2].get_velocity()(0), 4);
  BOOST_CHECK_EQUAL(bodies[2].get_velocity()(1), 5);
  BOOST_CHECK_EQUAL(bodies[2].get_velocity()(2), 6);

  BOOST_CHECK_EQUAL(bodies[3].get_velocity()(0), 7);
  BOOST_CHECK_EQUAL(bodies[3].get_velocity()(1), 8);
  BOOST_CHECK_EQUAL(bodies[3].get_velocity()(2), 9);
}
BOOST_AUTO_TEST_SUITE_END();