/***************************************************************************
* Copyright (C) 2013 - 2014 by                                                    *
* Tarek Taha and Rui P. de Figueiredo, Khalifa University Robotics Institute KURI *
*                     <tarek.taha@kustar.ac.ae>                            	  *
*                                                                          	  *
*                                                                           	  *
* This program is free software; you can redistribute it and/or modify     	  *
* it under the terms of the GNU General Public License as published by     	  *
* the Free Software Foundation; either version 2 of the License, or        	  *
* (at your option) any later version.                                      	  *
*                                                                          	  *
* This program is distributed in the hope that it will be useful,          	  *
* but WITHOUT ANY WARRANTY; without even the implied warranty of           	  *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the             	  *
* GNU General Public License for more details.                              	  *
*                                                                          	  *
* You should have received a copy of the GNU General Public License        	  *
* along with this program; if not, write to the                            	  *
* Free Software Foundation, Inc.,                                          	  *
* 51 Franklin Steet, Fifth Floor, Boston, MA 02111-1307, USA.              	  *
***********************************************************************************/
#include "ros/ros.h"
#include "std_msgs/String.h"
//#include "pctx_control/Control.h"
#include "visualeyez_tracker/TrackerPose.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Twist.h"
#include "nav_msgs/Odometry.h"
#include <Eigen/Eigen>
#include <std_msgs/String.h>
#include <dynamic_reconfigure/server.h>
#include <haptic_teleoperation/PIDControllerConfig.h>
#include <sstream>
#include <string>
#include <sys/time.h>

#define PI 3.14159265
#define BILLION 1000000000

double x,y,z;

const double epsilon=0.1;

inline bool equalFloat(double a, double b, double epsilon)
{
    return fabs(a - b) < epsilon;
}

class PositionCommand
{
public:
    dynamic_reconfigure::Server<navigation::PIDControllerConfig> master_server;
    dynamic_reconfigure::Server<navigation::PIDControllerConfig>::CallbackType master_callback_type;

    int count;
    ros::Time previous_time;
    // PID gains
    Eigen::Matrix<double,6,1> Kp;
    Eigen::Matrix<double,6,1> Kd;
    Eigen::Matrix<double,6,1> Ki;

    Eigen::Matrix<double,6,1> previous_accum_error;

    std::string base_marker_id_;
    std::string head_marker_id_;
    bool got_pose_update_;
    bool goal_;
    bool got_base_marker_;
    bool got_head_marker_;

    Eigen::Vector3d base_marker_position;
    Eigen::Vector3d head_marker_position;

    Eigen::Matrix<double,6,1> previous_pose_;
    Eigen::Matrix<double,6,1> current_pose_;

    Eigen::Matrix<double,6,6> vel_cmd_previous;

    ros::Subscriber goal_sub;
    ros::Subscriber markers_sub;
    ros::Publisher vel_cmd;

    ros::NodeHandle n_;

    Eigen::Matrix<double,6,1> goal_pose_;

    PositionCommand(ros::NodeHandle & n, std::string & base_marker_id, std::string & head_marker_id,
                    Eigen::Matrix<double,6,1> & kp, Eigen::Matrix<double,6,1> & kd, Eigen::Matrix<double,6,1> & ki) :
        n_(n),
        base_marker_id_(base_marker_id),
        head_marker_id_(head_marker_id),
        Kp(kp),
        Kd(kd),
        Ki(ki),
        count(0),

        goal_(false)
    {
        ROS_INFO_STREAM("base marker id: "<<base_marker_id_);
        ROS_INFO_STREAM("head marker id: "<<head_marker_id_);
        master_callback_type = boost::bind(&PositionCommand::paramsCallback, this, _1, _2);
        master_server.setCallback(master_callback_type);

        //vel_cm_previous_vec=Eigen
        goal_sub = n_.subscribe("Goal", 1000, &PositionCommand::getGoal, this);

        markers_sub = n_.subscribe("/TrackerPosition", 1000, &PositionCommand::getUpdatedPose, this);
        //markers_sub = n_.subscribe("/nav_msgs/Odometry", 1000, &PositionCommand::getUpdatedPoseOdometry, this);
        vel_cmd = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
    }

    void paramsCallback(navigation::PIDControllerConfig &config, uint32_t level)
    {
        ROS_INFO_STREAM("PID reconfigure Request ->"
                        << " kp_x:" << config.kp_x
                        << " kp_y:" << config.kp_y
                        << " kp_z:" << config.kp_z
                        << " kp_x:" << config.kp_roll
                        << " kp_y:" << config.kd_pitch
                        << " kp_z:" << config.kd_yaw

                        << " kd_x:" << config.kd_x
                        << " kd_y:" << config.kd_y
                        << " kd_z:" << config.kd_z
                        << " kd_x:" << config.kd_roll
                        << " kd_y:" << config.kd_pitch
                        << " kd_z:" << config.kd_yaw

                        << " ki_x:" << config.ki_x
                        << " ki_y:" << config.ki_y
                        << " ki_z:" << config.ki_z
                        << " ki_x:" << config.ki_roll
                        << " ki_y:" << config.ki_pitch
                        << " ki_z:" << config.ki_yaw);

        Kp << config.kp_x,
                config.kp_y,
                config.kp_z,
                config.kp_roll,
                config.kp_pitch,
                config.kp_yaw;

        Kd << config.kd_x,
                config.kd_y,
                config.kd_z,
                config.kd_roll,
                config.kd_pitch,
                config.kd_yaw;

        Ki << config.ki_x,
                config.ki_y,
                config.ki_z,
                config.ki_roll,
                config.ki_pitch,
                config.ki_yaw;

        //slave_to_master_scale=Eigen::Matrix<double,3,1> (fabs(config.master_size.x/config.slave_size.x), fabs(config.master_size.y/config.slave_size.y), fabs(config.master_size.z/config.slave_size.z));
    }

private:

    void getUpdatedPose(const visualeyez_tracker::TrackerPose::ConstPtr& odom_orig);
    //void getUpdatedPoseOdometry(const nav_msgs::Odometry::ConstPtr& odom_orig);
    void getGoal(const geometry_msgs::PoseStamped::ConstPtr & goal);
};

/*void PositionCommand::getUpdatedPoseOdometry(const nav_msgs::Odometry::ConstPtr& odom_orig)
{
    //ROS_INFO(" Recieved Tracker Location: [%s] [%f] [%f] [%f]",trackerPose->tracker_id.c_str(),trackerPose->pose.x ,trackerPose->pose.y ,trackerPose->pose.z );
    geometry_msgs::Twist vel_cmd_msg;

    if(!goal_)
    {
        vel_cmd.publish(vel_cmd_msg);
    return;
    }

    if(count==0)
    {
    ROS_INFO("entrou");
        gettimeofday( &previous_time, NULL );
    ++count;
ros::Time current_time = ros::Time::now();
    previous_accum_error << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
        return;
    }

    // Define a local reference frame
    Eigen::Vector3d uav_x=(head_marker_position-base_marker_position).normalized(); // Heading

    Eigen::Vector3d uav_y=(uav_x.cross(Eigen::Vector3d::UnitZ())).normalized();
    // LETS ASSUME THE UAV Z POINTS ALWAYS UP

    Eigen::Matrix<double, 3, 3> current_rotation_matrix;
    current_rotation_matrix << uav_x, uav_y, Eigen::Vector3d::UnitZ();
    Eigen::Matrix<double, 3, 1> current_euler = current_rotation_matrix.eulerAngles(2, 1, 0);

    double yaw = current_euler(0,0);
    double pitch = current_euler(1,0);
    double roll = current_euler(2,0);

        current_pose_ << odom_orig->pose.pose.position.x,
                     odom_orig->pose.pose.position.y,
             odom_orig->pose.pose.position.z,
             yaw,
             pitch,
             roll;

    std::cout << "current_pose: " << current_pose_ << std::endl;
    std::cout << "goal_pose: " << goal_pose_ << std::endl;

        Eigen::Matrix<double,6,1> error=goal_pose_-current_pose_;
        // Check if goal was reached

        if(error.norm()<epsilon) // CHANGE THIS
        {
            ROS_INFO("Reached goal!");
            goal_=false;
            count=0;
        }
        else //control
        {
        struct timeval current_time;
        gettimeofday( &previous_time, NULL );

                ROS_INFO_STREAM("position error norm: "<< error.transpose().norm());
        //timespec current_time;
            //clock_gettime(CLOCK_MONOTONIC_RAW, &current_time);
        //double period = (double) current_time.tv_sec + (double) 1e-6 * tv.tv_usec;

        ros::Time current_time = ros::Time::now();
            //double period = (current_time.tv_sec - previous_time.tv_sec) + ((current_time.tv_usec - previous_time.tv_usec) / 1000000.0);
        double period=0.020;
        previous_time=current_time;
            //std::cout << "time diff: " << period << std::endl;

                //Eigen::Matrix<double,6,6> vel_cmd_current=Kp*(goal_pose_-current_pose_).transpose() + Kd*(current_pose_-previous_pose_).transpose()*freq_;
            Eigen::Matrix<double,6,1> vel_cm_previous_vec;
            vel_cm_previous_vec << vel_cmd_previous(0,0), vel_cmd_previous(1,1),vel_cmd_previous(2,2),vel_cmd_previous(3,3),vel_cmd_previous(4,4),vel_cmd_previous(5,5);
        Eigen::Matrix<double,6,1> current_error=(goal_pose_ - previous_pose_);
            Eigen::Matrix<double,6,1> vdiff = (current_error - (goal_pose_ - vel_cm_previous_vec));   // Differential Part
            Eigen::Matrix<double,6,1> current_accum_error = (current_error + previous_accum_error);   // Integral Part

                Eigen::Matrix<double,6,6> vel_cmd_current=Kp*(goal_pose_-current_pose_).transpose() + Kd*vdiff.transpose() + Ki*current_accum_error.transpose();

        std::cout << "vdiff: " << vdiff.transpose() << std::endl;
        std::cout << "error: " << (goal_pose_-current_pose_).transpose() << std::endl;
        std::cout << "vel_cm_current: "<< vel_cmd_current.transpose() << std::endl;
                vel_cmd_msg.linear.x=vel_cmd_current(0,0);
                vel_cmd_msg.linear.y=vel_cmd_current(1,1);
                vel_cmd_msg.linear.z=vel_cmd_current(2,2);
                //vel_cmd_msg.angular.x=vel_cmd_current(3,3);
                //vel_cmd_msg.angular.y=vel_cmd_current(4,4);
                //vel_cmd_msg.angular.z=vel_cmd_current(5,5);
                vel_cmd.publish(vel_cmd_msg);

        previous_accum_error=current_accum_error;
            vel_cmd_previous=vel_cmd_current;
        }

        previous_pose_=current_pose_;

}
*/

void PositionCommand::getUpdatedPose(const visualeyez_tracker::TrackerPose::ConstPtr& trackerPose)
{

    geometry_msgs::Twist vel_cmd_msg;

    if(!goal_)
    {
        vel_cmd.publish(vel_cmd_msg);
        return;
    }
    std::cout << trackerPose->tracker_id << " " << base_marker_id_<<  std::endl;
    //if (base_marker_id_.compare(std::string(trackerPose->tracker_id)))
    //if(trackerPose->tracker_id==base_marker_id_)
    unsigned pos = base_marker_id_.find("Channel");
    std::string strBase =base_marker_id_.substr(7,10);
    std::string strMsg =trackerPose->tracker_id.substr(7,10);
    int baseInt =atoi(strBase.c_str());
    int msgInt =atoi(strMsg.c_str());
    std::cout << baseInt << std::endl;
    std::cout << msgInt << std::endl;
    if (head_marker_id_.compare(std::string(trackerPose->tracker_id)) == 0)
   // if(baseInt==msgInt)
    {
        ROS_INFO_STREAM(" Head Tracker");
        head_marker_position=Eigen::Vector3d(trackerPose->pose.x/1000.0f, trackerPose->pose.y/1000.0f, trackerPose->pose.z/1000.0f);
        got_head_marker_=true;
    }
    else// if(trackerPose->tracker_id==head_marker_id_)
    {
        ROS_INFO_STREAM(" Base Tracker");
        base_marker_position=Eigen::Vector3d(trackerPose->pose.x/1000.0f, trackerPose->pose.y/1000.0f, trackerPose->pose.z/1000.0f);
        got_base_marker_=true;
    }

    // If both markers are available, do the control
    if(got_base_marker_ && got_head_marker_)
    {
        if(count==0)
        {

            previous_time = ros::Time::now();
            previous_accum_error << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;

            ++count;
            return;
        }

        // Define a local reference frame
        Eigen::Vector3d uav_x=(head_marker_position-base_marker_position).normalized(); // Heading

        Eigen::Vector3d uav_y=(uav_x.cross(Eigen::Vector3d::UnitZ())).normalized();
        // LETS ASSUME THE UAV Z POINTS ALWAYS UP

        Eigen::Matrix<double, 3, 3> current_rotation_matrix;
        current_rotation_matrix << uav_x, uav_y, Eigen::Vector3d::UnitZ();
        Eigen::Matrix<double, 3, 1> current_euler = current_rotation_matrix.eulerAngles(2, 1, 0);

        double yaw = current_euler(0,0);
        double pitch = current_euler(1,0);
        double roll = current_euler(2,0);

        current_pose_ << base_marker_position.x(),
                base_marker_position.y(),
                base_marker_position.z(),
                roll,
                pitch,
                yaw;

        std::cout << "current_pose: " << current_pose_ << std::endl;
        std::cout << "goal_pose: " << goal_pose_ << std::endl;

        Eigen::Matrix<double,6,1> error=goal_pose_-current_pose_;
        // Check if goal was reached
        if(error.norm()<epsilon) // CHANGE THIS
        {
            ROS_INFO("Reached goal!");
            goal_=false;
            count=0;
        }
        else //control
        {
            ros::Time current_time = ros::Time::now();

            double period=current_time.toSec()-previous_time.toSec();
            std::cout << "period:" << period << std::endl;
            previous_time=current_time;
            //std::cout << "time diff: " << period << std::endl;

            //Eigen::Matrix<double,6,6> vel_cmd_current=Kp*(goal_pose_-current_pose_).transpose() + Kd*(current_pose_-previous_pose_).transpose()*freq_;
            Eigen::Matrix<double,6,1> vel_cm_previous_vec;
            vel_cm_previous_vec << vel_cmd_previous(0,0), vel_cmd_previous(1,1),vel_cmd_previous(2,2),vel_cmd_previous(3,3),vel_cmd_previous(4,4),vel_cmd_previous(5,5);
            Eigen::Matrix<double,6,1> current_error=(goal_pose_ - previous_pose_);
            Eigen::Matrix<double,6,1> vdiff = (current_error - (goal_pose_ - vel_cm_previous_vec));   // Differential Part
            Eigen::Matrix<double,6,1> current_accum_error = (current_error + previous_accum_error);   // Integral Part

            Eigen::Matrix<double,6,6> vel_cmd_current=Kp*(goal_pose_-current_pose_).transpose() + Kd*vdiff.transpose() + Ki*current_accum_error.transpose();

            std::cout << "vdiff: " << vdiff.transpose() << std::endl;
            std::cout << "error: " << (goal_pose_-current_pose_).transpose() << std::endl;
            //std::cout << "vel_cm_current: "<< vel_cmd_current.transpose() << std::endl;
            vel_cmd_msg.linear.x=vel_cmd_current(0,0);
            vel_cmd_msg.linear.y=vel_cmd_current(1,1);
            vel_cmd_msg.linear.z=vel_cmd_current(2,2);
            //vel_cmd_msg.angular.x=vel_cmd_current(3,3);
            //vel_cmd_msg.angular.y=vel_cmd_current(4,4);
            //vel_cmd_msg.angular.z=vel_cmd_current(5,5);
            vel_cmd.publish(vel_cmd_msg);

            previous_accum_error=current_accum_error;
            vel_cmd_previous=vel_cmd_current;
        }

        previous_pose_=current_pose_;

    }
}

void PositionCommand::getGoal(const geometry_msgs::PoseStamped::ConstPtr & goal)
{
    ROS_INFO("GOT NEW GOAL");
    Eigen::Matrix<double,3,1> euler=Eigen::Quaterniond(goal->pose.orientation.w,
                                                       goal->pose.orientation.x,
                                                       goal->pose.orientation.y,
                                                       goal->pose.orientation.z).matrix().eulerAngles(2, 1, 0);
    double yaw = euler(0,0);
    double pitch = euler(1,0);
    double roll = euler(2,0);

    goal_pose_ << goal->pose.position.x,
            goal->pose.position.y,
            goal->pose.position.z,
            roll,
            pitch,
            yaw;
    goal_=true;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "uav_commander");
    ros::NodeHandle n;
    ros::NodeHandle n_priv("~");
    std::string base_marker_id;
    std::string head_marker_id;
    double freq;

    n_priv.param<double>("freq", freq, 50.0);
    n_priv.param<std::string>("base_marker_id", base_marker_id, "teste");
    n_priv.param<std::string>("head_marker_id", head_marker_id, "teste2");

    ///////////
    // Gains //
    ///////////

    // Proportional (kd)
    double kp_x;
    double kp_y;
    double kp_z;
    double kp_roll;
    double kp_pitch;
    double kp_yaw;

    n_priv.param<double>("kp_x", kp_x, 0.5);
    n_priv.param<double>("kp_y", kp_y, 1.0);
    n_priv.param<double>("kp_z", kp_z, 1.0);
    n_priv.param<double>("kp_roll", kp_roll, 0.0);
    n_priv.param<double>("kp_pitch", kp_pitch, 0.0);
    n_priv.param<double>("kp_yaw", kp_yaw, 1.0);

    Eigen::Matrix<double,6,1> Kp;

    Kp << kp_x, kp_y, kp_z, kp_roll, kp_pitch, kp_yaw;

    // Derivative (kd)
    double kd_x;
    double kd_y;
    double kd_z;
    double kd_roll;
    double kd_pitch;
    double kd_yaw;

    n_priv.param<double>("kd_x", kd_x, 1.0);
    n_priv.param<double>("kd_y", kd_y, 1.0);
    n_priv.param<double>("kd_z", kd_z, 1.0);
    n_priv.param<double>("kd_roll", kd_roll, 0.0);
    n_priv.param<double>("kd_pitch", kd_pitch, 0.0);
    n_priv.param<double>("kd_yaw", kd_yaw, 1.0);

    Eigen::Matrix<double,6,1> Kd;

    Kd << kd_x,kd_y,kd_z, kd_roll, kd_pitch,kd_yaw;

    // Integral (ki)
    double ki_x;
    double ki_y;
    double ki_z;
    double ki_roll;
    double ki_pitch;
    double ki_yaw;

    n_priv.param<double>("ki_x", ki_x, 1.0);
    n_priv.param<double>("ki_y", ki_y, 1.0);
    n_priv.param<double>("ki_z", ki_z, 1.0);
    n_priv.param<double>("ki_roll", ki_roll, 0.0);
    n_priv.param<double>("ki_pitch", ki_pitch, 0.0);
    n_priv.param<double>("ki_yaw", ki_yaw, 1.0);

    Eigen::Matrix<double,6,1> Ki;

    Ki << ki_x, ki_y, ki_z, ki_roll, ki_pitch, ki_yaw;

    std::cout << "Kp gains:" << Kp << std::endl;
    std::cout << "Kd gains:" << Kd << std::endl;
    std::cout << "Ki gains:" << Ki << std::endl;
    PositionCommand position_commander(n, base_marker_id, head_marker_id, Kp, Kd, Ki);
    ros::Rate loop_rate(freq);
    while (ros::ok())
    {
        ros::spinOnce();

        loop_rate.sleep();
    }

    return 0;
}


#pragma once

#include <Eigen/Core>

#include <ccd/collision_detection.hpp>
#include <opt/collision_constraint.hpp>

namespace ccd {

namespace opt {

    class VolumeConstraint : public CollisionConstraint {
    public:
        VolumeConstraint();
        VolumeConstraint(const std::string& name);

        void settings(const nlohmann::json& json) override;
        nlohmann::json settings() const override;

        std::vector<EdgeVertexImpact> initialize(
            const Eigen::MatrixX2d& vertices,
            const Eigen::MatrixX2i& edges,
            const Eigen::VectorXi& group_ids,
            const Eigen::MatrixXd& Uk);

        std::vector<EdgeEdgeImpact>
        get_ee_collision_set(const Eigen::MatrixXd& Uk);

        void compute_constraints(
            const Eigen::MatrixXd& Uk,
            const std::vector<EdgeEdgeImpact>& ee_impacts,
            Eigen::VectorXd& g_uk);

        void compute_constraints_jacobian(
            const Eigen::MatrixXd& Uk,
            const std::vector<EdgeEdgeImpact>& ee_impacts,
            Eigen::MatrixXd& jac_uk);

        void compute_constraints_normals(
            const Eigen::MatrixXd& Uk,
            const std::vector<EdgeEdgeImpact>& ee_impacts,
            Eigen::MatrixXd& jac_uk);

        void compute_constraints(
            const Eigen::MatrixXd& Uk,
            const std::vector<EdgeEdgeImpact>& ee_impacts,
            Eigen::VectorXd& g_uk,
            Eigen::MatrixXd& g_uk_jacobian);

        // This versions of the functions get their own ee_impacts;
        void
        compute_constraints(const Eigen::MatrixXd& Uk, Eigen::VectorXd& g_uk);

        void compute_constraints(
            const Eigen::MatrixXd& Uk,
            Eigen::VectorXd& g_uk,
            Eigen::MatrixXd& g_uk_jacobian);

        int number_of_constraints();

        // Settings
        // ----------
        double volume_epsilon;
        int num_constraints;

        double time_epsilon;

        void dense_indices(
            const std::vector<EdgeEdgeImpact>& ee_impacts,
            Eigen::VectorXi& dense_indices);

        /// @brief #E,1 indices of the edges' first impact
        Eigen::VectorXi m_edge_impact_map;
    };

    long get_constraint_index(
        const EdgeEdgeImpact& impact, const bool impacted, const int num_edges);

    long get_constraints_size(const int num_edges);

} // namespace opt
} // namespace ccd


/*******************************************************************************
 *         Copyright 2003 & onward LASMEA UMR 6602 CNRS/Univ. Clermont II
 *         Copyright 2009 & onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_SDK_CONSTANT_DSL_DIGITS_HPP_INCLUDED
#define NT2_SDK_CONSTANT_DSL_DIGITS_HPP_INCLUDED

////////////////////////////////////////////////////////////////////////////////
// Turn some digits consatnt into DSL terminals
////////////////////////////////////////////////////////////////////////////////
#include <boost/proto/proto.hpp>
#include <nt2/sdk/constant/digits.hpp>
#include <nt2/sdk/constant/category.hpp>

namespace nt2
{
  boost::proto::terminal< constant_<tag::digit_< -10 > > >::type mten_    = {{}};
  boost::proto::terminal< constant_<tag::digit_< -9 > > >::type mnine_    = {{}};
  boost::proto::terminal< constant_<tag::digit_< -8 > > >::type meight_   = {{}};
  boost::proto::terminal< constant_<tag::digit_< -7 > > >::type mseven_   = {{}};
  boost::proto::terminal< constant_<tag::digit_< -6 > > >::type msix_     = {{}};
  boost::proto::terminal< constant_<tag::digit_< -5 > > >::type mfive_    = {{}};
  boost::proto::terminal< constant_<tag::digit_< -4 > > >::type mfour_    = {{}};
  boost::proto::terminal< constant_<tag::digit_< -3 > > >::type mthree_   = {{}};
  boost::proto::terminal< constant_<tag::digit_< -2 > > >::type mtwo_     = {{}};
  boost::proto::terminal< constant_<tag::digit_< -1 > > >::type mone_     = {{}};
  boost::proto::terminal< constant_<tag::digit_< 0 > > >::type zero_      = {{}};
  boost::proto::terminal< constant_<tag::digit_< 1 > > >::type one_       = {{}};
  boost::proto::terminal< constant_<tag::digit_< 2 > > >::type two_       = {{}};
  boost::proto::terminal< constant_<tag::digit_< 3 > > >::type three_     = {{}};
  boost::proto::terminal< constant_<tag::digit_< 4 > > >::type four_      = {{}};
  boost::proto::terminal< constant_<tag::digit_< 5 > > >::type five_      = {{}};
  boost::proto::terminal< constant_<tag::digit_< 6 > > >::type six_       = {{}};
  boost::proto::terminal< constant_<tag::digit_< 7 > > >::type seven_     = {{}};
  boost::proto::terminal< constant_<tag::digit_< 8 > > >::type eight_     = {{}};
  boost::proto::terminal< constant_<tag::digit_< 9 > > >::type nine_      = {{}};
  boost::proto::terminal< constant_<tag::digit_< 10 > > >::type ten_      = {{}};
  boost::proto::terminal< constant_<tag::digit_< 20 > > >::type twenty_   = {{}};
  boost::proto::terminal< constant_<tag::digit_< 100> > >::type hundred_  = {{}};
  boost::proto::terminal< constant_<tag::digit_<1000> > >::type thousand_ = {{}};

  boost::proto::terminal< constant_<tag::digit_< 45 > > >::type fortyfive_        = {{}};
  boost::proto::terminal< constant_<tag::digit_< 90 > > >::type ninety_           = {{}};
  boost::proto::terminal< constant_<tag::digit_< 180> > >::type onehundredeighty_ = {{}};
}

#endif


/*
* dixonfix - command line tool for correcting fat-water swaps in Dixon MRI.
*
* Copyright 2016 Ben Glocker <b.glocker@imperial.ac.uk>
*
* 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.
*
* Please cite:
*  B. Glocker, E. Konukoglu, I. Lavdas, J.E. Iglesias, E.O. Aboagye, A.G. Rockall, D. Rueckert
*  Correction of Fat-Water Swaps in Dixon MRI
*  International Conference on Medical Image Computing and Computer Assisted Intervention (MICCAI), 2016
*
*/

#include "RandomField.h"
#include "Optimization.h"
#include "miaImage.h"
#include "miaImageProcessing.h"
#include "itkio.h"
#include "itkBilateralImageFilter.h"

#include <vector>
#include <iostream>
#include <chrono>

#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

namespace po = boost::program_options;
namespace fs = boost::filesystem;

using namespace mrfopt;
using namespace mia;

class DixonFixEnergyFunction : public EnergyFunction<double, int>
{
public:
  DixonFixEnergyFunction(const Image &likelihoodMap0, const Image &likelihoodMap1, const std::vector<int> &nodeImageIndices, double lambda)
    : m_likelihood_0(likelihoodMap0)
    , m_likelihood_1(likelihoodMap1)
    , m_node_img_indices(nodeImageIndices)
    , m_lambda(lambda)
  {}

  double unary_potential(int nodeIndex, int label) const override
  {
    int img_idx = m_node_img_indices[nodeIndex];
    return (label == 0) ? m_likelihood_0.data()[img_idx] : m_likelihood_1.data()[img_idx];
  }

  double clique_potential(const Clique& clique, const std::vector<int>& labels) const override
  {
    auto clique_size = static_cast<int>(clique.size());
    if (clique_size == 2)
    {
      return pairwise_potential(clique.nodes[0], clique.nodes[1], labels[0], labels[1]);
    }
    else
    {
      return 0;
    }
  }

  double pairwise_potential(int nodeA, int nodeB, int labelA, int labelB) const
  {
    return (labelA == labelB) ? 0.0 : m_lambda;
  }

private:
  Image m_likelihood_0;
  Image m_likelihood_1;
  std::vector<int> m_node_img_indices;
  double m_lambda;
};

void likelihood_maps(const Image &f, const Image &w, const Image &fp, const Image &wp, const Image &fu, const Image &wu, const Image &labelmap, Image &map0, Image &map1)
{
  for (size_t i = 0; i < f.size(); i++)
  {
    if (labelmap.data()[i] > 0)
    {
      auto fu_i = std::max(fu.data()[i], 1.0f);
      auto wu_i = std::max(wu.data()[i], 1.0f);

      auto sqDiffF0 = abs(f.data()[i] - fp.data()[i]) / fu_i;
      auto sqDiffW0 = abs(w.data()[i] - wp.data()[i]) / wu_i;
      map0.data()[i] = sqDiffW0 + sqDiffF0;

      auto sqDiffF1 = abs(f.data()[i] - wp.data()[i]) / wu_i;
      auto sqDiffW1 = abs(w.data()[i] - fp.data()[i]) / fu_i;
      map1.data()[i] = sqDiffW1 + sqDiffF1;

      //map0.data()[i] = sqDiffW0 / (sqDiffW0 + sqDiffW1) + sqDiffF0 / (sqDiffF0 + sqDiffF1);
      //map1.data()[i] = sqDiffW1 / (sqDiffW0 + sqDiffW1) + sqDiffF1 / (sqDiffF0 + sqDiffF1);
    }
  }
}

void bilateral_filter(const Image &input, Image &output, double domainSigma, double rangeSigma)
{
  typedef float PixelType;
  const unsigned int Dimension = 3;
  typedef itk::Image< PixelType, Dimension > ImageType;

  typedef itk::BilateralImageFilter<ImageType, ImageType> FilterType;
  FilterType::Pointer filter = FilterType::New();

  ImageType::Pointer itkInputImage = itkio::miaToItk(input);
  double domainSigmas[Dimension];
  domainSigmas[0] = domainSigma;
  domainSigmas[1] = domainSigma;
  domainSigmas[2] = domainSigma;
  filter->SetInput(itkInputImage);
  filter->SetDomainSigma(domainSigmas);
  filter->SetRangeSigma(rangeSigma);
  filter->Update();

  ImageType::Pointer itkOutputImage = filter->GetOutput();

  ImageType::RegionType region = itkOutputImage->GetLargestPossibleRegion();
  ImageType::IndexType startIndex;
  startIndex.Fill(0);
  region.SetIndex(startIndex);

  typedef itk::ImageRegionConstIterator< ImageType > ConstIteratorType;
  ConstIteratorType outputImageIt(itkOutputImage, region);
  size_t index = 0;
  while (!outputImageIt.IsAtEnd())
  {
    output.data()[index] = outputImageIt.Get();
    ++outputImageIt;
    index++;
  }
}

Graph masked_grid_graph_first_order(const Image &mask, std::vector<int> &nodeImageIndices)
{
  int nodesX = mask.sizeX();
  int nodesY = mask.sizeY();
  int nodesZ = mask.sizeZ();

  int num_nodes = 0;
  Image idx = mask.clone();
  fill(idx, -1);
  for (size_t i = 0; i < mask.size(); i++)
  {
    if (mask.data()[i] > 0)
    {
      idx.data()[i] = num_nodes;
      nodeImageIndices.push_back(i);
      num_nodes++;
    }
  }

  Graph graph;
  graph.num_nodes(num_nodes);

  for (int z = 0; z < nodesZ; z++)
  {
    for (int y = 0; y < nodesY; y++)
    {
      for (int x = 0; x < nodesX; x++)
      {
        int index = x + y * nodesX + z * nodesX * nodesY;
        if (mask.data()[index] > 0)
        {
          if (x < nodesX - 1)
          {
            if (mask.data()[index + 1] > 0)
            {
              graph.add_clique(Clique(idx.data()[index], idx.data()[index + 1]));
            }
          }
          if (y < nodesY - 1)
          {
            if (mask.data()[index + nodesX] > 0)
            {
              graph.add_clique(Clique(idx.data()[index], idx.data()[index + nodesX]));
            }
          }
          if (z < nodesZ - 1)
          {
            if (mask.data()[index + nodesX * nodesY] > 0)
            {
              graph.add_clique(Clique(idx.data()[index], idx.data()[index + nodesX* nodesY]));
            }
          }
        }
      }
    }
  }

  return graph;
}

void swap_labeling(const Image &f, const Image &w, const Image &fp, const Image &wp, const Image &fu, const Image &wu, Image &labelmap, double lambda, std::string folderOut, bool writeTemp)
{
  std::cout << "+ computing likelihood maps...";
  auto start = std::chrono::high_resolution_clock::now();

  Image map0 = f.clone();
  Image map1 = f.clone();
  zeros(map0);
  zeros(map1);
  likelihood_maps(f, w, fp, wp, fu, wu, labelmap, map0, map1);

  if (writeTemp)
  {
    map0.dataType(mia::FLOAT);
    map1.dataType(mia::FLOAT);

    std::stringstream filename_likelihood_0;
    filename_likelihood_0 << folderOut << "/likelihood_0.nii.gz";
    itkio::save(map0, filename_likelihood_0.str());
    std::stringstream filename_likelihood_1;
    filename_likelihood_1 << folderOut << "/likelihood_1.nii.gz";
    itkio::save(map1, filename_likelihood_1.str());
  }

  auto stop = std::chrono::high_resolution_clock::now();
  std::cout << "done. (" << std::chrono::duration_cast< std::chrono::milliseconds >(stop - start).count() << " ms)" << std::endl;

  std::cout << "+ running max-flow..." << std::endl;
  start = std::chrono::high_resolution_clock::now();

  std::vector<int> nodeImageIndices;
  Graph graph = masked_grid_graph_first_order(labelmap, nodeImageIndices);
  DixonFixEnergyFunction function(map0, map1, nodeImageIndices, lambda);

  std::vector<int> labeling(graph.num_nodes());
  std::fill(labeling.begin(), labeling.end(), 0);

  std::vector<int> proposal(graph.num_nodes());
  std::fill(proposal.begin(), proposal.end(), 1);

  auto energy_initial = compute_energy(graph, function, labeling);
  std::cout << " - initial energy:\t" << energy_initial << std::endl;

  int numImprovements = 0;
  Reduction reductionMode = HOCR;
  fusion_move(graph, function, labeling, proposal, numImprovements, reductionMode);

  auto energy_final = compute_energy(graph, function, labeling);
  std::cout << " - final energy:\t" << energy_final << std::endl;

  // generate swap labeling map
  int node_idx = 0;
  for (int i = 0; i < labelmap.size(); i++)
  {
    if (labelmap.data()[i] > 0)
    {
      labelmap.data()[i] = (labeling[node_idx] == 1) ? 1.0f : 0.0f;
      node_idx++;
    }
  }

  stop = std::chrono::high_resolution_clock::now();
  std::cout << " - finished. (" << std::chrono::duration_cast< std::chrono::milliseconds >(stop - start).count() << " ms)" << std::endl;
}

int main(int argc, char* argv[])
{
  std::string filename_F;
  std::string filename_W;
  std::string filename_F_pred;
  std::string filename_W_pred;
  std::string filename_F_std;
  std::string filename_W_std;
  std::string folder_output;
  double spacing;
  double lambda;
  //double domain_sigma;
  //double range_sigma;

  bool write_temp;

  try
  {
    // Declare the supported options.
    po::options_description options("options");
    options.add_options()
    ("help,h", "produce help message")
    ("temp,t", po::bool_switch(&write_temp)->default_value(false), "enable output of temporary files")
    ("fat,f", po::value<std::string>(&filename_F), "filename of fat image")
    ("water,w", po::value<std::string>(&filename_W), "filename of water image")
    ("fat_prediction,p", po::value<std::string>(&filename_F_pred), "filename of predicted fat image")
    ("water_prediction,q", po::value<std::string>(&filename_W_pred), "filename of predicted water image")
    ("fat_uncertainty,u", po::value<std::string>(&filename_F_std), "filename of predicted fat image uncertainty map")
    ("water_uncertainty,v", po::value<std::string>(&filename_W_std), "filename of predicted water image uncertainty map")
    ("output,o", po::value<std::string>(&folder_output), "foldername for output")
    ("spacing,s", po::value<double>(&spacing), "voxel spacing in mm used for computations")
    ("lambda,l", po::value<double>(&lambda), "lambda for spatial regularization")
    //("domain_sigma,d", po::value<double>(&domain_sigma), "domain sigma for bilateral filter")
    //("range_sigma,r", po::value<double>(&range_sigma), "range sigma for bilateral filter")
    ;

    po::variables_map vm;

    po::store(po::parse_command_line(argc, argv, options), vm);
    po::notify(vm);

    if (vm.count("help") || vm.size() == 0)
    {
      std::cout << options << std::endl;
      return 0;
    }
  }
  catch (std::exception& e)
  {
    std::cout << e.what() << std::endl;
    return 1;
  }

  std::cout << std::endl;
  std::cout << "            DIXON-FIX" << std::endl;
  std::cout << "---------------------------------" << std::endl;

  std::cout << "+ loading and resampling images...";
  auto start = std::chrono::high_resolution_clock::now();

  Image F = itkio::load(filename_F);
  Image W = itkio::load(filename_W);
  Image FP = itkio::load(filename_F_pred);
  Image WP = itkio::load(filename_W_pred);
  Image FU = itkio::load(filename_F_std);
  Image WU = itkio::load(filename_W_std);

  // resample all into native resolution of F
  Image W_res = F.clone();
  resample(W, W_res, mia::LINEAR);

  Image FP_res = F.clone();
  Image WP_res = F.clone();
  resample(FP, FP_res, mia::LINEAR);
  resample(WP, WP_res, mia::LINEAR);

  Image FU_res = F.clone();
  Image WU_res = F.clone();
  resample(FU, FU_res, mia::LINEAR);
  resample(WU, WU_res, mia::LINEAR);

  // low resolution images for first stage
  Image F_low_resampled = resample(F, Eigen::Vector3d(spacing, spacing, spacing), mia::LINEAR);
  Image F_low_res = subimage(F_low_resampled, 1, 1, 1, F_low_resampled.sizeX() - 2, F_low_resampled.sizeY() - 2, F_low_resampled.sizeZ() - 2).clone();
  Image W_low_res = F_low_res.clone();
  resample(W, W_low_res, mia::LINEAR);

  Image FP_low_res = F_low_res.clone();
  Image WP_low_res = W_low_res.clone();
  resample(FP, FP_low_res, mia::LINEAR);
  resample(WP, WP_low_res, mia::LINEAR);

  Image FU_low_res = F_low_res.clone();
  Image WU_low_res = W_low_res.clone();
  resample(FU, FU_low_res, mia::LINEAR);
  resample(WU, WU_low_res, mia::LINEAR);

  auto stop = std::chrono::high_resolution_clock::now();
  std::cout << "done. (" << std::chrono::duration_cast< std::chrono::milliseconds >(stop - start).count() << " ms)" << std::endl;

  //std::cout << "+ smoothing images and predictions...";
// auto start = std::chrono::high_resolution_clock::now();

// bilateral_filter(f, f_filtered, domainSigma, rangeSigma);
// bilateral_filter(w, w_filtered, domainSigma, rangeSigma);
// bilateral_filter(fp, fp_filtered, domainSigma, rangeSigma);
// bilateral_filter(wp, wp_filtered, domainSigma, rangeSigma);
  //gauss(F_low_res, F_low_res, domainSigma, domainSigma, domainSigma);
  //gauss(W_low_res, W_low_res, domainSigma, domainSigma, domainSigma);
  //gauss(FP_res, FP_res, domainSigma, domainSigma, domainSigma);
  //gauss(WP_res, WP_res, domainSigma, domainSigma, domainSigma);

  //if (writeTemp)
  //{
  //  std::stringstream filename_F_bilateral;
  //  filename_F_bilateral << folderOut << "/F_bilateral.nii.gz";
  //  itkio::save(f_filtered, filename_F_bilateral.str());

  //  std::stringstream filename_W_bilateral;
  //  filename_W_bilateral << folderOut << "/W_bilateral.nii.gz";
  //  itkio::save(w_filtered, filename_W_bilateral.str());

  //  std::stringstream filename_FP_bilateral;
  //  filename_FP_bilateral << folderOut << "/FP_bilateral.nii.gz";
  //  itkio::save(fp_filtered, filename_FP_bilateral.str());

  //  std::stringstream filename_WP_bilateral;
  //  filename_WP_bilateral << folderOut << "/WP_bilateral.nii.gz";
  //  itkio::save(wp_filtered, filename_WP_bilateral.str());
  //}

  //auto stop = std::chrono::high_resolution_clock::now();
  //std::cout << "done. (" << std::chrono::duration_cast<std::chrono::milliseconds>(stop - start).count() << " ms)" << std::endl;

  std::cout << std::endl;
  std::cout << "+ FIRST STAGE - LOW RESOLUTION" << std::endl;
  std::cout << "---------------------------------" << std::endl;

  // compute swap labeling for low resolution images
  Image labelmap_low_res = F_low_res.clone();
  mia::ones(labelmap_low_res);
  swap_labeling(F_low_res, W_low_res, FP_low_res, WP_low_res, FU_low_res, WU_low_res, labelmap_low_res, lambda, folder_output, false);

  //if (write_temp)
  //{
  //  for (int i = 0; i < labelmap_low_res.size(); i++)
  //  {
  //    if (labelmap_low_res.data()[i] > 0.0f)
  //    {
  //      float temp = F_low_res.data()[i];
  //      F_low_res.data()[i] = W_low_res.data()[i];
  //      W_low_res.data()[i] = temp;
  //    }
  //  }

  //  std::stringstream filename_F_low_res_fixed;
  //  filename_F_low_res_fixed << folder_output << "/F_low_res_fixed.nii.gz";
  //  itkio::save(F_low_res, filename_F_low_res_fixed.str());

  //  std::stringstream filename_W_low_res_fixed;
  //  filename_W_low_res_fixed << folder_output << "/W_low_res_fixed.nii.gz";
  //  itkio::save(W_low_res, filename_W_low_res_fixed.str());
  //}

  std::cout << std::endl;
  std::cout << "+ SECOND STAGE - FULL RESOLUTION" << std::endl;
  std::cout << "---------------------------------" << std::endl;

  // one voxel dilation on swap labeling
  dilate_binary(labelmap_low_res.clone(), labelmap_low_res);

  // compute constrainted swap labeling on native resolution of F
  Image labelmap = F.clone();
  resample(labelmap_low_res, labelmap, mia::NEAREST);
  swap_labeling(F, W_res, FP_res, WP_res, FU_res, WU_res, labelmap, lambda, folder_output, write_temp);

  std::cout << "+ applying swap labeling...";
  start = std::chrono::high_resolution_clock::now();

  // apply swap labeling
  for (int i = 0; i < labelmap.size(); i++)
  {
    if (labelmap.data()[i] > 0.0f)
    {
      float temp = F.data()[i];
      F.data()[i] = W_res.data()[i];
      W_res.data()[i] = temp;
    }
  }

  stop = std::chrono::high_resolution_clock::now();
  std::cout << "done. (" << std::chrono::duration_cast< std::chrono::milliseconds >(stop - start).count() << " ms)" << std::endl;

  std::cout << "+ saving results...";
  start = std::chrono::high_resolution_clock::now();

  if (!fs::exists(folder_output)) fs::create_directories(folder_output);

  std::stringstream filename_f_fixed;
  filename_f_fixed << folder_output << "/f_fixed.nii.gz";
  itkio::save(F, filename_f_fixed.str());

  std::stringstream filename_w_fixed;
  filename_w_fixed << folder_output << "/w_fixed.nii.gz";
  itkio::save(W_res, filename_w_fixed.str());

  std::stringstream filename_labelmap;
  filename_labelmap << folder_output << "/labelmap.nii.gz";
  itkio::save(labelmap, filename_labelmap.str());

  stop = std::chrono::high_resolution_clock::now();
  std::cout << "done. (" << std::chrono::duration_cast< std::chrono::milliseconds >(stop - start).count() << " ms)" << std::endl;

  std::cout << "--------------------" << std::endl;
}


#define BIORBD_API_EXPORTS
#include "RigidBody/Integrator.h"

#include <Eigen/Dense>
#include <boost/numeric/odeint.hpp>
#include <rbdl/Dynamics.h>
#include "Utils/Error.h"
#include "Utils/String.h"
#include "RigidBody/GeneralizedCoordinates.h"
#include "RigidBody/Joints.h"

biorbd::rigidbody::Integrator::Integrator() :
    m_nbre(std::shared_ptr<unsigned int>()),
    m_steps(std::shared_ptr<unsigned int>()),
    m_model(std::shared_ptr<RigidBodyDynamics::Model>()),
    m_x_vec(std::shared_ptr<std::vector<state_type>>()),
    m_times(std::shared_ptr<std::vector<double>>()),
    m_u(std::shared_ptr<biorbd::utils::Vector>())

{

}

biorbd::rigidbody::Integrator biorbd::rigidbody::Integrator::DeepCopy() const
{
    biorbd::rigidbody::Integrator copy;
    copy.DeepCopy(*this);
    return copy;
}

void biorbd::rigidbody::Integrator::DeepCopy(const biorbd::rigidbody::Integrator &other)
{
    *m_nbre = *other.m_nbre;
    *m_steps = *other.m_steps;
    *m_model = *other.m_model;
    m_x_vec->resize(other.m_x_vec->size());
    for (unsigned int i=0; i<other.m_x_vec->size(); ++i)
        (*m_x_vec)[i] = (*other.m_x_vec)[i];
    m_times->resize(other.m_times->size());
    for (unsigned int i=0; i<other.m_times->size(); ++i)
        (*m_times)[i] = (*other.m_times)[i];
    *m_u = *other.m_u;
}

void biorbd::rigidbody::Integrator::operator() (
        const state_type &x ,
        state_type &dxdt ,
        double ){
    // Équation différentielle : x/xdot => xdot/xddot
    biorbd::rigidbody::GeneralizedCoordinates Q(*m_nbre);
    biorbd::rigidbody::GeneralizedCoordinates QDot(*m_nbre);
    biorbd::rigidbody::GeneralizedCoordinates QDDot(biorbd::utils::Vector(*m_nbre).setZero());
    for (unsigned int i=0; i<*m_nbre; i++){
        Q(i) = x[i];
        QDot(i) = x[i+*m_nbre];
    }

    RigidBodyDynamics::ForwardDynamics (*m_model, Q, QDot, *m_u, QDDot);

    // Faire sortir xdot/xddot
    for (unsigned int i=0; i<*m_nbre; i++){
        dxdt[i] = QDot[i];
        dxdt[i + *m_nbre] = QDDot[i];
    }

}

void biorbd::rigidbody::Integrator::showAll(){
    std::cout << "Test:" << std::endl;
    for (unsigned int i=0; i<=*m_steps; i++){
        std::cout << (*m_times)[i];
        for (unsigned int j=0; j<*m_nbre; j++)
            std::cout << " " << (*m_x_vec)[i][j];
        std::cout << std::endl;
    }
}

unsigned int biorbd::rigidbody::Integrator::steps() const
{
    return *m_steps+1;
}

biorbd::utils::Vector biorbd::rigidbody::Integrator::getX(
        unsigned int idx){
    biorbd::utils::Vector out(*m_nbre*2);
    biorbd::utils::Error::check(idx <= *m_steps, "Trying to get Q outside range");
    for (unsigned int i=0; i<*m_nbre*2; i++){
        out(i) = (*m_x_vec)[idx][i];
        }
    return out;
}

void biorbd::rigidbody::Integrator::integrate(
        biorbd::rigidbody::Joints& model,
        const biorbd::utils::Vector &Q_Qdot,
        const biorbd::utils::Vector &u,
        double t0,
        double tend,
        double timeStep){
    // Stocker le nombre d'élément à traiter
    *m_nbre = static_cast<unsigned int>(Q_Qdot.rows())/2; // Q et Qdot
    *m_u = u; // Copier les effecteurs
    *m_model = model;

    // Remplissage de la variable par les positions et vitesse
    state_type x(*m_nbre*2);
    for (unsigned int i=0; i<*m_nbre*2; i++)
        x[i] = Q_Qdot(i);

    // Choix de l'algorithme et intégration
    boost::numeric::odeint::runge_kutta4< state_type > stepper;
    *m_steps = static_cast<unsigned int>(boost::numeric::odeint::integrate_const( stepper, (*this), x, t0, tend, timeStep, push_back_state_and_time( *m_x_vec , *m_times )));
}


#include <boost/multiprecision/cpp_int.hpp>

#include <fc/crypto/sha256.hpp>
#include <fc/exception/exception.hpp>

#include <fstream>
#include <iomanip>
#include <iostream>

uint64_t endian_reverse( uint64_t x )
{
   uint64_t x0 = ((x        ) & 0xFF);
   uint64_t x1 = ((x >> 0x08) & 0xFF);
   uint64_t x2 = ((x >> 0x10) & 0xFF);
   uint64_t x3 = ((x >> 0x18) & 0xFF);
   uint64_t x4 = ((x >> 0x20) & 0xFF);
   uint64_t x5 = ((x >> 0x28) & 0xFF);
   uint64_t x6 = ((x >> 0x30) & 0xFF);
   uint64_t x7 = ((x >> 0x38) & 0xFF);

   return (x0 << 0x38)
        | (x1 << 0x30)
        | (x2 << 0x28)
        | (x3 << 0x20)
        | (x4 << 0x18)
        | (x5 << 0x10)
        | (x6 << 0x08)
        | (x7        );
}

int main(int argc, char**argv, char** envp)
{
   std::ifstream infile("log_test.txt");
   uint32_t ref_clz;
   std::string str_h;
   uint32_t ref_log;
   uint32_t cases = 0;
   uint32_t errors = 0;

   while( true )
   {
      if( !(infile >> std::hex >> ref_clz) )
         break;
      if( !(infile >> str_h) )
         break;
      if( !(infile >> std::hex >> ref_log) )
         break;
      fc::sha256 h(str_h);
      if( ref_clz != h.clz() )
      {
         std::cerr << "got error on clz(" << str_h << ")" << std::endl;
         ++errors;
      }
      if( ref_log != h.approx_log_32() )
      {
         std::cerr << "got error on log(" << str_h << ")" << std::endl;
         ++errors;
      }
      double d_ilog_h_test = h.inverse_approx_log_32_double( ref_log );
      h.set_to_inverse_approx_log_32( ref_log );
      if( ref_log != h.approx_log_32() )
      {
         std::cerr << "got error on ilog(" << ref_log << ")" << std::endl;
         ++errors;
      }

      std::string str_ilog_h = h.str();
      boost::multiprecision::uint256_t u256_ilog_h( "0x" + str_ilog_h );
      double d_ilog_h_ref = u256_ilog_h.template convert_to<double>();
      if( d_ilog_h_ref != d_ilog_h_test )
      {
         std::cerr << "got error on d_ilog(" << ref_log << ")" << std::endl;
         ++errors;
      }

      if( h != fc::sha256() )
      {
         fc::sha256 h_before = h;
         if( h._hash[3] == 0 )
         {
            if( h._hash[2] == 0 )
            {
               if( h._hash[1] == 0 )
               {
                  h._hash[0] = endian_reverse( endian_reverse( h._hash[0] )-1 );
               }
               h._hash[1] = endian_reverse( endian_reverse( h._hash[1] )-1 );
            }
            h._hash[2] = endian_reverse( endian_reverse( h._hash[2] )-1 );
         }
         h._hash[3] = endian_reverse( endian_reverse( h._hash[3] )-1 );
         bool ok = (h.approx_log_32() < ref_log);
         if( !ok )
         {
            std::cerr << "got error on logm1 for " << ref_log << std::endl;
            std::cerr << "h0:" << str_h << std::endl;
            std::cerr << "h1:" << h_before.str() << std::endl;
            std::cerr << "h2:" << h.str() << std::endl;
            std::cerr << "ref_log:" << std::hex << std::setw(8) << ref_log << std::endl;
            std::cerr << "log(h) :" << std::hex << std::setw(8) << h.approx_log_32() << std::endl;
            std::cerr << std::endl;
            ++errors;
         }
      }

      ++cases;
   }

   std::cerr << "sha256_log_test checked " << cases << " cases, got " << errors << " errors" << std::endl;
   if( errors )
      return 1;
   return 0;
}


//==============================================================================
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_SDK_BENCH_STATS_AVERAGE_HPP_INCLUDED
#define NT2_SDK_BENCH_STATS_AVERAGE_HPP_INCLUDED

#include <boost/accumulators/statistics/mean.hpp>
#include <string>

namespace nt2 { namespace bench { namespace stats
{
  /*!
    @brief Average statistics

    Used as a Metric parameter, average_ make the metric computes the
    average value of its samples
  **/
  struct average_
  {
    /// @brief Samples processing function
    template<typename Samples> static inline double evaluate(Samples const& d)
    {
      return boost::accumulators::mean(d);
    }

    /// @brief Statistics display
    static inline std::string unit() { return " (avg)"; }
  };
} } }

#endif


#pragma once
#include "arpackdef.h"

#include <Eigen/Dense>
#include <random>
#include <vector>

extern "C"
{
    void dsaupd_(a_int*, const char*, a_int*, const char*, a_int*, double*, double*, a_int*,
                 double*, a_int*, a_int*, a_int*, double*, double*, a_int*, a_int*);

    void dseupd_(a_int* rvec, const char* howmny, a_int* select, double* d, double* z, a_int* ldz,
                 double* sigma, const char* bmat, a_int* n, const char* which, a_int* nev,
                 double* tol, double* resid, a_int* ncv, double* v, a_int* ldv, a_int* iparam,
                 a_int* inptr, double* workd, double* workl, a_int* lworkl, a_int* info);
}

namespace edlib
{
enum class ErrorType
{
    NormalExit,
    NotConverged,
    IncorrectParams,
    Others
};

template<typename MatrixVectorOp> class ArpackSolver
{
private:
    MatrixVectorOp& op_;
    const a_int dim_;
    std::vector<double> d_;
    std::vector<double> z_;

public:
    explicit ArpackSolver(MatrixVectorOp& op) : op_{op}, dim_{static_cast<a_int>(op.rows())}
    {
        assert(op.rows() == op.cols());
    }

    ErrorType solve(a_int nev, uint32_t max_iter = 1024, double tol = 1e-10)
    {
        a_int dim = dim_;
        if(dim <= 0 || nev <= 0)
        {
            return ErrorType::IncorrectParams;
        }

        a_int ido = 0;
        const char* bmat = "I";
        a_int n = dim;
        const char* which = "SA";
        std::vector<double> resid(dim);

        a_int ncv = 3 * nev;
        std::vector<double> v(static_cast<size_t>(ncv * dim));

        a_int ldv = dim;
        std::array<a_int, 11> iparam = {
            1, // ishift
            0, // levec (not used)
            static_cast<a_int>(max_iter), // maxiter
            1, // nb
            0, // nconv
            0, // iupd (not used)
            1, // mode (1 is usual eigenvalue problem)
            0, // np
            0,
            0,
            0 // only for output
        };

        std::array<a_int, 14> ipntr{};
        std::vector<double> workd(3 * static_cast<size_t>(dim), 0.);

        int lworkl = 3 * ncv * ncv + 6 * ncv;
        std::vector<double> workl(lworkl, 0);

        a_int info = 0;

        // first call
        dsaupd_(&ido, bmat, &n, which, &nev, &tol, resid.data(), &ncv, v.data(), &ldv,
                iparam.data(), ipntr.data(), workd.data(), workl.data(), &lworkl, &info);

        while(ido == -1 || ido == 1)
        {
            op_.perform_op(workd.data() + ipntr[0] - 1, workd.data() + ipntr[1] - 1);

            dsaupd_(&ido, bmat, &n, which, &nev, &tol, resid.data(), &ncv, v.data(), &ldv,
                    iparam.data(), ipntr.data(), workd.data(), workl.data(), &lworkl, &info);
        }

        if(info == 1 || iparam[4] != nev)
        {
            return ErrorType::NotConverged;
        }
        else if(info != 0)
        {
            return ErrorType::IncorrectParams;
        }

        a_int rvec = 1;

        d_.resize(nev + 1);
        z_.resize(static_cast<size_t>(dim + 1) * static_cast<size_t>(nev + 1));
        a_int ldz = dim + 1;
        double sigma = 0.0;

        std::vector<a_int> select(ncv);
        std::fill(select.begin(), select.end(), 1);

        const char* howmny = "All";
        dseupd_(&rvec, howmny, select.data(), d_.data(), z_.data(), &ldz, &sigma, bmat, &n, which,
                &nev, &tol, resid.data(), &ncv, v.data(), &ldv, iparam.data(), ipntr.data(),
                workd.data(), workl.data(), &lworkl, &info);

        return ErrorType::NormalExit;
    }

    [[nodiscard]] auto eigenvalues() const -> Eigen::Map<const Eigen::VectorXd>
    {
        return {d_.data(), static_cast<Eigen::Index>(d_.size() - 1)};
    }

    [[nodiscard]] auto eigenvectors() const
        -> Eigen::Map<const Eigen::MatrixXd, 0, Eigen::OuterStride<Eigen::Dynamic>>
    {
        return {z_.data(), dim_, static_cast<Eigen::Index>(d_.size() - 1), {dim_ + 1}};
    }
};
} // namespace edlib


// Boost random_generator.hpp header file  ----------------------------------------------//

// Copyright 2010 Andy Tompkins.
// Copyright 2017 James E. King III
// 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_UUID_RANDOM_GENERATOR_HPP
#define BOOST_UUID_RANDOM_GENERATOR_HPP

#include <boost/assert.hpp>
#include <boost/core/enable_if.hpp>
#include <boost/core/null_deleter.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/tti/has_member_function.hpp>
#include <boost/uuid/detail/random_provider.hpp>
#include <boost/uuid/uuid.hpp>
#include <limits>

namespace boost {
namespace uuids {

namespace detail {
    template<class U>
    U& set_uuid_random_vv(U& u)
    {
        // set variant
        // must be 0b10xxxxxx
        *(u.begin() + 8) &= 0xBF;
        *(u.begin() + 8) |= 0x80;

        // set version
        // must be 0b0100xxxx
        *(u.begin() + 6) &= 0x4F; //0b01001111
        *(u.begin() + 6) |= 0x40; //0b01000000

        return u;
    }

    BOOST_TTI_HAS_MEMBER_FUNCTION(seed)
}

//! generate a random-based uuid
//! \param[in]  UniformRandomNumberGenerator  see Boost.Random documentation
template <typename UniformRandomNumberGenerator>
class basic_random_generator {
private:
    typedef uniform_int<unsigned long> distribution_type;
    typedef variate_generator<UniformRandomNumberGenerator*, distribution_type> generator_type;

public:
    typedef uuid result_type;

    // default constructor creates the random number generator and
    // if the UniformRandomNumberGenerator is a PseudoRandomNumberGenerator
    // then it gets seeded by a random_provider.
    basic_random_generator()
        : pURNG(new UniformRandomNumberGenerator)
        , generator
          ( pURNG.get()
          , distribution_type
            ( (std::numeric_limits<unsigned long>::min)()
            , (std::numeric_limits<unsigned long>::max)()
            )
          )
    {
        // seed the random number generator if it is capable
        seed(*pURNG);
    }

    // keep a reference to a random number generator
    // don't seed a given random number generator
    explicit basic_random_generator(UniformRandomNumberGenerator& gen)
        : pURNG(&gen, boost::null_deleter())
        , generator
          ( pURNG.get()
          , distribution_type
            ( (std::numeric_limits<unsigned long>::min)()
            , (std::numeric_limits<unsigned long>::max)()
            )
          )
    {}

    // keep a pointer to a random number generator
    // don't seed a given random number generator
    explicit basic_random_generator(UniformRandomNumberGenerator* pGen)
        : pURNG(pGen, boost::null_deleter())
        , generator
          ( pURNG.get()
          , distribution_type
            ( (std::numeric_limits<unsigned long>::min)()
            , (std::numeric_limits<unsigned long>::max)()
            )
          )
    {
        BOOST_ASSERT(pURNG);
    }

    result_type operator()()
    {
        result_type u;

        int i=0;
        unsigned long random_value = generator();
        for (uuid::iterator it=u.begin(); it!=u.end(); ++it, ++i) {
            if (i==sizeof(unsigned long)) {
                random_value = generator();
                i = 0;
            }

            // static_cast gets rid of warnings of converting unsigned long to boost::uint8_t
            *it = static_cast<uuid::value_type>((random_value >> (i*8)) & 0xFF);
        }

        return detail::set_uuid_random_vv(u);
    }

private:
    // Detect whether UniformRandomNumberGenerator has a seed() method which indicates that
    // it is a PseudoRandomNumberGenerator and needs a seed to initialize it.  This allows
    // basic_random_generator to take any type of UniformRandomNumberGenerator and still
    // meet the post-conditions for the default constructor.

    template<class MaybePseudoRandomNumberGenerator>
    typename boost::enable_if<detail::has_member_function_seed<MaybePseudoRandomNumberGenerator, void> >::type
        seed(MaybePseudoRandomNumberGenerator& rng)
    {
        detail::random_provider seeder;
        rng.seed(seeder);
    }

    template<class MaybePseudoRandomNumberGenerator>
    typename boost::disable_if<detail::has_member_function_seed<MaybePseudoRandomNumberGenerator, void> >::type
        seed(MaybePseudoRandomNumberGenerator&)
    {
    }

    shared_ptr<UniformRandomNumberGenerator> pURNG;
    generator_type generator;
};

//! \brief a far less complex random generator that uses
//!        operating system provided entropy which will
//!        satisfy the majority of use cases
class random_generator_pure
{
public:
    typedef uuid result_type;

    //! \returns a random, valid uuid
    //! \throws entropy_error
    result_type operator()()
    {
        result_type result;
        prov_.get_random_bytes(&result, sizeof(result_type));
        return detail::set_uuid_random_vv(result);
    }

private:
    detail::random_provider prov_;
};

#if defined(BOOST_UUID_RANDOM_GENERATOR_COMPAT)
typedef basic_random_generator<mt19937> random_generator;
#else
typedef random_generator_pure random_generator;
typedef basic_random_generator<mt19937> random_generator_mt19937;
#endif

}} // namespace boost::uuids

#endif // BOOST_UUID_RANDOM_GENERATOR_HPP


#ifndef GRID_HPP_NRC_20141031
#define GRID_HPP_NRC_20141031

#include "cube.hpp"
#include "pt3.hpp"
#include "units.hpp"
#include <boost/container/flat_set.hpp>
#include <boost/optional.hpp>
#include <range/v3/to_container.hpp>
#include <range/v3/view/map.hpp>
#include <algorithm>
#include <map>
#include <vector>

namespace jhmi
{

template <typename T>
class grid {
  using t_set = boost::container::flat_set<T>;
public:
  explicit grid(cube<m3> const& extents, m cell_size)
    : extents_(extents), cell_size_(cell_size),
      size_{ceil((extents.lr() - extents.ul()) / cell_size)},  grid_{} {
    grid_.resize(size_.x * size_.y * size_.z);
  }
  template <typename U>
  void remove_item(U&& item) {
    for_sets(item, [&](t_set& s) {
      s.erase(id(item));
    });
  }
  template <typename U>
  void add_item(U&& item) {
    for_sets(item, [&](t_set& s) {
      s.insert(id(item));
    });
  }
  t_set const& operator()(m3 const& pt) const {
    return at(pt_to_box(pt));
  }

  t_set cube_around(m3 const& pt) const {
    auto center = pt_to_box(pt);
    auto ext = intersect(cube<int3>{center - int3{1,1,1}, center + int3{2,2,2}},
                         cube<int3>{int3{0,0,0}, size_});
    t_set ret;
    traverse(ext, 1, [&](int3 const& upt) {
      auto& s = at(upt);
      ret.insert(boost::container::ordered_unique_range, s.begin(), s.end());
    });
    return ret;
  }

  t_set get_all() const {
    t_set ret;
    traverse(cube<int3>{int3{0,0,0}, size_}, 1, [&](int3 const& pt) {
      auto& s = at(pt);
      ret.insert(boost::container::ordered_unique_range, s.begin(), s.end());
    });
    return ret;
  }
private:
  typedef jhmi::pt3<int> int3;
  auto const& at(int3 const& upt) const {
    return grid_[upt.x + upt.y * size_.x + upt.z * size_.x * size_.y];
  }
  auto pt_to_box(m3 const& pt) const
  { return int3{floor((pt - extents_.ul()) / cell_size_)}; }

  template <typename U, typename F>
  auto for_sets(U const& item, F f) {
    bool contained = false;
    traverse(item, [&](m3 const& pt) { return pt_to_box(pt); }, [&](int3 const& pt) {
      if (contains(cube<int3>{int3{}, size_ - int3{1,1,1}}, pt)) {
        contained = true;
        f(grid_[pt.x + pt.y * size_.x + pt.z * size_.x * size_.y]);
      }
    });
    if (!contained) {
      auto e = extents(item);
      fmt::print("Item from\n{} to {} not overlapping grid from\n{} to {}\n",
        e.ul(), e.lr(), extents_.ul(), extents_.lr());
    }
  }

  friend cube<m3> const& extents(grid<T> const& g) {
    return g.extents_;
  }

  cube<m3> extents_;
  m cell_size_;
  int3 size_;
  std::vector<t_set> grid_;
};

}//jhmi

#endif


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include "toplevelfixture.hpp"
#include <boost/test/unit_test.hpp>
#include <ql/indexes/ibor/usdlibor.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/termstructures/yield/piecewiseyieldcurve.hpp>

#include <qle/termstructures/tenorbasisswaphelper.hpp>

#include <boost/make_shared.hpp>

using namespace boost::unit_test_framework;
using namespace QuantLib;

BOOST_FIXTURE_TEST_SUITE(QuantExtTestSuite, qle::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(RateHelpersTest)

BOOST_AUTO_TEST_CASE(testTenorBasisSwapHelperLastRelevantDate) {

    BOOST_TEST_MESSAGE("Testing QuantExt::TenorBasisSwapHelper last relevant date (regression test case)...");

    SavedSettings backup;
    Settings::instance().evaluationDate() = Date(8, Dec, 2016);
    Date today = Settings::instance().evaluationDate();

    Handle<YieldTermStructure> flat6m(
        boost::make_shared<FlatForward>(today, Handle<Quote>(boost::make_shared<SimpleQuote>(0.02)), Actual365Fixed()));
    boost::shared_ptr<IborIndex> usdLibor6m = boost::make_shared<USDLibor>(6 * Months, flat6m);
    boost::shared_ptr<IborIndex> usdLibor1m = boost::make_shared<USDLibor>(1 * Months);

    boost::shared_ptr<RateHelper> helper = boost::make_shared<QuantExt::TenorBasisSwapHelper>(
        Handle<Quote>(boost::make_shared<SimpleQuote>(0.0)), 30 * Years, usdLibor6m, usdLibor1m, 6 * Months);

    PiecewiseYieldCurve<Discount, LogLinear> curve(today, std::vector<boost::shared_ptr<RateHelper> >(1, helper),
                                                   Actual365Fixed());
    BOOST_CHECK_NO_THROW(curve.discount(1.0));
}

BOOST_AUTO_TEST_CASE(testTenorBasisSwapHelperDegenerateSchedule) {

    BOOST_TEST_MESSAGE("Testing QuantExt::TenorBasisSwapHelper degenerate schedule (regression test case)...");

    SavedSettings backup;
    Settings::instance().evaluationDate() = Date(28, Dec, 2016);
    Date today = Settings::instance().evaluationDate();

    Handle<YieldTermStructure> flat6m(
        boost::make_shared<FlatForward>(today, Handle<Quote>(boost::make_shared<SimpleQuote>(0.02)), Actual365Fixed()));
    boost::shared_ptr<IborIndex> usdLibor6m = boost::make_shared<USDLibor>(6 * Months, flat6m);
    boost::shared_ptr<IborIndex> usdLibor3m = boost::make_shared<USDLibor>(3 * Months);

    boost::shared_ptr<RateHelper> helper = boost::make_shared<QuantExt::TenorBasisSwapHelper>(
        Handle<Quote>(boost::make_shared<SimpleQuote>(0.0)), 18 * Months, usdLibor6m, usdLibor3m, 6 * Months);

    PiecewiseYieldCurve<Discount, LogLinear> curve(today, std::vector<boost::shared_ptr<RateHelper> >(1, helper),
                                                   Actual365Fixed());
    BOOST_CHECK_NO_THROW(curve.discount(1.0));
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


// Copyright (c) 2018 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ReduceSpecial23
#include <poplar/Engine.hpp>
#include <poplibs_support/TestDevice.hpp>
// codelets
#include "../../lib/popops/reduction/ReductionVertex.hpp"
#include "poplar/Target.hpp"
#include "poplibs_test/Check.hpp"
#include "poplibs_test/Util.hpp"
#include "popops/codelets.hpp"
#include "poputil/VertexTemplates.hpp"
#include <boost/program_options.hpp>
#include <poplibs_test/Reduce.hpp>
#include <popops/Reduce.hpp>
#include <stdexcept>
#include <string.h>

using namespace poplar;
using namespace poplar::program;
using namespace popops;
using namespace poputil;
using namespace poplibs_test::util;
using namespace poplibs_test::reduce;
using namespace poplibs_support;

const OptionFlags options;

static bool doTest(const DeviceType &deviceType, const Type &partialsType,
                   const Type &outType, const unsigned outerDim,
                   const unsigned innerDim, const unsigned outputDim,
                   const popops::Operation op, const float scale, bool isUpdate,
                   ReductionSpecialisation specialisation) {
  auto device = createTestDevice(deviceType);
  auto &target = device.getTarget();
  Graph graph(target);
  popops::addCodelets(graph);

  const float initialValue = 1.0;
  const auto partialsGrainSize = partialsType == poplar::HALF ? 4 : 2;

  // Check constraints:
  if (specialisation == ReductionSpecialisation::SCALAR_OUTPUT_SINGLE_INPUT) {
    if (outputDim != 1) {
      std::cerr
          << "Output dimension must be 1 for SCALAR_OUTPUT_SINGLE_INPUT\n";
      return false;
    }

  } else if (specialisation == ReductionSpecialisation::STRIDED_REDUCE) {
    if (innerDim % partialsGrainSize) {
      std::cerr << "Inner dimension must be a multiple of 4 (half) or 2 "
                   "(float) for STRIDED_REDUCE\n";
      return false;
    }
    if (outputDim % partialsGrainSize) {
      std::cerr << "Output dimension must be a multiple of 4 (partials type = "
                   "half) or 2 "
                   "(partials type = float) for STRIDED_REDUCE\n";
      return false;
    }
  } else {
    std::cerr << "Unsupported specialisation\n";
    return false;
  }

  // Claim enough space for floats
  std::vector<char> data(innerDim * outerDim * 4);
  std::vector<float> nums(innerDim * outerDim);
  std::vector<int> intData(innerDim * outerDim);

  for (unsigned i = 0; i < outerDim; ++i) {
    for (unsigned j = 0; j < innerDim; ++j) {
      nums[(i * innerDim) + j] = i + j;
      intData[(i * innerDim + j)] = i + j;
    }
  }

  copy(target, nums.data(), innerDim * outerDim, partialsType, data.data());
  std::vector<float> answers(outputDim, initialValue);
  std::vector<char> ans_data((outputDim)*4);
  copy(target, answers.data(), outputDim, outType, ans_data.data());

  Sequence prog;

  auto cs = graph.addComputeSet("cs");

  Tensor partials;
  partials = graph.addVariable(partialsType, {outerDim, innerDim});
  Tensor out;
  out = graph.addVariable(outType, {outputDim});

  const auto vertexClass =
      templateVertex(scale == 1.0f ? "popops::Reduce" : "popops::ScaledReduce",
                     "popops::" + getReductionVertexOpName(op), partialsType,
                     outType, isUpdate, specialisation);

  auto v1 = graph.addVertex(cs, vertexClass);

  graph.connect(v1["partials"], partials.flatten());
  graph.connect(v1["out"], out);
  if (specialisation == ReductionSpecialisation::SCALAR_OUTPUT_SINGLE_INPUT) {
    graph.setInitialValue(v1["numPartials"], innerDim * outerDim);
  } else {
    graph.setInitialValue(v1["numOutputs"], outputDim);
    graph.setInitialValue(v1["numPartialsM1"], outerDim - 1);
    graph.setInitialValue(v1["partialsWidth"], innerDim);
  }
  auto scaleTensor = graph.addVariable(FLOAT, {});
  graph.setTileMapping(scaleTensor, 0);
  graph.setInitialValue(scaleTensor, scale);
  if (scale != 1.0f) {
    graph.connect(v1["k"], scaleTensor.reshape({1}));
  }
  graph.setTileMapping(v1, 0);
  graph.setTileMapping(partials, 0);
  graph.setTileMapping(out, 0);

  graph.createHostWrite("partials", partials);
  graph.createHostWrite("outw", out);
  graph.createHostRead("out", out);

  prog.add(Execute(cs));

  Engine e(graph, prog, options);
  auto outSize = out.numElements() * target.getTypeSize(outType);

  device.bind([&](const Device &d) {
    e.load(d);
    if (outType == FLOAT || outType == HALF) {
      e.writeTensor("partials", data.data(),
                    data.data() + partials.numElements() *
                                      target.getTypeSize(partialsType));
      e.writeTensor("outw", ans_data.data(), ans_data.data() + outSize);
    } else if (outType == INT) {
      e.writeTensor("partials", intData.data(),
                    intData.data() + partials.numElements() *
                                         target.getTypeSize(partialsType));
      e.writeTensor("outw", ans_data.data(), ans_data.data() + outSize);
    }
    e.readTensor("out", ans_data.data(), ans_data.data() + outSize);

    e.run();

    e.readTensor("out", ans_data.data(), ans_data.data() + outSize);
  });

  copy(target, partialsType, data.data(), nums.data(), innerDim * outerDim);

  copy(target, outType, ans_data.data(), answers.data(), outputDim);
  copy(target, outType, ans_data.data(), intData.data(), outputDim);

  // Create / reduce the whole result, later we may only check part of it based
  // on outputDim.
  const auto arrayDim =
      (specialisation == ReductionSpecialisation::STRIDED_REDUCE) ? innerDim
                                                                  : outputDim;
  MultiArray<float> input{(outerDim * innerDim) / arrayDim, arrayDim};
  for (unsigned i = 0; i < input.numElements(); i++) {
    const unsigned row = i / (innerDim);
    const unsigned column = i % (innerDim);
    input.data()[i] = nums[row * innerDim + column];
  }

  auto result = reduce(input, {0}, op);

  std::vector<float> correct_answer(outputDim, initialValue);
  for (unsigned i = 0; i < outputDim; i++) {
    if (isUpdate) {
      correct_answer[i] += result[i] * scale;
    } else {
      correct_answer[i] = result[i] * scale;
    }
  }

  bool success = true;

  if (outType == FLOAT || outType == HALF) {
    for (unsigned i = 0; i < outputDim; ++i) {
      std::cout << answers[i] << ", ";
      CHECK_IF(success, correct_answer[i] == answers[i]);
      answers[i] = 0; // zero for next iteration
    }
  } else if (outType == INT) {
    for (unsigned i = 0; i < outputDim; ++i) {
      CHECK_IF(success, correct_answer[i] == intData[i]);
    }
  } else {
    success = false;
  }
  return success;
}

int main(int argc, char **argv) {
  namespace po = boost::program_options;

  popops::Operation op = popops::Operation::ADD;
  DeviceType deviceType;
  Type partialsType;
  Type outType;
  float scale = 2.0f;
  bool isUpdate = true;
  unsigned specialisation;
  unsigned outerDim, innerDim, outputDim;
  po::options_description desc("Options");
  // clang-format off
  desc.add_options()
    ("help", "Print help")
    ("device-type",
     po::value<DeviceType>(&deviceType)->required(),
     "Device Type")
    ("partials-type",
     po::value<Type>(&partialsType)->required(),
     "Partials Type")
    ("specialisation",
     po::value<unsigned>(&specialisation)->required(),
     "Specialisation: 2 = SCALAR_OUTPUT_SINGLE_INPUT or"
     " 3 = STRIDED_REDUCE")
    ("out-type",
     po::value<Type>(&outType)->required(),
     "Output type")
    ("operation",
     po::value(&op),
     "operation:ADD SQUARE_ADD MAX MIN MUL LOGICAL_OR or LOGICAL_AND")
    ("update",
     po::value<bool>(&isUpdate)->default_value(isUpdate),
     "reduce with update")
    ("scale",
     po::value<float>(&scale)->default_value(scale),
     "scale")
    ("outer-dim",
     po::value<unsigned>(&outerDim)->required(),
     "Outer dimension")
    ("inner-dim",
     po::value<unsigned>(&innerDim)->required(),
     "Inner dimension")
    ("output-dim",
     po::value<unsigned>(&outputDim)->required(),
     "Output dimension");
  // clang-format on
  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n\n";
      return 1;
    }
    po::notify(vm);
  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return 1;
  }

  const auto specialisationType =
      specialisation == 2 ? ReductionSpecialisation::SCALAR_OUTPUT_SINGLE_INPUT
                          : ReductionSpecialisation::STRIDED_REDUCE;
  if (!doTest(deviceType, partialsType, outType, outerDim, innerDim, outputDim,
              op, scale, isUpdate, specialisationType))
    return 1;
  return 0;
}


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

  @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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_TOUINT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_TOUINT_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/is_less.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/splat.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF(touint_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::int_<A0>, X>
                          )
   {
     using result = bd::as_integer_t<A0, unsigned> ;
     BOOST_FORCEINLINE result operator()(A0 const& a0) const BOOST_NOEXCEPT
     {
       return bitwise_cast<result>(a0);
     }
   };

   BOOST_DISPATCH_OVERLOAD(touint_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::uint_<A0>, X>
                          )
   {
     BOOST_FORCEINLINE A0 operator()(A0 const& a0) const BOOST_NOEXCEPT
     {
       return a0;
     }
   };

  BOOST_DISPATCH_OVERLOAD_IF(touint_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
     using result = bd::as_integer_t<A0, unsigned> ;
     BOOST_FORCEINLINE result operator()(A0 const& a0) const BOOST_NOEXCEPT
     {
       using signed_integer = bd::as_integer_t<typename bd::scalar_of_t<A0>, signed>;
       A0 sign_f = inc(splat<A0>(Valmax<signed_integer>()));
       result sign_i = inc(splat<result>(Valmax<signed_integer>()));
       return if_else(is_less(a0, sign_f)
                     , bitwise_cast<result>(toint(a0))
                     , bitwise_cast<result>(toint(a0 - sign_f)) + sign_i
                     );
     }
   };

} } }

#include <boost/simd/arch/common/simd/function/touint_s.hpp>

#endif


#ifndef MYMFEM_ERROR_HPP
#define MYMFEM_ERROR_HPP

#include "mfem.hpp"
using namespace mfem;

#include <Eigen/Core>
#include <Eigen/Dense>

#include <memory>

#include "../mymfem/utilities.hpp"

class ComputeH1Error
{
public:
    std::tuple <double, double, double, double>
    operator()(const GridFunction&, const GridFunction&,
               bool has_shared_vertices = false);

    std::pair < Array <int>, Array <IntegrationPoint> >
    get_ref_points (ElementTransformation&, int init_cell_id2);

    std::tuple <double, double, double, double>
    test_slow(const GridFunction&, const GridFunction&);

    std::pair < Array <int>, Array <IntegrationPoint> >
    get_ref_points_slow (ElementTransformation&, Mesh&);

private:
    std::unique_ptr <const IntegrationRule> m_ir;
    std::unique_ptr <PointLocator> m_point_locator;
};

#endif /// MYMFEM_ERROR_HPP


#include <exception>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Dense>

Eigen::VectorXd RK4_eigen(const Eigen::VectorXd &q, const double dT);


#include <gtest/gtest.h>
#include "headers/speed_constraints.h"
#include "headers/field_functions.h"
#include <boost/filesystem.hpp>
#include <chrono>

TEST(corners_4_robots_1, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 6);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 0; i < 21; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 3.0), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 3.0), *og);
  }
  for (int i = 21; i < 31; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 3.0), *og);
  }

  for (int i = 0; i < 10; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(49 - i, 39 + i), 3.0), *og);
  }
  for (int i = 10; i < 26; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(39 - i, 29 + i), 3.0), *og);
    boost::add_vertex(Obstacle(Position_t(49 - i, 39 + i), 3.0), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("corners_4_robots_1");

  for (int i = 0; i <= 1200; i++)
  {
    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 39)
    {
      leaderV = Vector_t(-sqrt(2), sqrt(2));
    }
    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 200 == 0)
    {
      printPlotWithArrows("corners_4_robots_1/corners_4_robots_1_" + std::to_string(i) + ".png",
                          "corners_4_robots_1", 0, 90, 1, vg.getRobotGraph(), std::function(&overallPotential),
                          vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

TEST(corners_4_robots_2, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 6);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 0; i < 21; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 4.5), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 4.5), *og);
  }
  for (int i = 21; i < 31; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 4.5), *og);
  }

  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(29 - i, 39 + i), 4.5), *og);
    boost::add_vertex(Obstacle(Position_t(49 - i, 39 + i), 4.5), *og);
  }
  for (int i = 0; i < 10; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(34 - i, 54 + i), 4.5), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("corners_4_robots_2");

  for (int i = 0; i <= 1200; i++)
  {
    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 39)
    {
      leaderV = Vector_t(-sqrt(2), sqrt(2));
    }
    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 200 == 0)
    {
      printPlotWithArrows("corners_4_robots_2/corners_4_robots_2_" + std::to_string(i) + ".png",
                          "corners_4_robots_2", 0, 90, 1, vg.getRobotGraph(), std::function(&overallPotential),
                          vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

TEST(corners_4_robots_3, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 20.0);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 6);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 0; i < 21; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 4.5), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 4.5), *og);
  }
  for (int i = 21; i < 31; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 4.5), *og);
  }
  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(29 - i, 39 + i), 4.5), *og);
    boost::add_vertex(Obstacle(Position_t(49 - i, 39 + i), 4.5), *og);
  }
  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(14 + i, 54 + i), 4.5), *og);
    boost::add_vertex(Obstacle(Position_t(34 + i, 54 + i), 4.5), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("corners_4_robots_3");

  for (int i = 0; i < 1500; i++)
  {
    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 39)
    {
      leaderV = Vector_t(-sqrt(2), sqrt(2));
    }

    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 54)
    {
      leaderV = Vector_t(sqrt(2), sqrt(2));
    }
    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 200 == 0)
    {
      printPlotWithArrows("corners_4_robots_3/corners_4_robots_3_" + std::to_string(i) + ".png",
                          "corners_4_robots_3", 0, 90, 1, vg.getRobotGraph(), std::function(&overallPotential),
                          vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

TEST(corners_4_robots_4, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 6);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 0; i < 21; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 4.5), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 4.5), *og);
  }
  for (int i = 21; i < 31; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 4.5), *og);
  }
  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(29 - i, 39 + i), 4.5), *og);
    boost::add_vertex(Obstacle(Position_t(49 - i, 39 + i), 4.5), *og);
  }
  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(14, 54 + i), 4.5), *og);
  }
  for (int i = 0; i < 30; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(14 + i * sqrt(2), 69), 4.5), *og);
    boost::add_vertex(Obstacle(Position_t(34 + i * sqrt(2), 54), 4.5), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("corners_4_robots_4");

  for (int i = 0; i < 2500; i++)
  {
    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 39)
    {
      leaderV = Vector_t(-sqrt(2), sqrt(2));
    }

    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 54)
    {
      leaderV = Vector_t(0, 1);
    }

    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 61)
    {
      leaderV = Vector_t(1, 0);
    }

    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 200 == 0)
    {
      printPlotWithArrows("corners_4_robots_4/corners_4_robots_4_" + std::to_string(i) + ".png",
                          "corners_4_robots_4", 0, 90, 1, vg.getRobotGraph(), std::function(&overallPotential),
                          vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

TEST(corners_4_robots_5, ShouldPass)
{
  Variables v = Variables();
  v.setParam("robots_avoidance_distance", 3.0);
  v.setParam("obstacles_avoidance_distance", 1.7);
  v.setParam("los_clearance_distance", 0.1);
  v.setParam("los_clearance_care_distance", 6.5);
  v.setParam("neighbourhood_distance", 10.0);
  v.setParam("edge_deletion_distance", 2.5);
  v.setParam("obstacle_care_distance", 6.5);
  v.setParam("desired_distance", 6.5);
  v.setParam("sensing_distance", 10.5);
  v.setParam("robot_max_speed", 0.1);
  v.setParam("k1", 10);
  v.setParam("k2", 10);
  v.setParam("c1", 2.5);
  v.setParam("c2", 1.25);
  v.setParam("c3", 0.1);
  v.setParam("c4", 10.0);

  ValidatedVariables vv(v);

  Robot r1(Position_t(13.0, 13.0), 0);
  Robot r2(Position_t(3.0, 3.0), 1);
  Robot r3(Position_t(6.0, 8.0), 2);
  Robot r4(Position_t(11.0, 3.0), 3);
  Robot r5(Position_t(1.0, 8.0), 4);
  Robot r6(Position_t(8.0, 0.0), 6);

  auto rg = std::make_unique<RobotGraph>();
  auto r1_desc = boost::add_vertex(r1, *rg);
  boost::add_vertex(r2, *rg);
  boost::add_vertex(r3, *rg);
  boost::add_vertex(r4, *rg);

  auto og = std::make_unique<ObstacleGraph>();
  for (int i = 0; i < 21; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(8 + i, 18 + i), 3.0), *og);
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 3.0), *og);
  }
  for (int i = 21; i < 31; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(18 + i, 8 + i), 3.0), *og);
  }
  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(29 - i, 39 + i), 3.0), *og);
    boost::add_vertex(Obstacle(Position_t(49 - i, 39 + i), 3.0), *og);
  }
  for (int i = 0; i < 16; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(14, 54 + i), 5.0), *og);
  }
  for (int i = 0; i < 30; i += 1)
  {
    boost::add_vertex(Obstacle(Position_t(14 + i * sqrt(2), 69), 5.0), *og);
    boost::add_vertex(Obstacle(Position_t(34 + i * sqrt(2), 54), 5.0), *og);
  }

  ValidatedGraphs vg(std::move(rg), std::move(og), vv);

  auto leaderV = Vector_t(sqrt(2), sqrt(2));
  boost::filesystem::create_directories("corners_4_robots_5");

  for (int i = 0; i < 2300; i++)
  {
    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 39)
    {
      leaderV = Vector_t(-sqrt(2), sqrt(2));
    }

    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 54)
    {
      leaderV = Vector_t(0, 1);
    }

    if (vg.getRobotGraph()[r1_desc].getPosition()(1, 0) > 61)
    {
      leaderV = Vector_t(1, 0);
    }

    std::cout << "\tIteration " << i << " starts." << std::endl;
    auto start = std::chrono::system_clock::now();

    vg.tick(r1_desc, leaderV, vv);

    auto end = std::chrono::system_clock::now();
    int elapsed_seconds = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
    std::cout << "\tIteration " << i << " ends in " << elapsed_seconds << std::endl;

    if (i % 200 == 0)
    {
      printPlotWithArrows("corners_4_robots_5/corners_4_robots_5_" + std::to_string(i) + ".png",
                          "corners_4_robots_5", 0, 90, 1, vg.getRobotGraph(), std::function(&overallPotential),
                          vg.getRobotGraph(), vg.getObstacleGraph(), v);
    }
  }
}

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}