#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "std_msgs/Bool.h"
#include "std_msgs/String.h"
#include "std_msgs/Float32.h"
#include "geometry_msgs/Twist.h"
#include "sensor_msgs/Imu.h"
#include "visualization_msgs/Marker.h"
#include "visualization_msgs/MarkerArray.h"
#include "intersection_recognition/Scenario.h"
#include <unistd.h>
#include <cmath>
#include <vector>
#include <Eigen/Dense>
#include <iostream>

class cmdVelController {
     public:
        cmdVelController();
        geometry_msgs::Twist vel_;
        std::string ROBOT_FRAME = "base_link";
        double IMU_HZ = 100.0;
        double robot_control_freq = 10.0;
        double path_update_freq = 2.0;
        double ROBOT_COLLISION_RADIUS = 0.5;
        double dt = 1 / path_update_freq;
        double predict_time = 3.0;
        double v_max_lim = 2.0;
        double v_min_lim = -2.0;
        double v_acc_max = 0.5;
        double delta_v = 0.05;
        double w_max_lim = 0.5;
        double w_min_lim = -0.5;
        double w_acc_max = 0.2;
        double delta_w = 0.05;
        double k_rho = 3.0;
        double k_alpha = 0.59;
        double k_v = 1.0;
        double lambda1 = 3/13;
        double lambda2 = 3/13;
        double lambda3 = 7/13;
        float reverse_turn = 1.0;
        float target_yaw_rad_ = 0;
        void getRosParam(void);
        void imuCallback(const sensor_msgs::Imu::ConstPtr& imu_data);
        void targetYawRadCallback(const std_msgs::Float32::ConstPtr& target_yaw_rad);
        void emergencyStopFlgCallback(const std_msgs::Bool::ConstPtr& emergency_stop_flg);
        void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
        Eigen::MatrixXd calcDistance(const sensor_msgs::LaserScan::ConstPtr& scan, const Eigen::VectorXd &v_range, const Eigen::VectorXd &w_range);
        Eigen::MatrixXd minMaxNormalize(const Eigen::MatrixXd &mat);
        void visualize_trajectory(const double v, const double w, const ros::Publisher& pub);
        void timerForCmdVelCallback(const ros::TimerEvent& e_cmd_vel);
        void timerForDWACallback(const ros::TimerEvent& e_dwa);

     private:
        ros::NodeHandle node_;
        ros::Publisher cmd_vel_pub_;
        ros::Publisher turn_finish_flg_pub_;
        ros::Publisher selected_trajectory_pub_;
        ros::Subscriber imu_sub_;
        ros::Subscriber target_yaw_rad_sub_;
        ros::Subscriber emergency_stop_flg_sub_;
        ros::Subscriber scan_sub_;
        ros::Timer timer_for_cmd_vel;
        ros::Timer timer_for_dwa;

        bool emergency_stop_flg_ = true;
        bool update_path_flg_ = true;
        double current_yaw_rad_ = 0.0;
        double rho = 5.0;
        double alpha = 0.0;
};

cmdVelController::cmdVelController(){
    cmd_vel_pub_ = node_.advertise<geometry_msgs::Twist>("cmd_vel", 1, false);
    turn_finish_flg_pub_ = node_.advertise<std_msgs::Bool>("turn_finish_flg", 1, false);
    selected_trajectory_pub_ = node_.advertise<visualization_msgs::Marker>("selected_trajectory", 1);

    imu_sub_ = node_.subscribe<sensor_msgs::Imu> ("imu_data", 1, &cmdVelController::imuCallback, this);
    target_yaw_rad_sub_ = node_.subscribe<std_msgs::Float32> ("target_yaw_rad", 1, &cmdVelController::targetYawRadCallback, this);
    emergency_stop_flg_sub_ = node_.subscribe<std_msgs::Bool> ("emergency_stop_flg", 1, &cmdVelController::emergencyStopFlgCallback, this);
    scan_sub_ = node_.subscribe<sensor_msgs::LaserScan> ("scan", 1, &cmdVelController::scanCallback, this);

    getRosParam();
    timer_for_cmd_vel = node_.createTimer(ros::Duration(1.0/robot_control_freq), &cmdVelController::timerForCmdVelCallback, this);
    timer_for_dwa = node_.createTimer(ros::Duration(1.0/path_update_freq), &cmdVelController::timerForDWACallback, this);
}

void cmdVelController::getRosParam(void){
    node_.getParam("cmd_vel_controller/IMU_HZ", IMU_HZ);
    node_.getParam("cmd_vel_controller/robot_control_freq", robot_control_freq);
    node_.getParam("cmd_vel_controller/path_update_freq", path_update_freq);
    node_.getParam("cmd_vel_controller/ROBOT_COLLISION_RADIUS", ROBOT_COLLISION_RADIUS);
    node_.getParam("cmd_vel_controller/ROBOT_FRAME", ROBOT_FRAME);
    node_.getParam("cmd_vel_controller/predict_time", predict_time);
    node_.getParam("cmd_vel_controller/reverse_turn", reverse_turn);
    if(reverse_turn != 1.0) reverse_turn = -1.0;
    node_.getParam("cmd_vel_controller/v_max_lim", v_max_lim);
    node_.getParam("cmd_vel_controller/v_min_lim", v_min_lim);
    node_.getParam("cmd_vel_controller/v_acc_max", v_acc_max);
    node_.getParam("cmd_vel_controller/delta_v", delta_v);
    node_.getParam("cmd_vel_controller/w_max_lim", w_max_lim);
    node_.getParam("cmd_vel_controller/w_min_lim", w_min_lim);
    node_.getParam("cmd_vel_controller/w_acc_max", w_acc_max);
    node_.getParam("cmd_vel_controller/k_rho", k_rho);
    node_.getParam("cmd_vel_controller/k_alpha", k_alpha);
    node_.getParam("cmd_vel_controller/k_v", k_v);
    node_.getParam("cmd_vel_controller/lambda1", lambda1);
    node_.getParam("cmd_vel_controller/lambda2", lambda2);
    node_.getParam("cmd_vel_controller/lambda3", lambda3);
}

void cmdVelController::imuCallback(const sensor_msgs::Imu::ConstPtr& imu_data){
    current_yaw_rad_ += imu_data->angular_velocity.z / IMU_HZ;
}

void cmdVelController::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan){
    if(! emergency_stop_flg_){
        if(update_path_flg_){
            alpha = target_yaw_rad_ - current_yaw_rad_; // [rad]
            if(alpha > M_PI){
                alpha -= 2 * M_PI;
            }
            else if(alpha < -M_PI){
                alpha += 2 * M_PI;
            }
            // calculate ideal control
            double vi = k_v * v_max_lim * std::cos(alpha) * std::tanh(rho / k_rho);
            double wi = k_alpha * alpha + k_v * v_max_lim * std::tanh(rho / k_rho) / rho * std::sin(2.0 * alpha) / 2.0;

            // calculate dynamic window
            double next_v_max = std::min(v_max_lim, vel_.linear.x + v_acc_max / path_update_freq);
            double next_v_min = std::max(v_min_lim, vel_.linear.x - v_acc_max / path_update_freq);
            double next_w_max = std::min(w_max_lim, vel_.angular.z + w_acc_max / path_update_freq);
            double next_w_min = std::max(w_min_lim, vel_.angular.z - w_acc_max / path_update_freq);
            int v_range_size  = std::round((next_v_max - next_v_min) / delta_v) + 1;
            int w_range_size  = std::round((next_w_max - next_w_min) / delta_w) + 1;

            Eigen::VectorXd v_range = Eigen::VectorXd::LinSpaced(v_range_size, next_v_min, next_v_max);
            Eigen::VectorXd w_range = Eigen::VectorXd::LinSpaced(w_range_size, next_w_min, next_w_max);

            // calculate objective function
            // about velocity
            Eigen::MatrixXd mat_one = Eigen::MatrixXd::Ones(v_range.size(), w_range.size());
            Eigen::VectorXd vec_one = Eigen::VectorXd::Ones(v_range.size());
            Eigen::MatrixXd G_v = (vec_one - ((v_range - vi * vec_one).cwiseAbs() / (2 * v_max_lim))).asDiagonal() * mat_one;
            G_v = minMaxNormalize(G_v);

            // about angular velocity
            vec_one = Eigen::VectorXd::Ones(w_range.size());
            //mat_one = Eigen::MatrixXd::Ones(w_range.size(), v_range.size());
            Eigen::MatrixXd G_w = mat_one * (vec_one - ((w_range - wi * vec_one).cwiseAbs() / (2 * w_max_lim))).asDiagonal();
            G_w = minMaxNormalize(G_w);

            // about distance to obstacles
            Eigen::MatrixXd min_distance = calcDistance(scan, v_range, w_range);
            Eigen::MatrixXd G_dist = min_distance;
            G_dist = minMaxNormalize(G_dist);

            // merge variables
            Eigen::MatrixXd G = lambda1 * G_v + lambda2 * G_w + lambda3 * G_dist;
            G -= (min_distance.array() < ROBOT_COLLISION_RADIUS).matrix().cast<double>() * 1e6;

            // get a pair of velocity and angle velocity which takes highest score
            Eigen::MatrixXd::Index maxRow, maxCol;
            double max_distance = G.maxCoeff(&maxRow, &maxCol);
            vel_.linear.x = v_range(maxRow);
            vel_.angular.z = w_range(maxCol);
            visualize_trajectory(vel_.linear.x, vel_.angular.z, selected_trajectory_pub_);
            std::cout << "IDWA: selected v is " << vel_.linear.x << " selected w is " << vel_.angular.z << std::endl;
        }
    }
}

Eigen::MatrixXd cmdVelController::calcDistance(const sensor_msgs::LaserScan::ConstPtr& scan, const Eigen::VectorXd &v_range, const Eigen::VectorXd &w_range){
    // skip inf
    int num_scan = scan->ranges.size();
    int valid_beams = 0;
    for(int i = 0; i < num_scan; i ++) {
        if(!(std::isnan(scan->ranges[i]) || std::isinf(scan->ranges[i]) || scan->ranges[i] > scan->range_max || scan->ranges[i] < scan->range_min)){
            valid_beams++;
        }
    }
    Eigen::VectorXd scan_cp_eigen(valid_beams);
    Eigen::VectorXd scan_direction(valid_beams);
    for(int i = 0; i < num_scan; i ++) {
        if(!(std::isnan(scan->ranges[i]) || std::isinf(scan->ranges[i]) || scan->ranges[i] > scan->range_max || scan->ranges[i] < scan->range_min)){
            scan_cp_eigen(i) = scan->ranges[i];
            scan_direction(i) = scan->angle_min + scan->angle_increment * i;
        }
    }

    // calculate obstacle point
    Eigen::VectorXd obs_point_x = (scan_direction.array() + M_PI_2).cos().matrix().asDiagonal() * scan_cp_eigen;
    Eigen::VectorXd obs_point_y = (scan_direction.array() + M_PI_2).sin().matrix().asDiagonal() * scan_cp_eigen;

    // calculate robot state
    Eigen::MatrixXd state_x = Eigen::MatrixXd::Zero(v_range.size(), w_range.size());
    Eigen::MatrixXd state_y = Eigen::MatrixXd::Zero(v_range.size(), w_range.size());
    Eigen::MatrixXd min_distance = Eigen::MatrixXd::Ones(v_range.size(), w_range.size()) * 1e6;

    Eigen::MatrixXd vec_one = Eigen::MatrixXd::Ones(v_range.size(), w_range.size());
    for(double t = 0; t <= predict_time; t += dt){
        state_x += v_range.asDiagonal() * vec_one * (w_range.array() + M_PI_2).cos().matrix().asDiagonal() * dt;
        state_y += v_range.asDiagonal() * vec_one * (w_range.array() + M_PI_2).sin().matrix().asDiagonal() * dt;
        // calculate distance to obstacle
        for(int index_row = 0; index_row < state_x.rows(); index_row++){
            for(int index_col = 0; index_col < state_x.cols(); index_col++){
                auto distance_x = obs_point_x.array() - state_x(index_row, index_col);
                auto distance_y = obs_point_y.array() - state_y(index_row, index_col);
                Eigen::MatrixXd distance = (distance_x.array().pow(2) + distance_y.array().pow(2)).sqrt();
                min_distance(index_row, index_col) = distance.minCoeff();
            }
        }
    }
    return min_distance;
}

Eigen::MatrixXd cmdVelController::minMaxNormalize(const Eigen::MatrixXd &mat){
    double min = mat.minCoeff();
    double max = mat.maxCoeff();
    Eigen::MatrixXd result = (mat.array() - min).matrix() / (max - min);
    return result;
}

void cmdVelController::visualize_trajectory(const double v, const double w, const ros::Publisher& pub)
{
    visualization_msgs::Marker v_trajectory;
    v_trajectory.header.frame_id = ROBOT_FRAME;
    v_trajectory.header.stamp = ros::Time::now();
    v_trajectory.color.r = 1;
    v_trajectory.color.g = 0;
    v_trajectory.color.b = 1;
    v_trajectory.color.a = 0.8;
    v_trajectory.ns = pub.getTopic();
    v_trajectory.type = visualization_msgs::Marker::LINE_STRIP;
    v_trajectory.action = visualization_msgs::Marker::ADD;
    v_trajectory.lifetime = ros::Duration();
    v_trajectory.scale.x = 0.05;
    geometry_msgs::Pose pose;
    pose.orientation.w = 1;
    v_trajectory.pose = pose;
    geometry_msgs::Point p;
    p.x = 0.0;
    p.y = 0.0;
    for(double t = 0.0; t <= predict_time; t+=dt){
        p.x += v * std::cos(w) * dt;
        p.y += v * std::sin(w) * dt;
        v_trajectory.points.push_back(p);
    }
    pub.publish(v_trajectory);
}

void cmdVelController::targetYawRadCallback(const std_msgs::Float32::ConstPtr& target_yaw_rad){
    current_yaw_rad_ = 0.0;
    target_yaw_rad_ = target_yaw_rad->data * reverse_turn;
}

void cmdVelController::emergencyStopFlgCallback(const std_msgs::Bool::ConstPtr& emergency_stop_flg){
    emergency_stop_flg_ = emergency_stop_flg->data;
}

void cmdVelController::timerForCmdVelCallback(const ros::TimerEvent& e_cmd_vel){
    if(emergency_stop_flg_){
        vel_.linear.x = 0.0;
        vel_.angular.z = 0.0;
    }

    cmd_vel_pub_.publish(vel_);
}

void cmdVelController::timerForDWACallback(const ros::TimerEvent& e_dwa){
    update_path_flg_ = true;
}

int main(int argc, char** argv){
    ros::init(argc, argv, "cmd_vel_controller");
    cmdVelController cmd_vel_controller;
    while(ros::ok()){
        ros::spin();
    }

    return 0;
}


#ifndef UUID_1CAA534B_0EB4_489D_8FA7_D5343EB58DD5
#define UUID_1CAA534B_0EB4_489D_8FA7_D5343EB58DD5

#include <nocopy/detail/align_to.hpp>

#include <nocopy/detail/ignore_warnings_from_dependencies.hpp>
BEGIN_IGNORE_WARNINGS_FROM_DEPENDENCIES
#include <boost/hana/at.hpp>
#include <boost/hana/maximum.hpp>
#include <boost/hana/size.hpp>
#include <boost/hana/tuple.hpp>
END_IGNORE_WARNINGS_FROM_DEPENDENCIES

namespace nocopy { namespace detail {
  template <typename Tag, typename ...Ts>
  class oneof_packer {
    static constexpr auto allowed_types() {
      return hana::make_tuple(hana::type_c<Ts>...);
    }

    struct lookup_fold {
      template <typename Pair, typename T>
      constexpr auto operator()(Pair&& pair, T t) const {
        auto& map = hana::first(pair);
        auto index = hana::second(pair);
        return hana::make_pair(hana::insert(map, hana::make_pair(t, index)), index + 1);
      }
    };
    static constexpr auto index_lookup() {
      return hana::first(hana::fold_left(
        allowed_types()
      , hana::make_pair(hana::make_map(), std::size_t{0})
      , lookup_fold{}
      ));
    }

    struct alignment_ordering {
      template <typename X, typename Y>
      constexpr auto operator()(X, Y) const {
        constexpr bool result = X::type::alignment < Y::type::alignment;
        return hana::bool_c<result>;
      }
    };
    static constexpr auto payload_alignment() {
      constexpr auto biggest = hana::maximum(allowed_types(), alignment_ordering{});
      return decltype(biggest)::type::alignment;
    }

  public:
    template <typename T>
    static constexpr bool is_allowed() {
      return hana::contains(allowed_types(), hana::type_c<T>);
    }

    static constexpr std::size_t num_types() {
      return hana::size(allowed_types());
    }

    template <typename T>
    static constexpr auto tag_for_type() {
      return index_lookup()[hana::type_c<T>];
    }

    template <std::size_t Index>
    static constexpr auto lookup_trait() {
      return typename decltype(+hana::at_c<Index>(allowed_types()))::type{};
    }

    static constexpr auto payload_offset() {
      return align_to(sizeof(Tag), payload_alignment());
    }

    static constexpr auto alignment() {
      return std::max(payload_alignment(), sizeof(Tag));
    }

    static constexpr auto size() {
      return align_to(payload_offset() + payload_alignment(), alignment());
    }
  };
}}

#endif


/*
Copyright 2009-2021 Nicolas Colombe

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

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

#pragma once

#include <core/coredef.hpp>
#include <core/pooledlist.hpp>
#include <math/segment.hpp>
#include <math/mathexp.hpp>

#include <boost/rational.hpp>
#include <boost/pool/object_pool.hpp>

namespace eXl
{
  template <typename IntReal = int32_t>
  struct FastRational
    :boost::less_than_comparable < FastRational<IntReal>,
    boost::equality_comparable < FastRational<IntReal>,
    boost::less_than_comparable2 < FastRational<IntReal>, IntReal,
    boost::equality_comparable2 < FastRational<IntReal>, IntReal,
    boost::addable < FastRational<IntReal>,
    boost::subtractable < FastRational<IntReal>,
    boost::multipliable < FastRational<IntReal>,
    boost::dividable < FastRational<IntReal>,
    boost::addable2 < FastRational<IntReal>, IntReal,
    boost::subtractable2 < FastRational<IntReal>, IntReal,
    boost::subtractable2_left < FastRational<IntReal>, IntReal,
    boost::multipliable2 < FastRational<IntReal>, IntReal,
    boost::dividable2 < FastRational<IntReal>, IntReal,
    boost::dividable2_left < FastRational<IntReal>, IntReal,
    boost::incrementable < FastRational<IntReal>,
    boost::decrementable < FastRational<IntReal>
    > > > > > > > > > > > > > > > >
  {
  public:

    FastRational(IntReal iNum)
      : m_N(iNum){}

    FastRational(IntReal iNum, IntReal iDen)
      : m_N(iNum)
      , m_D(iDen)
    {}

    FastRational(FastRational const& iR)
      : m_N(iR.m_N)
      , m_D(iR.m_D)
    {}

    FastRational& operator=(IntReal i) { m_N = i; m_D = 1; return *this; }
    FastRational& assign(IntReal n, IntReal d)
    {
      m_N = n;
      m_D = d;
      return *this;
    }

    IntReal numerator() const { return m_N; }
    IntReal denominator() const { return m_D; }

    FastRational& operator+= (const FastRational& r)
    {
      if(m_D == r.m_D)
      {
        m_N += r.m_N;
      }
      else
      {
        assign(m_N * r.m_D + r.m_N * m_D, m_D * r.m_D);
        sanitize();
      }
      return *this;
    }
    FastRational& operator-= (const FastRational& r)
    {
      if(m_D == r.m_D)
      {
        m_N -= r.m_N;
      }
      else
      {
        assign(m_N * r.m_D - r.m_N * m_D, m_D * r.m_D);
        sanitize();
      }
      return *this;
    }
    FastRational& operator*= (const FastRational& r)
    {
      assign(m_N * r.m_N, m_D * r.m_D);
      sanitize();
      return *this;
    }
    FastRational& operator/= (const FastRational& r)
    {
      assign(m_N * r.m_D, m_D * r.m_N);
      sanitize();
      return *this;
    }

    FastRational& operator+= (IntReal i) { m_N += i * m_D; return *this; }
    FastRational& operator-= (IntReal i) { m_N -= i * m_D; return *this; }
    FastRational& operator*= (IntReal i)
    {
      m_N *= i;
      sanitize();
    }
    FastRational& operator/= (IntReal i)
    {
      m_D *= i;
      sanitize();
    }

    const FastRational& operator++() { m_N += m_D; return *this; }
    const FastRational& operator--() { m_N -= m_D; return *this; }
    bool operator!() const { return !m_N; }

    explicit operator bool() const { return operator !() ? false : true; }

    explicit operator int() const
    {
      return m_N / m_D;
    }

    explicit operator float() const
    {
      return static_cast<float>(m_N) / m_D;
    }

    bool operator< (const FastRational& r) const
    {
      return (*this - r).m_N < 0;
    }

    bool operator== (const FastRational& r) const
    {
      return (*this - r).m_N == 0;
    }

    bool operator< (IntReal i) const
    {
      return m_N < i * m_D;
    }

    bool operator> (IntReal i) const
    {
      return m_N > i * m_D;
    }

    bool operator== (IntReal i) const
    {
      FastRational temp = *this;
      temp.reduce();
      return m_D == 1 && m_N == i;
    }

    void sanitize()
    {
      if (m_D < 0)
      {
        m_D *= -1;
        m_N *= -1;
      }
    }

    void reduce()
    {
      sanitize();

      if (m_D == 1)
      {
        return;
      }

      IntReal pgcd = Math<IntReal>::PGCD(m_N, m_D);
      m_N /= pgcd;
      m_D /= pgcd;
    }

  protected:
    IntReal m_N = 0;
    IntReal m_D = 1;
  };

  template <typename IntReal>
  inline FastRational<IntReal> operator+ (const FastRational<IntReal>& r)
  {
    return r;
  }

  template <typename IntReal>
  inline FastRational<IntReal> operator- (const FastRational<IntReal>& r)
  {
    return FastRational<IntReal>(-r.numerator(), r.denominator());
  }

  typedef /*boost::rational<int64_t>*/ /*float*/ FastRational<int64_t> QType;
  typedef Vector2<QType> Vector2Q;

  template <typename Real>
  Vector2Q ToVec2Q(Vector2<Real> const& iVec)
  {
    return Vector2Q(iVec.X(), iVec.Y());
  }

  template <typename Real>
  Vector2<Real> FromVec2Q(Vector2Q const& iVec)
  {
    return Vector2<Real>(static_cast<Real>(iVec.X()), static_cast<Real>(iVec.Y()));
    //return Vector2<Real>(iVec.X().numerator() / iVec.X().denominator(), iVec.Y().numerator() / iVec.Y().denominator());
  }

  class EXL_MATH_API Intersector
  {
  public:

    struct ActiveSegment
    {
      Vector2Q m_Point;
      uint32_t m_Idx;
    };

    struct UserEvent
    {
      Vector2i m_Position;
      uint32_t m_Idx;
    };

    typedef std::function<bool(Vector<ActiveSegment> const& iSweepLine, uint32_t iPosInSweepLine, Segment<QType> const& iCandidate)> Filter;
    typedef std::function<void(Vector<ActiveSegment> const& iSweepLine, int32_t iLowSeg, int32_t iHighSeg, UserEvent const& iEvent)> EvtCallback;

    struct Parameters
    {
      Filter m_Filter;
      EvtCallback m_UsrEvtCb;
      std::vector<UserEvent> m_UsrEvts;
    };

    Intersector();

    Err IntersectSegments(Vector<Segmenti> const& iSegments, Vector<std::pair<uint32_t, Segmenti>>& oSegs, Parameters const& iParams = Parameters());

  private:

    struct Event
    {
      enum Type
      {
        Intersection,
        End,
        Start,
        User,
      };

      Event(PooledList<uint32_t>::Pool& iPool, Vector2Q const& iPoint, uint32_t iSeg, Type iType);
      Event(PooledList<uint32_t>::Pool& iPool, Vector2Q const& iPoint, uint32_t iSeg1, uint32_t iSeg2);
      Event(Event const& iEvt);
      Event& operator=(Event const& iEvt);
      Event(Event&& iEvt);
      Event& operator=(Event&& iEvt);

      bool IsEmpty() const;

      bool operator<(Event const& iOther) const;

      Vector2Q m_Point;
      mutable PooledList<uint32_t> m_SegmentsStart;
      mutable PooledList<uint32_t> m_SegmentsInter;
      mutable PooledList<uint32_t> m_SegmentsEnd;
      //Type m_Type;
    };

    struct OrderedSeg
    {
      OrderedSeg(Segmenti const& iSeg, uint32_t iOrigSeg);

      QType GetYAt(QType iX, Event const& iEvt) const;

      Vector2Q m_Start;
      Vector2Q m_End;
      boost::optional<QType> m_Slope;
      uint32_t m_OrigSeg;
    };

    void CheckIntersection(Event const& iEvt, OrderedSeg const& iSeg1, OrderedSeg const& iSeg2);
    bool CheckIntersection(Event const& iEvt, OrderedSeg const& iSeg1, OrderedSeg const& iSeg2, boost::optional<Event>& outSeg);

    void InsertEvent(Event&& iEvt);

    void RemoveEvt(uint32_t iSeg1, uint32_t iSeg2);

    void SortSegmentExtremities();

    Vector<ActiveSegment> m_ActiveSegments;
    Vector<uint32_t> m_SegInsertCount;
    PooledList<uint32_t>::Pool m_SegListPool;
    Vector<uint32_t> m_SortArray;

    Vector<Event> m_EventQueue;

    //MemoryPool m_EvtListAlloc;
    //typedef std::multiset<Event, std::less<Event>, PooledAllocator<Event>> EventQueue;
    //EventQueue m_EventQueue;
    Vector<OrderedSeg> m_Segments;
  };
}


//  Copyright John Maddock 2008.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Basic sanity check that header <boost/math/distributions/non_central_beta.hpp>
// #includes all the files that it needs to.
//
// In order to pass our conceptual tests, we need to forward declare
// those functoins that the non-central beta (deliberately) doesn't
// implement.  These declarations have to appear *before* the
// non central beta is defined, otherwise two-phase lookup
// won't find them :-(
//
#include <boost/math/distributions/fwd.hpp>

namespace boost{ namespace math{

template <class T, class Policy>
T mean(const non_central_beta_distribution<T, Policy>&)
{
   return 0;
}

template <class T, class Policy>
T variance(const non_central_beta_distribution<T, Policy>&)
{
   return 0;
}

template <class T, class Policy>
T skewness(const non_central_beta_distribution<T, Policy>&)
{
   return 0;
}

template <class T, class Policy>
T kurtosis(const non_central_beta_distribution<T, Policy>&)
{
   return 0;
}

template <class T, class Policy>
T kurtosis_excess(const non_central_beta_distribution<T, Policy>&)
{
   return 0;
}

}}

#include <boost/math/distributions/non_central_beta.hpp>
//
// Note this header includes no other headers, this is
// important if this test is to be meaningful:
//
#include "test_compile_result.hpp"

void compile_and_link_test()
{
   TEST_DIST_FUNC(non_central_beta)
}

template class boost::math::non_central_beta_distribution<float, boost::math::policies::policy<> >;
template class boost::math::non_central_beta_distribution<double, boost::math::policies::policy<> >;
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
template class boost::math::non_central_beta_distribution<long double, boost::math::policies::policy<> >;
#endif


/*
 * Verifier_toom.cpp
 *
 *  Created on: 25.04.2011
 *      Author: stephaniebayer
 */

#include "Verifier_toom.h"

#include<vector>
#include "Cipher_elg.h"
#include "G_q.h"
#include "Mod_p.h"
#include "Functions.h"
#include "ElGammal.h"
#include "multi_expo.h"
#include "func_ver.h"
#include <fstream>

#include <time.h>
#include <NTL/ZZ.h>
NTL_CLIENT

extern G_q G;
extern G_q H;
extern Pedersen Ped;
extern ElGammal El;
extern long mu;
extern long mu_h;
extern long m_r;

Verifier_toom::Verifier_toom() {
	// TODO Auto-generated constructor stub

}

Verifier_toom::Verifier_toom(vector<long> num) {
	// sets the values of the matrix according to the input
	m = num[1]; //number of rows
	n = num[2]; //number of columns
	omega = num[3]; //windowsize for multi-expo-technique
	omega_sw = num[4]; //windowsize for multi-expo-technique sliding window and LL
	omega_LL = num[7]; //windowsize for multi-expo-technique of LL

	c_A = new vector<Mod_p>(m+1); //allocate the storage for the commitments of Y
	c_B = new vector<Mod_p>(m); //allocate the storage for the commitments of T
	c_B_small = new vector<Mod_p>(m_r);//commitments after reduction with challenges x
	C_small = new vector<vector<Cipher_elg>* >(m_r); //reduced Ciphertexte, with challenges x

	chal_x6 = new vector<ZZ>(2*m);// allocate the storage for the vector of Vandermonde challenges t, ... t^n
	chal_y6 = new vector<ZZ>(n);// allocate the storage for the vector of Vandermonde challenges t, ... t^n
	chal_x8 = new vector<ZZ>(2*m +1); //challenge from round 8
	basis_chal_x8 = new vector<vector<long>*>(2*m+2);//basis of vector e for multi-expo technique
	mul_chal_x8 = new vector<ZZ>(2*m+2); //shifted vector e, e(0) = 1, used for multi-expo
	x = new vector<ZZ>(mu_h); //challenges to reduce ciphertexts

	//Commitments vectors
	c_Dh= new vector<Mod_p>(m); //commitments to the matrix W
	c_Ds= new vector<Mod_p>(m+1); //contains a_W*t_1
	c_Dl= new vector<Mod_p>(2*m+1); //commitments to the values Cl
	c_a_c= new vector<Mod_p>(mu_h); //commitment to the values used to reencrypt the E_x
	c_a= new vector<Mod_p>(2*mu); //commitment to the values in the matrix a
	//Vector of product of the diagonals of permuted Ciphertexts from round 5
	E = new vector<Cipher_elg>(2*mu);
	C_c = new vector<Cipher_elg>(mu_h); //Ciphertexts to prove correctness of reduction

	D_h_bar = new vector<ZZ>(n);//sum over the rows in D_h
	d_bar = new vector<ZZ>(n);// chal_x8*D_h(m-1) +d
	Delta_bar = new vector<ZZ>(n);//chal_x8*d_h+Delta
	B_bar  = new vector<ZZ>(n); // sum over the rows in B multiplied by chal^i

	A_bar = new vector<ZZ>(n); //sum over the rows in Y times the challenges
	Ds_bar = new vector<ZZ>(n); // sum over the rows in U times thes challenges
}

Verifier_toom::~Verifier_toom() {

	delete c_A;
	delete c_B;
	delete chal_x6;
	delete chal_y6;
	delete chal_x8;
	delete mul_chal_x8;
	Functions::delete_vector(basis_chal_x8);
	delete x;
	delete c_Dh;
	delete c_Ds;
	delete c_Dl;
	delete c_a;
	delete E;
	delete D_h_bar;
	delete d_bar;
	delete Delta_bar;
	delete B_bar;
	delete A_bar;
	delete Ds_bar;

	delete c_B_small;
	delete c_a_c;
	delete C_c;
}

string Verifier_toom::round_2(string in_name){
	long i;
	string name;
	ZZ ord=H.get_ord();
	time_t rawtime;
	time ( &rawtime );
	//sets a_Y to the values in the file name
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	for (i = 0; i<m; i++){
		ist >> c_A->at(i);
	}

	chal_x2 = RandomBnd(ord);

	name = "round_2 ";
	name = name + ctime(&rawtime);
	ofstream ost(name.c_str());
	ost << chal_x2;

	return name;
}

string Verifier_toom::round_4(string in_name){
	long i;
	ZZ ord = H.get_ord();
	string name;
	time_t rawtime;
	time ( &rawtime );
	 //sets a_T to the values in the file
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	for (i = 0; i<m; i++){
		ist >> c_B->at(i);
	}

	//Set name of the output file and open stream
	name = "round_4 ";
	name = name + ctime(&rawtime);

	chal_z4 = RandomBnd(ord);
	chal_y4 = RandomBnd(ord);

	ofstream ost(name.c_str());
	ost<< chal_z4<<"\n";
	ost<<chal_y4 ;
	return name;
}

string Verifier_toom::round_6(string in_name){
	ZZ tem;
	ZZ ord = H.get_ord();
	long i,l;
	string name;
	time_t rawtime;
	time ( &rawtime );

	//reads the values out of the file name
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	ist>>c_z;
	for (i = 0; i<m; i++){
		ist >>c_Dh->at(i);
	}
	for(i=0;i<mu_h;i++){
		ist>>C_c->at(i);
	}
	for(i=0; i<mu_h; i++){
		ist>>c_a_c ->at(i);
	}

	//sets the vector t to the values temp, temp^2,...
	func_ver::fill_vector(chal_x6);

	//sets the vector t to the values temp, temp^2,...
	func_ver::fill_vector(chal_y6);

	name = "round_6 ";
	name = name + ctime(&rawtime);
	l=2*m;
	ofstream ost(name.c_str());
	for (i=0; i<l; i++){
		ost << chal_x6->at(i)<<" ";
	}
	ost << "\n";
	for (i=0; i<n; i++){
		ost << chal_y6->at(i)<<" ";
	}
	ost << "\n";

	return name;

}

string Verifier_toom::round_6_red(string in_name,vector<vector<Cipher_elg>* >* enc){
	long i;
	ZZ tem;
	ZZ ord = H.get_ord();
	Cipher_elg c;
	Mod_p temp;
	string name;
	time_t rawtime;
	time ( &rawtime );

	//reads the values out of the file name
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	for(i=0;i<mu_h;i++){
		ist>>C_c->at(i);
	}
	for(i=0; i<mu_h; i++){
		ist>>c_a_c ->at(i);
	}

	//calculate the value of c
	calculate_c(c, enc);

	temp = Mod_p(1,H.get_mod()); //a_a_c->at(mu-1) should equal the commitment to 0
	if(c_a_c->at(mu-1)==temp & c == C_c->at(mu-1)){
		//sets the vector x to the values temp, temp^2,...
		func_ver::fill_vector(x);
	}
	name = "round_6 ";
	name = name + ctime(&rawtime);

	ofstream ost(name.c_str());
	for (i=0; i<mu_h; i++){
		ost << x->at(i)<<" ";
	}
	return name;

}

string Verifier_toom::round_6_red1(string in_name){
	long i,l;
	Mod_p temp, com;
	Cipher_elg C;
	ZZ mod = G.get_mod();
	string name;
	time_t rawtime;
	time ( &rawtime );

	//calculates the product of the the old commitments a_a_c to the power of x
	calculate_ac(com);

	//Combines the committed values to B in the vector c_B_small with challenges x
	reduce_c_B();

	//calulates the new value C
	calculate_C(C, C_c, x);

	//reads the values out of the file name
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	ist>>c_z;
	for (i = 0; i<m; i++){
		ist >>c_Dh->at(i);
	}
	for(i=0;i<mu_h;i++){
		ist>>C_c->at(i);
	}
	for(i=0; i<mu_h; i++){
		ist>>c_a_c ->at(i);
	}
	ist>>a_c_bar;
	ist>>r_ac_bar;

	temp = Mod_p(1,mod);//a_a_c->at(mu-1) should equal the commitment to 0
	if(c_a_c->at(mu-1)==temp  & com == Ped.commit(a_c_bar, r_ac_bar)& C == C_c->at(mu-1)){
		//sets the vector chal_x6 to the values temp, temp^2,...
		func_ver::fill_vector(chal_x6);

		//sets the vector chal_y6 to the values temp, temp^2,...
		func_ver::fill_vector(chal_y6);
	}

	name = "round_6 ";
	name = name + ctime(&rawtime);

	l=2*m;
	ofstream ost(name.c_str());
	for (i=0; i<l; i++){
		ost << chal_x6->at(i)<<" ";
	}
	ost << "\n";
	for (i=0; i<n; i++){
		ost << chal_y6->at(i)<<" ";
	}
	ost << "\n";

	return name;
}

string  Verifier_toom::round_8(string in_name){
	long i,l;
	string name;
	time_t rawtime;
	time ( &rawtime );
	//reads the values out of the file name

	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;

	for (i = 0; i<=2*m; i++){
		ist >> c_Dl->at(i) ;
	}
	ist>>c_D0;
	ist>>c_Dm;
	ist>>c_d;
	ist>>c_Delta;
	ist>>c_dh;
	ist>>a_c_bar;
	ist>>r_ac_bar;
	for(i=0; i<8; i++){
		ist>>E->at(i);
	}
	ist>>c_B0;
	for(i=0; i<8; i++){
		ist>>c_a->at(i);
	}

	func_ver::fill_x8(chal_x8, basis_chal_x8, mul_chal_x8, omega);
	l= chal_x8->size();

	name = "round_8 ";
	name = name + ctime(&rawtime);
	ofstream ost(name.c_str());
	for (i = 0; i<l; i++){
		ost << chal_x8->at(i)<< " ";
	}
	return name;
}

ZZ Verifier_toom:: round_10(string in_name,vector<vector<Cipher_elg>* >* enc, vector<vector<Cipher_elg>* >* C){
	int b;
	long i;
	//reads the values out of the file name
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	for (i = 0; i<n; i++){
		ist >>D_h_bar->at(i);
	}
	ist>>r_Dh_bar;

	for(i=0; i<n; i++){
		ist>>d_bar->at(i);
	}
	ist>> r_d_bar;
	for(i=0; i<n; i++){
		ist>> Delta_bar->at(i);
	}
	ist>>r_Delta_bar;

	for (i = 0; i<n; i++){
		ist >>A_bar->at(i);
	}
	ist>>r_A_bar;
	for(i=0; i<n; i++){
		ist>>Ds_bar->at(i);
	}
	ist>>r_Ds_bar;
	ist>>r_Dl_bar;

	for (i = 0; i<n; i++){
		ist >>B_bar->at(i);
	}
	ist>> r_B_bar;
	ist >>a_bar;
	ist >> r_a_bar;
	ist >> rho_bar;

	//Check that the D_hi's are constructed correctly
	b=func_ver::check_Dh_op(c_Dh, mul_chal_x8, D_h_bar, r_Dh_bar, omega_LL);
	if(b==1){
		//Check that matrix D is constructed correctly
		b=func_ver::check_D_op(c_D0, c_z, c_A, c_B, chal_x8, chal_y4, A_bar, r_A_bar, n);
		if(b==1){
			//Check that D_s is constructed correctly
			b=func_ver::check_Ds_op(c_Ds, c_Dh, c_Dm, chal_x6, chal_x8, Ds_bar, r_Ds_bar);
			if(b==1){
				//Check that the Dl's are correct
				b=func_ver::check_Dl_op(c_Dl, chal_x8, A_bar, Ds_bar, chal_y6, r_Dl_bar);
				if(b==1){
					//Check that vector d was constructed correctly
					b=func_ver::check_d_op(c_Dh, c_d, chal_x8, d_bar, r_d_bar);
					if(b==1){
						//Check that Deltas are constructed correctly
						b=func_ver::check_Delta_op(c_dh, c_Delta, chal_x8, Delta_bar, d_bar, r_Delta_bar, chal_x2, chal_z4, chal_y4);
						if(b==1 ){
							//Check that the commitments a_T contain the right values
							b=check_B();
							if(b==1){
								//Check that the reecncryption was done correctly
								b=check_a();
								if(b==1){
									//Check that E_c->at(mu-1) contains c and D->at(4) = C
									b = check_c(enc); //Both commitments shoud be com(0,0)
									if(b==1 & c_a->at(4)==c_a_c->at(3)){
										//Check correctness of the chiphertexts
										b =check_E(C);
										if(b==1 ){
											//Check the the reencryption of the E_c is correct
											b = check_ac();
											if(b==1){
												//cout<<"Accept!";
											}
										}
									}
								}
							}
						}
					}
				}
			}
		}
	}
	return to_ZZ(-1);
}

ZZ Verifier_toom:: round_10_red(string in_name,vector<vector<Cipher_elg>* >* enc, vector<vector<Cipher_elg>* >* C){
	int b;
	long i;
	string name;
	time_t rawtime;
	time ( &rawtime );

	//reads the values out of the file name
	ifstream ist(in_name.c_str());
	if(!ist) cout<<"Can't open "<< in_name;
	for (i = 0; i<n; i++){
		ist >>D_h_bar->at(i);
	}
	ist>>r_Dh_bar;

	for(i=0; i<n; i++){
		ist>>d_bar->at(i);
	}
	ist>> r_d_bar;
	for(i=0; i<n; i++){
		ist>> Delta_bar->at(i);
	}
	ist>>r_Delta_bar;

	for (i = 0; i<n; i++){
		ist >>A_bar->at(i);
	}
	ist>>r_A_bar;
	for(i=0; i<n; i++){
		ist>>Ds_bar->at(i);
	}
	ist>>r_Ds_bar;
	ist>>r_Dl_bar;

	for (i = 0; i<n; i++){
		ist >>B_bar->at(i);
	}
	ist>> r_B_bar;
	ist >>a_bar;
	ist >> r_a_bar;
	ist >> rho_bar;
	//Check that the Dhi's are constructed correctly
	b=func_ver::check_Dh_op(c_Dh, mul_chal_x8, D_h_bar, r_Dh_bar, omega_sw);
	if(b==1){
		//Check that matrix D is constructed correctly
		b=func_ver::check_D_op(c_D0, c_z, c_A, c_B, chal_x8, chal_y4, A_bar, r_A_bar, n);
		if(b==1){
			//Check that Ds is constructed correctly
			b=func_ver::check_Ds_op(c_Ds, c_Dh, c_Dm, chal_x6, chal_x8, Ds_bar, r_Ds_bar);
			if(b==1){
				//Check that the Dl's are correct
				b=func_ver::check_Dl_op(c_Dl, chal_x8, A_bar, Ds_bar, chal_y6, r_Dl_bar);
				if(b==1){
					//Check that vector d was constructed correctly
					b=func_ver::check_d_op(c_Dh, c_d, chal_x8, d_bar, r_d_bar);
					if(b==1){
						//Check that Deltas are constructed correctly
						b=func_ver::check_Delta_op(c_dh, c_Delta, chal_x8, Delta_bar, d_bar, r_Delta_bar, chal_x2, chal_z4, chal_y4);
						if(b==1 ){
							//Check that the commitments c_B contain the right values
							b=check_B_red();
							if(b==1){
								//Check that the reecncryption was done correctly
								b=check_a();
								if(b==1){
									// D->at(4) = C
									b = check_c_red(); //Both commitments shoud be com(0,0)
										if(b==1 & c_a->at(4)==c_a_c->at(3)){
										//Check correctness of the ciphertexts
										b =check_E_red(C);
										if(b==1 ){
											//Check the the reencryption of the c_c is correct
											b = check_ac();
											if(b==1){
												//cout<<"Accept!";
											}
											else{
												return to_ZZ(-1);
											}
										}
									}
								}
							}
						}
					}
				}
			}
		}
	}
	return to_ZZ(-1);
}

void Verifier_toom::calculate_c(Cipher_elg& c, vector<vector<Cipher_elg>* >* enc){
	long i, j;
	Cipher_elg temp;
	ZZ chal_temp;
	ZZ ord = H.get_ord();
	vector<ZZ>* v_chal=0;

	chal_temp=to_ZZ(1);
	c=Cipher_elg(1,1,H.get_mod());
	v_chal = new vector<ZZ>(n);
	for(i=0; i<m ; i++){
		for(j=0; j<n; j++){
			MulMod(chal_temp, chal_temp, chal_x2, ord);
			v_chal->at(j)=chal_temp;
		}
		multi_expo::expo_mult(temp, enc->at(i), v_chal, omega);
		Cipher_elg::mult(c,c,temp);
	}

	delete v_chal;
}

void Verifier_toom::calculate_ac(Mod_p& com){
	long i;
	Mod_p temp;

	com = c_a_c->at(0);
	for(i=1; i<mu_h; i++){
		Mod_p::expo(temp,c_a_c->at(i), x->at(i-1));
		Mod_p::mult(com, com, temp);
	}
}

void Verifier_toom::reduce_c_B(){
	long i,j;
	Mod_p temp, temp_1;

	for(i = 0; i<4*m_r; i++){
		temp = c_B->at(4*i);
		for(j = 1; j<mu; j++){
			Mod_p::expo(temp_1, c_B->at(4*i+j), x->at(j-1));
			Mod_p::mult(temp, temp, temp_1);
		}
		c_B_small->at(i)=temp;
	}
}

void Verifier_toom::calculate_C(Cipher_elg& C, vector<Cipher_elg>* C_c, vector<ZZ>* x){
	long i;
	ZZ t_1;
	ZZ ord = H.get_ord();
	Mod_p temp;
	Mod_p gen = H.get_gen();
	Cipher_elg temp_1;

	NegateMod(t_1, a_c_bar, ord);
	Mod_p::expo(temp, gen, t_1);
	C = El.encrypt(temp,to_ZZ(0));
	Cipher_elg::mult(C,C, C_c->at(0));
	for(i=1; i<mu_h; i++){
		Cipher_elg::expo(temp_1, C_c->at(i), x->at(i-1));
		Cipher_elg::mult(C,C, temp_1);
	}

}

int Verifier_toom::check_B(){
	long i,j;
	Mod_p temp, temp_1, t_B, co_B;
	ZZ mod = G.get_mod();
	vector<Mod_p>* c_B_small = new vector<Mod_p>(5);
	vector<Mod_p>* c_B_temp = new vector<Mod_p>(4);

	c_B_small->at(0)= c_B0;
	for(i = 0; i<m_r; i++){
		temp =  c_B->at(4*i);
		for(j = 1; j<4; j++){
			Mod_p::expo(temp_1, c_B->at(4*i+j), chal_x6->at(j-1));
			Mod_p::mult(temp, temp, temp_1);
		}
		c_B_small->at(i+1)=temp;
	}
	t_B = c_B_small->at(0);
	for(i=1; i<5;i++){
		Mod_p::expo(temp, c_B_small->at(i), chal_x8->at(i-1));
		Mod_p::mult(t_B, t_B, temp);
	}

	delete c_B_temp;
	delete c_B_small;

	co_B= Ped.commit_opt(B_bar,r_B_bar);
//	cout<<"B "<<t_B<<" "<<co_B<<endl;
	if(t_B == co_B){
		return 1;
	}
	return 0;
}

int Verifier_toom::check_B_red(){
	long i,j;
	Mod_p temp, temp_1, t_B, co_B;
	ZZ mod = G.get_mod();
	vector<Mod_p>* c_B_temp = new vector<Mod_p>(m_r+1);

	c_B_temp->at(0)= c_B0;
	for(i = 0; i<m_r; i++){
		temp =  c_B_small->at(4*i);
		for(j = 1; j<mu; j++){
			Mod_p::expo(temp_1, c_B_small->at(4*i+j), chal_x6->at(j-1));
			Mod_p::mult(temp, temp, temp_1);
		}
		c_B_temp->at(i+1)=temp;
	}
	t_B = c_B_temp->at(0);
	for(i=1; i<m_r+1;i++){
		Mod_p::expo(temp, c_B_temp->at(i), chal_x8->at(i-1));
		Mod_p::mult(t_B, t_B, temp);
	}
	delete c_B_temp;
	co_B= Ped.commit_opt(B_bar,r_B_bar);
//	cout<<"B "<<t_B<<endl;
//	cout<<"B "<<co_B<<endl;

	if(t_B == co_B){
		return 1;
	}
	return 0;
}

int Verifier_toom::check_a(){
	long i;
	Mod_p t_a, co_a;
	vector<ZZ>* chal_temp = new vector<ZZ>(8);

	chal_temp->at(0)=1;
	for(i=1; i<8; i++){
		chal_temp->at(i)=chal_x8->at(i-1);
	}
	multi_expo::multi_expo_LL(t_a, c_a, chal_temp, omega_sw);
	co_a = Ped.commit_sw(a_bar, r_a_bar);

	//cout<<"a "<<t_a<<" "<<co_a<<" "<<c_a->at(4)<<endl;
	delete chal_temp;
	if(t_a == co_a){
		return 1;
	}
	return 0;
}

int Verifier_toom::check_c(vector<vector<Cipher_elg>* >* enc){
	Cipher_elg c,C;

	calculate_c(c, enc);

	calculate_C(C,C_c, chal_x6);
//	cout<<"C "<<C_c->at(mu-1)<<" "<<c<<endl;
//	cout<<"C "<<E->at(4)<<" "<<C<<endl;
 	if(C_c->at(mu-1)==c & E->at(4)==C){
 		return 1;
 	}
 	return 0;
}

int Verifier_toom::check_c_red(){
	Cipher_elg C;

	calculate_C(C,C_c, chal_x6);
//	cout<<C<<endl;
//	cout<<E->at(4)<<endl;
 	if(E->at(4)==C){
 		return 1;
 	}
 	return 0;
}

int Verifier_toom::check_E(vector<vector<Cipher_elg>* >* C){
	long i,j;
	Mod_p temp;
	Mod_p gen = H.get_gen();
	Cipher_elg temp_1,  temp_2, t_D, c_D;
	vector<ZZ>* chal_1_temp= new vector<ZZ>(4);
	vector<ZZ>* chal_2_temp= new vector<ZZ>(4);
	vector<Cipher_elg>* row_C;

	for(i = 0; i<3; i++){
		chal_1_temp->at(i) = chal_x6->at(2-i);
	}
	chal_1_temp->at(3) = 1;

	for(i=0; i<m_r;i++){
		row_C = new vector<Cipher_elg>(n);
		for(j=0; j<n; j++){
			multi_expo::multi_expo_LL(row_C->at(j), C->at(4*i)->at(j), C->at(4*i+1)->at(j),C->at(4*i+2)->at(j),C->at(4*i+3)->at(j), chal_1_temp, omega_sw);
		}
		C_small->at(i)=row_C;
	}

	for(i=0; i<3;i++){
		chal_2_temp->at(i)=chal_x8->at(2-i);
	}
	chal_2_temp->at(3)=to_ZZ(1);

	Mod_p::expo(temp, gen,a_bar);
	temp_1 = El.encrypt(temp, rho_bar);
	multi_expo::expo_mult(temp_2, C_small, chal_2_temp, B_bar, omega);
	Cipher_elg::mult(c_D,temp_1,temp_2);
	//c_D=temp_1*temp_2;

	multi_expo::expo_mult(t_D, E, basis_chal_x8 , omega);

	delete chal_1_temp;
	delete chal_2_temp;
	Functions::delete_vector(C_small);
//	cout<<"E"<<t_D<<endl;
//	cout<<"E"<<c_D<<endl;
	if(t_D==c_D){
		return 1;
	}
	return 0;
}

int Verifier_toom::check_E_red(vector<vector<Cipher_elg>* >* C){
	long i,j,l;
	Mod_p temp;
	Mod_p gen = H.get_gen();
	Cipher_elg temp_1,  temp_2, t_D, c_D;
	vector<ZZ>* x_temp= new vector<ZZ>(4);
	vector<ZZ>* chal_1_temp= new vector<ZZ>(4);
	vector<ZZ>* chal_2_temp= new vector<ZZ>(4);
	vector<vector<Cipher_elg>* >* C_small_temp=0;
	vector<Cipher_elg>* row_C;

	for(i = 0; i<3; i++){
		x_temp->at(i) = x->at(2-i);
	}
	x_temp->at(3) = 1;

	l=mu*m_r;
	for(i=0; i<l;i++){
		row_C = new vector<Cipher_elg>(n);
		for(j=0; j<n; j++){
			multi_expo::multi_expo_LL(row_C->at(j), C->at(4*i)->at(j), C->at(4*i+1)->at(j),C->at(4*i+2)->at(j),C->at(4*i+3)->at(j), x_temp, omega_sw);
		}
		C_small->at(i)=row_C;
	}

	for(i = 0; i<3; i++){
		chal_1_temp->at(i) = chal_x6->at(2-i);
	}
	chal_1_temp->at(3) = 1;

	C_small_temp = new vector<vector<Cipher_elg>* >(m_r);
	for(i=0; i<m_r;i++){
		row_C = new vector<Cipher_elg>(n);
		for(j=0; j<n; j++){
			multi_expo::multi_expo_LL(row_C->at(j), C_small->at(4*i)->at(j), C_small->at(4*i+1)->at(j),C_small->at(4*i+2)->at(j),C_small->at(4*i+3)->at(j), chal_1_temp, omega);
		}
		C_small_temp->at(i)=row_C;
	}

	for(i=0; i<3;i++){
		chal_2_temp->at(i)=chal_x8->at(2-i);
	}
	chal_2_temp->at(3)=to_ZZ(1);

	Mod_p::expo(temp, gen,a_bar);
	temp_2 = El.encrypt(temp, rho_bar);
	multi_expo::expo_mult(temp_1, C_small_temp, chal_2_temp, B_bar, omega);
	Cipher_elg::mult(c_D,temp_1,temp_2);
	//c_D=temp_2*temp_1;

	multi_expo::expo_mult(t_D, E, basis_chal_x8 , omega);
	Functions::delete_vector(C_small);
	Functions::delete_vector(C_small_temp);
	delete chal_2_temp;
	delete chal_1_temp;
	delete x_temp;
	//cout<<"E"<<t_D<<endl;
	//cout<<"E"<<c_D<<endl;
	if(t_D ==c_D){
		return 1;
	}
	return 0;
}

int Verifier_toom::check_ac(){
	Mod_p t_a_c, co_a_c, temp;
	int i;

	t_a_c = c_a_c->at(0);
	for(i=1; i<7; i++){
		Mod_p::expo(temp, c_a_c->at(i), chal_x6->at(i-1));
		Mod_p::mult(t_a_c,t_a_c, temp);
	}
	co_a_c = Ped.commit_sw(a_c_bar, r_ac_bar);
//	cout<<"ac "<<t_a_c<<" "<<c_a_c;
	if(t_a_c ==co_a_c){
		return 1;
	}
	return 0;
}


#ifndef SHAPE_RETRIEVAL__VOCABULARY_H_
#define SHAPE_RETRIEVAL__VOCABULARY_H_

#include <Eigen/Core>
#include <fstream>
#include <cstdlib>
#include <iostream>
#include "opencv2/core.hpp"
#include "opencv2/core/eigen.hpp"

using namespace Eigen;
using namespace std;

#define FEATURE_DIM 64

void eigen2std(const MatrixXd &mat, vector<array<double, FEATURE_DIM>> &vec);
void std2eigen(const vector<array<double, FEATURE_DIM>> &vec, MatrixXd &mat);

class Vocabulary
{
public:
    Vocabulary() = default;
    Vocabulary(MatrixXd const& featureSamples, int vocabSize, int numberOfViews) {
        this->numberOfViews = numberOfViews;
        setSamples(featureSamples, vocabSize);
    };

    void setSamples(MatrixXd const& featureSamples, int vocabSize);
    void setNViews(int numberOfViews) {this->numberOfViews = numberOfViews; };
    void setCentroids(MatrixXd const& centroids){this->centroids = centroids; };
    void setFrequecies(VectorXd const& frequencies){this->frequencies = frequencies; };
    void setVocabFromFile(string vocabPath, size_t vocabSize);

    int getNumberOfViews() {return this->numberOfViews; };
    VectorXd getFrequecies() {return this->frequencies; }
    MatrixXd getVocabulary() {return this->centroids; };

private:
    int numberOfViews;
    VectorXd frequencies;
    MatrixXd centroids;
};

#endif //SHAPE_RETRIEVAL__VOCABULARY_H_


#include <gtest/gtest.h>

#include <scheme/search/SpatialBandB.hh>

#include <scheme/actor/BackboneActor.hh>
#include <scheme/kinematics/Scene.hh>
#include <scheme/objective/ObjectiveFunction.hh>

#include <Eigen/Geometry>

#include <boost/mpl/vector.hpp>

namespace scheme { namespace search { namespace spbbtest {

using std::cout;
using std::endl;

typedef float Float;
typedef Eigen::Transform<Float,3,Eigen::AffineCompact> Xform;
typedef actor::BackboneActor<Xform> BBActor;
typedef Eigen::Matrix<Float,3,1> Vec;

// why is this here?
struct TestScoreBBactor {
	typedef double Result;
	typedef BBActor Interaction;
	static std::string name(){ return "TestScoreBBactor"; }
	Xform target_inv_;
	Float radius_, lever_;
	TestScoreBBactor() : target_inv_(Xform::Identity()), radius_(1.0), lever_(10.0) {}
	TestScoreBBactor( Xform target, Float rad, Float lever ) : target_inv_(target.inverse()), radius_(rad), lever_(lever) {}
	template<class Config>
	Result operator()(BBActor const & a, Config const& c) const {
		// for( int i = 0; i < 12; ++i ){
			// cout << i << " " << target_inv_.data()[i] << endl;
			// cout << i << " " << a.position().data()[i] << endl;
		// }
		// cout << a.position().rotation().trace() << " " << target_inv_.rotation().trace() << endl;
		// cout << target_inv_.inverse().translation().transpose() << endl;
		// cout << a.position().translation().transpose() << endl;
		Xform xrel = target_inv_ * a.position();
		// cout << xrel.translation().transpose() << endl;
		// cout << (xrel.rotation().trace()-1.0)/2.0 << std::endl;
		Float err2 = xrel.translation().squaredNorm();
		Float trace = xrel.data()[0] + xrel.data()[4] +  xrel.data()[8];
		Float roterr = ( 1.0 - ( trace - 1.0 ) / 2.0 ) * lever_;
		// cout << roterr << " " << ( ( trace - 1.0 ) / 2.0 ) << " " << acos(( ( trace - 1.0 ) / 2.0 ) )*180.0/3.14159 << endl;
		roterr = roterr < 0.0 ? 9e9 : roterr;
		Float toterr = err2+roterr*roterr;
		return (toterr < radius_*radius_ ) ? -1.0 : 0.0 ;
	}
};

TEST( SpatialBandB, test_test_utils ){
	typedef kinematics::Scene< ::scheme::kinematics::impl::Conformation< boost::mpl::vector< BBActor > > , Xform > Scene;
	typedef objective::ObjectiveFunction< boost::mpl::vector< TestScoreBBactor >, int > Objective;

	Objective obj;
	obj.get_objective<TestScoreBBactor>().lever_ = 10.0;
	obj.get_objective<TestScoreBBactor>().radius_ = 1.0;

	{
		Xform x( Xform::Identity() );
		Scene s;
		s.add_body();
		BBActor actor;
		s.add_actor( 0, actor );
		ASSERT_FLOAT_EQ( -1.0, obj(s).sum() );
		{
			x.translation() = Vec(0.99,0,0);
			s.set_position( 0, x );
			ASSERT_FLOAT_EQ( -1.0, obj(s).sum() );
		}
		{
			x.translation() = Vec(1.01,0,0);
			s.set_position( 0, x );
			ASSERT_FLOAT_EQ(  0.0, obj(s).sum() );
		}
		{
			x = Xform::Identity();
			x.rotate( Eigen::AngleAxis<Float>( 25.84/180.0*3.14159, Vec(1,0,0) ) );
			s.set_position( 0, x );
			ASSERT_FLOAT_EQ( -1.0, obj(s).sum() );
		}
		{
			x = Xform::Identity();
			x.rotate( Eigen::AngleAxis<Float>( 25.85/180.0*3.14159, Vec(1,0,0) ) );
			s.set_position( 0, x );
			ASSERT_FLOAT_EQ(  0.0, obj(s).sum() );
		}

		Xform target( Xform::Identity() );
		target.translation() = Vec(1,2,3);
		obj.get_objective<TestScoreBBactor>().target_inv_ = target.inverse();
		{
			x = Xform::Identity();
			x.translation() = Vec(1,2,3);
			s.set_position( 0, x );
			ASSERT_FLOAT_EQ( -1.0, obj(s).sum() );
		}
		{
			x = Xform::Identity();
			x.translation() = Vec(3,2,1);
			s.set_position( 0, x );
			ASSERT_FLOAT_EQ(  0.0, obj(s).sum() );
		}
		{
			x = Xform::Identity();
			x.translation() = Vec(1,2,3.0);
			x.rotate( Eigen::AngleAxis<Float>( 25.84/180.0*3.14159, Vec(1,0,0) ) );
			s.set_position( 0, x );
			ASSERT_FLOAT_EQ( -1.0, obj(s).sum() );
		}
		{
			x = Xform::Identity();
			x.translation() = Vec(1,2,3);
			x.rotate( Eigen::AngleAxis<Float>( 25.85/180.0*3.14159, Vec(1,0,0) ) );
			s.set_position( 0, x );
			ASSERT_FLOAT_EQ(  0.0, obj(s).sum() );
		}
		{
			obj.get_objective<TestScoreBBactor>().lever_ = 60;
			x = Xform::Identity();
			x.translation() = Vec(1,2,3);
			x.rotate( Eigen::AngleAxis<Float>( 25.85/180.0*3.14159, Vec(1,0,0) ) );
			s.set_position( 0, x );
			ASSERT_FLOAT_EQ(  0.0, obj(s).sum() );
		}

	}

	// BoundingObjectiveFunction<

}

}}}


// Copyright Louis Dionne 2013-2017
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/concept/comparable.hpp>
#include <boost/hana/concept/euclidean_ring.hpp>
#include <boost/hana/concept/group.hpp>
#include <boost/hana/concept/monoid.hpp>
#include <boost/hana/concept/orderable.hpp>
#include <boost/hana/concept/ring.hpp>
#include <boost/hana/integral_constant.hpp>
namespace hana = boost::hana;

using T = decltype(hana::int_c<1>);

static_assert(hana::Comparable<T>::value, "");
static_assert(hana::Orderable<T>::value, "");
static_assert(hana::Monoid<T>::value, "");
static_assert(hana::Group<T>::value, "");
static_assert(hana::Ring<T>::value, "");
static_assert(hana::EuclideanRing<T>::value, "");

int main() { }


#include "../kontsevich_graph_series.hpp"
#include <ginac/ginac.h>
#include <iostream>
#include <fstream>
#include <limits>
#include "../util/continued_fraction.hpp"
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include <Eigen/SparseQR>
#include <Eigen/OrderingMethods>
using namespace std;
using namespace GiNaC;

typedef Eigen::Triplet<double> Triplet;
typedef Eigen::SparseMatrix<double> SparseMatrix;

double threshold = 1e-5;

int main(int argc, char* argv[])
{
    if (argc != 2 && argc != 3 && argc != 4 && argc != 5)
    {
        cout << "Usage: " << argv[0] << " <graph-series-filename> [max-jacobiators] [max-jac-indegree] [--solve]\n\n"
             << "Accepts only homogeneous power series: graphs with n internal vertices at order n.\n"
             << "The optional arguments [max-jacobiators] and [max-jac-indegree] restrict the types of differential consequences of Jacobi taken into account:\n"
             << "- [max-jacobiators] restricts the number of Jacobiators per differential consequence, while\n"
             << "- [max-jac-indegree] restricts the number of arrows falling on Jacobiators.\n"
             << "When the optional argument [--solve] is specified, the undetermined variables in the input are added to the linear system to-be-solved.\n";
        return 1;
    }

    size_t max_jacobiators = numeric_limits<size_t>::max();
    size_t max_jac_indegree = numeric_limits<size_t>::max();
    if (argc == 3 || argc == 4 || argc == 5)
        max_jacobiators = stoi(argv[2]);
    if (argc == 4 || argc == 5)
        max_jac_indegree = stoi(argv[3]);

    // Reading in graph series
    string graph_series_filename(argv[1]);
    ifstream graph_series_file(graph_series_filename);
    parser coefficient_reader;
    bool homogeneous = true;
    map<size_t, set< vector<size_t> > > in_degrees;
    KontsevichGraphSeries<ex> graph_series = KontsevichGraphSeries<ex>::from_istream(graph_series_file,
        [&coefficient_reader](std::string s) -> ex { return coefficient_reader(s); },
        [&homogeneous, &in_degrees](KontsevichGraph graph, size_t order) -> bool
                                   {
                                       in_degrees[order].insert(graph.in_degrees());
                                       return homogeneous &= graph.internal() == order;
                                   }
    );
    size_t order = graph_series.precision();
    if (!homogeneous)
    {
        cerr << "Only accepting homogeneous power series: graphs with n internal vertices at order n.\n";
        return 1;
    }

    // Find number of external vertices
    // TODO: move to method in KontsevichGraphSum / KontsevichGraphSeries?
    size_t external = 0;
    for (size_t n = 0; n <= order; ++n)
    {
        if (graph_series[n].size() != 0)
        {
            external = graph_series[n].front().second.external();
            break;
        }
    }

    graph_series.reduce_mod_skew();

    size_t counter = 0;
    std::vector<symbol> coefficient_list;

    if (argc == 5 && string(argv[4]) == "--solve")
        for (auto namevar : coefficient_reader.get_syms())
            coefficient_list.push_back(ex_to<symbol>(namevar.second));

    std::map<symbol, KontsevichGraph, ex_is_less> kontsevich_jacobi_leibniz_graphs;

    for (size_t n = 2; n <= order; ++n) // need at least 2 internal vertices for Jacobi
    {
        if (graph_series[n].size() == 0)
            continue;

        cout << "h^" << n << ":\n";

        // First we choose the target vertices i,j,k of the Jacobiators (which contain 2 bivectors), in increasing order (without loss of generality)

        // Jacobi must have three distinct arguments, and not act on itself, but can act on other Jacobi

        for (size_t k = 1; k <= min(n/2, max_jacobiators); ++k)
        {
            std::vector<size_t> jacobi_vertices(3*k, n + external);
            CartesianProduct jacobi_indices(jacobi_vertices);
            for (auto jacobi_index = jacobi_indices.begin(); jacobi_index != jacobi_indices.end(); ++jacobi_index)
            {
                bool accept = true;
                for (size_t i = 0; i != k; ++i)
                {
                    if ((*jacobi_index)[i*3] >= (*jacobi_index)[i*3 + 1] || (*jacobi_index)[i*3 + 1] >= (*jacobi_index)[i*3 + 2]) // not strictly increasing
                    {
                        accept = false;
                        break;
                    }
                }
                if (!accept)
                    continue;

                // Then we choose the target vertices of the remaining n - 2*k bivectors, stored in a multi-index of length 2*(n-2*k)
                // Here we have k fewer possible targets: out of the last 2*k internal vertices, the first k act as placeholders for the respective Jacobiators,
                // to be replaced by the Leibniz rule later on
                std::vector<size_t> remaining_edges(2*(n-2*k), n + external - k);
                CartesianProduct indices(remaining_edges);
                for (auto multi_index = indices.begin(); multi_index != indices.end(); ++multi_index)
                {
                    bool accept = true;
                    for (size_t idx = 0; idx != n - 2*k; ++idx)
                    {
                        if ((*multi_index)[2*idx] >= (*multi_index)[2*idx+1])
                        {
                            accept = false; // accept only strictly increasing indices
                            break;
                        }
                        // TODO: filter out tadpoles, maybe?
                    }
                    if (!accept)
                        continue;

                    // We build the list of targets for the graph, as described above (using i,j,k and the multi-index)
                    std::vector<KontsevichGraph::VertexPair> targets(n);
                    // first part:
                    for (size_t idx = 0; idx != n - 2*k; ++idx)
                        targets[idx] = {(*multi_index)[2*idx], (*multi_index)[2*idx+1]};
                    // second part:
                    for (size_t i = 0; i != k; ++i)
                    {
                        targets[n - 2*k + 2*i].first = KontsevichGraph::Vertex((*jacobi_index)[3*i]);
                        targets[n - 2*k + 2*i].second = KontsevichGraph::Vertex((*jacobi_index)[3*i + 1]);
                        targets[n - 2*k + 2*i + 1].first = KontsevichGraph::Vertex(n + external - 2*k + 2*i);
                        targets[n - 2*k + 2*i + 1].second = KontsevichGraph::Vertex((*jacobi_index)[3*i + 2]);
                    }

                    KontsevichGraph template_graph(n, external, targets, 1, true);

                    vector<size_t> indegrees = template_graph.in_degrees();
                    if (in_degrees[n].find(indegrees) == in_degrees[n].end()) // skip terms
                        continue;

                    // Make vector of references to bad targets: those in first part with target >= (n + external - 2*k), the placeholders for the Jacobiators:
                    std::map<KontsevichGraph::Vertex*, int> bad_targets;
                    for (size_t idx = 0; idx != n - 2*k; ++idx) // look for bad targets in first part
                    {
                        if ((int)targets[idx].first >= (int)n + (int)external - 2*(int)k)
                            bad_targets[&targets[idx].first] = (int)targets[idx].first - (n + external - 2*k);
                        if ((int)targets[idx].second >= (int)n + (int)external - 2*(int)k)
                            bad_targets[&targets[idx].second] = (int)targets[idx].second - (n + external - 2*k);
                    }

                    // Count number of arrows falling on Jacs:
                    std::map<int, size_t> in_degree;
                    bool acceptable = true;
                    for (auto pair : bad_targets)
                    {
                        if (++in_degree[pair.second] == max_jac_indegree + 1)
                        {
                            acceptable = false;
                            break;
                        }
                    }
                    if (!acceptable)
                        continue;

                    KontsevichGraphSum<ex> graph_sum;

                    // Replace bad targets by Leibniz rule:
                    symbol coefficient("c_" + to_string(k) + "_" + to_string(counter));
                    std::vector<size_t> leibniz_sizes(bad_targets.size(), 2);
                    CartesianProduct leibniz_indices(leibniz_sizes);
                    for (auto leibniz_index = leibniz_indices.begin(); leibniz_index != leibniz_indices.end(); ++leibniz_index)
                    {
                        size_t idx = 0;
                        for (auto& bad_target : bad_targets)
                            *(bad_target.first) = KontsevichGraph::Vertex(external + n - 2*k + 2*(bad_target.second) + (*leibniz_index)[idx++]);

                        for (size_t i = 0; i != k; ++i)
                        {
                            for (auto jacobi_targets_choice : std::vector< std::vector<KontsevichGraph::Vertex> >({ { targets[n-2*k+2*i].first, targets[n-2*k+2*i].second, targets[n-2*k+2*i+1].second },
                                                                                { targets[n-2*k+2*i].second, targets[n-2*k+2*i+1].second, targets[n-2*k+2*i].first },
                                                                                { targets[n-2*k+2*i+1].second, targets[n-2*k+2*i].first, targets[n-2*k+2*i].second } }))
                            {
                                // Set Jacobiator targets to one of the three permutatations
                                targets[n-2*k+2*i].first = jacobi_targets_choice[0];
                                targets[n-2*k+2*i].second = jacobi_targets_choice[1];
                                targets[n-2*k+2*i+1].second = jacobi_targets_choice[2];

                                KontsevichGraph graph(n, external, targets);

                                graph_sum += KontsevichGraphSum<ex>({ { coefficient, graph } });
                            }
                        }
                    }

                    graph_sum.reduce_mod_skew();
                    if (graph_sum.size() != 0)
                    {
                        cerr << "\r" << ++counter;
                        coefficient_list.push_back(coefficient);
                        kontsevich_jacobi_leibniz_graphs[coefficient] = template_graph;
                    }
                    graph_series[n] -= graph_sum;
                }
            }
        }
    }

    cout << "\nNumber of coefficients: " << coefficient_list.size() << "\n";
    cout << "\nNumber of terms: " << graph_series[order].size() << "\n";
    cout << "\nNumber of terms per coefficient: " << (float)graph_series[order].size()/coefficient_list.size() << "\n";

    cout.flush();

    cerr << "\nReducing...\n";
    graph_series.reduce_mod_skew();

    lst equations;

    for (size_t n = 0; n <= order; ++n)
        for (auto& term : graph_series[n])
        {
            cerr << term.second.encoding() << "    " << term.first << "==0\n";
            equations.append(term.first);
        }

    // Set up sparse matrix linear system

    cerr << "Setting up linear system for numerical solution...\n";
    size_t rows = equations.nops();
    size_t cols = coefficient_list.size();

    Eigen::VectorXd b(rows);
    SparseMatrix matrix(rows,cols);

    std::vector<Triplet> tripletList;
    size_t idx = 0;
    for (ex equation : equations)
    {
        if (!is_a<add>(equation))
            equation = lst({ equation });
        for (ex term : equation)
        {
            if (!is_a<mul>(term))
                term = lst({ term });
            double prefactor = 1;
            symbol coefficient("one");
            for (ex factor : term)
            {
                if (is_a<numeric>(factor))
                    prefactor *= ex_to<numeric>(factor).to_double();
                else if (is_a<symbol>(factor))
                    coefficient = ex_to<symbol>(factor);
            }
            if (coefficient.get_name() == "one") // constant term
                b(idx) = -prefactor;
            else
                tripletList.push_back(Triplet(idx,find(coefficient_list.begin(), coefficient_list.end(), coefficient) - coefficient_list.begin(), prefactor));
                // NB: Eigen uses zero-based indices (contrast MATLAB, Mathematica)
        }
        ++idx;
    }

    matrix.setFromTriplets(tripletList.begin(), tripletList.end());

    cerr << "Solving linear system numerically...\n";

    Eigen::SparseQR< SparseMatrix, Eigen::COLAMDOrdering<int> > qr(matrix);
    Eigen::VectorXd x = qr.solve(b);

    cerr << "Residual norm = " << (matrix * x - b).squaredNorm() << "\n";

    cerr << "Rounding...\n";
    x = x.unaryExpr([](double elem) { return fabs(elem) < threshold ? 0.0 : elem; });

    cerr << "Still a solution? Residual norm = " << (matrix * x - b).squaredNorm() << "\n";

    cerr << "Approximating numerical solution by rational solution...\n";

    lst zero_substitution;
    lst solution_substitution;
    for (int i = 0; i != x.size(); i++)
    {
        ex result = best_rational_approximation(x.coeff(i), threshold);
        if (result == 0)
            zero_substitution.append(coefficient_list[i] == 0);
        else
            solution_substitution.append(coefficient_list[i] == result);
    }

    cerr << "Substituting zeros...\n";

    for (auto& order: graph_series)
        for (auto& term : graph_series[order.first])
            term.first = term.first.subs(zero_substitution);

    cerr << "Reducing zeros...\n";

    graph_series.reduce_mod_skew();

    for (size_t n = 0; n <= graph_series.precision(); ++n)
    {
        cout << "h^" << n << ":\n";
        for (auto& term : graph_series[n])
        {
            cout << term.second.encoding() << "    " << term.first << "\n";
        }
    }

    cerr << "Verifying solution...\n";

    for (auto& order: graph_series)
        for (auto& term : graph_series[order.first])
            term.first = term.first.subs(solution_substitution);

    graph_series.reduce_mod_skew();

    cout << "Do we really have a solution? " << (graph_series == 0 ? "Yes" : "No") << "\n";

    if (graph_series == 0)
    {
        for (ex subs : solution_substitution)
            cout << kontsevich_jacobi_leibniz_graphs[ex_to<symbol>(subs.lhs())].encoding() << "    " << subs << "\n";
    }
}


//
//  Copyright (c) 2009-2011 Artyom Beilis (Tonkikh)
//
//  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)
//
#define BOOST_LOCALE_SOURCE
#include <boost/config.hpp>
#ifdef BOOST_MSVC
#pragma warning(disable : 4996)
#endif
#include "gregorian.hpp"
#include "timezone.hpp"

#include <stdlib.h>

#include <algorithm>
#include <boost/locale/date_time.hpp>
#include <boost/locale/date_time_facet.hpp>
#include <boost/locale/hold_ptr.hpp>
#include <ctime>
#include <ios>
#include <limits>
#include <locale>
#include <memory>
#include <string>

namespace boost
{
namespace locale
{
namespace util
{
namespace
{

int is_leap(int year)
{
    if (year % 400 == 0)
        return 1;
    if (year % 100 == 0)
        return 0;
    if (year % 4 == 0)
        return 1;
    return 0;
}

int days_in_month(int year, int month)
{
    static const int tbl[2][12] = {
        {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31},
        {31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}};
    return tbl[is_leap(year)][month - 1];
}

inline int days_from_0(int year)
{
    year--;
    return 365 * year + (year / 400) - (year / 100) + (year / 4);
}

int days_from_1970(int year)
{
    static const int days_from_0_to_1970 = days_from_0(1970);
    return days_from_0(year) - days_from_0_to_1970;
}

int days_from_1jan(int year, int month, int day)
{
    static const int days[2][12] = {
        {0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334},
        {0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335}};
    return days[is_leap(year)][month - 1] + day - 1;
}

std::time_t internal_timegm(std::tm const* t)
{
    int year = t->tm_year + 1900;
    int month = t->tm_mon;
    if (month > 11)
    {
        year += month / 12;
        month %= 12;
    }
    else if (month < 0)
    {
        int years_diff = (-month + 11) / 12;
        year -= years_diff;
        month += 12 * years_diff;
    }
    month++;
    int day = t->tm_mday;
    int day_of_year = days_from_1jan(year, month, day);
    int days_since_epoch = days_from_1970(year) + day_of_year;

    std::time_t seconds_in_day = 3600 * 24;
    std::time_t result = seconds_in_day * days_since_epoch + 3600 * t->tm_hour +
                         60 * t->tm_min + t->tm_sec;

    return result;
}

} // namespace

namespace
{

// Locale dependent data

bool comparator(char const* left, char const* right)
{
    return strcmp(left, right) < 0;
}

//
// Ref: CLDR 1.9 common/supplemental/supplementalData.xml
//
// monday - default
// fri - MV
// sat - AE AF BH DJ DZ EG ER ET IQ IR JO KE KW LY MA OM QA SA SD SO SY TN YE
// sun - AR AS AZ BW CA CN FO GE GL GU HK IL IN JM JP KG KR LA MH MN MO MP MT NZ
// PH PK SG TH TT TW UM US UZ VI ZW
//

int first_day_of_week(char const* terr)
{
    static char const* const sat[] = {
        "AE", "AF", "BH", "DJ", "DZ", "EG", "ER", "ET", "IQ", "IR", "JO", "KE",
        "KW", "LY", "MA", "OM", "QA", "SA", "SD", "SO", "SY", "TN", "YE"};
    static char const* const sunday[] = {
        "AR", "AS", "AZ", "BW", "CA", "CN", "FO", "GE", "GL", "GU", "HK", "IL",
        "IN", "JM", "JP", "KG", "KR", "LA", "MH", "MN", "MO", "MP", "MT", "NZ",
        "PH", "PK", "SG", "TH", "TT", "TW", "UM", "US", "UZ", "VI", "ZW"};
    if (strcmp(terr, "MV") == 0)
        return 5; // fri
    if (std::binary_search<char const* const*>(
            sat, sat + sizeof(sat) / (sizeof(sat[0])), terr, comparator))
        return 6; // sat
    if (std::binary_search<char const* const*>(
            sunday, sunday + sizeof(sunday) / (sizeof(sunday[0])), terr,
            comparator))
        return 0; // sun
    // default
    return 1; // mon
}
} // namespace

class gregorian_calendar : public abstract_calendar
{
  public:
    gregorian_calendar(std::string const& terr)
    {
        first_day_of_week_ = first_day_of_week(terr.c_str());
        time_ = std::time(0);
        is_local_ = true;
        tzoff_ = 0;
        from_time(time_);
    }

    ///
    /// Make a polymorphic copy of the calendar
    ///
    virtual gregorian_calendar* clone() const
    {
        return new gregorian_calendar(*this);
    }

    ///
    /// Set specific \a value for period \a p, note not all values are settable.
    ///
    virtual void set_value(period::marks::period_mark p, int value)
    {
        using namespace period::marks;
        switch (p)
        {
            case era: ///< Era i.e. AC, BC in Gregorian and Julian calendar,
                      ///< range [0,1]
                return;
            case year:          ///< Year, it is calendar specific
            case extended_year: ///< Extended year for Gregorian/Julian
                                ///< calendars, where 1 BC == 0, 2 BC == -1.
                tm_updated_.tm_year = value - 1900;
                break;
            case month:
                tm_updated_.tm_mon = value;
                break;
            case day:
                tm_updated_.tm_mday = value;
                break;
            case hour: ///< 24 clock hour [0..23]
                tm_updated_.tm_hour = value;
                break;
            case hour_12: ///< 12 clock hour [0..11]
                tm_updated_.tm_hour = tm_updated_.tm_hour / 12 * 12 + value;
                break;
            case am_pm: ///< am or pm marker, [0..1]
                tm_updated_.tm_hour = 12 * value + tm_updated_.tm_hour % 12;
                break;
            case minute: ///< minute [0..59]
                tm_updated_.tm_min = value;
                break;
            case second:
                tm_updated_.tm_sec = value;
                break;
            case day_of_year:
                normalize();
                tm_updated_.tm_mday += (value - (tm_updated_.tm_yday + 1));
                break;
            case day_of_week:  ///< Day of week, starting from Sunday, [1..7]
                if (value < 1) // make sure it is positive
                    value += (-value / 7) * 7 + 7;
                // convert to local DOW
                value = (value - 1 - first_day_of_week_ + 14) % 7 + 1;
                // fall throght
            case day_of_week_local: ///< Local day of week, for example in
                                    ///< France Monday is 1, in US Sunday is 1,
                                    ///< [1..7]
                normalize();
                tm_updated_.tm_mday +=
                    (value - 1) -
                    (tm_updated_.tm_wday - first_day_of_week_ + 7) % 7;
                break;
            case day_of_week_in_month: ///< Original number of the day of the
                                       ///< week in month. (1st sunday, 2nd
                                       ///< sunday etc)
            case week_of_year:  ///< The week number in the year, 4 is the
                                ///< minimal number of days to be in month
            case week_of_month: ///< The week number withing current month
            {
                normalize();
                int current_week = get_value(p, current);
                int diff = 7 * (value - current_week);
                tm_updated_.tm_mday += diff;
            }
            break;
            case period::marks::first_day_of_week: ///< For example Sunday in
                                                   ///< US, Monday in France
            default:
                return;
        }
        normalized_ = false;
    }

    void normalize()
    {
        if (!normalized_)
        {
            std::tm val = tm_updated_;
            val.tm_isdst = -1;
            val.tm_wday = -1; // indecator of error
            std::time_t point = -1;
            if (is_local_)
            {
                point = std::mktime(&val);
                if (point == static_cast<std::time_t>(-1))
                {
#ifndef BOOST_WINDOWS
                    // windows does not handle negative time_t, under other
                    // plaforms it may be actually valid value in  1969-12-31
                    // 23:59:59 so we check that a filed was updated - does not
                    // happen in case of error
                    if (val.tm_wday == -1)
#endif
                    {
                        throw date_time_error(
                            "boost::locale::gregorian_calendar: invalid time");
                    }
                }
            }
            else
            {
                point = internal_timegm(&val);
#ifdef BOOST_WINDOWS
                // Windows uses TLS, thread safe
                std::tm* revert_point = 0;
                if (point < 0 || (revert_point = gmtime(&point)) == 0)
                    throw date_time_error("boost::locale::gregorian_calendar "
                                          "time is out of range");
                val = *revert_point;
#else
                if (!gmtime_r(&point, &val))
                    throw date_time_error(
                        "boost::locale::gregorian_calendar invalid time");
#endif
            }

            time_ = point - tzoff_;
            tm_ = val;
            tm_updated_ = val;
            normalized_ = true;
        }
    }

    int get_week_number(int day, int wday) const
    {
        ///
        /// This is the number of days that are considered withing
        /// period such that the week belongs there
        ///
        static const int days_in_full_week = 4;

        // Alaways use local week start
        int current_dow = (wday - first_day_of_week_ + 7) % 7;
        // Calculate local week day of Jan 1st.
        int first_week_day = (current_dow + 700 - day) % 7;
        // adding something big devidable by 7

        int start_of_period_in_weeks;
        if (first_week_day < days_in_full_week)
        {
            start_of_period_in_weeks = -first_week_day;
        }
        else
        {
            start_of_period_in_weeks = 7 - first_week_day;
        }
        int week_number_in_days = day - start_of_period_in_weeks;
        if (week_number_in_days < 0)
            return -1;
        return week_number_in_days / 7 + 1;
    }

    ///
    /// Get specific value for period \a p according to a value_type \a v
    ///
    virtual int get_value(period::marks::period_mark p, value_type v) const
    {
        using namespace period::marks;
        switch (p)
        {
            case era:
                return 1;
            case year:
            case extended_year:
                switch (v)
                {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
#ifdef BOOST_WINDOWS
                        return 1970; // Unix epoch windows can't handle negative
                                     // time_t
#else
                        if (sizeof(std::time_t) == 4)
                            return 1901; // minimal year with 32 bit time_t
                        else
                            return 1;
#endif
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        if (sizeof(std::time_t) == 4)
                            return 2038; // Y2K38 - maximal with 32 bit time_t
                        else
                            return std::numeric_limits<int>::max();
                    case current:
                        return tm_.tm_year + 1900;
                };
                break;
            case month:
                switch (v)
                {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 0;
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        return 11;
                    case current:
                        return tm_.tm_mon;
                };
                break;
            case day:
                switch (v)
                {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 1;
                    case absolute_maximum:
                        return 31;
                    case least_maximum:
                        return 28;
                    case actual_maximum:
                        return days_in_month(tm_.tm_year + 1900,
                                             tm_.tm_mon + 1);
                    case current:
                        return tm_.tm_mday;
                };
                break;
            case day_of_year: ///< The number of day in year, starting from 1
                switch (v)
                {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 1;
                    case absolute_maximum:
                        return 366;
                    case least_maximum:
                        return 365;
                    case actual_maximum:
                        return is_leap(tm_.tm_year + 1900) ? 366 : 365;
                    case current:
                        return tm_.tm_yday + 1;
                }
                break;
            case day_of_week: ///< Day of week, starting from Sunday, [1..7]
                switch (v)
                {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 1;
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        return 7;
                    case current:
                        return tm_.tm_wday + 1;
                }
                break;
            case day_of_week_local: ///< Local day of week, for example in
                                    ///< France Monday is 1, in US Sunday is 1,
                                    ///< [1..7]
                switch (v)
                {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 1;
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        return 7;
                    case current:
                        return (tm_.tm_wday - first_day_of_week_ + 7) % 7 + 1;
                }
                break;
            case hour: ///< 24 clock hour [0..23]
                switch (v)
                {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 0;
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        return 23;
                    case current:
                        return tm_.tm_hour;
                }
                break;
            case hour_12: ///< 12 clock hour [0..11]
                switch (v)
                {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 0;
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        return 11;
                    case current:
                        return tm_.tm_hour % 12;
                }
                break;
            case am_pm: ///< am or pm marker, [0..1]
                switch (v)
                {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 0;
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        return 1;
                    case current:
                        return tm_.tm_hour >= 12 ? 1 : 0;
                }
                break;
            case minute: ///< minute [0..59]
                switch (v)
                {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 0;
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        return 59;
                    case current:
                        return tm_.tm_min;
                }
                break;
            case second: ///< second [0..59]
                switch (v)
                {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 0;
                    case absolute_maximum:
                    case least_maximum:
                    case actual_maximum:
                        return 59;
                    case current:
                        return tm_.tm_sec;
                }
                break;
            case period::marks::first_day_of_week: ///< For example Sunday in
                                                   ///< US, Monday in France
                return first_day_of_week_ + 1;

            case week_of_year: ///< The week number in the year
                switch (v)
                {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 1;
                    case absolute_maximum:
                        return 53;
                    case least_maximum:
                        return 52;
                    case actual_maximum:
                    {
                        int year = tm_.tm_year + 1900;
                        int end_of_year_days = (is_leap(year) ? 366 : 365) - 1;
                        int dow_of_end_of_year =
                            (end_of_year_days - tm_.tm_yday + tm_.tm_wday) % 7;
                        return get_week_number(end_of_year_days,
                                               dow_of_end_of_year);
                    }
                    case current:
                    {
                        int val = get_week_number(tm_.tm_yday, tm_.tm_wday);
                        if (val < 0)
                            return 53;
                        return val;
                    }
                }
            case week_of_month: ///< The week number withing current month
                switch (v)
                {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 1;
                    case absolute_maximum:
                        return 5;
                    case least_maximum:
                        return 4;
                    case actual_maximum:
                    {
                        int end_of_month_days =
                            days_in_month(tm_.tm_year + 1900, tm_.tm_mon + 1);
                        int dow_of_end_of_month =
                            (end_of_month_days - tm_.tm_mday + tm_.tm_wday) % 7;
                        return get_week_number(end_of_month_days,
                                               dow_of_end_of_month);
                    }
                    case current:
                    {
                        int val = get_week_number(tm_.tm_mday, tm_.tm_wday);
                        if (val < 0)
                            return 5;
                        return val;
                    }
                }

            case day_of_week_in_month: ///< Original number of the day of the
                                       ///< week in month.
                switch (v)
                {
                    case absolute_minimum:
                    case greatest_minimum:
                    case actual_minimum:
                        return 1;
                    case absolute_maximum:
                        return 5;
                    case least_maximum:
                        return 4;
                    case actual_maximum:
                        if (tm_.tm_mon == 1 && !is_leap(tm_.tm_year + 1900))
                        {
                            // only in february in non leap year is 28 days, the
                            // rest conver more then 4 weeks
                            return 4;
                        }
                        return 5;
                    case current:
                        return (tm_.tm_mday - 1) / 7 + 1;
                    default:;
                }
            default:;
        }
        return 0;
    }

    ///
    /// Set current time point
    ///
    virtual void set_time(posix_time const& p)
    {
        from_time(static_cast<std::time_t>(p.seconds));
    }
    virtual posix_time get_time() const
    {
        posix_time pt = {time_, 0};
        return pt;
    }

    ///
    /// Set option for calendar, for future use
    ///
    virtual void set_option(calendar_option_type opt, int /*v*/)
    {
        switch (opt)
        {
            case is_gregorian:
                throw date_time_error(
                    "is_gregorian is not settable options for calendar");
            case is_dst:
                throw date_time_error(
                    "is_dst is not settable options for calendar");
            default:;
        }
    }
    ///
    /// Get option for calendar, currently only check if it is Gregorian
    /// calendar
    ///
    virtual int get_option(calendar_option_type opt) const
    {
        switch (opt)
        {
            case is_gregorian:
                return 1;
            case is_dst:
                return tm_.tm_isdst == 1;
            default:
                return 0;
        };
    }

    ///
    /// Adjust period's \a p value by \a difference items using a update_type \a
    /// u. Note: not all values are adjustable
    ///
    virtual void adjust_value(period::marks::period_mark p, update_type u,
                              int difference)
    {
        switch (u)
        {
            case move:
            {
                using namespace period::marks;
                switch (p)
                {
                    case year:          ///< Year, it is calendar specific
                    case extended_year: ///< Extended year for Gregorian/Julian
                                        ///< calendars, where 1 BC == 0, 2 BC ==
                                        ///< -1.
                        tm_updated_.tm_year += difference;
                        break;
                    case month:
                        tm_updated_.tm_mon += difference;
                        break;
                    case day:
                    case day_of_year:
                    case day_of_week: ///< Day of week, starting from Sunday,
                                      ///< [1..7]
                    case day_of_week_local: ///< Local day of week, for example
                                            ///< in France Monday is 1, in US
                                            ///< Sunday is 1, [1..7]
                        tm_updated_.tm_mday += difference;
                        break;
                    case hour:    ///< 24 clock hour [0..23]
                    case hour_12: ///< 12 clock hour [0..11]
                        tm_updated_.tm_hour += difference;
                        break;
                    case am_pm: ///< am or pm marker, [0..1]
                        tm_updated_.tm_hour += 12 * difference;
                        break;
                    case minute: ///< minute [0..59]
                        tm_updated_.tm_min += difference;
                        break;
                    case second:
                        tm_updated_.tm_sec += difference;
                        break;
                    case week_of_year:  ///< The week number in the year
                    case week_of_month: ///< The week number withing current
                                        ///< month
                    case day_of_week_in_month: ///< Original number of the day
                                               ///< of the week in month.
                        tm_updated_.tm_mday += difference * 7;
                        break;
                    default:; // Not all values are adjustable
                }
                normalized_ = false;
                normalize();
            }
            break;
            case roll:
            { // roll
                int cur_min = get_value(p, actual_minimum);
                int cur_max = get_value(p, actual_maximum);
                int max_diff = cur_max - cur_min + 1;
                if (max_diff > 0)
                {
                    int value = get_value(p, current);
                    int addon = 0;
                    if (difference < 0)
                        addon = ((-difference / max_diff) + 1) * max_diff;
                    value = (value - cur_min + difference + addon) % max_diff +
                            cur_min;
                    set_value(p, value);
                    normalize();
                }
            }
            default:;
        }
    }

    int get_diff(period::marks::period_mark p, int diff,
                 gregorian_calendar const* other) const
    {
        if (diff == 0)
            return 0;
        hold_ptr<gregorian_calendar> self(clone());
        self->adjust_value(p, move, diff);
        if (diff > 0)
        {
            if (self->time_ > other->time_)
                return diff - 1;
            else
                return diff;
        }
        else
        {
            if (self->time_ < other->time_)
                return diff + 1;
            else
                return diff;
        }
    }

    ///
    /// Calculate the difference between this calendar  and \a other in \a p
    /// units
    ///
    virtual int difference(abstract_calendar const* other_cal,
                           period::marks::period_mark p) const
    {
        hold_ptr<gregorian_calendar> keeper;
        gregorian_calendar const* other =
            dynamic_cast<gregorian_calendar const*>(other_cal);
        if (!other)
        {
            keeper.reset(clone());
            keeper->set_time(other_cal->get_time());
            other = keeper.get();
        }

        int factor = 1; // for weeks vs days handling

        using namespace period::marks;
        switch (p)
        {
            case era:
                return 0;
            case year:
            case extended_year:
            {
                int diff = other->tm_.tm_year - tm_.tm_year;
                return get_diff(period::marks::year, diff, other);
            }
            case month:
            {
                int diff = 12 * (other->tm_.tm_year - tm_.tm_year) +
                           other->tm_.tm_mon - tm_.tm_mon;
                return get_diff(period::marks::month, diff, other);
            }
            case day_of_week_in_month:
            case week_of_month:
            case week_of_year:
                factor = 7;
                // fall
            case day:
            case day_of_year:
            case day_of_week:
            case day_of_week_local:
            {
                int diff = other->tm_.tm_yday - tm_.tm_yday;
                if (other->tm_.tm_year != tm_.tm_year)
                {
                    diff += days_from_0(other->tm_.tm_year + 1900) -
                            days_from_0(tm_.tm_year + 1900);
                }
                return get_diff(period::marks::day, diff, other) / factor;
            }
            case am_pm:
                return static_cast<int>((other->time_ - time_) / (3600 * 12));
            case hour:
            case hour_12:
                return static_cast<int>((other->time_ - time_) / 3600);
            case minute:
                return static_cast<int>((other->time_ - time_) / 60);
            case second:
                return static_cast<int>(other->time_ - time_);
            default:
                return 0;
        };
    }

    ///
    /// Set time zone, empty - use system
    ///
    virtual void set_timezone(std::string const& tz)
    {
        if (tz.empty())
        {
            is_local_ = true;
            tzoff_ = 0;
        }
        else
        {
            is_local_ = false;
            tzoff_ = parse_tz(tz);
        }
        from_time(time_);
        time_zone_name_ = tz;
    }
    virtual std::string get_timezone() const
    {
        return time_zone_name_;
    }

    virtual bool same(abstract_calendar const* other) const
    {
        gregorian_calendar const* gcal =
            dynamic_cast<gregorian_calendar const*>(other);
        if (!gcal)
            return false;
        return gcal->tzoff_ == tzoff_ && gcal->is_local_ == is_local_ &&
               gcal->first_day_of_week_ == first_day_of_week_;
    }

    virtual ~gregorian_calendar()
    {
    }

  private:
    void from_time(std::time_t point)
    {
        std::time_t real_point = point + tzoff_;
        std::tm* t = 0;
#ifdef BOOST_WINDOWS
        // Windows uses TLS, thread safe
        t = is_local_ ? localtime(&real_point) : gmtime(&real_point);
#else
        std::tm tmp_tm;
        t = is_local_ ? localtime_r(&real_point, &tmp_tm)
                      : gmtime_r(&real_point, &tmp_tm);
#endif
        if (!t)
        {
            throw date_time_error(
                "boost::locale::gregorian_calendar: invalid time point");
        }
        tm_ = *t;
        tm_updated_ = *t;
        normalized_ = true;
        time_ = point;
    }
    int first_day_of_week_;
    std::time_t time_;
    std::tm tm_;
    std::tm tm_updated_;
    bool normalized_;
    bool is_local_;
    int tzoff_;
    std::string time_zone_name_;
};

abstract_calendar* create_gregorian_calendar(std::string const& terr)
{
    return new gregorian_calendar(terr);
}

class gregorian_facet : public calendar_facet
{
  public:
    gregorian_facet(std::string const& terr, size_t refs = 0) :
        calendar_facet(refs), terr_(terr)
    {
    }
    virtual abstract_calendar* create_calendar() const
    {
        return create_gregorian_calendar(terr_);
    }

  private:
    std::string terr_;
};

std::locale install_gregorian_calendar(std::locale const& in,
                                       std::string const& terr)
{
    return std::locale(in, new gregorian_facet(terr));
}

} // namespace util
} // namespace locale
} // namespace boost

// vim: tabstop=4 expandtab shiftwidth=4 softtabstop=4