#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include <map>
#include <string>
#include <iostream>
#include <random>
#include <Eigen/Geometry>
#include <time.h>
#include <chrono>
#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <sensor_msgs/JointState.h>
#include <baxter_core_msgs/JointCommand.h>
#include <baxter_robot_controllers/baxter_robot_config.hpp>
#include <arc_utilities/pretty_print.hpp>
#include <arc_utilities/eigen_helpers.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>

namespace baxter_robot_controllers
{
    class BaxterRobotVelocityTorqueController
    {
    protected:

        enum ARM_TYPE {LEFT_ARM, RIGHT_ARM};

        ARM_TYPE arm_;

        std::vector<std::string> joint_names_;
        std::map<std::string, JointLimits> joint_limits_;
        std::map<std::string, PIDParams> joint_controller_params_;

        KDL::Chain chain_;
        std::shared_ptr<KDL::ChainIdSolver_RNE> id_solver_;

        bool target_velocities_valid_;
        bool current_config_valid_;
        std::vector<double> target_velocities_;
        std::vector<double> current_config_;

        uint32_t velocity_filter_window_size_;
        std::vector<std::vector<double>> velocity_filter_window_;

        std::vector<double> current_velocities_;
        std::vector<double> current_accelerations_;
        ros::Time last_config_time_;

        std::vector<double> velocity_error_integrals_;
        std::vector<double> last_velocity_errors_;

        ros::NodeHandle nh_;
        ros::Publisher command_pub_;
        ros::Subscriber feedback_sub_;
        ros::Subscriber velocity_target_sub_;
        ros::ServiceServer abort_server_;

    public:

        BaxterRobotVelocityTorqueController(ros::NodeHandle& nh,
                                            const std::string& velocity_command_topic,
                                            const std::string& config_feedback_topic,
                                            const std::string& torque_command_topic,
                                            const std::string& abort_service,
                                            const std::string& xml_model_string,
                                            const bool model_gravity,
                                            const uint32_t velocity_filter_window_size,
                                            const std::map<std::string, JointLimits> joint_limits,
                                            const std::map<std::string, PIDParams> joint_controller_params) : nh_(nh)
        {
            target_velocities_valid_ = false;
            current_config_valid_ = false;
            target_velocities_ = std::vector<double>();
            current_config_ = std::vector<double>();
            assert(velocity_filter_window_size >= 1u);
            velocity_filter_window_size_ = velocity_filter_window_size;
            velocity_filter_window_ = std::vector<std::vector<double>>();
            current_velocities_ = std::vector<double>();
            current_accelerations_ = std::vector<double>();
            assert(SetsEqual(arc_helpers::GetKeys(joint_limits), arc_helpers::GetKeys(joint_controller_params)));
            joint_names_ = arc_helpers::GetKeys(joint_limits);
            velocity_error_integrals_ = std::vector<double>(joint_names_.size(), 0.0);
            last_velocity_errors_ = std::vector<double>(joint_names_.size(), 0.0);
            joint_limits_ = joint_limits;
            joint_controller_params_ = joint_controller_params;
            // Figure out which arm we have
            const bool is_left = arc_helpers::CheckAllStringsForSubstring(joint_names_, "left");
            const bool is_right = arc_helpers::CheckAllStringsForSubstring(joint_names_, "right");
            assert(!is_left != !is_right);
            if (is_left && ! is_right)
            {
                arm_ = LEFT_ARM;
            }
            else if (is_right && !is_left)
            {
                arm_ = RIGHT_ARM;
            }
            // Setup KDL chain
            KDL::Tree tree;
            if (!kdl_parser::treeFromString(xml_model_string, tree))
            {
                ROS_ERROR("Could not initialize tree object");
                assert(false);
            }
            const std::string root_name = "base";
            const std::string tip_name = (arm_ == LEFT_ARM) ? "left_gripper" : "right_gripper";
            if (!tree.getChain(root_name, tip_name, chain_))
            {
                ROS_ERROR("Could not initialize chain object for root_name %s and tip_name %s", root_name.c_str(), tip_name.c_str());
                assert(false);
            }
            // Setup ID solver
            const KDL::Vector gravity = model_gravity ? KDL::Vector(0.0, 0.0, -9.81) : KDL::Vector(0.0, 0.0, 0.0);
            id_solver_ = std::shared_ptr<KDL::ChainIdSolver_RNE>(new KDL::ChainIdSolver_RNE(chain_, gravity));
            // Setup publishers and subscribers
            command_pub_ = nh_.advertise<baxter_core_msgs::JointCommand>(torque_command_topic, 1, false);
            feedback_sub_ = nh_.subscribe(config_feedback_topic, 1, &BaxterRobotVelocityTorqueController::ConfigFeedbackCallback, this);
            velocity_target_sub_ = nh_.subscribe(velocity_command_topic, 1, &BaxterRobotVelocityTorqueController::VelocityTargetCallback, this);
            abort_server_ = nh_.advertiseService(abort_service, &BaxterRobotVelocityTorqueController::AbortCB, this);
        }

        void Loop(const double control_rate)
        {
            const double control_interval = 1.0 / control_rate;
            ros::Rate spin_rate(control_rate);
            uint8_t iteration_count = 0x00;
            while (ros::ok())
            {
                // Process callbacks
                ros::spinOnce();
                // Run controller
                if (iteration_count == 0x00)
                {
                    if (target_velocities_valid_ && current_config_valid_)
                    {
                        const std::vector<double> raw_torque_command = ComputeRawNextStep(current_config_, current_velocities_, current_accelerations_, target_velocities_, control_interval);
                        //std::cout << "Raw torque command: " << PrettyPrint::PrettyPrint(raw_torque_command) << std::endl;
                        const std::vector<double> limited_torque_command = EnforceTorqueLimits(raw_torque_command, joint_names_, joint_limits_);
                        //std::cout << "Limited torque command: " << PrettyPrint::PrettyPrint(limited_torque_command) << std::endl;
                        const std::vector<double> torque_command = EnforcePositionSafetyLimits(current_config_, limited_torque_command, joint_names_, joint_limits_);
                        //std::cout << "Final torque command: " << PrettyPrint::PrettyPrint(torque_command) << std::endl;
                        CommandTorques(torque_command);
                    }
                    iteration_count = 0x00;
                }
                else
                {
                    iteration_count++;
                }
                // Spin
                spin_rate.sleep();
            }
        }

        inline bool AbortCB(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res)
        {
            UNUSED(req);
            UNUSED(res);
            ROS_INFO("Aborting config target");
            target_velocities_valid_ = false;
            velocity_error_integrals_ = std::vector<double>(joint_names_.size(), 0.0);
            last_velocity_errors_ = std::vector<double>(joint_names_.size(), 0.0);
            return true;
        }

        inline static double ClampValue(const double value, const double lower_limit, const double upper_limit)
        {
            return std::max(lower_limit, std::min(upper_limit, value));
        }

        inline std::vector<double> ComputeVelocityError(const std::vector<double>& current_velocities, const std::vector<double>& target_velocities) const
        {
            const std::vector<double> velocity_error = EigenHelpers::Sub(target_velocities, current_velocities);
            return velocity_error;
        }

        inline std::vector<double> ComputeNewVelocityErrorIntegrals(const std::vector<double>& raw_velocity_error_integrals, const std::vector<std::string>& joint_names, const std::map<std::string, PIDParams>& joint_params) const
        {
            assert(raw_velocity_error_integrals.size() == joint_names.size());
            std::vector<double> new_velocity_error_integrals(raw_velocity_error_integrals.size(), 0.0);
            for (size_t idx = 0; idx < new_velocity_error_integrals.size(); idx++)
            {
                const double joint_velocity_error_integral = raw_velocity_error_integrals[idx];
                const std::string& joint_name = joint_names[idx];
                const auto found_itr = joint_params.find(joint_name);
                assert(found_itr != joint_params.end());
                const double i_clamp = found_itr->second.Iclamp();
                const double clamped_joint_velocity_error_integral = ClampValue(joint_velocity_error_integral, -i_clamp, i_clamp);
                new_velocity_error_integrals[idx] = clamped_joint_velocity_error_integral;
            }
            return new_velocity_error_integrals;
        }

        inline std::vector<double> ComputeFeedbackTerms(const std::vector<double>& current_velocities, const std::vector<double>& target_velocities, const double time_interval)
        {
            assert(joint_names_.size() == current_velocities.size());
            assert(joint_names_.size() == target_velocities.size());
            // Compute the pose error in our 'world frame'
            const std::vector<double> velocity_errors = ComputeVelocityError(current_velocities, target_velocities);
            // Compute the integral of pose error & update the stored value
            const std::vector<double> velocity_error_integrals_update = EigenHelpers::Add(EigenHelpers::Multiply(last_velocity_errors_, 0.5), EigenHelpers::Multiply(velocity_errors, 0.0));
            velocity_error_integrals_ = ComputeNewVelocityErrorIntegrals(EigenHelpers::Add(velocity_error_integrals_, velocity_error_integrals_update), joint_names_, joint_controller_params_);
            // Compute the derivative of pose error
            const std::vector<double> velocity_error_derivatives = EigenHelpers::Divide(EigenHelpers::Sub(velocity_errors, last_velocity_errors_), time_interval);
            // Update the stored pose error
            last_velocity_errors_ = velocity_errors;
            // Compute Feedback terms
            std::vector<double> feedback_terms(velocity_errors.size(), 0.0);
            for (size_t idx = 0; idx < feedback_terms.size(); idx++)
            {
                const double joint_velocity_error = velocity_errors[idx];
                const double joint_velocity_error_integral = velocity_error_integrals_[idx];
                const double joint_velocity_error_derivative = velocity_error_derivatives[idx];
                const std::string& joint_name = joint_names_[idx];
                const auto found_itr = joint_controller_params_.find(joint_name);
                assert(found_itr != joint_controller_params_.end());
                const PIDParams& params = found_itr->second;
                const double p_term = joint_velocity_error * params.Kp();
                const double i_term = joint_velocity_error_integral * params.Ki();
                const double d_term = joint_velocity_error_derivative * params.Kd();
    //            if (std::abs(d_term) > std::abs(p_term))
    //            {
    //                std::cout << "Dterm > Pterm!!!" << std::endl;
    //            }
    //            const double safe_d_term = ClampValue(d_term, -std::abs(p_term), std::abs(p_term));
                const double feedback_term = p_term + i_term + d_term;
                feedback_terms[idx] = feedback_term;
            }
            return feedback_terms;
        }

        inline std::vector<double> ComputeFeedForwardTerms(const std::vector<double>& current_config, const std::vector<double>& current_velocities, const std::vector<double>& current_accelerations, const std::vector<double>& target_velocities)
        {
            assert(joint_names_.size() == current_config.size());
            assert(joint_names_.size() == current_velocities.size());
            assert(joint_names_.size() == current_accelerations.size());
            assert(joint_names_.size() == target_velocities.size());
            // Zero external wrench
            KDL::Wrenches external_wrench(chain_.getNrOfSegments(), KDL::Wrench::Zero());
            // Copy the arguments to the ID solver
            KDL::JntArray joint_positions((unsigned int)joint_names_.size());
            KDL::JntArray joint_velocities((unsigned int)joint_names_.size());
            KDL::JntArray joint_accelerations((unsigned int)joint_names_.size());
            for (size_t idx = 0; idx < joint_names_.size(); idx++)
            {
                joint_positions((unsigned int)idx) = current_config[idx]; // Current position
                joint_velocities((unsigned int)idx) = target_velocities[idx]; // Desired velocities
                joint_accelerations((unsigned int)idx) = 0.0; // Zero desired accelerations
            }
            // Storage for the computed torques
            KDL::JntArray computed_joint_torques;
            computed_joint_torques.resize((unsigned int)joint_names_.size());
            // Call the ID solver
            int ret = id_solver_->CartToJnt(joint_positions, joint_velocities, joint_accelerations, external_wrench, computed_joint_torques);
            assert(ret >= 0);
            // Copy the results
            std::vector<double> feedforward_torques(joint_names_.size(), 0.0);
            for (size_t idx = 0; idx < joint_names_.size(); idx++)
            {
                feedforward_torques[idx] = computed_joint_torques((unsigned int)idx);
            }
            return feedforward_torques;
        }

        inline std::vector<double> ComputeRawNextStep(const std::vector<double>& current_config, const std::vector<double>& current_velocities, const std::vector<double>& current_accelerations, const std::vector<double>& target_velocities, const double time_interval)
        {
            //const std::chrono::time_point<std::chrono::high_resolution_clock> start = (std::chrono::time_point<std::chrono::high_resolution_clock>)std::chrono::high_resolution_clock::now();
            assert(current_config.size() == joint_names_.size());
            // Compute feedback terms
            const std::vector<double> feedback_terms = ComputeFeedbackTerms(current_velocities, target_velocities, time_interval);
            // Compute feedforward terms
            const std::vector<double> feedforward_terms = ComputeFeedForwardTerms(current_config, current_velocities, current_accelerations, target_velocities);
            // Combine
            const std::vector<double> raw_torque_command = EigenHelpers::Add(feedforward_terms, feedback_terms);
            //const std::chrono::time_point<std::chrono::high_resolution_clock> end = (std::chrono::time_point<std::chrono::high_resolution_clock>)std::chrono::high_resolution_clock::now();
            //const std::chrono::duration<double> elapsed = end - start;
            //const double elapsed_time = elapsed.count();
            //std::cout << "Elapsed FF+FB compute time: " << elapsed_time << std::endl;
            return raw_torque_command;
        }

        inline std::vector<double> EnforceTorqueLimits(const std::vector<double>& raw_torques, const std::vector<std::string>& joint_names, const std::map<std::string, JointLimits>& limits) const
        {
            assert(raw_torques.size() == joint_names.size());
            std::vector<double> limited_torques(raw_torques.size(), 0.0);
            for (size_t idx = 0; idx < limited_torques.size(); idx++)
            {
                const double raw_torque = raw_torques[idx];
                const std::string& joint_name = joint_names[idx];
                const auto found_itr = limits.find(joint_name);
                assert(found_itr != limits.end());
                const double torque_limit = found_itr->second.MaxEffort();
                const double limited_torque = ClampValue(raw_torque, -torque_limit, torque_limit);
                limited_torques[idx] = limited_torque;
            }
            return limited_torques;
        }

        inline std::vector<double> EnforcePositionSafetyLimits(const std::vector<double>& current_positions, const std::vector<double>& raw_torques, const std::vector<std::string>& joint_names, const std::map<std::string, JointLimits>& limits) const
        {
            assert(current_positions.size() == joint_names.size());
            assert(raw_torques.size() == joint_names.size());
            std::vector<double> limited_torques(raw_torques.size(), 0.0);
            for (size_t idx = 0; idx < limited_torques.size(); idx++)
            {
                const double raw_torque = raw_torques[idx];
                const double current_position = current_positions[idx];
                const std::string& joint_name = joint_names[idx];
                const auto found_itr = limits.find(joint_name);
                assert(found_itr != limits.end());
                const std::pair<double, double> limits = found_itr->second.PositionLimits();
                const double lower_limit = limits.first;
                const double upper_limit = limits.second;
                const bool is_under_limits = (current_position < lower_limit) ? true : false;
                const bool is_over_limits = (current_position > upper_limit) ? true : false;
                if (is_under_limits && raw_torque < 0.0)
                {
                    limited_torques[idx] = 0.0;
                }
                else if (is_over_limits && raw_torque > 0.0)
                {
                    limited_torques[idx] = 0.0;
                }
                else
                {
                    limited_torques[idx] = raw_torque;
                }
            }
            return limited_torques;
        }

        inline void CommandTorques(const std::vector<double>& torques)
        {
            if (torques.size() == joint_names_.size())
            {
                baxter_core_msgs::JointCommand command_msg;
                command_msg.mode = baxter_core_msgs::JointCommand::TORQUE_MODE;
                command_msg.names = joint_names_;
                command_msg.command = torques;
                command_pub_.publish(command_msg);
            }
        }

        inline bool IsSubset(const std::vector<std::string>& candidate_set, const std::vector<std::string>& candidate_subset) const
        {
            if (candidate_set.size() < candidate_subset.size())
            {
                return false;
            }
            std::map<std::string, uint8_t> names_map;
            for (size_t idx = 0; idx < candidate_set.size(); idx++)
            {
                const std::string& name = candidate_set[idx];
                names_map[name] = 0x01;
            }
            for (size_t idx = 0; idx < candidate_subset.size(); idx++)
            {
                const std::string& name = candidate_subset[idx];
                const auto found_itr = names_map.find(name);
                if (found_itr == names_map.end())
                {
                    return false;
                }
            }
            return true;
        }

        inline bool SetsEqual(const std::vector<std::string>& set1, const std::vector<std::string>& set2) const
        {
            if (IsSubset(set1, set2) && IsSubset(set2, set1))
            {
                return true;
            }
            else
            {
                return false;
            }
        }

        inline void VelocityTargetCallback(baxter_core_msgs::JointCommand velocity_target)
        {
            if (velocity_target.mode == baxter_core_msgs::JointCommand::VELOCITY_MODE)
            {
                if (velocity_target.names.size() == velocity_target.command.size())
                {
                    // Push the command into a map
                    std::map<std::string, double> command_map;
                    for (size_t idx = 0; idx < velocity_target.names.size(); idx++)
                    {
                        const std::string& name = velocity_target.names[idx];
                        const double command = velocity_target.command[idx];
                        command_map[name] = command;
                    }
                    // Extract the joint commands in order
                    std::vector<double> target_velocities(joint_names_.size(), 0.0);
                    bool command_valid = true;
                    for (size_t idx = 0; idx < joint_names_.size(); idx++)
                    {
                        // Get the name of the joint
                        const std::string& joint_name = joint_names_[idx];
                        // Get the commanded value
                        const auto found_itr = command_map.find(joint_name);
                        if (found_itr != command_map.end())
                        {
                            const double velocity = found_itr->second;
                            // Get the limit for the joint
                            const auto limit_found_itr = joint_limits_.find(joint_name);
                            // Limit the joint command
                            assert(limit_found_itr != joint_limits_.end());
                            const double limit = limit_found_itr->second.MaxVelocity();
                            const double limited_velocity = ClampValue(velocity, -limit, limit);
                            target_velocities[idx] = limited_velocity;
                        }
                        else
                        {
                            ROS_WARN("Invalid JointCommand: joint %s missing", joint_name.c_str());
                            command_valid = false;
                        }
                    }
                    if (command_valid == true)
                    {
                        target_velocities_ = target_velocities;
                        target_velocities_valid_ = true;
                    }
                }
                else
                {
                    ROS_WARN("Invalid JointCommand: %zu names, %zu commands", velocity_target.names.size(), velocity_target.command.size());
                    target_velocities_valid_ = false;
                }
            }
            else
            {
                ROS_WARN("JointCommand mode is not VELOCITY_MODE");
                target_velocities_valid_ = false;
            }
        }

        inline std::vector<double> FilterVelocities(const std::vector<std::vector<double>>& velocities) const
        {
            std::vector<Eigen::VectorXd> window_velocities(velocities.size());
            for (size_t idx = 0; idx < velocities.size(); idx++)
            {
                window_velocities[idx] = EigenHelpers::StdVectorDoubleToEigenVectorXd(velocities[idx]);
            }
            const Eigen::VectorXd average_velocity = EigenHelpers::AverageEigenVectorXd(window_velocities);
            return EigenHelpers::EigenVectorXdToStdVectorDouble(average_velocity);
        }

        inline void ConfigFeedbackCallback(sensor_msgs::JointState config_feedback)
        {
            if ((config_feedback.name.size() == config_feedback.position.size()) && (config_feedback.name.size() == config_feedback.velocity.size()) && IsSubset(config_feedback.name, joint_names_))
            {
                // Push the joint state into a map
                std::map<std::string, std::pair<double, double>> joint_state_map;
                for (size_t idx = 0; idx < config_feedback.name.size(); idx++)
                {
                    const std::string& name = config_feedback.name[idx];
                    const double position = config_feedback.position[idx];
                    const double velocity = config_feedback.velocity[idx];
                    joint_state_map[name] = std::make_pair(position, velocity);
                }
                // Extract the joint state in order
                std::vector<double> current_config(joint_names_.size(), 0.0);
                std::vector<double> current_velocities(joint_names_.size(), 0.0);
                bool config_valid = true;
                for (size_t idx = 0; idx < joint_names_.size(); idx++)
                {
                    const std::string& joint_name = joint_names_[idx];
                    const auto found_itr = joint_state_map.find(joint_name);
                    if (found_itr != joint_state_map.end())
                    {
                        const double position = found_itr->second.first;
                        const double velocity = found_itr->second.second;
                        current_config[idx] = position;
                        current_velocities[idx] = velocity;
                    }
                    else
                    {
                        ROS_WARN("Invalid JointState feedback: joint %s missing", joint_name.c_str());
                        config_valid = false;
                    }
                }
                if (config_valid == true)
                {
                    // Update the velocity filter window
                    if (velocity_filter_window_.size() < velocity_filter_window_size_)
                    {
                        velocity_filter_window_.push_back(current_velocities);
                    }
                    else
                    {
                        velocity_filter_window_.erase(velocity_filter_window_.begin(), velocity_filter_window_.begin() + 1);
                        velocity_filter_window_.push_back(current_velocities);
                    }
                    // If our filter window is full, keep going
                    if (velocity_filter_window_.size() == velocity_filter_window_size_)
                    {
                        current_config_ = current_config;
                        const std::vector<double> last_velocities = current_velocities_;
                        current_velocities_ = FilterVelocities(velocity_filter_window_);
                        // Compute accelerations if we have valid previous data
                        if (current_config_valid_)
                        {
                            const ros::Time& current_time = config_feedback.header.stamp;
                            const ros::Time& previous_time = last_config_time_;
                            const ros::Duration interval = current_time - previous_time;
                            const std::vector<double> velocity_delta = EigenHelpers::Sub(current_velocities_, last_velocities);
                            const double time_interval = interval.toSec();
                            const std::vector<double> accelerations = EigenHelpers::Divide(velocity_delta, time_interval);
                            current_accelerations_ = accelerations;
                        }
                        else
                        {
                            current_accelerations_ = std::vector<double>(current_velocities.size(), 0.0);
                        }
                        current_config_valid_ = true;
                        last_config_time_ = config_feedback.header.stamp;
                    }
                    else
                    {
                        ROS_INFO("Waiting to fill velocity filter window for the first time...");
                        current_config_valid_ = false;
                    }
                }
            }
            else if (config_feedback.name.size() == 1 && config_feedback.position.size() == 1)
            {
                ;
            }
            else
            {
                ROS_WARN("Invalid JointState feedback: %zu names, %zu positions, %zu velocities", config_feedback.name.size(), config_feedback.position.size(), config_feedback.velocity.size());
                current_config_valid_ = false;
            }
        }
    };
}


#include <ext/itoa.hpp>
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(itoa_test)
{
	const std::size_t sz = 30;
	char buffer[sz];
	char * res;

	using ext::unsafe_itoa;
	res = unsafe_itoa(30, buffer, sz, 10);
	BOOST_CHECK(strcmp(res, "30") == 0);

	res = unsafe_itoa(-30, buffer, sz, 10);
	BOOST_CHECK(strcmp(res, "-30") == 0);

	res = unsafe_itoa(0, buffer, sz, 10);
	BOOST_CHECK(strcmp(res, "0") == 0);

	res = unsafe_itoa(-1, buffer, sz, 10);
	BOOST_CHECK(strcmp(res, "-1") == 0);

	res = unsafe_itoa(INT_MAX, buffer, sz, 10);
	BOOST_CHECK(res == std::to_string(INT_MAX));

	res = unsafe_itoa(INT_MIN, buffer, sz, 10);
	BOOST_CHECK(res == std::to_string(INT_MIN));
}


//=======================================================================
// Copyright (c) 2013 Robert Rosolek
//
// 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)
//=======================================================================
/**
 * @file winner_determination_in_MUCA_test_utils.hpp
 * @brief
 * @author Robert Rosolek
 * @version 1.0
 * @date 2014-3-24
 */
#ifndef PAAL_WINNER_DETERMINATION_IN_MUCA_TEST_UTILS_HPP
#define PAAL_WINNER_DETERMINATION_IN_MUCA_TEST_UTILS_HPP

#include "test_utils/auctions.hpp"
#include "test_utils/test_gamma_oracle_xor_bids.hpp"
#include "test_utils/test_result_check.hpp"

#include "paal/auctions/auction_components.hpp"
#include "paal/auctions/auction_traits.hpp"
#include "paal/auctions/auction_utils.hpp"
#include "paal/auctions/xor_bids.hpp"
#include "paal/auctions/winner_determination_in_MUCA/winner_determination_in_MUCA.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/make.hpp"
#include "paal/utils/type_functions.hpp"

#include <boost/function_output_iterator.hpp>
#include <boost/property_map/property_map.hpp>

#include <cmath>
#include <iterator>
#include <utility>
#include <vector>

template <class GammaOracleAuction>
auto approximation_ratio(GammaOracleAuction&& auction) {
   auto e = std::exp(1.0);
   auto m = paal::auctions::items_number(auction);
   auto b = paal::auctions::get_minimum_copies_num(auction);
   auto gamma = auction.template get<paal::auctions::gamma>();
   return e * (2 * gamma + 1) * std::pow(m, 1.0 / (b + 1));
}

template<
   class Bidders,
   class Items,
   class GetBids,
   class GetValue,
   class GetItems,
   class GetCopiesNum,
   class PriceMap,
   class Epsilon
>
void check_determine_winners_in_gamma_oracle(
   Bidders&& bidders,
   Items&& items,
   GetBids get_bids,
   GetValue get_value,
   GetItems get_items,
   GetCopiesNum get_copies_num,
   double gamma,
   double lower_bound,
   PriceMap prices,
   Epsilon epsilon
) {
   auto auction = make_test_xor_bids_to_gamma_oracle_auction(
      std::forward<Bidders>(bidders),
      std::forward<Items>(items),
      get_bids,
      get_value,
      get_items,
      gamma,
      get_copies_num
   );

   using Traits = paal::auctions::gamma_oracle_auction_traits<decltype(auction)>;
   using Bidder = typename Traits::bidder_val_t;
   using Item = typename Traits::item_val_t;
   using ItemsBundle = typename Traits::items_t;
   using Value = typename Traits::value_t;

   auto valuation = paal::auctions::make_xor_bids_to_value_query_auction(
      bidders, items, get_bids, get_value, get_items, get_copies_num
   );
   Value social_welfare = 0;
   std::unordered_map<Item, Value> item_count;
   std::unordered_map<Bidder, Value> bidder_count;
   paal::auctions::determine_winners_in_gamma_oracle_auction(
      auction,
      boost::iterators::make_function_output_iterator([&](std::pair<Bidder, ItemsBundle> p)
      {
         auto bidder = p.first;
         auto& items = p.second;
         auto item_set = paal::make_unordered_set(items);
         social_welfare += valuation.template call<paal::auctions::value_query>(bidder, item_set);
         for (auto item: items) {
            auto cnt = ++item_count[item];
            BOOST_CHECK_LE(cnt, get_copies_num(item));
         }
         auto cnt = ++bidder_count[bidder];
         BOOST_CHECK_LE(cnt, 1);
      }),
      prices,
      epsilon
   );
   auto ratio = approximation_ratio(auction);
   check_result_compare_to_bound(social_welfare, lower_bound, 1.0 / ratio,
         paal::utils::greater_equal(), epsilon);
}

template<
   class Bidders,
   class Items,
   class GetBids,
   class GetValue,
   class GetItems,
   class GetCopiesNum,
   class Epsilon = double
>
void check_determine_winners_in_gamma_oracle(
   Bidders&& bidders,
   Items&& items,
   GetBids get_bids,
   GetValue get_value,
   GetItems get_items,
   GetCopiesNum get_copies_num,
   double gamma,
   double lower_bound,
   Epsilon epsilon = 1e-8
) {
   using Value = paal::pure_result_of_t<
                    GetValue(paal::range_to_ref_t<
                            paal::pure_result_of_t<GetBids(paal::range_to_ref_t<paal::decay_t<Bidders>>)>
                            >)
                 >;
   using ItemVal = paal::range_to_elem_t<paal::decay_t<Items>>;

   std::unordered_map<ItemVal, Value> map;
   check_determine_winners_in_gamma_oracle(
      std::forward<Bidders>(bidders),
      std::forward<Items>(items),
      get_bids,
      get_value,
      get_items,
      get_copies_num,
      gamma,
      lower_bound,
      boost::make_assoc_property_map(map),
      epsilon
   );
}

#endif /* PAAL_WINNER_DETERMINATION_IN_MUCA_TEST_UTILS_HPP */


/*
 * Copyright (C) 2015 HEPfit Collaboration
 * All rights reserved.
 *
 * For the licensing terms see doc/COPYING.
 */

/**
 * @example myModel_MCMC.cpp
 * @example myModel.h
 * @example myModel.cpp
 * @example myObservables.h
 * @example myObservables.cpp
 * This is an example of how to add user-defined model and observables
 * and to perform a Bayesian analysis with the Markov Chain Monte Carlo.
 *
 */

#pragma warning disable 2196
#include <iostream>
#include <HEPfit.h>
#include <boost/bind.hpp>
#include "myModel.h"
#include "myObservables.h"

/* Necessary if MPI support is enabled during compilation. */
#ifdef _MPI
#include <mpi.h>
#endif

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

/* Necessary if MPI support is enabled during compilation. */
#ifdef _MPI
    MPI_Init(&argc, &argv);
    int rank;
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);
#else
    /* In our MPI implementation the process with rank 0 is the master. */
    int rank = 0;
#endif

    try {

        if(argc != 3){
            /* Print usage and exit. */
            if (rank == 0) std::cout << "\nusage: " << argv[0] << " ModelConf.conf MonteCarlo.conf\n" << std::endl;
            return EXIT_SUCCESS;
        }

        /* Define the model configuration file.                                */
        /* Here it is passed as the first argument to the executable. The      */
        /* model configuration file provides the values with errors for the    */
        /* mandatory model parameters, as well as the list of observables,     */
        /* observables2D, correlated Gaussian observables.                     */
        /* See documentation for details.                                      */
        std::string ModelConf = argv[1];

        /* Define the Monte Carlo configuration file.                         */
        /* Here it is passed as the second argument to the executable. The    */
        /* Monte Carlo configuration file provides the parameters used in the */
        /* Monte Carlo run. See documentation for details.                    */
        std::string MCMCConf = argv[2];

        /* Define the ROOT output file (w/o extension, empty string will set it to MCout) */
        std::string FileOut = "";

        /* Define the optional job tag. */
        std::string JobTag = "";

        /* Create objects of the classes ModelFactory and ThObsFactory */
        ThObsFactory ThObsF;
        ModelFactory ModelF;
        myModel my_model;

        /* register user-defined model named ModelName defined in class ModelClass using the following syntax: */
        ModelF.addModelToFactory("myModel", boost::factory<myModel*>() );

        /* register user-defined ThObservable named ThObsName defined in class ThObsClass using the following syntax: */
        ThObsF.addObsToFactory("BRpppm", boost::bind(boost::factory<BRpppm*>(), _1, 1) );
        ThObsF.addObsToFactory("BRp0p0", boost::bind(boost::factory<BRp0p0*>(), _1, 1) );
        ThObsF.addObsToFactory("BRppp0", boost::bind(boost::factory<BRppp0*>(), _1, 1) );
        ThObsF.addObsToFactory("BRkpkm", boost::bind(boost::factory<BRkpkm*>(), _1, 1) );
        ThObsF.addObsToFactory("BRkSkS", boost::bind(boost::factory<BRkSkS*>(), _1, 1) );
        ThObsF.addObsToFactory("BRkpkS", boost::bind(boost::factory<BRkpkS*>(), _1, 1) );
        ThObsF.addObsToFactory("BRkpkL", boost::bind(boost::factory<BRkpkS*>(), _1, 1) ); /*kpKL is given by KpKS*/
        ThObsF.addObsToFactory("BRetet", boost::bind(boost::factory<BRetet*>(), _1, 1) );
        ThObsF.addObsToFactory("BRetetp", boost::bind(boost::factory<BRetetp*>(), _1, 1) );

        ThObsF.addObsToFactory("BR4pi_ppmm", boost::bind(boost::factory<BR4pi_ppmm*>(), _1, 1) );
        ThObsF.addObsToFactory("BR4pi_pmzz", boost::bind(boost::factory<BR4pi_pmzz*>(), _1, 1) );

        ThObsF.addObsToFactory("aCPpppm", boost::bind(boost::factory<BRpppm*>(), _1, 2) );
        ThObsF.addObsToFactory("aCPp0p0", boost::bind(boost::factory<BRp0p0*>(), _1, 2) );
        ThObsF.addObsToFactory("aCPkpkm", boost::bind(boost::factory<BRkpkm*>(), _1, 2) );
        ThObsF.addObsToFactory("aCPkSkS", boost::bind(boost::factory<BRkSkS*>(), _1, 2) );
        ThObsF.addObsToFactory("aCPkpkS", boost::bind(boost::factory<BRkpkS*>(), _1, 2) );
        ThObsF.addObsToFactory("aCPetet", boost::bind(boost::factory<BRetet*>(), _1, 2) );
        ThObsF.addObsToFactory("aCPetetp", boost::bind(boost::factory<BRetetp*>(), _1, 2) );

        ThObsF.addObsToFactory("DACP", boost::bind(boost::factory<DACP*>(), _1) );

        ThObsF.addObsToFactory("A_PI_0", boost::bind(boost::factory<A_PI_0*>(), _1) );
        ThObsF.addObsToFactory("PHI_PI_0", boost::bind(boost::factory<PHI_PI_0*>(), _1) );
        ThObsF.addObsToFactory("A_RHO_0", boost::bind(boost::factory<A_RHO_0*>(), _1) );
        ThObsF.addObsToFactory("PHI_RHO_0", boost::bind(boost::factory<PHI_RHO_0*>(), _1) );
        ThObsF.addObsToFactory("A_K_13", boost::bind(boost::factory<A_K_13*>(), _1) );
        ThObsF.addObsToFactory("A_K_11", boost::bind(boost::factory<A_K_11*>(), _1) );
        ThObsF.addObsToFactory("A_K_0", boost::bind(boost::factory<A_K_0*>(), _1) );
        ThObsF.addObsToFactory("PHI_K_0", boost::bind(boost::factory<PHI_K_0*>(), _1) );
        ThObsF.addObsToFactory("A_ETET", boost::bind(boost::factory<A_ETET*>(), _1) );
        ThObsF.addObsToFactory("A_ETETP", boost::bind(boost::factory<A_ETETP*>(), _1) );
        ThObsF.addObsToFactory("PHI_ETET", boost::bind(boost::factory<PHI_ETET*>(), _1) );
        ThObsF.addObsToFactory("PHI_ETETP", boost::bind(boost::factory<PHI_ETETP*>(), _1) );

        /* Create an object of the class MonteCarlo. */
        MonteCarlo MC(ModelF, ThObsF, ModelConf, MCMCConf, FileOut, JobTag);

        /* Do a test run if you wish to see the values of the observables      */
        /* and the correlated Gaussian observables defined in the model        */
        /* configuration file computed with the central value of the mandatory */
        /* parameters defined in the same file.                                */
        if (MCMCConf.compare("--noMC") == 0) MC.TestRun(rank);

        /* Initiate the Mote Carlo run. */
        else MC.Run(rank);

/* Necessary if MPI support is enabled during compilation. */
#ifdef _MPI
        MPI_Finalize();
#endif

        return EXIT_SUCCESS;
    } catch (const std::runtime_error& e) {
        std::cerr << e.what() << std::endl;
        return EXIT_FAILURE;
    }
}


#include <iostream>
#include <string>
#include <fstream>
#include <boost/spirit/include/qi.hpp>

#include "lexer.hpp"
#include "core.hpp"

int main(int argc, char *argv[])
{
    std::string line;
    if(argc < 2){
        std::cout << "Insufficient arguments" << std::endl;
        return 1;
    }

    lexer *lex = new lexer;
    lex->lex(argv[1]);
    clauses cls = lex->getClauses();

    //debug console
    //for(auto i = 0; i < cls.CNF.size(); i++){
    //    std::cout << "Literal=" << i << " : ";
    //    for(auto j = 1; j < cls.CNF.at(i).size(); j++){
    //        std::cout << j << "=" << cls.CNF.at(i).at(j) << ", ";
    //    }
    //    std::cout << std::endl;
    //}

    std::cout << std::endl;

    DPLL core(cls);
    if(core.FullSearch() == SAT){
        std::cout << "SAT" << std::endl;
    }
    else{
        std::cout << "UNSAT" << std::endl;
    }

    return 0;

}


//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.
//Copyright (c) 2019 agate-pris

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

#include <boost/qvm/static_assert.hpp>

#ifdef BOOST_QVM_TEST_SINGLE_HEADER
#   include BOOST_QVM_TEST_SINGLE_HEADER
#else
#   include <boost/qvm/vec.hpp>
#   include <boost/qvm/vec_operations.hpp>
#endif

#include <boost/qvm/vec_traits_array.hpp>
#include "test_qvm_vector.hpp"
#include "gold.hpp"
#include <functional>

namespace
    {
    template <class T>
    struct
    test_scalar
        {
        T value_;
        test_scalar( T value ): value_(value) {}
        }; //No operator==

    struct
    equal_to
        {
        template <class T,class U>
        bool
        operator()( T const & a, U const & b )
            {
            return a.value_==b.value_;
            }
        };
    }

namespace boost { namespace qvm {
    template <class T>
    struct
    is_scalar<test_scalar<T> >
        {
        static bool const value = is_scalar<T>::value;
        };
} }

namespace
    {
    template <int Dim>
    void
    test()
        {
        using namespace boost::qvm::sfinae;
        test_qvm::vector<V1,Dim> const x(42,1);
        for( int i=0; i!=Dim; ++i )
            {
                {
                test_qvm::vector<V1,Dim> y(x);
                BOOST_TEST(cmp(x,y,std::equal_to<float>()));
                y.a[i]=0;
                BOOST_TEST(!cmp(x,y,std::equal_to<float>()));
                }
            }
        }

    template <class A, class B>
    void
    test2()
        {
        typedef test_scalar<A> scalar_a;
        typedef test_scalar<B> scalar_b;
        typedef boost::qvm::vec<scalar_a, 5> vec_a;
        typedef boost::qvm::vec<scalar_b, 5> vec_b;
        vec_a const a = { { 42, 94, 96, 72, 95 } };
        vec_b const b = { { 42, 94, 96, 72, 95 } };
        vec_a const c = { { 21, 47, 48, 36, 47 } };
        vec_b const d = { { 21, 47, 48, 36, 47 } };
        BOOST_TEST(cmp(a,a,equal_to()));
        BOOST_TEST(cmp(a,b,equal_to()));
        BOOST_TEST(cmp(b,a,equal_to()));
        BOOST_TEST(cmp(b,b,equal_to()));
        BOOST_TEST(cmp(c,c,equal_to()));
        BOOST_TEST(cmp(c,d,equal_to()));
        BOOST_TEST(cmp(d,c,equal_to()));
        BOOST_TEST(cmp(d,d,equal_to()));
        BOOST_TEST(!cmp(a,c,equal_to()));
        BOOST_TEST(!cmp(c,a,equal_to()));
        BOOST_TEST(!cmp(a,d,equal_to()));
        BOOST_TEST(!cmp(d,a,equal_to()));
        BOOST_TEST(!cmp(b,c,equal_to()));
        BOOST_TEST(!cmp(c,b,equal_to()));
        BOOST_TEST(!cmp(b,d,equal_to()));
        BOOST_TEST(!cmp(d,b,equal_to()));
        }
    }

int
main()
    {
    test<2>();
    test<3>();
    test<4>();
    test<5>();
    test2<int, int>();
    test2<int, double>();
    test2<double, int>();
    test2<double, double>();
    return boost::report_errors();
    }


#include <boost/test/unit_test.hpp>
#include <templ/solvers/temporal/QualitativeTemporalConstraintNetwork.hpp>
#include <templ/solvers/temporal/point_algebra/TimePointComparator.hpp>
#include <templ/Constraint.hpp>

using namespace templ;
using namespace templ::solvers;
using namespace templ::solvers::temporal;

BOOST_AUTO_TEST_SUITE(qualitative_temporal_constraint_network)

BOOST_AUTO_TEST_CASE(constraint)
{
    point_algebra::TimePoint::Ptr tp0(new point_algebra::QualitativeTimePoint("tp0"));
    point_algebra::TimePoint::Ptr tp1(new point_algebra::QualitativeTimePoint("tp1"));
    point_algebra::QualitativeTimePointConstraint qtcn(tp0, tp1, point_algebra::QualitativeTimePointConstraint::GreaterOrEqual);
    templ::Constraint::Category category = qtcn.getCategory();
    BOOST_REQUIRE_MESSAGE( category == templ::Constraint::TEMPORAL_QUALITATIVE, "Qual: QualitativeType expected, but got: " << templ::Constraint::CategoryTxt[category]);
}

BOOST_AUTO_TEST_CASE(timepoint_comparison)
{
    {
        QualitativeTemporalConstraintNetwork::Ptr qtcn(new QualitativeTemporalConstraintNetwork());

        point_algebra::TimePoint::Ptr tp0(new point_algebra::QualitativeTimePoint("tp0"));
        point_algebra::TimePoint::Ptr tp1(new point_algebra::QualitativeTimePoint("tp1"));
        point_algebra::TimePoint::Ptr tp2(new point_algebra::QualitativeTimePoint("tp2"));

        qtcn->addQualitativeConstraint(tp0, tp1, point_algebra::QualitativeTimePointConstraint::Greater);
        BOOST_REQUIRE_MESSAGE( qtcn->isConsistent(), "QualitativeTemporalConstraintNetwork is consistent" );

        point_algebra::QualitativeTimePointConstraint::Type constraint = qtcn->getQualitativeConstraint(tp0, tp1);
        BOOST_REQUIRE_MESSAGE(constraint == point_algebra::QualitativeTimePointConstraint::Greater, "Query result of constraint between two vertices" << point_algebra::QualitativeTimePointConstraint::TypeSymbol[constraint]);

        qtcn->addQualitativeConstraint(tp1, tp2, point_algebra::QualitativeTimePointConstraint::Greater);

        point_algebra::TimePointComparator comparator(qtcn);
        BOOST_REQUIRE_MESSAGE(comparator.greaterThan(tp0, tp1), "tp0 greaterThan tp1");
        BOOST_REQUIRE_MESSAGE(!comparator.equals(tp0, tp1), "tp0 not equals tp1");

        BOOST_REQUIRE_MESSAGE(comparator.greaterThan(tp1, tp2), "tp1 greaterThan tp2");
    }
    {
        QualitativeTemporalConstraintNetwork::Ptr qtcn(new QualitativeTemporalConstraintNetwork());

        point_algebra::TimePointComparator comparator(qtcn);

        point_algebra::TimePoint::Ptr t0_start(new point_algebra::QualitativeTimePoint("t0_start"));
        point_algebra::TimePoint::Ptr t0_end(new point_algebra::QualitativeTimePoint("t0_end"));
        point_algebra::TimePoint::Ptr t1_start(new point_algebra::QualitativeTimePoint("t1_start"));
        point_algebra::TimePoint::Ptr t1_end(new point_algebra::QualitativeTimePoint("t1_end"));

        // t0_s <= t0_e
        // t1_s <= t1_e
        //
        qtcn->addQualitativeConstraint(t0_start, t0_end, point_algebra::QualitativeTimePointConstraint::LessOrEqual);
        qtcn->addQualitativeConstraint(t1_start, t1_end, point_algebra::QualitativeTimePointConstraint::LessOrEqual);
        BOOST_REQUIRE_MESSAGE(qtcn->isConsistent(), "Network is consistent");

        BOOST_REQUIRE_MESSAGE(!comparator.hasIntervalOverlap(t0_start, t0_end, t1_start, t1_end), "No interval overlap when not constrained is defined");

        // t0_s <= t1_e
        // t0_e > t1_s
        //
        // [t0_s     t0_e]
        //      [t1_s       t1_e]
        qtcn->addQualitativeConstraint(t0_start, t1_end, point_algebra::QualitativeTimePointConstraint::Less);
        qtcn->addQualitativeConstraint(t0_end, t1_start, point_algebra::QualitativeTimePointConstraint::Greater);
        BOOST_REQUIRE_MESSAGE(qtcn->isConsistent(), "Network is consistent after adding overlap constraints");

        {
            point_algebra::QualitativeTimePointConstraint::Type constraint0 = qtcn->getQualitativeConstraint(t0_start, t1_end);
            point_algebra::QualitativeTimePointConstraint::Type constraint1 = qtcn->getQualitativeConstraint(t1_end, t0_start);
            BOOST_REQUIRE_MESSAGE(constraint0 == point_algebra::QualitativeTimePointConstraint::Less || constraint1 == point_algebra::QualitativeTimePointConstraint::GreaterOrEqual, "Query result of constraint between two vertices " << point_algebra::QualitativeTimePointConstraint::TypeSymbol[constraint0] <<  "--"<< point_algebra::QualitativeTimePointConstraint::TypeSymbol[constraint1]);
            BOOST_REQUIRE_MESSAGE(comparator.lessThan(t0_start, t1_end), "t0_start < t1_end");
        }
        {
            point_algebra::QualitativeTimePointConstraint::Type constraint0 = qtcn->getQualitativeConstraint(t0_end, t1_start);
            point_algebra::QualitativeTimePointConstraint::Type constraint1 = qtcn->getQualitativeConstraint(t1_start, t0_end);
            BOOST_REQUIRE_MESSAGE(constraint0 == point_algebra::QualitativeTimePointConstraint::Greater || constraint1 == point_algebra::QualitativeTimePointConstraint::LessOrEqual, "Query result of constraint between two vertices " << point_algebra::QualitativeTimePointConstraint::TypeSymbol[constraint0] << "--" << point_algebra::QualitativeTimePointConstraint::TypeSymbol[constraint1]);
            BOOST_REQUIRE_MESSAGE(comparator.greaterThan(t0_end, t1_start), "t0_end > t1_start");
        }

        // check t0_end and t1_start: hasOverlap if !(t0_end <= t1_start || t1_end <= t0_start )
        //
        // t0_start < t1_end
        // t0_end > t1_start
        BOOST_REQUIRE_MESSAGE(!comparator.lessOrEqual(t0_end, t1_start), "NOT(!) t0_end <= t1_start");
        BOOST_REQUIRE_MESSAGE(comparator.hasIntervalOverlap(t0_start, t0_end, t1_start, t1_end), "Interval overlap");

        qtcn->addQualitativeConstraint(t0_end, t1_end, point_algebra::QualitativeTimePointConstraint::Less);
        BOOST_REQUIRE_MESSAGE(qtcn->isConsistent(), "Network is consistent after adding overlap constraints");

        BOOST_REQUIRE_MESSAGE(comparator.lessOrEqual(t0_end, t0_end), "t0_end <= t0_end");
        BOOST_REQUIRE_MESSAGE(!comparator.lessOrEqual(t1_end, t0_end), "NOT(1) t1_end <= t0_end");

        BOOST_REQUIRE_MESSAGE(!comparator.hasIntervalOverlap(t0_start, t0_end, t0_end, t1_end), "Interval do not overlap with identical end and startpoint");
    }

}

BOOST_AUTO_TEST_CASE(consistency_checking)
{
//    {
//        QualitativeTemporalConstraintNetwork qtcn;
//        point_algebra::TimePoint::Ptr tp0(new point_algebra::QualitativeTimePoint("tp0"));
//        point_algebra::TimePoint::Ptr tp1(new point_algebra::QualitativeTimePoint("tp1"));
//        point_algebra::TimePoint::Ptr tp2(new point_algebra::QualitativeTimePoint("tp2"));
//
//
//        qtcn.addQualitativeConstraint(tp0, tp1, point_algebra::QualitativeTimePointConstraint::GreaterOrEqual);
//        qtcn.addQualitativeConstraint(tp1, tp2, point_algebra::QualitativeTimePointConstraint::GreaterOrEqual);
//
//        BOOST_REQUIRE_MESSAGE(qtcn.isConsistent(), "qtcn is consistent");
//    }
//    {
//        QualitativeTemporalConstraintNetwork qtcn;
//        point_algebra::TimePoint::Ptr tp0(new point_algebra::QualitativeTimePoint("tp0"));
//        point_algebra::TimePoint::Ptr tp1(new point_algebra::QualitativeTimePoint("tp1"));
//        point_algebra::TimePoint::Ptr tp2(new point_algebra::QualitativeTimePoint("tp2"));
//
//        qtcn.addQualitativeConstraint(tp0, tp1, point_algebra::QualitativeTimePointConstraint::GreaterOrEqual);
//        qtcn.addQualitativeConstraint(tp1, tp2, point_algebra::QualitativeTimePointConstraint::GreaterOrEqual);
//        qtcn.addQualitativeConstraint(tp0, tp2, point_algebra::QualitativeTimePointConstraint::Less);
//
//        BOOST_REQUIRE_MESSAGE(!qtcn.isConsistent(), "qtcn is inconsistent: tp0 should be >= t2");
//    }
//    {
//        QualitativeTemporalConstraintNetwork qtcn;
//        point_algebra::TimePoint::Ptr tp0(new point_algebra::QualitativeTimePoint("tp0"));
//        point_algebra::TimePoint::Ptr tp1(new point_algebra::QualitativeTimePoint("tp1"));
//        point_algebra::TimePoint::Ptr tp2(new point_algebra::QualitativeTimePoint("tp2"));
//
//        qtcn.addQualitativeConstraint(tp0, tp1, point_algebra::QualitativeTimePointConstraint::Greater);
//        qtcn.addQualitativeConstraint(tp1, tp2, point_algebra::QualitativeTimePointConstraint::Less);
//        qtcn.addQualitativeConstraint(tp0,tp2,  point_algebra::QualitativeTimePointConstraint::Less);
//
//        BOOST_REQUIRE_MESSAGE(qtcn.isConsistent(), "qtcn is consistent");
//    }
//
//    {
//        QualitativeTemporalConstraintNetwork qtcn;
//        point_algebra::TimePoint::Ptr tp0(new point_algebra::QualitativeTimePoint("tp0"));
//        point_algebra::TimePoint::Ptr tp1(new point_algebra::QualitativeTimePoint("tp1"));
//
//        qtcn.addQualitativeConstraint(tp0,tp1, point_algebra::QualitativeTimePointConstraint::Less);
//        qtcn.addQualitativeConstraint(tp1,tp0, point_algebra::QualitativeTimePointConstraint::Greater);
//
//        BOOST_REQUIRE_MESSAGE(qtcn.isConsistent(), "qtcn is consistent for exact timepoint");
//    }
//    {
//        QualitativeTemporalConstraintNetwork qtcn;
//        point_algebra::TimePoint::Ptr tp0(new point_algebra::QualitativeTimePoint("tp0"));
//        point_algebra::TimePoint::Ptr tp1(new point_algebra::QualitativeTimePoint("tp1"));
//
//        qtcn.addQualitativeConstraint(tp0,tp1, point_algebra::QualitativeTimePointConstraint::Greater);
//        qtcn.addQualitativeConstraint(tp1,tp0, point_algebra::QualitativeTimePointConstraint::Greater);
//
//        BOOST_REQUIRE_MESSAGE(!qtcn.isConsistent(), "qtcn should be not consistent for contradicting timepoint relationships");
//    }
    {
        QualitativeTemporalConstraintNetwork qtcn;
        point_algebra::TimePoint::Ptr tp0(new point_algebra::QualitativeTimePoint("tp0"));
        point_algebra::TimePoint::Ptr tp1(new point_algebra::QualitativeTimePoint("tp1"));

        qtcn.addQualitativeConstraint(tp0,tp1, point_algebra::QualitativeTimePointConstraint::GreaterOrEqual);
        qtcn.addQualitativeConstraint(tp0,tp1, point_algebra::QualitativeTimePointConstraint::Less);

        BOOST_REQUIRE_MESSAGE(!qtcn.isConsistent(), "qtcn is not consistent for contradicting timepoint relationships");
    }
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * raflvis: main.cpp
 * Copyright (c) Torr Vision Group, University of Oxford, 2015. All rights reserved.
 */

#include <boost/assign/list_of.hpp>
#include <boost/format.hpp>
using boost::assign::list_of;

#include <evaluation/core/ParamSetUtil.h>
#include <evaluation/core/PerformanceMeasure.h>
#include <evaluation/util/CartesianProductParameterSetGenerator.h>
#include <evaluation/util/ConfusionMatrixUtil.h>
using namespace evaluation;

#include <rafl/choppers/HeightLimitingTreeChopper.h>
#include <rafl/choppers/TimeBasedTreeChopper.h>
#include <rafl/core/RandomForest.h>
#include <rafl/examples/ExampleUtil.h>
#include <rafl/examples/UnitCircleExampleGenerator.h>
using namespace rafl;

#include <tvgplot/PaletteGenerator.h>
#include <tvgplot/PlotWindow.h>
using namespace tvgplot;

#include <tvgutil/persistence/SerializationUtil.h>
using namespace tvgutil;

#include "TestDecisionFunctionGenerator.h"

//#################### TYPEDEFS ####################

typedef int Label;

typedef boost::shared_ptr<const Example<Label> > Example_CPtr;

typedef DecisionTree<Label> DT;
typedef RandomForest<Label> RF;
typedef boost::shared_ptr<RF> RF_Ptr;

typedef boost::shared_ptr<const TreeChopper<Label> > TreeChopper_CPtr;

//#################### TYPES ####################

/**
 * \brief An instance of this class can be used to pick random elements from a set.
 */
template <typename T>
class SetSampler
{
  //~~~~~~~~~~~~~~~~~~~~ PRIVATE VARIABLES ~~~~~~~~~~~~~~~~~~~~
private:
  /** The random number generator used to generate random indices for sampling. */
  tvgutil::RandomNumberGenerator m_rng;

  //~~~~~~~~~~~~~~~~~~~~ CONSTRUCTORS ~~~~~~~~~~~~~~~~~~~~
public:
  /**
   * \brief Constructs a set sampler.
   *
   * \param seed  The seed for the random number generator.
   */
  explicit SetSampler(unsigned int seed)
  : m_rng(seed)
  {}

  //~~~~~~~~~~~~~~~~~~~~ PUBLIC MEMBER FUNCTIONS ~~~~~~~~~~~~~~~~~~~~
public:
  /**
   * \brief Picks a random element from the specified set.
   *
   * \param s The set from which to sample.
   * \return  The sampled element.
   */
  T get_sample(const std::set<T>& s)
  {
    int randomIndex = m_rng.generate_int_from_uniform(0, static_cast<int>(s.size()) - 1);
    typename std::set<T>::const_iterator it(s.begin());
    std::advance(it, randomIndex);
    return *it;
  }
};

//#################### FUNCTIONS ####################

/**
 * \brief Makes a 2D descriptor.
 *
 * \param x The x component of the descriptor.
 * \param y The y component of the descriptor.
 * \return  The descriptor.
 */
static Descriptor_CPtr make_2d_descriptor(float x, float y)
{
  Descriptor_Ptr d(new Descriptor(2));
  (*d)[0] = x;
  (*d)[1] = y;
  return d;
}

/**
 * \brief Generates a grid of 2D descriptors over the (min,min)-(max,max) bounding box, with the specified spacing.
 *
 * \param min     The minimum coordinate in the box in both the x and y directions.
 * \param max     The maximum coordinate in the box in both the x and y directions.
 * \param spacing The spacing between the individual descriptors.
 * \return        The grid of descriptors.
 */
static std::vector<Descriptor_CPtr> generate_2d_descriptor_grid(float min, float max, float spacing)
{
  std::vector<Descriptor_CPtr> descriptors;
  for(float x = min; x < max; x += spacing)
  {
    for(float y = min; y < max; y += spacing)
    {
      descriptors.push_back(make_2d_descriptor(x, y));
    }
  }
  return descriptors;
}

/**
 * \brief Rotates a set of 2D examples by a specified angle.
 *
 * \param examples   The set of examples.
 * \param angle      The angle by which to rotate each example (in radians).
 * \return           The rotated set of examples.
 */
static std::vector<Example_CPtr> rotate_examples(const std::vector<Example_CPtr> examples, float angle)
{
  std::vector<Example_CPtr> rotatedExamples;
  for(size_t i = 0, size = examples.size(); i < size; ++i)
  {
    const Descriptor_CPtr& desc = examples[i]->get_descriptor();
    const float x = (*desc)[0], y = (*desc)[1];
    const float rx = x * cosf(angle) - y * sinf(angle);
    const float ry = x * sinf(angle) + y * cosf(angle);
    rotatedExamples.push_back(Example_CPtr(new Example<Label>(make_2d_descriptor(rx,ry), examples[i]->get_label())));
  }
  return rotatedExamples;
}

/**
 * \brief Evaluates the accuracy of the random forest on a set of examples.
 *
 * \param forest    The random forest.
 * \param examples  The set of examples.
 * \return          The accuracy of the random forest on the examples.
 */
static float evaluate_forest_accuracy(const RF_Ptr& forest, const std::vector<Example_CPtr>& examples)
{
  std::set<Label> classLabels;
  size_t examplesSize = examples.size();
  std::vector<Label> expectedLabels(examplesSize), predictedLabels(examplesSize);

  for(size_t i = 0; i < examplesSize; ++i)
  {
    const Example<Label>& example = *examples[i];
    predictedLabels[i] = forest->predict(example.get_descriptor());
    expectedLabels[i] = example.get_label();
    classLabels.insert(expectedLabels[i]);
  }

  Eigen::MatrixXf confusionMatrix = ConfusionMatrixUtil::make_confusion_matrix(classLabels, expectedLabels, predictedLabels);
  return ConfusionMatrixUtil::calculate_accuracy(ConfusionMatrixUtil::normalise_rows_L1(confusionMatrix));
}

int main(int argc, char *argv[])
{
//#define CLASS_IMBALANCE_TEST
#define EXAMPLE_ROTATION_TEST

  // The seed for the random number generator.
  const unsigned int seed = 1234;

  // Generate a set of labels.
#if defined(CLASS_IMBALANCE_TEST)
  const int labelCount = 5;
#elif defined(EXAMPLE_ROTATION_TEST)
  const int labelCount = 5;
#else
  const int labelCount = 20;
#endif
  std::set<Label> classLabels;
  for(int i = 0; i < labelCount; ++i) classLabels.insert(i);

#ifndef CLASS_IMBALANCE_TEST
  // Initialise the set sampler.
  SetSampler<Label> classLabelSampler(seed);
#endif

  // Generate a subset of the labels that are currently observable.
#ifdef CLASS_IMBALANCE_TEST
  std::set<Label> unbiasedClassLabels;
  for(int i = 0; i < labelCount - 1; ++i) unbiasedClassLabels.insert(i);
  std::set<Label> biasedClassLabels;
  biasedClassLabels.insert(labelCount - 1);
#else
  const int currentLabelCount = 2;
  std::set<Label> currentClassLabels;
  for(int i = 0; i < currentLabelCount; ++i) currentClassLabels.insert(i);
#endif

  // Generate the palettes.
  std::map<std::string,cv::Scalar> basicPalette = PaletteGenerator::generate_basic_rgba_palette();
  std::map<Label,cv::Scalar> randomPalette = PaletteGenerator::generate_random_rgba_palette<Label>(classLabels, seed);

  // Initialise a unit example generator.
  const float lowerSTD = 0.05f;
  const float upperSTD = 0.18f;
  UnitCircleExampleGenerator<Label> uceg(classLabels, seed, lowerSTD, upperSTD);

  // Generate the parameter set with which to train the random forest (note that we're using the parameter set generator for convenience only).
  std::vector<ParamSet> params = CartesianProductParameterSetGenerator()
    .add_param("candidateCount", list_of<int>(256))
    .add_param("decisionFunctionGeneratorParams", list_of<std::string>(""))
    .add_param("decisionFunctionGeneratorType", list_of<std::string>(PairwiseOpAndThresholdDecisionFunctionGenerator<Label>::get_static_type()))
    .add_param("gainThreshold", list_of<float>(0.0f))
    .add_param("maxClassSize", list_of<size_t>(10000))
    .add_param("maxTreeHeight", list_of<size_t>(20))
    .add_param("randomSeed", list_of<unsigned int>(seed))
    .add_param("seenExamplesThreshold", list_of<size_t>(50))
    .add_param("splittabilityThreshold", list_of<float>(0.5f))
    .add_param("usePMFReweighting", list_of<bool>(true))
    .generate_param_sets();

  // Register the relevant decision function generators with the factory.
  DecisionFunctionGeneratorFactory<Label>::instance().register_rafl_makers();

  // Initialise the online random forest with the specified parameters.
  const size_t treeCount = 2;
  DT::Settings settings(params[0]);
  RF_Ptr randomForest(new RF(treeCount, settings));

  // Create the tree chopper.
  const size_t maxTreeHeight = 5;
  const size_t timePeriod = 20;
  TimeBasedTreeChopper<Label> treeChopper(TreeChopper_CPtr(new HeightLimitingTreeChopper<Label>(maxTreeHeight, seed)), timePeriod);

  // Generate the windows into which we will display the output of the random forest.
  PlotWindow accuracyPlot("ClassificationAccuracy");
  PlotWindow decisionBoundaryPlot("DecisionBoundary");
  PlotWindow featureSpacePlot("UnitCircleExampleGenerator");

  // Set the initial and maximum time delays between learning iterations (in milliseconds).
  int timeDelay = 20;
  const int maxDelay = 3000;

  // Create an OpenCV trackbar to allow the user to vary the time delay on the fly.
  cv::createTrackbar("Delay", "DecisionBoundary", &timeDelay, maxDelay);

  // Generate a dense set of points covering a square bounding box in the 2D plane.
  const float minVal = -2.5f;
  const float maxVal = 2.5f;
  const float spacing = 0.03f;
  std::vector<Descriptor_CPtr> descriptorGrid = generate_2d_descriptor_grid(minVal, maxVal, spacing);

  // Evaluate the accuracy of the random forest as we add new examples to it over a specified number of learning rounds, and visualise the results.
  std::vector<float> accuracyOverTime;
  const size_t maxLearningRounds = 500;
  for(size_t roundCount = 0; roundCount < maxLearningRounds; ++roundCount)
  {
#if defined(CLASS_IMBALANCE_TEST)
    // Generate a set of examples with an unbalanced number of class labels.
    std::vector<Example_CPtr> currentExamples = uceg.generate_examples(unbiasedClassLabels, 30);
    std::vector<Example_CPtr> currentBiasedExamples = uceg.generate_examples(biasedClassLabels, 3000);
    currentExamples.insert(currentExamples.end(), currentBiasedExamples.begin(), currentBiasedExamples.end());
#elif defined(EXAMPLE_ROTATION_TEST)
    // Rotate the examples by an angle that increases by 10 degrees every 20 rounds until it hits a maximum of 90 degrees.
    static int degreeAngle = 0;
    if(roundCount % 20 == 0) degreeAngle = std::min(90, degreeAngle + 10);
    float radianAngle = static_cast<float>(degreeAngle * M_PI / 180.0);
    std::vector<Example_CPtr> currentExamples = rotate_examples(uceg.generate_examples(classLabels, 50), radianAngle);

    // Chop old trees in the forest as necessary to make the forest adapt better to the new examples as the rotation increases.
    treeChopper.chop_tree_if_necessary(randomForest);
#else
    // Add an additional current class label after every 20 rounds of training.
    if(roundCount % 20 == 0) currentClassLabels.insert(classLabelSampler.get_sample(classLabels));

    // Generate a set of examples from each current class around the unit circle.
    std::vector<Example_CPtr> currentExamples = uceg.generate_examples(currentClassLabels, 50);
#endif

    // Draw the generated points in the feature space plot.
    for(size_t i = 1, iend = currentExamples.size(); i < iend; ++i)
    {
      const Example<Label>& example = *currentExamples[i];
      const Descriptor& descriptor = *example.get_descriptor();
      featureSpacePlot.draw_cartesian_circle(cv::Point2f(descriptor[0], descriptor[1]), randomPalette[example.get_label()], 2, 2);
    }
    featureSpacePlot.draw_cartesian_axes(basicPalette["Red"]);
    featureSpacePlot.refresh();

    // Add the set of examples to the random forest.
    randomForest->add_examples(currentExamples);

    // Predict the labels of the examples passed to the forest under the current hypothesis.
    accuracyOverTime.push_back(evaluate_forest_accuracy(randomForest, currentExamples));

    // Train the random forest by splitting up to the specified number of leaf nodes, and output forest statistics each time the forest
    // has split some nodes in order to help diagnose the learning.
    static const size_t splitBudget = 1;
    if(randomForest->train(splitBudget)) randomForest->output_statistics(std::cout);

    // Plot a line graph showing the accuracy of the forest on the training examples over time.
    accuracyPlot.draw_line_graph(accuracyOverTime, basicPalette["Blue"]);

    // Draw the accuracy values on the same plot as the line graph.
    boost::format threeDecimalPlaces("%0.3f");
    std::string currentAccuracy = (threeDecimalPlaces % accuracyOverTime.back()).str();
    std::string cumulativeAverageAccuracy = (threeDecimalPlaces % static_cast<float>(std::accumulate(accuracyOverTime.begin(), accuracyOverTime.end(), 0.0f) / accuracyOverTime.size())).str();
    accuracyPlot.draw_canvas_text( std::string("CurAccuracy") + currentAccuracy, cv::Point2i(10, accuracyPlot.canvas_height() - 50), basicPalette["White"]);
    accuracyPlot.draw_canvas_text( std::string("AvgAccuracy") + cumulativeAverageAccuracy, cv::Point2i(10, accuracyPlot.canvas_height() - 10), basicPalette["White"]);
    accuracyPlot.refresh();

    // Draw the current decision boundary of the random forest by predicting the labels of the dense set of points generated in the 2D plane.
    for(size_t i = 1, iend = descriptorGrid.size(); i < iend; ++i)
    {
      const Descriptor_CPtr& descriptor = descriptorGrid[i];
      decisionBoundaryPlot.draw_cartesian_circle(cv::Point2f((*descriptor)[0],(*descriptor)[1]), randomPalette[randomForest->predict(descriptor)], 2, 2);
    }
    decisionBoundaryPlot.refresh();

    // Process keyboard input.
    if(cv::waitKey(timeDelay) == 'q') break;

    // Clear the relevant plots.
    featureSpacePlot.clear_figure();
    accuracyPlot.clear_figure();

#ifndef CLASS_IMBALANCE_TEST
    // Remove a current class label after every 40 rounds of training.
    if(roundCount % 40 == 0) currentClassLabels.erase(classLabelSampler.get_sample(classLabels));
#endif

    if(roundCount % 50 == 0)
    {
      const std::string path = "./randomForest" + boost::lexical_cast<std::string>(roundCount) + ".rf";
      SerializationUtil::save_text(path, *randomForest);
      randomForest = SerializationUtil::load_text(path, randomForest);
    }
  }

#undef CLASS_IMBALANCE_TEST

  return 0;
}


// This file is part of the dune-stuff project:
//   https://github.com/wwu-numerik/dune-stuff
// Copyright holders: Rene Milk, Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//
// Contributors: Sven Kaulmann

#ifndef DUNE_STUFF_COMMON_MATH_HH
#define DUNE_STUFF_COMMON_MATH_HH

#include <vector>
#include <limits>
#include <algorithm>
#include <cstring>
#include <iostream>
#include <type_traits>
#include <complex>

#include <dune/stuff/common/disable_warnings.hh>
#include <boost/static_assert.hpp>
#include <boost/fusion/include/void.hpp>
#include <boost/format.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <dune/stuff/common/reenable_warnings.hh>

#include <dune/stuff/common/type_utils.hh>
#include <dune/common/deprecated.hh>

namespace Dune {
namespace Stuff {
namespace Common {

/**
 * since std::numeric_limits<T>::epsilon() is 0 for integral types
 * use this to get the minimum increment/difference for all basic types
 * (or add specializations as necessary ofc)
 **/
template <class T, bool is_integral = std::is_integral<T>::value>
struct Epsilon
{
};

template <class T>
struct Epsilon<T, true>
{
  static const T value;
};

template <class T>
struct Epsilon<T, false>
{
  static const T value;
};

template <>
struct Epsilon<std::string, false>
{
  static const std::string value;
};

template <class T>
const T Epsilon<T, true>::value = 1;
template <class T>
const T Epsilon<T, false>::value = std::numeric_limits<T>::epsilon();

/**
 *  Helper struct to compute absolute values of signed and unsigned values,
 *  std::abs is only defined for signed types.
 **/
template <class T, bool isUnsigned = std::is_unsigned<T>::value>
struct absoluteValue
{
  static T result(const T& val) { return std::abs(val); }
};
template <class T>
struct absoluteValue<T, true>
{
  static T result(const T& val) { return val; }
};

template <class T, bool is_enum = std::is_enum<T>::value>
struct absretval
{
  typedef T type;
};

template <class T>
struct absretval<T, true>
{
  typedef typename underlying_type<T>::type type;
};

template <class T>
typename absretval<T>::type abs(const T& val)
{
  typedef typename absretval<T>::type R;
  return absoluteValue<R>::result(static_cast<R>(val));
}

//! a vector wrapper for continiously updating min,max,avg of some element type vector
template <class ElementType>
class MinMaxAvg
{
  static_assert(!is_complex<ElementType>::value, "complex accumulation not supported");

protected:
  typedef MinMaxAvg<ElementType> ThisType;

public:
  MinMaxAvg() {}

  template <class stl_container_type>
  MinMaxAvg(const stl_container_type& elements)
  {
    static_assert((boost::is_same<ElementType, typename stl_container_type::value_type>::value),
                  "cannot assign mismatching types");
    acc_ = std::for_each(elements.begin(), elements.end(), acc_);
  }

  std::size_t count() const { return boost::accumulators::count(acc_); }
  ElementType sum() const { return boost::accumulators::sum(acc_); }
  ElementType min() const { return boost::accumulators::min(acc_); }
  ElementType max() const { return boost::accumulators::max(acc_); }
  ElementType average() const
  {
    // for integer ElementType this just truncates from floating-point
    return ElementType(boost::accumulators::mean(acc_));
  }

  void operator()(const ElementType& el) { acc_(el); }

  void output(std::ostream& stream)
  {
    stream << boost::format("min: %e\tmax: %e\tavg: %e\n") % min() % max() % average();
  }

protected:
  typedef boost::accumulators::stats<boost::accumulators::tag::max, boost::accumulators::tag::min,
                                     boost::accumulators::tag::mean, boost::accumulators::tag::count,
                                     boost::accumulators::tag::sum> StatsType;
  boost::accumulators::accumulator_set<ElementType, StatsType> acc_;
};

//! \return var bounded in [min, max]
template <typename T>
T clamp(const T var, const T min, const T max)
{
  return (var < min) ? min : (var > max) ? max : var;
}

/**
 * \returns: -1 iff val < 0
 *            0 iff val == 0
 *            1 iff val > 0
 */
template <typename T>
int signum(T val)
{
  return (T(0) < val) - (val < T(0));
}

/** enable us to use DSC::numeric_limits for all types, even when no specialization is avaliable.
 * If there is one, it's used. Otherwise we default to numerical_limtis of double
 **/
template <class T, typename = void>
class numeric_limits : public std::numeric_limits<double>
{
};

template <class T>
class numeric_limits<T, typename std::enable_if<std::numeric_limits<T>::is_specialized>::type>
    : public std::numeric_limits<T>
{
};

//! forward to std::isnan for general types, overload for complex below
template <class T>
bool isnan(T val)
{
  return std::isnan(val);
}

//! override isnan for complex here so it doesn't bleed into the std namespace
template <class T>
bool isnan(std::complex<T> val)
{
  return isnan(std::real(val)) || isnan(std::imag(val));
}

//! forward to std::isinf for general types, overload for complex below
template <class T>
bool isinf(T val)
{
  return std::isinf(val);
}

//! override isinf for complex here so it doesn't bleed into the std namespace
template <class T>
bool isinf(std::complex<T> val)
{
  return isinf(std::real(val)) || isinf(std::imag(val));
}

} // namespace Common
} // namespace Stuff
} // namespace Dune

namespace std {
}
#endif // DUNE_STUFF_COMMON_MATH_HH


//
// For licensing see accompanying LICENSE.txt file.
// Copyright (C) 2020 Apple Inc. All Rights Reserved.
//

#include <cassert>
#include <iostream>
#include <string>

#include <args.hxx>

#include <armadillo>

#include <octomap/octomap.h>
#include <octomap/OcTree.h>

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

    //
    // parse input arguments
    //

    args::ArgumentParser parser("generate_octomap_samples", "");

    args::HelpFlag               help                     (parser, "__DUMMY__",            "Display this help menu", {'h', "help"});
    args::ValueFlag<std::string> octomap_file_arg         (parser, "OCTOMAP_FILE",         "octomap_file",           {"octomap_file"},         args::Options::Required);
    args::ValueFlag<std::string> query_positions_file_arg (parser, "QUERY_POSITIONS_FILE", "query_positions_file",   {"query_positions_file"}, args::Options::Required);
    args::ValueFlag<std::string> output_file_arg          (parser, "OUTPUT_FILE",          "output_file",            {"output_file"},          args::Options::Required);
    args::Flag                   silent_arg               (parser, "__DUMMY__",            "silent",                 {"silent"});

    try {
        parser.ParseCLI(argc, argv);
    } catch (args::Completion e) {
        std::cout << parser;
        return 1;
    } catch (args::Help e) {
        std::cout << parser;
        return 1;
    } catch (args::ParseError e) {
        std::cout << parser;
        std::cout << std::endl << std::endl << std::endl << e.what() << std::endl << std::endl << std::endl;
        return 1;
    } catch (args::RequiredError e) {
        std::cout << parser;
        std::cout << std::endl << std::endl << std::endl << e.what() << std::endl << std::endl << std::endl;
        return 1;
    }

    auto octomap_file         = args::get(octomap_file_arg);
    auto query_positions_file = args::get(query_positions_file_arg);
    auto output_file          = args::get(output_file_arg);
    auto silent               = args::get(silent_arg);

    arma::mat query_positions;

    if (!silent) {
        std::cout << "[HYPERSIM: GENERATE_OCTOMAP_SAMPLES] Begin..." << std::endl;
    }

    //
    // load input data
    //

    octomap::OcTree octree_octomap(octomap_file);
    query_positions.load(query_positions_file, arma::hdf5_binary_trans);

    assert(query_positions.n_cols == 3);

    //
    // perform octomap queries
    //

    if (!silent) {
        std::cout << "[HYPERSIM: GENERATE_OCTOMAP_SAMPLES] Performing Octomap queries..." << std::endl;
    }

    arma::ivec occupancy_samples = arma::ones<arma::ivec>(query_positions.n_rows) * -3;

    for (int i = 0; i < query_positions.n_rows; i++) {
        arma::vec query_position = query_positions.row(i).t();
        if (query_position.is_finite()) {
            auto node_octomap = octree_octomap.search(octomap::point3d(query_positions(i,0), query_positions(i,1), query_positions(i,2)));
            if (node_octomap != nullptr) {
                if (node_octomap->getOccupancy() <= octree_octomap.getOccupancyThres()) {
                    occupancy_samples(i) = 0;
                } else {
                    occupancy_samples(i) = 1;
                }
            } else {
                occupancy_samples(i) = -1;
            }
        } else {
            occupancy_samples(i) = -2;
        }
    }

    //
    // save octomap
    //

    if (!silent) {
        std::cout << "[HYPERSIM: GENERATE_OCTOMAP_SAMPLES] Saving Octomap query results..." << std::endl;
    }

    occupancy_samples.save(output_file, arma::hdf5_binary_trans);

    if (!silent) {
        std::cout << "[HYPERSIM: GENERATE_OCTOMAP_SAMPLES] Finished." << std::endl;
    }

    return 0;
}


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

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/constant/allbits.hpp>
#include <boost/simd/logical.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

template <typename T, std::size_t N, typename Env>
void testl(Env& $)
{
  namespace bs = boost::simd;
  using lT = bs::logical<T>;
  using pl_t = bs::pack<lT, N>;
  using p_t = bs::pack<T, N>;
  lT a1[N];
  T b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? bs::True<lT>() : bs::False<lT>();
    b[i] = bs::genmask(a1[i]);
  }
  pl_t aa1(&a1[0], &a1[0]+N);
  p_t bb(&b[0], &b[0]+N);
  STF_IEEE_EQUAL(bs::genmask(aa1), bb);
}

STF_CASE_TPL("Check genmask on pack of logical", STF_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<bs::logical<T>>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  testl<T, N>($);
  testl<T, N/2>($);
  testl<T, N*2>($);
}

// TODO when if_else_zero is available
#if 0
// STF_CASE_TPL ("Check genmask logical behavior when re-targeted",  STF_NUMERIC_TYPES)
// {
//   namespace bs = boost::simd;
//   using bs::genmask;

//   STF_TYPE_IS(decltype(bs::genmask<char>(bs::logical<T>())),char);

//   STF_IEEE_EQUAL(genmask<char>(bs::logical<T>(true)), bs::Allbits<char>());
//   STF_EQUAL(genmask<char>(bs::logical<T>(false)) , char(0));
// }
#endif


/*
 * Copyright (c) 2018 The ProofOfGaming Core developers
 * Distributed under the MIT software license, see the accompanying
 * file COPYING or http://www.opensource.org/licenses/mit-license.php.
 *
 * FounderPayment.cpp
 *
 *  Created on: Jun 24, 2018
 *      Author: Tri Nguyen
 */

#include "FounderPayment.h"
//#include "standard.h";
#include "util.h"
#include "chainparams.h"
#include <boost/foreach.hpp>
#include "base58.h"

CAmount FounderPayment::getFounderPaymentAmount(int blockHeight, CAmount blockReward) {
	 if (blockHeight <= startBlock){
		 return 0;
	 }
	 for(int i = 0; i < rewardStructures.size(); i++) {
		 FounderRewardStrcuture rewardStructure = rewardStructures[i];
		 if(blockHeight <= rewardStructure.blockHeight) {
			 return blockReward * rewardStructure.rewardPercentage / 100;
		 }
	 }
	 return 0;
}

void FounderPayment::FillFounderPayment(CMutableTransaction& txNew, int nBlockHeight, CAmount blockReward, CTxOut& txoutFounderRet) {
    // make sure it's not filled yet
	txoutFounderRet = CTxOut();
    CScript payee;
    // fill payee with the founder address
    payee = GetScriptForDestination(DecodeDestination(founderAddress));
    // GET FOUNDER PAYMENT VARIABLES SETUP
    CAmount founderPayment = getFounderPaymentAmount(nBlockHeight, blockReward);
    // split reward between miner ...
    txNew.vout[0].nValue -= founderPayment;
    txoutFounderRet = CTxOut(founderPayment, payee);
    txNew.vout.push_back(txoutFounderRet);
    LogPrintf("FounderPayment::FillFounderPayment -- Founder payment %lld to %s\n", founderPayment, founderAddress.c_str());
}

bool FounderPayment::IsBlockPayeeValid(const CTransaction& txNew, const int height, const CAmount blockReward) {
	CScript payee;
	// fill payee with the founder address
	payee = GetScriptForDestination(DecodeDestination(founderAddress));
	const CAmount founderReward = getFounderPaymentAmount(height, blockReward);
	BOOST_FOREACH(const CTxOut& out, txNew.vout) {
		if(out.scriptPubKey == payee && out.nValue >= founderReward) {
			return true;
		}
	}

	return false;
}


#include "stdafx.h"
#include <boost/test/unit_test.hpp>
#include "..\\Lab2Task4Var4\GeneratePrimeNumbersSet.h"

BOOST_AUTO_TEST_SUITE(GeneratePrimeNumbersSet_function)
	BOOST_AUTO_TEST_CASE(NO_PRIME_NUMBERS)
	{
		std::set<int> primeSet = GeneratePrimeNumbersSet(1);
		BOOST_CHECK(primeSet.size() == 0);
	}

	BOOST_AUTO_TEST_CASE(SOME_PRIME_NUMBERS)
	{
		std::set<int> primeSet = GeneratePrimeNumbersSet(9);
		std::set<int> checkPrimeSet = { 2, 3, 5, 7 };
		BOOST_CHECK(primeSet == checkPrimeSet);
	}

	BOOST_AUTO_TEST_CASE(HUNDRED_MILLION_COUNT_OF_PRIME_NUMBERS)
	{
		std::set<int> primeSet = GeneratePrimeNumbersSet(100000000);
		BOOST_CHECK(primeSet.size() == 5761455);
	}
BOOST_AUTO_TEST_SUITE_END()


/**
 * @file 	main.cpp
 * @author 	Fabian Wegscheider
 * @date 	Jul 14, 2017
 */

#include <iostream>
#include <boost/program_options.hpp>
#include <boost/timer/timer.hpp>
#include <boost/graph/adjacency_list.hpp>
#include "GraphParser.h"
#include "PrimeNumbers.h"
#include "SteinerTreeHeuristic.h"

using std::string;
using std::cout;
using std::cerr;
using std::endl;
using namespace boost;
namespace po = boost::program_options;

using Graph = adjacency_list<vecS, vecS, undirectedS,
		no_property, property<edge_weight_t, double>>;

/**
 * The main function which reads in a graph from a .gph file, considers all
 * vertices with prime indices as terminals and calculates a steiner tree
 * using the an improved shortest-path-heuristic based on Dijkstra
 * @param numargs number of inputs on command line
 * @param args array of inputs on command line
 * @return whether the program operated successfully
 */
int main(int numargs, char* args[]) {

	timer::cpu_timer overallTimer;

	string input;
	bool printTreeSelected;
	int root = 1;

	/*parsing command line options*/
	po::options_description desc("Allowed options");
	desc.add_options()
			("help,h", "produce help message")
			("showtree,s", "print tree")
			("root,r", po::value<int>(&root)->default_value(1),
					"set root node (prime number)")
			("input-file", po::value<string>(), "input file");
	po::positional_options_description p;
	p.add("input-file", -1);
	po::variables_map vm;
	po::store(po::command_line_parser(numargs, args).
			options(desc).positional(p).run(), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		exit(EXIT_SUCCESS);
	}

	if (vm.count("input-file")) {
		input = vm["input-file"].as< string >();
	} else {
		cerr << "please specify an input file in the .gph format" << endl;
		exit(EXIT_FAILURE);
	}

	printTreeSelected = vm.count("showtree");
	/*end of parsing command line options*/

	int numVertices;
	int numEdges;

	GraphParser parser(input);
	if (!parser.openedSuccessfully) {
		cerr << "file could not be read" << endl;
		exit(EXIT_FAILURE);
	}

	//first line is read to get number of vertices and edges
	if (!parser.readFirstLine(numVertices, numEdges)) {
		cerr << "error while reading file, not the right format" << endl;
		exit(EXIT_FAILURE);
	}

	Edge* edges = new Edge[numEdges];
	double* weights = new double[numEdges];

	//rest of the file is read and parsed to a graph
	if (!parser.readEdgeData(edges, weights)) {
		cerr << "error while reading file, not the right format" << endl;
		exit(EXIT_FAILURE);
	}

	//undirected graph is constructed with all edges and their weights
	Graph g(edges, edges + numEdges , weights, numVertices);

	//In this array the steiner tree is implicitly stored by remembering the predecessor
	//of each vertex. A -1 means that the vertex is not contained in the tree
	int* treePredecessors = new int[numVertices];

	//all primes in {2,...,numVertices} are considered to be terminals
	vector<int> terminals = PrimeNumbers::findPrimes(numVertices);

	timer::cpu_timer algoTimer;

	//here the steiner tree is finally constructed
	double objectiveValue = SteinerTreeHeuristic::computeSteinerTree(g, numVertices,
			treePredecessors, terminals, root);

	if (objectiveValue == -1) {
		cerr << "the root you selected is not a valid prime number" << endl;
		exit(EXIT_FAILURE);
	}

	string algoTime = algoTimer.format(3, "%w");

	//to this string all edges in the tree are printed if user chose option -s
	string edgeString;
	if (printTreeSelected) {
		assert(SteinerTreeHeuristic::testAndPrintTree(g, numVertices, treePredecessors,
			terminals, root, edgeString));
	} else {
		assert(SteinerTreeHeuristic::testTree(g, numVertices, treePredecessors,
			terminals, root));
	}

	cout << "TLEN: " << objectiveValue << endl;
	if (printTreeSelected) cout << "TREE: " << edgeString << endl;
	cout << "TIME: " << overallTimer.format(3, "%t") << endl;
	cout << "WALL: " << algoTime << endl;
	cout << endl;

	delete[] edges;
	delete[] weights;
	delete[] treePredecessors;

	exit(EXIT_SUCCESS);
}


// Copyright (c) 2005 - 2015 Marc de Kamps
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
//
//    * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
//    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation
//      and/or other materials provided with the distribution.
//    * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software
//      without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
// USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <fstream>
#include <TwoDLib.hpp>
#include "FixtureOde2DSystemGroup.hpp"

using namespace std;
using namespace TwoDLib;

BOOST_FIXTURE_TEST_CASE(SystemGroupConstructionTest,FixtureOde2DSystemGroup){

	std::vector<Redistribution> vec_dummy;
	std::vector<std::vector<Redistribution> > vec_vec_dummy { std::vector< Redistribution>(),std::vector< Redistribution>() } ;
	std::vector<Mesh> vec_mesh  {_mesh1,_mesh2};
	Ode2DSystemGroup sys(vec_mesh,vec_vec_dummy,vec_vec_dummy);
	BOOST_CHECK( sys.Map(0)  == 0 );
	BOOST_CHECK( sys.Map(1)  == 1 );
	BOOST_CHECK( sys.Map(2)  == 2 );
	BOOST_CHECK( sys.Map(3)  == 3 );
	BOOST_CHECK( sys.Map(4)  == 4 );
	BOOST_CHECK( sys.Map(5)  == 5 );
	BOOST_CHECK( sys.Map(6)  == 6 );
	BOOST_CHECK( sys.Map(7)  == 7 );
	BOOST_CHECK( sys.Map(8)  == 8 );
	BOOST_CHECK( sys.Map(9)  == 9 );
	BOOST_CHECK( sys.Map(10)  == 10 );
	BOOST_CHECK( sys.Map(11)  == 11 );
	BOOST_CHECK( sys.Map(12)  == 12);

	sys.Evolve();

	BOOST_CHECK( sys.Map(0)  == 0 );
	BOOST_CHECK( sys.Map(1)  == 1 );
	BOOST_CHECK( sys.Map(2)  == 3 );
	BOOST_CHECK( sys.Map(3)  == 2 );
	BOOST_CHECK( sys.Map(4)  == 6 );
	BOOST_CHECK( sys.Map(5)  == 4 );
	BOOST_CHECK( sys.Map(6)  == 5 );
	BOOST_CHECK( sys.Map(7)  == 7 );
	BOOST_CHECK( sys.Map(8)  == 10 );
	BOOST_CHECK( sys.Map(9)  == 8 );
	BOOST_CHECK( sys.Map(10)  == 9 );
	BOOST_CHECK( sys.Map(11)  == 12 );
	BOOST_CHECK( sys.Map(12)  == 11);

}

BOOST_FIXTURE_TEST_CASE(GroupMapTest, FixtureOde2DSystemGroup){

	std::vector<Redistribution> vec_dummy;
	std::vector<std::vector<Redistribution> > vec_vec_dummy { std::vector< Redistribution>(),std::vector< Redistribution>(),std::vector< Redistribution>() };
	std::vector<Mesh> vec_mesh  {_mesh1, _mesh2, _mesh3};
	Ode2DSystemGroup sys(vec_mesh,vec_vec_dummy,vec_vec_dummy);

	BOOST_CHECK( sys.Map(0,0,0) == 0);
	BOOST_CHECK( sys.Map(0,0,1) == 1);
	BOOST_CHECK( sys.Map(0,1,0) == 2);
	BOOST_CHECK( sys.Map(0,1,1) == 3);
	BOOST_CHECK( sys.Map(0,2,0) == 4);
	BOOST_CHECK( sys.Map(0,2,1) == 5);
	BOOST_CHECK( sys.Map(0,2,2) == 6);
	BOOST_CHECK( sys.Map(1,0,0) == 7);
	BOOST_CHECK( sys.Map(1,1,0) == 8);
	BOOST_CHECK( sys.Map(1,1,1) == 9);
	BOOST_CHECK( sys.Map(1,1,2) == 10);
	BOOST_CHECK( sys.Map(1,2,0) == 11);
	BOOST_CHECK( sys.Map(1,2,1) == 12);
    BOOST_CHECK( sys.Map(2,0,0) == 13);
    BOOST_CHECK( sys.Map(2,0,1) == 14);
    BOOST_CHECK( sys.Map(2,1,0) == 15);
    BOOST_CHECK( sys.Map(2,1,1) == 16);
    BOOST_CHECK( sys.Map(2,2,0) == 17);
    BOOST_CHECK( sys.Map(2,2,1) == 18);
    BOOST_CHECK( sys.Map(2,2,2) == 19);

	sys.Evolve();
	BOOST_CHECK( sys.Map(0,0,0) == 0);
	BOOST_CHECK( sys.Map(0,0,1) == 1);
	BOOST_CHECK( sys.Map(0,1,0) == 3);
	BOOST_CHECK( sys.Map(0,1,1) == 2);
	BOOST_CHECK( sys.Map(0,2,0) == 6);
	BOOST_CHECK( sys.Map(0,2,1) == 4);
	BOOST_CHECK( sys.Map(0,2,2) == 5);
	BOOST_CHECK( sys.Map(1,0,0) == 7);
	BOOST_CHECK( sys.Map(1,1,0) == 10);
	BOOST_CHECK( sys.Map(1,1,1) == 8);
	BOOST_CHECK( sys.Map(1,1,2) == 9);
	BOOST_CHECK( sys.Map(1,2,0) == 12);
	BOOST_CHECK( sys.Map(1,2,1) == 11);
	BOOST_CHECK( sys.Map(2,0,0) == 13);
	BOOST_CHECK( sys.Map(2,0,1) == 14);
	BOOST_CHECK( sys.Map(2,1,0) == 16);
	BOOST_CHECK( sys.Map(2,1,1) == 15);
	BOOST_CHECK( sys.Map(2,2,0) == 19);
	BOOST_CHECK( sys.Map(2,2,1) == 17);
	BOOST_CHECK( sys.Map(2,2,2) == 18);

}

BOOST_AUTO_TEST_CASE(ConductanceMapTest){

	Mesh mesh("condee2a5ff4-0087-4d69-bae3-c0a223d03693.mesh");
	Stat stat("condee2a5ff4-0087-4d69-bae3-c0a223d03693.stat");

	std::vector<Quadrilateral> vecq = stat.Extract();
	mesh.InsertStationary(vecq[0]);

	std::vector<Redistribution> vec_dummy;
	std::vector<std::vector<Redistribution> > vec_vec_dummy{ std::vector< Redistribution>() };

	std::vector<Mesh> vec_mesh{mesh};
	Ode2DSystemGroup sys(vec_mesh,vec_vec_dummy,vec_vec_dummy);

	BOOST_CHECK( sys.Map(0) == 0);
	BOOST_CHECK( sys.Map(1) == 1);
	BOOST_CHECK( sys.Map(2) == 2);
	BOOST_CHECK( sys.Map(3) == 3);

	sys.Evolve();

	BOOST_CHECK( sys.Map(0) == 0);
	BOOST_CHECK( sys.Map(1) == 700);
	BOOST_CHECK( sys.Map(2) == 1);
	BOOST_CHECK( sys.Map(3) == 2);

	sys.Evolve();

	BOOST_CHECK( sys.Map(0) == 0);
	BOOST_CHECK( sys.Map(1) == 699);
	BOOST_CHECK( sys.Map(2) == 700);
	BOOST_CHECK( sys.Map(3) == 1);
}


#include <boost/graph/adjacency_list.hpp>
#include <string>
typedef std::string vertex_t;

typedef boost::adjacency_list<
    boost::vecS
    , boost::vecS
    , boost::bidirectionalS
    , vertex_t
> graph_type;

#include <boost/utility/string_ref.hpp>
#include <iostream>
template <class GraphT>
void find_and_print(const GraphT& g, boost::string_ref name) {
    typedef typename boost::graph_traits<graph_type>
            ::vertex_iterator vert_it_t;
    vert_it_t it, end;
    boost::tie(it, end) = boost::vertices(g);

    typedef boost::graph_traits<graph_type>::vertex_descriptor desc_t;
    for (; it != end; ++ it) {
        desc_t desc = *it;
        if (boost::get(boost::vertex_bundle, g)[desc] == name.data()) {
            break;
        }
    }

    assert(it != end);
    std::cout << name << '\n';
} /* find_and_print */

int main() {
    graph_type graph;

    static const std::size_t vertex_count = 5;
    graph.m_vertices.reserve(vertex_count);
    /*
    C++ -> STL -> Boost -> C++ guru <- C
    */

    typedef boost::graph_traits<graph_type>
            ::vertex_descriptor descriptor_t;
    descriptor_t cpp
        = boost::add_vertex(vertex_t("C++"), graph);
    descriptor_t stl
        = boost::add_vertex(vertex_t("STL"), graph);
    descriptor_t boost
        = boost::add_vertex(vertex_t("Boost"), graph);
    descriptor_t guru
        = boost::add_vertex(vertex_t("C++ guru"), graph);
    descriptor_t ansic
        = boost::add_vertex(vertex_t("C"), graph);

    BOOST_STATIC_ASSERT((boost::is_same<descriptor_t, std::size_t>::value));

    boost::add_edge(cpp, stl, graph);
    boost::add_edge(stl, boost, graph);
    boost::add_edge(boost, guru, graph);
    boost::add_edge(ansic, guru, graph);

    find_and_print(graph, "Boost");
    find_and_print(graph, "C++ guru");
}


#ifndef COMMON_HEADER_HPP
#define COMMON_HEADER_HPP

#include <type_traits>
#include <cassert>
#include <iterator>

#include <boost/mpl/apply.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/multiplies.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/lambda.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/identity.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/not_equal_to.hpp>
#include <boost/mpl/greater.hpp>

namespace mpl = boost::mpl;
using namespace mpl::placeholders;

#endif