/*!
 * @file CCommandLine.cpp
 * @brief The main subroutines for handling command line input and output.
 * @author S.Ramon (seraco)
 * @version 0.0.1
 *
 * Copyright 2018 S.Ramon
 *
 * 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.
 */

#ifndef __CCOMMANDLINE_CPP
#define __CCOMMANDLINE_CPP

#include <iostream>
#include <fstream>
#include <string>
#include <boost/program_options.hpp>
#include <mpi.h>

#include "../include/CCommandLine.hpp"

CCommandLine::CCommandLine(int argc, char *argv[]) {
    int rank;
    int nRanks;
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);
    MPI_Comm_size(MPI_COMM_WORLD, &nRanks);

    /*--- Print solver logo and version. ---*/
    if (rank == 0) {
        std::cout << R"(
             _       _    _            _                 _
            / /\    / /\ /\ \         / /\              /\ \         _          _
           / / /   / / //  \ \       / /  \             \_\ \       /\ \       /\ \
          / /_/   / / // /\ \ \     / / /\ \            /\__ \   ___\ \_\   ___\ \_\
         / /\ \__/ / // / /\ \_\   / / /\ \ \          / /_ \ \ /___/\/_/_ /___/\/_/_
        / /\ \___\/ // /_/_ \/_/  / / /  \ \ \        / / /\ \ \\__ \/___/\\__ \/___/\
       / / /\/___/ // /____/\    / / /___/ /\ \      / / /  \/_/  /\/____\/  /\/____\/
      / / /   / / // /\____\/   / / /_____/ /\ \    / / /         \ \_\      \ \_\
     / / /   / / // / /______  / /_________/\ \ \  / / /           \/_/       \/_/
    / / /   / / // / /_______\/ / /_       __\ \_\/_/ /
    \/_/    \/_/ \/__________/\_\___\     /____/_/\_\/

        )" << "\n";
        std::cout << "Version 0.0.1\n\n";
    }

    /*--- Controll command line parameters with boost library. ---*/
    namespace po = boost::program_options;
    po::options_description desc("Program options");
    desc.add_options()
        ("help", "produce this help message")
        ("a-constant,A", po::value<double>(), "a constant for height distribution")
        ("left-height", po::value<double>(), "height at the left edge")
        ("right-height", po::value<double>(), "height at the right edge")
        ("length,L", po::value<double>(), "length of the plate")
        ("thickness,T", po::value<double>(), "thickness of the plate")
        ("k-xx", po::value<double>(), "conductivity in the xx direction")
        ("k-xy", po::value<double>(), "conductivity in the xy direction")
        ("k-yy", po::value<double>(), "conductivity in the yy direction")
        ("n-x", po::value<unsigned>(), "number of elements in x direction")
        ("n-y", po::value<unsigned>(), "number of elements in y direction")
        ("flux-location", po::value<std::string>(), "location of flux BC")
        ("flux-value", po::value<double>(), "value of flux BC")
        ("temp-location", po::value<std::string>(), "location of temperature BC")
        ("temp-value", po::value<double>(), "value of temperature BC");
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    /*--- If help parameter is passed show help information. ---*/
    if (vm.count("help")) {
        if (rank == 0) std::cout << desc;
        ableToRun = false;

    /*--- If all the parameters required are specified, the program is able to
          run. ---*/
    } else if (
        vm.count("a-constant") &&
        vm.count("left-height") &&
        vm.count("right-height") &&
        vm.count("length") &&
        vm.count("thickness") &&
        vm.count("k-xx") &&
        vm.count("k-xy") &&
        vm.count("k-yy") &&
        vm.count("n-x") &&
        vm.count("n-y") &&
        vm.count("flux-location") &&
        vm.count("flux-value") &&
        vm.count("temp-location") &&
        vm.count("temp-value")
    ) {
        aConst = vm["a-constant"].as<double>();
        leftHeight = vm["left-height"].as<double>();
        rightHeight = vm["right-height"].as<double>();
        length = vm["length"].as<double>();
        thickness = vm["thickness"].as<double>();
        kXX = vm["k-xx"].as<double>();
        kXY = vm["k-xy"].as<double>();
        kYY = vm["k-yy"].as<double>();
        Nx = vm["n-x"].as<unsigned>();
        Ny = vm["n-y"].as<unsigned>();
        fluxLocation = vm["flux-location"].as<std::string>();
        fluxValue = vm["flux-value"].as<double>();
        tempLocation = vm["temp-location"].as<std::string>();
        tempValue = vm["temp-value"].as<double>();
        ableToRun = true;

    /*--- If not all the required parameters are specified, show the help
          information. ---*/
    } else {
        if (rank == 0) {
            std::cout << "All the command line arguments should be specified\n\n";
            std::cout << desc;
        }
        ableToRun = false;
    }
}

CCommandLine::~CCommandLine() {}

double CCommandLine::getAConst() const {
    return aConst;
}

double CCommandLine::getLeftHeight() const {
    return leftHeight;
}

double CCommandLine::getRightHeight() const {
    return rightHeight;
}

double CCommandLine::getLength() const {
    return length;
}

double CCommandLine::getThickness() const {
    return thickness;
}

double CCommandLine::getKXX() const {
    return kXX;
}

double CCommandLine::getKXY() const {
    return kXY;
}

double CCommandLine::getKYY() const {
    return kYY;
}

unsigned CCommandLine::getNx() const {
    return Nx;
}

unsigned CCommandLine::getNy() const {
    return Ny;
}

std::string CCommandLine::getFluxLocation() const {
    return fluxLocation;
}

double CCommandLine::getFluxValue() const {
    return fluxValue;
}

std::string CCommandLine::getTempLocation() const {
    return tempLocation;
}

double CCommandLine::getTempValue() const {
    return tempValue;
}

bool CCommandLine::getAbleToRun() const {
    return ableToRun;
}

#endif


#include <ros/ros.h>
#include <ros/console.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <librealsense/rs.hpp>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/Image.h>
#include <opencv2/core/core.hpp>
#include <sensor_msgs/CameraInfo.h>
#include <camera_info_manager/camera_info_manager.h>
#include <math.h>
#include <thread>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <boost/algorithm/string.hpp>

namespace rs
{
  /// \brief Video stream intrinsics
  struct my_intrinsics : rs_intrinsics
  {
      float       hfov() const                                                        { return (atan2f(ppx + 0.5f, fx) + atan2f(width - (ppx + 0.5f), fx)) * 57.2957795f; }
      float       vfov() const                                                        { return (atan2f(ppy + 0.5f, fy) + atan2f(height - (ppy + 0.5f), fy)) * 57.2957795f; }
      distortion  my_model() const                                                       { return (distortion)rs_intrinsics::model; }

                  // Helpers for mapping between pixel coordinates and texture coordinates
      float2      pixel_to_texcoord(const float2 & pixel) const                       { return {(pixel.x+0.5f)/width, (pixel.y+0.5f)/height}; }
      float2      texcoord_to_pixel(const float2 & coord) const                       { return {coord.x*width - 0.5f, coord.y*height - 0.5f}; }

                  // Helpers for mapping from image coordinates into 3D space
      float3      deproject(const float2 & pixel, float depth) const                  { float3 point = {}; rs_deproject_pixel_to_point(&point.x, this, &pixel.x, depth); return point; }
      float3      deproject_from_texcoord(const float2 & coord, float depth) const    { return deproject(texcoord_to_pixel(coord), depth); }

                  // Helpers for mapping from 3D space into image coordinates
      float2      project(const float3 & point) const                                 { float2 pixel = {}; rs_project_point_to_pixel(&pixel.x, this, &point.x); return pixel; }
      float2      project_to_texcoord(const float3 & point) const                     { return pixel_to_texcoord(project(point)); }

      bool        operator == (const intrinsics & r) const                            { return memcmp(this, &r, sizeof(r)) == 0; }

  };
}

std::string camera_name;

rs::my_intrinsics depth_intrin;
rs::extrinsics depth_to_color;
rs::my_intrinsics color_intrin;

ros::Publisher depth_registered_pub;
ros::Publisher depth_registered_camera_info_pub;
ros::Publisher rgb_sync_depth_registered_pub;
ros::Publisher rgb_sync_depth_registered_camera_info_pub;

double x_param, y_param, z_param, yaw_param, pitch_param, roll_param;

// sensor_msgs::CameraInfo rgb_sync_depth_registered_camera_info, depth_camera_info;

// cv::Mat to sensor_msgs::Image
sensor_msgs::ImagePtr cv_mat_to_sensor_msgs_image(cv::Mat &image, std::string encoding, std::string frame) {
    static int header_sequence_id = 0;

    std_msgs::Header header;

    header.seq = header_sequence_id++;
    header.stamp = ros::Time::now();
    header.frame_id = frame;

    sensor_msgs::ImagePtr a;

    try {
        cv_bridge::CvImage::Ptr cv_msg_ptr = cv_bridge::CvImage::Ptr(new cv_bridge::CvImage(header, encoding, image));
        a = cv_msg_ptr->toImageMsg();
    } catch (cv_bridge::Exception& e) {
        ROS_ERROR("cv_mat_to_sensor_msgs_image FAILED with: %s", e.what());
    }

    return a;
}

// sensor_msgs::Image to cv::Mat
cv_bridge::CvImagePtr sensor_msgs_image_to_cv_mat(const sensor_msgs::Image& msg, std::string encoding) {
    cv_bridge::CvImagePtr cv_im;
    try {
        cv_im = cv_bridge::toCvCopy(msg, encoding);
    }
    catch(cv_bridge::Exception& e) {
        ROS_ERROR("sensor_msgs_image_to_cv_mat FAILED with: %s", e.what());
    }
    return cv_im;
}

void callback(const sensor_msgs::Image& depth_raw_msg, const sensor_msgs::CameraInfo& depth_camera_info_msg, const sensor_msgs::Image& rgb_rect_msg, const sensor_msgs::CameraInfo& rgb_camera_info_msg) {

    sensor_msgs::CameraInfo rgb_sync_depth_registered_camera_info = rgb_camera_info_msg;
    sensor_msgs::CameraInfo depth_camera_info = depth_camera_info_msg;
    sensor_msgs::Image rgb_sync_depth_registered_msg = rgb_rect_msg;

    color_intrin.model = RS_DISTORTION_NONE;
    color_intrin.height = rgb_sync_depth_registered_camera_info.height;
    color_intrin.width = rgb_sync_depth_registered_camera_info.width;

    color_intrin.fx=rgb_sync_depth_registered_camera_info.K[0];
    color_intrin.fy=rgb_sync_depth_registered_camera_info.K[4];
    color_intrin.ppx=rgb_sync_depth_registered_camera_info.K[2];
    color_intrin.ppy=rgb_sync_depth_registered_camera_info.K[5];
    color_intrin.coeffs[0]=rgb_sync_depth_registered_camera_info.D[0];
    color_intrin.coeffs[1]=rgb_sync_depth_registered_camera_info.D[1];
    color_intrin.coeffs[2]=rgb_sync_depth_registered_camera_info.D[2];
    color_intrin.coeffs[3]=rgb_sync_depth_registered_camera_info.D[3];
    color_intrin.coeffs[4]=rgb_sync_depth_registered_camera_info.D[4];

    depth_intrin.model = RS_DISTORTION_INVERSE_BROWN_CONRADY;
    depth_intrin.height = depth_camera_info.height;
    depth_intrin.width = depth_camera_info.width;

    depth_intrin.fx=depth_camera_info.K[0];
    depth_intrin.fy=depth_camera_info.K[4];
    depth_intrin.ppx=depth_camera_info.K[2];
    depth_intrin.ppy=depth_camera_info.K[5];
    depth_intrin.coeffs[0]=depth_camera_info.D[0];
    depth_intrin.coeffs[1]=depth_camera_info.D[1];
    depth_intrin.coeffs[2]=depth_camera_info.D[2];
    depth_intrin.coeffs[3]=depth_camera_info.D[3];
    depth_intrin.coeffs[4]=depth_camera_info.D[4];

    // Transform depth to rgb
    // x y z yaw pitch roll
    // -0.02426723 -0.00047571 -0.00023367 -0.0001231  -0.01946618 -0.01579435
    // angle's in degrees to support online converter here: http://danceswithcode.net/engineeringnotes/rotations_in_3d/demo3D/rotations_in_3d_tool.html
    // -0.00705311045808 -1.115329957245 -0.9049495951547

    // double yaw = -0.0001231;
    // double pitch = -0.01946618;
    // double roll = -0.01579435;
    // yaw in ros is roll in realsense
    // pitch in ros is yaw in realsense
    // roll in ros is pitch in realsense

    // Current camera is one used to collect dataset
    double pitch = roll_param;
    double yaw = pitch_param;
    double roll = yaw_param;
    // double yaw = roll_param;
    // double pitch = pitch_param;
    // double roll = yaw_param;
    // double roll = roll_param;
    // double pitch = pitch_param;
    // double yaw = yaw_param;

    double x = x_param;
    double y = y_param;
    double z = z_param;

    // Flip from ros to realsense - I guess convention is different
    x = -x;
    y = -y;
    z = -z;

    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitX());

    // NOTE flipping the unit axes as below didn't work
    // Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitY());
    // Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitX());
    // Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitZ());

    // Y is yaw
    // Z is roll
    // X is pitch

    Eigen::Quaternion<double> q = rollAngle * pitchAngle * yawAngle;

    Eigen::Matrix3d rotationMatrix = q.matrix();

    // ROS_INFO_STREAM(rotationMatrix);

    depth_to_color.rotation[0]=rotationMatrix(0,0);
    depth_to_color.rotation[1]=rotationMatrix(0,1);
    depth_to_color.rotation[2]=rotationMatrix(0,2);
    depth_to_color.rotation[3]=rotationMatrix(1,0);
    depth_to_color.rotation[4]=rotationMatrix(1,1);
    depth_to_color.rotation[5]=rotationMatrix(1,2);
    depth_to_color.rotation[6]=rotationMatrix(2,0);
    depth_to_color.rotation[7]=rotationMatrix(2,1);
    depth_to_color.rotation[8]=rotationMatrix(2,2);
    // depth_to_color.rotation[0]=0.9998;depth_to_color.rotation[1]=0.0004;depth_to_color.rotation[2]=-0.0195;
    // depth_to_color.rotation[3]=-0.0002;depth_to_color.rotation[4]=0.9998;depth_to_color.rotation[5]=0.0157;
    // depth_to_color.rotation[6]=0.0194;depth_to_color.rotation[7]=-0.0158;depth_to_color.rotation[8]=0.9996;
    // depth_to_color.translation[0]=0.0239998574975702;
    // depth_to_color.translation[1]=-0.0000631696874657;
    // depth_to_color.translation[2]=0.0002111893155177;
    depth_to_color.translation[0]=x;
    depth_to_color.translation[1]=y;
    depth_to_color.translation[2]=z;

    cv_bridge::CvImagePtr depth_cv_ptr = sensor_msgs_image_to_cv_mat(depth_raw_msg, sensor_msgs::image_encodings::TYPE_16UC1);

    cv::Mat depth_registered_cv(color_intrin.height, color_intrin.width, CV_32FC1);

    // Pre-fill image with 0's to account for pixels that fall outside of depth image sweep
    for (int h=0;h<color_intrin.height;h++) {
        for (int w=0;w<color_intrin.width;w++) {
            depth_registered_cv.at<float>(cv::Point(w,h)) = 0;
        }
    }

    for (int dy=0; dy<depth_intrin.height; ++dy){
        for (int dx=0; dx<depth_intrin.width; ++dx){
            // Obtain the depth value and apply scale factor
            uint16_t depth_value = depth_cv_ptr->image.at<uint16_t>(cv::Point(dx,dy));
            float depth_in_meters = depth_value / 1000.0;

            // Map from pixel coordinates in the depth image to pixel coordinates in the color image
            rs::float2 depth_pixel = {(float)dx, (float)dy};
            rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters);
            rs::float3 color_point = depth_to_color.transform(depth_point);
            rs::float2 color_pixel = color_intrin.project(color_point);

            // Choose the nearest pixel in the color image to the reprojected depth pixel
            // Ignore this point if it falls outside the color image
            const int cx = (int)std::round(color_pixel.x), cy = (int)std::round(color_pixel.y);
            if (!(cx < 0 || cy < 0 || cx >= color_intrin.width || cy >= color_intrin.height))
            {
                depth_registered_cv.at<float>(cv::Point(cx,cy)) = depth_in_meters;
            }
        }
    }

    sensor_msgs::ImagePtr depth_registered_msg_ptr = cv_mat_to_sensor_msgs_image(depth_registered_cv, sensor_msgs::image_encodings::TYPE_32FC1, camera_name + "_rgb_optical_frame");

    // Update timestamps
    ros::Time time_now = ros::Time::now();
    depth_registered_msg_ptr->header.stamp = time_now;
    rgb_sync_depth_registered_camera_info.header.stamp = time_now;
    rgb_sync_depth_registered_msg.header.stamp = time_now;

    depth_registered_pub.publish(*depth_registered_msg_ptr);
    depth_registered_camera_info_pub.publish(rgb_sync_depth_registered_camera_info);
    rgb_sync_depth_registered_pub.publish(rgb_sync_depth_registered_msg);
    rgb_sync_depth_registered_camera_info_pub.publish(rgb_sync_depth_registered_camera_info);
}

int main(int argc, char * argv[]) {
    ros::init(argc, argv, "register_depth_node");
    ros::NodeHandle n("~");

    if (n.getParam("camera_name", camera_name)) {
        ROS_INFO_STREAM("\"camera_name\" = " << camera_name);
    } else {
        ROS_ERROR_STREAM("\"camera_name\" not provided.");
        return 0;
    }

    std::string xyzypr;
    std::vector<std::string> xyzypr_vector;
    if (n.getParam("xyzypr", xyzypr)) {
        ROS_INFO_STREAM("\"xyzypr\" = " << xyzypr);
        boost::split(xyzypr_vector, xyzypr, boost::is_any_of(" "));
    } else {
        ROS_ERROR_STREAM("\"xyzypr\" not provided.");
        return 0;
    }

    x_param = std::stod(xyzypr_vector[0]);
    y_param = std::stod(xyzypr_vector[1]);
    z_param = std::stod(xyzypr_vector[2]);
    yaw_param = std::stod(xyzypr_vector[3]);
    pitch_param = std::stod(xyzypr_vector[4]);
    roll_param = std::stod(xyzypr_vector[5]);

    ROS_INFO_STREAM("xyzypr = " << x_param
                    << ", " << y_param
                    << ", " << z_param
                    << ", " << yaw_param
                    << ", " << pitch_param
                    << ", " << roll_param);

    message_filters::Subscriber<sensor_msgs::Image> depth_image_raw_sub(n, "/" + camera_name + "/depth/image_raw", 5);
    message_filters::Subscriber<sensor_msgs::CameraInfo> depth_camera_info_sub(n, "/" + camera_name + "/depth/camera_info", 5);
    message_filters::Subscriber<sensor_msgs::Image> rgb_image_rect_sub(n, "/" + camera_name + "/rgb/image_raw", 5);
    message_filters::Subscriber<sensor_msgs::CameraInfo> rgb_camera_info_sub(n, "/" + camera_name + "/rgb/camera_info", 5);

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;

    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(5), depth_image_raw_sub, depth_camera_info_sub, rgb_image_rect_sub, rgb_camera_info_sub);
    sync.registerCallback(callback);

    depth_registered_pub = n.advertise<sensor_msgs::Image>("/" + camera_name + "/depth_registered/image_rect", 30);
    depth_registered_camera_info_pub = n.advertise<sensor_msgs::CameraInfo>("/" + camera_name + "/depth_registered/camera_info", 30);
    rgb_sync_depth_registered_pub = n.advertise<sensor_msgs::Image>("/" + camera_name + "/rgb_sync_depth_registered/image_rect", 30);
    rgb_sync_depth_registered_camera_info_pub = n.advertise<sensor_msgs::CameraInfo>("/" + camera_name + "/rgb_sync_depth_registered/camera_info", 30);

    ros::Rate r(30);

    // Continuously publish rgb image and depth_inpainted image
    while(ros::ok()) {
        ros::spinOnce();
        r.sleep();
    }
}


//   Copyright: Copyright (C) 2005, Jan Ringos, http://Tringi.Mx-3.cz
//   Version: 1.1
//
//  Copyright (c) 2014 Hartmut Kaiser
//
//  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)

#ifndef INT128_HPP
#define INT128_HPP

#include <hpx/hpx_fwd.hpp>

#include <exception>
#include <cstdlib>
#include <cstdio>
#include <new>

#include <boost/cstdint.hpp>
#include <boost/serialization/serialization.hpp>
#include <boost/serialization/split_member.hpp>

namespace hpx { namespace util { namespace numerics
{
    ///////////////////////////////////////////////////////////////////////////
    class HPX_EXPORT int128
    {
    private:
        // Binary correct representation of signed 128bit integer
        boost::uint64_t lo;
        boost::int64_t hi;

    protected:
        // Some global operator functions must be friends
        friend bool operator <  (const int128 &, const int128 &) throw ();
        friend bool operator == (const int128 &, const int128 &) throw ();
        friend bool operator || (const int128 &, const int128 &) throw ();
        friend bool operator && (const int128 &, const int128 &) throw ();

    public:
        // Constructors
        inline int128 () throw () {};

        inline int128 (const unsigned int & a) throw () : lo (a), hi (0ll) {};
        inline int128 (const signed int & a) throw () : lo (a), hi (0ll) {
            if (a < 0) this->hi = -1ll;
        };

        inline int128 (const boost::uint64_t & a) throw () : lo (a), hi (0ll) {};
        inline int128 (const boost::int64_t & a) throw () : lo (a), hi (0ll) {
            if (a < 0) this->hi = -1ll;
        };

        int128 (const float a) throw ();
        int128 (const double & a) throw ();
        int128 (const long double & a) throw ();

        int128 (const char * sz) throw ();

        // TODO: Consider creation of operator= to eliminate
        //       the need of intermediate objects during assignments.

    private:
        // Special internal constructors
        int128 (const boost::uint64_t & a, const boost::int64_t & b) throw ()
            : lo (a), hi (b) {};

    public:
        // Operators
        bool operator ! () const throw ();

        int128 operator - () const throw ();
        int128 operator ~ () const throw ();

        int128 & operator ++ ();
        int128 & operator -- ();
        int128 operator ++ (int);
        int128 operator -- (int);

        int128 & operator += (const int128 & b) throw ();
        int128 & operator *= (const int128 & b) throw ();

        int128 & operator >>= (unsigned int n) throw ();
        int128 & operator <<= (unsigned int n) throw ();

        int128 & operator |= (const int128 & b) throw ();
        int128 & operator &= (const int128 & b) throw ();
        int128 & operator ^= (const int128 & b) throw ();

        // Inline simple operators
        inline const int128 & operator + () const throw () { return *this; };

        // Rest of inline operators
        inline int128 & operator -= (const int128 & b) throw () {
            return *this += (-b);
        };
        inline int128 & operator /= (const int128 & b) throw () {
            int128 dummy;
            *this = this->div (b, dummy);
            return *this;
        };
        inline int128 & operator %= (const int128 & b) throw () {
            this->div (b, *this);
            return *this;
        };

        // Common methods
        int toInt () const throw () { return (int) this->lo; };
        boost::int64_t toInt64 () const throw ()
            { return (boost::uint64_t) this->lo; };

        const char * toString (unsigned int radix = 10) const throw ();
        float toFloat () const throw ();
        double toDouble () const throw ();
        long double toLongDouble () const throw ();

        // Arithmetic methods
        int128  div (const int128 &, int128 &) const throw ();

        // Bit operations
        bool    bit (unsigned int n) const throw ();
        void    bit (unsigned int n, bool val) throw ();

    private:
        friend class boost::serialization::access;

        BOOST_SERIALIZATION_SPLIT_MEMBER()

        template <typename Archive>
        void save(Archive& ar, const unsigned int version) const;
        template <typename Archive>
        void load(Archive& ar, const unsigned int version);
    }
#ifdef __GNUC__
    __attribute__ ((__aligned__ (16), __packed__))
#endif
    ;

    // GLOBAL OPERATORS

    bool operator <  (const int128 & a, const int128 & b) throw ();
    bool operator == (const int128 & a, const int128 & b) throw ();
    bool operator || (const int128 & a, const int128 & b) throw ();
    bool operator && (const int128 & a, const int128 & b) throw ();

    // GLOBAL OPERATOR INLINES

    inline int128 operator + (const int128 & a, const int128 & b) throw () {
        return int128 (a) += b; };
    inline int128 operator - (const int128 & a, const int128 & b) throw () {
        return int128 (a) -= b; };
    inline int128 operator * (const int128 & a, const int128 & b) throw () {
        return int128 (a) *= b; };
    inline int128 operator / (const int128 & a, const int128 & b) throw () {
        return int128 (a) /= b; };
    inline int128 operator % (const int128 & a, const int128 & b) throw () {
        return int128 (a) %= b; };

    inline int128 operator >> (const int128 & a, unsigned int n) throw () {
        return int128 (a) >>= n; };
    inline int128 operator << (const int128 & a, unsigned int n) throw () {
        return int128 (a) <<= n; };

    inline int128 operator & (const int128 & a, const int128 & b) throw () {
        return int128 (a) &= b; };
    inline int128 operator | (const int128 & a, const int128 & b) throw () {
        return int128 (a) |= b; };
    inline int128 operator ^ (const int128 & a, const int128 & b) throw () {
        return int128 (a) ^= b; };

    inline bool operator >  (const int128 & a, const int128 & b) throw () {
        return   b < a; };
    inline bool operator <= (const int128 & a, const int128 & b) throw () {
        return !(b < a); };
    inline bool operator >= (const int128 & a, const int128 & b) throw () {
        return !(a < b); };
    inline bool operator != (const int128 & a, const int128 & b) throw () {
        return !(a == b); };
}}}

// MISC

// typedef hpx::util::numerics::int128 __int128_t;

#endif


#pragma once

#include <vector>
#include <Eigen/Geometry>

#include "pipelinemodule.hpp"
#include "types.h"
#include "pointcloud.h"

namespace PoseEstimation
{
    /**
     * @brief Abstract module for transformation estimation between two point clouds.
     */
    template<typename PointT, typename DescriptorT>
    class TransformationEstimator : public PipelineModule
    {
    public:
        TransformationEstimator() : PipelineModule(PipelineModuleType::TransformationEstimator)
        {}

        /**
         * @brief Estimates the rigid transformation of the source to the target point cloud
         * using the correspondences of descriptors calculated at the keypoints of both
         * point clouds.
         * @param source The source point cloud.
         * @param target The target point cloud.
         * @param source_keypoints The keypoints of the source point cloud at which
         * descriptors were calculated.
         * @param target_keypoints The keypoints of the target point cloud at which
         * descriptors were calculated.
         * @param source_features The descriptors of the source point cloud.
         * @param target_features The descriptors of the target point cloud.
         * @param correspondences The correspondences between source and target descriptors.
         * @param transformations The resulting 4x4 transformation matrix of the estimated
         * rigid transformation.
         * @return Whether the estimation was successfull.
         */
        // not every transformation estimator uses all of these arguments
        virtual bool estimate(PC<PointT> &source,
                              PC<PointT> &target,
                              const typename pcl::PointCloud<PointT>::Ptr &source_keypoints,
                              const typename pcl::PointCloud<PointT>::Ptr &target_keypoints,
                              const typename pcl::PointCloud<DescriptorT>::Ptr &source_features,
                              const typename pcl::PointCloud<DescriptorT>::Ptr &target_features,
                              const pcl::CorrespondencesPtr &correspondences,
                              std::vector<Eigen::Matrix4f> &transformations) = 0;
    };
}


/*
 [auto_generated]
 libs/numeric/odeint/test/prepare_stepper_testing.hpp

 [begin_description]
 This file defines some helper functions for the stepper tests.
 [end_description]

 Copyright 2009-2012 Karsten Ahnert
 Copyright 2009-2012 Mario Mulansky

 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)
 */

#ifndef PREPARE_STEPPER_TESTING_HPP_
#define PREPARE_STEPPER_TESTING_HPP_

#include <boost/array.hpp>
#include <vector>
#include <complex>

#include <boost/fusion/sequence.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>

#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>
#include <boost/numeric/odeint/algebra/array_algebra.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>

namespace mpl = boost::mpl;
namespace fusion = boost::fusion;

using namespace boost::numeric::odeint;

/* the state types that will be tested */
typedef std::vector< double > vector_type;
typedef std::vector< std::complex<double> > complex_vector_type;
typedef double vector_space_type;
typedef boost::array< double , 1 > array_type;
typedef boost::array< std::complex<double> , 1 > complex_array_type;

typedef boost::multiprecision::cpp_dec_float_50 mp_type;
typedef boost::array< mp_type , 1 > mp_array_type;

typedef mpl::vector< vector_type , complex_vector_type , vector_space_type ,
                     array_type , complex_array_type , mp_type , mp_array_type
                     >::type container_types;

namespace boost { namespace numeric { namespace odeint {

    // mp_type forms a vector space
    template<>
    struct algebra_dispatcher< mp_type >
    {
        typedef vector_space_algebra algebra_type;
    };

    // add norm computation
    template<>
    struct vector_space_norm_inf< mp_type >
    {
        typedef mp_type result_type;
        mp_type operator()( mp_type x ) const
        {
            using std::abs;
            return abs(x);
        }
    };

} } }

#endif /* PREPARE_STEPPER_TESTING_HPP_ */


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

  Copyright 2015 NumScale SAS
  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/rem_pio2_straight.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/constant/pio_2.hpp>

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

  using bs::rem_pio2_straight;
  using iT = bd::as_integer_t<T>;
  using r_t = decltype(rem_pio2_straight(T()));

  STF_EXPR_IS( (rem_pio2_straight(T()))
                  , (std::pair<iT,T>)
                  );

  {
    T r1;
    STF_EQUAL( rem_pio2_straight(bs::Pio_2<T>(), r1), bs::One<iT>());
    STF_ULP_EQUAL( r1, bs::Zero<T>(), 0.5);
    STF_EQUAL( rem_pio2_straight(bs::Pio_4<T>(), r1), bs::One<iT>());
    STF_ULP_EQUAL( r1, -bs::Pio_4<T>(), 0.5);
  }
  {
    r_t res = rem_pio2_straight(bs::Pio_4<T>());
    STF_ULP_EQUAL( res.first, bs::One<iT>(), 0.5);
    STF_ULP_EQUAL( res.second, -bs::Pio_4<T>(), 0.5);
  }
  {
    r_t res = rem_pio2_straight(bs::Zero<T>());
    STF_ULP_EQUAL( res.first, bs::One<iT>(), 0.5);
    STF_ULP_EQUAL( res.second, -bs::Pio_2<T>(), 0.5);
  }

}


/*
 *            Copyright 2009-2018 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      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 "A_ol I_ol" BA_olI_ol,
 * WITHOUT WARRANTIE_ol OR CONDITION_ol OF ANY KIND, either express or implied.
 * _olee the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <boost/math/constants/constants.hpp>
#include "votca/xtp/radial_euler_maclaurin_rule.h"
#include "votca/xtp/aobasis.h"
#include "votca/xtp/qmatom.h"
#include <votca/xtp/aomatrix.h>

namespace votca { namespace xtp {

    std::vector<double> EulerMaclaurinGrid::CalculatePruningIntervals(const std::string & element){
        std::vector<double> r;
        // get Bragg-Slater Radius for this element
        double BSradius = _BraggSlaterRadii.at(element);
        // row type of element
        int RowType = _pruning_set.at(element);

        if ( RowType == 1 ){
            r.push_back( 0.25 * BSradius );
            r.push_back( 0.5 * BSradius );
            r.push_back( 1.0 * BSradius );
            r.push_back( 4.5 * BSradius );
        } else if ( RowType == 2 ){
            r.push_back( 0.1667 * BSradius );
            r.push_back( 0.5 * BSradius );
            r.push_back( 0.9 * BSradius );
            r.push_back( 3.5 * BSradius );
        } else if ( RowType == 3 ) {
            r.push_back( 0.1 * BSradius );
            r.push_back( 0.4 * BSradius );
            r.push_back( 0.8 * BSradius );
            r.push_back( 2.5 * BSradius );
        } else {
          throw std::runtime_error("EulerMaclaurinGrid::CalculatePruningIntervals:Pruning unsupported for RowType");
        }
        return r;
     }

    void EulerMaclaurinGrid::FillElementRangeMap(const AOBasis& aobasis, std::vector<QMAtom*>& atoms, double eps){
      std::map<std::string, min_exp>::iterator it;
      for (QMAtom* atom:atoms) {
        std::string name = atom->getType();
        // is this element already in map?
        it = _element_ranges.find(name);
        // only proceed, if element data does not exist yet
        if (it == _element_ranges.end()) {
          min_exp this_atom;
          double range_max = std::numeric_limits<double>::min();
          double decaymin = std::numeric_limits<double>::max();
          int    lvalue = std::numeric_limits<int>::min();
          const std::vector<const AOShell*>  shells=aobasis.getShellsofAtom(atom->getAtomID());
          // and loop over all shells to figure out minimum decay constant and angular momentum of this function
          for (const AOShell* shell:shells) {
            int lmax = shell->getLmax();
            if (shell->getMinDecay() < decaymin) {
              decaymin =shell->getMinDecay();
              lvalue = lmax;
            }
            double range = DetermineCutoff(2 * decaymin, 2 * lvalue + 2, eps);
            if (range > range_max) {
              this_atom.alpha = decaymin;
              this_atom.l = lvalue;
              this_atom.range = range;
              range_max = range;
            }
          } // shells
          _element_ranges[name] = this_atom;
        } // new element
      } // atoms
    }

    void EulerMaclaurinGrid::RefineElementRangeMap(const AOBasis& aobasis, std::vector<QMAtom*>& atoms, double eps){
      AOOverlap overlap;
      overlap.Fill(aobasis);

      // get collapsed index list
      std::vector<int> idxstart;
      const std::vector<int>& idxsize = aobasis.getFuncPerAtom();
      int start = 0;
      for (int size : idxsize) {
        idxstart.push_back(start);
        start += size;
      }
      // refining by going through all atom combinations
      for (unsigned i = 0; i < atoms.size(); ++i) {
        QMAtom* atom_a = atoms[i];
        int a_start = idxstart[i];
        int a_size = idxsize[i];
        double range_max = std::numeric_limits<double>::min();
        // get preset values for this atom type
        double alpha_a = _element_ranges.at(atom_a->getType()).alpha;
        int l_a = _element_ranges.at(atom_a->getType()).l;
        const tools::vec& pos_a = atom_a->getPos();
        // Cannot iterate only over j<i because it is not symmetric due to shift_2g
        for (unsigned j = 0; j < atoms.size(); ++j) {
          if (i == j) {
            continue;
          }
          QMAtom* atom_b = atoms[j];
          int b_start = idxstart[j];
          int b_size = idxsize[j];
          const tools::vec& pos_b = atom_b->getPos();
          // find overlap block of these two atoms
          Eigen::MatrixXd overlapblock = overlap.Matrix().block(a_start, b_start, a_size, b_size);
          // determine abs max of this block
          double s_max = overlapblock.cwiseAbs().maxCoeff();

          if (s_max > 1e-5) {
            double range = DetermineCutoff(alpha_a + _element_ranges.at(atom_b->getType()).alpha, l_a + _element_ranges.at(atom_b->getType()).l + 2, eps);
            // now do some update trickery from Gaussian product formula
            double dist = tools::abs(pos_b - pos_a);
            double shift_2g = dist * alpha_a / (alpha_a + _element_ranges.at(atom_b->getType()).alpha);
            range += (shift_2g + dist);
            if (range > range_max) {
              range_max = range;
            }
          }
        }
        if (std::round(range_max) > _element_ranges.at(atom_a->getType()).range) {
          _element_ranges.at(atom_a->getType()).range = std::round(range_max);
        }
      }
    }

    void EulerMaclaurinGrid::CalculateRadialCutoffs(const AOBasis& aobasis, std::vector<QMAtom* > atoms, const std::string& gridtype) {

      double eps = Accuracy[gridtype];
      FillElementRangeMap(aobasis, atoms, eps);
      RefineElementRangeMap(aobasis, atoms, eps);
      return;
    }

    std::map<std::string, GridContainers::radial_grid> EulerMaclaurinGrid::CalculateAtomicRadialGrids(const AOBasis& aobasis , std::vector<QMAtom* > atoms,const std::string& type) {

    CalculateRadialCutoffs(aobasis,atoms,type);
    std::map<std::string,GridContainers::radial_grid>result;
    for (const auto& element:_element_ranges){
      result[element.first]=CalculateRadialGridforAtom( type,element );
    }
     return result;
    }

    GridContainers::radial_grid EulerMaclaurinGrid::CalculateRadialGridforAtom(const std::string& type, const std::pair<std::string, min_exp>& element) {
      GridContainers::radial_grid result;
      int np = getGridParameters(element.first, type);
      double cutoff = element.second.range;
      result.radius=Eigen::VectorXd::Zero(np);
      result.weight=Eigen::VectorXd::Zero(np);
      double alpha = -cutoff / (log(1.0 - std::pow((1.0 + double(np)) / (2.0 + double(np)), 3)));
      double factor = 3.0 / (1.0 + double(np));

      for (int i = 0; i < np; i++) {
        double q = double(i + 1) / (double(np) + 1.0);
        double r = -alpha * std::log(1.0 - std::pow(q, 3));
        double w = factor * alpha * r * r / (1.0 - std::pow(q, 3)) * std::pow(q, 2);
        result.radius[i]=r;
        result.weight[i]=w;
      }
      return result;
    }

    double EulerMaclaurinGrid::DetermineCutoff(double alpha, int l, double eps){
      // determine norm of function
     /* For a function f(r) = r^k*exp(-alpha*r^2) determine
        the radial distance r such that the fraction of the
        function norm that is neglected if the 3D volume
        integration is terminated at a distance r is less
        than or equal to eps. */

        double cutoff    = 1.0; // initial value
        double increment = 0.5; // increment

            while (increment > 0.01) {
                double residual = CalcResidual(alpha,l ,  cutoff);
                if (residual > eps) {
                    cutoff += increment;
                } else {
                    cutoff -= increment;
                    if (cutoff < 0.0) cutoff = 0.0;
                    increment = 0.5 * increment;
                    cutoff += increment;
                }
            }
        return cutoff;
    }

    double EulerMaclaurinGrid::CalcResidual(double alpha, int l, double cutoff){
        return RadialIntegral(alpha,l+2,cutoff) / RadialIntegral(alpha,l+2, 0.0);
    }

    double EulerMaclaurinGrid::RadialIntegral(double alpha, int l, double cutoff){
        const double pi = boost::math::constants::pi<double>();
        int ilo = l % 2;
        double value = 0.0;
        double valexp;
        if ( ilo == 0 ){
            double expo = sqrt(alpha)*cutoff;
            if ( expo > 40.0 ) {
                value = 0.0;
            } else {
                value = 0.5 * sqrt(  pi /alpha  ) * std::erfc(expo);
            }
        }
        double exponent = alpha*cutoff*cutoff;
        if ( exponent > 500.0 ) {
            valexp = 0.0;
            value = 0.0;
        } else {
            valexp = exp(-exponent);
            value = valexp/2.0/alpha;
        }
        for (int i = ilo+2; i <= l; i+=2){
            value = ((i-1)*value + std::pow(cutoff,i-1)*valexp)/2.0/alpha;
        }
        return value;
    }

    int EulerMaclaurinGrid::getGridParameters(const std::string& element, const std::string& type){
        if ( type == "medium"){
            return MediumGrid.at(element);
        }
        else if ( type == "coarse"){
            return CoarseGrid.at(element);
        }
        else if ( type == "xcoarse"){
            return XcoarseGrid.at(element);
        }
        else if ( type == "fine"){
            return FineGrid.at(element);
        }
        else if ( type == "xfine"){
            return XfineGrid.at(element);
        }
        throw std::runtime_error("Grid type "+type+" is not implemented");
        return -1;
    }

}
}


/*
  [auto_generated]
  boost/numeric/odeint/external/eigen/eigen_algebra.hpp

  [begin_description]
  tba.
  [end_description]

  Copyright 2013 Christian Shelton
  Copyright 2013 Karsten Ahnert

  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)
*/

#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_EIGEN_EIGEN_ALGEBRA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_EIGEN_EIGEN_ALGEBRA_HPP_INCLUDED

#include <Eigen/Dense>
#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>

// Necessary routines for Eigen matrices to work with vector_space_algebra
// from odeint
// (that is, it lets odeint treat the eigen matrices correctly, knowing
// how to add, multiply, compute the norm, etc)

namespace Eigen {

template<typename D>
inline const
typename Eigen::CwiseUnaryOp<
          typename Eigen::internal::scalar_add_op<
               typename Eigen::internal::traits<D>::Scalar>,
          const D >
operator+(const typename Eigen::MatrixBase<D> &m,
          const typename Eigen::internal::traits<D>::Scalar &s) {
     return Eigen::CwiseUnaryOp<
          typename Eigen::internal::scalar_add_op<
               typename Eigen::internal::traits<D>::Scalar>,
          const D >(m.derived(),Eigen::internal::scalar_add_op<
                    typename Eigen::internal::traits<D>::Scalar>(s));
}

template<typename D>
inline const
typename Eigen::CwiseUnaryOp<
          typename Eigen::internal::scalar_add_op<
               typename Eigen::internal::traits<D>::Scalar>,
          const D >
operator+(const typename Eigen::internal::traits<D>::Scalar &s,
const typename Eigen::MatrixBase<D> &m) {
     return Eigen::CwiseUnaryOp<
          typename Eigen::internal::scalar_add_op<
               typename Eigen::internal::traits<D>::Scalar>,
          const D >(m.derived(),Eigen::internal::scalar_add_op<
                    typename Eigen::internal::traits<D>::Scalar>(s));
}

template<typename D1,typename D2>
inline const
typename Eigen::CwiseBinaryOp<
    typename Eigen::internal::scalar_quotient_op<
        typename Eigen::internal::traits<D1>::Scalar>,
    const D1, const D2>
operator/(const Eigen::MatrixBase<D1> &x1, const Eigen::MatrixBase<D2> &x2) {
    return x1.cwiseQuotient(x2);
}

template< typename D >
inline const
typename Eigen::CwiseUnaryOp<
    typename Eigen::internal::scalar_abs_op<
        typename Eigen::internal::traits< D >::Scalar > ,
        const D >
abs( const Eigen::MatrixBase< D > &m ) {
    return m.cwiseAbs();
}

} // end Eigen namespace

namespace boost {
namespace numeric {
namespace odeint {

template<typename B,int S1,int S2,int O, int M1, int M2>
struct vector_space_norm_inf< Eigen::Matrix<B,S1,S2,O,M1,M2> >
{
    typedef B result_type;
    result_type operator()( const Eigen::Matrix<B,S1,S2,O,M1,M2> &m ) const
    {
        return m.template lpNorm<Eigen::Infinity>();
    }
};

} } } // end boost::numeric::odeint namespace

#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_EIGEN_EIGEN_ALGEBRA_HPP_INCLUDED


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_VECTOR_SPLIT_COMPLEX_VECTOR_INCLUDE
#define MTL_VECTOR_SPLIT_COMPLEX_VECTOR_INCLUDE

#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/operation/size.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace vector {

/// Split one complex-valued vector into two real-valued vectors.
/** Elements of the real vector must be assignable from the real and imaginary part of the complex elements.
    Real vectors are resized if their size is 0 otherwise the vectors must have
    the same length. **/
template <typename VectorComplex, typename VectorReal, typename VectorImaginary>
inline void split_complex_vector(const VectorComplex& c, VectorReal& r, VectorImaginary& i)
{
    using mtl::size;
    using mtl::real;
    using mtl::imag;

    vampir_trace<2034> tracer;
    r.checked_change_dim(size(c));
    i.checked_change_dim(size(c));

    for (std::size_t j= 0; j < size(c); ++j)
	r[j]= real(c[j]),
	    i[j]= imag(c[j]);
}

}} // namespace mtl::vector

#endif // MTL_VECTOR_SPLIT_COMPLEX_VECTOR_INCLUDE


#if COMPILATION_INSTRUCTIONS
mpic++ -O3 -std=c++14 -Wall `#-Wfatal-errors` $0 -o $0x.x -lboost_serialization && time mpirun -np 4 $0x.x $@ && rm -f $0x.x; exit
#endif

#include "alf/boost/mpi3/shared_main.hpp"

#include <boost/multi_array.hpp>

#include<iostream>
#include<vector>
#include<cstdlib>

void mySum(void *in_, void *out_, int *len, MPI_Datatype *dtype) {
  double* in = reinterpret_cast<double*>(in_);
  double* out = reinterpret_cast<double*>(out_);
  MPI_Aint sz,lb;
  MPI_Type_get_extent(*dtype,&lb,&sz);
  for(int i=0, iend=sz/sizeof(double); i<iend; i++)
    out[i] += in[i];
}

namespace mpi3 = boost::mpi3;

using std::cout;
using std::endl;
using boost::extents;

// run with 4 procs
int mpi3::main(int argc,char* argv[], mpi3::shared_communicator& node){

//  MPI_Init(&argc,&argv);;

//  int rank, size;
//  MPI_Comm_size(MPI_COMM_WORLD, &size);
//  MPI_Comm_rank(MPI_COMM_WORLD, &rank);

  int nw=16;
  int ni=2*624*624;
  double t1,t2;

  boost::multi_array<std::complex<double>,2> A(extents[ni][nw*node.size()]);  // 2x2 matrix
  boost::multi_array<std::complex<double>,2> B(extents[ni][nw]);  // 2x2 matrix
  boost::multi_array<std::complex<double>,2> C(extents[ni][nw]);  // 2x2 matrix

  std::vector<int> nr{10000,100000,250000,500000,ni};

  for(std::size_t n=0; n != nr.size(); n++) {
	ni = nr[n];
	mpi3::type tA = mpi3::double_.vector(ni, 2*nw, 2*A.strides()[0]);
//  MPI_Datatype typeA;
//  MPI_Type_vector(ni, 2*nw, 2*A.strides()[0], MPI_DOUBLE, &typeA);
	tA.commit();
//  MPI_Type_commit(&typeA);
//  MPI_Op myOpSum;
//  MPI_Op_create(mySum,true,&myOpSum);
//  mpi3::operation my_sum =
	mpi3::commutative_operation my_sum(&mySum);
//  MPI_Allreduce(MPI_IN_PLACE,A.data(),2*ni*nw*size,MPI_DOUBLE,MPI_SUM,MPI_COMM_WORLD);
	node.all_reduce_in_place_n(A.data(), A.num_elements(), std::plus<>{});

	int nt = 4;
	node.barrier();
//  MPI_Barrier(MPI_COMM_WORLD);
	{
		boost::timer::auto_cpu_timer t("All reduce");
//  t1=getTime();
		for(int i = 0; i != nt; i++)
			node.all_reduce_in_place_n(A.data(), A.num_elements(), std::plus<>{});
///			MPI_Allreduce(MPI_IN_PLACE,A.data(),2*ni*nw*size,MPI_DOUBLE,MPI_SUM,MPI_COMM_WORLD);
	}
//  t2=getTime();
//  double tv1=(t2-t1)/nt;
	{
		for(int r=0, p=0; r != node.size(); r++, p+=nw)
//		if(rank==r) node.reduce_in_place_n(A.data() + p, 1, my_sum,
			MPI_Reduce(MPI_IN_PLACE, A.data()+p, 1, tA.impl_, myOpSum, r, node.impl_);
		else
			MPI_Reduce(A.data()+p, NULL, 1, tA.impl_, myOpSum, r, MPI_COMM_WORLD);
	}
	node.barrier();
	{
		boost::timer::auto_cpu_timer t;
		for(int i=0; i != nt; i++)
			for(int r=0, p=0; r != size; r++, p+=nw)
				if(rank==r)
					MPI_Reduce(MPI_IN_PLACE, A.data()+p, 1, A.impl_, myOpSum, r, node.impl_);
				else
					MPI_Reduce(A.data()+p, NULL, 1, typeA, myOpSum, r, node.impl_);
	}
	std::vector<mpi3::request> req(node.size());
//	std::vector<mpi3::status>  st(node.size());
	for(int r=0; r != node.size(); r++)
		if(node.rank()==r)
			MPI_Ireduce(C.data(), B.data(),2*ni*nw,MPI_DOUBLE,MPI_SUM,r, node.impl_, &req[r].impl_);
		else
			MPI_Ireduce(C.data(), B.data(),2*ni*nw,MPI_DOUBLE,MPI_SUM,r, node.impl_, &req[r].impl_);
	mpi3::wait_all(req);
	node.barrier();

	{
		boost::timer::auto_cpu_timer t("multiple Reduce");
		for(int i=0; i!=nt; i++){
			for(int r=0; r != node.size(); r++)
				if(node.rank() == r)
					MPI_Ireduce(C.data(), B.data(), 2*ni*nw, MPI_DOUBLE, MPI_SUM, r, node.impl_, &req[r].impl_);
				else
					MPI_Ireduce(C.data(), B.data(), 2*ni*nw, MPI_DOUBLE, MPI_SUM, r, node.impl_, &req[r].impl_);
				mpi3::wait_all(req);
		}
	}

	for(int r=0; r != node.size(); r++)
		if(node.rank()==r)
			MPI_Reduce(MPI_IN_PLACE, B.data(), 2*ni*nw, MPI_DOUBLE, MPI_SUM, r, node.impl_);
		else
			MPI_Reduce(B.data(), NULL, 2*ni*nw, MPI_DOUBLE, MPI_SUM, r, node.impl_);

	node.barrier();

	{
		boost::timer::auto_cpu_timer t("multiple Ireduce");
		for(int i=0; i != nt; i++)
			for(int r=0; r<size; r++)
				if(rank==r)
					MPI_Reduce(MPI_IN_PLACE, B.data(), 2*ni*nw, MPI_DOUBLE, MPI_SUM, r, node.impl_);
				else
					MPI_Reduce(B.data(), NULL, 2*ni*nw, MPI_DOUBLE, MPI_SUM, r, node.impl_);
	}
  if(rank==0) cout<<" n, times (Allreduce, multiple Reduce, multiple IReduce): " <<ni <<" " <<tv1 <<"  " <<tv2 <<" " <<tv3 <<" " <<tv4  <<endl;
  MPI_Type_free(&typeA);

  }

  MPI_Finalize();

  return 0;
}


/* ************************************************************************
 * Copyright 2015 Vratis, Ltd.
 *
 * 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 <gtest/gtest.h>
#include <vector>
#include <string>
#include <numeric>
#include <boost/program_options.hpp>

//boost ublas
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/blas.hpp>

#include "resources/clsparse_environment.h"
#include "resources/blas1_environment.h"

clsparseControl ClSparseEnvironment::control = NULL;
cl_command_queue ClSparseEnvironment::queue = NULL;
cl_context ClSparseEnvironment::context = NULL;

double Blas1Environment::alpha = 0;
double Blas1Environment::beta = 0;
int Blas1Environment::size = 0;

namespace po = boost::program_options;
namespace uBLAS = boost::numeric::ublas;

template <typename T>
class Blas1 : public ::testing::Test
{

     using CLSE = ClSparseEnvironment;
     using BLAS1E = Blas1Environment;

public:

    void SetUp()
    {
        clsparseInitScalar(&gAlpha);
        clsparseInitScalar(&gBeta);

        clsparseInitVector(&gX);
        clsparseInitVector(&gY);

        hY = uBLAS::vector<T>(BLAS1E::size, 2);
        hX = uBLAS::vector<T>(BLAS1E::size, 4);
        //std::iota(hX.begin(), hX.end(), 1);

        cl_int status;
        gX.values = clCreateBuffer(CLSE::context,
                                   CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
                                   hX.size() * sizeof(T), hX.data().begin(), &status);
        gX.num_values = hX.size();
        ASSERT_EQ(CL_SUCCESS, status);

        gY.values = clCreateBuffer(CLSE::context,
                                   CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
                                   hY.size() * sizeof(T), hY.data().begin(), &status);

        gY.num_values = hY.size();
        ASSERT_EQ(CL_SUCCESS, status);

        gAlpha.value = clCreateBuffer(CLSE::context,
                                      CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
                                      sizeof(T), &hAlpha, &status);

        ASSERT_EQ(CL_SUCCESS, status);

        gBeta.value = clCreateBuffer(CLSE::context,
                                     CL_MEM_READ_WRITE | CL_MEM_COPY_HOST_PTR,
                                     sizeof(T), &hBeta, &status);

        ASSERT_EQ(CL_SUCCESS, status);

    }

    void TearDown()
    {
       clReleaseMemObject( gX.values );
       clReleaseMemObject( gY.values );
       clReleaseMemObject( gAlpha.value );
       clReleaseMemObject( gBeta.value );
    }

    boost::numeric::ublas::vector<T> hY;
    boost::numeric::ublas::vector<T> hX;
    T hAlpha = BLAS1E::alpha;
    T hBeta = BLAS1E::beta;

    cldenseVector gX;
    cldenseVector gY;
    clsparseScalar gAlpha;
    clsparseScalar gBeta;

    /*  Test functions  */

    void test_reduce()
    {
        clsparseStatus status;

        //  GPU result;
        if (typeid(T) == typeid(cl_float))
        {
            status = cldenseSreduce(&gAlpha, &gX, CLSE::control);
        }
        else if ( typeid(T) == typeid(cl_double) )
        {
            status = cldenseDreduce(&gAlpha, &gX, CLSE::control);
        }

        ASSERT_EQ (clsparseSuccess, status);

        T ublas_result = uBLAS::blas_1::asum(hX);

        // Map device data to host
        cl_int cl_status;
        T* host_result = (T*) ::clEnqueueMapBuffer(CLSE::queue, gAlpha.value,
                                              CL_TRUE, CL_MAP_READ,
                                              0, sizeof(T), 0, nullptr, nullptr, &cl_status);
        ASSERT_EQ (CL_SUCCESS, cl_status);

        // change it to function passing an assertion here;
        if ( typeid(T) == typeid(cl_float) )
            ASSERT_NEAR (ublas_result, *host_result, 1e-7);
        if ( typeid(T) == typeid(cl_double) )
            ASSERT_NEAR (ublas_result, *host_result, 1e-14);

        cl_status = ::clEnqueueUnmapMemObject(CLSE::queue, gAlpha.value,
                                              host_result, 0, nullptr, nullptr);
        ASSERT_EQ (CL_SUCCESS, cl_status);
    }

    void test_norm1()
    {
        clsparseStatus status;

        //  GPU result;
        if (typeid(T) == typeid(cl_float))
        {
            status = cldenseSnrm1(&gAlpha, &gX, CLSE::control);
        }
        else if ( typeid(T) == typeid(cl_double) )
        {
            status = cldenseDnrm1(&gAlpha, &gX, CLSE::control);
        }

        ASSERT_EQ (clsparseSuccess, status);

        T ublas_result = uBLAS::blas_1::asum(hX);

        // Map device data to host
        cl_int cl_status;
        T* host_result = (T*) ::clEnqueueMapBuffer(CLSE::queue, gAlpha.value,
                                              CL_TRUE, CL_MAP_READ,
                                              0, sizeof(T), 0, nullptr, nullptr, &cl_status);
        ASSERT_EQ (CL_SUCCESS, cl_status);

        // change it to function passing an assertion here;
        if ( typeid(T) == typeid(cl_float) )
            ASSERT_NEAR (ublas_result, *host_result, 1e-7);
        if ( typeid(T) == typeid(cl_double) )
            ASSERT_NEAR (ublas_result, *host_result, 1e-14);

        cl_status = ::clEnqueueUnmapMemObject(CLSE::queue, gAlpha.value,
                                              host_result, 0, nullptr, nullptr);
        ASSERT_EQ (CL_SUCCESS, cl_status);

    }

    void test_norm2()
    {
        clsparseStatus status;

        //  GPU result;
        if (typeid(T) == typeid(cl_float))
        {
            status = cldenseSnrm2(&gAlpha, &gX, CLSE::control);
        }
        else if ( typeid(T) == typeid(cl_double) )
        {
            status = cldenseDnrm2(&gAlpha, &gX, CLSE::control);
        }

        ASSERT_EQ (clsparseSuccess, status);

        T ublas_result = uBLAS::blas_1::nrm2(hX);

        // Map device data to host
        cl_int cl_status;
        T* host_result = (T*) ::clEnqueueMapBuffer(CLSE::queue, gAlpha.value,
                                              CL_TRUE, CL_MAP_READ,
                                              0, sizeof(T), 0, nullptr, nullptr, &cl_status);
        ASSERT_EQ (CL_SUCCESS, cl_status);

        // change it to function passing an assertion here;
        if ( typeid(T) == typeid(cl_float) )
            ASSERT_NEAR (ublas_result, *host_result, 1e-7);
        if ( typeid(T) == typeid(cl_double) )
            ASSERT_NEAR (ublas_result, *host_result, 1e-14);

        cl_status = ::clEnqueueUnmapMemObject(CLSE::queue, gAlpha.value,
                                              host_result, 0, nullptr, nullptr);
        ASSERT_EQ (CL_SUCCESS, cl_status);
    }

    void test_scale()
    {
        clsparseStatus status;

        //  GPU result;
        if (typeid(T) == typeid(cl_float))
        {
            status = cldenseSscale(&gX, &gAlpha, &gX, CLSE::control);
        }
        else if ( typeid(T) == typeid(cl_double) )
        {
            status = cldenseDscale(&gX, &gAlpha, &gX, CLSE::control);
        }

        ASSERT_EQ (clsparseSuccess, status);

        hX = uBLAS::blas_1::scal(hX, hAlpha);

        // Map device data to host
        cl_int cl_status;
        T* host_result = (T*) ::clEnqueueMapBuffer(CLSE::queue, gX.values,
                                              CL_TRUE, CL_MAP_READ,
                                              0, gX.num_values * sizeof(T),
                                              0, nullptr, nullptr, &cl_status);
        ASSERT_EQ (CL_SUCCESS, cl_status);

        // change it to some compare template functions
        if ( typeid(T) == typeid(cl_float) )
            for(int i = 0; i < hX.size(); i++)
                ASSERT_NEAR (hX[i], host_result[i], 1e-7);

        if ( typeid(T) == typeid(cl_double) )
            for(int i = 0; i < hX.size(); i++)
                ASSERT_NEAR (hX[i], host_result[i], 1e-14);

        cl_status = ::clEnqueueUnmapMemObject(CLSE::queue, gX.values,
                                              host_result, 0, nullptr, nullptr);
        ASSERT_EQ (CL_SUCCESS, cl_status);

    }

    void test_dot()
    {
        clsparseStatus status;
        if ( typeid(T) == typeid(cl_float) )
        {
            status = cldenseSdot(&gAlpha, &gX, &gY, CLSE::control);
        }
        else if( typeid(T)  == typeid (cl_double) )
        {
            status = cldenseDdot(&gAlpha, &gX, &gY, CLSE::control);
        }

        ASSERT_EQ (clsparseSuccess, status);

        T ublas_result = uBLAS::blas_1::dot(hX, hY);

        cl_int cl_status;
        T* host_result = (T*) ::clEnqueueMapBuffer(CLSE::queue, gAlpha.value,
                                              CL_TRUE, CL_MAP_READ,
                                              0, sizeof(T), 0, nullptr, nullptr, &cl_status);
        ASSERT_EQ (CL_SUCCESS, cl_status);

        // change it to function passing an assertion here;
        if ( typeid(T) == typeid(cl_float) )
            ASSERT_NEAR (ublas_result, *host_result, 1e-7);
        if ( typeid(T) == typeid(cl_double) )
            ASSERT_NEAR (ublas_result, *host_result, 1e-14);

        cl_status = ::clEnqueueUnmapMemObject(CLSE::queue, gAlpha.value,
                                              host_result, 0, nullptr, nullptr);
        ASSERT_EQ (CL_SUCCESS, cl_status);

    }

    void test_axpy()
    {
        clsparseStatus status;

        //  GPU result;
        if (typeid(T) == typeid(cl_float))
        {
            status = cldenseSaxpy(&gY, &gAlpha, &gX, &gY, CLSE::control);
        }
        else if ( typeid(T) == typeid(cl_double) )
        {
            status = cldenseDaxpy(&gY, &gAlpha, &gX, &gY, CLSE::control);
        }

        ASSERT_EQ (clsparseSuccess, status);

        hY = uBLAS::blas_1::axpy(hY, hAlpha, hX);

        // Map device data to host
        cl_int cl_status;
        T* host_result = (T*) ::clEnqueueMapBuffer(CLSE::queue, gY.values,
                                              CL_TRUE, CL_MAP_READ,
                                              0, gY.num_values * sizeof(T),
                                              0, nullptr, nullptr, &cl_status);
        ASSERT_EQ (CL_SUCCESS, cl_status);

        // change it to some compare template functions
        if ( typeid(T) == typeid(cl_float) )
            for(int i = 0; i < hY.size(); i++)
                ASSERT_NEAR (hY[i], host_result[i], 1e-7);

        if ( typeid(T) == typeid(cl_double) )
            for(int i = 0; i < hY.size(); i++)
                ASSERT_NEAR (hY[i], host_result[i], 1e-14);

        cl_status = ::clEnqueueUnmapMemObject(CLSE::queue, gY.values,
                                              host_result, 0, nullptr, nullptr);
        ASSERT_EQ (CL_SUCCESS, cl_status);
    }

    void test_axpby()
    {
        clsparseStatus status;

        //  GPU result;
        if (typeid(T) == typeid(cl_float))
        {
            status = cldenseSaxpby(&gY, &gAlpha, &gX, &gBeta, &gY, CLSE::control);
        }
        else if ( typeid(T) == typeid(cl_double) )
        {
            status = cldenseDaxpby(&gY, &gAlpha, &gX, &gBeta, &gY, CLSE::control);
        }

        ASSERT_EQ (clsparseSuccess, status);

        hY = uBLAS::blas_1::axpy(uBLAS::blas_1::scal(hY, hBeta), hAlpha, hX);

        // Map device data to host
        cl_int cl_status;
        T* host_result = (T*) ::clEnqueueMapBuffer(CLSE::queue, gY.values,
                                              CL_TRUE, CL_MAP_READ,
                                              0, gY.num_values * sizeof(T),
                                              0, nullptr, nullptr, &cl_status);
        ASSERT_EQ (CL_SUCCESS, cl_status);

        // change it to some compare template functions
        if ( typeid(T) == typeid(cl_float) )
            for(int i = 0; i < hY.size(); i++)
                ASSERT_NEAR (hY[i], host_result[i], 1e-7);

        if ( typeid(T) == typeid(cl_double) )
            for(int i = 0; i < hY.size(); i++)
                ASSERT_NEAR (hY[i], host_result[i], 1e-14);

        cl_status = ::clEnqueueUnmapMemObject(CLSE::queue, gY.values,
                                              host_result, 0, nullptr, nullptr);
        ASSERT_EQ (CL_SUCCESS, cl_status);

    }

    void test_add()
    {
        clsparseStatus status;

        if (typeid(T) == typeid(cl_float))
        {
            status = cldenseSadd(&gY, &gX, &gY, CLSE::control);
        }
        else if ( typeid(T) == typeid(cl_double) )
        {
            status = cldenseDadd(&gY, &gX, &gY, CLSE::control);
        }

        ASSERT_EQ(clsparseSuccess, status);

        hY = hX + hY;

        cl_int cl_status;
        T* host_result = (T*) ::clEnqueueMapBuffer(CLSE::queue, gY.values,
                                              CL_TRUE, CL_MAP_READ,
                                              0, gY.num_values * sizeof(T),
                                              0, nullptr, nullptr, &cl_status);
        ASSERT_EQ (CL_SUCCESS, cl_status);

        // change it to some compare template functions
        if ( typeid(T) == typeid(cl_float) )
            for(int i = 0; i < hY.size(); i++)
                ASSERT_NEAR (hY[i], host_result[i], 1e-7);

        if ( typeid(T) == typeid(cl_double) )
            for(int i = 0; i < hY.size(); i++)
                ASSERT_NEAR (hY[i], host_result[i], 1e-14);

        cl_status = ::clEnqueueUnmapMemObject(CLSE::queue, gY.values,
                                              host_result, 0, nullptr, nullptr);
        ASSERT_EQ (CL_SUCCESS, cl_status);
    }

    void test_sub()
    {
        clsparseStatus status;

        if (typeid(T) == typeid(cl_float))
        {
            status = cldenseSsub(&gY, &gX, &gY, CLSE::control);
        }
        else if ( typeid(T) == typeid(cl_double) )
        {
            status = cldenseDsub(&gY, &gX, &gY, CLSE::control);
        }

        ASSERT_EQ(clsparseSuccess, status);

        hY = hX - hY;

        cl_int cl_status;
        T* host_result = (T*) ::clEnqueueMapBuffer(CLSE::queue, gY.values,
                                              CL_TRUE, CL_MAP_READ,
                                              0, gY.num_values * sizeof(T),
                                              0, nullptr, nullptr, &cl_status);
        ASSERT_EQ (CL_SUCCESS, cl_status);

        // change it to some compare template functions
        if ( typeid(T) == typeid(cl_float) )
            for(int i = 0; i < hY.size(); i++)
                ASSERT_NEAR (hY[i], host_result[i], 1e-7);

        if ( typeid(T) == typeid(cl_double) )
            for(int i = 0; i < hY.size(); i++)
                ASSERT_NEAR (hY[i], host_result[i], 1e-14);

        cl_status = ::clEnqueueUnmapMemObject(CLSE::queue, gY.values,
                                              host_result, 0, nullptr, nullptr);
        ASSERT_EQ (CL_SUCCESS, cl_status);

    }

    void test_mul()
    {
        clsparseStatus status;

        if (typeid(T) == typeid(cl_float))
        {
            status = cldenseSmul(&gY, &gX, &gY, CLSE::control);
        }
        else if ( typeid(T) == typeid(cl_double) )
        {
            status = cldenseDmul(&gY, &gX, &gY, CLSE::control);
        }

        ASSERT_EQ(clsparseSuccess, status);

        hY = uBLAS::element_prod(hX, hY);

        cl_int cl_status;
        T* host_result = (T*) ::clEnqueueMapBuffer(CLSE::queue, gY.values,
                                              CL_TRUE, CL_MAP_READ,
                                              0, gY.num_values * sizeof(T),
                                              0, nullptr, nullptr, &cl_status);
        ASSERT_EQ (CL_SUCCESS, cl_status);

        // change it to some compare template functions
        if ( typeid(T) == typeid(cl_float) )
            for(int i = 0; i < hY.size(); i++)
                ASSERT_NEAR (hY[i], host_result[i], 1e-7);

        if ( typeid(T) == typeid(cl_double) )
            for(int i = 0; i < hY.size(); i++)
                ASSERT_NEAR (hY[i], host_result[i], 1e-14);

        cl_status = ::clEnqueueUnmapMemObject(CLSE::queue, gY.values,
                                              host_result, 0, nullptr, nullptr);
        ASSERT_EQ (CL_SUCCESS, cl_status);

    }

    void test_div()
    {
        clsparseStatus status;

        if (typeid(T) == typeid(cl_float))
        {
            status = cldenseSdiv(&gY, &gX, &gY, CLSE::control);
        }
        else if ( typeid(T) == typeid(cl_double) )
        {
            status = cldenseDdiv(&gY, &gX, &gY, CLSE::control);
        }

        ASSERT_EQ(clsparseSuccess, status);

        hY = uBLAS::element_div(hX, hY);

        cl_int cl_status;
        T* host_result = (T*) ::clEnqueueMapBuffer(CLSE::queue, gY.values,
                                              CL_TRUE, CL_MAP_READ,
                                              0, gY.num_values * sizeof(T),
                                              0, nullptr, nullptr, &cl_status);
        ASSERT_EQ (CL_SUCCESS, cl_status);

        // change it to some compare template functions
        if ( typeid(T) == typeid(cl_float) )
            for(int i = 0; i < hY.size(); i++)
                ASSERT_NEAR (hY[i], host_result[i], 1e-7);

        if ( typeid(T) == typeid(cl_double) )
            for(int i = 0; i < hY.size(); i++)
                ASSERT_NEAR (hY[i], host_result[i], 1e-14);

        cl_status = ::clEnqueueUnmapMemObject(CLSE::queue, gY.values,
                                              host_result, 0, nullptr, nullptr);
        ASSERT_EQ (CL_SUCCESS, cl_status);

    }

};

typedef ::testing::Types<cl_float, cl_double> TYPES;
TYPED_TEST_CASE(Blas1, TYPES);

TYPED_TEST(Blas1, reduce)
{
    this->test_reduce();
}

TYPED_TEST(Blas1, norm1)
{
    this->test_norm1();
}

TYPED_TEST(Blas1, norm2)
{
    this->test_norm2();
}

TYPED_TEST(Blas1, scale)
{
    this->test_scale();
}

TYPED_TEST(Blas1, dot)
{
    this->test_dot();
}

TYPED_TEST(Blas1, axpy)
{
    this->test_axpy();
}

TYPED_TEST(Blas1, axpby)
{
    this->test_axpby();
}

TYPED_TEST(Blas1, add)
{
    this->test_add();
}

TYPED_TEST(Blas1, sub)
{
    this->test_sub();
}

TYPED_TEST(Blas1, mul)
{
    this->test_mul();
}

TYPED_TEST(Blas1, div)
{
    this->test_div();
}

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

    using CLSE = ClSparseEnvironment;
    using BLAS1E = Blas1Environment;

    std::string platform;
    cl_platform_type pID;
    cl_uint dID;
    cl_uint size;

    double alpha;
    double beta;

    po::options_description desc("Allowed options");

    desc.add_options()
            ("help,h", "Produce this message.")
            ("platform,l", po::value(&platform)->default_value("AMD"),
             "OpenCL platform: AMD or NVIDIA.")
            ("device,d", po::value(&dID)->default_value(0),
             "Device id within platform.")
            ("size,s",po::value(&size)->default_value(1024),
             "Size of the vectors used for testing")
            ("alpha,a", po::value(&alpha)->default_value(2),
             "Alpha coefficient for blas1 operations i.e. r = alpha * x + beta * y")
            ("beta,b", po::value(&beta)->default_value(4),
             "Beta coefficient for blas1 operations i.e. r = alpha * x + beta * y");

    po::variables_map vm;
    po::parsed_options parsed =
            po::command_line_parser( argc, argv ).options( desc ).allow_unregistered( ).run( );

    try {
        po::store(parsed, vm);
        if (vm.count("help"))
        {
            std::cout << desc << std::endl;
            return 0;
        }
        po::notify(vm);
    }
    catch (po::error& error)
    {
        std::cerr << "Parsing command line options..." << std::endl;
        std::cerr << "Error: " << error.what() << std::endl;
        std::cerr << desc << std::endl;
        return false;
    }

    std::vector< std::string > to_pass_further =
            po::collect_unrecognized( parsed.options, po::include_positional );

    //check platform
    if(vm.count("platform"))
    {
        if ("AMD" == platform)
        {
            pID = AMD;
        }
        else if ("NVIDIA" == platform)
        {
            pID = NVIDIA;
        }
        else
        {

            std::cout << "The platform option is missing or is ill defined!\n";
            std::cout << "Given [" << platform << "]" << std::endl;
            platform = "AMD";
            pID = AMD;
            std::cout << "Setting [" << platform << "] as default" << std::endl;
        }

    }

    ::testing::InitGoogleTest(&argc, argv);
    //order does matter!
    ::testing::AddGlobalTestEnvironment( new CLSE(pID, dID, size));
    ::testing::AddGlobalTestEnvironment( new BLAS1E(alpha, beta, size));

    return RUN_ALL_TESTS();

}


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

  Copyright 2016 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/nextafter.hpp>
#include <scalar_test.hpp>
#include <boost/simd/detail/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/bitincrement.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/constant/valmin.hpp>

STF_CASE_TPL (" nextafter real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::nextafter;
  using r_t = decltype(nextafter(T(), T()));

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

  // specific values tests
  STF_EQUAL(nextafter(bs::Inf<T>(), bs::Inf<T>()), bs::Inf<r_t>());
  STF_EQUAL(nextafter(bs::Minf<T>(), bs::One<T>()), bs::Valmin<r_t>());
  STF_IEEE_EQUAL(nextafter(bs::Nan<T>(), bs::One<T>()), bs::Nan<r_t>());
  STF_EQUAL(nextafter(bs::One<T>(), bs::Inf<T>()), bs::One<r_t>()+bs::Eps<r_t>());
  STF_EQUAL(nextafter(bs::Valmax<T>(), bs::Inf<T>()), bs::Inf<r_t>());
  STF_EQUAL(nextafter(bs::Mone<T>(), bs::One<T>()), bs::Mone<r_t>()+bs::Eps<r_t>()/2);
  STF_EQUAL(nextafter(bs::Zero<T>(), bs::One<T>()), bs::Bitincrement<T>());
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2014 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2014 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2014 Mateusz Loskot, London, UK.
// Copyright (c) 2014 Adam Wulkiewicz, Lodz, Poland.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_EQUALS_COLLECT_VECTORS_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_EQUALS_COLLECT_VECTORS_HPP

#include <boost/numeric/conversion/cast.hpp>

#include <boost/geometry/algorithms/detail/interior_iterator.hpp>
#include <boost/geometry/algorithms/detail/normalize.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/interior_rings.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/formulas/spherical.hpp>

#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/range.hpp>

namespace boost { namespace geometry
{

// Since these vectors (though ray would be a better name) are used in the
// implementation of equals() for Areal geometries the internal representation
// should be consistent with the default side strategy for CS because currently
// it's used in other relops.

template <
    typename T,
    typename Geometry,
    typename CSTag = typename cs_tag<Geometry>::type
>
struct collected_vector
{
    typedef T type;

    inline collected_vector()
    {}

    inline collected_vector(T const& px, T const& py,
                            T const& pdx, T const& pdy)
        : x(px)
        , y(py)
        , dx(pdx)
        , dy(pdy)
        //, dx_0(dx)
        //, dy_0(dy)
    {}

    template <typename Point>
    inline collected_vector(Point const& p1, Point const& p2)
        : x(get<0>(p1))
        , y(get<1>(p1))
        , dx(get<0>(p2) - x)
        , dy(get<1>(p2) - y)
        //, dx_0(dx)
        //, dy_0(dy)
    {}

    bool normalize()
    {
        T magnitude = math::sqrt(
            boost::numeric_cast<T>(dx * dx + dy * dy));

        // NOTE: shouldn't here math::equals() be called?
        if (magnitude > 0)
        {
            dx /= magnitude;
            dy /= magnitude;
            return true;
        }

        return false;
    }

    // For sorting
    inline bool operator<(collected_vector const& other) const
    {
        if (math::equals(x, other.x))
        {
            if (math::equals(y, other.y))
            {
                if (math::equals(dx, other.dx))
                {
                    return dy < other.dy;
                }
                return dx < other.dx;
            }
            return y < other.y;
        }
        return x < other.x;
    }

    inline bool next_is_collinear(collected_vector const& other) const
    {
        return same_direction(other);
    }

    // For std::equals
    inline bool operator==(collected_vector const& other) const
    {
        return math::equals(x, other.x)
            && math::equals(y, other.y)
            && same_direction(other);
    }

private:
    inline bool same_direction(collected_vector const& other) const
    {
        // For high precision arithmetic, we have to be
        // more relaxed then using ==
        // Because 2/sqrt( (0,0)<->(2,2) ) == 1/sqrt( (0,0)<->(1,1) )
        // is not always true (at least, it is not for ttmath)
        return math::equals_with_epsilon(dx, other.dx)
            && math::equals_with_epsilon(dy, other.dy);
    }

    T x, y;
    T dx, dy;
    //T dx_0, dy_0;
};

template <typename T, typename Geometry>
struct collected_vector<T, Geometry, spherical_equatorial_tag>
{
    typedef T type;

    typedef typename coordinate_system<Geometry>::type cs_type;
    typedef model::point<T, 2, cs_type> point_type;
    typedef model::point<T, 3, cs::cartesian> vector_type;

    collected_vector()
    {}

    template <typename Point>
    collected_vector(Point const& p1, Point const& p2)
        : origin(get<0>(p1), get<1>(p1))
    {
        origin = detail::return_normalized<point_type>(origin);

        using namespace geometry::formula;
        prev = sph_to_cart3d<vector_type>(p1);
        next = sph_to_cart3d<vector_type>(p2);
        direction = cross_product(prev, next);
    }

    bool normalize()
    {
        T magnitude_sqr = dot_product(direction, direction);

        // NOTE: shouldn't here math::equals() be called?
        if (magnitude_sqr > 0)
        {
            divide_value(direction, math::sqrt(magnitude_sqr));
            return true;
        }

        return false;
    }

    bool operator<(collected_vector const& other) const
    {
        if (math::equals(get<0>(origin), get<0>(other.origin)))
        {
            if (math::equals(get<1>(origin), get<1>(other.origin)))
            {
                if (math::equals(get<0>(direction), get<0>(other.direction)))
                {
                    if (math::equals(get<1>(direction), get<1>(other.direction)))
                    {
                        return get<2>(direction) < get<2>(other.direction);
                    }
                    return get<1>(direction) < get<1>(other.direction);
                }
                return get<0>(direction) < get<0>(other.direction);
            }
            return get<1>(origin) < get<1>(other.origin);
        }
        return get<0>(origin) < get<0>(other.origin);
    }

    // For consistency with side and intersection strategies used by relops
    // IMPORTANT: this method should be called for previous vector
    // and next vector should be passed as parameter
    bool next_is_collinear(collected_vector const& other) const
    {
        return formula::sph_side_value(direction, other.next) == 0;
    }

    // For std::equals
    bool operator==(collected_vector const& other) const
    {
        return math::equals(get<0>(origin), get<0>(other.origin))
            && math::equals(get<1>(origin), get<1>(other.origin))
            && is_collinear(other);
    }

private:
    // For consistency with side and intersection strategies used by relops
    bool is_collinear(collected_vector const& other) const
    {
        return formula::sph_side_value(direction, other.prev) == 0
            && formula::sph_side_value(direction, other.next) == 0;
    }

    /*bool same_direction(collected_vector const& other) const
    {
        return math::equals_with_epsilon(get<0>(direction), get<0>(other.direction))
            && math::equals_with_epsilon(get<1>(direction), get<1>(other.direction))
            && math::equals_with_epsilon(get<2>(direction), get<2>(other.direction));
    }*/

    point_type origin; // used for sorting and equality check
    vector_type direction; // used for sorting, only in operator<
    vector_type prev; // used for collinearity check, only in operator==
    vector_type next; // used for collinearity check
};

template <typename T, typename Geometry>
struct collected_vector<T, Geometry, spherical_polar_tag>
    : public collected_vector<T, Geometry, spherical_equatorial_tag>
{
    typedef collected_vector<T, Geometry, spherical_equatorial_tag> base_type;

    collected_vector() {}

    template <typename Point>
    collected_vector(Point const& p1, Point const& p2)
        : base_type(to_equatorial(p1), to_equatorial(p2))
    {}

private:
    template <typename Point>
    Point polar_to_equatorial(Point const& p)
    {
        typedef typename coordinate_type<Point>::type coord_type;

        typedef math::detail::constants_on_spheroid
            <
                coord_type,
                typename coordinate_system<Point>::type::units
            > constants;

        coord_type const pi_2 = constants::half_period() / 2;

        Point res = p;
        set<1>(res, pi_2 - get<1>(p));
        return res;
    }
};

// This is consistent with the currently used default geographic side
// and intersection strategies. Spherical strategies are used by default.
// When default strategies are changed in the future this specialization
// should be changed too.
template <typename T, typename Geometry>
struct collected_vector<T, Geometry, geographic_tag>
    : public collected_vector<T, Geometry, spherical_equatorial_tag>
{
    typedef collected_vector<T, Geometry, spherical_equatorial_tag> base_type;

    collected_vector() {}

    template <typename Point>
    collected_vector(Point const& p1, Point const& p2)
        : base_type(p1, p2)
    {}
};

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace collect_vectors
{

template <typename Range, typename Collection>
struct range_collect_vectors
{
    typedef typename boost::range_value<Collection>::type item_type;
    typedef typename item_type::type calculation_type;

    static inline void apply(Collection& collection, Range const& range)
    {
        if (boost::size(range) < 2)
        {
            return;
        }

        typedef typename boost::range_size<Collection>::type collection_size_t;
        collection_size_t c_old_size = boost::size(collection);

        typedef typename boost::range_iterator<Range const>::type iterator;

        bool is_first = true;
        iterator it = boost::begin(range);

        for (iterator prev = it++;
            it != boost::end(range);
            prev = it++)
        {
            typename boost::range_value<Collection>::type v(*prev, *it);

            // Normalize the vector -> this results in points+direction
            // and is comparible between geometries
            // Avoid non-duplicate points (AND division by zero)
            if (v.normalize())
            {
                // Avoid non-direction changing points
                if (is_first || ! collection.back().next_is_collinear(v))
                {
                    collection.push_back(v);
                }
                is_first = false;
            }
        }

        // If first one has same direction as last one, remove first one
        collection_size_t collected_count = boost::size(collection) - c_old_size;
        if ( collected_count > 1 )
        {
            typedef typename boost::range_iterator<Collection>::type c_iterator;
            c_iterator first = range::pos(collection, c_old_size);

            if (collection.back().next_is_collinear(*first) )
            {
                //collection.erase(first);
                // O(1) instead of O(N)
                *first = collection.back();
                collection.pop_back();
            }
        }
    }
};

// Default version (cartesian)
template <typename Box, typename Collection, typename CSTag = typename cs_tag<Box>::type>
struct box_collect_vectors
{
    // Calculate on coordinate type, but if it is integer,
    // then use double
    typedef typename boost::range_value<Collection>::type item_type;
    typedef typename item_type::type calculation_type;

    static inline void apply(Collection& collection, Box const& box)
    {
        typename point_type<Box>::type lower_left, lower_right,
            upper_left, upper_right;
        geometry::detail::assign_box_corners(box, lower_left, lower_right,
            upper_left, upper_right);

        typedef typename boost::range_value<Collection>::type item;

        collection.push_back(item(get<0>(lower_left), get<1>(lower_left), 0, 1));
        collection.push_back(item(get<0>(upper_left), get<1>(upper_left), 1, 0));
        collection.push_back(item(get<0>(upper_right), get<1>(upper_right), 0, -1));
        collection.push_back(item(get<0>(lower_right), get<1>(lower_right), -1, 0));
    }
};

// NOTE: This is not fully correct because Box in spherical and geographic
// cordinate systems cannot be seen as Polygon
template <typename Box, typename Collection>
struct box_collect_vectors<Box, Collection, spherical_equatorial_tag>
{
    static inline void apply(Collection& collection, Box const& box)
    {
        typename point_type<Box>::type lower_left, lower_right,
                upper_left, upper_right;
        geometry::detail::assign_box_corners(box, lower_left, lower_right,
                upper_left, upper_right);

        typedef typename boost::range_value<Collection>::type item;

        collection.push_back(item(lower_left, upper_left));
        collection.push_back(item(upper_left, upper_right));
        collection.push_back(item(upper_right, lower_right));
        collection.push_back(item(lower_right, lower_left));
    }
};

template <typename Box, typename Collection>
struct box_collect_vectors<Box, Collection, spherical_polar_tag>
    : box_collect_vectors<Box, Collection, spherical_equatorial_tag>
{};

template <typename Box, typename Collection>
struct box_collect_vectors<Box, Collection, geographic_tag>
    : box_collect_vectors<Box, Collection, spherical_equatorial_tag>
{};

template <typename Polygon, typename Collection>
struct polygon_collect_vectors
{
    static inline void apply(Collection& collection, Polygon const& polygon)
    {
        typedef typename geometry::ring_type<Polygon>::type ring_type;

        typedef range_collect_vectors<ring_type, Collection> per_range;
        per_range::apply(collection, exterior_ring(polygon));

        typename interior_return_type<Polygon const>::type
            rings = interior_rings(polygon);
        for (typename detail::interior_iterator<Polygon const>::type
                it = boost::begin(rings); it != boost::end(rings); ++it)
        {
            per_range::apply(collection, *it);
        }
    }
};

template <typename MultiGeometry, typename Collection, typename SinglePolicy>
struct multi_collect_vectors
{
    static inline void apply(Collection& collection, MultiGeometry const& multi)
    {
        for (typename boost::range_iterator<MultiGeometry const>::type
                it = boost::begin(multi);
            it != boost::end(multi);
            ++it)
        {
            SinglePolicy::apply(collection, *it);
        }
    }
};

}} // namespace detail::collect_vectors
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Tag,
    typename Collection,
    typename Geometry
>
struct collect_vectors
{
    static inline void apply(Collection&, Geometry const&)
    {}
};

template <typename Collection, typename Box>
struct collect_vectors<box_tag, Collection, Box>
    : detail::collect_vectors::box_collect_vectors<Box, Collection>
{};

template <typename Collection, typename Ring>
struct collect_vectors<ring_tag, Collection, Ring>
    : detail::collect_vectors::range_collect_vectors<Ring, Collection>
{};

template <typename Collection, typename LineString>
struct collect_vectors<linestring_tag, Collection, LineString>
    : detail::collect_vectors::range_collect_vectors<LineString, Collection>
{};

template <typename Collection, typename Polygon>
struct collect_vectors<polygon_tag, Collection, Polygon>
    : detail::collect_vectors::polygon_collect_vectors<Polygon, Collection>
{};

template <typename Collection, typename MultiPolygon>
struct collect_vectors<multi_polygon_tag, Collection, MultiPolygon>
    : detail::collect_vectors::multi_collect_vectors
        <
            MultiPolygon,
            Collection,
            detail::collect_vectors::polygon_collect_vectors
            <
                typename boost::range_value<MultiPolygon>::type,
                Collection
            >
        >
{};

} // namespace dispatch
#endif

/*!
    \ingroup collect_vectors
    \tparam Collection Collection type, should be e.g. std::vector<>
    \tparam Geometry geometry type
    \param collection the collection of vectors
    \param geometry the geometry to make collect_vectors
*/
template <typename Collection, typename Geometry>
inline void collect_vectors(Collection& collection, Geometry const& geometry)
{
    concepts::check<Geometry const>();

    dispatch::collect_vectors
        <
            typename tag<Geometry>::type,
            Collection,
            Geometry
        >::apply(collection, geometry);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_EQUALS_COLLECT_VECTORS_HPP