// Copyright 2015-2020 The ALMA Project Developers
//
// 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.

/// @file
///
/// Definitions of geometry_2d.hpp

#include <geometry_2d.hpp>
#include <io_utils.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <utilities.hpp>

namespace alma {

Eigen::MatrixXd geometry_2d::bounding_box(polygon& p) {
    Eigen::Matrix2d limits;
    Eigen::MatrixXd points;
    std::size_t cols = 0;
    boost::geometry::for_each_point(p, [&cols](point& i) { cols++; });
    points.resize(2, cols);
    cols = 0;
    boost::geometry::for_each_point(p, [&cols, &points](point& i) {
        points(0, cols) = boost::geometry::get<0>(i);
        points(1, cols) = boost::geometry::get<1>(i);
        cols++;
    });
    // Xmin
    limits(0, 0) = points.row(0).minCoeff();
    // Xmax
    limits(1, 0) = points.row(0).maxCoeff();
    // Ymin
    limits(0, 1) = points.row(1).minCoeff();
    // Ymax
    limits(1, 1) = points.row(1).maxCoeff();

    return limits;
}

void geometry_2d::get_sides(Eigen::MatrixXd& vertices_) {
    if (vertices_.rows() != 2) {
        throw alma::input_error("Error in provided vertices,"
                                " bad dimension\n");
    }
    for (auto i = 0; i < vertices_.cols(); i++) {
        Eigen::Vector2d p1 = vertices_.col(i);
        for (auto j = i; j < vertices_.cols(); j++) {
            Eigen::Vector2d p2 = vertices_.col(j);
            /// If same point pass
            if ((p1 - p2).norm() < 1.0e-6)
                continue;
            segment s({p1(0), p1(1)}, {p2(0), p2(1)});
            point sc;
            /// Looking for midpoint
            boost::geometry::centroid(s, sc);
            /// To prevent numerical issues about boder
            /// we slightly displace the point
            /// away from hull center
            Eigen::Vector2d ncsc;
            ncsc << sc.get<0>() - this->center(0),
                sc.get<1>() - this->center(1);

            if (ncsc.norm() == 0.) {
                continue;
            }

            double nc = 1. / ncsc.norm();
            /// Normalize and multiply by the eps
            ncsc(0) *= 1.0e-4 * nc;
            ncsc(1) *= 1.0e-4 * nc;
            point fp(ncsc(0) + sc.get<0>(), ncsc(1) + sc.get<1>());

            bool valid_segment = boost::geometry::covered_by(fp, this->hull);
            /// If valid segment
            if (!valid_segment) {
                geom2d_border b;
                b.p1 = p1;
                b.p2 = p2;
                b.nd = (p2 - p1);
                // Build it to point outside
                Eigen::Vector2d np_t;
                np_t << b.nd(1), -b.nd(0);
                if (np_t.dot(ncsc) < 0.) {
                    np_t *= -1;
                }
                b.np = np_t / np_t.norm();

                auto pa = (p1 - 1.0e-6 * b.nd).eval();
                auto pb = (p2 + 1.0e-6 * b.nd).eval();

                segment sf({pa(0), pa(1)}, {pb(0), pb(1)});

                b.sb = s;
                b.sbl = sf;

                if (std::find_if(borders.begin(),
                                 borders.end(),
                                 [&b](const geom2d_border& a) -> bool {
                                     if ((a.np - b.np).norm() < 1.0e-6)
                                         return true;
                                     return false;
                                 }) == borders.end()) {
                    borders.push_back(b);
                }

                if (borders.size() >
                    static_cast<std::size_t>(vertices_.cols())) {
                    throw alma::geometry_error("To much borders");
                }
            }
        }
    }
}

geometry_2d::geometry_2d(Eigen::MatrixXd& vertices_) {
    //     if (vertices_.cols()!=3) {
    //         std::cerr << "Error in vertices, we are only accepting
    //         triangles\n"; exit(EXIT_FAILURE);
    //     }

    polygon poly;

    for (auto i = 0; i < vertices_.cols(); i++) {
        double x = vertices_(0, i);
        double y = vertices_(1, i);
        boost::geometry::append(poly, boost::make_tuple(x, y));
    }
    /// Creating convex hull
    boost::geometry::convex_hull(poly, this->hull);
    /// Calculating centroid
    point c;
    boost::geometry::centroid(this->hull, c);
    this->center << c.get<0>(), c.get<1>();
    /// Calculating area
    this->area = boost::geometry::area(this->hull);
    /// Geting borders
    get_sides(vertices_);
    /// Getting bounding_box
    this->bbox = bounding_box(this->hull);
}

bool geometry_2d::inside(Eigen::Vector2d& point2check) const {
    point p(point2check(0), point2check(1));
    return boost::geometry::covered_by(p, this->hull);
}

std::tuple<double, Eigen::Vector2d, std::vector<int>>
geometry_2d::get_inter_side(Eigen::Vector2d& r0,
                            Eigen::Vector2d& v,
                            double dt) {
    point p0(r0(0), r0(1));
    point pf(r0(0) + dt * v(0), r0(1) + dt * v(1));
    segment s(p0, pf);

    double time = 1.0e+7;
    Eigen::Vector2d rf;
    int border_id = 0;
    std::vector<int> bids;
    bool already_there = true;

    for (auto& b : borders) {
        /// To rule out already there stuff
        if (boost::geometry::intersects(s, b.sbl) and
            !boost::geometry::covered_by(p0, b.sbl)) {
            if (b.np.dot(v) > 0.) {
                std::vector<point> rinter;
                boost::geometry::intersection(s, b.sbl, rinter);

                double newtime =
                    std::max({(std::abs(v(0)) < 1.0e-6)
                                  ? -1
                                  : (rinter[0].get<0>() - r0(0)) / v(0),
                              (std::abs(v(1)) < 1.0e-6)
                                  ? -1
                                  : (rinter[0].get<1>() - r0(1)) / v(1)});

                /// Corner case
                if (newtime == time) {
                    bids.push_back(border_id);
                }

                if (newtime < time) {
                    bids.clear();
                    rf(0) = rinter[0].get<0>();
                    rf(1) = rinter[0].get<1>();
                    time = newtime;
                    already_there = false;
                    bids.push_back(border_id);
                }
            }
        }

        border_id++;
    }

    /// In the case already there and going outside
    /// of box
    if (already_there) {
        point p0p(r0(0) - 1.0e-6 * v(0), r0(1) - 1.0e-6 * v(1));
        segment sp(p0p, pf);

        border_id = 0;

        for (auto& b : borders) {
            /// To rule out already there stuff
            if (boost::geometry::intersects(sp, b.sbl)) {
                if (b.np.dot(v) > 0.) {
                    std::vector<point> rinter;
                    boost::geometry::intersection(sp, b.sbl, rinter);

                    double newtime =
                        std::max({(std::abs(v(0)) < 1.0e-6)
                                      ? -1.0e+8
                                      : (rinter[0].get<0>() - r0(0)) / v(0),
                                  (std::abs(v(1)) < 1.0e-6)
                                      ? -1.0e+8
                                      : (rinter[0].get<1>() - r0(1)) / v(1)});

                    /// Corner case
                    if (newtime == time) {
                        bids.push_back(border_id);
                    }

                    if (std::abs(newtime) < time) {
                        bids.clear();
                        rf(0) = rinter[0].get<0>();
                        rf(1) = rinter[0].get<1>();
                        time = newtime;
                        bids.push_back(border_id);
                    }
                }
            }

            border_id++;
        }

        if (time > 1.0e+6) {
            //             std::cout << "Error in get_inter_side\n";
            //             std::cout << time << '\t' << dt << std::endl;
            //             std::cout << "v\n" << v << std::endl;
            //             std::cout << "r0\n" << r0 << std::endl;
            //             for (auto &b : borders) {
            //                 std::cout << "nb\n"<< b.np << std::endl;
            //                 std::cout << std::boolalpha <<
            //                 boost::geometry::intersects(sp, b.sbl) << '\t'
            //                 << boost::geometry::intersects(s, b.sbl) << '\t'
            //                 << boost::geometry::covered_by(pf,b.sb) << '\t'
            //                 << boost::geometry::covered_by(p0,b.sb) <<
            //                 std::endl;
            //             }
            //             std::cout << "box_id: " << this->get_id() <<
            //             std::endl;
            // 	    Eigen::Vector2d rf_ = r0+dt*v;
            //             std::cout << std::boolalpha <<
            //                 this->inside(r0) << '\t'  << std::endl;
            //             std::cout << "rf:\n" << rf_ << std::endl <<
            //             std::boolalpha <<
            //                 this->inside(rf_) << '\t' << std::endl;
            throw alma::geometry_error("Error to huge time");
        }
    }

    /// This is to clean out some numerical noise
    /// from geometric library
    if (alma::almost_equal(time, 0.))
        time = 0.;

    if (time < 0.) {
        std::cout << "Error in get_inter_side\n";
        std::cout << time << std::endl;
        std::cout << "v\n" << v << std::endl;
        std::cout << "r0\n" << r0 << std::endl;
        std::cout << "rf\n" << rf << std::endl;
        throw alma::geometry_error("Bad time");
    }

    return std::make_tuple(time, rf, bids);
}

void geometry_2d::calculate_contacts(std::vector<geometry_2d>& system) {
    /// First get real contacts
    for (auto& element : system) {
        if (element.id == this->id)
            continue;
        if (boost::geometry::intersects(this->hull, element.hull)) {
            std::vector<point> pinter;
            /// This gives the 2 points of the segment
            boost::geometry::intersection(this->hull, element.hull, pinter);

            /// Border object
            geom2d_border cb;

            /// If point contact
            if (pinter.size() == 1) {
                cb.p1(0) = pinter[0].get<0>();
                cb.p1(1) = pinter[0].get<1>();
                cb.p2(0) = pinter[0].get<0>();
                cb.p2(1) = pinter[0].get<1>();

                cb.nd = Eigen::Vector2d::Zero();
                cb.sb = segment(pinter[0], pinter[0]);
                cb.np = cb.p1 - this->get_center();
                cb.np /= cb.np.norm();

                contacts.emplace(
                    std::make_pair(element.id, std::vector<geom2d_border>{cb}));
                continue;
            }

            cb.p1(0) = pinter[0].get<0>();
            cb.p1(1) = pinter[0].get<1>();
            cb.p2(0) = pinter[1].get<0>();
            cb.p2(1) = pinter[1].get<1>();

            if ((cb.p2 - cb.p1).norm() < 1.0e-6) {
                cb.p1(0) = pinter[0].get<0>();
                cb.p1(1) = pinter[0].get<1>();
                cb.p2(0) = pinter[1].get<0>();
                cb.p2(1) = pinter[1].get<1>();

                cb.nd = Eigen::Vector2d::Zero();
                cb.sb = segment(pinter[0], pinter[1]);
                cb.np = cb.p1 - this->get_center();
                cb.np /= cb.np.norm();

                contacts.emplace(
                    std::make_pair(element.id, std::vector<geom2d_border>{}));
                continue;
            }

            /// Get director vector of segment
            cb.nd = (cb.p2 - cb.p1); //
                                     //(cb.p2 - cb.p1).norm();
            /// Get perpendicular vector pointing outside
            segment s(pinter[0], pinter[1]);
            point sc;
            /// Looking for midpoint
            boost::geometry::centroid(s, sc);
            /// To prevent numerical issues about boder
            /// we slightly displace the point
            /// away from hull center
            Eigen::Vector2d ncsc;
            ncsc << sc.get<0>() - this->center(0),
                sc.get<1>() - this->center(1);
            Eigen::Vector2d np_t;
            np_t << cb.nd(1), -cb.nd(0);
            if (np_t.dot(ncsc) < 0.) {
                np_t *= -1;
            }
            cb.np = np_t / np_t.norm();
            cb.sb = s;

            auto pa = (cb.p1 - 1.0e-6 * cb.nd / cb.nd.norm()).eval();
            auto pb = (cb.p2 + 1.0e-6 * cb.nd / cb.nd.norm()).eval();
            segment sf({pa(0), pa(1)}, {pb(0), pb(1)});
            cb.sbl = sf;

            contacts.emplace(
                std::make_pair(element.id, std::vector<geom2d_border>{cb}));
        }
    }

    if (contacts.size() == 0 and system.size() > 1) {
        throw alma::geometry_error("There are unconnected boxes");
    }
}

/// Helper function definitions
///@param[in] r point to check if tri-or-higher
///         intersection
///@param[in] gs vector containing the geometry
std::pair<bool, std::vector<std::size_t>> in_corner3(
    Eigen::Vector2d& r,
    std::vector<geometry_2d>& gs) {
    std::size_t ic = 0;
    std::vector<std::size_t> ids;
    ids.reserve(gs.size());
    boost::tuple<double, double> p(r(0), r(1));
    for (auto& g : gs) {
        if (boost::geometry::covered_by(p, g.get_poly())) {
            ids.push_back(g.get_id());
            ic++;
        }
    }

    if (ic > 2) {
        return std::make_pair(true, ids);
    }
    return std::make_pair(false, ids);
}

std::vector<alma::geometry_2d> read_geometry_XML(std::string xmlfname) {
    // Vector containing geometry
    std::vector<alma::geometry_2d> geometries;

    // Create empty property tree object
    boost::property_tree::ptree tree;

    // Parse XML input file into the tree
    boost::property_tree::read_xml(xmlfname, tree);

    for (const auto& v : tree.get_child("Geometry")) {
        if (v.first == "number_of_boxes") {
            std::size_t ngeometries =
                alma::parseXMLfield<std::size_t>(v, "Ngeom");
            geometries.reserve(ngeometries);
        }
        /// Iterate throught boxes
        if (v.first == "Box") {
            auto box_tree = v.second;
            std::string matname;
            Eigen::MatrixXd vertices;
            double Teq = -1.;
            bool periodic = false;
            std::size_t box2translate;
            Eigen::Vector2d translation;
            bool reservoir = false;
            double theta = 0.;
            double Treal = -1.;

            for (auto it = box_tree.begin(); it != box_tree.end(); it++) {
                /// Parsing name
                if (it->first == "MaterialID")
                    matname = alma::parseXMLfield<std::string>(*it, "name");
                // Parsing vertices:
                if (it->first == "Vertices") {
                    int rows, cols;
                    rows = alma::parseXMLfield<int>(*it, "dim");
                    cols = alma::parseXMLfield<int>(*it, "npoints");
                    vertices.resize(rows, cols);

                    std::stringstream datass(
                        (box_tree).get<std::string>("Vertices"));

                    for (auto i = 0; i < rows * cols; i++) {
                        datass >> vertices.data()[i];
                    }
                }
                /// Equilibrium temperature
                if (it->first == "initCnd") {
                    Teq = alma::parseXMLfield<double>(*it, "Teq");
                    if (alma::probeXMLfield<double>(*it, "Tinit")) {
                        Treal = alma::parseXMLfield<double>(*it, "Tinit");
                    }
                    else {
                        Treal = Teq;
                    }
                }
                /// Parsing periodic:
                if (it->first == "Translate_to") {
                    box2translate = alma::parseXMLfield<std::size_t>(*it, "id");

                    periodic = true;

                    std::stringstream datass(
                        (box_tree).get<std::string>("Translate_to"));

                    datass >> translation(0) >> translation(1);
                }
                /// Reservoir
                if (it->first == "Reservoir")
                    reservoir = true;
                /// Parsing angle:
                if (it->first == "theta")
                    theta = alma::parseXMLfield<double>(*it, "angle");
            }

            /// Set properties
            alma::geometry_2d g(vertices);
            if (periodic) {
                g.periodic = true;
                g.translation = translation;
                g.box2translate = box2translate;
            }
            if (reservoir)
                g.reservoir = true;

            g.Teq = Teq;
            g.Treal = Treal;
            g.material = matname;
            g.theta = theta;
            g.rotmat.setZero();
            /// Obtain rotation matrix
            if (theta == 0.) {
                g.rotmat = Eigen::Matrix2d::Identity();
            }
            else {
                g.rotmat(0, 0) = std::cos(theta);
                g.rotmat(1, 1) = std::cos(theta);
                g.rotmat(0, 1) = -std::sin(theta);
                g.rotmat(1, 0) = std::sin(theta);
            }
            geometries.push_back(g);
        }
    }

    /// Assing ids
    assign_geom_ids(geometries);
    /// Calculate contacts
    for (auto& g : geometries)
        g.calculate_contacts(geometries);

    return geometries;
}

Eigen::MatrixXd calculate_gradientT(std::vector<alma::geometry_2d>& sys) {
    Eigen::MatrixXd gradients(3, sys.size());
    gradients.setZero();

    for (auto& s : sys) {
        if (s.reservoir or s.periodic)
            continue;
        /// Get data for gradient
        /// calculation
        double T0 = s.Teq;
        auto id0 = s.get_id();
        Eigen::Vector2d c0 = s.get_center();

        auto& contacts = s.get_contacts();

        std::size_t csize = 0;
        for (auto& [c, b] : contacts) {
            if (!sys[c].reservoir and !sys[c].periodic)
                csize++;
        }

        /// We want to solve:
        /// f(r0+hi) - f(r0) = h_i * gradf
        /// dfi = hi*gradf
        /// gradf = MPinv(hi)*dfi
        /// where MPinv is the Moore–Penrose inverse
        Eigen::VectorXd dfi(csize);
        Eigen::MatrixXd hi(csize, 2);

        int ci = 0;
        for (auto& [c, b] : contacts) {
            if (sys[c].reservoir or sys[c].periodic)
                continue;
            dfi(ci) = sys[c].Teq - T0;
            Eigen::Vector2d hi_ = sys[c].get_center() - c0;
            hi.row(ci) = hi_;
            ci++;
        }

        /// The actual MPinv is not used
        Eigen::CompleteOrthogonalDecomposition<Eigen::MatrixXd> cqr(hi);

        /// But we solve the minimum-norm solution gradT
        /// to a least squares problem
        Eigen::Vector2d gradT = cqr.solve(dfi);

        gradients(0, id0) = gradT(0);
        gradients(1, id0) = gradT(1);
    }
    return gradients;
}

}; // namespace alma


#include <ros/ros.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/ndt.h>
#include <pcl/filters/voxel_grid.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <pclomp/ndt_omp.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl_ros/point_cloud.h>

sensor_msgs::PointCloud2 pcl2pointcloud (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, char *fix_frame);
void cloud_callback(const sensor_msgs::PointCloud2::ConstPtr& msg);
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud(new pcl::PointCloud<pcl::PointXYZ>());
ros::Subscriber sub_sourcecloud;
ros::Subscriber sub_device_tf;
ros::Publisher pub_targetcloud;
ros::Publisher pub_transformcloud;

bool is_initial;

int main(int argc, char** argv) {
    ros::init (argc, argv, "ndt_odometry");
    ros::NodeHandle nh;
    sub_sourcecloud = nh.subscribe ("/tango/point_cloud", 1, cloud_callback);
    pub_targetcloud = nh.advertise<sensor_msgs::PointCloud2> ("/target_cloud", 1);
    pub_transformcloud = nh.advertise<sensor_msgs::PointCloud2> ("/transform_cloud", 1);
    is_initial = true;
    ros::spin ();

	return 0;
}

sensor_msgs::PointCloud2 pcl2pointcloud (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, char *fix_frame) {
    sensor_msgs::PointCloud2 ros_cloud;
    pcl::toROSMsg(*cloud, ros_cloud); //convert from PCL to ROS type this way
    ros_cloud.header.frame_id = fix_frame;
    return ros_cloud;
}

void cloud_callback(const sensor_msgs::PointCloud2::ConstPtr& msg) {
    //tf parameter
    static tf::TransformBroadcaster broadcaster;
    static tf::TransformListener listener;
    static tf::StampedTransform transform;
    static tf::Transform transform_pose;
    //initialize
    if(is_initial) {
        if(pcl::io::loadPCDFile("/home/ros20/Desktop/ndt_ws/src/ndt_odometry/map/target_cloud.pcd", *target_cloud)) {
            std::cerr << "failed to load " << std::endl;
            return;
        }
        pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::VoxelGrid<pcl::PointXYZ> voxelgrid;
        voxelgrid.setLeafSize(0.1f, 0.1f, 0.1f);
        voxelgrid.setInputCloud(target_cloud);
        voxelgrid.filter(*downsampled);
        target_cloud = downsampled;

    	std::cout << "Loaded " << target_cloud->size () << " data points from target" << std::endl;
        is_initial = false;
        listener.waitForTransform("/start_of_service", "/camera_depth", ros::Time(0), ros::Duration(1.0) );
        listener.lookupTransform("/start_of_service", "/camera_depth", ros::Time(0), transform);
        transform_pose.setOrigin( transform.getOrigin());
        transform_pose.setRotation( transform.getRotation());
        broadcaster.sendTransform(tf::StampedTransform(transform_pose, ros::Time::now(), "start_of_service", "ndt_transformed"));

    }
    else{
        pcl::fromROSMsg (*msg, *source_cloud);

        //downsampling
        pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled(new pcl::PointCloud<pcl::PointXYZ>());

        pcl::VoxelGrid<pcl::PointXYZ> voxelgrid;
        voxelgrid.setLeafSize(0.1f, 0.1f, 0.1f);

        voxelgrid.setInputCloud(source_cloud);
        voxelgrid.filter(*downsampled);
        source_cloud = downsampled;

        //Set initial alignment estimate found using robot odometry.

        listener.waitForTransform("/start_of_service", "/ndt_transformed", ros::Time(0), ros::Duration(1.0) );
        listener.lookupTransform("/start_of_service", "/ndt_transformed", ros::Time(0), transform);
        Eigen::Quaterniond init_rotation_Qua = Eigen::Quaterniond(transform.getRotation());
        Eigen::Translation3f init_translation (transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z());
        Eigen::Matrix3f init_rotation = init_rotation_Qua.toRotationMatrix().cast <float> ();
        Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();

        pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
        ndt_omp->setInputTarget(target_cloud);
        ndt_omp->setInputSource(source_cloud);
        ndt_omp->setTransformationEpsilon (0.01);
        ndt_omp->setStepSize (0.1);
        ndt_omp->setResolution(1.0);
        ndt_omp->setMaximumIterations (100);
        ndt_omp->setNumThreads(6);
        ndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7);
        ndt_omp->align(*transform_cloud, init_guess);
        std::cout << " score: " << ndt_omp->getFitnessScore() << std::endl;

        Eigen::Matrix4f target_pose= ndt_omp->getFinalTransformation();
        if((fabs(target_pose(0,3))+fabs(target_pose(1,3))+fabs(target_pose(2,3))) > 0.4)        //transform pose
        {
            Eigen::Matrix3f rotate_3f = target_pose.topLeftCorner(3,3);
            Eigen::Quaternionf rotate_q(rotate_3f);
            transform_pose.setOrigin( tf::Vector3(target_pose(0,3), target_pose(1,3), target_pose(2,3)) );
            transform_pose.setRotation(tf::Quaternion(rotate_q.x(), rotate_q.y(), rotate_q.z(), rotate_q.w()) );
            //transform_pose.setRotation( tf::Quaternion(0, 0, 0, 1) );
            broadcaster.sendTransform(tf::StampedTransform(transform_pose, ros::Time::now(), "start_of_service", "ndt_transformed"));

            pub_transformcloud.publish( pcl2pointcloud(transform_cloud, (char*)"start_of_service"));
        }

        pub_targetcloud.publish( pcl2pointcloud(target_cloud, (char*)"start_of_service"));
    }
    return;
}


//
//  array_module.cpp
//  ConvNet.array
//
//  Created by Wang Yi on 5/11/17.
//  Copyright © 2017 Wang Lei. All rights reserved.
//

#include "matrix_array_module.hpp"
#include <boost/python/module.hpp>
#include <boost/python/def.hpp>

BOOST_PYTHON_MODULE(matrix_array)
{
    using namespace boost::python;

    class_<Matrix>("Matrix",
                          "NetConv.array.Matrix is a n-dimensional matrix container based on research 'matrix_array' published in 2014 in PyPi"
                          "The project is aimed to provided easy interactive matrix container for ConvNet project",
                          init<ll>(args("size"), "Row contructor. This will be called by upper layer constructors")
                   )
    .def("getitem", &Matrix::getitem)
    .def("getitem_multi", &Matrix::getitem_multi)
    .def("setitem", &Matrix::setitem, return_internal_reference<>())
    .def("setitem_multi", &Matrix::setitem_multi, return_internal_reference<>())
    ;
}


#define BOOST_TEST_MODULE autotest_vision

#include <boost/test/included/unit_test.hpp>
#include <boost/format.hpp>
#include <goliath/vision.h>

using namespace goliath;

class TestVisionFixture {
public:
    TestVisionFixture() : imagePath(GOLIATH_TEST_IMAGE_DIR) {}
    std::string imagePath;
};

BOOST_FIXTURE_TEST_SUITE(BOOST_TEST_MODULE, TestVisionFixture)
    BOOST_AUTO_TEST_CASE(test_line_detection) {
        BOOST_TEST_CHECKPOINT("Loading image");
        cv::Mat input = cv::imread(imagePath + "highway.jpg", CV_BGR2GRAY);

        BOOST_TEST_CHECKPOINT("Creating a canny");
        vision::CannyProcessor cannyProcessor(input, 200, 50);
        cv::Mat cannyImage = cannyProcessor.process();

        BOOST_TEST_CHECKPOINT("Detecting lines");
        vision::LineDetector lineDetector(cannyImage, 125, 50, 2);
        std::vector<cv::Vec4d> lines = lineDetector.detect();

        BOOST_ASSERT(!lines.empty());
    }

    BOOST_AUTO_TEST_CASE(test_color_detection) {
        BOOST_TEST_CHECKPOINT("Loading image");
        cv::Mat input = cv::imread(imagePath + "mondriaan.jpg");
        cv::cvtColor(input, input, CV_BGR2HSV);

        BOOST_TEST_CHECKPOINT("Processing image too only keep reds");
        vision::ColorProcessor colorProcessor(input, cv::Scalar(160, 100, 100), cv::Scalar(179, 255, 255));
        cv::Mat red = colorProcessor.process();

        BOOST_TEST_CHECKPOINT("Check if there is any red in image");
        for(int row = 0; row < red.rows; ++row) {
            for(int col = 0; col < red.cols; ++col) {
                cv::Vec3d pixel = red.at<cv::Vec3d>(row, col);
                if(cv::sum(pixel)[0] > 0) {
                    BOOST_ASSERT(true);
                    return;
                }
            }
        }

        BOOST_ASSERT(false);
    }

    BOOST_AUTO_TEST_CASE(test_roi) {
        BOOST_TEST_CHECKPOINT("Loading image");
        cv::Mat input = cv::imread(imagePath + "mondriaan.jpg");

        BOOST_TEST_CHECKPOINT("Create a ROI");
        vision::RoiProcessor roi(input, 10, 10, 200, 200);

        cv::Mat output = roi.process();
        BOOST_CHECK_EQUAL(output.cols, 200);
        BOOST_CHECK_EQUAL(output.rows, 200);
    }

    BOOST_AUTO_TEST_CASE(test_line_following) {
        BOOST_TEST_CHECKPOINT("Loading image");
        cv::Mat input = cv::imread(imagePath + "line.jpg");

        vision::FollowLineDetector followLineDetector(input, 4, 100, 100, 50, 10, 10000);
        std::vector<cv::Vec4d> output = followLineDetector.detect();

        BOOST_CHECK_EQUAL(output[0][0], vision::FollowLineDirection::RIGHT);
    }

BOOST_AUTO_TEST_SUITE_END()


#include <cassert>
#include <climits>
#include <cstdint>
#include <iostream>

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

#include "FlatIntervalTree.h"

//BOOST_AUTO_TEST_SUITE( flat_interval_tree_test_suite )

#define MIN 0
#define MAX 100
#define FILL 0

BOOST_AUTO_TEST_CASE( normal_ctor1 )
{
    FlatIntervalTree<IntervalEntry_u32_u32> t( MIN, MAX, FILL );

    for ( const auto& v: t.tree ) {
        BOOST_TEST_MESSAGE( "Entry: lower: " << v.lower << ", value:" << v.value);
    }

    BOOST_CHECK( t.tree.size() == 2 );
    BOOST_CHECK( t.max == MAX );
    BOOST_CHECK( t.min == MIN );
    BOOST_CHECK( t.fill == FILL );
    //BOOST_CHECK( t.tree.front().lower == 0 );
    //BOOST_CHECK( t.tree.front().value == 0 );
    //BOOST_CHECK( t.tree.back().lower == SIZE_1D );
    //BOOST_CHECK( t.tree.back().value == 0 );
}

BOOST_AUTO_TEST_CASE( write_single_1 )
{
    FlatIntervalTree<IntervalEntry_u32_u32> t( MIN, MAX, FILL );
    t.write(42, 1);

    BOOST_CHECK( t.tree.size() == 3 );
    BOOST_CHECK( t.tree.size() == 2 );
    BOOST_CHECK( t.max == MAX );
    BOOST_CHECK( t.min == MIN );
    BOOST_CHECK( t.fill == FILL );
    BOOST_CHECK( t.tree.front().lower == MIN );
    BOOST_CHECK( t.tree.front().value == FILL );
    BOOST_CHECK( t.tree.back().lower == MAX );
    BOOST_CHECK( t.tree.back().value == FILL );
}

// int main( int argc, char* argv[] )
//{
// std::vector<float> map( SIZE_1D, 0.0f );
// std::vector<uint32_t> imap( SIZE_1D, UINT_MAX );

// for ( size_t i = 0; i < SIZE_1D; i += 128 ) {
// for ( size_t j = 0; j < 64; ++j ) {
// imap[i + j] = 1;
// map[i + j] = 3;
//}
//}

// for ( size_t i = 0; i < SIZE_1D; i += 64 ) {
// for ( size_t j = 0; j < 64; ++j ) {
// if(imap[i + j] == 1) {
// std::cout << "1";
//} else {
// std::cout << "0";
//}
//}
// std::cout << "\n";
//}

// Plate p( MAPSIDE, map, imap, 1 );

// for ( auto& v: p.occupied.tree  ) {
// std::cout << v.value << ", " << v.lower << "\n";
//}
////for(auto pit = p.begin(); pit != p.end(); ++pit) {
////std::cout << *pit << " ";
////}

//{
// FlatIntervalTree<IntervalEntry_u32_u32> t( 0, SIZE_1D, false );
// for ( auto&& v : t.tree ) {
// std::cout << v.value << ", " << v.lower << "\n";
//}
// std::cout << "*****************************************\n";
// for ( size_t i = 0; i < SIZE_1D; i += 128 ) {
// for ( size_t j = 0; j < 64; ++j ) {
// t.write( i + j, true );
//[> code <]
//}
//}
// for ( auto&& v : t.tree ) {
// std::cout << v.value << ", " << v.lower << "\n";
//}
//}

// return 0;
//}
//BOOST_AUTO_TEST_SUITE_END()


#include "overdrive.h"

#include "render/renderstate.h"
#include "render/shaderprogram.h"
#include "render/vertexbuffer.h"
#include "render/indexbuffer.h"
#include "render/vertexarray.h"
#include "video/video.h"
#include "scene/camera.h"
#include "render/shape_cube.h"
#include "render/shape_sphere.h"
#include "render/texture2d.h"
#include "render/texturecube.h"
#include "render/defaultShaders.h"

#include <iostream>
#include <boost/math/constants/constants.hpp>

#include "util/csv.h"

using namespace overdrive;

class Test :
	public app::Application,
	public MessageHandler<input::Keyboard::OnKeyPress>
{
public:
	int counter = 0;
	render::RenderState mRenderState;
	render::Texture2D mTexture;
	render::ShaderProgram mProgram;

	render::TextureCube mSkyBoxTexture;
	render::ShaderProgram mSkyBoxProgram;
	std::unique_ptr<render::shape::Cube> mSkyBox;

	scene::Camera mCamera;

	float mCameraPitch = 0.0f;
	float mCameraYaw = 0.0f;

	std::unique_ptr<render::shape::Cube> mCube;
	std::unique_ptr<render::shape::Sphere> mSphere;

	Test():
		Application("Test")
	{
		addDependency("Video");
	}

	virtual void initialize() override {
		System::initialize();

		mRenderState.setClearColor(0.2f, 0.2f, 0.0f);
		mRenderState.enable(render::eRenderOptions::DEPTH_TEST);
		mRenderState.apply();

		const char* skybox_vertex_shader = R"(
			#version 400
			uniform mat4 uModel;
			uniform mat4 uView;
			uniform mat4 uProjection;

			in vec3 aVertexPosition;

			smooth out vec3 cubemap_texcoords;

			void main() {
				gl_Position =
					uProjection *
					uView *
					uModel *
					vec4(aVertexPosition, 1.0);

				cubemap_texcoords = aVertexPosition;
			}
		)";

		const char* skybox_fragment_shader = R"(
			#version 400

			uniform samplerCube uCubeMap;
			smooth in vec3 cubemap_texcoords;

			out vec4 outColor;

			void main() {
				outColor = texture(uCubeMap, cubemap_texcoords);
			}
		)";

		mProgram.attachShader(render::DefaultShader<render::attributes::PositionNormalTexCoord>::getVertexShader(), render::eShaderType::VERTEX);
		mProgram.attachShader(render::DefaultShader<render::attributes::PositionNormalTexCoord>::getFragmentShader(), render::eShaderType::FRAGMENT);
		mProgram.link();
		mProgram.bind();

		mSkyBoxProgram.attachShader(skybox_vertex_shader, render::eShaderType::VERTEX);
		mSkyBoxProgram.attachShader(skybox_fragment_shader, render::eShaderType::FRAGMENT);
		mSkyBoxProgram.link();
		mSkyBoxProgram.bind();

		mCamera.setPosition(0.0f, 0.0f, 1.0f);
		mCamera.setProjection(boost::math::float_constants::pi * 0.25f, 1.0f, 0.1f, 100.0f);

		auto mainWindow = mEngine->get<Video>()->getMainWindow();

		mCamera.setViewportWindow(mainWindow);
		mainWindow->getMouse()->setCursorState(input::Mouse::eCursorState::DISABLED); // hide that cursor

		mCube = std::make_unique<render::shape::Cube>(1.0f);
		mSphere = std::make_unique<render::shape::Sphere>();

		mSkyBox = std::make_unique<render::shape::Cube>(
			115.0f // far plane should end up at the center of the skybox cube
		); // [NOTE] the skybox cube should be smaller than the far clipping plane;

		mTexture = render::loadTexture2D("assets/image/test_pattern_001.png");

		mSkyBoxTexture = render::loadTextureCube(
			"assets/image/skybox_px.png",
			"assets/image/skybox_py.png",
			"assets/image/skybox_pz.png",
			"assets/image/skybox_nx.png",
			"assets/image/skybox_ny.png",
			"assets/image/skybox_nz.png"
		);

		//mSkyBoxTexture = render::loadTextureCube("assets/image/skybox_px.png");
		gLog << mProgram;
	}

	virtual void update() override {
		auto kbd = mEngine->get<Input>()->getKeyboardList().front(); // should mainKeyboard be a thing?
		auto mouse = mEngine->get<Input>()->getMouseList().front(); // should mainMouse be a thing?

		// [WASD keyboard controls]
		{
			auto cameraAxes = mCamera.getDirections();

			glm::vec3 deltaPosition(0.0f, 0.0f, 0.0f);

			if ((*kbd)['W'])
				deltaPosition -= cameraAxes.mForward;
			if ((*kbd)['S'])
				deltaPosition += cameraAxes.mForward;
			if ((*kbd)['A'])
				deltaPosition -= cameraAxes.mRight;
			if ((*kbd)['D'])
				deltaPosition += cameraAxes.mRight;
			if ((*kbd)['Q'])
				deltaPosition += cameraAxes.mUp;
			if ((*kbd)['Z'])
				deltaPosition -= cameraAxes.mUp;

			if ((*kbd)[GLFW_KEY_LEFT_SHIFT])
				deltaPosition *= 5;

			float elapsedSeconds = mEngine->getClock().deltaFrame().count() * 0.001f; // deltaframe is in milliseconds
			float movementSpeed = 1.0f * elapsedSeconds;
			deltaPosition *= movementSpeed;

			mCamera.setPosition(mCamera.getPosition() + deltaPosition);
		}

		// [Mouselook] ~ yaw/pitch based
		{
			static std::pair<double, double> previousMousePosition = std::make_pair(0.0, 0.0);
			auto currentPos = mouse->getPosition();

			if ((previousMousePosition.first == 0.0) && (previousMousePosition.second == 0.0))
				previousMousePosition = currentPos;

			float dx = 0.01f * static_cast<float>(currentPos.first - previousMousePosition.first);
			float dy = 0.01f * static_cast<float>(currentPos.second - previousMousePosition.second);
			previousMousePosition = currentPos;

			mCameraYaw += dx;
			mCameraPitch += dy;

			auto yawQ = glm::rotate(mCameraYaw, glm::vec3(0.0f, 1.0f, 0.0f));
			auto pitchQ = glm::rotate(mCameraPitch, glm::vec3(1.0f, 0.0f, 0.0f));

			glm::quat q = glm::quat_cast(pitchQ * yawQ);

			mCamera.setOrientation(q);
		}

		mCamera.update();

		mRenderState.clear();

		// render that skybox
		mSkyBoxProgram.bind();
		mSkyBoxProgram.setUniform("uView", mCamera.getView());
		mSkyBoxProgram.setUniform("uProjection", mCamera.getProjection());
		mSkyBoxProgram.setUniform("uModel", glm::translate(mCamera.getPosition()));

		mSkyBoxTexture.bind(0);
		mSkyBoxProgram.setUniform("uCubeMap", 0);

		mSkyBox->draw();

		// render the cubes
		mProgram.bind();
		mProgram.setUniform("uView", mCamera.getView());
		mProgram.setUniform("uProjection", mCamera.getProjection());

		mProgram.setUniform("uLightDirection", glm::vec4(0.2, -1, 0.5, 1));
		mProgram.setUniform("uLightAmbient", glm::vec4(0.1, 0.1, 0.1, 1.0));
		mProgram.setUniform("uLightDiffuse", glm::vec4(0.85, 0.85, 0.85, 1.0));

		mTexture.bind(0);
		mProgram.setUniform("uTexture", 0);

		// draw the cube at 100 different locations
		for (int i = 0; i < 10; ++i)
			for (int j = 0; j < 10; ++j) {
				mProgram.setUniform("uModel", glm::translate(glm::vec3(2 * (i - 5), 0, 2 * (j - 5))));

				mSphere->draw();
				//mCube->draw();
			}
	}

	virtual void shutdown() override {
		System::shutdown();
	}

	void operator()(const input::Keyboard::OnKeyPress& kp) {
		switch (kp.mKey) {
		case GLFW_KEY_ESCAPE:
			mChannel.broadcast(core::Engine::OnStop());
			break;

		case GLFW_KEY_F1:
			gLog << mProgram;
			break;
		}
	}
};

int main() {
	gLog << "Starting Overdrive Assault";

	core::Engine engine;
	engine.add(new overdrive::Video);
	engine.add(new overdrive::Input);
	engine.setApplication(new Test);

	engine.run();

	gLog << "Done";
}


/* Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 *
 * Copyright 2020 INRIA.
 *
 * 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 "SiconosConfig.h"
#include <boost/numeric/bindings/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/std/vector.hpp>

namespace siconosBindings = boost::numeric::bindings;

// for ublas::axpy_prod, ...
#include <boost/numeric/ublas/operation.hpp>

#include "SiconosVector.hpp"
#include "SimpleMatrix.hpp"
#include "BlockMatrixIterators.hpp"
#include "BlockMatrix.hpp"
#include "SiconosAlgebra.hpp"

using namespace Siconos;

// void axpy_prod(const SiconosMatrix& A, const SiconosVector& x, SiconosVector& y, bool init)
// {
//   // To compute y = A * x ( init = true) or y += A * x (init = false) using ublas::axpy_prod
//   assert(!(A.isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

//   if(A.size(1) != x.size())
//     THROW_EXCEPTION("prod(A,x,y) error: inconsistent sizes between A and x.");

//   if(A.size(0) != y.size())
//     THROW_EXCEPTION("prod(A,x,y) error: inconsistent sizes between A and y.");

//   Siconos::UBLAS_TYPE numA = A.num();
//   Siconos::UBLAS_TYPE numX = x.num();
//   Siconos::UBLAS_TYPE numY = y.num();

//   if(numA == 0)  // If A is Block
//     THROW_EXCEPTION("axpy_prod(A,x,y) error: not yet implemented for block matrices.");

//   if(numA == ZERO)  // A = 0
//   {
//     if(init) y.zero();  // else nothing ...
//   }

//   else if(numA == IDENTITY)  // A = identity
//   {
//     if(!init) y += x;
//     else
//     {
//       if(&x != &y)
//         y = x ; // if x and y do not share memory (ie are different objects)
//     }
//     // else nothing
//   }

//   else // A is not 0 or identity
//   {
//     {
//       {
//         if(&x != &y)  // if no common memory between x and y.
//         {
//           if(numX == DENSE)
//           {
//             if(numY != DENSE)
//               THROW_EXCEPTION("prod(A,x,y) error: y (output) must be a dense vector.");

//             if(numA == DENSE)
//               ublas::axpy_prod(*A.dense(), *x.dense(), *y.dense(), init);
//             else if(numA == TRIANGULAR)
//               ublas::axpy_prod(*A.triang(), *x.dense(), *y.dense(), init);
//             else if(numA == SYMMETRIC)
//               ublas::axpy_prod(*A.sym(), *x.dense(), *y.dense(), init);
//             else if(numA == SPARSE)
//               ublas::axpy_prod(*A.sparse(), *x.dense(), *y.dense(), init);
//             else //if(numA==BANDED)
//               ublas::axpy_prod(*A.banded(), *x.dense(), *y.dense(), init);
//           }
//           else //if(numX == SPARSE)
//           {
//             if(numY != DENSE && numA != SPARSE)
//               THROW_EXCEPTION("axpy_prod(A,x,y) error: y (output) must be a dense vector.");

//             if(numA == DENSE)
//               ublas::axpy_prod(*A.dense(), *x.sparse(), *y.dense(), init);
//             else if(numA == TRIANGULAR)
//               ublas::axpy_prod(*A.triang(), *x.sparse(), *y.dense(), init);
//             else if(numA == SYMMETRIC)
//               ublas::axpy_prod(*A.sym(), *x.sparse(), *y.dense(), init);
//             else if(numA == SPARSE)
//             {
//               if(numY == DENSE)
//                 ublas::axpy_prod(*A.sparse(), *x.sparse(), *y.dense(), init);
//               else
//                 ublas::axpy_prod(*A.sparse(), *x.sparse(), *y.sparse(), init);
//             }
//             else //if(numA==BANDED)
//               ublas::axpy_prod(*A.banded(), *x.sparse(), *y.dense(), init);
//           }
//         }
//         else // if x and y are the same object => alias
//         {
//           if(numX == DENSE)
//           {
//             if(numA == DENSE)
//               ublas::axpy_prod(*A.dense(), *x.dense(), *x.dense(), init);
//             else if(numA == TRIANGULAR)
//               ublas::axpy_prod(*A.triang(), *x.dense(), *x.dense(), init);
//             else if(numA == SYMMETRIC)
//               ublas::axpy_prod(*A.sym(), *x.dense(), *x.dense(), init);
//             else if(numA == SPARSE)
//               ublas::axpy_prod(*A.sparse(), *x.dense(), *x.dense(), init);
//             else //if(numA==BANDED)
//               ublas::axpy_prod(*A.banded(), *x.dense(), *x.dense(), init);
//           }
//           else //if(numX == SPARSE)
//           {
//             if(numA == DENSE)
//               ublas::axpy_prod(*A.dense(), *x.sparse(), *x.sparse(), init);
//             else if(numA == TRIANGULAR)
//               ublas::axpy_prod(*A.triang(), *x.sparse(), *x.sparse(), init);
//             else if(numA == SYMMETRIC)
//               ublas::axpy_prod(*A.sym(), *x.sparse(), *x.sparse(), init);
//             else if(numA == SPARSE)
//               ublas::axpy_prod(*A.sparse(), *x.sparse(), *x.sparse(), init);
//             else //if(numA==BANDED)
//               ublas::axpy_prod(*A.banded(), *x.sparse(), *x.sparse(), init);
//           }
//         }
//       }
//     }
//   }
// }

// void gemvtranspose(double a, const SiconosMatrix& A, const SiconosVector& x, double b, SiconosVector& y)
// {
//   if(A.isBlock())
//     THROW_EXCEPTION("gemv(...) not yet implemented for block vectors or matrices.");
//   assert(!(A.isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

//   Siconos::UBLAS_TYPE numA = A.num();
//   Siconos::UBLAS_TYPE numX = x.num();
//   Siconos::UBLAS_TYPE numY = y.num();
//   if(numA != DENSE || numX != DENSE || numY != DENSE)
//     THROW_EXCEPTION("gemv(...) failed: reserved to dense matrices or vectors.");

//   siconosBindings::blas::gemv(a, siconosBindings::trans(*A.dense()), *x.dense(), b, *y.dense());
// }

// void gemv(double a, const SiconosMatrix& A, const SiconosVector& x, double b, SiconosVector& y)
// {
//   if(A.isBlock())
//     THROW_EXCEPTION("gemv(...) not yet implemented for block vectors or matrices.");
//   assert(!(A.isPLUFactorizedInPlace()) && "A is PLUFactorizedInPlace in prod !!");

//   Siconos::UBLAS_TYPE numA = A.num();
//   Siconos::UBLAS_TYPE numX = x.num();
//   Siconos::UBLAS_TYPE numY = y.num();
//   if(numA != DENSE || numX != DENSE || numY != DENSE)
//     THROW_EXCEPTION("gemv(...) failed: reserved to dense matrices or vectors.");

//   siconosBindings::blas::gemv(a, *A.dense(), *x.dense(), b, *y.dense());
// }


#define BOOST_TEST_MODULE Test

#include "utility/math.hpp"
#include "utility/misc.hpp"
#include "utility/type_traits.hpp"
#include <array>
#include <boost/test/included/unit_test.hpp>
#include <complex>
#include <execution>
#include <functional>
#include <random>
#include <unordered_map>
#include <unordered_set>
#include <utility>

using namespace Utility;

int foo(int x) { return x; };
void bar(int x) { std::cout << x << std::endl; };

BOOST_AUTO_TEST_CASE(arg_traits_test) {
  auto lambda1 = [](std::string const &) { return 0; };
  using T = CallableTraits<decltype(lambda1)>::Type<1>;
  // NOLINTNEXTLINE
  auto lambda2 = [&](T t) { return lambda1(t); };

  BOOST_TEST(CallableTraits<decltype(lambda1)>::isConst<1>);
  BOOST_TEST(CallableTraits<decltype(lambda1)>::isLValueReference<1>);
  BOOST_TEST(CallableTraits<decltype(lambda2)>::isConst<1>);
  BOOST_TEST(CallableTraits<decltype(lambda2)>::isLValueReference<1>);
  BOOST_TEST(CallableTraits<decltype(foo)>::isValue<0>);
  BOOST_TEST(CallableTraits<decltype(foo)>::isValue<1>);
  BOOST_TEST(!CallableTraits<decltype(bar)>::isValue<0>);
}

BOOST_AUTO_TEST_CASE(arg_traits_test_2) {
  auto x = 0;
  // NOLINTNEXTLINE
  auto lambda1 = [x](std::string const &) mutable {
    ++x;
    return 0;
  };
  // NOLINTNEXTLINE
  auto const lambda2 = [x](std::string const &) mutable {
    ++x;
    return 0;
  };
  // NOLINTNEXTLINE
  auto lambda3 = [&](std::string const &) {
    ++x;
    return 0;
  };
  // NOLINTNEXTLINE
  auto const lambda4 = [&](std::string const &) {
    ++x;
    return 0;
  };

  struct AAA {
    void X() const {};
    void Y(){};
  };

  auto func1 = std::function(lambda1);
  auto func2 = std::function(lambda2);
  auto func3 = std::function(lambda3);
  auto func4 = std::function(lambda4);

  BOOST_TEST(!CallableTraits<decltype(lambda1)>::isCallableConst);
  BOOST_TEST(!CallableTraits<decltype(lambda2)>::isCallableConst);
  BOOST_TEST(CallableTraits<decltype(lambda3)>::isCallableConst);
  BOOST_TEST(CallableTraits<decltype(lambda4)>::isCallableConst);
  BOOST_TEST(CallableTraits<decltype(func1)>::isCallableConst);
  BOOST_TEST(CallableTraits<decltype(func2)>::isCallableConst);
  BOOST_TEST(CallableTraits<decltype(func3)>::isCallableConst);
  BOOST_TEST(CallableTraits<decltype(func4)>::isCallableConst);
  BOOST_TEST(CallableTraits<decltype(&AAA::X)>::isCallableConst);
  BOOST_TEST(!CallableTraits<decltype(&AAA::Y)>::isCallableConst);
  BOOST_TEST(CallableTraits<decltype(foo)>::isCallableConst);

  BOOST_TEST(CallableTraits<
                 std::add_rvalue_reference_t<decltype(lambda1)>>::nArguments ==
             size_t{1});
  BOOST_TEST(
      CallableTraits<std::remove_cvref_t<decltype(lambda1)>>::nArguments ==
      size_t{1});
}

BOOST_AUTO_TEST_CASE(arg_traits_test_3) {
  auto lambda1 = [](std::string const &, int, double, int, char) -> int {
    return 0;
  };
  using U1 = typename CallableTraits<decltype(lambda1)>::std_function;
  using T1 = std::function<int(std::string const &, int, double, int, char)>;
  auto lambda2 = [](int, double) {};
  using T2 = std::function<void(int, double)>;
  using U2 = typename CallableTraits<decltype(lambda2)>::std_function;
  BOOST_TEST((std::is_same_v<U1, T1>));
  BOOST_TEST((std::is_same_v<U2, T2>));
}

BOOST_AUTO_TEST_CASE(type_traits_test) {
  BOOST_TEST((isInstanceOf<std::vector, std::vector<int>>));
  BOOST_TEST((!isInstanceOf<std::vector, std::unordered_set<int>>));
  BOOST_TEST((!isInstanceOf<std::vector, int>));
}

BOOST_AUTO_TEST_CASE(perm_test) {
  auto perm = std::vector<size_t>{5, 2, 3, 0, 1, 4};
  auto perm2 = std::vector<size_t>{5, 2, 3, 0, 1, 4};
  auto v = std::vector<size_t>(perm.size());
  std::iota(v.begin(), v.end(), 0);
  Permute(v, perm);
  BOOST_TEST(v == perm);
  BOOST_TEST(v == perm2);
}

BOOST_AUTO_TEST_CASE(utility_test) {
  auto x = std::vector<size_t>{0, 1, 2, 3, 4};
  BOOST_TEST(abs(Mean(x) - 2) < 0.000001);
  BOOST_TEST(abs(RMS(x) - sqrt(6)) < 0.000001);
  BOOST_TEST(abs(Variance(x) - 2) < 0.000001);
  auto fit = LeastSquares(x);
  BOOST_TEST(abs(fit.Slope - 1) < 0.000001);

  auto N = 1001;
  auto v = GetIndices(N);
  BOOST_TEST(abs(Mean(v) - (N - 1) / 2) < 0.000001);
  BOOST_TEST(abs(RMS(v) - sqrt((N - 1) * (2 * N - 1) / 6)) < 0.0001);
  BOOST_TEST(abs(Variance(v) - (N * N - 1) / 12) < 0.000001);

  auto y = std::vector<std::complex<int>>{0, 1, 2, 3, 4};
  BOOST_TEST(abs(Mean(x) - 2) < 0.000001);
  BOOST_TEST(abs(RMS(x) - sqrt(6)) < 0.000001);
  BOOST_TEST(abs(Variance(x) - 2) < 0.000001);
  auto fit2 = LeastSquares(x);
  BOOST_TEST(abs(fit2.Slope - 1) < 0.000001);
}

BOOST_AUTO_TEST_CASE(save_restore) {
  auto perm = std::vector<size_t>{5, 2, 3, 0, 1, 4};
  auto checkperm = std::vector<size_t>{5, 2, 3, 0, 1, 4};
  {
    auto save = SaveRestore(perm);
    perm = std::vector<size_t>{};
  }
  BOOST_TEST(checkperm == perm);

  {
    auto save = SaveRestore(std::move(perm), perm);
    perm = std::vector<size_t>{};
  }
  BOOST_TEST(checkperm == perm);

  {
    auto save = SaveRestore(std::vector<size_t>{5, 2, 3, 0, 1, 4}, perm);
    perm = std::vector<size_t>{};
  }
  BOOST_TEST(checkperm == perm);
  {
    auto save = SaveRestore(std::move(perm));
    perm = std::vector<size_t>{};
  }
  BOOST_TEST(checkperm == perm);

  auto i = 123456;
  auto checki = i;
  {
    auto save = SaveRestore{i};
    i = 100;
  }
  BOOST_TEST(checki = i);

  { auto save = SaveRestore<int>{}; }
  {
    auto save = SaveRestore{i};
    i = 100;
    auto b = std::move(save);
  }
  BOOST_TEST(checki = i);
  {
    auto b = SaveRestore<int>{};
    {
      auto save = SaveRestore{i};
      i = 100;
      b = std::move(save);
    }
    BOOST_TEST(i == 100);
  }
  BOOST_TEST(checki = i);

  auto s1 = SaveRestore(perm);
  auto s2 = SaveRestore(perm);
  static_assert(noexcept(swap(s1, s2)));
}

namespace std {
template <> struct hash<std::pair<const int, double>> {
  size_t operator()(std::pair<const int, double> const &x) const noexcept {
    return std::hash<int>{}(x.first) * std::hash<double>{}(x.second);
  }
};
} // namespace std

BOOST_AUTO_TEST_CASE(exception_guard_test) {
  struct ThrowingStruct {
    int val = 1;
    void ThrowingMethod1() {
      auto g = RAII([]() {}, [&]() noexcept { Clear(); });
      if (val == 10)
        throw std::runtime_error("");
    }
    void Clear() noexcept { val = 0; }
  };
  auto g1 = RAII([]() noexcept {});
  auto s = ThrowingStruct{};
  s.ThrowingMethod1();
  BOOST_TEST(s.val == 1);
  s.val = 10;
  try {
    s.ThrowingMethod1();
  } catch (...) {
  }
  BOOST_TEST(s.val == 0);
}

BOOST_AUTO_TEST_CASE(benchmark_test) {
  struct AAA {
    AAA(size_t) {}
    AAA(AAA const &) = delete;
    AAA(AAA &&) = delete;
    AAA &operator=(AAA const &) = delete;
    AAA &operator=(AAA &&) = delete;
  };
  auto aaa = AAA(size_t{3});
  auto f1 = []() {};
  auto f2 = [](int) {};
  auto f3 = [](int, int) {};
  auto f4 = [](size_t) {};
  auto f5 = [](size_t, size_t) {};
  auto f6 = [](AAA &&, size_t) {};
  auto f7 = [](AAA &, size_t) {};
  auto f8 = [](AAA const &, size_t) {};
  Benchmark(f1);
  Benchmark(f1, size_t{10});
  Benchmark(f2, 3);
  Benchmark(f2, 3, size_t{10});
  Benchmark(f3, 3, 4);
  Benchmark(f3, 3, 4, size_t{10});
  Benchmark(f4, 3);
  Benchmark(f4, 3, size_t{10});
  Benchmark(f5, 3, size_t{4});
  Benchmark(f5, 3, size_t{4}, size_t{10});
  Benchmark(f6, AAA{3}, size_t{4});
  Benchmark(f6, AAA{3}, size_t{4}, size_t{10});
  Benchmark(f7, aaa, size_t{4});
  Benchmark(f7, aaa, size_t{4}, size_t{10});
  Benchmark(f8, AAA{3}, size_t{4});
  Benchmark(f8, AAA{3}, size_t{4}, size_t{10});
}

BOOST_AUTO_TEST_CASE(auto_option_test) {
  using namespace Utility;
  auto t = AutoOption::True();
  auto f = AutoOption::False();
  auto a = AutoOption::Auto();
  BOOST_TEST(t.isTrue());
  BOOST_TEST(f.isFalse());
  BOOST_TEST(!a.isTrue());
  BOOST_TEST(!a.isFalse());
  BOOST_TEST(a.isAuto());
  BOOST_TEST(static_cast<bool>(t == t));
  BOOST_TEST(static_cast<bool>(f == f));
  BOOST_TEST(static_cast<bool>(a == a));

  BOOST_TEST(static_cast<bool>(t == !f));
  BOOST_TEST(static_cast<bool>(f == !t));
  BOOST_TEST(static_cast<bool>(a == !a));
  BOOST_TEST(static_cast<bool>(t == !!t));
  BOOST_TEST(static_cast<bool>(f == !!f));
  BOOST_TEST(static_cast<bool>(a == !!a));

  BOOST_TEST(static_cast<bool>(t != f));
  BOOST_TEST(static_cast<bool>(f != t));
  BOOST_TEST(!static_cast<bool>(t != a));
  BOOST_TEST(!static_cast<bool>(f != a));

  BOOST_TEST(static_cast<bool>(t == true));
  BOOST_TEST(static_cast<bool>(f == false));
  BOOST_TEST(static_cast<bool>(t != false));
  BOOST_TEST(static_cast<bool>(f != true));
  BOOST_TEST(!static_cast<bool>(a != true));
  BOOST_TEST(!static_cast<bool>(a != false));

  BOOST_TEST(static_cast<bool>(t));
  BOOST_TEST(static_cast<bool>(!f));
  BOOST_TEST(!static_cast<bool>(a));
  BOOST_TEST(!static_cast<bool>(!a));
  BOOST_TEST(!static_cast<bool>(!!a));

  auto Impl = [](AutoOption x, AutoOption y) {
    if (x.isFalse() || y.isTrue())
      return AutoOption::True();
    if (x.isAuto() && y.isAuto())
      return AutoOption::True();
    if (x.isTrue() && y.isFalse())
      return AutoOption::False();
    return AutoOption::Auto();
  };
  auto Or = [&](AutoOption x, AutoOption y) { return Impl(Impl(x, y), y); };
  auto And = [&](AutoOption x, AutoOption y) { return !(Or(!x, !y)); };
  auto Eq = [&](AutoOption x, AutoOption y) {
    return And(Impl(x, y), Impl(y, x));
  };

  auto X = std::vector<AutoOption>{t, f, a};
  for (auto x : X)
    for (auto y : X) {
      BOOST_TEST(static_cast<bool>((x || y) == Or(x, y)));
      BOOST_TEST(static_cast<bool>((x && y) == And(x, y)));
      BOOST_TEST(static_cast<bool>((x == y) == Eq(x, y)));
    }

  if (t)
    BOOST_TEST(true);
  else
    BOOST_TEST(false);
  if (f)
    BOOST_TEST(false);
  else
    BOOST_TEST(true);
  if (a)
    BOOST_TEST(false);
  else
    BOOST_TEST(true);
  if (!a)
    BOOST_TEST(false);
  else
    BOOST_TEST(true);
}

BOOST_AUTO_TEST_CASE(exception_handler_test) {
  auto h = ExceptionSaver{10};
  auto i1 = GetIndices(20);

  std::for_each(std::execution::par_unseq, i1.begin(), i1.end(),
                h.Wrap([&](size_t) { throw std::runtime_error{""}; }));

  std::generate(std::execution::par_unseq, i1.begin(), i1.end(),
                h.Wrap([true_cond = !i1.empty()]() {
                  if (true_cond)
                    throw std::runtime_error{""};
                  return 0;
                }));

  while (h.NSavedExceptions()) {
    try {
      h.Rethrow();
    } catch (std::runtime_error &) {
    }
  }
  h.Rethrow();

  auto h1 = ExceptionSaver{10};
  auto h2 = ExceptionSaver{20};
  auto i2 = GetIndices(50);

  std::for_each(std::execution::par_unseq, i1.begin(), i1.end(),
                h1.Wrap([&](size_t) { throw std::exception{}; }));
  std::for_each(std::execution::par_unseq, i2.begin(), i2.end(),
                h2.Wrap([&](size_t) { throw std::exception{}; }));
  h1.swap(h2);
  static_assert(noexcept(swap(h1, h2)));
  BOOST_TEST(h1.NSavedExceptions() == size_t{20});
  BOOST_TEST(h1.NCapturedExceptions() == size_t{50});
  BOOST_TEST(h2.NSavedExceptions() == size_t{10});
  BOOST_TEST(h2.NCapturedExceptions() == size_t{20});
  h1 = std::move(h2);
  BOOST_TEST(h2.NSavedExceptions() == size_t{20});
  BOOST_TEST(h2.NCapturedExceptions() == size_t{50});
  BOOST_TEST(h1.NSavedExceptions() == size_t{10});
  BOOST_TEST(h1.NCapturedExceptions() == size_t{20});

  auto h3 = ExceptionSaver{0};
  std::for_each(std::execution::par_unseq, i1.begin(), i1.end(),
                h3.Wrap([&](size_t) { throw std::exception{}; }));
  h3.Rethrow();
  auto h4 = std::move(h1);
  h1.Rethrow();
  std::for_each(std::execution::par_unseq, i1.begin(), i1.end(),
                h1.Wrap([&](size_t) { throw std::exception{}; }));
  h1.Rethrow();
  BOOST_TEST(h1.NCapturedExceptions() == size_t{20});
  h2.Drop();
  h4.Drop();
}

BOOST_AUTO_TEST_CASE(raii_test) {
  auto copyCnt = size_t{0};
  auto noex = false;
  auto ex = false;
  struct Act {
    size_t &copyCnt;
    bool &act;
    Act(size_t &copyCnt, bool &act) : copyCnt(copyCnt), act(act) {}
    Act(Act const &a) : copyCnt(a.copyCnt), act(a.act) { ++copyCnt; }
    Act(Act &&a) : copyCnt(a.copyCnt), act(a.act) {}
    Act &operator=(Act const &) {
      ++copyCnt;
      return *this;
    }
    Act &operator=(Act &&) { return *this; }
    void operator()() noexcept { act = true; }
  };

  {
    copyCnt = size_t{0};
    noex = false;
    ex = false;
    auto exact = Act(copyCnt, ex);
    auto noexact = Act(copyCnt, noex);
    auto raii = RAII(std::move(noexact), std::move(exact));
  }
  BOOST_TEST(noex == true);
  BOOST_TEST(ex == false);
  BOOST_TEST(copyCnt == size_t{0});

  {
    copyCnt = size_t{0};
    noex = false;
    ex = false;
    auto raii = RAII(Act(copyCnt, noex), Act(copyCnt, ex));
  }
  BOOST_TEST(noex == true);
  BOOST_TEST(ex == false);
  BOOST_TEST(copyCnt == size_t{0});

  {
    copyCnt = size_t{0};
    noex = false;
    ex = false;
    auto raii = RAII(Act(copyCnt, noex));
  }
  BOOST_TEST(noex == true);
  BOOST_TEST(ex == false);
  BOOST_TEST(copyCnt == size_t{0});

  try {
    copyCnt = size_t{0};
    noex = false;
    ex = false;
    auto raii = RAII(Act(copyCnt, noex), Act(copyCnt, ex));
    throw std::runtime_error("");
  } catch (std::exception &) {
  }
  BOOST_TEST(noex == false);
  BOOST_TEST(ex == true);
  BOOST_TEST(copyCnt == size_t{0});

  try {
    copyCnt = size_t{0};
    noex = false;
    ex = false;
    auto raii = RAII(Act(copyCnt, ex));
    throw std::runtime_error("");
  } catch (std::exception &) {
  }
  BOOST_TEST(noex == false);
  BOOST_TEST(ex == true);
  BOOST_TEST(copyCnt == size_t{0});

  struct Restore {
    int *restoreTo;
    int value;
    Restore(int &restoreTo, int value) : restoreTo{&restoreTo}, value{value} {}
    void operator()() noexcept { *restoreTo = value; }
  };

  auto i = 123456;
  auto checki = i;
  {
    i = 100;
    auto restore = std::function([&]() noexcept { i = checki; });
    auto g = RAII<Restore>{};
    {
      auto r = RAII{Restore{i, checki}};
      r = std::move(g);
    }
    BOOST_TEST(i == 100);
    auto x = std::move(g);
  }
  BOOST_TEST(i == checki);

  auto r1 = RAII{Restore{i, checki}};
  auto r2 = RAII{Restore{i, checki}};
  r1.swap(r2);
  static_assert(noexcept(swap(r1, r2)));

  {
    i = 100;
    auto r = RAII{[&]() noexcept { i = checki; }, CallAlways{}};
  }
  BOOST_TEST(i == checki);

  {
    i = 100;
    auto r = RAII{[&]() noexcept { i = checki; }, CallOnException{}};
  }
  BOOST_TEST(i == 100);
  i = checki;

  try {
    i = 100;
    auto r = RAII{[&]() noexcept { i = checki; }, CallOnException{}};
    throw std::exception{};
  } catch (...) {
  }
  BOOST_TEST(i == checki);

  try {
    i = 100;
    auto r = RAII{[&]() {
                    i = checki;
                    throw std::exception{};
                  },
                  [&]() noexcept {}};
  } catch (...) {
  }
  BOOST_TEST(i == checki);
}

BOOST_AUTO_TEST_CASE(type_traits_forward_test) {
  auto F = [](std::string, std::string &, std::string &&, std::string const &,
              std::string const, std::string const &&) {};

  auto a = std::string("a");
  auto b = std::string("b");
  auto c = std::string("c");
  auto d = std::string("d");
  auto e = std::string("e");
  auto f = std::string("f");

  using T = CallableTraits<decltype(F)>;

  F(T::Forward<1>(a), T::Forward<2>(b), T::Forward<3>(c), T::Forward<4>(d),
    T::Forward<5>(e), T::Forward<6>(f));
  BOOST_TEST(a == "");
  BOOST_TEST(b == "b");
  BOOST_TEST(c == "c");
  BOOST_TEST(d == "d");
  BOOST_TEST(e == "");
  BOOST_TEST(f == "f");
}

BOOST_AUTO_TEST_CASE(sort_test) {
  auto x = std::vector<int>{5, 2, 1, 4, 3};
  auto sorted = x;
  std::sort(sorted.begin(), sorted.end());
  auto p = GetSortPermutation(x);
  Permute(x, p);
  BOOST_TEST(x == sorted);

  Permute(x, p, [](size_t const &i) noexcept { return i; });
}

BOOST_AUTO_TEST_CASE(random_test) {
  auto N = 1000;
  auto d = 0.0;
  auto rd = std::random_device{};

  Benchmark([&]() { d += rd(); }, N);
  auto gentime = Benchmark(
      [&]() {
        auto gen = std::mt19937{};
        d += gen();
      },
      N);
  auto staticgentime = Benchmark(
      [&]() {
        auto &&gen = GetRandomGenerator();
        d += gen();
      },
      N);
  BOOST_TEST((gentime / staticgentime > 10));
  auto v = std::vector<int>{};
  std::generate(std::execution::par_unseq, v.begin(), v.end(), []() {
    auto &&gen = GetRandomGenerator();
    auto rand = std::uniform_int_distribution<int>(1);
    return rand(gen);
  });
}

BOOST_AUTO_TEST_CASE(enum_iterator_test) {
  using namespace Utility;
  enum class SomeEnum { A, B, C, D, E, F };
  auto h1 = SomeEnum::A;
  auto h2 = operator++<SomeEnum, SomeEnum::F>(h1);
  BOOST_TEST((h1 == SomeEnum::B));
  BOOST_TEST((h2 == SomeEnum::B));
  h1 = SomeEnum::F;
  h2 = operator++<SomeEnum, SomeEnum::F>(h1, 0);
  BOOST_TEST((h1 == SomeEnum::A));
  BOOST_TEST((h2 == SomeEnum::F));
  h1 = SomeEnum::A;
  h2 = operator--<SomeEnum, SomeEnum::F>(h1);
  BOOST_TEST((h1 == SomeEnum::F));
  BOOST_TEST((h2 == SomeEnum::F));
  h1 = SomeEnum::F;
  h2 = operator--<SomeEnum, SomeEnum::F>(h1, 0);
  BOOST_TEST((h1 == SomeEnum::E));
  BOOST_TEST((h2 == SomeEnum::F));

  auto cnt = 0;
  for (auto h = SomeEnum::F;
       operator++<SomeEnum, SomeEnum::F>(h) != SomeEnum::F;)
    ++cnt;
  BOOST_TEST(cnt == static_cast<int>(SomeEnum::F));
  cnt = 0;
  for (auto h = SomeEnum::F;
       operator--<SomeEnum, SomeEnum::F>(h) != SomeEnum::F;)
    ++cnt;
  BOOST_TEST(cnt == static_cast<int>(SomeEnum::F));
}

template <typename NoExceptionCallable, typename ExceptionCallable>
class RAII2 final {
  static_assert(std::is_nothrow_invocable_v<ExceptionCallable>);
  static_assert(!std::is_reference_v<NoExceptionCallable>);
  NoExceptionCallable callNoException;
  ExceptionCallable callException;

public:
  RAII2(NoExceptionCallable &&callAlways)
      : callNoException(std::move(callAlways)), callException(callNoException) {
    static_assert(std::is_reference_v<ExceptionCallable>);
  }

  RAII2(NoExceptionCallable &&callNoException,
        ExceptionCallable &&callException)
      : callNoException(std::move(callNoException)),
        callException(std::move(callException)) {}

  RAII2(RAII2 const &) = delete;
  RAII2(RAII2 &&) = delete;
  RAII2 &operator=(RAII2 const &) = delete;
  RAII2 &operator=(RAII2 &&) = delete;

  ~RAII2() noexcept(std::is_nothrow_invocable_v<NoExceptionCallable>) {
    if (!std::uncaught_exceptions())
      callNoException();
    else
      callException();
  }
};
template <typename NoExceptionCallable>
RAII2(NoExceptionCallable &&)
    -> RAII2<std::remove_reference_t<NoExceptionCallable>,
             std::add_lvalue_reference_t<NoExceptionCallable>>;

template <class Policy>
concept CallPolicy = std::is_same_v<Policy, CallAlways> ||
    std::is_same_v<Policy, CallOnException>;
class RAII3 final {
  std::function<void()> callNormally;
  std::function<void()> callOnException;

public:
  RAII3() noexcept {};
  template <std::invocable Callable, CallPolicy Policy = CallAlways>
  RAII3(Callable &&callable, Policy = CallAlways{}) {
    static_assert(std::is_nothrow_invocable_v<Callable>);
    if constexpr (std::is_same_v<Policy, CallAlways>)
      callNormally = std::forward<Callable>(callable);
    else
      callOnException = std::forward<Callable>(callable);
  }

  template <std::invocable Callable, std::invocable CallableOnException>
  RAII3(Callable &&callNormally, CallableOnException &&callOnException)
      : callNormally(std::forward<Callable>(callNormally)),
        callOnException(std::forward<CallableOnException>(callOnException)) {
    static_assert(std::is_nothrow_invocable_v<CallableOnException>);
  }

  RAII3(RAII3 const &) = delete;
  RAII3(RAII3 &&other) noexcept { swap(other); }
  RAII3 &operator=(RAII3 const &) = delete;
  RAII3 &operator=(RAII3 &&other) noexcept {
    swap(other);
    return *this;
  }

  void swap(RAII3 &other) noexcept {
    std::swap(callNormally, other.callNormally);
    std::swap(callOnException, other.callOnException);
  }

  ~RAII3() {
    if (!std::uncaught_exceptions())
      CallNormally();
    else
      CallOnException();
  }

private:
  void CallNormally() {
    if (callNormally)
      callNormally();
  }
  void CallOnException() noexcept {
    if (callOnException)
      callOnException();
    else if (callNormally)
      callNormally();
  }
};

BOOST_AUTO_TEST_CASE(raii_bench_test) {
  constexpr static auto n = 2;
  struct S {
    std::array<int, n> v;
    int hash = 0;
    S() { std::iota(v.begin(), v.end(), 0); }
    int operator()() noexcept {
      auto res = 0;
      std::transform(v.begin(), v.end(), v.begin(),
                     [](int i) { return (i + 1) % n; });
      for (auto i = 0; i != n; ++i)
        res = v[res];
      hash += res;
      return hash;
    }
  };

  auto raii = [s = S{}]() noexcept { RAII(S{s}); };
  auto raii2 = [s = S{}]() noexcept { RAII2(S{s}); };
  auto raii3 = [s = S{}]() noexcept { RAII3(S{s}); };
  auto s = S{};
  auto ol = std::optional(s);
  auto f = std::function(s);

  auto lt = Benchmark(s, 1000);
  auto ft = Benchmark(f, 1000);
  auto olt = Benchmark(ol.value(), 1000);
  auto rt = Benchmark(raii, 1000);
  auto rt2 = Benchmark(raii2, 1000);
  auto rt3 = Benchmark(raii3, 1000);

  auto x = static_cast<double>(rt.count());
  auto y = static_cast<double>(rt2.count());
  auto overhead = (x - y) / y;
  BOOST_TEST((rt2.count() == size_t{0} || overhead < 100));

  if (s()) {
    std::cout << "Hash:   " << s() << ol.value().hash << f() << std::endl;
    std::cout << "Lambda test:   " << lt.count() << std::endl;
    std::cout << "Optional Lambda test: " << olt.count() << std::endl;
    std::cout << "Function test: " << ft.count() << std::endl;
    std::cout << "RAII test: " << rt.count() << std::endl;
    std::cout << "RAII2 test: " << rt2.count() << std::endl;
    std::cout << "RAII3 test: " << rt3.count() << std::endl;
  }
}

template <typename T> class SaveRestore2 final {
  static_assert(std::is_nothrow_move_assignable_v<T>);
  static_assert(!std::is_reference_v<T>);
  struct Restore {
    T originalValue;
    T *restoreTo = nullptr;
    void operator()() noexcept { *restoreTo = std::move(originalValue); }
  };
  RAII<Restore> restore;

public:
  constexpr SaveRestore2() noexcept {};
  explicit SaveRestore2(T &value) noexcept(
      std::is_nothrow_copy_constructible_v<T>)
      : restore{{T{std::as_const(value)}, &value}} {}
  explicit SaveRestore2(T &&value) noexcept(
      std::is_nothrow_move_constructible_v<T>)
      : restore{{std::move(value), &value}} {}
  explicit SaveRestore2(T &&value, T &restoreTo) noexcept(
      std::is_nothrow_move_constructible_v<T>)
      : restore{{std::move(value), &restoreTo}} {}
};

BOOST_AUTO_TEST_CASE(saverestore_bench_test) {
  auto v = 1000;
  auto vcheck = v;
  auto hash = size_t{0};

  auto sr = [&]() {
    {
      auto s = SaveRestore(v);
      v = 0;
      hash += v;
    }
    hash += v;
  };
  auto sr2 = [&]() {
    {
      auto s = SaveRestore2(v);
      v = 0;
      hash += v;
    }
    hash += v;
  };
  auto srt = Benchmark(sr, 1000);
  auto srt2 = Benchmark(sr2, 1000);
  BOOST_TEST(v == vcheck);

  auto x = static_cast<double>(srt.count());
  auto y = static_cast<double>(srt2.count());
  auto overhead = (x - y) / y;
  BOOST_TEST((srt2.count() == size_t{0} || overhead < 1));

  if (hash == 0) {
    std::cout << "Hash:   " << hash << std::endl;
    std::cout << "SaveRestore test:  " << srt.count() << std::endl;
    std::cout << "SaveRestore2 test: " << srt2.count() << std::endl;
  }
}


/*! @file pitts_tensor2_eigen_adaptor.hpp
* @brief Get an Eigen::Map for a simple PITTS::Tensor2
* @author Melven Roehrig-Zoellner <Melven.Roehrig-Zoellner@DLR.de>
* @date 2019-10-31
* @copyright Deutsches Zentrum fuer Luft- und Raumfahrt e. V. (DLR), German Aerospace Center
*
**/

// include guard
#ifndef PITTS_TENSOR2_EIGEN_ADAPTOR_HPP
#define PITTS_TENSOR2_EIGEN_ADAPTOR_HPP

// includes
#include "pitts_tensor2.hpp"
#pragma GCC push_options
#pragma GCC optimize("no-unsafe-math-optimizations")
#include <Eigen/Dense>
#pragma GCC pop_options

//! namespace for the library PITTS (parallel iterative tensor train solvers)
namespace PITTS
{
  //! Get a const Eigen::Map for a Tensor2
  //!
  //! @tparam T  underlying data type (double, complex, ...)
  //!
  template<typename T>
  auto ConstEigenMap(const Tensor2<T>& t2)
  {
    using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
    return Eigen::Map<const Matrix>(&t2(0,0), t2.r1(), t2.r2());
  }

  //! Get a mutable Eigen::Map for a Tensor2
  //!
  //! @tparam T  underlying data type (double, complex, ...)
  //!
  template<typename T>
  auto EigenMap(Tensor2<T>& t2)
  {
    using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
    return Eigen::Map<Matrix>(&t2(0,0), t2.r1(), t2.r2());
  }
}

#endif // PITTS_TENSOR2_EIGEN_ADAPTOR_HPP


#ifndef SDF_LOADER_UTIL_HPP
#define SDF_LOADER_UTIL_HPP

#include <cstdlib>
#include <iostream>
#include <regex>

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/range/adaptor/indexed.hpp>

namespace sdfl
{
std::map<std::string, chrono::ChColor> presets = {
    {"Gazebo/Orange", chrono::ChColor(1, 0.5088, 0.0468)},
    {"Gazebo/Grey", chrono::ChColor(0.2, 0.2, 0.2)},
    {"Gazebo/Black", chrono::ChColor(0, 0, 0)},
};

std::shared_ptr<chrono::ChColorAsset> get_color_by_name(std::string const& name);
std::shared_ptr<chrono::ChColorAsset> get_color_by_name(std::string const& name)
{
  return std::make_shared<chrono::ChColorAsset>(presets[name]);
}

template <typename T>
std::vector<std::string> split(std::string const& src, T pat)
{
  std::vector<std::string> result{};
  boost::algorithm::split(result, src, boost::is_any_of(pat));
  return result;
}

template <typename T>
std::tuple<T, T, T> destruct_three(std::string const& value)
{
  auto const list = split(value, " ");
  if (list.size() != 3) {
    throw std::runtime_error("Not three pair but " + std::to_string(list.size()));
  }

  return std::make_tuple(boost::lexical_cast<T>(list[0]), boost::lexical_cast<T>(list[1]),
                         boost::lexical_cast<T>(list[2]));
}

template <typename T = double>
std::tuple<chrono::ChVector<T>, chrono::ChMatrix33<T>> destruct_pose(std::string const& pose)
{
  chrono::ChVector<T> pos;
  chrono::ChVector<T> rot;

  auto const list = split(pose, " ");
  for (auto const& val : list | boost::adaptors::indexed()) {
    auto const idx = val.index();
    T dval;
    try {
      dval = boost::lexical_cast<T>(val.value());
    } catch (boost::bad_lexical_cast&) {
      throw std::runtime_error("Invalid value in pose: " + val.value());
    }
    if (idx < 3) {
      pos[idx] = dval;
    } else {
      rot[idx - 3] = dval;
    }
  }

  return std::make_tuple(pos, chrono::ChMatrix33(rot));
}

template <typename T = double>
decltype(auto) get_pose(sdf::ElementPtr e)
{
  auto elem = e->GetElement("pose");
  if (!elem) {
    throw std::runtime_error("No pose found");
  }
  auto const str = elem->GetValue()->GetAsString();
  return destruct_pose<T>(str);
}

template <typename T = double>
std::tuple<T, T, T> get_box_size(sdf::ElementPtr e)
{
  auto elem = e->GetElement("geometry");
  if (!elem) {
    throw std::runtime_error("No geometry found");
  }
  if (!elem->HasElement("box")) {
    throw std::runtime_error("not a box");
  }
  auto size_e = elem->GetElement("box")->GetElement("size");
  if (!size_e) {
    throw std::runtime_error("No size in box");
  }
  auto const str = size_e->GetValue()->GetAsString();
  return destruct_three<T>(str);
}

template <typename T = double>
chrono::ChMatrix33<T> get_inertia(sdf::ElementPtr e)
{
  if (!e->HasElement("inertia")) {
    throw std::runtime_error("No inertia found");
  }
  auto elem = e->GetElement("inertia");
  T ixx, ixy, ixz, iyy, iyz, izz;
  elem->GetElement("ixx")->GetValue()->Get<T>(ixx);
  elem->GetElement("ixy")->GetValue()->Get<T>(ixy);
  elem->GetElement("ixz")->GetValue()->Get<T>(ixz);
  elem->GetElement("iyy")->GetValue()->Get<T>(iyy);
  elem->GetElement("iyz")->GetValue()->Get<T>(iyz);
  elem->GetElement("izz")->GetValue()->Get<T>(izz);
  return chrono::ChMatrix33<T>(ixx, ixy, ixz, ixy, iyy, iyz, ixz, iyz, izz);
}

template <typename T>
void print_vec(chrono::ChVector<T> vec, std::ostream& os)
{
  os << '(' << vec.x() << ',' << vec.y() << ',' << vec.z() << ')';
}

}  // namespace sdfl

#endif  // SDF_LOADER_UTIL_HPP


#include "parameters.h"
#include "shell_solid_solver.h"
#include "utilities.h"
#include <deal.II/grid/grid_in.h>

int main(int argc, char *argv[])
{
  using namespace dealii;

  try
    {
      std::string infile("parameters.prm");
      if (argc > 1)
        {
          infile = argv[1];
        }
      std::string meshfile("solid.gmsh");
      if (argc > 2)
        {
          meshfile = argv[2];
        }
      std::string forcefile("solid_f");
      if (argc > 3)
        {
          forcefile = argv[3];
        }

      Parameters::AllParameters params(infile);
      if (params.dimension == 3)
        {
          libMesh::LibMeshInit libmesh_init(argc, argv);
          Triangulation<2, 3> tria;
          GridIn<2, 3> gridin;
          gridin.attach_triangulation(tria);
          std::ifstream input_solid(meshfile);
          gridin.read_msh(input_solid);
          Solid::ShellSolidSolver solid(tria, params, &libmesh_init);
          std::cout << "Reading forcefile: " << forcefile << std::endl;
          solid.get_forcing_file(forcefile);
          solid.run();
        }
      else
        {
          AssertThrow(false, ExcNotImplemented());
        }
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  return 0;
}


// (C) Copyright David James Pearce 2005. Permission to copy, use,
// modify, sell and distribute this software is granted provided this
// copyright notice appears in all copies. This software is provided
// "as is" without express or implied warranty, and with no claim as
// to its suitability for any purpose.
//
// Email: d.pearce@mcs.vuw.ac.nz

#include <iostream>
#include <iomanip>
#include <sstream>
#include <fstream>
#include <cstdlib>
#include <getopt.h>
#include <sys/time.h>

#include <set>
#include <vector>
#include <algorithm>
#include <numeric>

// unsure why this is needed
// #define BOOST_NO_STDC_NAMESPACE

#include <boost/lexical_cast.hpp>
#include <boost/random.hpp>
#include "range.hpp"
#include "random_graph.hpp"

using namespace std;
using namespace boost;

#define MAJOR_VERSION 0
#define MINOR_VERSION 6

#define OPT_HELP 0
#define OPT_VERSION 1
#define OPT_VERBOSE 2
#define OPT_NODES 3
#define OPT_EDGES 4
#define OPT_ACYCLIC 5
#define OPT_OUTDEGREE 6
#define OPT_DENSITY 8
#define OPT_COUNT 10
#define OPT_BINARY 11
#define OPT_OUTPUT 12
#define OPT_PRECISION 13

bool find_replace(string &str, string const &match, string const &replace) {
  unsigned int pos(0);
  string r;

  if((pos=str.find(match,pos)) < str.size()) {
    str.replace(pos,match.size(),replace);
    return true;
  }
  return false;
}

unsigned int range2nedges(range<double> r, unsigned int V, unsigned int fmt) {
  switch(fmt) {
  case OPT_EDGES:
    return (unsigned int) (r.value());
    break;
  case OPT_OUTDEGREE:
    return (unsigned int) (r.value() * V);
    break;
  case OPT_DENSITY:
    return (unsigned int) (r.value() * 0.5 * V * (V-1));
    break;
  default:
    throw runtime_error("invalid conversion specified");
  }
}

string double2str(double d, unsigned int precision) {
  ostringstream s;
  s << setprecision(precision) << d;
  return s.str();
}

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

  struct option long_options[]={
    {"help",no_argument,NULL,OPT_HELP},
    {"version",no_argument,NULL,OPT_VERSION},
    {"verbose",no_argument,NULL,OPT_VERBOSE},
    {"nodes",required_argument,NULL,OPT_NODES},
    {"edges",required_argument,NULL,OPT_EDGES},
    {"outdegree",required_argument,NULL,OPT_OUTDEGREE},
    {"density",required_argument,NULL,OPT_DENSITY},
    {"binary",required_argument,NULL,OPT_BINARY},
    {"file",required_argument,NULL,OPT_OUTPUT},
    {"precision",required_argument,NULL,OPT_PRECISION},
    {"ngraphs",required_argument,NULL,OPT_COUNT},
    {"acyclic",no_argument,NULL,OPT_ACYCLIC},
    NULL
  };

  char *descriptions[]={
    " -h     --help                    display this information",
    " -v     --version                 display version information",
    "        --verbose                 show addition information",
    " -b     --binary                  produce binary output",
    " -n<x>  --ngraphs                 generate <x> graphs",
    " -v<x>  --nodes=<x>               set value of V (default=50)",
    " -e<x>  --edges=<x>               set value of E (default=50)",
    " -d<x>  --outdegree=<x>           set expected initial outdegree",
    "                                  determintes the probility p to use in the graph",
    "                                  generator and does not *guarantee* an exact outdegree",
    "                                  of x",
    "        --density=<x>             set the ratio of expected actual edges versus the maximum",
    "                                  number of possible edges.",
    " -f<x>  --file=<x>                set output file to x",
    "        --precision=<x>           set precision on output filenames to x",
    "        --acyclic                 generate acyclic graphs only",
    "",
    NULL
  };

  bool verbose = false, binary = false;
  bool acyclic = false;
  double over(0.0005);
  unsigned int over_fixed = 0;
  range<double> Er;
  range<unsigned int> Vr;
  unsigned int conversion = 0;
  unsigned int precision = 5;
  unsigned int count = 1;
  string outfile("graphs-%V-%E-%N.dat");

  typedef boost::mt19937 UniformRandomNumberGenerator;

  UniformRandomNumberGenerator urng;

  // seed the random number generator
  struct timeval tmp;
  gettimeofday(&tmp,NULL);
  urng.seed((tmp.tv_sec * 1000000) + tmp.tv_usec);

  try {
    // parse command line arguments
    char v;
    while((v=getopt_long(argc,argv,"hv:n:d:e:o:bf:",long_options,NULL)) != -1) {
      switch(v)
	{
	case 'h':
	case OPT_HELP:
	  cout << "usage: " << argv[0] << " [options]" << endl;
	  cout << "options:" << endl;
	  for(char **ptr=descriptions;*ptr != NULL; ptr++) {
	    cout << *ptr << endl;
	  }
	  exit(1);
	case OPT_VERSION:
	  cout << "graphgen v" << MAJOR_VERSION << "." << MINOR_VERSION << endl;
	  cout << "\tWritten by David J. Pearce, January 2005" << endl;
	  exit(1);
	case OPT_VERBOSE:
	  verbose = true;
	  break;
	case 'v':
	case OPT_NODES:
	  Vr = range<unsigned int>(optarg);
	  break;
	case 'e':
	case OPT_EDGES:
	  Er = range<unsigned int>(optarg);
	  conversion = OPT_EDGES;
	  break;
	case 'o':
	case OPT_OUTDEGREE:
	  Er = range<double>(optarg);
	  conversion = OPT_DENSITY;
	  break;
	case 'd':
	case OPT_DENSITY:
	  Er = range<double>(optarg);
	  conversion = OPT_DENSITY;
	  break;
	case 'n':
	case OPT_COUNT:
	  count = atoi(optarg);
	  break;
	case 'b':
	case OPT_BINARY:
	  binary = true;
	  break;
	case 'f':
	case OPT_OUTPUT:
	  outfile = optarg;
	  break;
	case OPT_PRECISION:
	  precision = atoi(optarg);
	  break;
	case OPT_ACYCLIC:
	  acyclic = true;
	  break;
	default:
	  // getopt will print invalid argument for us
	  exit(1);
	}
    }

    // Now, generate the graphs!
    do {
      unsigned int V = Vr.value();

      do {

	boost::random_number_generator<UniformRandomNumberGenerator,unsigned int> rng(urng);
	vector<pair<unsigned int, unsigned int> > edges;
	unsigned int E = range2nedges(Er,V,conversion);

	// create filename
	string tmp(outfile);
	string El("e");
	if(conversion == OPT_DENSITY) { El = "d"; }
	else if(conversion == OPT_DENSITY) { El = "o"; }
	while(find_replace(tmp,"%V",string("v") + lexical_cast<string>(V)));
	while(find_replace(tmp,"%E",El + double2str(Er.value(),precision)));
	while(find_replace(tmp,"%N",string("n") + lexical_cast<string>(count)));
	ofstream output(tmp.c_str());

	for(int i=0;i!=count;++i) {
	  if(verbose) {
	    float p = ((double) i) / count;
	    cerr << "\rv = " << V << ", " << El << " = " << Er.value() << ": Completed " << (p*100) << "%              ";
	  }
	  // generate random edgelist.  Ensure that we always
	  // generate at least E edges.
	  while(E > edges.size()) {
	    edges.clear();
	    if(acyclic) {
	      random_acyclic_edgelist(V,E,edges,urng);
	    } else {
	      random_edgelist(V,E,edges,urng);
	    }
	  }

	  // generate random permutation of edge list
	  random_shuffle(edges.begin(),edges.end(),rng);

	  // now print the graph.
	  if(binary) {
	    int i = 2, j = 0;
	    int array[512];
	    array[0] = V;
	    array[1] = edges.size();
	    while(j < edges.size()) {
	      for(;i!=512 && j != edges.size();++i,++j) {
		unsigned int w = edges[j].first;
		w = (w << 16) | (edges[j].second & 0xFFFF);
		array[i] = w;
	      }
	      // write the block
	      output.write((char*)array,i*4);
	      i=0;
	    }
	    output.flush();
	  } else {
	    output << "V=" << V << endl;
	    output << "E={";
	    for(int j=0;j!=edges.size();++j) {
	      if(j!=0) { output << ","; }
	      output << edges[j].first << ">" << edges[j].second;
	    }
	    output << "}" << endl;
	  }
	  edges.clear();
	}
	if(verbose) { cout <<  endl; }
      } while(!Er.step());
    } while(!Vr.step());
  } catch(bad_alloc e) {
    cout << "Out of memory (" << e.what() << ")" << endl;
  } catch(runtime_error &e) {
    cout << "Internal failure - " << e.what() << endl;
    exit(1);
  } catch (...) {
    cout << "Internal failure" << endl;
    exit(1);
  }

  fflush(stdout);

  exit(0);
}


// Copyright 2018 Peter Dimov
// 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/mp11.hpp>
using namespace boost::mp11;

int main()
{
    using L1 = mp_list<int, float, int, float>;
    return mp_size<mp_unique<L1>>::value == 2? 0: 1;
}


#include "data/cross_sections/material_cross_sections.hpp"

#include <unordered_map>
#include <vector>

#include <gtest/gtest.h>
#include <deal.II/lac/full_matrix.h>

#include "problem/parameters_dealii_handler.hpp"
#include "data/material/tests/material_mock.hpp"
#include "test_helpers/bart_test_helper.h"
#include "test_helpers/gmock_wrapper.h"
#include "test_helpers/test_helper_functions.h"

#include "data/cross_sections/tests/cross_sections_mock.hpp"

namespace {

using namespace bart;

class CrossSectionsTest : public ::testing::Test {
 protected:
  using id_vector_map = std::unordered_map<int, std::vector<double>>;
  using id_matrix_map = std::unordered_map<int, dealii::FullMatrix<double>>;
  ::testing::NiceMock<data::material::MaterialMock> mock_material_properties;
};

class CrossSectionsTestConstructor : public CrossSectionsTest {
 protected:
  void SetUp() override;
  id_vector_map diffusion_coef_map = test_helpers::RandomIntVectorMap();
  id_vector_map sigma_t_map = test_helpers::RandomIntVectorMap();
  id_vector_map sigma_t_inv_map = test_helpers::RandomIntVectorMap();
  id_vector_map q_map = test_helpers::RandomIntVectorMap();
  id_vector_map q_per_ster_map = test_helpers::RandomIntVectorMap();
  id_vector_map nu_sigf_map = test_helpers::RandomIntVectorMap();
  id_matrix_map sig_s_map = test_helpers::RandomIntMatrixMap();
  id_matrix_map sig_s_per_ster_map = test_helpers::RandomIntMatrixMap();
  id_matrix_map chi_nu_sig_f_map = test_helpers::RandomIntMatrixMap();
  id_matrix_map chi_nu_sig_f_per_ster_map = test_helpers::RandomIntMatrixMap();
  std::unordered_map<int, bool> fissile_id_map{{1, true}, {2, false}};
};

void CrossSectionsTestConstructor::SetUp() {
  ON_CALL(mock_material_properties, GetDiffusionCoef()).WillByDefault(::testing::Return(diffusion_coef_map));
  ON_CALL(mock_material_properties, GetSigT()).WillByDefault(::testing::Return(sigma_t_map));
  ON_CALL(mock_material_properties, GetInvSigT()).WillByDefault(::testing::Return(sigma_t_inv_map));
  ON_CALL(mock_material_properties, GetQ()).WillByDefault(::testing::Return(q_map));
  ON_CALL(mock_material_properties, GetQPerSter()).WillByDefault(::testing::Return(q_per_ster_map));
  ON_CALL(mock_material_properties, GetNuSigF()).WillByDefault(::testing::Return(nu_sigf_map));
  ON_CALL(mock_material_properties, GetSigS()).WillByDefault(::testing::Return(sig_s_map));
  ON_CALL(mock_material_properties, GetSigSPerSter()).WillByDefault(::testing::Return(sig_s_per_ster_map));
  ON_CALL(mock_material_properties, GetChiNuSigF()).WillByDefault(::testing::Return(chi_nu_sig_f_map));
  ON_CALL(mock_material_properties, GetChiNuSigFPerSter()).WillByDefault(::testing::Return(chi_nu_sig_f_per_ster_map));
  ON_CALL(mock_material_properties, GetFissileIDMap()).WillByDefault(::testing::Return(fissile_id_map));
}

TEST_F(CrossSectionsTestConstructor, CrossSectionsConstructor) {
  bart::data::cross_sections::MaterialCrossSections test_xsections(mock_material_properties);
  EXPECT_EQ(test_xsections.diffusion_coef(), diffusion_coef_map);
  EXPECT_EQ(test_xsections.sigma_t(), sigma_t_map);
  EXPECT_EQ(test_xsections.inverse_sigma_t(), sigma_t_inv_map);
  EXPECT_EQ(test_xsections.q(), q_map);
  EXPECT_EQ(test_xsections.q_per_ster(), q_per_ster_map);
  EXPECT_EQ(test_xsections.nu_sigma_f(), nu_sigf_map);
  EXPECT_EQ(test_xsections.sigma_s(), sig_s_map);
  EXPECT_EQ(test_xsections.sigma_s_per_ster(), sig_s_per_ster_map);
  EXPECT_EQ(test_xsections.fiss_transfer(), chi_nu_sig_f_map);
  EXPECT_EQ(test_xsections.fiss_transfer_per_ster(), chi_nu_sig_f_per_ster_map);
  EXPECT_EQ(test_xsections.is_material_fissile(), fissile_id_map);
}

} // namespace