text:
/**
BSD 3-Clause License

Copyright (c) 2018, Khalifa University Robotics Institute
Copyright (c) 2018, Tarek Taha tarek@tarektaha.com
Copyright (c) 2018, Reem Ashour reemashour1@gmail.com
Copyright (c) 2020, Mohamed Abdelkader mohamedashraf123@gmail.com
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 HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <eigen_conversions/eigen_msg.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <math.h>
#include <ros/package.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <std_msgs/Bool.h>
#include <tf/tf.h>
#include <tf_conversions/tf_eigen.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cmath>
#include <deque>
#include "geometry_msgs/PoseStamped.h"
#include "message_filters/subscriber.h"
#include "nav_msgs/Odometry.h"
#include "ros/ros.h"
#include "sensor_msgs/PointCloud.h"
#include "tf/message_filter.h"
#include "tf/transform_listener.h"

#include <pcl/common/eigen.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/range_image/range_image.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl_conversions/pcl_conversions.h>
#include <iostream>

#include <octomap/ColorOcTree.h>
#include <octomap/OcTree.h>
#include <octomap/octomap.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <pcl/filters/voxel_grid.h>
#include <tf/transform_broadcaster.h>
#include <string>

#include <octomap/OcTree.h>
#include <octomap/Pointcloud.h>
#include <octomap/octomap.h>
#include <octomap_msgs/conversions.h>
#include <chrono>
#include <thread>

#include <std_srvs/Empty.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>

#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <semantic_exploration/GetPath.h>
#include <waypoint_navigator/GoToPoseWaypoints.h>
#include <geometry_msgs/PoseArray.h>
#include <controller_msgs/FlatTarget.h>

#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

#define SQ(x) ((x) * (x))
#define SQRT2 0.70711

using namespace std;
using namespace octomap;

struct Params
{
    double dt;
    int numIterations;
    bool iflog;
    int root_note_delay_indicator ;
};

class ExplorationPlanner
{
protected:
    ros::NodeHandle nh_;
    ros::NodeHandle nh_private_;
    Params params_;
    geometry_msgs::PoseStamped currentPose, explorationViewPointPose;
    ros::Publisher explorationViewpointPub;
    ros::Publisher rotationPublisher;
    ros::Subscriber localPoseSub;
    ros::Subscriber odomSub;
    ros::Subscriber stateSub;
    bool currentPositionUpdated;
    mavros_msgs::State currentState;

public:
    ExplorationPlanner(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private);
    ~ExplorationPlanner();
    void poseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg);
    void stateCallback(const mavros_msgs::State::ConstPtr& msg);
    bool SetParams();
    void RunStateMachine();
    void rotateOnSpot(float duration);
    void moveOnSpot(float duration);
    void odomCallback(const nav_msgs::Odometry& pose);
};

ExplorationPlanner::~ExplorationPlanner()
{

}

ExplorationPlanner::ExplorationPlanner(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private)
    : nh_(nh), nh_private_(nh_private)
{
    currentPositionUpdated = false;
    std::string droneName = ros::this_node::getName();
    std::string droneNameSpace = ros::this_node::getNamespace();

    ROS_INFO("Drone Name:%s NameSpace:%s", droneName.c_str(), droneNameSpace.c_str());

    explorationViewpointPub =
            nh_.advertise<geometry_msgs::PoseStamped>("semantic_exploration_viewpoint", 10);

    //TODO: make this a parameter and make it global
    bool use_gazebo_ground_truth_ = false;
    if(!ros::param::get("/system/localization/use_gazebo_ground_truth", use_gazebo_ground_truth_)){
        ROS_WARN("Could not get param  /system/localization/use_gazebo_ground_truth . Setting default to true");
        use_gazebo_ground_truth_ = true;
    }

    // Either use perfect positioning from gazebo, or get the px4 estimator position through mavros
    if (use_gazebo_ground_truth_)
    {
        odomSub = nh_.subscribe("odometry", 10, &ExplorationPlanner::odomCallback, this);
    }
    else
    {
        localPoseSub = nh_.subscribe<geometry_msgs::PoseStamped>(droneNameSpace +
                                                                 "/mavros/local_position/pose", 10, &ExplorationPlanner::poseCallback, this);
    }

    rotationPublisher = nh_.advertise<controller_msgs::FlatTarget>("reference/flatsetpoint", 100);
    stateSub = nh_.subscribe("/mavros/state", 10, &ExplorationPlanner::stateCallback, this);

    if (!ExplorationPlanner::SetParams())
    {
        ROS_ERROR("Could not start. Parameters missing!");
    }
}

void ExplorationPlanner::rotateOnSpot(float duration)
{

    double sleepTime = duration / 360.0f;
    double lowerAltitude = 0.05;
    double altitudeIterations = 3.0f;
    double altitudeSteps = (currentPose.pose.position.z - lowerAltitude) / altitudeIterations;
    ros::Rate loopRate(1/sleepTime);
    for(int j=0;j<=altitudeIterations;j++)
    {
        for(int i=0; i<360; i++)
        {
            double yaw = i * 3.14 / 180.0;
            controller_msgs::FlatTarget yawCommand;
            yawCommand.type_mask = 2;
            yawCommand.position.x = this->currentPose.pose.position.x + j*0.2;
            yawCommand.position.y = this->currentPose.pose.position.y - j*0.2;
            yawCommand.position.z = lowerAltitude + j*altitudeSteps;
            yawCommand.snap.z     = yaw;
            //ROS_INFO_THROTTLE(0.5,"Pose Yaw sent:%f sleep time:%f x:%f y:%f z:%f", yaw, sleepTime,yawCommand.position.x ,yawCommand.position.y, yawCommand.position.z);
            rotationPublisher.publish(yawCommand);
            loopRate.sleep();
        }
    }
}
void ExplorationPlanner::moveOnSpot(float duration)
{
    double sleepTime = duration / 360.0f;
    double lowerAltitude = 1;
    double altitudeIterations = 3.0f;
    ros::Rate loopRate(1/sleepTime);
    for(int j=0;j<=altitudeIterations;j++)
    {
        for(int i=0; i<360; i++)
        {
            double yaw = 0;
            controller_msgs::FlatTarget movrCommand;
            movrCommand.type_mask = 2;
            movrCommand.position.x = this->currentPose.pose.position.x + j*0.2;
            movrCommand.position.y = this->currentPose.pose.position.y;
            movrCommand.position.z = this->currentPose.pose.position.z;
            movrCommand.snap.z     = yaw;
            //ROS_INFO_THROTTLE(0.5,"Pose Yaw 2 sent:%f sleep time:%f x:%f y:%f z:%f", yaw, sleepTime,movrCommand.position.x ,movrCommand.position.y, movrCommand.position.z);
            rotationPublisher.publish(movrCommand);
            loopRate.sleep();
        }
    }
    ROS_INFO("####################### DONE ###################################");
}

void ExplorationPlanner::stateCallback(const mavros_msgs::State::ConstPtr& msg)
{
    currentState = *msg;
}

void ExplorationPlanner::poseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    this->currentPose = *msg;
    currentPositionUpdated = true;
}

void ExplorationPlanner::odomCallback(const nav_msgs::Odometry& pose)
{
    this->currentPose.pose = pose.pose.pose;
    currentPositionUpdated = true;
}

void ExplorationPlanner::RunStateMachine()
{
    ros::Rate loopRate(10);

    std_srvs::Empty srv;
    bool unpaused = ros::service::call("/gazebo/unpause_physics", srv);
    unsigned int i = 0;

    // Trying to unpause Gazebo for 10 seconds.
    while (i <= 10 && !unpaused)
    {
        ROS_INFO("Wait for 1 second before trying to unpause Gazebo again.");
        std::this_thread::sleep_for(std::chrono::seconds(1));
        unpaused = ros::service::call("/gazebo/unpause_physics", srv);
        ++i;
    }

    if (!unpaused)
    {
        ROS_FATAL("Could not wake up Gazebo.");
        return;
    }
    else
    {
        ROS_INFO("Unpaused the Gazebo simulation.");
    }

    while (!currentPositionUpdated)
    {
        ROS_INFO_THROTTLE(1, "Waiting for Pose");
        ros::spinOnce();
        loopRate.sleep();
    }

    ROS_INFO("initial Position %f , %f , %f", currentPose.pose.position.x,
             currentPose.pose.position.y, currentPose.pose.position.z);
    int iteration = 0;
    int seqNum = 0;

    while(currentState.mode != "OFFBOARD" && currentState.system_status !=4)
    {
        //ROS_INFO_THROTTLE(0.1,"Mode:%s Status:%d",currentState.mode.c_str(), currentState.system_status);
        ros::spinOnce();
        loopRate.sleep();
    }

    // Dirty hack, please fix Wait for the drone to take off
    for(int i=0;i<100;i++)
    {
        ros::spinOnce();
        loopRate.sleep();
    }

    //slowly rotate on the spot for 10 seconds
    rotateOnSpot(20);
    moveOnSpot(10);

    // simulate the camera position publisher by broadcasting the /tf
    tf::TransformBroadcaster br;
    tf::StampedTransform transform;
    tf::TransformListener listener;
    std::string targetFrame = "local_origin";
    geometry_msgs::PoseStamped transformedPose;

    // Start planning: The planner is called and the computed path sent to the controller.
    ros::Time startTime = ros::Time::now();
    while (ros::ok())
    {
        ROS_INFO("Planning iteration %i", iteration);

        /* TODO: handling the frames need improvement.
         * Either the service call should pass the target frame and recieved the already transformed
         * target frames back, or make target frame something that could be modified in a yaml file.
         * Currently it's hard coded above
         */
        std::vector<geometry_msgs::Pose> waypoints;
        geometry_msgs::PoseStamped poseMsg_;
        semantic_exploration::GetPath planSrv;
        planSrv.request.header.stamp = ros::Time::now();
        planSrv.request.header.seq = static_cast<uint>(iteration);
        planSrv.request.header.frame_id = "world";
        waypoint_navigator::GoToPoseWaypoints gotoPoseWaypointsSrv;

        ROS_INFO("Planner Call");
        ros::service::waitForService("rrt_planner", ros::Duration(1.0));
        if (ros::service::call("rrt_planner", planSrv))
        {
            ROS_INFO("Planner Call Successfull");
            seqNum++;
            if (planSrv.response.path.size() == 0)
            {
                ROS_INFO(" ****************** No Path *************************");
                ros::Duration(1.0).sleep();
            }
            else
            {
                ROS_INFO(" #################### Path Found #######################");

                ROS_INFO("planSrv.response.path.size():%zu", planSrv.response.path.size());

                for (uint i = 0; i < planSrv.response.path.size(); i++)
                {
                    poseMsg_.header.seq = static_cast<uint>(seqNum);
                    poseMsg_.header.stamp = ros::Time::now();
                    poseMsg_.header.frame_id = "world";
                    poseMsg_.pose.position.x = planSrv.response.path[i].position.x;
                    poseMsg_.pose.position.y = planSrv.response.path[i].position.y;
                    poseMsg_.pose.position.z = planSrv.response.path[i].position.z;

                    tf::Pose pose;
                    tf::poseMsgToTF(planSrv.response.path[i], pose);
                    double yaw = tf::getYaw(pose.getRotation());
                    tf::Quaternion quat = tf::Quaternion(tf::Vector3(0.0, 0.0, 1.0), yaw);
                    poseMsg_.pose.orientation.x = quat[0];
                    poseMsg_.pose.orientation.y = quat[1];
                    poseMsg_.pose.orientation.z = quat[2];
                    poseMsg_.pose.orientation.w = quat[3];
                    try
                    {
                        listener.lookupTransform(poseMsg_.header.frame_id, targetFrame, ros::Time(0),
                                                 transform);
                    }
                    catch (tf::TransformException ex)
                    {
                        ROS_ERROR("%s", ex.what());
                        continue;
                    }
                    listener.transformPose(targetFrame, poseMsg_, transformedPose);

                    ROS_DEBUG("Sent In iterate %f %f  %f %f %f %f %f ", poseMsg_.pose.position.x,
                              poseMsg_.pose.position.y, poseMsg_.pose.position.z,
                              poseMsg_.pose.orientation.x, poseMsg_.pose.orientation.y,
                              poseMsg_.pose.orientation.z, poseMsg_.pose.orientation.w);

                    ROS_INFO("Received: %f %f %f Transformed: %f %f %f", poseMsg_.pose.position.x,
                             poseMsg_.pose.position.y, poseMsg_.pose.position.z,
                             transformedPose.pose.position.x, transformedPose.pose.position.y,
                             transformedPose.pose.position.z);

                    std_msgs::String mission_id;
                    mission_id.data = "mission_id" + std::to_string(iteration);
                    waypoints.clear();
                    waypoints.push_back(poseMsg_.pose);
                    gotoPoseWaypointsSrv.request.waypoints = waypoints;
                    gotoPoseWaypointsSrv.request.mission_id = mission_id;

                    ros::service::waitForService("go_to_pose_waypoints", ros::Duration(1.0));

                    if(!ros::service::call("go_to_pose_waypoints", gotoPoseWaypointsSrv))
                    {
                        ROS_WARN("Calling waypoint navigator service failed");
                    }

                    //explorationViewpointPub.publish(transformedPose);
                    ros::spinOnce();
                    sleep(params_.root_note_delay_indicator); // An indication that the drone reached the NBV
                    ros::Duration(params_.dt).sleep();
                }
            }
        }
        else
        {
            ROS_WARN_THROTTLE(1, "Planner not reachable");
            ros::Duration(1.0).sleep();
        }

        if (++iteration > params_.numIterations)
        {
            ROS_INFO("Finished all iterations:%d, exiting", iteration);
            break;
        }
        ros::spinOnce();
        loopRate.sleep();
    }

    ros::Time endTime = ros::Time::now();
    double elapsed = endTime.toSec() - startTime.toSec();
    std::cout << "The planning Time " << elapsed << std::endl;
}

bool ExplorationPlanner::SetParams()
{
    //std::string ns = ros::this_node::getName();
    //std::cout<<"Node name is:"<<ns<<"\n";
    std::string ns = "";
    bool ret = true;

    // Exploration Algorithm Params
    // 1- Termination

    params_.numIterations = 50;
    if (!ros::param::get(ns + "/exploration/num_iterations", params_.numIterations))
    {
        ROS_WARN("No number of iteration for termination specified. Looking for %s",
                 (ns + "/exploration/num_iterations").c_str());
    }

    params_.dt = 1.0;
    if (!ros::param::get(ns + "/exploration/dt", params_.dt))
    {
        ROS_WARN("Delta T between Iterations not found. Looking for %s",
                 (ns + "/exploration/dt").c_str());
    }
    params_.iflog = false;
    if (!ros::param::get(ns + "/exploration/log/on", params_.iflog))
    {
        ROS_WARN("No log is specified. Looking for %s.", (ns + "/exploration/log/on").c_str());
    }

    params_.root_note_delay_indicator = 5;
    if (!ros::param::get(ns + "/exploration/drone_arrival_duration", params_.root_note_delay_indicator))
    {
        ROS_WARN("No number of iteration for termination specified. Looking for %s",
                 (ns + "/exploration/drone_arrival_duration").c_str());
    }

    return ret;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "exploration_planner_node");
    ros::NodeHandle nh;
    ros::NodeHandle nh_private("~");
    ExplorationPlanner expObj(nh, nh_private);
    expObj.RunStateMachine();
    return 0;
}


text:
/* Unit testing for outcomes
(C) 2013-2020 Niall Douglas <http://www.nedproductions.biz/> (1 commit)

Boost Software License - Version 1.0 - August 17th, 2003

Permission is hereby granted, free of charge, to any person or organization
obtaining a copy of the software and accompanying documentation covered by
this license (the "Software") to use, reproduce, display, distribute,
execute, and transmit the Software, and to prepare derivative works of the
Software, and to permit third-parties to whom the Software is furnished to
do so, all subject to the following:

The copyright notices in the Software and this entire statement, including
the above license grant, this restriction and the following disclaimer,
must be included in all copies of the Software, in whole or in part, and
all derivative works of the Software, unless such copies or derivative
works are solely in the form of machine-executable object code generated by
a source language processor.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
DEALINGS IN THE SOFTWARE.
*/

#include <boost/outcome/experimental/status_result.hpp>
#include <boost/outcome/try.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/unit_test_monitor.hpp>

#ifndef BOOST_OUTCOME_SYSTEM_ERROR2_NOT_POSIX
namespace issues220
{
  namespace outcome_e = BOOST_OUTCOME_V2_NAMESPACE::experimental;

  template <class T, class E = outcome_e::error> using Result = outcome_e::status_result<T, E>;

  template <class T> using PosixResult = outcome_e::status_result<T, outcome_e::posix_code>;

  Result<int> convert(const PosixResult<int> &posix_result) { return Result<int>(posix_result); }
}  // namespace issues220
#endif

BOOST_OUTCOME_AUTO_TEST_CASE(issues_0220_test, "ubsan reports reference binding to null pointer")
{
#ifndef BOOST_OUTCOME_SYSTEM_ERROR2_NOT_POSIX
  using namespace issues220;
  BOOST_CHECK(convert(PosixResult<int>(0)).value() == 0);
#endif
}

ext: cpp

max_issues_repo_name: qingkouwei/mediaones

max_forks_repo_name: qingkouwei/mediaones


text:
//
// Copyright (c) 2016-2019 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// 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)
//
// Official repository: https://github.com/CPPAlliance/url
//

#ifndef BOOST_URL_IMPL_QUERY_PARAMS_BNF_IPP
#define BOOST_URL_IMPL_QUERY_PARAMS_BNF_IPP

#include <boost/url/rfc/query_params_bnf.hpp>
#include <boost/url/bnf/parse.hpp>
#include <boost/url/rfc/char_sets.hpp>
#include <boost/url/rfc/pct_encoded_bnf.hpp>

namespace boost {
namespace urls {

bool
query_params_bnf::
begin(
    char const*& it,
    char const* const end,
    error_code& ec,
    query_param& t) noexcept
{
    using bnf::parse;

    // key
    if(! parse(it, end, ec,
        pct_encoded_bnf<
            masked_char_set<
                qpchar_mask>>{t.key}))
        return false;

    // "="
    t.has_value = parse(
        it, end, ec, '=');
    if(! t.has_value)
    {
        // key with no value
        ec = {};
        return true;
    }

    // value
    return parse(it, end, ec,
        pct_encoded_bnf<
            masked_char_set<
                qpchar_mask |
                equals_char_mask>>{
                    t.value});
}

bool
query_params_bnf::
increment(
    char const*& it,
    char const* const end,
    error_code& ec,
    query_param& t) noexcept
{
    using bnf::parse;
    if(! parse(it, end, ec, '&'))
    {
        // end of list
        ec = error::end;
        return false;
    }

    // key
    if(! parse(it, end, ec,
        pct_encoded_bnf<
            masked_char_set<
                qpchar_mask>>{t.key}))
        return false;

    // "="
    t.has_value = parse(
        it, end, ec, '=');
    if(! t.has_value)
    {
        // key with no value
        ec = {};
        return true;
    }

    // value
    return parse(it, end, ec,
        pct_encoded_bnf<
            masked_char_set<
                qpchar_mask |
                equals_char_mask>>{
                    t.value});
}

} // urls
} // boost

#endif

ext: ipp

max_stars_repo_name: TankerHQ/url

max_issues_repo_name: TankerHQ/url

max_forks_repo_name: TankerHQ/url


text:
//=============================================================================================================
/**
* @file     subjecttreeitem.cpp
* @author   Lorenz Esch <Lorenz.Esch@tu-ilmenau.de>;
*           Matti Hamalainen <msh@nmr.mgh.harvard.edu>
* @version  1.0
* @date     May, 2016
*
* @section  LICENSE
*
* Copyright (C) 2016, Lorenz Esch and Matti Hamalainen. 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 MNE-CPP authors 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.
*
*
* @brief    SubjectTreeItem class definition.
*
*/

//*************************************************************************************************************
//=============================================================================================================
// INCLUDES
//=============================================================================================================

#include "subjecttreeitem.h"
#include "../measurement/measurementtreeitem.h"
#include "../mri/mritreeitem.h"
#include "../bem/bemsurfacetreeitem.h"
#include "../bem/bemtreeitem.h"
#include "../sensorspace/sensorsettreeitem.h"
#include "../sensorspace/sensorsurfacetreeitem.h"

//*************************************************************************************************************
//=============================================================================================================
// Qt INCLUDES
//=============================================================================================================

//*************************************************************************************************************
//=============================================================================================================
// Eigen INCLUDES
//=============================================================================================================

#include <Eigen/Core>

//*************************************************************************************************************
//=============================================================================================================
// USED NAMESPACES
//=============================================================================================================

using namespace FSLIB;
using namespace DISP3DLIB;

//*************************************************************************************************************
//=============================================================================================================
// DEFINE MEMBER METHODS
//=============================================================================================================

SubjectTreeItem::SubjectTreeItem(int iType, const QString& text)
: AbstractTreeItem(iType, text)
{
    initItem();
}

//*************************************************************************************************************

void SubjectTreeItem::initItem()
{
    this->setEditable(false);
    this->setCheckable(true);
    this->setCheckState(Qt::Checked);
    this->setToolTip("Subject");
}

//*************************************************************************************************************

void SubjectTreeItem::connectMeasurementToMriItems(MeasurementTreeItem* pMeasurementItem)
{
    //Connect mri item with the measurement tree items in case the real time color changes (i.e. rt source loc)
    //or the user changes the color origin
    QList<QStandardItem*> mriItemList = this->findChildren(Data3DTreeModelItemTypes::MriItem);

    foreach(QStandardItem* item, mriItemList) {
        if(MriTreeItem* pMriItem = dynamic_cast<MriTreeItem*>(item)) {
            connect(pMeasurementItem, &MeasurementTreeItem::sourceColorChanged,
                pMriItem, &MriTreeItem::setRtVertColor);
            connect(pMriItem, &MriTreeItem::colorOriginChanged,
                pMeasurementItem, &MeasurementTreeItem::setSourceColors);
        }
    }
}

//*************************************************************************************************************

void SubjectTreeItem::connectEEGMeasurementToBemHeadItems(MeasurementTreeItem* pMeasurementItem)
{
    //Connect bem head item with the measurement tree items in case the real time color changes (i.e. rt source loc)
    QList<QStandardItem*> bemItemList = this->findChildren(Data3DTreeModelItemTypes::BemItem);
    QList<QStandardItem*> bemSurfacesItemList;

    foreach(QStandardItem* item, bemItemList) {
        if(BemTreeItem* pBemItem = dynamic_cast<BemTreeItem*>(item)) {
            bemSurfacesItemList = pBemItem->findChildren(Data3DTreeModelItemTypes::BemSurfaceItem);

            foreach(QStandardItem* itemBemSurf, bemSurfacesItemList) {
                if(BemSurfaceTreeItem* pBemSurfItem = dynamic_cast<BemSurfaceTreeItem*>(itemBemSurf)) {
                    if(pBemSurfItem->text() == "Head") {
                        connect(pMeasurementItem, &MeasurementTreeItem::sensorEEGColorChanged,
                            pBemSurfItem, &BemSurfaceTreeItem::setVertColor);
                        connect(pBemSurfItem, &BemSurfaceTreeItem::colorOriginChanged,
                            pMeasurementItem, &MeasurementTreeItem::setSensorEEGColors);
                    }
                }
            }
        }
    }
}

//*************************************************************************************************************

void SubjectTreeItem::connectMEGMeasurementToSensorItems(MeasurementTreeItem* pMeasurementItem, QStandardItem* parent)
{
    //Connect bem sensor surface item with the measurement tree items in case the real time color changes (i.e. rt source loc)
    if(parent) {
        //Find sensor items which are stored as subejct items
        for(int j = 0; j < parent->rowCount(); ++j) {
            if(SubjectTreeItem* pSensorItem = dynamic_cast<SubjectTreeItem*>(parent->child(j))) {
                QList<QStandardItem*> sensorSetItemList = pSensorItem->findChildren(Data3DTreeModelItemTypes::SensorSetItem);
                QList<QStandardItem*> sensorSurfacesItemList;

                for(int i = 0; i < sensorSetItemList.size(); ++i) {
                    if(SensorSetTreeItem* pSensorSetItem = dynamic_cast<SensorSetTreeItem*>(sensorSetItemList.at(i))) {
                        sensorSurfacesItemList = pSensorSetItem->findChildren(Data3DTreeModelItemTypes::SensorSurfaceItem);

                        for(int k = 0; k < sensorSurfacesItemList.size(); ++k) {
                            if(SensorSurfaceTreeItem* pSensorSetSurfItem = dynamic_cast<SensorSurfaceTreeItem*>(sensorSurfacesItemList.at(k))) {
                                if(pSensorSetSurfItem->text() == "Sensor Surface") {
                                    connect(pMeasurementItem, &MeasurementTreeItem::sensorMEGColorChanged,
                                            pSensorSetSurfItem, &SensorSurfaceTreeItem::setVertColor);
                                    connect(pSensorSetSurfItem, &SensorSurfaceTreeItem::colorOriginChanged,
                                            pMeasurementItem, &MeasurementTreeItem::setSensorMEGColors);
                                }
                            }
                        }
                    }
                }
            }
        }
    }
}

max_stars_repo_name: ChunmingGu/mne-cpp-master

max_issues_repo_name: ChunmingGu/mne-cpp-master

max_forks_repo_name: ChunmingGu/mne-cpp-master


text:
/**
 * Copyright (c) 2017-present, Facebook, Inc. and its affiliates.
 * All rights reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree.
 */
#include "TextConfigUpdater.h"

#include <boost/filesystem.hpp>
#include <folly/hash/SpookyHashV2.h>
#include <folly/compression/Compression.h>

#include "logdevice/common/debug.h"
#include "logdevice/common/settings/SettingsUpdater.h"

#include "logdevice/common/configuration/ParsingHelpers.h"
#include "logdevice/include/Err.h"

using namespace facebook::logdevice::configuration;
namespace facebook { namespace logdevice {

static const char* LOCATION_SCHEME_DELIMITER = ":";
// This assumes all sources use '/' for path delimiters which won't
// necessarily be universally true.  Refactor as needed.
static const char PATH_DELIMITER = '/';
// Calculates a 10-character hash of the config (used when the source doesn't
// provide one), exposed by the "info config --hash" admin command
static std::string hash_contents(const std::string& str);
// Compares hashes in ConfigMetadata, return true if they are equal.
static bool hashes_equal(const ServerConfig::ConfigMetadata& a,
                         const ServerConfig::ConfigMetadata& b);

int TextConfigUpdaterImpl::load(
    const std::string& location,
    std::unique_ptr<LogsConfig> alternative_logs_config,
    const ConfigParserOptions& options) {
  std::shared_ptr<UpdateableServerConfig> server_config =
      target_server_config_.lock();
  ld_check(server_config != nullptr);
  alternative_logs_config_ = std::move(alternative_logs_config);
  config_parser_options_ = options;

  std::tie(main_config_state_.source, main_config_state_.path) =
      parseLocation(location);
  if (main_config_state_.source == nullptr) {
    err = E::INVALID_PARAM;
    return -1;
  }

  return fetchFromSource();
}

int TextConfigUpdaterImpl::fetchFromSource() {
  // Request main config contents from the source.
  ld_info("Requesting main config \"%s\" (source: %s)",
          main_config_state_.path.c_str(),
          main_config_state_.source->getName().c_str());

  ConfigSource::Output output;
  Status st =
      main_config_state_.source->getConfig(main_config_state_.path, &output);
  if (st == E::OK) {
    onContents(&main_config_state_, std::move(output), true);
    return 0;
  } else if (st == E::NOTREADY) {
    // Not calling update() but returning success, expecting an async result
    // to construct the config.
    return 0;
  } else {
    return -1;
  }
}

bool TextConfigUpdaterImpl::hasValidConfig() {
  std::shared_ptr<UpdateableServerConfig> server_config =
      target_server_config_.lock();
  return server_config->get() != nullptr && !invalid_logs_config_;
}

void TextConfigUpdaterImpl::onAsyncGet(ConfigSource* source,
                                       const std::string& path,
                                       Status status,
                                       ConfigSource::Output output) {
  if (destroying_) {
    // This TextConfigUpdater is being destroyed.
    return;
  }

  if (status != E::OK) {
    initial_config_sem_.post();
    return;
  }

  State* affected_state;
  if (source == main_config_state_.source && path == main_config_state_.path) {
    affected_state = &main_config_state_;
  } else if (source == included_config_state_.source &&
             path == included_config_state_.path) {
    affected_state = &included_config_state_;
  } else {
    ld_info("Fetched config data from %s for path \"%s\" but not interested; "
            "may be a stale request but should be unlikely",
            source->getName().c_str(),
            path.c_str());
    return;
  }

  onContents(affected_state, std::move(output), true);
}

void TextConfigUpdaterImpl::onContents(State* state,
                                       ConfigSource::Output output,
                                       bool call_update) {
  output.contents = maybeDecompress(state->path, std::move(output.contents));

  // NOTE: If decompression failed, `output.contents' is now empty and will
  // soon fail to parse.  This could be handled more gracefully with extra
  // code but probably not worth it.

  if (output.hash.empty()) {
    output.hash = hash_contents(output.contents);
  }

  ld_info("%s config from %s, hash = %s",
          state == &main_config_state_ ? "Main" : "Included log",
          state->source->getName().c_str(),
          output.hash.c_str());

  state->output = std::move(output);
  using namespace std::chrono;
  state->last_loaded_time =
      duration_cast<milliseconds>(system_clock::now().time_since_epoch());
  if (call_update) {
    update();
  }
}

void TextConfigUpdaterImpl::update(bool force_reload_logsconfig) {
  ld_check(main_config_state_.output.hasValue());

  // In most cases (success or error) we'll wake the main thread at the end of
  // the function if it's waiting for the first config.
  auto sem_post_guard =
      folly::makeGuard([this]() { initial_config_sem_.post(); });

  // Wipe `included_config_state_' in case the main config no longer refers to
  // it.  (If it does, the callback will repopulate it.)
  State previous_included_state;
  std::swap(previous_included_state, included_config_state_);

  bool waiting_for_included = false;
  auto parse_include_callback = [&](const char* path,
                                    std::string* contents_out) {
    // This is the callback that the ServerConfig parser invokes when it
    // detects that the main config refers to another location for the logs
    // config.

    std::tie(included_config_state_.source, included_config_state_.path) =
        parseMaybeRelativeLocation(
            path, main_config_state_.source, main_config_state_.path);

    if (!included_config_state_.source) {
      return E::FAILED;
    }

    // If we've already fetched the correct included log config (for example
    // with an async request), return that.
    if (previous_included_state.source == included_config_state_.source &&
        previous_included_state.path == included_config_state_.path &&
        previous_included_state.output.hasValue()) {
      std::swap(included_config_state_, previous_included_state);
      *contents_out = included_config_state_.output->contents;
      return E::OK;
    }

    // Otherwise, request the config from the source.
    ld_info("Requesting included log config \"%s\" (source: %s)",
            included_config_state_.path.c_str(),
            included_config_state_.source->getName().c_str());

    ConfigSource::Output output;
    Status rv = included_config_state_.source->getConfig(
        included_config_state_.path, &output);
    if (rv == E::OK) {
      onContents(&included_config_state_, std::move(output), false);
      // Copy the contents to return to Configuration parser.  Note that this
      // may not be the same as `output.contents' above; onContents() tries to
      // decompress when applicable.
      *contents_out = included_config_state_.output->contents;
    }
    waiting_for_included = rv == E::NOTREADY;
    return rv;
  };

  std::shared_ptr<Configuration> config = Configuration::fromJson(
      main_config_state_.output->contents,
      alternative_logs_config_ ? alternative_logs_config_->copy() : nullptr,
      parse_include_callback,
      config_parser_options_);
  Status config_parse_status = err;

  if (!config || !config->serverConfig()) {
    setRecentConfigValidity(false);
    ld_error("Config update aborted");
    return;
  }

  if (!config->logsConfig() && waiting_for_included) {
    // The main config refers to another location for the log config but the
    // contents have not been fetched yet.  Silently ignore.  There is a
    // request for the logs config inflight with some source; when that
    // comes back, we'll be able to form the full `ServerConfig' object.

    // Don't wake the main thread until then, though.
    sem_post_guard.dismiss();
    return;
  }

  ServerConfig::ConfigMetadata main_config_metadata, included_config_metadata;
  if (included_config_state_.source) {
    included_config_metadata.uri = included_config_state_.source->getName() +
        ':' + included_config_state_.path;
    ConfigSource::Output& included_output =
        included_config_state_.output.value();
    if (included_output.hash.empty()) {
      included_output.hash = hash_contents(included_output.contents);
    }
    included_config_metadata.hash = included_output.hash;
    included_config_metadata.modified_time = included_output.mtime;
    included_config_metadata.loaded_time =
        included_config_state_.last_loaded_time;
  }
  main_config_metadata.uri =
      main_config_state_.source->getName() + ':' + main_config_state_.path;
  main_config_metadata.hash = main_config_state_.output->hash;
  main_config_metadata.modified_time = main_config_state_.output->mtime;
  main_config_metadata.loaded_time = main_config_state_.last_loaded_time;

  config->serverConfig()->setMainConfigMetadata(main_config_metadata);
  config->serverConfig()->setIncludedConfigMetadata(included_config_metadata);

  std::shared_ptr<UpdateableServerConfig> server_config =
      target_server_config_.lock();
  if (!server_config) {
    ld_debug("Attempting to update ServerConfig after it has been destroyed");
    return;
  }

  int server_config_cmp = compareServerConfig(server_config->get(),  // old
                                              config->serverConfig() // new
  );

  int rv;

  // Reloading config is expensive, so we try as much as we can to avoid it.
  // The logic is the following:
  // if the server config changed, we have no choice but to reload both server
  // and logs config.
  // if the server config has not changed, but force_reload_logsconfig is true,
  // or the hash of the included logs config has changed, we reload the logs
  // config only.
  // otherwise, neither are reloaded.

  if (server_config_cmp <= 0) {
    // received config is not newer than running config. skip server
    // config update.

    // check if logsconfig needs to be reloaded
    if (!force_reload_logsconfig) {
      auto old_metadata = server_config->get()->getIncludedConfigMetadata();
      auto new_metadata = config->serverConfig()->getIncludedConfigMetadata();
      if (hashes_equal(old_metadata, new_metadata)) {
        // no need to reload logsconfig either. we can return early.
        return;
      } else {
        ld_debug("Server config has not changed but included logs config "
                 "was updated (%s != %s). Reloading logs config.",
                 old_metadata.hash.c_str(),
                 new_metadata.hash.c_str());
      }
    }
  } else {
    // Need to increase the stat _before_ publishing config because some
    // integration tests rely on it.
    STAT_INCR(stats_, updated_config);

    rv = server_config->update(config->serverConfig());

    if (rv != 0) {
      STAT_INCR(stats_, config_update_invalid);
      setRecentConfigValidity(false);
      ld_error("ServerConfig update aborted");
      return;
    }

    ld_info("Updated config (version: %u - hash: %s)",
            server_config->get()->getVersion().val(),
            main_config_metadata.hash.c_str());
  }

  // Should we mark this config update invalid?
  bool config_validity = false;
  // See if we need to care about the logsconfig changes or not. At this point
  // since the ServerConfig has been pushed to UpdateableServerConfig we can
  // rely that the updateable_settings are up-to-date and they (potentially)
  // include the updated settings
  std::shared_ptr<UpdateableLogsConfig> updateable_logsconfig =
      target_logs_config_.lock();
  if (!updateable_logsconfig) {
    ld_debug("Attempting log config update, but config doesn't exist anymore");
    return;
  }
  if (!updateable_settings_->server &&
      (updateable_settings_->on_demand_logs_config ||
       updateable_settings_->force_on_demand_logs_config)) {
    ld_info("Ignoring the 'logs' section in the config file because "
            "'on-demand-logs-config' is ENABLED (Client)");
    // config->logsConfig() contains the alternative_logs
    updateable_logsconfig->update(config->logsConfig());
    config_validity = true;
  } else if (!updateable_settings_->enable_logsconfig_manager) {
    // if we didn't get logsconfig as a return of the parsing
    if (!config->logsConfig()) {
      // While we could successfully parse and load the ServerConfig, the
      // LogsConfig section failed to be loaded from the file.
      ld_error("LogsConfig update (from file) was aborted because: %s",
               error_description(config_parse_status));
      err = E::INVALID_CONFIG;
      config_validity = false;
    } else {
      ld_info("Updating LogsConfig from file: version %lu",
              config->logsConfig()->getVersion());
      rv = updateable_logsconfig->update(config->logsConfig());
      config_validity = true;
      if (rv != 0) {
        // This happens if any of the hooks registered on the
        // UpdateableLogsConfig failed. We reject this config.
        config_validity = false;
        ld_error("LogsConfig update aborted");
      }
    }
  } else {
    ld_info("Ignoring changes in the 'logs' section in the config file because "
            "LogsConfigManager is ENABLED");
    // We mark this as a valid update because we don't care about the logs
    // config in that context.
    config_validity = true;
  }
  invalid_logs_config_ = !config_validity;
  setRecentConfigValidity(config_validity);
}

std::pair<ConfigSource*, std::string>
TextConfigUpdaterImpl::parseLocation(const std::string& location) {
  size_t pos = location.find(LOCATION_SCHEME_DELIMITER);
  std::string scheme, path;
  if (pos == std::string::npos) {
    scheme = "";
    path = location;
  } else {
    scheme = location.substr(0, pos);
    path = location.substr(pos + strlen(LOCATION_SCHEME_DELIMITER));
  }

  // Look for an appropriate source
  for (const auto& source : sources_) {
    for (const std::string& source_scheme : source->getSchemes()) {
      if (source_scheme == scheme) {
        // Success!  This source is registered for the location's scheme.
        return std::make_pair(source.get(), std::move(path));
      }
    }
  }

  ld_error("Unable to read config \"%s\": no config source is registered "
           "for scheme \"%s\"",
           location.c_str(),
           scheme.c_str());
  return std::make_pair(nullptr, std::string());
}

std::pair<ConfigSource*, std::string>
TextConfigUpdaterImpl::parseMaybeRelativeLocation(const std::string& location,
                                                  ConfigSource* ref_source,
                                                  const std::string& ref_path) {
  if (location.find(LOCATION_SCHEME_DELIMITER) != std::string::npos) {
    // Scheme explicitly specified in location, parse as absolute
    return parseLocation(location);
  }

  std::string full_path;
  if (!location.empty() && location[0] == PATH_DELIMITER) {
    // Absolute path.
    full_path = location;
  } else {
    // Relative path.  Extract path prefix from `ref_path' and attach
    // `location' to it.
    boost::filesystem::path path_prefix(ref_path);
    path_prefix.remove_filename();
    full_path = (path_prefix / location).string();
  }
  return std::make_pair(ref_source, std::move(full_path));
}

std::string TextConfigUpdaterImpl::maybeDecompress(const std::string& path,
                                                   std::string raw_contents) {
  using folly::IOBuf;
  boost::filesystem::path ext = boost::filesystem::path(path).extension();
  if (ext == ".gz") {
    std::unique_ptr<IOBuf> input =
        IOBuf::wrapBuffer(raw_contents.data(), raw_contents.size());
    auto codec = folly::io::getCodec(folly::io::CodecType::GZIP);
    std::unique_ptr<IOBuf> uncompressed;
    try {
      uncompressed = codec->uncompress(input.get());
    } catch (const std::runtime_error& ex) {
      ld_error(
          "gzip decompression of \"%s\" failed: %s", path.c_str(), ex.what());
      return "";
    }
    return std::string(uncompressed->moveToFbString().toStdString());
  } else {
    return raw_contents;
  }
}

static std::string hash_contents(const std::string& str) {
  const uint64_t SEED = 0xbfc655bfa4fd6c49L; // random
  uint64_t h = folly::hash::SpookyHashV2::Hash64(str.data(), str.size(), SEED);
  std::string result;
  for (int i = 0; i < 10; ++i) {
    int d = h % 36;
    h /= 36;
    result += d < 10 ? ('0' + d) : ('a' + d - 10);
  }
  std::reverse(result.begin(), result.end());
  return result;
}

void TextConfigUpdaterImpl::setRecentConfigValidity(bool state) {
  if (isRecentConfigValid_ == state) { // config state did not change
    return;
  }

  isRecentConfigValid_ = state;
  if (isRecentConfigValid_) {
    STAT_INCR(stats_, last_config_valid);
  } else {
    STAT_DECR(stats_, last_config_valid);
  }
}

int TextConfigUpdaterImpl::compareServerConfig(
    const std::shared_ptr<ServerConfig>& old_config,
    const std::shared_ptr<ServerConfig>& new_config) {
  if (old_config == nullptr) {
    ld_debug("ServerConfig is not set");
    return 1;
  }

  config_version_t new_version = new_config->getVersion();
  config_version_t old_version = old_config->getVersion();
  RATELIMIT_INFO(std::chrono::seconds(1),
                 1,
                 "Comparing new config (version %u) with existing config "
                 "(version %u)",
                 new_version.val(),
                 old_version.val());
  if (new_version < old_version) {
    ld_error("Not updating config with an older version (%u < %u)",
             new_version.val(),
             old_version.val());
    setRecentConfigValidity(false);
    STAT_INCR(stats_, config_update_old_version);
    return -1;
  } else if (new_version == old_version) {
    auto old_metadata = old_config->getMainConfigMetadata();
    auto new_metadata = new_config->getMainConfigMetadata();

    if (hashes_equal(old_metadata, new_metadata)) {
      ld_info("Received same config as already running "
              "(version: %u - hash: %s).",
              new_version.val(),
              new_metadata.hash.c_str());
      STAT_INCR(stats_, config_update_same_version);
      return 0;
    } else {
      // the hashes don't match. log a warning and proceed with the
      // update assuming it is newer.
      ld_warning("Received config with same version (%u) but mismatched "
                 "hash (%s != %s)",
                 new_version.val(),
                 old_metadata.hash.c_str(),
                 new_metadata.hash.c_str());
      STAT_INCR(stats_, config_update_hash_mismatch);
    }
  }

  return 1;
}

int TextConfigUpdater::waitForInitialLoad(std::chrono::milliseconds timeout) {
  int rv = initial_config_sem_.timedwait(timeout);
  if (rv != 0) {
    ld_error("Timed out waiting for config after %ld ms", timeout.count());
    err = E::TIMEDOUT;
    return -1;
  }
  if (!impl_->hasValidConfig()) {
    err = E::INVALID_CONFIG;
    return -1;
  }
  return 0;
}

static bool hashes_equal(const ServerConfig::ConfigMetadata& a,
                         const ServerConfig::ConfigMetadata& b) {
  if (a.hash.empty() || b.hash.empty()) {
    return a.hash == b.hash;
  }
  // use std::mismatch here to check hash values as the hash may
  // be truncated (when sent via CONFIG_UPDATED_Message).
  // but we will consider them equal if one is prefix of the other
  auto result =
      std::mismatch(a.hash.begin(), a.hash.end(), b.hash.begin(), b.hash.end());
  return (result.first == a.hash.end() || result.second == b.hash.end());
};

}} // namespace facebook::logdevice

max_stars_repo_name: SimonKinds/LogDevice


text:
/*
 * Copyright (c) 2013-2019 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef AFFINE_HPP
#define AFFINE_HPP

// Affine Arithmetic

#include <iostream>
#include <stdexcept>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <vector>
#include <algorithm>
#include <cmath>

#include <map> //@hylagi

#include <kv/interval.hpp>
#include <kv/rdouble.hpp>

#include <kv/convert.hpp>

/*
 * define simplicity of affine arithmetic
 *
 *   value of AFFINE_SIMPLE                   | 0 | 1 | 2
 *  ------------------------------------------+---+---+---
 *   add dummy epsilin on linear operation    | o | x | x
 *   add dummy epsilin on nonlinear operation | o | o | x
 *
 *  default: 1
 */

#ifndef AFFINE_SIMPLE
#define AFFINE_SIMPLE 1
#endif

/*
 * select the method for multiplication
 *
 *  0: Stolfi's simple method (default, O(n))
 *  1: better multiplication (give smaller extra epsilon but slow, O(n^2))
 *  2: best multiplication (give smallest extra epsilon but slow, O(n^2))
 */

#ifndef AFFINE_MULT
#define AFFINE_MULT 0
#endif

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T> class affine;

template <class C, class T> struct acceptable_s<C, affine<T> > {
  static const bool value = boost::is_same<C, interval<T> >::value || boost::is_convertible<C, std::string>::value;
};

template <class C, class T> struct acceptable_n<C, affine<T> > {
  static const bool value = convertible<C, T>::value && (!acceptable_s<C, affine<T> >::value);
};

template <class C, class T> struct convertible<C, affine<T> > {
  static const bool value = acceptable_n<C, affine<T> >::value || acceptable_s<C, affine<T> >::value || boost::is_same<C, affine<T> >::value;
};

#if 0
template <class C, class T> struct acceptable_n<C, affine<T> > {
  static const bool value = convertible<C, T>::value && (!boost::is_same<C, interval<T> >::value) && (!boost::is_convertible<C, std::string>::value);
};

template <class C, class T> struct convertible<C, affine<T> > {
  static const bool value = convertible<C, T>::value || boost::is_same<C, interval<T> >::value || boost::is_convertible<C, std::string>::value || boost::is_same<C, affine<T> >::value;
};
#endif

template <class T> class affine {
  public:
  ub::vector<T> a;
  #if AFFINE_SIMPLE >= 1
  T er;
  #endif

  typedef T base_type;

  static int& maxnum() {
    static int m = 0;
    #pragma omp threadprivate(m)
    return m;
  }

  friend inline T rad(const affine& x) {
    int i, xs;
    T r(0.);

    xs = x.a.size();

    rop<T>::begin();
    for (i=1; i<xs; i++) {
      using std::abs;
      r = rop<T>::add_up(r, abs(x.a(i)));
    }
    #if AFFINE_SIMPLE >= 1
    r = rop<T>::add_up(r, x.er);
    #endif
    rop<T>::end();

    return r;
  }

  friend inline interval<T> to_interval(const affine& x) {
    T t1, t2, t3;

    t1 = rad(x);
    rop<T>::begin();
    t2 = rop<T>::sub_down(x.a(0), t1);
    t3 = rop<T>::add_up(x.a(0), t1);
    rop<T>::end();
    return interval<T>(t2, t3);
  }

  interval<T> as_interval() const{ // @hylagi modified
    T t1, t2, t3;
    t1 = rad(*this);
    rop<T>::begin();
    t2 = rop<T>::sub_down(a(0), t1);
    t3 = rop<T>::add_up(a(0), t1);
    rop<T>::end();
    return interval<T>(t2, t3);
  }

  affine() {
  }

  template <class C> explicit affine(const C& x, typename boost::enable_if_c< acceptable_n<C, affine>::value >::type* =0) {
    a.resize(1);
    a(0) = x;
    #if AFFINE_SIMPLE >= 1
    er = 0.;
    #endif
  }

  template <class C> explicit affine(const C& x, typename boost::enable_if_c< acceptable_s<C, affine>::value >::type* =0) {
    int i;
    interval<T> I(x);
    maxnum()++;
    a.resize(maxnum()+1);

    rop<T>::begin();
    a(0) = rop<T>::mul_up(rop<T>::add_up(I.upper(), I.lower()), T(0.5));
    a(maxnum()) = rop<T>::sub_up(a(0), I.lower());
    rop<T>::end();

    for (i=1; i<maxnum(); i++) a(i) = 0.;

    #if AFFINE_SIMPLE >= 1
    er = 0.;
    #endif
  }

  template <class C> typename boost::enable_if_c< acceptable_n<C, affine>::value, affine& >::type operator=(const C& x) {
    a.resize(1);
    a(0) = x;
    #if AFFINE_SIMPLE >= 1
    er = 0.;
    #endif

    return *this;
  }

  template <class C> typename boost::enable_if_c< acceptable_s<C, affine>::value, affine& >::type operator=(const C& x) {
    int i;
    interval<T> I(x);
    maxnum()++;
    a.resize(maxnum()+1);

    rop<T>::begin();
    a(0) = rop<T>::mul_up(rop<T>::add_up(I.upper(), I.lower()), T(0.5));
    a(maxnum()) = rop<T>::sub_up(a(0), I.lower());
    rop<T>::end();

    for (i=1; i<maxnum(); i++) a(i) = 0.;

    #if AFFINE_SIMPLE >= 1
    er = 0.;
    #endif

    return *this;
  }

  T get_coef (int i) const {
    if (i >= a.size()) return T(0.);
    else return a(i);
  }

  T get_mid() const {
    return a(0);
  }

  T get_err() const {
    #if AFFINE_SIMPLE >= 1
    return er;
    #else
    return T(0.);
    #endif
  }

  friend affine operator+(const affine& x, const affine& y) {
    affine r;
    int xs, ys, i;
    T err(0.);

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #endif

    xs = x.a.size();
    ys = y.a.size();
    if (xs > ys) {
      #if AFFINE_SIMPLE >= 1
      r.a.resize(xs);
      #endif
      rop<T>::begin();
      for (i=0; i<ys; i++) {
        r.a(i) = rop<T>::add_down(x.a(i), y.a(i));
      }
      for (i=0; i<ys; i++) {
        err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::add_up(x.a(i), y.a(i)), r.a(i)));
      }
      rop<T>::end();

      for (i=ys; i<xs; i++) {
        r.a(i) = x.a(i);
      }
      #if AFFINE_SIMPLE == 0
      for (i=xs; i<maxnum(); i++) {
        r.a(i) = 0.;
      }
      #endif
    } else {
      #if AFFINE_SIMPLE >= 1
      r.a.resize(ys);
      #endif
      rop<T>::begin();
      for (i=0; i<xs; i++) {
        r.a(i) = rop<T>::add_down(x.a(i), y.a(i));
      }
      for (i=0; i<xs; i++) {
        err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::add_up(x.a(i), y.a(i)), r.a(i)));
      }
      rop<T>::end();

      for (i=xs; i<ys; i++) {
        r.a(i) = y.a(i);
      }
      #if AFFINE_SIMPLE == 0
      for (i=ys; i<maxnum(); i++) {
        r.a(i) = 0.;
      }
      #endif
    }
    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    r.er = rop<T>::add_up(rop<T>::add_up(x.er, y.er), err);
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif
    return r;
  }

  // same as operator+, but do not add extra epsilon.
  // This function can be used for adding affine variables
  // which have no common epsilons.

  friend affine append(const affine& x, const affine& y) {
    affine r;
    int xs, ys, i;

    xs = x.a.size();
    ys = y.a.size();

    if (xs > ys) {
      r.a.resize(xs);
      for (i=0; i<ys; i++) {
        r.a(i) = x.a(i) + y.a(i);
      }
      for (i=ys; i<xs; i++) {
        r.a(i) = x.a(i);
      }
    } else {
      r.a.resize(ys);
      for (i=0; i<xs; i++) {
        r.a(i) = x.a(i) + y.a(i);
      }
      for (i=xs; i<ys; i++) {
        r.a(i) = y.a(i);
      }
    }
    #if AFFINE_SIMPLE >= 1
    r.er = x.er + y.er;
    #endif
    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type operator+(const affine& x, const C& y) {
    affine r;
    int xs, i;
    T err;

    xs = x.a.size();

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #else
    r.a.resize(xs);
    #endif

    rop<T>::begin();
    r.a(0) = rop<T>::add_down(x.a(0), (T)y);
    err = rop<T>::sub_up(rop<T>::add_up(x.a(0), (T)y), r.a(0));
    rop<T>::end();

    for (i=1; i<xs; i++) {
      r.a(i) = x.a(i);
    }
    #if AFFINE_SIMPLE == 0
    for (i=xs; i<maxnum(); i++) {
      r.a(i) = 0.;
    }
    #endif
    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    r.er = rop<T>::add_up(x.er, err);
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif
    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type operator+(const C& x, const affine& y) {
    affine r;
    int ys, i;
    T err;

    ys = y.a.size();

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #else
    r.a.resize(ys);
    #endif

    rop<T>::begin();
    r.a(0) = rop<T>::add_down((T)x, y.a(0));
    err = rop<T>::sub_up(rop<T>::add_up((T)x, y.a(0)), r.a(0));
    rop<T>::end();

    for (i=1; i<ys; i++) {
      r.a(i) = y.a(i);
    }
    #if AFFINE_SIMPLE == 0
    for (i=ys; i<maxnum(); i++) {
      r.a(i) = 0.;
    }
    #endif
    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    r.er = rop<T>::add_up(y.er, err);
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif
    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type operator+(const affine& x, const C& y) {
    return x + affine(y);
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type operator+(const C& x, const affine& y) {
    return affine(x) + y;
  }

  friend affine& operator+=(affine& x, const affine& y) {
    x = x + y;
    return x;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine& >::type operator+=(affine& x, const C& y) {
    x = x + y;
    return x;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine& >::type operator+=(affine& x, const C& y) {
    x = x + y;
    return x;
  }

  friend affine operator-(const affine& x, const affine& y) {
    affine r;
    int xs, ys, i;
    T err(0.);

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #endif

    xs = x.a.size();
    ys = y.a.size();
    if (xs > ys) {
      #if AFFINE_SIMPLE >= 1
      r.a.resize(xs);
      #endif
      rop<T>::begin();
      for (i=0; i<ys; i++) {
        r.a(i) = rop<T>::sub_down(x.a(i), y.a(i));
      }
      for (i=0; i<ys; i++) {
        err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::sub_up(x.a(i), y.a(i)), r.a(i)));
      }
      rop<T>::end();

      for (i=ys; i<xs; i++) {
        r.a(i) = x.a(i);
      }
      #if AFFINE_SIMPLE == 0
      for (i=xs; i<maxnum(); i++) {
        r.a(i) = 0.;
      }
      #endif
    } else {
      #if AFFINE_SIMPLE >= 1
      r.a.resize(ys);
      #endif
      rop<T>::begin();
      for (i=0; i<xs; i++) {
        r.a(i) = rop<T>::sub_down(x.a(i), y.a(i));
      }
      for (i=0; i<xs; i++) {
        err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::sub_up(x.a(i), y.a(i)), r.a(i)));
      }
      rop<T>::end();

      for (i=xs; i<ys; i++) {
        r.a(i) = - y.a(i);
      }
      #if AFFINE_SIMPLE == 0
      for (i=ys; i<maxnum(); i++) {
        r.a(i) = 0.;
      }
      #endif
    }
    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    r.er = rop<T>::add_up(rop<T>::add_up(x.er, y.er), err);
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif
    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type operator-(const affine& x, const C& y) {
    affine r;
    int xs, i;
    T err;

    xs = x.a.size();

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #else
    r.a.resize(xs);
    #endif

    rop<T>::begin();
    r.a(0) = rop<T>::sub_down(x.a(0), (T)y);
    err = rop<T>::sub_up(rop<T>::sub_up(x.a(0), (T)y), r.a(0));
    rop<T>::end();

    for (i=1; i<xs; i++) {
      r.a(i) = x.a(i);
    }
    #if AFFINE_SIMPLE == 0
    for (i=xs; i<maxnum(); i++) {
      r.a(i) = 0.;
    }
    #endif
    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    r.er = rop<T>::add_up(x.er, err);
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif
    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type operator-(const C& x, const affine& y) {
    affine r;
    int ys, i;
    T err;

    ys = y.a.size();

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #else
    r.a.resize(ys);
    #endif

    rop<T>::begin();
    r.a(0) = rop<T>::sub_down((T)x, y.a(0));
    err = rop<T>::sub_up(rop<T>::sub_up((T)x, y.a(0)), r.a(0));
    rop<T>::end();

    for (i=1; i<ys; i++) {
      r.a(i) = - y.a(i);
    }
    #if AFFINE_SIMPLE == 0
    for (i=ys; i<maxnum(); i++) {
      r.a(i) = 0.;
    }
    #endif
    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    r.er = rop<T>::add_up(y.er, err);
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif
    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type operator-(const affine& x, const C& y) {
    return x - affine(y);
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type operator-(const C& x, const affine& y) {
    return affine(x) - y;
  }

  friend affine& operator-=(affine& x, const affine& y) {
    x = x - y;
    return x;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine& >::type operator-=(affine& x, const C& y) {
    x = x - y;
    return x;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine& >::type operator-=(affine& x, const C& y) {
    x = x - y;
    return x;
  }

  friend affine operator-(const affine& x) {
    affine r;

    r.a = - x.a;
    #if AFFINE_SIMPLE >= 1
    r.er = x.er;
    #endif
    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type operator*(const affine& x, const C& y) {
    affine r;
    int xs, i;
    T err(0.);

    xs = x.a.size();

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #else
    r.a.resize(xs);
    #endif

    rop<T>::begin();
    for (i=0; i<xs; i++) {
      r.a(i) = rop<T>::mul_down(x.a(i), (T)y);
    }
    for (i=0; i<xs; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(x.a(i), (T)y), r.a(i)));
    }
    rop<T>::end();

    #if AFFINE_SIMPLE == 0
    for (i=xs; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    using std::abs;
    r.er = rop<T>::add_up(rop<T>::mul_up(x.er, T(abs(y))), err);
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif

    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type operator*(const C& x, const affine& y) {
    affine r;
    int ys, i;
    T err(0.);

    ys = y.a.size();

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #else
    r.a.resize(ys);
    #endif

    rop<T>::begin();
    for (i=0; i<ys; i++) {
      r.a(i) = rop<T>::mul_down((T)x, y.a(i));
    }
    for (i=0; i<ys; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up((T)x, y.a(i)), r.a(i)));
    }
    rop<T>::end();

    #if AFFINE_SIMPLE == 0
    for (i=ys; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    using std::abs;
    r.er = rop<T>::add_up(rop<T>::mul_up(y.er, T(abs(x))), err);
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif

    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type operator*(const affine& x, const C& y) {
    return x * affine(y);
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type operator*(const C& x, const affine& y) {
    return affine(x) * y;
  }

  #if AFFINE_MULT >= 1

  static interval<T> bestmult_error(const affine& x, const affine& y)
  {
    int n, i, j, s;
    bool f1, f2;
    ub::vector<T> vx, vy;
    kv::interval<T> tmp, C, D, E, X, Y;

    n = std::max(x.a.size(), y.a.size()) - 1;

    #if AFFINE_SIMPLE >= 1
    vx.resize(n+3);
    vy.resize(n+3);
    #else
    vx.resize(n+1);
    vy.resize(n+1);
    #endif

    for (i=0; i<x.a.size(); i++) {
      vx(i) = x.a(i);
    }
    for (i=x.a.size(); i<=n; i++) {
      vx(i) = 0.;
    }
    for (i=0; i<y.a.size(); i++) {
      vy(i) = y.a(i);
    }
    for (i=y.a.size(); i<=n; i++) {
      vy(i) = 0.;
    }

    #if AFFINE_SIMPLE >= 1
    vx(n+1) = x.er;
    vy(n+1) = 0.;
    vx(n+2) = 0.;
    vy(n+2) = y.er;
    n += 2;
    #endif

    E = 0.;

    #if AFFINE_MULT == 1
    for (i=1; i<=n; i++) {
      X = vx(i);
      Y = vy(i);
      E += X * Y * interval<T>(0., 1.);
      for (j=i+1; j<=n; j++) {
        tmp = X * vy(j) + Y * vx(j);
        E += tmp * interval<T>(-1., 1.);
      }
    }
    #else // AFFINE_MULT == 1

    for (i=1; i<=n; i++) {
      X = vx(i);
      Y = vy(i);
      if (X == 0. && Y == 0.) continue;
      C = D = 0.;
      for (j=1; j<=n; j++) {
        if (j == i) continue;
        if (vy(j) == 0. && vx(j) == 0.) continue;
        tmp = X * vy(j) - Y * vx(j);
        if (tmp.lower() >= 0.) {
          C += vx(j);
          D += vy(j);
        } else if (tmp.upper() <= 0.) {
          C -= vx(j);
          D -= vy(j);
        } else {
          if ((X.upper() > 0. && vx(j) < 0.)
          || (X.lower() < 0. && vx(j) > 0.)) {
            X -= vx(j);
            Y -= vy(j);
          } else {
            X += vx(j);
            Y += vy(j);
          }
        }
      }
      E = interval<T>::hull(E, X * Y + (X * D + Y * C) + C * D);
      E = interval<T>::hull(E, X * Y - (X * D + Y * C) + C * D);
      if (!zero_in(X * Y) ){
        tmp = -0.5 * (X * D + Y * C) / (X * Y);
        if (overlap(tmp, interval<T>(-1., 1.))) {
          E = interval<T>::hull(E, X * Y * tmp * tmp + (X * D + Y * C) * tmp + C * D);
        }
      }
    }

    #endif // AFFINE_MULT == 1

    return E;
  }

  #endif // AFFINE_MULT >= 1

  friend affine operator*(const affine& x, const affine& y) {
    affine r;
    int i, j, xs, ys;
    T err;
    T tmp_u, tmp_l;

    // if (&x == &y) return square(x);

    xs = x.a.size();
    ys = y.a.size();

    #if AFFINE_SIMPLE != 2
    maxnum()++;
    r.a.resize(maxnum()+1);
    #else
    r.a.resize(std::max(xs, ys));
    #endif

    rop<T>::begin();
    r.a(0) = rop<T>::mul_down(x.a(0), y.a(0));
    err = rop<T>::sub_up(rop<T>::mul_up(x.a(0), y.a(0)), r.a(0));
    rop<T>::end();

    if (xs > ys) {
      rop<T>::begin();
      for (i=1; i<ys; i++) {
        r.a(i) = rop<T>::add_down(rop<T>::mul_down(y.a(0), x.a(i)), rop<T>::mul_down(x.a(0), y.a(i)));
      }
      for (i=ys; i<xs; i++) {
        r.a(i) = rop<T>::mul_down(y.a(0), x.a(i));
      }
      for (i=1; i<ys; i++) {
        err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::add_up(rop<T>::mul_up(y.a(0), x.a(i)), rop<T>::mul_up(x.a(0), y.a(i))), r.a(i)));
      }
      for (i=ys; i<xs; i++) {
        err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(y.a(0), x.a(i)), r.a(i)));
      }
      rop<T>::end();
      #if AFFINE_SIMPLE != 2
      for (i=xs; i<maxnum(); i++) {
        r.a(i) = 0.;
      }
      #endif
    } else {
      rop<T>::begin();
      for (i=1; i<xs; i++) {
        r.a(i) = rop<T>::add_down(rop<T>::mul_down(y.a(0), x.a(i)), rop<T>::mul_down(x.a(0), y.a(i)));
      }
      for (i=xs; i<ys; i++) {
        r.a(i) = rop<T>::mul_down(x.a(0), y.a(i));
      }
      for (i=1; i<xs; i++) {
        err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::add_up(rop<T>::mul_up(y.a(0), x.a(i)), rop<T>::mul_up(x.a(0), y.a(i))), r.a(i)));
      }
      for (i=xs; i<ys; i++) {
        err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(x.a(0), y.a(i)), r.a(i)));
      }
      rop<T>::end();
      #if AFFINE_SIMPLE != 2
      for (i=ys; i<maxnum(); i++) {
        r.a(i) = 0.;
      }
      #endif
    }

    #if AFFINE_MULT >= 1

    interval<T> E;
    E = r.a(0) + bestmult_error(x, y);
    rop<T>::begin();
    r.a(0) = rop<T>::mul_up(rop<T>::add_up(E.upper(), E.lower()), 0.5);
    err = rop<T>::add_up(err, rop<T>::sub_up(r.a(0), E.lower()));
    rop<T>::end();

    #else // AFFINE_MULT >= 1

    tmp_l = rad(x);
    tmp_u = rad(y);
    rop<T>::begin();
    err = rop<T>::add_up(err, rop<T>::mul_up(tmp_l, tmp_u));
    rop<T>::end();

    #endif // AFFINE_MULT >= 1

    #if AFFINE_SIMPLE >= 1
    rop<T>::begin();
    using std::abs;
    err = rop<T>::add_up(err, rop<T>::add_up(rop<T>::mul_up(abs(y.a(0)), x.er), rop<T>::mul_up(abs(x.a(0)), y.er)));
    rop<T>::end();
    #endif

    #if AFFINE_SIMPLE == 2
    r.er = err;
    #else
    r.a(maxnum()) = err;
    # if AFFINE_SIMPLE == 1
    r.er = 0.;
    # endif
    #endif

    return r;
  }

  friend affine& operator*=(affine& x, const affine& y) {
    x = x * y;
    return x;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine& >::type operator*=(affine& x, const C& y) {
    x = x * y;
    return x;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine& >::type operator*=(affine& x, const C& y) {
    x = x * y;
    return x;
  }

  friend affine inv(const affine& x) {
    affine r;
    T err;
    interval<T> I, tmp, range;
    T a, b, l, u;
    int i, xs;

    xs = x.a.size();

    #if AFFINE_SIMPLE == 2
    r.a.resize(xs);
    #else
    maxnum()++;
    r.a.resize(maxnum()+1);
    #endif

    I = to_interval(x);
    l = I.lower();
    u = I.upper();
    if (l < 0. && u > 0.) {
      throw std::domain_error("affine: division by 0");
    }

    a = -1. /(l * u);
    tmp = a; // tmp is used to force interval calculation
    if (u > 0.) {
      range = 2. * sqrt(-tmp);
    } else {
      range = -2. * sqrt(-tmp);
    }
    tmp = l;
    range = interval<T>::hull(range, 1./tmp - a * tmp);
    tmp = u;
    range = interval<T>::hull(range, 1./tmp - a * tmp);

    rop<T>::begin();
    b = rop<T>::mul_up(rop<T>::add_up(range.upper(), range.lower()), T(0.5));
    err = rop<T>::sub_up(b, range.lower());
    for (i=1; i<xs; i++) {
      r.a(i) = rop<T>::mul_down(x.a(i), a);
    }
    for (i=1; i<xs; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(x.a(i), a), r.a(i)));
    }
    #if AFFINE_SIMPLE != 2
    for (i=xs; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    l = rop<T>::add_down(rop<T>::mul_down(x.a(0), a), b);
    u = rop<T>::add_up(rop<T>::mul_up(x.a(0), a), b);
    r.a(0) = rop<T>::mul_up(rop<T>::add_up(l, u), T(0.5));
    err = rop<T>::add_up(err, rop<T>::sub_up(r.a(0), l));
    #if AFFINE_SIMPLE >= 1
    // err += abs(a) * x.er;
    using std::abs;
    err = rop<T>::add_up(err, rop<T>::mul_up(abs(a), x.er));
    #endif
    rop<T>::end();

    #if AFFINE_SIMPLE == 2
    r.er = err;
    #else
    r.a(maxnum()) = err;
    # if AFFINE_SIMPLE == 1
    r.er = 0.;
    # endif
    #endif

    return r;
  }

  friend affine operator/(const affine& x, const affine& y) {
    return x * inv(y);
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type operator/(const C& x, const affine& y) {
    return x * inv(y);
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type operator/(const affine& x, const C& y) {
    affine r;
    int xs, i;
    T err(0.);

    xs = x.a.size();

    #if AFFINE_SIMPLE == 0
    maxnum()++;
    r.a.resize(maxnum()+1);
    #else
    r.a.resize(xs);
    #endif

    rop<T>::begin();
    for (i=0; i<xs; i++) {
      r.a(i) = rop<T>::div_down(x.a(i), (T)y);
    }
    for (i=0; i<xs; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::div_up(x.a(i), (T)y), r.a(i)));
    }
    rop<T>::end();

    #if AFFINE_SIMPLE == 0
    for (i=xs; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    #if AFFINE_SIMPLE >= 1
    // r.er = x.er / abs(y) + err;
    rop<T>::begin();
    using std::abs;
    r.er = rop<T>::add_up(err, rop<T>::div_up(x.er, T(abs(y))));
    rop<T>::end();
    #else
    r.a(maxnum()) = err;
    #endif

    return r;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type operator/(const affine& x, const C& y) {
    return x / affine(y);
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type operator/(const C& x, const affine& y) {
    return affine(x) / y;
  }

  friend affine& operator/=(affine& x, const affine& y) {
    x = x / y;
    return x;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine& >::type operator/=(affine& x, const C& y) {
    x = x / y;
    return x;
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine& >::type operator/=(affine& x, const C& y) {
    x = x / y;
    return x;
  }

  friend affine sqrt(const affine& x) {
    affine r;
    T err;
    interval<T> I, tmp, range;
    T a, b, l, u;
    int i, xs;

    xs = x.a.size();

    #if AFFINE_SIMPLE == 2
    r.a.resize(xs);
    #else
    maxnum()++;
    r.a.resize(maxnum()+1);
    #endif

    I = to_interval(x);
    l = I.lower();
    u = I.upper();
    if (l < 0.) {
      throw std::domain_error("affine: sqrt of negative value");

    }

    using std::sqrt;
    a = 1. /(sqrt(l) + sqrt(u));
    tmp = a;
    range = 1. / (4. * tmp);
    tmp = l;
    range = interval<T>::hull(range, sqrt(tmp) - a * tmp);
    tmp = u;
    range = interval<T>::hull(range, sqrt(tmp) - a * tmp);

    rop<T>::begin();
    b = rop<T>::mul_up(rop<T>::add_up(range.upper(), range.lower()), T(0.5));
    err = rop<T>::sub_up(b, range.lower());
    for (i=1; i<xs; i++) {
      r.a(i) = rop<T>::mul_down(x.a(i), a);
    }
    for (i=1; i<xs; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(x.a(i), a), r.a(i)));
    }
    #if AFFINE_SIMPLE != 2
    for (i=xs; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    l = rop<T>::add_down(rop<T>::mul_down(x.a(0), a), b);
    u = rop<T>::add_up(rop<T>::mul_up(x.a(0), a), b);
    r.a(0) = rop<T>::mul_up(rop<T>::add_up(l, u), T(0.5));
    err = rop<T>::add_up(err, rop<T>::sub_up(r.a(0), l));
    #if AFFINE_SIMPLE >= 1
    // err += abs(a) * x.er;
    using std::abs;
    err = rop<T>::add_up(err, rop<T>::mul_up(abs(a), x.er));
    #endif
    rop<T>::end();

    #if AFFINE_SIMPLE == 2
    r.er = err;
    #else
    r.a(maxnum()) = err;
    # if AFFINE_SIMPLE == 1
    r.er = 0.;
    # endif
    #endif

    return r;
  }

  friend affine square(const affine& x) {
    affine r;
    T err;
    interval<T> I, tmp, range;
    T a, b, l, u;
    int i, xs;

    xs = x.a.size();

    #if AFFINE_SIMPLE == 2
    r.a.resize(xs);
    #else
    maxnum()++;
    r.a.resize(maxnum()+1);
    #endif

    I = to_interval(x);
    l = I.lower();
    u = I.upper();

    a = l + u;
    tmp = a;
    range = - tmp * tmp * 0.25;
    tmp = l;
    // range = hull(range, tmp * tmp - a * tmp);
    range = interval<T>::hull(range, tmp * (tmp - a));
    tmp = u;
    range = interval<T>::hull(range, tmp * (tmp - a));

    rop<T>::begin();
    b = rop<T>::mul_up(rop<T>::add_up(range.upper(), range.lower()), T(0.5));
    err = rop<T>::sub_up(b, range.lower());
    for (i=1; i<xs; i++) {
      r.a(i) = rop<T>::mul_down(x.a(i), a);
    }
    for (i=1; i<xs; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(x.a(i), a), r.a(i)));
    }
    #if AFFINE_SIMPLE != 2
    for (i=xs; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    l = rop<T>::add_down(rop<T>::mul_down(x.a(0), a), b);
    u = rop<T>::add_up(rop<T>::mul_up(x.a(0), a), b);
    r.a(0) = rop<T>::mul_up(rop<T>::add_up(l, u), T(0.5));
    err = rop<T>::add_up(err, rop<T>::sub_up(r.a(0), l));
    #if AFFINE_SIMPLE >= 1
    // err += abs(a) * x.er;
    using std::abs;
    err = rop<T>::add_up(err, rop<T>::mul_up(abs(a), x.er));
    #endif
    rop<T>::end();

    #if AFFINE_SIMPLE == 2
    r.er = err;
    #else
    r.a(maxnum()) = err;
    # if AFFINE_SIMPLE == 1
    r.er = 0.;
    # endif
    #endif

    return r;
  }

  friend affine exp(const affine& x) {
    affine r;
    T err;
    interval<T> I, tmp, range;
    T a, b, l, u;
    int i, xs;

    xs = x.a.size();

    #if AFFINE_SIMPLE == 2
    r.a.resize(xs);
    #else
    maxnum()++;
    r.a.resize(maxnum()+1);
    #endif

    I = to_interval(x);
    l = I.lower();
    u = I.upper();

    using std::exp;
    if (u == l) a = exp(u);
    else a = (exp(u) - exp(l)) / (u - l);
    tmp = a;
    range = tmp * (1. - log(tmp));
    tmp = l;
    range = interval<T>::hull(range, exp(tmp) - a * tmp);
    tmp = u;
    range = interval<T>::hull(range, exp(tmp) - a * tmp);

    rop<T>::begin();
    b = rop<T>::mul_up(rop<T>::add_up(range.upper(), range.lower()), T(0.5));
    err = rop<T>::sub_up(b, range.lower());
    for (i=1; i<xs; i++) {
      r.a(i) = rop<T>::mul_down(x.a(i), a);
    }
    for (i=1; i<xs; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(x.a(i), a), r.a(i)));
    }
    #if AFFINE_SIMPLE != 2
    for (i=xs; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    l = rop<T>::add_down(rop<T>::mul_down(x.a(0), a), b);
    u = rop<T>::add_up(rop<T>::mul_up(x.a(0), a), b);
    r.a(0) = rop<T>::mul_up(rop<T>::add_up(l, u), T(0.5));
    err = rop<T>::add_up(err, rop<T>::sub_up(r.a(0), l));
    #if AFFINE_SIMPLE >= 1
    // err += abs(a) * x.er;
    using std::abs;
    err = rop<T>::add_up(err, rop<T>::mul_up(abs(a), x.er));
    #endif
    rop<T>::end();

    #if AFFINE_SIMPLE == 2
    r.er = err;
    #else
    r.a(maxnum()) = err;
    # if AFFINE_SIMPLE == 1
    r.er = 0.;
    # endif
    #endif

    return r;
  }

  friend affine log(const affine& x) {
    affine r;
    T err;
    interval<T> I, tmp, range;
    T a, b, l, u;
    int i, xs;

    xs = x.a.size();

    #if AFFINE_SIMPLE == 2
    r.a.resize(xs);
    #else
    maxnum()++;
    r.a.resize(maxnum()+1);
    #endif

    I = to_interval(x);
    l = I.lower();
    u = I.upper();

    if (l <= 0.) {
      throw std::domain_error("affine: log of nagative value");
    }

    using std::log;
    if (u == l) a = 1. / u;
    else a = (log(u) - log(l)) / (u - l);
    tmp = a;
    range = log(1. / tmp) - 1.;
    tmp = l;
    range = interval<T>::hull(range, log(tmp) - a * tmp);
    tmp = u;
    range = interval<T>::hull(range, log(tmp) - a * tmp);

    rop<T>::begin();
    b = rop<T>::mul_up(rop<T>::add_up(range.upper(), range.lower()), T(0.5));
    err = rop<T>::sub_up(b, range.lower());
    for (i=1; i<xs; i++) {
      r.a(i) = rop<T>::mul_down(x.a(i), a);
    }
    for (i=1; i<xs; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(x.a(i), a), r.a(i)));
    }
    #if AFFINE_SIMPLE != 2
    for (i=xs; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    l = rop<T>::add_down(rop<T>::mul_down(x.a(0), a), b);
    u = rop<T>::add_up(rop<T>::mul_up(x.a(0), a), b);
    r.a(0) = rop<T>::mul_up(rop<T>::add_up(l, u), T(0.5));
    err = rop<T>::add_up(err, rop<T>::sub_up(r.a(0), l));
    #if AFFINE_SIMPLE >= 1
    // err += abs(a) * x.er;
    using std::abs;
    err = rop<T>::add_up(err, rop<T>::mul_up(abs(a), x.er));
    #endif
    rop<T>::end();

    #if AFFINE_SIMPLE == 2
    r.er = err;
    #else
    r.a(maxnum()) = err;
    # if AFFINE_SIMPLE == 1
    r.er = 0.;
    # endif
    #endif

    return r;
  }

  friend affine abs(const affine& x) {
    affine r;
    T err;
    interval<T> I, range;
    T a, b, l, u;
    int i, xs;

    I = to_interval(x);
    l = I.lower();
    u = I.upper();

    if (l >= 0.) {
      return x;
    }
    if (u <= 0.) {
      return -x;
    }

    xs = x.a.size();

    #if AFFINE_SIMPLE == 2
    r.a.resize(xs);
    #else
    maxnum()++;
    r.a.resize(maxnum()+1);
    #endif

    a = (u + l) / (u - l);

    range = 0.;
    range = interval<T>::hull(range, u - a * u);
    range = interval<T>::hull(range, -l - a * l);

    rop<T>::begin();
    b = rop<T>::mul_up(rop<T>::add_up(range.upper(), range.lower()), T(0.5));
    err = rop<T>::sub_up(b, range.lower());
    for (i=1; i<xs; i++) {
      r.a(i) = rop<T>::mul_down(x.a(i), a);
    }
    for (i=1; i<xs; i++) {
      err = rop<T>::add_up(err, rop<T>::sub_up(rop<T>::mul_up(x.a(i), a), r.a(i)));
    }
    #if AFFINE_SIMPLE != 2
    for (i=xs; i<maxnum(); i++) r.a(i) = 0.;
    #endif
    l = rop<T>::add_down(rop<T>::mul_down(x.a(0), a), b);
    u = rop<T>::add_up(rop<T>::mul_up(x.a(0), a), b);
    r.a(0) = rop<T>::mul_up(rop<T>::add_up(l, u), T(0.5));
    err = rop<T>::add_up(err, rop<T>::sub_up(r.a(0), l));
    #if AFFINE_SIMPLE >= 1
    // err += abs(a) * x.er;
    using std::abs;
    err = rop<T>::add_up(err, rop<T>::mul_up(abs(a), x.er));
    #endif
    rop<T>::end();

    #if AFFINE_SIMPLE == 2
    r.er = err;
    #else
    r.a(maxnum()) = err;
    # if AFFINE_SIMPLE == 1
    r.er = 0.;
    # endif
    #endif

    return r;
  }

  // lazy implementation of integer pow
  friend affine pow(const affine& x, int y) {
    affine r, xp;
    int a, tmp;

    if (y == 0) return affine(1.);

    a = (y >= 0) ? y : -y;

    tmp = a;
    r = 1.;
    xp = x;
    while (tmp != 0) {
      if (tmp % 2 != 0) {
        r *= xp;
      }
      tmp /= 2;
      xp = xp * xp;
    }

    if (y < 0) {
      r = 1. / r;
    }

    return r;
  }

  friend affine pow(const affine& x, const affine& y) {
    return exp(y * log(x));
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value && ! boost::is_integral<C>::value, affine >::type pow(const affine& x, const C& y) {
    return pow(x, affine(y));
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type pow(const affine& x, const C& y) {
    return pow(x, affine(y));
  }

  template <class C> friend typename boost::enable_if_c< acceptable_n<C, affine>::value, affine >::type pow(const C& x, const affine& y) {
    return pow(affine(x), y);
  }

  template <class C> friend typename boost::enable_if_c< acceptable_s<C, affine>::value, affine >::type pow(const C& x, const affine& y) {
    return pow(affine(x), y);
  }

  // lazy implementation of sin
  friend affine sin(const affine& x) {
    // return sin((interval<T>)(x.a(0))) + cos(to_interval(x)) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = sin((interval<T>)(x.a(0)));
    r2 = cos(to_interval(x));
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of cos
  friend affine cos(const affine& x) {
    // return cos((interval<T>)(x.a(0))) - sin(to_interval(x)) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = cos((interval<T>)(x.a(0)));
    r2 = -sin(to_interval(x));
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of tan
  friend affine tan(const affine& x) {
    // return tan((interval<T>)(x.a(0))) + pow(cos(to_interval(x)), -2) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = tan((interval<T>)(x.a(0)));
    r2 = pow(cos(to_interval(x)), -2);
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of asin
  friend affine asin(const affine& x) {
    // return asin((interval<T>)(x.a(0))) + (1. / sqrt(1. - pow(to_interval(x), 2))) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = asin((interval<T>)(x.a(0)));
    r2 = 1. / sqrt(1. - pow(to_interval(x), 2));
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of acos
  friend affine acos(const affine& x) {
    // return acos((interval<T>)(x.a(0))) + (- 1. / sqrt(1. - pow(to_interval(x), 2))) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = acos((interval<T>)(x.a(0)));
    r2 = - 1. / sqrt(1. - pow(to_interval(x), 2));
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of atan
  friend affine atan(const affine& x) {
    // return atan((interval<T>)(x.a(0))) + (1. / sqrt(1. + pow(to_interval(x), 2))) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = atan((interval<T>)(x.a(0)));
    r2 = 1. / sqrt(1. + pow(to_interval(x), 2));
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of sinh
  friend affine sinh(const affine& x) {
    // return sinh((interval<T>)(x.a(0))) + cosh(to_interval(x)) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = sinh((interval<T>)(x.a(0)));
    r2 = cosh(to_interval(x));
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of cosh
  friend affine cosh(const affine& x) {
    // return cosh((interval<T>)(x.a(0))) + sinh(to_interval(x)) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = cosh((interval<T>)(x.a(0)));
    r2 = sinh(to_interval(x));
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of tanh
  friend affine tanh(const affine& x) {
    // return tanh((interval<T>)(x.a(0))) + pow(cosh(to_interval(x)), -2) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = tanh((interval<T>)(x.a(0)));
    r2 = pow(cosh(to_interval(x)), -2);
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of asinh
  friend affine asinh(const affine& x) {
    // return asinh((interval<T>)(x.a(0))) + (1 / sqrt(pow(to_interval(x), 2) + 1)) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = asinh((interval<T>)(x.a(0)));
    r2 = 1 / sqrt(pow(to_interval(x), 2) + 1);
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of acosh
  friend affine acosh(const affine& x) {
    // return acosh((interval<T>)(x.a(0))) + (1 / sqrt(pow(to_interval(x), 2) - 1)) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = acosh((interval<T>)(x.a(0)));
    r2 = 1 / sqrt(pow(to_interval(x), 2) - 1);
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  // lazy implementation of atanh
  friend affine atanh(const affine& x) {
    // return atanh((interval<T>)(x.a(0))) + (1 / (1 - pow(to_interval(x), 2))) * (x - x.a(0));

    affine tmp;
    interval<T> r, r2;
    T m;

    tmp = x;
    tmp.a(0) = 0.;
    r = atanh((interval<T>)(x.a(0)));
    r2 = 1 / (1 - pow(to_interval(x), 2));
    m = mid(r2);
    r += (r2 - m) * to_interval(tmp);

    return r + m * tmp;
  }

  friend std::ostream& operator<<(std::ostream& s, const affine& x) {
    int i;

    s << "[(" << x.a(0) << ")";
    for (i=1; i<x.a.size(); i++) {
      s << "+(" << x.a(i) << ")e" << i;
    }
    #if AFFINE_SIMPLE >= 1
    s << "+(" << x.er << ")er";
    #endif
    s << "]";

    return s;
  }

  friend inline void split(const affine& x, int n, affine& y, affine& z) {
    int i;
    int s = x.a.size();

    y.a.resize(s);
    z.a.resize(s);
    for (i=0; i<=n; i++) {
      y.a(i) = x.a(i);
      z.a(i) = 0.;
    }
    for (i=n+1; i<s; i++) {
      y.a(i) = 0.;
      z.a(i) = x.a(i);
    }
    #if AFFINE_SIMPLE >= 1
    y.er = 0.;
    z.er = x.er;
    #endif
  }

  void resize() {
    int i;
    ub::vector<T> r;

    r.resize(maxnum()+1);
    for (i=0; i<=maxnum(); i++) {
      r(i) = a(i);
    }

    a = r;
  }
};

template <class T> inline ub::vector< interval<T> > to_interval(const ub::vector< affine<T> >& x) {
  int s = x.size();
  ub::vector< interval<T> > r;
  int i;

  r.resize(s);
  for (i=0; i<s; i++) r(i) = to_interval(x(i));

  return r;
}

/*
 * epsilon_reduce(x, n, n_limit)
 *  x: vector of affine (overwrited)
 *  reduce the number of epsilons to n if the number of epsilons > n_limit.
 *
 *  s: size of x
 *  keep n-s "important" epsilons and convert other "non-important" epsilons
 *  to s-dimensional rectangular.
 */

// a class to store column vector and its "importance"

template <class T> class ep_reduce_v {
  public:
  ub::vector<T> v;
  T score;
  int index; // @hylagi to identify intervalized dummy variables
  void calc_score() {
    int s = v.size();
    int i, j;
    T m1, m2, tmp;
    using std::abs;
    if (s<2){score = abs(v(0)); return;} // @hylagi
    m1 = abs(v(0));
    if (s == 1) {
      score = m1;
      return;
    }
    m2 = abs(v(1));
    if (m2 > m1) {
      tmp = m2; m2 = m1; m1 = tmp;
    }
    for (i=2; i<s; i++) {
      tmp = abs(v(i));
      if (tmp > m1) {
        m2 = m1; m1 = tmp;
      } else if (tmp > m2) {
        m2 = tmp;
      }
    }
    if (m1 == 0.) score = 0.;
    else score = (m1*m2)/(m1+m2);
  }
};

// function to sort column vector by score

template <class T> inline bool ep_reduce_cmp(ep_reduce_v<T>* a, ep_reduce_v<T>* b) {
#ifdef EP_REDUCE_REVERSE
  return a->score < b->score;
#else
  return a->score > b->score;
#endif
}
//template <class T> inline void epsilon_reduce(ub::vector< affine<T> >& x, int n, int n_limit = 0) {
template <class T> inline std::map<int, int> epsilon_reduce(ub::vector< affine<T> >& x, int n, int n_limit = 0) { // @hylagi modified
  int s = x.size();
  int m = affine<T>::maxnum();
  int i, j;
  std::vector< ep_reduce_v<T> > a;
  std::vector< ep_reduce_v<T>* > pa;
  ub::vector< affine<T> > r;
  T tmp;
  std::map<int, int> result_map; // @hylagi

  if (n_limit < n) n_limit = n;

  // if (m <= n_limit) return;
  // if (n < s) return; // impossible
  // @hylagi modified
  if (m <= n_limit) return result_map;
  if (n < s) return result_map; // impossible

  a.resize(m);
  pa.resize(m);

  for (i=1; i<=m; i++) {
    a[i-1].v.resize(s);
    a[i-1].index = i; // @hylagi
    result_map.insert(std::make_pair(i, -1)); // @hylagi
    for (j=0; j<s; j++) {
      a[i-1].v(j) = (i < x(j).a.size()) ? x(j).a(i) : (T)0.;
    }
    a[i-1].calc_score();
    pa[i-1] = &(a[i-1]);
  }

#ifdef EP_REDUCE_REVERSE
  std::partial_sort(pa.begin(), pa.begin()+m-n+s, pa.end(), ep_reduce_cmp<T>);
#else
  std::partial_sort(pa.begin(), pa.begin()+n-s, pa.end(), ep_reduce_cmp<T>);
#endif

  r.resize(s);
  for (i=0; i<s; i++) {
    r(i).a.resize(n+1);
    r(i).a(0) = x(i).a(0);
    for (j=0; j<n-s; j++) {
#ifdef EP_REDUCE_REVERSE
      r(i).a(j+1) = pa[m-1-j]->v(i);
      result_map[pa[m-1-j]->index] = j+1; // @hylagi
#else
      r(i).a(j+1) = pa[j]->v(i);
#endif
    }
    tmp = 0.;
    rop<T>::begin();
    for (j=n-s; j<m; j++) {
      using std::abs;
#ifdef EP_REDUCE_REVERSE
      tmp = rop<T>::add_up(tmp, abs(pa[m-1-j]->v(i)));
#else
      tmp = rop<T>::add_up(tmp, abs(pa[j]->v(i)));
#endif
    }
    #if AFFINE_SIMPLE >= 1
    tmp = rop<T>::add_up(tmp, x(i).er);
    #endif
    rop<T>::end();
    for (j=n-s; j<n; j++) {
      r(i).a(j+1) = 0.;
    }
    r(i).a(n-s+i+1) = tmp;
    #if AFFINE_SIMPLE >= 1
    r(i).er = 0.;
    #endif
  }

  x = r;
  affine<T>::maxnum() = n;
  return result_map; //@hylagi
}

// simple version of epsilon_reduce
// keep ep_1...ep_n and "intervalize" epsilons newer than ep_n

template <class T> inline void epsilon_reduce2(ub::vector< affine<T> >& x, int n) {
  int s = x.size();
  int i, j;
  T tmp;

  for (i=0; i<s; i++) {
    tmp = 0.;
    rop<T>::begin();
    for (j=n+1; j<x(i).a.size(); j++) {
      using std::abs;
      tmp = rop<T>::add_up(tmp, abs(x(i).a(j)));
    }
    #if AFFINE_SIMPLE >= 1
    tmp = rop<T>::add_up(tmp, x(i).er);
    #endif
    rop<T>::end();

    x(i).a.resize(n+1+i+1, true);
    for (j=0; j<i; j++) {
      x(i).a(n+1+j) = 0.;
    }
    x(i).a(n+1+i) = tmp;
    #if AFFINE_SIMPLE >= 1
    x(i).er = 0.;
    #endif
  }

  affine<T>::maxnum() = n + s;
}

template <class T> struct constants< affine<T> > {
  static affine<T> pi() {
    static const affine<T> tmp(constants< interval<T> >::pi());
    return tmp;
  }

  static affine<T> e() {
    static const affine<T> tmp(constants< interval<T> >::e());
    return tmp;
  }

  static affine<T> ln2() {
    static const affine<T> tmp(constants< interval<T> >::ln2());
    return tmp;
  }
  static affine<T> str(const std::string& s) {
    return affine<T>(constants< interval<T> >::str(s));
  }
  static affine<T> str(const std::string& s1, const std::string& s2) {
    return affine<T>(constants< interval<T> >::str(s1, s2));
  }
};

} // namespace kv

#endif //AFFINE_HPP

ext: hpp

max_stars_repo_name: takafumihoriuchi/HyLaGI

max_issues_repo_name: takafumihoriuchi/HyLaGI

max_forks_repo_name: takafumihoriuchi/HyLaGI