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

#ifndef VISIONCORE_MATH_KALMAN_HPP
#define VISIONCORE_MATH_KALMAN_HPP

#include <VisionCore/Platform.hpp>

#include <Eigen/LU>

namespace vc
{

namespace math
{

#if 0 // TODO FIXME
template<int dimension, class StateType>
class ConstantProcess {
public:
    typedef Eigen::Matrix<double, StateType::DIM, 1> VecState;
    typedef Eigen::Matrix<double, StateType::DIM, StateType::DIM> MatStateState;

    /// Noise per second, per component
    VecState sigma;
    MatStateState noise;
    MatStateState jacobian;

    inline ConstantProcess() :
    sigma(VecState::Zero()),
    noise(MatStateState::Zero()),
    jacobian(MatStateState::Identity())
    {}

    MatStateState const& getJacobian(StateType const& /*state*/, const double /*dt*/) {
        /// No change over time due to process model
        return jacobian;
    }

    void updateState(StateType const& /*state*/, const double /*dt*/) {
        /// no-op - no change due to process model
    }

    MatStateState const& getNoiseCovariance(const double dt) {
        noise = (dt * sigma).asDiagonal();
        return noise;
    }

    void updateFromMeasurement(StateType & state, VecState const & innovation) {
        state.x += innovation;
    }

};

template<class StateType>
class AbsoluteMeasurement {
public:
    static const int DIM = StateType::DIM;
    typedef Eigen::Matrix<double, DIM, 1> VecMeas;
    typedef Eigen::Matrix<double, DIM, 1> VecState;
    typedef Eigen::Matrix<double, DIM, DIM> MatMeasMeas;
    typedef Eigen::Matrix<double, DIM, DIM> MatMeasState;

    VecMeas measurement;

    MatMeasState jacobian;
    MatMeasMeas covariance;

    AbsoluteMeasurement() :
    measurement(VecMeas::Zero()),
    jacobian(MatMeasState::Identity()),
    covariance(MatMeasMeas::Identity()) {}

    MatMeasState const& getJacobian(StateType const& /*state*/) {
        return jacobian;
    }

    /// Measurement noise covariance, aka uncertainty
    /// in measurement
    MatMeasMeas const& getCovariance(StateType const& /*state*/) {
        return covariance;
    }

    VecState const getInnovation(StateType const& state) {
        return measurement - state.x;
    }

};

template<typename T, typename TimeT = T>
class KalmanFilter
{
public:
    typedef T Scalar;
    typedef TimeT TimeType;

    template<unsigned int dim>
    class StateT
    {
    public:
        static const int Dimension = dim;
        typedef Eigen::Matrix<Scalar, Dimension, 1> StateVectorT;
        typedef Eigen::Matrix<Scalar, Dimension, Dimension> CovarianceMatrixT;

        StateVectorT StateVector;
        CovarianceMatrixT Covariance;

        StateT() : StateVector(StateVectorT::Zero()), Covariance(CovarianceMatrixT::Identity())
        {

        }
    };

    template<unsigned int sdim, unsigned int mdim>
    class MeasurementT
    {
    public:
        typedef StateT<sdim> StateType;
        static const int Dimension = mdim;
        typedef Eigen::Matrix<Scalar, Dimension, 1> MeasurementVectorT;
        typedef Eigen::Matrix<Scalar, StateType::Dimension, 1> StateVectorT;
        typedef Eigen::Matrix<Scalar, Dimension, Dimension> JacobianMatrixT;
        typedef Eigen::Matrix<Scalar, Dimension, StateType::Dimension> HMatrixT;

        MeasurementT()
        {

        }
    };

    template<unsigned int dim>
    void predict(StateT<dim>& state, TimeType dt)
    {
        const StateT<dim>::CovarianceMatrixT A(processModel.getJacobian(state, dt));
        state.Covariance = A * state.Covariance * A.transpose() + processModel.getNoiseCovariance(dt);
        /// @todo symmetrize?
        processModel.updateState(state, dt);
    }

    template<class MeasurementType>
    void correct(MeasurementType & m)
    {
        typedef Eigen::Matrix<Scalar, MeasurementType::DIM, StateType::DIM> MatMeasState;
        typedef Eigen::Matrix<Scalar, MeasurementType::DIM, MeasurementType::DIM> MatMeasMeas;
        typedef Eigen::Matrix<Scalar, MeasurementType::DIM, 1> VecMeas;
        /// @todo implement
        const MatMeasState & H = m.getJacobian(state);
        const MatMeasMeas & R = m.getCovariance(state);
        const VecState innovation = m.getInnovation(state);
        const MatMeasMeas S = H * state.covariance * H.transpose() + R;

        Eigen::eikfLUType<MatMeasMeas> luOfS = S.eikfLUFunc();
        MatStateState K = state.covariance * H.transpose() * luOfS.inverse();
        processModel.updateFromMeasurement(state, K * innovation);
        state.covariance = (MatStateState::Identity() - K * H) * state.covariance;

    }

};
#endif

}

}

#endif // VISIONCORE_MATH_KALMAN_HPP


//
// Created by pengpeng on 11/4/20.
//

#include <ros/ros.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <geometry_msgs/PoseStamped.h>
#include <math.h>
#include <Eigen/Eigen>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>

using namespace Eigen;

Vector3d current_p;
mavros_msgs::State current_state;
ros::Publisher pva_pub;

void positionCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{

    current_p << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;

    //ROS_INFO("X:%f Y:%f  Z:%f",-current_p(0),current_p(1),current_p(2));
}

void stateCallback(const mavros_msgs::State::ConstPtr &msg)
{
    current_state = *msg;

}

void setPVA(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw=0.0)
{
    trajectory_msgs::JointTrajectoryPoint pva_setpoint;

    pva_setpoint.positions.push_back(p(0)); //x
    pva_setpoint.positions.push_back(p(1)); //y
    pva_setpoint.positions.push_back(p(2)); //z
    pva_setpoint.positions.push_back(yaw);

    pva_setpoint.velocities.push_back(v(0));
    pva_setpoint.velocities.push_back(v(1));
    pva_setpoint.velocities.push_back(v(2));

    pva_setpoint.accelerations.push_back(a(0));
    pva_setpoint.accelerations.push_back(a(1));
    pva_setpoint.accelerations.push_back(a(2));

    pva_pub.publish(pva_setpoint);

//    ROS_INFO_THROTTLE(1.0, "P x=%f, y=%f, z=%f", pva_setpoint.positions[0], pva_setpoint.positions[1], pva_setpoint.positions[2]);
//    ROS_INFO_THROTTLE(1.0, "V x=%f, y=%f, z=%f", pva_setpoint.velocities[0], pva_setpoint.velocities[1], pva_setpoint.velocities[2]);
//    ROS_INFO_THROTTLE(1.0, "A x=%f, y=%f, z=%f", pva_setpoint.accelerations[0], pva_setpoint.accelerations[1], pva_setpoint.accelerations[2]);
//
//    ROS_INFO("P x=%f, y=%f, z=%f", pva_setpoint.positions[0], pva_setpoint.positions[1], pva_setpoint.positions[2]);
//    ROS_INFO("V x=%f, y=%f, z=%f", pva_setpoint.velocities[0], pva_setpoint.velocities[1], pva_setpoint.velocities[2]);
//    ROS_INFO("A x=%f, y=%f, z=%f", pva_setpoint.accelerations[0], pva_setpoint.accelerations[1], pva_setpoint.accelerations[2]);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "test_take_off");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/mavros/state", 1, stateCallback);
    ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 1, positionCallback);

    pva_pub = nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/pva_setpoint", 1);

    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local", 1);

    const int LOOPRATE = 40;
    ros::Rate loop_rate(LOOPRATE);

    double take_off_height = 2.0;
    double take_off_acc = 0.5;
    double take_off_time=sqrt(take_off_height/take_off_acc)*2.0;
    double delt_t = 1.0 / LOOPRATE;
    double take_off_send_times = take_off_time / delt_t;
    int counter = 0;
    int take_off_init=1;
    Vector3d take_off_position;
    ros::Time last_time=ros::Time::now();
    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        loop_rate.sleep();
    }
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
    ros::Time last_request = ros::Time::now();
    geometry_msgs::PoseStamped pose;

    double yaw_set = 3.1415926;
    int mode=0;
    double z_sp=0;
    double vz_sp=0;
    ROS_INFO("Arm and takeoff-----------------------------");

    while(ros::ok())
    {
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }
        if(current_state.mode != "OFFBOARD" || !current_state.armed)
        {
            ros::spinOnce();
            loop_rate.sleep();
            pose.header.stamp = ros::Time::now();
            pose.pose.position.x = 0;
            pose.pose.position.y = 0;
            pose.pose.position.z =0;
            double theta=0/180.0*3.14;
            pose.pose.orientation.w=cos(theta/2);
            pose.pose.orientation.x=0.0;
            pose.pose.orientation.y=0;
            pose.pose.orientation.z=sin(theta/2);
            local_pos_pub.publish(pose);
            continue;
        }

        if(mode==0)
        {

            if(take_off_init==1)
            {
                ros::spinOnce();
                take_off_position=current_p;
                take_off_height = 2.0;
                take_off_acc = 0.1;
                take_off_time=sqrt(take_off_height/take_off_acc)*2.0;
                take_off_send_times = take_off_time / delt_t;
                counter=0;
                take_off_init=0;
                last_time=ros::Time::now();

            }

            if(counter < take_off_send_times / 2.0)
            {

                z_sp = 0.5*take_off_acc*counter*delt_t*counter*delt_t;
                vz_sp = counter*delt_t*take_off_acc;
                Vector3d p_sp(take_off_position(0), take_off_position(1), z_sp);
                Vector3d v_sp(0, 0, vz_sp);
                setPVA(p_sp, v_sp, Vector3d::Zero(), yaw_set);

            }
            else if(counter < take_off_send_times)
            {

                double t_this = (counter-take_off_send_times/2.0)*delt_t;
                //ROS_INFO("THIS:%f",t_this);
                //z_sp = take_off_send_times/2*delt_t*take_off_acc*t_this - 0.5*take_off_acc*t_this*t_this;

                vz_sp = take_off_send_times/2.0*delt_t*take_off_acc - take_off_acc*t_this;

                z_sp=1.0/2.0*take_off_acc*(take_off_time/2.0)*(take_off_time/2.0)+(vz_sp+take_off_time/2.0*take_off_acc)*t_this/2.0;

                //ROS_INFO("Z_SP:%f",z_sp);
                Vector3d p_sp(take_off_position(0), take_off_position(1), z_sp);
                Vector3d v_sp(0, 0, vz_sp);
                setPVA(p_sp, v_sp, Vector3d::Zero(), yaw_set);

            }
            else
            {
                Vector3d p_sp(take_off_position(0), take_off_position(1), take_off_height);
                setPVA(p_sp, Vector3d::Zero(), Vector3d::Zero(), yaw_set);
//                counter --;
            }
            counter++;
            if(ros::Time::now() - last_time > ros::Duration(20))
            {
                mode=1;
                take_off_init=1;
                ROS_INFO("Land Mode--------------");
            }
        }
        else if(mode==1)
        {
            if(take_off_init==1)
            {
                ros::spinOnce();
                take_off_position=current_p;
                //ROS_INFO("Z:%f",take_off_position(2));
                take_off_acc=0.1;
                take_off_time=sqrt(take_off_position(2)/take_off_acc)*2.0;
                take_off_send_times=take_off_time/delt_t;
                //ROS_INFO("aaaaaaaatake_off_send_times:%f",take_off_send_times);
                counter=0;
                take_off_init=0;
                last_time=ros::Time::now();
            }
            //ROS_INFO("take_off_position:%f",take_off_position);
            //ROS_INFO("counter:%d",counter);

            if(counter < take_off_send_times / 2)
            {
                //ROS_INFO("DDDDDDD");
                z_sp = take_off_position(2)-0.5*take_off_acc*counter*delt_t*counter*delt_t;
                vz_sp = -counter*delt_t*take_off_acc;
                //ROS_INFO("z_sp:%f",z_sp);
                Vector3d p_sp(take_off_position(0), take_off_position(1), z_sp);
                Vector3d v_sp(0, 0, vz_sp);
                setPVA(p_sp, v_sp, Vector3d::Zero(), yaw_set);

            }
            else if(counter < take_off_send_times)
            {
                //ROS_INFO("SSSSSS");

                double t_this = (counter-take_off_send_times/2.0)*delt_t;
                vz_sp = -(take_off_send_times/2.0*delt_t*take_off_acc - take_off_acc*t_this);
                z_sp = take_off_position(2)-(1.0/2.0*take_off_acc*(take_off_time/2.0)*(take_off_time/2.0)+(-vz_sp+take_off_time/2.0*take_off_acc)*t_this/2.0);
               // ROS_INFO("vz_sp:%f",vz_sp);
                //ROS_INFO("z_sp:%f",z_sp);
                Vector3d p_sp(take_off_position(0), take_off_position(1), z_sp);
                Vector3d v_sp(0, 0, vz_sp);
                setPVA(p_sp, v_sp, Vector3d::Zero(), yaw_set);

            }
            else
            {
                Vector3d p_sp(take_off_position(0), take_off_position(1), 0);
                setPVA(p_sp, Vector3d::Zero(), Vector3d::Zero(), yaw_set);
            }
            counter++;
            if(ros::Time::now() - last_time > ros::Duration(20))
            {
                mode=0;
                take_off_init=1;
                ROS_INFO("TAKE OFF Mode--------------");
            }
        }
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <algorithms/test_length.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

template <typename P>
void test_all()
{
    test_geometry<bg::model::multi_linestring<bg::model::linestring<P> > >
        ("MULTILINESTRING((0 0,3 4,4 3))", 5 + sqrt(2.0));
}

template <typename P>
void test_empty_input()
{
    test_empty_input(bg::model::multi_linestring<P>());
}

int test_main( int , char* [] )
{
    test_all<bg::model::d2::point_xy<double> >();

#ifdef HAVE_TTMATH
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif

    // test_empty_input<bg::model::d2::point_xy<int> >();

    return 0;
}


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file scheduling_jobs.hpp
 * @brief
 * @author Robert Rosolek
 * @version 1.0
 * @date 2013-11-19
 */
#ifndef PAAL_SCHEDULING_JOBS_HPP
#define PAAL_SCHEDULING_JOBS_HPP

#define BOOST_RESULT_OF_USE_DECLTYPE

#include "paal/data_structures/fraction.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/type_functions.hpp"
#include "paal/utils/irange.hpp"
#include "paal/utils/assign_updates.hpp"

#include <boost/range/algorithm/copy.hpp>
#include <boost/range/algorithm/sort.hpp>
#include <boost/range/counting_range.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/range/numeric.hpp>

#include <algorithm>
#include <cassert>
#include <iterator>
#include <numeric>
#include <random>
#include <utility>
#include <vector>

namespace paal {
namespace greedy {

namespace detail {

template <class MachineIterator, class JobIterator, class GetSpeed,
          class GetLoad>
struct sched_traits {
    typedef typename std::iterator_traits<MachineIterator>::reference
        machine_reference;
    typedef typename std::iterator_traits<JobIterator>::reference job_reference;
    typedef pure_result_of_t<GetSpeed(machine_reference)> speed_t;
    typedef pure_result_of_t<GetLoad(job_reference)> load_t;
    typedef data_structures::fraction<load_t, speed_t> frac_t;
};

template <class MachineIterator, class JobIterator, class GetSpeed,
          class GetLoad, class Traits = sched_traits<
                             MachineIterator, JobIterator, GetSpeed, GetLoad>>
typename Traits::frac_t calculate_bound(const MachineIterator mfirst,
                                        const MachineIterator mlast,
                                        const JobIterator jfirst,
                                        const JobIterator jlast,
                                        GetSpeed get_speed, GetLoad get_load) {
    typedef typename Traits::speed_t Speed;
    typedef typename Traits::load_t Load;
    typedef typename Traits::frac_t Frac;

    auto jobs_num = jlast - jfirst;
    auto machines_num = mlast - mfirst;

    std::vector<Speed> speed_sum(machines_num);
    std::transform(mfirst, mlast, speed_sum.begin(), get_speed);
    boost::partial_sum(speed_sum, speed_sum.begin());

    std::vector<Load> load_sum(jobs_num);
    std::transform(jfirst, jlast, load_sum.begin(), get_load);
    boost::partial_sum(load_sum, load_sum.begin());

    typedef decltype(machines_num) MachinesNumType;
    assert(jobs_num > 0 && machines_num > 0);
    Frac result(get_load(*jfirst), get_speed(*mfirst));
    for (auto jobID : irange(jobs_num)) {
        Load load = get_load(jfirst[jobID]);
        auto get_single = [ = ](MachinesNumType i) {
            return Frac(load, get_speed(mfirst[i]));
        };
        auto get_summed = [&](MachinesNumType i) {
            return Frac(load_sum[jobID], speed_sum[i]);
        };
        auto condition = [ = ](MachinesNumType i) {
            return get_summed(i) >= get_single(i);
        };
        auto machines_ids = boost::counting_range(
            static_cast<MachinesNumType>(0), machines_num);
        // current range based version in boost is broken
        // should be replaced when released
        // https://github.com/boostorg/algorithm/pull/4
        auto it = std::partition_point(machines_ids.begin(), machines_ids.end(),
                                       condition);
        MachinesNumType machineID =
            (it != machines_ids.end()) ? *it : machines_num - 1;
        auto getMax = [ = ](MachinesNumType i) {
            return std::max(get_single(i), get_summed(i));
        };

        Frac candidate = getMax(machineID);
        if (machineID != 0) {
            assign_min(candidate, getMax(machineID - 1));
        }
        assign_max(result, candidate);
    }
    return result;
}

template <class MachineIterator, class JobIterator, class OutputIterator,
          class GetSpeed, class GetLoad, class RoundFun>
void schedule(MachineIterator mfirst, MachineIterator mlast, JobIterator jfirst,
              JobIterator jlast, OutputIterator result, GetSpeed get_speed,
              GetLoad get_load, RoundFun round) {
    typedef sched_traits<MachineIterator, JobIterator, GetSpeed, GetLoad>
        Traits;
    typedef typename Traits::speed_t Speed;
    typedef typename Traits::load_t Load;

    if (mfirst == mlast || jfirst == jlast) {
        return;
    }

    std::vector<MachineIterator> machines;
    boost::copy(boost::counting_range(mfirst, mlast),
                std::back_inserter(machines));
    auto get_speed_from_iterator = utils::make_lift_iterator_functor(get_speed);
    boost::sort(machines, utils::make_functor_to_comparator(
                              get_speed_from_iterator, utils::greater{}));

    std::vector<JobIterator> jobs;
    boost::copy(boost::counting_range(jfirst, jlast), std::back_inserter(jobs));
    auto get_load_from_iterator = utils::make_lift_iterator_functor(get_load);
    boost::sort(jobs, utils::make_functor_to_comparator(get_load_from_iterator,
                                                        utils::greater{}));

    auto bound = detail::calculate_bound(
        machines.begin(), machines.end(), jobs.begin(), jobs.end(),
        get_speed_from_iterator, get_load_from_iterator);
    Load bound_load = bound.num;
    Speed bound_speed = bound.den;
    Load current_load{};
    auto emit = [&result](MachineIterator miter, JobIterator jiter) {
        *result = std::make_pair(miter, jiter);
        ++result;
    };
    auto job_iter = jobs.begin();
    for (auto machine_iter = machines.begin(); machine_iter != machines.end();
         ++machine_iter) {
        auto &&machine = *(*machine_iter);
        Speed speed = get_speed(machine);
        while (job_iter != jobs.end()) {
            auto &&job = *(*job_iter);
            Load job_load = get_load(job) * bound_speed,
                 new_load = current_load + job_load;
            assert(new_load <= bound_load * (2 * speed));
            if (bound_load * speed < new_load) {
                Load frac_load = bound_load * speed - current_load;
                if (round(frac_load, job_load)) {
                    emit(*machine_iter, *job_iter);
                } else {
                    auto next_machine_iter = std::next(machine_iter);
                    assert(next_machine_iter != machines.end());
                    emit(*next_machine_iter, *job_iter);
                }
                ++job_iter;
                current_load = job_load - frac_load;
                break;
            }
            emit(*machine_iter, *job_iter);
            ++job_iter;
            current_load = new_load;
        }
    }
    assert(job_iter == jobs.end());
}
} //!detail

/*
 * @brief This is deterministic solve scheduling jobs on machines with different
 * speeds problem and return schedule
 *
 * Example:
 *  \snippet scheduling_jobs_example.cpp Scheduling Jobs Example
 *
 * example file is scheduling_jobs_example.cpp
 *
 * @param mfirst
 * @param mlast
 * @param jfirst
 * @param jlast
 * @param result
 * @param get_speed
 * @param get_load
 * @tparam MachineIterator
 * @tparam JobIterator
 * @tparam OutputIterator
 * @tparam GetSpeed
 * @tparam GetLoad
 */
template <class MachineIterator, class JobIterator, class OutputIterator,
          class GetSpeed, class GetLoad>
void schedule_deterministic(const MachineIterator mfirst,
                            const MachineIterator mlast,
                            const JobIterator jfirst, const JobIterator jlast,
                            OutputIterator result, GetSpeed get_speed,
                            GetLoad get_load) {
    detail::schedule(mfirst, mlast, jfirst, jlast, result, get_speed, get_load,
                     utils::always_true{});
}

/*
 * @brief This is randomized solve scheduling jobs on machines with different
 * speeds problem and return schedule.
 *
 * Example:
 *  \snippet scheduling_jobs_example.cpp Scheduling Jobs Example
 *
 * example file is scheduling_jobs_example.cpp
 *
 * @param mfirst
 * @param mlast
 * @param jfirst
 * @param jlast
 * @param result
 * @param get_speed
 * @param get_load
 * @param gen
 * @tparam MachineIterator
 * @tparam JobIterator
 * @tparam OutputIterator
 * @tparam GetSpeed
 * @tparam GetLoad
 * @tparam RandomNumberGenerator
 */
template <class MachineIterator, class JobIterator, class OutputIterator,
          class GetSpeed, class GetLoad,
          class RandomNumberGenerator = std::default_random_engine>
void schedule_randomized(const MachineIterator mfirst,
                         const MachineIterator mlast, const JobIterator jfirst,
                         const JobIterator jlast, OutputIterator result,
                         GetSpeed get_speed, GetLoad get_load,
                         RandomNumberGenerator &&gen =
                             std::default_random_engine(97345631u)) {
    typedef typename detail::sched_traits<MachineIterator, JobIterator,
                                          GetSpeed, GetLoad> Traits;
    double alpha = std::uniform_real_distribution<double>()(gen);
    auto round = [alpha](typename Traits::load_t fractional_load,
                         typename Traits::load_t total_load) {
        return total_load * alpha < fractional_load;
    };
    detail::schedule(mfirst, mlast, jfirst, jlast, result, get_speed, get_load,
                     round);
}

} //!greedy
} //!paal

#endif // PAAL_SCHEDULING_JOBS_HPP


#include "router.hpp"

#include <boost/pending/disjoint_sets.hpp>

#include <algorithm>
#include <list>
#include <map>

using namespace std;

using DisjointSet = boost::disjoint_sets<int*, int*>;

auto larger = [](int x, int y) { return x > y; };
using reverse_multimap = multimap<int, int, decltype(larger)>;

Router::Router(Database& database)
    : database_(database),
      spanning_graph_(database_.num_pins()),
      minimum_spanning_tree_(database_.num_pins()) {}

void Router::DrawSpanningGraph(ostream& os) const {
  spanning_graph_.Draw(os, database_);
}

void Router::DrawMinimumSpanningTree(ostream& os) const {
  minimum_spanning_tree_.Draw(os, database_);
}

void Router::DrawRectilinearMinimumSpanningTree(ostream& os) const {
  os << "set title 'RMST'" << endl;
  os << "set xrange[" << database_.outline_lower_left().x() << ":"
     << database_.outline_upper_right().x() << "]" << endl;
  os << "set yrange[" << database_.outline_lower_left().y() << ":"
     << database_.outline_upper_right().y() << "]" << endl;
  os << "set style line 1 lt 1 lc rgb 'black' lw 1 pt 2 ps 1" << endl;
  os << "unset key" << endl;

  os << "plot '-' w lp ls 1" << endl;
  for (const Line& line : horizontal_lines_) {
    os << line.end_point_a().x() << " " << line.end_point_a().y() << endl;
    os << line.end_point_b().x() << " " << line.end_point_b().y() << endl;
    os << endl;
  }
  for (const Line& line : vertical_lines_) {
    os << line.end_point_a().x() << " " << line.end_point_a().y() << endl;
    os << line.end_point_b().x() << " " << line.end_point_b().y() << endl;
    os << endl;
  }
  os << "e" << endl;
  os << "pause -1 'Press any key'" << endl;
}

void Router::Output(ostream& os) const {
  long long int wirelength = 0;
  /* for (const Line& line : horizontal_lines_) { */
  /*   wirelength += */
  /*       Point::ManhattanDistance(line.end_point_a(), line.end_point_b()); */
  /* } */
  /* for (const Line& line : vertical_lines_) { */
  /*   wirelength += */
  /*       Point::ManhattanDistance(line.end_point_a(), line.end_point_b()); */
  /* } */

  os << "NumRoutedPins = " << database_.num_pins() << endl;
  os << "WireLength = " << wirelength << endl;
  for (const Line& line : horizontal_lines_) {
    os << "H-line "
       << "(" << line.end_point_a().x() << "," << line.end_point_a().y() << ") "
       << "(" << line.end_point_b().x() << "," << line.end_point_b().y() << ")"
       << endl;
  }
  for (const Line& line : vertical_lines_) {
    os << "V-line "
       << "(" << line.end_point_a().x() << "," << line.end_point_a().y() << ") "
       << "(" << line.end_point_b().x() << "," << line.end_point_b().y() << ")"
       << endl;
  }
}

void Router::Run() {
  ConstructSpanningGraph();
  ConstructMinimumSpanningTree();
  RectilinearizeMinimumSpanningTree();
}

// private

void Router::ConstructSpanningGraph() {
  const int num_pins = database_.num_pins();

  vector<int> sorted_pin_ids;
  sorted_pin_ids.reserve(num_pins);
  for (int i = 0; i < num_pins; ++i) {
    const int id = i;
    sorted_pin_ids.push_back(id);
  }

  // Sort pins by x + y
  sort(sorted_pin_ids.begin(), sorted_pin_ids.end(),
       [&](int pin_a_id, int pin_b_id) {
         const Pin& pin_a = database_.pin(pin_a_id);
         const Pin& pin_b = database_.pin(pin_b_id);
         return pin_a.x() + pin_a.y() < pin_b.x() + pin_b.y();
       });

  reverse_multimap R1(larger);
  reverse_multimap R2(larger);
  for (const int current_pin_id : sorted_pin_ids) {
    const Pin& current_pin = database_.pin(current_pin_id);
    const int current_pin_x = current_pin.x();
    const int current_pin_y = current_pin.y();

    R1.insert(make_pair(current_pin_x, current_pin_id));
    R2.insert(make_pair(current_pin_y, current_pin_id));

    for (auto it = R1.lower_bound(current_pin_x); it != R1.end();) {
      const int pin_id = it->second;
      if (pin_id != current_pin_id) {
        const Pin& pin = database_.pin(pin_id);
        const int pin_x = it->first;
        const int pin_y = pin.y();
        if (pin_x - pin_y > current_pin_x - current_pin_y) {
          spanning_graph_.AddEdge(
              pin_id, current_pin_id,
              Point::ManhattanDistance(pin.coordinates(),
                                       current_pin.coordinates()));
          it = R1.erase(it);
        } else {
          break;
        }
      } else {
        ++it;
      }
    }
    for (auto it = R2.upper_bound(current_pin_y); it != R2.end();) {
      const int pin_id = it->second;
      if (pin_id != current_pin_id) {
        const Pin& pin = database_.pin(pin_id);
        const int pin_x = pin.x();
        const int pin_y = it->first;
        if (pin_x - pin_y <= current_pin_x - current_pin_y) {
          spanning_graph_.AddEdge(
              pin_id, current_pin_id,
              Point::ManhattanDistance(pin.coordinates(),
                                       current_pin.coordinates()));
          it = R2.erase(it);
        } else {
          break;
        }
      } else {
        ++it;
      }
    }
  }

  // Sort pins by x - y
  sort(sorted_pin_ids.begin(), sorted_pin_ids.end(),
       [&](int pin_a_id, int pin_b_id) {
         const Pin& pin_a = database_.pin(pin_a_id);
         const Pin& pin_b = database_.pin(pin_b_id);
         return pin_a.x() - pin_a.y() < pin_b.x() - pin_b.y();
       });

  multimap<int, int> R3;
  reverse_multimap R4(larger);
  for (const int current_pin_id : sorted_pin_ids) {
    const Pin& current_pin = database_.pin(current_pin_id);
    const int current_pin_x = current_pin.x();
    const int current_pin_y = current_pin.y();

    R3.insert(make_pair(current_pin_y, current_pin_id));
    R4.insert(make_pair(current_pin_x, current_pin_id));

    for (auto it = R3.lower_bound(current_pin_y); it != R3.end();) {
      const int pin_id = it->second;
      if (pin_id != current_pin_id) {
        const Pin& pin = database_.pin(pin_id);
        const int pin_x = pin.x();
        const int pin_y = it->first;
        if (pin_x + pin_y < current_pin_x + current_pin_y) {
          spanning_graph_.AddEdge(
              pin_id, current_pin_id,
              Point::ManhattanDistance(pin.coordinates(),
                                       current_pin.coordinates()));
          it = R3.erase(it);
        } else {
          break;
        }
      } else {
        ++it;
      }
    }
    for (auto it = R4.upper_bound(current_pin_x); it != R4.end();) {
      const int pin_id = it->second;
      if (pin_id != current_pin_id) {
        const Pin& pin = database_.pin(pin_id);
        const int pin_x = it->first;
        const int pin_y = pin.y();
        if (pin_x + pin_y >= current_pin_x + current_pin_y) {
          spanning_graph_.AddEdge(
              pin_id, current_pin_id,
              Point::ManhattanDistance(pin.coordinates(),
                                       current_pin.coordinates()));
          it = R4.erase(it);
        } else {
          break;
        }
      } else {
        ++it;
      }
    }
  }
}

void Router::ConstructMinimumSpanningTree() {
  const int num_edges = spanning_graph_.num_edges();

  vector<int> sorted_edge_ids;
  sorted_edge_ids.reserve(num_edges);
  for (int i = 0; i < num_edges; ++i) {
    const int id = i;
    sorted_edge_ids.push_back(id);
  }

  sort(sorted_edge_ids.begin(), sorted_edge_ids.end(),
       [&](int edge_a_id, int edge_b_id) {
         const Edge& edge_a = spanning_graph_.edge(edge_a_id);
         const Edge& edge_b = spanning_graph_.edge(edge_b_id);
         return edge_a.cost() < edge_b.cost();
       });

  vector<int> ranks(num_edges);
  vector<int> parents(num_edges);
  DisjointSet disjoint_set(&ranks[0], &parents[0]);
  for (const int edge_id : sorted_edge_ids) {
    disjoint_set.make_set(edge_id);
  }

  for (const int edge_id : sorted_edge_ids) {
    const Edge& edge = spanning_graph_.edge(edge_id);
    const int vertex_a_id = edge.vertex_a_id();
    const int vertex_b_id = edge.vertex_b_id();
    const int set_a = disjoint_set.find_set(vertex_a_id);
    const int set_b = disjoint_set.find_set(vertex_b_id);
    if (set_a != set_b) {
      minimum_spanning_tree_.AddEdge(vertex_a_id, vertex_b_id, edge.cost());
      disjoint_set.link(vertex_a_id, vertex_b_id);
    }
  }
}

void Router::RectilinearizeMinimumSpanningTree() {
  const int num_edges = minimum_spanning_tree_.num_edges();

  vector<Line> horizontal_lines;
  vector<Line> vertical_lines;
  horizontal_lines.reserve(num_edges);
  vertical_lines.reserve(num_edges);

  for (int i = 0; i < num_edges; ++i) {
    const int id = i;
    const Edge& edge = minimum_spanning_tree_.edge(id);
    const int vertex_a_id = edge.vertex_a_id();
    const int vertex_b_id = edge.vertex_b_id();
    const int pin_a_id = vertex_a_id;
    const int pin_b_id = vertex_b_id;
    const Pin& pin_a = database_.pin(pin_a_id);
    const Pin& pin_b = database_.pin(pin_b_id);
    int x = pin_a.x();
    if (pin_a.x() < pin_b.x()) {
      horizontal_lines.push_back(
          Line(Point(pin_a.x(), pin_a.y()), Point(pin_b.x(), pin_a.y())));
      x = pin_b.x();
    }
    if (pin_a.x() > pin_b.x()) {
      horizontal_lines.push_back(
          Line(Point(pin_b.x(), pin_b.y()), Point(pin_a.x(), pin_b.y())));
      x = pin_a.x();
    }
    if (pin_a.y() < pin_b.y()) {
      vertical_lines.push_back(Line(Point(x, pin_a.y()), Point(x, pin_b.y())));
    }
    if (pin_a.y() > pin_b.y()) {
      vertical_lines.push_back(Line(Point(x, pin_b.y()), Point(x, pin_a.y())));
    }
  }

  horizontal_lines_ = horizontal_lines;
  vertical_lines_ = vertical_lines;

  /* sort(horizontal_lines.begin(), horizontal_lines.end(), */
  /*      [](const Line& line_a, const Line& line_b) { */
  /*        return line_a.end_point_a().x() < line_b.end_point_a().x(); */
  /*      }); */
  /* sort(vertical_lines.begin(), vertical_lines.end(), */
  /*      [](const Line& line_a, const Line& line_b) { */
  /*        return line_a.end_point_a().y() < line_b.end_point_a().y(); */
  /*      }); */

  /* list<Line> nonoverlap_horizontal_lines(horizontal_lines.begin(), */
  /*                                        horizontal_lines.end()); */
  /* list<Line> nonoverlap_vertical_lines(vertical_lines.begin(), */
  /*                                      vertical_lines.end()); */
  /* for (auto it = nonoverlap_horizontal_lines.begin(); */
  /*      it != nonoverlap_horizontal_lines.end();) { */
  /*   auto current_it = it; */
  /*   const int current_x_left = current_it->end_point_a().x(); */
  /*   const int current_x_right = current_it->end_point_b().x(); */
  /*   const int current_y = current_it->end_point_a().y(); */
  /*   ++it; */
  /*   const int next_x_left = it->end_point_a().x(); */
  /*   const int next_x_right = it->end_point_b().x(); */
  /*   const int next_y = it->end_point_a().y(); */
  /*   if (next_x_left <= current_x_right && next_y == current_y) { */
  /*     *current_it = Line( */
  /*         Point(current_x_left, current_y), */
  /*         Point(next_x_right > current_x_right ? next_x_right : current_x_right, */
  /*               current_y)); */
  /*     it = nonoverlap_horizontal_lines.erase(it); */
  /*     --it; */
  /*   } */
  /* } */
  /* for (auto it = nonoverlap_vertical_lines.begin(); */
  /*      it != nonoverlap_vertical_lines.end();) { */
  /*   auto current_it = it; */
  /*   const int current_y_lower = current_it->end_point_a().y(); */
  /*   const int current_y_upper = current_it->end_point_b().y(); */
  /*   const int current_x = current_it->end_point_a().x(); */
  /*   ++it; */
  /*   const int next_y_lower = it->end_point_a().y(); */
  /*   const int next_y_upper = it->end_point_b().y(); */
  /*   const int next_x = it->end_point_a().x(); */
  /*   if (next_y_lower <= current_y_upper && next_x == current_x) { */
  /*     *current_it = */
  /*         Line(Point(current_x, current_y_lower), */
  /*              Point(current_x, */
  /*                    next_y_upper > current_y_upper ? next_y_upper */
  /*                                                   : current_y_upper)); */
  /*     it = nonoverlap_vertical_lines.erase(it); */
  /*     --it; */
  /*   } */
  /* } */

  /* horizontal_lines_ = vector<Line>(nonoverlap_horizontal_lines.begin(), */
  /*                                  nonoverlap_horizontal_lines.end()); */
  /* vertical_lines_ = vector<Line>(nonoverlap_vertical_lines.begin(), */
  /*                                nonoverlap_vertical_lines.end()); */
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2003, 2004, 2005, 2006, 2007, 2008 StatPro Italia srl
 Copyright (C) 2007, 2008, 2009, 2015 Ferdinando Ametrano
 Copyright (C) 2007, 2009 Roland Lichters
 Copyright (C) 2015 Maddalena Zanzi
 Copyright (C) 2015 Paolo Mazzocchi
 Copyright (C) 2018 Matthias Lungwitz

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file ratehelpers.hpp
    \brief deposit, FRA, futures, and various swap rate helpers
*/

#ifndef quantlib_ratehelpers_hpp
#define quantlib_ratehelpers_hpp

#include <ql/termstructures/bootstraphelper.hpp>
#include <ql/instruments/vanillaswap.hpp>
#include <ql/instruments/bmaswap.hpp>
#include <ql/instruments/futures.hpp>
#include <ql/time/calendar.hpp>
#include <ql/time/daycounter.hpp>
#include <ql/time/calendars/unitedstates.hpp>

#include <boost/optional.hpp>

namespace QuantLib {

    class SwapIndex;
    class Quote;

    typedef BootstrapHelper<YieldTermStructure> RateHelper;
    typedef RelativeDateBootstrapHelper<YieldTermStructure>
                                                        RelativeDateRateHelper;

    //! Rate helper for bootstrapping over IborIndex futures prices
    class FuturesRateHelper : public RateHelper {
      public:
        FuturesRateHelper(const Handle<Quote>& price,
                          const Date& iborStartDate,
                          Natural lengthInMonths,
                          const Calendar& calendar,
                          BusinessDayConvention convention,
                          bool endOfMonth,
                          const DayCounter& dayCounter,
                          Handle<Quote> convexityAdjustment = Handle<Quote>(),
                          Futures::Type type = Futures::IMM);
        FuturesRateHelper(Real price,
                          const Date& iborStartDate,
                          Natural lengthInMonths,
                          const Calendar& calendar,
                          BusinessDayConvention convention,
                          bool endOfMonth,
                          const DayCounter& dayCounter,
                          Rate convexityAdjustment = 0.0,
                          Futures::Type type = Futures::IMM);
        FuturesRateHelper(const Handle<Quote>& price,
                          const Date& iborStartDate,
                          const Date& iborEndDate,
                          const DayCounter& dayCounter,
                          Handle<Quote> convexityAdjustment = Handle<Quote>(),
                          Futures::Type type = Futures::IMM);
        FuturesRateHelper(Real price,
                          const Date& iborStartDate,
                          const Date& endDate,
                          const DayCounter& dayCounter,
                          Rate convexityAdjustment = 0.0,
                          Futures::Type type = Futures::IMM);
        FuturesRateHelper(const Handle<Quote>& price,
                          const Date& iborStartDate,
                          const ext::shared_ptr<IborIndex>& iborIndex,
                          const Handle<Quote>& convexityAdjustment = Handle<Quote>(),
                          Futures::Type type = Futures::IMM);
        FuturesRateHelper(Real price,
                          const Date& iborStartDate,
                          const ext::shared_ptr<IborIndex>& iborIndex,
                          Rate convexityAdjustment = 0.0,
                          Futures::Type type = Futures::IMM);
        //! \name RateHelper interface
        //@{
        Real impliedQuote() const override;
        //@}
        //! \name FuturesRateHelper inspectors
        //@{
        Real convexityAdjustment() const;
        //@}
		//********************************************************************
		//Deriscope: Added inspectors
		Time const yearFraction() const { return yearFraction_; }
		//********************************************************************
        //! \name Visitability
        //@{
        void accept(AcyclicVisitor&) override;
        //@}
      private:
        Time yearFraction_;
        Handle<Quote> convAdj_;
    };

    //! Rate helper for bootstrapping over deposit rates
    class DepositRateHelper : public RelativeDateRateHelper {
      public:
        DepositRateHelper(const Handle<Quote>& rate,
                          const Period& tenor,
                          Natural fixingDays,
                          const Calendar& calendar,
                          BusinessDayConvention convention,
                          bool endOfMonth,
                          const DayCounter& dayCounter);
        DepositRateHelper(Rate rate,
                          const Period& tenor,
                          Natural fixingDays,
                          const Calendar& calendar,
                          BusinessDayConvention convention,
                          bool endOfMonth,
                          const DayCounter& dayCounter);
        DepositRateHelper(const Handle<Quote>& rate,
                          const ext::shared_ptr<IborIndex>& iborIndex);
        DepositRateHelper(Rate rate,
                          const ext::shared_ptr<IborIndex>& iborIndex);
        //! \name RateHelper interface
        //@{
        Real impliedQuote() const override;
        void setTermStructure(YieldTermStructure*) override;
		//********************************************************************
		//Deriscope: Added inspectors
		Date const & fixingDate() const { return fixingDate_; }
		ext::shared_ptr<IborIndex> const & iborIndex() const { return iborIndex_; }
		//********************************************************************
        //@}
        //! \name Visitability
        //@{
        void accept(AcyclicVisitor&) override;
        //@}
      private:
        void initializeDates() override;
        Date fixingDate_;
        ext::shared_ptr<IborIndex> iborIndex_;
        RelinkableHandle<YieldTermStructure> termStructureHandle_;
    };

    //! Rate helper for bootstrapping over %FRA rates
    class FraRateHelper : public RelativeDateRateHelper {
      public:
        FraRateHelper(const Handle<Quote>& rate,
                      Natural monthsToStart,
                      Natural monthsToEnd,
                      Natural fixingDays,
                      const Calendar& calendar,
                      BusinessDayConvention convention,
                      bool endOfMonth,
                      const DayCounter& dayCounter,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(Rate rate,
                      Natural monthsToStart,
                      Natural monthsToEnd,
                      Natural fixingDays,
                      const Calendar& calendar,
                      BusinessDayConvention convention,
                      bool endOfMonth,
                      const DayCounter& dayCounter,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(const Handle<Quote>& rate,
                      Natural monthsToStart,
                      const ext::shared_ptr<IborIndex>& iborIndex,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(Rate rate,
                      Natural monthsToStart,
                      const ext::shared_ptr<IborIndex>& iborIndex,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(const Handle<Quote>& rate,
                      Period periodToStart,
                      Natural lengthInMonths,
                      Natural fixingDays,
                      const Calendar& calendar,
                      BusinessDayConvention convention,
                      bool endOfMonth,
                      const DayCounter& dayCounter,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(Rate rate,
                      Period periodToStart,
                      Natural lengthInMonths,
                      Natural fixingDays,
                      const Calendar& calendar,
                      BusinessDayConvention convention,
                      bool endOfMonth,
                      const DayCounter& dayCounter,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(const Handle<Quote>& rate,
                      Period periodToStart,
                      const ext::shared_ptr<IborIndex>& iborIndex,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(Rate rate,
                      Period periodToStart,
                      const ext::shared_ptr<IborIndex>& iborIndex,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(const Handle<Quote>& rate,
                      Natural immOffsetStart,
                      Natural immOffsetEnd,
                      const ext::shared_ptr<IborIndex>& iborIndex,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        FraRateHelper(Rate rate,
                      Natural immOffsetStart,
                      Natural immOffsetEnd,
                      const ext::shared_ptr<IborIndex>& iborIndex,
                      Pillar::Choice pillar = Pillar::LastRelevantDate,
                      Date customPillarDate = Date(),
                      bool useIndexedCoupon = true);
        //! \name RateHelper interface
        //@{
        Real impliedQuote() const override;
        void setTermStructure(YieldTermStructure*) override;
        //@}
		//********************************************************************
		//Deriscope: Added inspectors
		Date const & fixingDate() const { return fixingDate_; }
		boost::optional<Period> const & periodToStart() const { return periodToStart_; }
		ext::shared_ptr<IborIndex> const & iborIndex() const { return iborIndex_; }
		boost::optional<Natural> const immOffsetStart() const { return immOffsetStart_; }
		boost::optional<Natural> const immOffsetEnd() const { return immOffsetEnd_; }
		Pillar::Choice const pillarChoice() const { return pillarChoice_; }
		bool const useIndexedCoupon() const { return useIndexedCoupon_; }
		Real const spanningTime() const { return spanningTime_; }
		//********************************************************************
        //! \name Visitability
        //@{
        void accept(AcyclicVisitor&) override;
        //@}
      private:
        void initializeDates() override;
        Date fixingDate_;
        boost::optional<Period> periodToStart_;
        boost::optional<Natural> immOffsetStart_, immOffsetEnd_;
        Pillar::Choice pillarChoice_;
        ext::shared_ptr<IborIndex> iborIndex_;
        RelinkableHandle<YieldTermStructure> termStructureHandle_;
        bool useIndexedCoupon_;
        Real spanningTime_;
    };

    //! Rate helper for bootstrapping over swap rates
    /*! \todo use input SwapIndex to create the swap */
    class SwapRateHelper : public RelativeDateRateHelper {
      public:
        SwapRateHelper(const Handle<Quote>& rate,
                       const ext::shared_ptr<SwapIndex>& swapIndex,
                       Handle<Quote> spread = Handle<Quote>(),
                       const Period& fwdStart = 0 * Days,
                       // exogenous discounting curve
                       Handle<YieldTermStructure> discountingCurve = Handle<YieldTermStructure>(),
                       Pillar::Choice pillar = Pillar::LastRelevantDate,
                       Date customPillarDate = Date(),
                       bool endOfMonth = false);
        SwapRateHelper(const Handle<Quote>& rate,
                       const Period& tenor,
                       Calendar calendar,
                       // fixed leg
                       Frequency fixedFrequency,
                       BusinessDayConvention fixedConvention,
                       DayCounter fixedDayCount,
                       // floating leg
                       const ext::shared_ptr<IborIndex>& iborIndex,
                       Handle<Quote> spread = Handle<Quote>(),
                       const Period& fwdStart = 0 * Days,
                       // exogenous discounting curve
                       Handle<YieldTermStructure> discountingCurve = Handle<YieldTermStructure>(),
                       Natural settlementDays = Null<Natural>(),
                       Pillar::Choice pillar = Pillar::LastRelevantDate,
                       Date customPillarDate = Date(),
                       bool endOfMonth = false);
        SwapRateHelper(Rate rate,
                       const ext::shared_ptr<SwapIndex>& swapIndex,
                       Handle<Quote> spread = Handle<Quote>(),
                       const Period& fwdStart = 0 * Days,
                       // exogenous discounting curve
                       Handle<YieldTermStructure> discountingCurve = Handle<YieldTermStructure>(),
                       Pillar::Choice pillar = Pillar::LastRelevantDate,
                       Date customPillarDate = Date(),
                       bool endOfMonth = false);
        SwapRateHelper(Rate rate,
                       const Period& tenor,
                       Calendar calendar,
                       // fixed leg
                       Frequency fixedFrequency,
                       BusinessDayConvention fixedConvention,
                       DayCounter fixedDayCount,
                       // floating leg
                       const ext::shared_ptr<IborIndex>& iborIndex,
                       Handle<Quote> spread = Handle<Quote>(),
                       const Period& fwdStart = 0 * Days,
                       // exogenous discounting curve
                       Handle<YieldTermStructure> discountingCurve = Handle<YieldTermStructure>(),
                       Natural settlementDays = Null<Natural>(),
                       Pillar::Choice pillar = Pillar::LastRelevantDate,
                       Date customPillarDate = Date(),
                       bool endOfMonth = false);
        //! \name RateHelper interface
        //@{
        Real impliedQuote() const override;
        void setTermStructure(YieldTermStructure*) override;
        //@}
        //! \name SwapRateHelper inspectors
        //@{
        Spread spread() const;
        ext::shared_ptr<VanillaSwap> swap() const;
        const Period& forwardStart() const;
        //@}
        //! \name Visitability
        //@{
        void accept(AcyclicVisitor&) override;
        //@}
      protected:
        void initializeDates() override;
        Natural settlementDays_;
        Period tenor_;
        Pillar::Choice pillarChoice_;
        Calendar calendar_;
        BusinessDayConvention fixedConvention_;
        Frequency fixedFrequency_;
        DayCounter fixedDayCount_;
        ext::shared_ptr<IborIndex> iborIndex_;
        ext::shared_ptr<VanillaSwap> swap_;
        RelinkableHandle<YieldTermStructure> termStructureHandle_;
        Handle<Quote> spread_;
        bool endOfMonth_;
        Period fwdStart_;
        Handle<YieldTermStructure> discountHandle_;
        RelinkableHandle<YieldTermStructure> discountRelinkableHandle_;
    };

    //! Rate helper for bootstrapping over BMA swap rates
    class BMASwapRateHelper : public RelativeDateRateHelper {
      public:
        BMASwapRateHelper(const Handle<Quote>& liborFraction,
                          const Period& tenor, // swap maturity
                          Natural settlementDays,
                          Calendar calendar,
                          // bma leg
                          const Period& bmaPeriod,
                          BusinessDayConvention bmaConvention,
                          DayCounter bmaDayCount,
                          ext::shared_ptr<BMAIndex> bmaIndex,
                          // ibor leg
                          ext::shared_ptr<IborIndex> index);
        //! \name RateHelper interface
        //@{
        Real impliedQuote() const override;
        void setTermStructure(YieldTermStructure*) override;
		//********************************************************************
		//Deriscope: Added inspectors
		Period const & tenor() const { return tenor_; }
		Natural const settlementDays() const { return settlementDays_; }
		Calendar const & calendar() const { return calendar_; }
		Period const & bmaPeriod() const { return bmaPeriod_; }
		BusinessDayConvention const bmaConvention() const { return bmaConvention_; }
		DayCounter const & bmaDayCount() const { return bmaDayCount_; }
		ext::shared_ptr<BMAIndex> const & bmaIndex() const { return bmaIndex_; }
		ext::shared_ptr<IborIndex> const & iborIndex() const { return iborIndex_; }
		ext::shared_ptr<BMASwap> const & swap() const { return swap_; }
		//********************************************************************
        //@}
        //! \name Visitability
        //@{
        void accept(AcyclicVisitor&) override;
        //@}
    protected:
      void initializeDates() override;
      Period tenor_;
      Natural settlementDays_;
      Calendar calendar_;
      Period bmaPeriod_;
      BusinessDayConvention bmaConvention_;
      DayCounter bmaDayCount_;
      ext::shared_ptr<BMAIndex> bmaIndex_;
      ext::shared_ptr<IborIndex> iborIndex_;

      ext::shared_ptr<BMASwap> swap_;
      RelinkableHandle<YieldTermStructure> termStructureHandle_;
    };

    //! Rate helper for bootstrapping over Fx Swap rates
    /*! The forward is given by `fwdFx = spotFx + fwdPoint`.

        `isFxBaseCurrencyCollateralCurrency` indicates if the base
        currency of the FX currency pair is the one used as collateral.

        `calendar` is usually the joint calendar of the two currencies
        in the pair.

        `tradingCalendar` can be used when the cross pairs don't
        include the currency of the business center (usually USD; the
        corresponding calendar is `UnitedStates`).  If given, it will
        be used for adjusting the earliest settlement date and for
        setting the latest date. Due to FX spot market conventions, it
        is not sufficient to pass a JointCalendar with UnitedStates
        included as `calendar`; with regard the earliest date, this
        calendar is only used in case the spot date of the two
        currencies is not a US business day.

        \warning The ON fx swaps can be achieved by setting
                 `fixingDays` to 0 and using a tenor of '1d'. The same
                 tenor should be used for TN swaps, with `fixingDays`
                 set to 1.  However, handling ON and TN swaps for
                 cross rates without USD is not trivial and should be
                 treated with caution. If today is a US holiday, ON
                 trade is not possible. If tomorrow is a US Holiday,
                 the ON trade will be at least two business days long
                 in the other countries and the TN trade will not
                 exist. In such cases, if this helper is used for
                 curve construction, probably it is safer not to pass
                 a trading calendar to the ON and TN helpers and
                 provide fwdPoints that will yield proper level of
                 discount factors.
    */
    class FxSwapRateHelper : public RelativeDateRateHelper {
      public:
        FxSwapRateHelper(const Handle<Quote>& fwdPoint,
                         Handle<Quote> spotFx,
                         const Period& tenor,
                         Natural fixingDays,
                         Calendar calendar,
                         BusinessDayConvention convention,
                         bool endOfMonth,
                         bool isFxBaseCurrencyCollateralCurrency,
                         Handle<YieldTermStructure> collateralCurve,
                         Calendar tradingCalendar = Calendar());
        //! \name RateHelper interface
        //@{
        Real impliedQuote() const override;
        void setTermStructure(YieldTermStructure*) override;
        //@}
        //! \name FxSwapRateHelper inspectors
        //@{
        Real spot() const { return spot_->value(); }
        Period tenor() const { return tenor_; }
        Natural fixingDays() const { return fixingDays_; }
        Calendar calendar() const { return cal_; }
        BusinessDayConvention businessDayConvention() const { return conv_; }
        bool endOfMonth() const { return eom_; }
        bool isFxBaseCurrencyCollateralCurrency() const {
                                return isFxBaseCurrencyCollateralCurrency_; }
        Calendar tradingCalendar() const { return tradingCalendar_; }
        Calendar adjustmentCalendar() const { return jointCalendar_; }
        //@}
        //! \name Visitability
        //@{
        void accept(AcyclicVisitor&) override;
        //@}
    private:
      void initializeDates() override;
      Handle<Quote> spot_;
      Period tenor_;
      Natural fixingDays_;
      Calendar cal_;
      BusinessDayConvention conv_;
      bool eom_;
      bool isFxBaseCurrencyCollateralCurrency_;

      RelinkableHandle<YieldTermStructure> termStructureHandle_;

      Handle<YieldTermStructure> collHandle_;
      RelinkableHandle<YieldTermStructure> collRelinkableHandle_;

      Calendar tradingCalendar_;
      Calendar jointCalendar_;
    };

    // inline

    inline Spread SwapRateHelper::spread() const {
        return spread_.empty() ? 0.0 : spread_->value();
    }

    inline ext::shared_ptr<VanillaSwap> SwapRateHelper::swap() const {
        return swap_;
    }

    inline const Period& SwapRateHelper::forwardStart() const {
        return fwdStart_;
    }

}

#endif


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * 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.
 */

#include "embed_pla.hpp"
#include "synthesis_utils_p.hpp"

#include <fstream>

#include <boost/algorithm/string/join.hpp>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/irange.hpp>

#include <gmpxx.h>

#include <core/io/pla_parser.hpp>
#include <core/utils/timer.hpp>

namespace cirkit
{

BDD create_bdd_from_incube(const rcbdd& cf, const std::string& incube, unsigned offset = 0u, std::vector<BDD>* dont_cares = 0)
{
  BDD cube = cf.manager().bddOne();

  for (unsigned i = 0u; i < incube.size(); ++i)
  {
    switch (incube.at(i))
    {
    case '0':
      cube &= !cf.x(offset + i);
      break;
    case '1':
      cube &= cf.x(offset + i);
      break;
    case '-':
      if (dont_cares)
      {
        *dont_cares += cf.x(offset + i);
      }
      break;
    }
  }

  return cube;
}

BDD create_bdd_from_outcube(const rcbdd& cf, const std::string& outcube)
{
  BDD cube = cf.manager().bddOne();

  for (unsigned i = 0u; i < outcube.size(); ++i)
  {
    switch (outcube.at(i))
    {
    case '1':
      cube &= cf.y(i);
      break;
    case '0':
    case '~':
    case '-':
      //cube &= !cf.y(i);
      break;
    }
  }

  return cube;
}

std::vector<BDD> _dec(const rcbdd& cf, const std::vector<BDD>& vars)
{
  std::vector<BDD> outputs;

  for (unsigned i = 0u; i < vars.size(); ++i)
  {
    BDD cube = cf.manager().bddOne();

    for (unsigned j = 0u; j < i; ++j)
    {
      cube &= !vars.at(j);
    }

    outputs += vars.at(i) ^ cube;
  }

  return outputs;
}

class embed_pla_processor : public pla_processor
{
public:
  embed_pla_processor( rcbdd& _cf )
    : cf( _cf ),
      n( 0u ), m( 0u ),
      variables_generated( false )
  {
    u = cf.manager().bddZero();
  }

  void on_num_inputs( unsigned num_inputs )
  {
    cf.set_num_inputs( n = num_inputs );
  }

  void on_num_outputs( unsigned num_outputs )
  {
    cf.set_num_outputs( m = num_outputs );
  }

  void on_input_labels( const std::vector<std::string>& input_labels )
  {
    cf.set_input_labels( input_labels );
  }

  void on_output_labels( const std::vector<std::string>& output_labels )
  {
    cf.set_output_labels( output_labels );
  }

  void on_cube( const std::string& in, const std::string& out )
  {
    if (!variables_generated)
    {
      cf.create_variables(n);
      variables_generated = false;
    }

    cubes += std::make_pair(in, out);

    /* Calculate number of patterns */
    BDD cube = create_bdd_from_incube(cf, in);

    mpz_class patterns(cube.CountMinterm(n));

    /* Update entry in mu */
    if (mu.find(out) == mu.end()) {
      mu[out] = patterns;
    } else {
      mu[out] += patterns;
    }

    /* Update used cubes BDD */
    u |= cube;
  }

public:
  rcbdd& cf;
  unsigned n, m;
  BDD u;
  std::map<std::string, mpz_class> mu;
  std::vector<std::pair<std::string, std::string> > cubes;

private:
  bool variables_generated;
};

bool embed_pla( rcbdd& cf, const std::string& filename,
                properties::ptr settings,
                properties::ptr statistics )
{
  /* Settings */
  bool        verbose     = get( settings, "verbose",     false         );
  bool        truth_table = get( settings, "truth_table", false         ); /* prints the truth table (for debugging) */
  std::string write_pla   = get( settings, "write_pla",   std::string() );
  bool        const_value = get( settings, "const_value", false         ); /* value that is used for constant embedding */

  /* Timing */
  properties_timer t( statistics );

  /* BDD manager? */
  cf.initialize_manager();

  /* Constant value */
  cf.set_constant_value( const_value );

  /* Parser */
  embed_pla_processor p( cf );
  pla_parser( filename, p );

  /* Cubes that map to zero */
  int *cube;
  CUDD_VALUE_TYPE value;
  DdGen *gen;

  BDD zeroCubes = !p.u;
  std::string zerocube = std::string(p.m, '0');
  if (p.mu.find(zerocube) == p.mu.end())
  {
    p.mu[zerocube] = 0;
  }

  Cudd_ForeachCube(zeroCubes.manager(), zeroCubes.getNode(), gen, cube, value)
  {
    std::string incube;
    char c;
    for (unsigned i = 0u; i < p.n; ++i)
    {
      c = cube[3u * i];
      switch (c)
      {
      case 0:
        incube += "0";
        break;
      case 1:
        incube += "1";
        break;
      case 2:
        incube += "-";
        break;
      }
    }

    p.mu[zerocube] += mpz_class(create_bdd_from_incube(cf, incube).CountMinterm(p.n));
    p.cubes += std::make_pair(incube, zerocube);
  }

  /* Maximum MU */
  using boost::adaptors::map_values;
  mpz_class maxmu = *boost::max_element(p.mu | map_values);
  unsigned req_vars = calculate_required_lines(p.n, p.m, maxmu);

  cf.create_variables(req_vars);

  /* Now create the BDD */
  BDD func = cf.manager().bddZero();
  p.mu.clear();
  p.u = cf.manager().bddZero();

  /* We store the garbage variables */
  std::vector<BDD> garbage;
  for (int i = req_vars - 1; i >= static_cast<int>( p.m ); --i)
  {
    garbage += cf.y(i);
  }

  /* Memoize dec garbage */
  std::vector<std::vector<BDD>> dec_garbage_store;
  dec_garbage_store += garbage;

  for (const auto& cube : p.cubes) {
    /* Assign cubes to local variables */
    const std::string& incube = cube.first;
    const std::string& outcube = cube.second;

    std::vector<BDD> dont_cares;
    BDD icube = create_bdd_from_incube(cf, incube, req_vars - p.n, &dont_cares);
    BDD ocube = create_bdd_from_outcube(cf, outcube);
    mpz_class patterns(icube.CountMinterm(p.n));

    /* compute h */
    BDD constants = cf.manager().bddOne();
    for ( unsigned i = 0; i < req_vars - p.n; ++i ) { constants += cf.x( i ); }
    BDD h = cf.remove_ys( func ).ExistAbstract(constants);
    assert( !h & icube == icube );

    /* add new cubes */
    BDD fcube = cf.manager().bddOne();
    for (unsigned i = 0u; i < req_vars - p.n; i++)
    {
      fcube &= ( const_value ? cf.x(i) : !cf.x(i) );
    }
    fcube &= !h & icube & ocube;

    if (p.mu.find(outcube) == p.mu.end()) {
      p.mu[outcube] = 0u;
    }

    if ( p.mu[outcube].get_ui() >= dec_garbage_store.size() )
    {
      for ( unsigned i = dec_garbage_store.size(); i <= p.mu[outcube].get_ui(); ++i )
      {
        dec_garbage_store += _dec( cf, dec_garbage_store[i - 1u] );
      }
    }
    auto dec_garbage = dec_garbage_store[p.mu[outcube].get_ui()];
    //std::vector<BDD> dec_garbage = garbage;

    /*for (unsigned i = 0u; i < p.mu[outcube]; ++i)
    {
      dec_garbage = _dec(cf, dec_garbage);
      }*/

    /* Assign don't cares to dec_garbage (from back to front) */
    for (unsigned i = 0u; i < (req_vars - p.m); ++i)
    {
      if (i < dont_cares.size())
      {
        fcube &= dec_garbage.at(i).Xnor(dont_cares.at(dont_cares.size() - i - 1u));
      }
      else
      {
        fcube &= !dec_garbage.at(i);
      }
    }

    func |= fcube;
    p.mu[outcube] += patterns;

    /* Update existing cubes */
    assert( (h & icube) == cf.manager().bddZero() );
    func &= !(h & icube) | ocube;
  }

  /* Assign zeros */
  for ( unsigned i = 0; i < p.m; ++i )
  {
    BDD f0 = func.Cofactor( !cf.y( i ) );
    BDD f1 = func.Cofactor(  cf.y( i ) );

    func = ((f1 & !f0) & cf.y( i )) | ((f1 & f0) & !cf.y( i ));
  }

  if ( verbose )
  {
    func.PrintMinterm();
    std::cout << "|f|:   " << func.CountMinterm( 2 * cf.num_vars() ) << std::endl;
    std::cout << "|f_x|: " << cf.remove_ys( func ).CountMinterm( cf.num_vars() ) << std::endl;
    std::cout << "|f_y|: " << cf.remove_xs( func ).CountMinterm( cf.num_vars() ) << std::endl;
  }

  cf.set_chi( func );

  if ( truth_table )
  {
    cf.print_truth_table();
  }

  if ( write_pla.size() )
  {
    cf.write_pla( write_pla );
  }

  return true;

}

pla_embedding_func embed_pla_func( properties::ptr settings, properties::ptr statistics )
{
  pla_embedding_func f = [&settings, &statistics]( rcbdd& cf, const std::string& filename ) {
    return embed_pla( cf, filename, settings, statistics );
  };
  f.init( settings, statistics );
  return f;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


/*
 system.cpp

 Copyright (c) 2014, 2015, 2016 Terumasa Tadano

 This file is distributed under the terms of the MIT license.
 Please see the file 'LICENCE.txt' in the root directory
 or http://opensource.org/licenses/mit-license.php for information.
*/

#include <iostream>
#include <iomanip>
#include <fstream>
#include "system.h"
#include "constants.h"
#include "mathfunctions.h"
#include "timer.h"
#include "memory.h"
#include "error.h"
#include "constraint.h"
#include "fcs.h"
#include "symmetry.h"
#include "fitting.h"
#include "xml_parser.h"
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/foreach.hpp>
#include <boost/optional.hpp>
#include <boost/lexical_cast.hpp>

using namespace ALM_NS;

System::System(ALM *alm): Pointers(alm) {}

System::~System()
{
    memory->deallocate(x_cartesian);
    memory->deallocate(atomlist_class);
    memory->deallocate(magmom);
}

void System::init()
{
    using namespace std;

    int i, j;

    cout << " SYSTEM" << endl;
    cout << " ======" << endl << endl;

    recips(lavec, rlavec);

    cout.setf(ios::scientific);

    cout << "  Lattice Vector" << endl;
    cout << setw(16) << lavec[0][0] << setw(15) << lavec[1][0] << setw(15) << lavec[2][0] << " : a1" << endl;
    cout << setw(16) << lavec[0][1] << setw(15) << lavec[1][1] << setw(15) << lavec[2][1] << " : a2" << endl;
    cout << setw(16) << lavec[0][2] << setw(15) << lavec[1][2] << setw(15) << lavec[2][2] << " : a3" << endl;
    cout << endl;

    double vec_tmp[3][3];
    for (i = 0; i < 3; ++i) {
        for (j = 0; j < 3; ++j) {
            vec_tmp[i][j] = lavec[j][i];
        }
    }

    cell_volume = volume(vec_tmp[0], vec_tmp[1], vec_tmp[2]);
    cout << "  Cell volume = " << cell_volume << " (a.u)^3" << endl << endl;

    cout << "  Reciprocal Lattice Vector" << std::endl;
    cout << setw(16) << rlavec[0][0] << setw(15) << rlavec[0][1] << setw(15) << rlavec[0][2] << " : b1" << endl;
    cout << setw(16) << rlavec[1][0] << setw(15) << rlavec[1][1] << setw(15) << rlavec[1][2] << " : b2" << endl;
    cout << setw(16) << rlavec[2][0] << setw(15) << rlavec[2][1] << setw(15) << rlavec[2][2] << " : b3" << endl;
    cout << endl;

    cout << "  Atomic species:" << endl;
    for (i = 0; i < nkd; ++i) {
        cout << setw(6) << i + 1 << setw(5) << kdname[i] << endl;
    }
    cout << endl;

    cout << "  Atomic positions in fractional basis and atomic species" << endl;
    for (i = 0; i < nat; ++i) {
        cout << setw(6) << i + 1;
        cout << setw(15) << xcoord[i][0];
        cout << setw(15) << xcoord[i][1];
        cout << setw(15) << xcoord[i][2];
        cout << setw(5) << kd[i] << endl;
    }
    cout << endl << endl;
    cout.unsetf(ios::scientific);

    // Generate Cartesian coordinate

    memory->allocate(x_cartesian, nat, 3);

    for (i = 0; i < nat; ++i) {
        for (j = 0; j < 3; ++j) {
            x_cartesian[i][j] = xcoord[i][j];
        }
    }
    frac2cart(x_cartesian);
    setup_atomic_class(kd);

    if (lspin) {
        cout << "  MAGMOM is given. The magnetic moments of each atom are as follows:" << endl;
        for (i = 0; i < nat; ++i) {
            cout << setw(6) << i + 1;
            cout << setw(5) << magmom[i][0];
            cout << setw(5) << magmom[i][1];
            cout << setw(5) << magmom[i][2];
            cout << endl;
        }
        cout << endl;
        if (noncollinear == 0) {
            cout << "  NONCOLLINEAR = 0: magnetic moments are considered as scalar variables." << endl;
        } else if (noncollinear == 1) {
            cout << "  NONCOLLINEAR = 1: magnetic moments are considered as vector variables." << endl;
            if (symmetry->trev_sym_mag) {
                cout << "  TREVSYM = 1: Time-reversal symmetry will be considered for generating magnetic space group" << endl;
            } else {
                cout << "  TREVSYM = 0: Time-reversal symmetry will NOT be considered for generating magnetic space group" << endl;
            }
        }
        cout << endl << endl;
    }

    timer->print_elapsed();
    cout << " --------------------------------------------------------------" << endl;
    cout << endl;
}

void System::recips(double aa[3][3], double bb[3][3])
{
    /*
    Calculate Reciprocal Lattice Vectors

    Here, BB is just the inverse matrix of AA (multiplied by factor 2 Pi)

    BB = 2 Pi AA^{-1},
    = t(b1, b2, b3)

    (b11 b12 b13)
    = (b21 b22 b23)
    (b31 b32 b33),

    b1 = t(b11, b12, b13) etc.
    */

    double det;
    det = aa[0][0] * aa[1][1] * aa[2][2]
    + aa[1][0] * aa[2][1] * aa[0][2]
    + aa[2][0] * aa[0][1] * aa[1][2]
    - aa[0][0] * aa[2][1] * aa[1][2]
    - aa[2][0] * aa[1][1] * aa[0][2]
    - aa[1][0] * aa[0][1] * aa[2][2];

    if (std::abs(det) < eps12) {
        error->exit("recips", "Lattice Vector is singular");
    }

    double factor = 2.0 * pi / det;

    bb[0][0] = (aa[1][1] * aa[2][2] - aa[1][2] * aa[2][1]) * factor;
    bb[0][1] = (aa[0][2] * aa[2][1] - aa[0][1] * aa[2][2]) * factor;
    bb[0][2] = (aa[0][1] * aa[1][2] - aa[0][2] * aa[1][1]) * factor;

    bb[1][0] = (aa[1][2] * aa[2][0] - aa[1][0] * aa[2][2]) * factor;
    bb[1][1] = (aa[0][0] * aa[2][2] - aa[0][2] * aa[2][0]) * factor;
    bb[1][2] = (aa[0][2] * aa[1][0] - aa[0][0] * aa[1][2]) * factor;

    bb[2][0] = (aa[1][0] * aa[2][1] - aa[1][1] * aa[2][0]) * factor;
    bb[2][1] = (aa[0][1] * aa[2][0] - aa[0][0] * aa[2][1]) * factor;
    bb[2][2] = (aa[0][0] * aa[1][1] - aa[0][1] * aa[1][0]) * factor;
}

void System::frac2cart(double **xf)
{
    // x_cartesian = A x_fractional

    int i, j;

    double *x_tmp;
    memory->allocate(x_tmp, 3);

    for (i = 0; i < nat; ++i) {

        rotvec(x_tmp, xf[i], lavec);

        for (j = 0; j < 3; ++j) {
            xf[i][j] = x_tmp[j];
        }
    }
    memory->deallocate(x_tmp);
}

void System::load_reference_system_xml(std::string file_reference_fcs, const int order_fcs, double *const_out)
{
    using namespace boost::property_tree;
    ptree pt;

    int nat_ref, natmin_ref, ntran_ref;
    int **intpair_ref;
    std::string str_error;
    double *fcs_ref;
    int nfcs_ref;

    try {
        read_xml(file_reference_fcs, pt);
    }
    catch (std::exception &e) {
        if (order_fcs == 0) {
            str_error = "Cannot open file FC2XML ( " + file_reference_fcs + " )";
        } else if (order_fcs == 1) {
            str_error = "Cannot open file FC3XML ( " + file_reference_fcs + " )";
        }
        error->exit("load_reference_system_xml", str_error.c_str());
    }

    nat_ref = boost::lexical_cast<int>(get_value_from_xml(pt, "Data.Structure.NumberOfAtoms"));
    ntran_ref = boost::lexical_cast<int>(get_value_from_xml(pt, "Data.Symmetry.NumberOfTranslations"));
    natmin_ref = nat_ref / ntran_ref;

    if (natmin_ref != symmetry->natmin) {
        error->exit("load_reference_system_xml", "The number of atoms in the primitive cell is not consistent.");
    }

    if (order_fcs == 0) {
        nfcs_ref = boost::lexical_cast<int>(get_value_from_xml(pt, "Data.ForceConstants.HarmonicUnique.NFC2"));

        if (nfcs_ref != fcs->ndup[0].size()) {
            error->exit("load_reference_system_xml", "The number of harmonic force constants is not the same.");
        }

    } else if (order_fcs == 1) {
        nfcs_ref = boost::lexical_cast<int>(get_value_from_xml(pt, "Data.ForceConstants.CubicUnique.NFC3"));

        if (nfcs_ref != fcs->ndup[1].size()) {
            error->exit("load_reference_system_xml", "The number of cubic force constants is not the same.");
        }
    }
    memory->allocate(fcs_ref, nfcs_ref);
    memory->allocate(intpair_ref, nfcs_ref, 3);

    int counter = 0;

    if (order_fcs == 0) {
        BOOST_FOREACH (const ptree::value_type& child_, pt.get_child("Data.ForceConstants.HarmonicUnique")) {
            if (child_.first == "FC2") {
                const ptree& child = child_.second;
                const std::string str_intpair = child.get<std::string>("<xmlattr>.pairs");
                const std::string str_multiplicity = child.get<std::string>("<xmlattr>.multiplicity");

                std::istringstream is(str_intpair);
                is >> intpair_ref[counter][0] >> intpair_ref[counter][1];
                fcs_ref[counter] = boost::lexical_cast<double>(child.data());
                ++counter;
            }
        }
    } else if (order_fcs == 1) {
        BOOST_FOREACH (const ptree::value_type& child_, pt.get_child("Data.ForceConstants.CubicUnique")) {
            if (child_.first == "FC3") {
                const ptree& child = child_.second;
                const std::string str_intpair = child.get<std::string>("<xmlattr>.pairs");
                const std::string str_multiplicity = child.get<std::string>("<xmlattr>.multiplicity");

                std::istringstream is(str_intpair);
                is >> intpair_ref[counter][0] >> intpair_ref[counter][1] >> intpair_ref[counter][2];
                fcs_ref[counter] = boost::lexical_cast<double>(child.data());
                ++counter;
            }
        }
    }

    int i;
    std::set<FcProperty> list_found;
    std::set<FcProperty>::iterator iter_found;
    int *ind;
    int nterms = order_fcs + 2;
    memory->allocate(ind, nterms);

    list_found.clear();

    for (std::vector<FcProperty>::iterator p = fcs->fc_set[order_fcs].begin();
        p != fcs->fc_set[order_fcs].end(); ++p) {
            FcProperty list_tmp = *p; // Using copy constructor
            for (i = 0; i < nterms; ++i) {
                ind[i] = list_tmp.elems[i];
            }
            list_found.insert(FcProperty(nterms, list_tmp.coef, ind, list_tmp.mother));
    }
    //
    //     for (i = 0; i < nfcs_ref; ++i) {
    //         constraint->const_mat[i][i] = 1.0;
    //     }

    for (i = 0; i < nfcs_ref; ++i) {
        iter_found = list_found.find(FcProperty(nterms, 1.0, intpair_ref[i], 1));
        if (iter_found == list_found.end()) {
            error->exit("load_reference_system", "Cannot find equivalent force constant, number: ", i + 1);
        }
        FcProperty arrtmp = *iter_found;
        const_out[arrtmp.mother] = fcs_ref[i];
    }

    memory->deallocate(intpair_ref);
    memory->deallocate(fcs_ref);
    memory->deallocate(ind);
    list_found.clear();
}

void System::load_reference_system()
{
    int i;
    int iat, jat;
    int icrd;

    unsigned int nat_s, nkd_s;
    unsigned int natmin_ref, ntran_ref;
    double lavec_s[3][3];
    int *kd_s;
    double **xcoord_s;
    int *map_ref;
    int **map_p2s_s;
    Symmetry::Maps *map_s2p_s;
    std::ifstream ifs_fc2;

    ifs_fc2.open(constraint->fc2_file.c_str(), std::ios::in);
    if(!ifs_fc2) error->exit("calc_constraint_matrix", "cannot open file fc2_file");

    bool is_found_system = false;

    int nparam_harmonic_ref;
    int nparam_harmonic = fcs->ndup[0].size();

    std::string str_tmp;

    while(!ifs_fc2.eof() && !is_found_system)
    {
        std::getline(ifs_fc2, str_tmp);
        if (str_tmp == "##SYSTEM INFO") {

            is_found_system = true;

            std::getline(ifs_fc2, str_tmp);
            for (i = 0; i < 3; ++i) {
                ifs_fc2 >> lavec_s[0][i] >> lavec_s[1][i] >> lavec_s[2][i];
            }
            ifs_fc2.ignore();
            std::getline(ifs_fc2, str_tmp);
            ifs_fc2 >> nkd_s;
            ifs_fc2.ignore();
            std::getline(ifs_fc2, str_tmp);
            std::getline(ifs_fc2, str_tmp);

            ifs_fc2 >> nat_s >> natmin_ref >> ntran_ref;

            if (natmin_ref != symmetry->natmin) {
                error->exit("load_reference_system", "The number of atoms in the primitive cell is not consistent");
            }

            if (nat_s != nat) {
                std::cout << "The number of atoms in the reference system differs from input." << std::endl;
                std::cout << "Trying to map the related force constants (^o^)" << std::endl << std::endl;
            }

            memory->allocate(xcoord_s, nat_s, 3);
            memory->allocate(kd_s, nat_s);
            memory->allocate(map_p2s_s, natmin_ref, ntran_ref);
            memory->allocate(map_s2p_s, nat_s);

            unsigned int ikd, itran, icell;
            std::getline(ifs_fc2, str_tmp);
            std::getline(ifs_fc2, str_tmp);
            for (i = 0; i < nat_s; ++i) {
                ifs_fc2 >> str_tmp >> ikd >> xcoord_s[i][0] >> xcoord_s[i][1] >> xcoord_s[i][2] >> itran >> icell;
                kd_s[i] = ikd;
                map_p2s_s[icell - 1][itran - 1] = i;
                map_s2p_s[i].atom_num = icell - 1;
                map_s2p_s[i].tran_num = itran - 1;
            }
        }
    }
    if (!is_found_system) error->exit("load_reference_system", "SYSTEM INFO flag not found in the fc2_file");

    //
    // Generate Mapping Information (big supercell -> small supercell)
    //

    double *xtmp;
    double *xdiff;
    int **intpair_tmp;

    memory->allocate(xtmp, 3);
    memory->allocate(xdiff, 3);
    memory->allocate(map_ref, nat_s);

    bool map_found;
    double dist;

    for (iat = 0; iat < nat_s; ++iat) {
        map_found = false;

        rotvec(xtmp, xcoord_s[iat], lavec_s);
        rotvec(xtmp, xtmp, rlavec);

        for (icrd = 0; icrd < 3; ++icrd) xtmp[icrd] /= 2.0 * pi;

        for (jat = 0; jat < nat; ++jat) {
            for (icrd = 0; icrd < 3; ++icrd) {
                xdiff[icrd] = xtmp[icrd] - xcoord[jat][icrd];
                xdiff[icrd] = std::fmod(xdiff[icrd], 1.0);
            }
            dist = xdiff[0] * xdiff[0] + xdiff[1] * xdiff[1] + xdiff[2] * xdiff[2];

            if (dist < eps12 && kd_s[iat] == kd[jat]) {
                map_ref[iat] = jat;
                map_found = true;
                break;
            }
        }
        if (!map_found) error->exit("load_reference_system", "Could not find an equivalent atom for atom ", iat + 1);
    }

    memory->deallocate(xtmp);
    memory->deallocate(xdiff);
    memory->deallocate(xcoord_s);
    memory->deallocate(kd_s);

    ifs_fc2.clear();
    ifs_fc2.seekg(0, std::ios_base::beg);

    double *fc2_ref;

    bool is_found_fc2 = false;

    while(!ifs_fc2.eof() && !is_found_fc2)
    {
        std::getline(ifs_fc2, str_tmp);
        if (str_tmp == "##HARMONIC FORCE CONSTANTS")
        {
            ifs_fc2 >> nparam_harmonic_ref;
            if (nparam_harmonic_ref < nparam_harmonic) {
                error->exit("load_reference_system", "Reference file doesn't contain necessary fc2. (too few)");
            } else if (nparam_harmonic_ref > nparam_harmonic){
                error->exit("load_reference_system","Reference file contains extra force constants." );
            }

            is_found_fc2 = true;

            memory->allocate(fc2_ref, nparam_harmonic);
            memory->allocate(intpair_tmp, nparam_harmonic, 2);

            for (i = 0; i < nparam_harmonic; ++i) {
                ifs_fc2 >> fc2_ref[i] >> intpair_tmp[i][0] >> intpair_tmp[i][1];
            }

            std::set<FcProperty> list_found;
            std::set<FcProperty>::iterator iter_found;
            int *ind;
            memory->allocate(ind, 2);

            list_found.clear();
            for (std::vector<FcProperty>::iterator p = fcs->fc_set[0].begin(); p != fcs->fc_set[0].end(); ++p) {
                FcProperty list_tmp = *p; // Using copy constructor
                for (i = 0; i < 2; ++i){
                    ind[i] = list_tmp.elems[i];
                }
                list_found.insert(FcProperty(2, list_tmp.coef, ind, list_tmp.mother));
            }

            for (i = 0; i < nparam_harmonic; ++i) {
                constraint->const_mat[i][i] = 1.0;
            }

            for (i = 0; i < nparam_harmonic; ++i) {

                iter_found = list_found.find(FcProperty(2, 1.0, intpair_tmp[i], 1));
                if (iter_found == list_found.end()) {
                    error->exit("load_reference_system", "Cannot find equivalent force constant, number: ", i + 1);
                }
                FcProperty arrtmp = *iter_found;
                constraint->const_rhs[arrtmp.mother] = fc2_ref[i];
            }

            memory->deallocate(intpair_tmp);
            memory->deallocate(ind);
            memory->deallocate(fc2_ref);
            list_found.clear();
        }
    }

    if(!is_found_fc2) error->exit("load_reference_system", "HARMONIC FORCE CONSTANTS flag not found in the fc2_file");
    ifs_fc2.close();
}

double System::volume(double vec1[3], double vec2[3], double vec3[3])
{
    double vol;

    vol = std::abs(vec1[0]*(vec2[1]*vec3[2] - vec2[2]*vec3[1])
        + vec1[1]*(vec2[2]*vec3[0] - vec2[0]*vec3[2])
        + vec1[2]*(vec2[0]*vec3[1] - vec2[1]*vec3[0]));

    return vol;
}

void System::setup_atomic_class(int *kd)
{
    // In the case of collinear calculation, spin moments are considered as scalar
    // variables. Therefore, the same elements with different magnetic moments are
    // considered as different types. In noncollinear calculations,
    // magnetic moments are not considered in this stage. They will be treated
    // separately in symmetry.cpp where spin moments will be rotated and flipped
    // using time-reversal symmetry.

    unsigned int i;
    AtomType type_tmp;
    std::set<AtomType> set_type;
    set_type.clear();

    for (i = 0; i < nat; ++i) {
        type_tmp.element = kd[i];

        if (noncollinear == 0) {
            type_tmp.magmom = magmom[i][2];
        } else {
            type_tmp.magmom = 0.0;
        }
        set_type.insert(type_tmp);
    }

    nclassatom = set_type.size();

    memory->allocate(atomlist_class, nclassatom);

    for (i = 0; i < nat; ++i) {
        int count = 0;
        for (std::set<AtomType>::iterator it = set_type.begin(); it != set_type.end(); ++it) {
            if (noncollinear) {
                if (kd[i] == (*it).element) {
                    atomlist_class[count].push_back(i);
                }
            } else {
                if (kd[i] == (*it).element && std::abs(magmom[i][2] - (*it).magmom) < eps6) {
                    atomlist_class[count].push_back(i);
                }
            }
            ++count;
        }
    }
    set_type.clear();
}


#include <algorithm>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/multiprecision/gmp.hpp>
#include <cstdlib>
#include <iterator>
#include <memory>
#include <sodium.h>
#include <span>
#include <stdexcept>
#include <stdint.h>
#include <string>
#include <vector>

namespace usebitcoins
{
namespace hash
{

namespace
{
uint64_t from_byte_array(const uint8_t* src, size_t n)
{
    n = std::max(0UL, std::min(8UL, n));
    uint64_t out = 0;
    for (int i = 0; i < n; ++i) {
    	out |= static_cast<uint64_t>(src[i] & 0xff) << (8 * i);
    }
    return out;
}

/// Condense a long hash into a u64 by repeatedly XORing its 8 byte pieces
uint64_t condense_hash(const std::vector<uint8_t>& byte_array)
{
    assert(byte_array.size() % 8 == 0);
    uint64_t out = 0;
    for (int i = 0; i < byte_array.size() / 8; ++i) {
	std::ptrdiff_t offset = i * 8;
	uint64_t chunk =
		from_byte_array(byte_array.data() + offset,
				std::min(8UL, byte_array.size() - offset));
	out ^= chunk;
    }
    return out;
}

} // namespace

/// Fit a byte array into a bignum
/// Assumes that `src` is arranged in little-endian order
boost::multiprecision::uint256_t
pack_byte_array(const std::vector<uint8_t>& src)
{
    using namespace boost::multiprecision;
    size_t len = src.size();
    if (len % sizeof(limb_type)) {
	throw std::runtime_error{
		"length of byte array is not a multiple of the "
		"boost::multiprecision `limb_type`; the source byte array "
		"won't perfectly encode to the bignum"};
    }
    uint256_t out{};
    uint32_t sz = static_cast<uint32_t>(len) / sizeof(limb_type);
    out.backend().resize(sz, sz);
    ::memcpy(out.backend().limbs(), src.data(), src.size());
    out.backend().normalize();
    return out;
}

using email_address_t = std::string;

/// Perform straightforward hash of arbitrary data
std::vector<uint8_t> blake2_hash(const std::vector<uint8_t>& src)
{
    // TODO wrap libsodium's implementation of BLAKE2
    // std::vector<uint8_t> out{crypto_generichash_BYTES, 0};
    std::unique_ptr<uint8_t[]> buf{new uint8_t[crypto_generichash_BYTES]};
    auto err = crypto_generichash(buf.get(), crypto_generichash_BYTES,
				  src.data(), src.size(), nullptr, 0);
    if (err > 0) {
	throw std::runtime_error{""};
    }
    std::vector<uint8_t> out{};
    out.reserve(crypto_generichash_BYTES);
    std::copy(buf.get(), buf.get() + crypto_generichash_BYTES,
	      std::back_inserter(out));
    return out;
}

std::vector<uint8_t> hash_string(std::string_view src) {
    std::vector<uint8_t> bytes;
    std::copy(src.begin(), src.end(), std::back_inserter(bytes));
    return bytes;
}

uint64_t hash_email(const email_address_t& email_address)
{
    auto email_address_bytes = hash_string(email_address);
    auto hash = blake2_hash(email_address_bytes);
    assert(hash.size() >= 32);
    uint64_t out = condense_hash(hash);
    return out;
}

} // namespace hash
} // namespace usebitcoins


#ifndef STATS_HPP
#define STATS_HPP

#include <iomanip>
#include <sstream>
#include <ss/stats.hpp>
#include "data/data.hpp"
#include <boost/format.hpp>
#include <boost/algorithm/string/join.hpp>
#include <boost/range/adaptor/transformed.hpp>

namespace Anaquin
{
    static inline std::string d2str(double x)
    {
        std::ostringstream out;
        out << std::setprecision(6) << x;
        return out.str();
    }

    static inline std::string x2str(unsigned x)
    {
        return std::to_string(x);
    }

    template <typename T> static std::string concat(const std::vector<T> &x, std::string (*f)(T) = d2str)
    {
        return boost::algorithm::join(x | boost::adaptors::transformed(static_cast<std::string(*)(T)>(f)), ", ");
    }

    static std::string concat(const std::vector<std::string> &x, const std::string &join = "\'")
    {
        return (join + boost::algorithm::join(x, join + ", " + join) + join);
    }

    #define STRING(x) static_cast<std::string>(x)

    template <typename T> class SSamples
    {
        public:

            inline void add(const T &x)
            {
                _data.push_back(x);
            }

            inline std::size_t size() const { return _data.size(); }

            virtual operator std::string() const = 0;

        protected:

            std::vector<T> _data;
    };

    struct SReals : public SSamples<double>
    {
        inline double mean() const
        {
            auto data = SSamples<double>::_data;

            // Some samples might not define...
            data.erase(std::remove_if(data.begin(), data.end(),[](double x) { return std::isnan(x); }), data.end());

            return SS::mean(data);
        }

        virtual operator std::string() const
        {
            auto data = SSamples<double>::_data;

            // Some samples might not define...
            data.erase(std::remove_if(data.begin(), data.end(),[](double x) { return std::isnan(x); }), data.end());

            if (data.size() == 0)
            {
                return "-";
            }
            else if (data.size() > 1)
            {
                return (boost::format("%1$.2f \u00B1 %2$.2f") % SS::mean(data) % SS::getSD(data)).str();
            }
            else
            {
                return (boost::format("%1$.2f") % data.front()).str();
            }
        }
    };

    struct SStrings : public SSamples<std::string>
    {
        virtual operator std::string() const
        {
            return concat(SSamples<std::string>::_data, "");
        }
    };

    struct SPercent : public SReals
    {
        virtual operator std::string() const
        {
            return SReals::operator std::string() + "%";
        }
    };

    struct SProps : public SReals
    {
        // Emtpy Implementation
    };

    struct SCounts : public SSamples<Counts>
    {
        virtual operator std::string() const
        {
            const auto &data = SSamples<Counts>::_data;

            if (data.size() == 0)
            {
                return "-";
            }
            else if (data.size() > 1)
            {
                return (boost::format("%1% \u00B1 %2%") % SS::mean(data) % SS::getSD(data)).str();
            }
            else
            {
                return (boost::format("%1%") % data.front()).str();
            }
        }
    };
}

#endif