//    Copyright 2021 Jij Inc.

//    Licensed under the Apache License, Version 2.0 (the "License");
//    you may not use this file except in compliance with the License.
//    You may obtain a copy of the License at

//        http://www.apache.org/licenses/LICENSE-2.0

//    Unless required by applicable law or agreed to in writing, software
//    distributed under the License is distributed on an "AS IS" BASIS,
//    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//    See the License for the specific language governing permissions and
//    limitations under the License.

/**
 * @mainpage cimod
 *
 * @section s_overview Overview
 * cimod is a C++ library for a binary quadratic model.
 * This library provides a binary quadratic model class which contains an Ising model or a quadratic unconstrained binary optimization (QUBO) model.
 * It also provides utilities for constructing a model and transforming to some other interfaces.
 * This library is created based on dimod (https://github.com/dwavesystems/dimod).
 *
 * @section s_bqm Binary quadratic model
 * A binary quadratic model class can contain an Ising model or a QUBO model.
 *
 * @subsection ss_ising Ising model
 * An energy of an Ising model \f$E_{\mathrm{Ising}}\f$ is represented by
 * \f[
 * E_{\mathrm{Ising}} = \sum_{i} h_i s_i + \sum_{i \neq j} J_{ij} s_i s_j + \delta_{\mathrm{Ising}},
 * \f]
 * where \f$s_i \in \{+1, -1\}\f$ denotes a spin at the site \f$i\f$, \f$h_i\f$ denotes an external magnetic field parameter, \f$J_{ij}\f$ denotes an interaction parameter and \f$\delta_{\mathrm{Ising}}\f$ denotes an offset.
 * Note that this library assumes that the interaction is not symmetric, i.e., \f$J_{ij} \neq J_{ji}\f$.
 *
 * @subsection ss_qubo QUBO model
 * An evaluation function of a QUBO model \f$E_{\mathrm{QUBO}}\f$ is represented by
 * \f[
 * E_{\mathrm{QUBO}} = \sum_{i, j} Q_{ij} x_i x_j + \delta_{\mathrm{QUBO}},
 * \f]
 * where \f$x_i \in \{0, 1\}\f$ denotes a decision variable, \f$Q_{ij}\f$ denotes a quadratic bias and \f$\delta_{\mathrm{QUBO}}\f$ denotes an offset.
 * Note that this library assumes that the quadratic bias is not symmetric, i.e., \f$Q_{ij} \neq Q_{ji}\f$ if \f$i \neq j\f$.
 *
 * @section s_example Example
 * @code
 * #include "src/binary_quadratic_model.hpp"
 *
 * using namespace cimod;
 * int main()
 * {
 * // Set linear biases and quadratic biases
 * Linear<uint32_t, double> linear{ {1, 1.0}, {2, 2.0}, {3, 3.0}, {4, 4.0} };
 * Quadratic<uint32_t, double> quadratic
 * {
 *      {std::make_pair(1, 2), 12.0}, {std::make_pair(1, 3), 13.0}, {std::make_pair(1, 4), 14.0},
 *      {std::make_pair(2, 3), 23.0}, {std::make_pair(2, 4), 24.0},
 *      {std::make_pair(3, 4), 34.0}
 *  };
 *
 * // Set offset
 * double offset = 0.0;
 *
 * // Set variable type
 * Vartype vartype = Vartype::BINARY;
 * // Create a BinaryQuadraticModel instance
 * BinaryQuadraticModel<uint32_t, double> bqm(linear, quadratic, offset, vartype);
 *
 * // Print informations of bqm
 * bqm.print();
 *
 * return 0;
 * }
 * @endcode
 */

/**
 * @file binary_quadratic_model.hpp
 * @author Kohji Nishimura
 * @brief Dense BinaryQuadraticModel class
 * @version 1.0.0
 * @date 2020-03-24
 *
 * @copyright Copyright (c) Jij Inc. 2020
 *
 */

#ifndef BINARY_QUADRATIC_MODEL_HPP__
#define BINARY_QUADRATIC_MODEL_HPP__

#include "disable_eigen_warning.hpp"

#include "vartypes.hpp"
#include "hash.hpp"
#include "utilities.hpp"
#include "json.hpp"

#include <algorithm>
#include <type_traits>
#include <limits>
#include <cstdint>
#include <cassert>
#include <iostream>
#include <string>
#include <tuple>
#include <typeinfo>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>
#include <functional>
#include <Eigen/Dense>
#include <Eigen/Sparse>

namespace cimod
{
    /**
     * @brief Type alias for linear bias
     *
     * @tparam IndexType
     */
    template <typename IndexType, typename FloatType>
    using Linear = std::unordered_map<IndexType, FloatType>;

    /**
     * @brief Type alias for quadratic bias
     *
     * @tparam IndexType
     */
    template <typename IndexType, typename FloatType>
    using Quadratic = std::unordered_map<std::pair<IndexType, IndexType>, FloatType, pair_hash>;

    /**
     * @brief Type alias for adjacency list
     *
     * @tparam IndexType
     */
    template <typename IndexType, typename FloatType>
    using Adjacency = std::unordered_map<IndexType, std::unordered_map<IndexType, FloatType>>;

    /**
     * @brief Type alias for sample
     *
     * @tparam IndexType
     */
    template <typename IndexType>
    using Sample = std::unordered_map<IndexType, int32_t>;

    struct Dense{};
    struct Sparse{};

    /**
     * @brief Class for dense binary quadratic model.
     * @tparam IndexType index type. type must be hashable and comparable.
     * @tparam FloatType
     * @tparam IndexType
     */

    template <typename IndexType, typename FloatType, typename DataType>
    class BinaryQuadraticModel
    {
    private:

        /**
         * @brief template type for dispatch
         * used for SFINAE
         *
         * @tparam T
         * @tparam U
         */
        template<typename T, typename U>
        using dispatch_t = std::enable_if_t<std::is_same_v<T, U>, std::nullptr_t>;
    public:
    /**
     * @brief Eigen Matrix
     * if DataType is Dense , Matrix is equal to Eigen::Matrix
     * if DataType is Sparse, Matrix is equal to Eigen::SparseMatrix
     */

    using DenseMatrix   = Eigen::Matrix<FloatType, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
    using SparseMatrix  = Eigen::SparseMatrix<FloatType, Eigen::RowMajor>;
    using SpIter        = typename SparseMatrix::InnerIterator;

    using Matrix = std::conditional_t<
            std::is_same_v<DataType, Dense>,
            DenseMatrix,
            SparseMatrix
          >;

    using Vector = Eigen::Matrix<FloatType, Eigen::Dynamic, 1>;

    protected:

        /**
         * @brief quadratic dense-type matrix
         * The stored matrix has the following triangular form:
         *
         * \f[
         * \begin{pmatrix}
         * J_{0,0} & J_{0,1} & \cdots & J_{0,N-1} & h_{0}\\
         * 0 & J_{1,1} & \cdots & J_{1,N-1} & h_{1}\\
         * \vdots & \vdots & \vdots & \vdots & \vdots \\
         * 0 & 0 & \cdots & J_{N-1,N-1} & h_{N-1}\\
         * 0 & 0 & \cdots & 0 & 1 \\
         * \end{pmatrix}
         * \f]
         */
        Matrix _quadmat;

        /**
         * @brief vector for converting index to label
         * the list is asssumed to be sorted
         */
        std::vector<IndexType> _idx_to_label;

        /**
         * @brief dict for converting label to index
         */
        std::unordered_map<IndexType, size_t> _label_to_idx;

        /**
         * @brief The energy offset associated with the model.
         *
         */
        FloatType m_offset;

        /**
         * @brief The model's type.
         *
         */
        Vartype m_vartype = Vartype::NONE;

        /**
         * @brief set _label_to_idx from _idx_to_label
         */
        inline void _set_label_to_idx(){
            //reset
            _label_to_idx.clear();
            //initialize
            for(size_t i=0; i<_idx_to_label.size(); i++){
                _label_to_idx[_idx_to_label[i]] = i;
            }
        }

        /**
         * @brief access elements for dense matrix
         *
         * @tparam T
         * @param i
         * @param j
         * @param dispatch_t
         *
         * @return
         */
        template<typename T=DataType>
        inline FloatType& _quadmat_get(size_t i, size_t j, dispatch_t<T, Dense> = nullptr){
            return _quadmat(i, j);
        }

        /**
         * @brief access elements for dense matrix
         *
         * @tparam T
         * @param i
         * @param j
         * @param dispatch_t
         *
         * @return
         */
        template<typename T=DataType>
        inline FloatType _quadmat_get(size_t i, size_t j, dispatch_t<T, Dense> = nullptr) const{
            return _quadmat(i, j);
        }

        /**
         * @brief access elements for sparse matrix
         *
         * @tparam T
         * @param i
         * @param j
         * @param dispatch_t
         *
         * @return
         */
        template<typename T=DataType>
        inline FloatType& _quadmat_get(size_t i, size_t j, dispatch_t<T, Sparse> = nullptr){
            return _quadmat.coeffRef(i, j);
        }

        /**
         * @brief access elements for sparse matrix
         *
         * @tparam T
         * @param i
         * @param j
         * @param dispatch_t
         *
         * @return
         */
        template<typename T=DataType>
        inline FloatType _quadmat_get(size_t i, size_t j, dispatch_t<T, Sparse> = nullptr) const{
            return _quadmat.coeff(i, j);
        }

        /**
         * @brief get reference of _quadmat(i,j)
         *
         * @param label_i
         * @param label_j
         *
         * @return reference of _quadmat(i,j)
         */
        inline FloatType& _mat(IndexType label_i, IndexType label_j){
            size_t i = _label_to_idx.at(label_i);
            size_t j = _label_to_idx.at(label_j);
            if(i != j)
                return _quadmat_get(std::min(i, j), std::max(i, j));
            else
                throw std::runtime_error("No self-loop (mat(i,i)) allowed");
        }

        /**
         * @brief get reference of _quadmat(i,i)
         *
         * @param label_i
         *
         * @return reference of _quadmat(i,i)
         */
        inline FloatType& _mat(IndexType label_i){
            size_t i = _label_to_idx.at(label_i);
            return _quadmat_get(i, _quadmat.rows()-1);
        }

        /**
         * @brief get reference of _quadmat(i,j)
         *
         * @param label_i
         * @param label_j
         *
         * @return reference of _quadmat(i,j)
         */
        inline FloatType _mat(IndexType label_i, IndexType label_j) const{
            size_t i = _label_to_idx.at(label_i);
            size_t j = _label_to_idx.at(label_j);

            if(i != j)
                return _quadmat_get(std::min(i, j), std::max(i, j));
            else
                throw std::runtime_error("No self-loop (mat(i,i)) allowed");
        }

        /**
         * @brief get reference of _quadmat(i,i)
         *
         * @param label_i
         *
         * @return reference of _quadmat(i,i)
         */
        inline FloatType _mat(IndexType label_i) const{
            size_t i = _label_to_idx.at(label_i);
            return _quadmat_get(i, _quadmat.rows()-1);
        }

        /**
         * @brief calculate maximum element in linear term for dense graph
         *
         * @return
         */
        template<typename T=DataType>
        inline FloatType _max_linear(dispatch_t<T, Dense> = nullptr) const{
            size_t N = _quadmat.rows();
            return _quadmat.block(0,N-1,N-1,1).maxCoeff();
        }

        /**
         * @brief calculate maximum element in quadratic term for dense graph
         *
         * @return
         */
        template<typename T=DataType>
        inline FloatType _max_quadratic(dispatch_t<T, Dense> = nullptr) const{
            size_t N = _quadmat.rows();
            return _quadmat.block(0,0,N-1,N-1).maxCoeff();
        }

        /**
         * @brief calculate minimum element in linear term for dense graph
         *
         * @return
         */
        template<typename T=DataType>
        inline FloatType _min_linear(dispatch_t<T, Dense> = nullptr) const{
            size_t N = _quadmat.rows();
            return _quadmat.block(0,N-1,N-1,1).minCoeff();
        }

        /**
         * @brief calculate minimum element in quadratic term for dense graph
         *
         * @return
         */
        template<typename T=DataType>
        inline FloatType _min_quadratic(dispatch_t<T, Dense> = nullptr) const{
            size_t N = _quadmat.rows();
            return _quadmat.block(0,0,N-1,N-1).minCoeff();
        }

        /**
         * @brief calculate maximum element in linear term for dense graph
         *
         * @return
         */
        template<typename T=DataType>
        inline FloatType _max_linear(dispatch_t<T, Sparse> = nullptr) const{
            size_t N = _quadmat.rows();
            SparseMatrix mat(N-1,1);
            mat = _quadmat.block(0,N-1,N-1,1);
            return mat.coeffs().maxCoeff();
        }

        /**
         * @brief calculate maximum element in quadratic term for dense graph
         *
         * @return
         */
        template<typename T=DataType>
        inline FloatType _max_quadratic(dispatch_t<T, Sparse> = nullptr) const{
            size_t N = _quadmat.rows();
            SparseMatrix mat(N-1,N-1);
            mat = _quadmat.block(0,0,N-1,N-1);
            return mat.coeffs().maxCoeff();
        }

        /**
         * @brief calculate minimum element in linear term for dense graph
         *
         * @return
         */
        template<typename T=DataType>
        inline FloatType _min_linear(dispatch_t<T, Sparse> = nullptr) const{
            size_t N = _quadmat.rows();
            SparseMatrix mat(N-1,1);
            mat = _quadmat.block(0,N-1,N-1,1);
            return mat.coeffs().minCoeff();
        }

        /**
         * @brief calculate minimum element in quadratic term for dense graph
         *
         * @return
         */
        template<typename T=DataType>
        inline FloatType _min_quadratic(dispatch_t<T, Sparse> = nullptr) const{
            size_t N = _quadmat.rows();
            SparseMatrix mat(N-1,N-1);
            mat = _quadmat.block(0,0,N-1,N-1);
            return mat.coeffs().minCoeff();
        }

        /**
         * @brief insert row and column that corresponds to added label into _quadmat for dense matrix
         *
         * @param label_i
         */
        template<typename T=DataType>
        inline void _insert_label_into_mat(IndexType label_i, dispatch_t<T, Dense> = nullptr){
            size_t i = _label_to_idx.at(label_i);
            //define temp mat
            size_t N = _quadmat.rows()+1;
            Matrix tempmat = Matrix(N, N);
            tempmat.setZero();
            //copy elements to new matrix
            tempmat.block(0,0,i,i)              = _quadmat.block(0,0,i,i);
            tempmat.block(0,i+1,i,N-i-1)        = _quadmat.block(0,i,i,N-i-1);
            tempmat.block(i+1,i+1,N-i-1,N-i-1)  = _quadmat.block(i,i,N-i-1,N-i-1);

            _quadmat = tempmat;
        }

        /**
         * @brief insert row and column that corresponds to added label into _quadmat for sparse matrix
         *
         * @param label_i
         */
        template<typename T=DataType>
        inline void _insert_label_into_mat(IndexType label_i, dispatch_t<T, Sparse> = nullptr){
            size_t i = _label_to_idx.at(label_i);
            //define temp mat
            size_t N = _quadmat.rows()+1;
            //Matrix tempmat = Matrix(N, N);
            //copy elements to new matrix
            //tempmat.block(0,0,i,i)              = _quadmat.block(0,0,i,i);
            //tempmat.block(0,i+1,i,N-i-1)        = _quadmat.block(0,i,i,N-i-1);
            //tempmat.block(i+1,i+1,N-i-1,N-i-1)  = _quadmat.block(i,i,N-i-1,N-i-1);

            std::vector<Eigen::Triplet<FloatType>> triplets;
            triplets.reserve(_quadmat.nonZeros());

            for(int k=0; k<_quadmat.outerSize(); k++){
                for(SpIter it(_quadmat, k); it; ++it){
                    size_t r        = it.row();
                    size_t c        = it.col();
                    FloatType val   = it.value();

                    if(r >= i && c >= i){
                        triplets.emplace_back(r+1, c+1, val);
                    }
                    else if(r >= i){
                        triplets.emplace_back(r+1, c  , val);
                    }
                    else if(c >= i){
                        triplets.emplace_back(r  , c+1, val);
                    }
                    else{
                        triplets.emplace_back(r  , c  , val);
                    }
                }
            }

            _quadmat.resize(N, N);
            _quadmat.setFromTriplets(triplets.begin(), triplets.end());
        }

        /**
         * @brief delete row and column that corresponds to existing label from _quadmat for dense matrix
         *
         * @param label_i
         */
        template<typename T=DataType>
        inline void _delete_label_from_mat(IndexType label_i, dispatch_t<T, Dense> = nullptr){
            size_t i = _label_to_idx.at(label_i);
            //define temp mat
            size_t N = _quadmat.rows();
            Matrix tempmat = Matrix(N-1, N-1);
            tempmat.setZero();
            //copy elements to new matrix
            tempmat.block(0,0,i,i)              = _quadmat.block(0,0,i,i);
            tempmat.block(0,i,i,N-i-1)          = _quadmat.block(0,i+1,i,N-i-1);
            tempmat.block(i,i,N-i-1,N-i-1)      = _quadmat.block(i+1,i+1,N-i-1,N-i-1);

            _quadmat = tempmat;
        }

        /**
         * @brief delete row and column that corresponds to existing label from _quadmat for sparse matrix
         *
         * @param label_i
         */
        template<typename T=DataType>
        inline void _delete_label_from_mat(IndexType label_i, dispatch_t<T, Sparse> = nullptr){
            size_t i = _label_to_idx.at(label_i);
            //define temp mat
            size_t N = _quadmat.rows();
            //copy elements to new matrix
            //tempmat.block(0,0,i,i)              = _quadmat.block(0,0,i,i);
            //tempmat.block(0,i,i,N-i-1)          = _quadmat.block(0,i+1,i,N-i-1);
            //tempmat.block(i,i,N-i-1,N-i-1)      = _quadmat.block(i+1,i+1,N-i-1,N-i-1);

            std::vector<Eigen::Triplet<FloatType>> triplets;
            triplets.reserve(_quadmat.nonZeros());

            for(int k=0; k<_quadmat.outerSize(); k++){
                for(SpIter it(_quadmat, k); it; ++it){
                    size_t r        = it.row();
                    size_t c        = it.col();
                    FloatType val   = it.value();

                    if(r == i || c == i)
                        continue;

                    if(r > i && c > i){
                        triplets.emplace_back(r-1, c-1, val);
                    }
                    else if(r > i){
                        triplets.emplace_back(r-1, c  , val);
                    }
                    else if(c > i){
                        triplets.emplace_back(r  , c-1, val);
                    }
                    else{
                        triplets.emplace_back(r  , c  , val);
                    }
                }
            }

            _quadmat.resize(N-1, N-1);
            _quadmat.setFromTriplets(triplets.begin(), triplets.end());
        }

        /**
         * @brief add new label
         * if label_i already exists, this process is skipped.
         *
         * @param label_i
         */
        inline void _add_new_label(IndexType label_i){
            if(_label_to_idx.find(label_i) == _label_to_idx.end()){
                //add label_i
                _idx_to_label.push_back(label_i);
                std::sort(_idx_to_label.begin(), _idx_to_label.end());
                _set_label_to_idx();

                _insert_label_into_mat(label_i);
            }
        }

        /**
         * @brief delete label
         * if label_i does not exist, this process is skipped.
         *
         * @param label_i
         * @param force_delete if true, delete label whenever there exists corresponding nonzero elements in the matrix.
         * otherwise, delete label only if there are no corresponding nonzero elements in the matrix.
         */
        inline void _delete_label(IndexType label_i, bool force_delete = true){
            auto position = std::find(_idx_to_label.begin(), _idx_to_label.end(), label_i);
            if(position != _idx_to_label.end()){
                if(force_delete == false){
                    //check if there are corresponding nonzero elements
                    size_t i = std::distance(_idx_to_label.begin(), position);
                    if(_quadmat.col(i).squaredNorm() > std::numeric_limits<FloatType>::epsilon() ||
                       _quadmat.row(i).squaredNorm() > std::numeric_limits<FloatType>::epsilon() ){
                        // exists nonzero elements
                        return;
                    }
                }
                //delete from matrix first
                _delete_label_from_mat(label_i);
                //add label_i
                _idx_to_label.erase(position);
                // already sorted
                //std::sort(_idx_to_label.begin(), _idx_to_label.end());
                _set_label_to_idx();
            }
        }

        /**
         * @brief initialize matrix with linear and quadratic dicts (for dense matrix)
         *
         * @param linear
         * @param quadratic
         */
        template<typename T=DataType>
        inline void _initialize_quadmat(const Linear<IndexType, FloatType> &linear, const Quadratic<IndexType, FloatType> &quadratic,
                dispatch_t<T, Dense> = nullptr){
            //gather labels
            std::unordered_set<IndexType> labels;

            for(const auto& kv : linear){
                labels.insert(kv.first);
            }

            for(const auto& kv : quadratic){
                labels.insert(kv.first.first);
                labels.insert(kv.first.second);
            }

            // init label <-> index conversion variables
            _idx_to_label = std::vector<IndexType>(labels.begin(), labels.end());
            std::sort(_idx_to_label.begin(), _idx_to_label.end());
            _set_label_to_idx();

            //initialize _quadmat
            size_t mat_size = _idx_to_label.size() + 1;
            _quadmat = Matrix(mat_size, mat_size);
            _quadmat.fill(0);
            _quadmat_get(mat_size-1, mat_size-1) = 1;

            //copy linear and quadratic to _quadmat
            for(const auto& kv : linear){
                IndexType key = kv.first;
                FloatType val = kv.second;
                _mat(key) += val;
            }

            for(const auto& kv : quadratic){
                std::pair<IndexType, IndexType> key = kv.first;
                FloatType val = kv.second;
                _mat(key.first, key.second) += val;
            }
        }

        /**
         * @brief initialize matrix with linear and quadratic dicts (for sparse matrix)
         *
         * @param linear
         * @param quadratic
         */
        template<typename T=DataType>
        inline void _initialize_quadmat(const Linear<IndexType, FloatType> &linear, const Quadratic<IndexType, FloatType> &quadratic,
                dispatch_t<T, Sparse> = nullptr){
            //gather labels
            std::unordered_set<IndexType> labels;

            for(const auto& kv : linear){
                labels.insert(kv.first);
            }

            for(const auto& kv : quadratic){
                labels.insert(kv.first.first);
                labels.insert(kv.first.second);
            }

            // init label <-> index conversion variables
            _idx_to_label = std::vector<IndexType>(labels.begin(), labels.end());
            std::sort(_idx_to_label.begin(), _idx_to_label.end());
            _set_label_to_idx();

            //initialize _quadmat
            size_t mat_size = _idx_to_label.size() + 1;
            _quadmat = Matrix(mat_size, mat_size);

            std::vector<Eigen::Triplet<FloatType>> triplets;
            const size_t buffer = 5;
            triplets.reserve(linear.size() + quadratic.size() + buffer);

            //set triplets to _quadmat
            for(const auto& kv : linear){
                size_t idx1 = _label_to_idx.at(kv.first);
                size_t idx2 = mat_size-1;
                FloatType val = kv.second;

                //NOTE: duplicated elements are summed up.
                triplets.emplace_back(std::min(idx1, idx2), std::max(idx1, idx2), val);
            }

            for(const auto& kv : quadratic){
                size_t idx1 = _label_to_idx.at(kv.first.first);
                size_t idx2 = _label_to_idx.at(kv.first.second);
                FloatType val = kv.second;

                //NOTE: duplicated elements are summed up.
                triplets.emplace_back(std::min(idx1, idx2), std::max(idx1, idx2), val);
            }

            triplets.emplace_back(mat_size-1, mat_size-1, 1);

            _quadmat.setFromTriplets(triplets.begin(), triplets.end());
        }

        /**
         * @brief add non-diagonal elements to upper triangular components for dense matrix
         *
         * @tparam T
         * @param mat
         * @param fix_format
         */
        template<typename T=DataType>
        inline void _add_triangular_elements(const DenseMatrix& mat, bool fix_format, dispatch_t<T, Dense> = nullptr){

            size_t mat_size = _idx_to_label.size() + 1;

            if((size_t)mat.rows() == _idx_to_label.size() + 1){
                if(fix_format == false){
                    _quadmat = mat;
                }
                else{
                    _quadmat += mat.template triangularView<Eigen::StrictlyUpper>();
                    _quadmat += mat.template triangularView<Eigen::StrictlyLower>().transpose();
                }
            }
            else if((size_t)mat.rows() == _idx_to_label.size()){
                _quadmat.block(0,0,mat_size-1,mat_size-1) += mat.template triangularView<Eigen::StrictlyUpper>();
                _quadmat.block(0,0,mat_size-1,mat_size-1) += mat.template triangularView<Eigen::StrictlyLower>().transpose();
                //local fields
                Vector loc = mat.diagonal();
                _quadmat.block(0,mat_size-1,mat_size-1,1) += loc;
            }
            else{
                throw std::runtime_error("the number of variables and dimension do not match.");
            }
        }

        /**
         * @brief add non-diagonal elements to upper triangular components for sparse matrix
         *
         * @tparam T
         * @param mat
         * @param fix_format
         */
        template<typename T=DataType>
        inline void _add_triangular_elements(const DenseMatrix& mat, bool fix_format, dispatch_t<T, Sparse> = nullptr){

            //generate sparse matrix
            SparseMatrix sparse_mat;

            size_t mat_size = _idx_to_label.size() + 1;

            if((size_t)mat.rows() == _idx_to_label.size() + 1){
                if(fix_format == false){
                    _quadmat = mat.sparseView();
                }
                else{
                    sparse_mat = mat.sparseView();
                    _quadmat += sparse_mat.template triangularView<Eigen::StrictlyUpper>();
                    sparse_mat = mat.sparseView().transpose();
                    _quadmat += sparse_mat.template triangularView<Eigen::StrictlyUpper>();
                }
            }
            else if((size_t)mat.rows() == _idx_to_label.size()){
                //generate Dense Matrix
                DenseMatrix temp_mat = DenseMatrix::Zero(mat_size, mat_size);

                temp_mat.block(0,0,mat_size-1,mat_size-1) += mat.template triangularView<Eigen::StrictlyUpper>();
                temp_mat.block(0,0,mat_size-1,mat_size-1) += mat.template triangularView<Eigen::StrictlyLower>().transpose();
                //local fields
                Vector loc = mat.diagonal();
                temp_mat.block(0,mat_size-1,mat_size-1,1) += loc;

                //insert in sparsematrix
                _quadmat += temp_mat.sparseView();
            }
            else{
                throw std::runtime_error("the number of variables and dimension do not match.");
            }
        }

        /**
         * @brief initialize matrix with matrix and labels
         * the form of matrix is assumed to be the following two forms:
         *
         * \f[
         * \begin{pmatrix}
         * J_{0,0} & J_{0,1} & \cdots & J_{0,N-1} & h_{0}\\
         * J_{1,0} & J_{1,1} & \cdots & J_{1,N-1} & h_{1}\\
         * \vdots & \vdots & \vdots & \vdots & \vdots \\
         * J_{N-1,0} & J_{N-1,1} & \cdots & J_{N-1,N-1} & h_{N-1}\\
         * h_{0} & h_{1} & \cdots & h_{N-1} & 1 \\
         * \end{pmatrix}
         * \f]
         *
         * or
         *
         * \f[
         * \begin{pmatrix}
         * h_{0} & J_{0,1} & \cdots & J_{0,N-1}\\
         * J_{1,0} & h_{1} & \cdots & J_{1,N-1}\\
         * \vdots & \vdots & \vdots & \vdots\\
         * J_{N-1,0} & J_{N-1,1} & \cdots & h_{N-1}\\
         * \end{pmatrix}
         * \f]
         *
         * if fix_format is set to false, the following shape is assumed:
         * \f[
         * \begin{pmatrix}
         * J_{0,0} & J_{0,1} & \cdots & J_{0,N-1} & h_{0}\\
         * 0 & J_{1,1} & \cdots & J_{1,N-1} & h_{1}\\
         * \vdots & \vdots & \vdots & \vdots & \vdots \\
         * 0 & 0 & \cdots & J_{N-1,N-1} & h_{N-1}\\
         * 0 & 0 & \cdots & 0 & 1 \\
         * \end{pmatrix}
         * \f]
         *
         * @param mat
         * @param labels
         * @param fix_format
         */
        inline void _initialize_quadmat(const DenseMatrix& mat, const std::vector<IndexType>& labels_vec, bool fix_format){

            //initlaize label <-> index dict
            std::unordered_set<IndexType> labels(labels_vec.begin(), labels_vec.end());
            _idx_to_label = std::vector<IndexType>(labels.begin(), labels.end());
            std::sort(_idx_to_label.begin(), _idx_to_label.end());
            _set_label_to_idx();

            if(mat.rows() != mat.cols()){
                throw std::runtime_error("matrix must be a square matrix");
            }

            size_t mat_size = _idx_to_label.size() + 1;
            _quadmat = Matrix(mat_size, mat_size);
            _quadmat.setZero();
            _add_triangular_elements(mat, fix_format);
            _quadmat_get(mat_size-1, mat_size-1) = 1;
        }

        inline Linear<IndexType, FloatType> _generate_linear() const{
            Linear<IndexType, FloatType> ret_linear;
            for(size_t i=0; i<_idx_to_label.size(); i++){
                FloatType val = _quadmat_get(i, _idx_to_label.size());
                if(val != 0)
                    ret_linear[_idx_to_label[i]] = val;
            }

            return ret_linear;
        }

        inline Quadratic<IndexType, FloatType> _generate_quadratic() const{
            Quadratic<IndexType, FloatType> ret_quadratic;
            for(size_t i=0; i<_idx_to_label.size(); i++){
                for(size_t j=i+1; j<_idx_to_label.size(); j++){
                    FloatType val = _quadmat_get(i, j);
                    if(val != 0)
                        ret_quadratic[std::make_pair(_idx_to_label[i], _idx_to_label[j])] = val;
                }
            }

            return ret_quadratic;
        }

        /**
         * @brief initialize matrix with matrix and labels
         * the form of matrix is assumed to be the following form:
         *
         * \f[
         * \begin{pmatrix}
         * J_{0,0} & J_{0,1} & \cdots & J_{0,N-1} & h_{0}\\
         * 0 & J_{1,1} & \cdots & J_{1,N-1} & h_{1}\\
         * \vdots & \vdots & \vdots & \vdots & \vdots \\
         * 0 & 0 & \cdots & J_{N-1,N-1} & h_{N-1}\\
         * 0 & 0 & \cdots & 0 & 1 \\
         * \end{pmatrix}
         * \f]
         *
         * @param mat
         * @param labels_vec
         */
        inline void _initialize_quadmat(const SparseMatrix& mat, const std::vector<IndexType>& labels_vec){
            this->_quadmat = mat;
            _idx_to_label = std::vector<IndexType>(labels_vec.begin(), labels_vec.end());
            std::sort(_idx_to_label.begin(), _idx_to_label.end());
            _set_label_to_idx();
        }

        /**
         * @brief change internal variable from Ising to QUBO ones for dense matrix
         * The following conversion is applied:
         *
         * \f[
         * \mathrm{offset} += \sum_{i<j} J_{ij} - \sum_{i}h_{i}
         * \f]
         * \f[
         * Q_ii += -2\left(\sum_{j}J_{ji}+\sum_{j}J_{ij}\right) + 2h_{i}
         * \f]
         * \f[
         * Q_{ij} = 4J_{ij}
         * \f]
         */
        template<typename T=DataType>
        inline void _spin_to_binary(dispatch_t<T, Dense> = nullptr){
            size_t num_variables = _idx_to_label.size();
            m_vartype = Vartype::BINARY;
            //calc col(row)wise-sum ((num_variables, 1))
            //Vector colwise_sum = _quadmat.block(0,0,num_variables,num_variables).colwise().sum();
            Vector colwise_sum(num_variables);
            for(size_t i=0; i<num_variables; i++){
                colwise_sum(i) = _quadmat.block(0,0,i,num_variables).col(i).sum();
            }

            Vector rowwise_sum = _quadmat.block(0,0,num_variables,num_variables).rowwise().sum();

            Vector local_field = _quadmat.block(0,num_variables,num_variables,1);

            //offset
            m_offset += colwise_sum.sum() - local_field.sum();

            //local field
            _quadmat.block(0,num_variables,num_variables,1)
                = 2 * local_field - 2 * (colwise_sum + rowwise_sum);

            //quadratic
            _quadmat.block(0,0,num_variables,num_variables) *= 4;
        }

        /**
         * @brief change internal variable from Ising to QUBO ones for sparse matrix
         * The following conversion is applied:
         *
         * \f[
         * \mathrm{offset} += \sum_{i<j} J_{ij} - \sum_{i}h_{i}
         * \f]
         * \f[
         * Q_ii += -2\left(\sum_{j}J_{ji}+\sum_{j}J_{ij}\right) + 2h_{i}
         * \f]
         * \f[
         * Q_{ij} = 4J_{ij}
         * \f]
         */
        template<typename T=DataType>
        inline void _spin_to_binary(dispatch_t<T, Sparse> = nullptr){
            size_t num_variables = _idx_to_label.size();
            m_vartype = Vartype::BINARY;
            //calc col(row)wise-sum ((num_variables, 1))
            //Vector colwise_sum = _quadmat.block(0,0,num_variables,num_variables).colwise().sum();
            //Vector rowwise_sum = _quadmat.block(0,0,num_variables,num_variables).rowwise().sum();

            Vector colwise_sum(num_variables);
            Vector rowwise_sum(num_variables);
            colwise_sum.setZero();
            rowwise_sum.setZero();

            for(int k=0; k<_quadmat.outerSize(); k++){
                //k -> row index
                for(SpIter it(_quadmat, k); it; ++it){
                    size_t r        = it.row();
                    size_t c        = it.col();
                    FloatType val   = it.value();

                    if((r < num_variables) && (c < num_variables)){
                        colwise_sum(c) += val;
                        rowwise_sum(r) += val;
                    }
                }
            }

            Vector local_field = _quadmat.block(0,num_variables,num_variables,1);

            //offset
            m_offset += colwise_sum.sum() - local_field.sum();

            //local field
            //_quadmat.block(0,num_variables,num_variables,1)
            //    = 2 * local_field - 2 * (colwise_sum + rowwise_sum);

            //quadratic
            //_quadmat.block(0,0,num_variables,num_variables) *= 4;

            Vector new_local_field = 2 * local_field - 2 * (colwise_sum + rowwise_sum);

            std::vector<Eigen::Triplet<FloatType>> triplets;
            triplets.reserve(_quadmat.nonZeros());
            for(size_t r=0; r<num_variables; r++){
                triplets.emplace_back(r, num_variables, new_local_field(r));
            }

            for(int k=0; k<_quadmat.outerSize(); k++){
                //k -> row index
                for(SpIter it(_quadmat, k); it; ++it){
                    size_t r        = it.row();
                    size_t c        = it.col();
                    FloatType val   = it.value();

                    if((r < num_variables) && (c < num_variables)){
                        triplets.emplace_back(r, c, 4*val);
                    }
                }
            }

            triplets.emplace_back(num_variables, num_variables, 1);

            _quadmat = SparseMatrix(num_variables+1, num_variables+1);
            _quadmat.setFromTriplets(triplets.begin(), triplets.end());
        }

        /**
         * @brief change internal variable from QUBO to Ising ones for dense matrix
         * The following conversion is applied:
         *
         * \f[
         * \mathrm{offset} += \frac{1}{4}\sum_{i<j} Q_{ij} + \frac{1}{2}\sum_{i}Q_{ii}
         * \f]
         * \f[
         * h_i += \frac{1}{4}\left(\sum_{j}Q_{ji}+\sum_{j}Q_{ij}\right) + \frac{1}{2}Q_{ii}
         * \f]
         * \f[
         * J_{ij} = \frac{1}{4}Q_{ij}
         * \f]
         */
        template<typename T=DataType>
        inline void _binary_to_spin(dispatch_t<T, Dense> = nullptr){
            size_t num_variables = _idx_to_label.size();
            m_vartype = Vartype::SPIN;
            //calc col(row)wise-sum ((num_variables, 1))
            //Vector colwise_sum = _quadmat.block(0,0,num_variables,num_variables).colwise().sum();
            Vector colwise_sum(num_variables);
            for(size_t i=0; i<num_variables; i++){
                colwise_sum(i) = _quadmat.block(0,0,i,num_variables).col(i).sum();
            }
            Vector rowwise_sum = _quadmat.block(0,0,num_variables,num_variables).rowwise().sum();

            Vector local_field = _quadmat.block(0,num_variables,num_variables,1);

            //offset
            m_offset += 0.25 * colwise_sum.sum() + 0.5 * local_field.sum();

            //local field
            _quadmat.block(0,num_variables,num_variables,1)
                = 0.5 * local_field + 0.25 * (colwise_sum + rowwise_sum);

            //quadratic
            _quadmat.block(0,0,num_variables,num_variables) *= 0.25;
        }

        /**
         * @brief change internal variable from QUBO to Ising ones for sparse matrix
         * The following conversion is applied:
         *
         * \f[
         * \mathrm{offset} += \frac{1}{4}\sum_{i<j} Q_{ij} + \frac{1}{2}\sum_{i}Q_{ii}
         * \f]
         * \f[
         * h_i += \frac{1}{4}\left(\sum_{j}Q_{ji}+\sum_{j}Q_{ij}\right) + \frac{1}{2}Q_{ii}
         * \f]
         * \f[
         * J_{ij} = \frac{1}{4}Q_{ij}
         * \f]
         */
        template<typename T=DataType>
        inline void _binary_to_spin(dispatch_t<T, Sparse> = nullptr){
            size_t num_variables = _idx_to_label.size();
            m_vartype = Vartype::SPIN;
            //calc col(row)wise-sum ((num_variables, 1))
            //Vector colwise_sum = _quadmat.block(0,0,num_variables,num_variables).colwise().sum();
            //Vector rowwise_sum = _quadmat.block(0,0,num_variables,num_variables).rowwise().sum();

            Vector colwise_sum(num_variables);
            Vector rowwise_sum(num_variables);
            colwise_sum.setZero();
            rowwise_sum.setZero();

            for(int k=0; k<_quadmat.outerSize(); k++){
                //k -> row index
                for(SpIter it(_quadmat, k); it; ++it){
                    size_t r        = it.row();
                    size_t c        = it.col();
                    FloatType val   = it.value();

                    if((r < num_variables) && (c < num_variables)){
                        colwise_sum(c) += val;
                        rowwise_sum(r) += val;
                    }
                }
            }

            Vector local_field = _quadmat.block(0,num_variables,num_variables,1);

            //offset
            m_offset += 0.25 * colwise_sum.sum() + 0.5 * local_field.sum();

            //local field
            //_quadmat.block(0,num_variables,num_variables,1)
            //    = 0.5 * local_field + 0.25 * (colwise_sum + rowwise_sum);

            //quadratic
            //quadmat.block(0,0,num_variables,num_variables) *= 0.25;

            Vector new_local_field = 0.5 * local_field + 0.25 * (colwise_sum + rowwise_sum);

            std::vector<Eigen::Triplet<FloatType>> triplets;
            triplets.reserve(_quadmat.nonZeros());
            for(size_t r=0; r<num_variables; r++){
                triplets.emplace_back(r, num_variables, new_local_field(r));
            }

            for(int k=0; k<_quadmat.outerSize(); k++){
                //k -> row index
                for(SpIter it(_quadmat, k); it; ++it){
                    size_t r        = it.row();
                    size_t c        = it.col();
                    FloatType val   = it.value();

                    if((r < num_variables) && (c < num_variables)){
                        triplets.emplace_back(r, c, 0.25*val);
                    }
                }
            }

            triplets.emplace_back(num_variables, num_variables, 1);

            _quadmat.setFromTriplets(triplets.begin(), triplets.end());
        }

    public:
        /**
         * @brief BinaryQuadraticModel constructor.
         *
         * @param linear
         * @param quadratic
         * @param offset
         * @param vartype
         */
        BinaryQuadraticModel
        (
            const Linear<IndexType, FloatType> &linear,
            const Quadratic<IndexType, FloatType> &quadratic,
            const FloatType &offset,
            const Vartype vartype
        ):
            m_offset(offset),
            m_vartype(vartype)
        {
            _initialize_quadmat(linear, quadratic);
        }

        /**
         * @brief BinaryQuadraticModel constructor.
         *
         * @param linear
         * @param quadratic
         * @param vartype
         *
         */
        BinaryQuadraticModel
        (
            const Linear<IndexType, FloatType> &linear,
            const Quadratic<IndexType, FloatType> &quadratic,
            const Vartype vartype
        ): BinaryQuadraticModel(linear, quadratic, 0.0, vartype){}

        /**
         * @brief BinaryQuadraticModel constructor (with matrix);
         *
         * @param mat
         * @param labels_vec
         * @param offset
         * @param vartype
         *
         */
        BinaryQuadraticModel
        (
            const Eigen::Ref<const DenseMatrix>& mat,
            const std::vector<IndexType>& labels_vec,
            const FloatType &offset,
            const Vartype vartype,
            bool fix_format = true
        ):
            m_offset(offset),
            m_vartype(vartype)
        {
            _initialize_quadmat(mat, labels_vec, fix_format);
        }

        /**
         * @brief BinaryQuadraticModel constructor (with matrix);
         *
         * @param mat
         * @param labels_vec
         * @param vartype
         *
         */
        BinaryQuadraticModel
        (
            const Eigen::Ref<const DenseMatrix>& mat,
            const std::vector<IndexType>& labels_vec,
            const Vartype vartype,
            bool fix_format = true
        ): BinaryQuadraticModel(mat, labels_vec, 0.0, vartype, fix_format){}

        /**
         * @brief BinaryQuadraticModel constructor (with sparse matrix);
         * this constructor is for developers.
         *
         * @param mat
         * @param labels_vec
         * @param offset
         * @param vartype
         *
         */
        BinaryQuadraticModel
        (
            const SparseMatrix& mat,
            const std::vector<IndexType>& labels_vec,
            const FloatType &offset,
            const Vartype vartype
        ):
            m_offset(offset),
            m_vartype(vartype)
        {
            _initialize_quadmat(mat, labels_vec);
        }

        /**
         * @brief BinaryQuadraticModel constructor (with sparse matrix);
         * this constructor is for developers.
         *
         * @param mat
         * @param labels_vec
         * @param vartype
         *
         */
        BinaryQuadraticModel
        (
            const SparseMatrix& mat,
            const std::vector<IndexType>& labels_vec,
            const Vartype vartype
        ): BinaryQuadraticModel(mat, labels_vec, 0.0, vartype){}

        BinaryQuadraticModel(const BinaryQuadraticModel&) = default;

        /**
         * @brief get the number of variables
         *
         * @return The number of variables.
         */
        size_t get_num_variables() const{
            return _idx_to_label.size();
        }

        /**
         * @brief Return the number of variables.
         * @deprecated use get_num_variables instead.
         *
         * @return The number of variables.
         */
        size_t length() const
        {
            return get_num_variables();
        }

        /**
         * @brief Return true if the variable contains v.
         *
         * @return Return true if the variable contains v.
         * @param v
         */
        bool contains(const IndexType &v) const
        {
            if(_label_to_idx.find(v) != _label_to_idx.end())
            {
                return true;
            }
            else
            {
                return false;
            }

        }

        /**
         * @brief Get the element of linear object
         *
         * @return A linear bias.
         */
        FloatType get_linear(IndexType label_i) const{
            return _mat(label_i);
        }

        /**
         * @brief Get linear object
         *
         * @return A linear object
         */
        Linear<IndexType, FloatType> get_linear() const{
            return _generate_linear();
        }

        /**
         * @brief Get the element of quadratic object
         *
         * @return A quadratic bias.
         */
        FloatType get_quadratic(IndexType label_i, IndexType label_j) const
        {
            return _mat(label_i, label_j);
        }

        /**
         * @brief Get uadratic object
         *
         * @return A quadratic object.
         */
        Quadratic<IndexType, FloatType> get_quadratic() const
        {
            return _generate_quadratic();
        }

        /**
         * @brief Get the offset
         *
         * @return An offset.
         */
        FloatType get_offset() const
        {
            return this->m_offset;
        }

        /**
         * @brief Get the vartype object
         *
         * @return Type of the model.
         */
        Vartype get_vartype() const
        {
            return this->m_vartype;
        }

        /**
         * @brief Get variables
         *
         * @return variables
         */
        const std::vector<IndexType>& get_variables() const{
            return this->_idx_to_label;
        }

        /**
         * @brief Create an empty BinaryQuadraticModel
         *
         * @return empty object
         */
        BinaryQuadraticModel<IndexType, FloatType, DataType> empty(Vartype vartype)
        {
            return BinaryQuadraticModel<IndexType, FloatType, DataType>(
                    Linear<IndexType, FloatType>(),
                    Quadratic<IndexType, FloatType>(),
                    0.0,
                    vartype
                    );
        }

        /* Update methods */

        /**
         * @brief Add variable v and/or its bias to a binary quadratic model.
         *
         * @param v
         * @param bias
         */
        void add_variable
        (
            const IndexType &v,
            const FloatType &bias
        )
        {
            //add new label if not exist
            _add_new_label(v);
            _mat(v) += bias;
        }

        /**
         * @brief Add variables and/or linear biases to a binary quadratic model.
         *
         * @param linear
         */
        void add_variables_from
        (
            const Linear<IndexType, FloatType> &linear
        )
        {
            for(auto &it : linear)
            {
                add_variable(it.first, it.second);
            }
        }

        /**
         * @brief Add an interaction and/or quadratic bias to a binary quadratic model.
         *
         * @param u
         * @param v
         * @param bias
         */
        void add_interaction
        (
            const IndexType &u,
            const IndexType &v,
            const FloatType &bias
        )
        {
            //add labels u and v
            _add_new_label(u);
            _add_new_label(v);
            _mat(u, v) += bias;
        }

        /**
         * @brief Add interactions and/or quadratic biases to a binary quadratic model.
         *
         * @param quadratic
         */
        void add_interactions_from
        (
            const Quadratic<IndexType, FloatType> &quadratic
        )
        {
            for(auto &it : quadratic)
            {
                add_interaction(it.first.first, it.first.second, it.second);
            }
        }

        /**
         * @brief Remove variable v and all its interactions from a binary quadratic model.
         *
         * @param v
         */
        void remove_variable
        (
            const IndexType &v
        )
        {
            _delete_label(v);
        }

        /**
         * @brief Remove specified variables and all of their interactions from a binary quadratic model.
         *
         * @param variables
         */
        void remove_variables_from
        (
            const std::vector<IndexType> &variables
        )
        {
            for(auto &it : variables)
            {
                remove_variable(it);
            }
        }

        /**
         * @brief Remove interaction of variables u, v from a binary quadratic model.
         *
         * @param u
         * @param v
         */
        void remove_interaction
        (
            const IndexType &u,
            const IndexType &v
        )
        {
            _mat(u, v) = 0;
            _delete_label(u, false);
            _delete_label(v, false);
        }

        /**
         * @brief Remove all specified interactions from the binary quadratic model.
         *
         * @param interactions
         */
        void remove_interactions_from
        (
            const std::vector<std::pair<IndexType, IndexType>> &interactions
        )
        {
            for(auto &it : interactions)
            {
                remove_interaction(it.first, it.second);
            }
        }

        /**
         * @brief Add specified value to the offset of a binary quadratic model.
         *
         * @param offset
         */
        void add_offset
        (
            const FloatType &offset
        )
        {
            m_offset += offset;
        }

        /**
         * @brief Set the binary quadratic model's offset to zero.
         */
        void remove_offset()
        {
            add_offset(-m_offset);
        }

        /**
         * @brief Multiply by the specified scalar all the biases and offset of a binary quadratic model.
         *
         * @param scalar
         * @param ignored_variables
         * @param ignored_interactions
         * @param ignored_offset
         */
        void scale
        (
            const FloatType &scalar,
            const std::vector<IndexType> &ignored_variables = {},
            const std::vector<std::pair<IndexType, IndexType>> &ignored_interactions = {},
            const bool ignored_offset = false
        )
        {
            if(scalar == 0.0)
                throw std::runtime_error("scalar must not be zero");

            // scale
            _quadmat *= scalar;

            // revert scale of linear
            for(const auto &it : ignored_variables)
            {
                _mat(it) *= 1.0/scalar;
            }

            // revert scale of quadratic
            for(const auto &it : ignored_interactions)
            {
                _mat(it.first, it.second) *= 1.0/scalar;
            }

            // scaling offset
            if(!ignored_offset)
            {
                m_offset *= scalar;
            }
        }

        /**
         * @brief Normalizes the biases of the binary quadratic model such that they fall in the provided range(s), and adjusts the offset appropriately.
         *
         * @param bias_range
         * @param use_quadratic_range
         * @param quadratic_range
         * @param ignored_variables
         * @param ignored_interactions
         * @param ignored_offset
         *
         */
        void normalize
        (
            const std::pair<FloatType, FloatType> &bias_range = {1.0, 1.0},
            const bool use_quadratic_range = false,
            const std::pair<FloatType, FloatType> &quadratic_range = {1.0, 1.0},
            const std::vector<IndexType> &ignored_variables = {},
            const std::vector<std::pair<IndexType, IndexType>> &ignored_interactions = {},
            const bool ignored_offset = false
        )
        {
            // parse range
            std::pair<FloatType, FloatType> l_range = bias_range;
            std::pair<FloatType, FloatType> q_range;
            if(!use_quadratic_range)
            {
                q_range = bias_range;
            }
            else
            {
                q_range = quadratic_range;
            }

            // calculate scaling value
            FloatType lin_min = _min_linear();
            FloatType lin_max = _max_linear();
            FloatType quad_min = _min_quadratic();
            FloatType quad_max = _max_quadratic();

            std::vector<FloatType> v_scale =
            {
                lin_min / l_range.first,
                lin_max / l_range.second,
                quad_min / q_range.first,
                quad_max / q_range.second
            };

            FloatType inv_scale = *std::max_element(v_scale.begin(), v_scale.end());

            // scaling
            if(inv_scale != 0.0)
            {
                scale(1.0 / inv_scale, ignored_variables, ignored_interactions, ignored_offset);
            }
        }

        /**
         * @brief Fix the value of a variable and remove it from a binary quadratic model.
         *
         * @param v
         * @param value
         */
        void fix_variable
        (
            const IndexType &v,
            const int32_t &value
        )
        {
            std::vector<std::pair<IndexType, IndexType>> interactions;
            const Quadratic<IndexType, FloatType>& quadratic = this->get_quadratic();
            for(const auto &it : quadratic)
            {
                if(it.first.first == v)
                {
                    add_variable(it.first.second, value*it.second);
                    interactions.push_back(it.first);
                }
                else if(it.first.second == v)
                {
                    add_variable(it.first.first, value*it.second);
                    interactions.push_back(it.first);
                }
            }
            remove_interactions_from(interactions);
            add_offset(_mat(v)*value);
            remove_variable(v);
        }

        /**
         * @brief Fix the value of the variables and remove it from a binary quadratic model.
         *
         * @param fixed
         */
        void fix_variables
        (
            const std::vector<std::pair<IndexType, int32_t>> &fixed
        )
        {
            for(auto &it : fixed)
            {
                fix_variable(it.first, it.second);
            }
        }

        /**
         * @brief Flip variable v in a binary quadratic model.
         *
         * @param v
         */
        void flip_variable
        (
            const IndexType &v
        )
        {

            if(m_vartype == Vartype::SPIN)
            {
                size_t i = _label_to_idx.at(v);
                _quadmat.row(i) *= -1;
                _quadmat.col(i) *= -1;
            }
            else if(m_vartype == Vartype::BINARY)
            {
                //change vartype to spin
                this->change_vartype(Vartype::SPIN);

                size_t i = _label_to_idx.at(v);
                _quadmat.row(i) *= -1;
                _quadmat.col(i) *= -1;

                //change vartype to binary
                this->change_vartype(Vartype::BINARY);
            }
        }

        ///**
        // * @brief Enforce u, v being the same variable in a binary quadratic model. (currently disabled)
        // *
        // * @param u
        // * @param v
        // */
        //void contract_variables
        //(
        //    const IndexType &u,
        //    const IndexType &v
        //)
        //{

        //    if(this->m_vartype == Vartype::BINARY){
        //        // the quadratic bias becomes linear
        //        _mat(u) += _mat(v) + _mat(u,v);
        //    }
        //    else if(this->m_vartype == Vartype::SPIN){
        //        _mat(u) += _mat(v);
        //        m_offset += _mat(u,v);
        //    }
        //    this->remove_interaction(u,v);

        //}

        /* Transformations */

        /**
         * @brief Create a binary quadratic model with the specified vartype.
         * This function does not return any object.
         *
         * @param vartype
         */
        void change_vartype
        (
            const Vartype &vartype
        )
        {
            if(m_vartype == Vartype::BINARY && vartype == Vartype::SPIN) // binary -> spin
            {
                _binary_to_spin();
            }
            else if(m_vartype == Vartype::SPIN && vartype == Vartype::BINARY) // spin -> binary
            {
                _spin_to_binary();
            }
        }

        /**
         * @brief Create a binary quadratic model with the specified vartype.
         * This function generates and returns a new object.
         *
         * @param vartype
         * @param inplace if set true, the current object is converted.
         *
         * @return created object
         */
        BinaryQuadraticModel<IndexType, FloatType, DataType> change_vartype
        (
            const Vartype &vartype,
            bool inplace
        )
        {
            BinaryQuadraticModel<IndexType, FloatType, DataType> new_bqm = *this;
            if(inplace == true){
                this->change_vartype(vartype);
            }
            new_bqm.change_vartype(vartype);

            return new_bqm;
        }

        /* Methods */

        /**
         * @brief Determine the energy of the specified sample of a binary quadratic model.
         *
         * @param sample
         * @return An energy with respect to the sample.
         */
        FloatType energy(const Sample<IndexType> &sample) const
        {
            FloatType en = m_offset;
            //initialize vector
            Vector s = Vector::Zero(_quadmat.rows());
            for(const auto& elem : sample){
                s[_label_to_idx.at(elem.first)] = elem.second;
            }
            s[_quadmat.rows()-1] = 1;

            return en + (s.transpose() * _quadmat * s) - 1;
        }

        /**
         * @brief Determine the energies of the given samples.
         *
         * @param samples_like
         * @return A vector including energies with respect to the samples.
         */
        std::vector<FloatType> energies(const std::vector<Sample<IndexType>> &samples_like) const
        {
            std::vector<FloatType> en_vec;
            for(auto &it : samples_like)
            {
                en_vec.push_back(energy(it));
            }
            return en_vec;
        }

        /* Conversions */
        /**
         * @brief Convert a binary quadratic model to QUBO format.
         *
         * @return A tuple including a quadratic bias and an offset.
         */
        std::tuple<Quadratic<IndexType, FloatType>, FloatType> to_qubo()
        {
            // change vartype to binary
            BinaryQuadraticModel<IndexType, FloatType, DataType> bqm = change_vartype(Vartype::BINARY, false);

            const Linear<IndexType, FloatType>& linear = bqm.get_linear();
            Quadratic<IndexType, FloatType> Q = bqm.get_quadratic();
            FloatType offset = bqm.get_offset();
            for(const auto &it : linear)
            {
                Q[std::make_pair(it.first, it.first)] = it.second;
            }
            return std::make_tuple(Q, offset);
        }

        /**
         * @brief Create a binary quadratic model from a QUBO model.
         *
         * @param Q
         * @param offset
         *
         * @return Binary quadratic model with vartype set to `.Vartype.BINARY`.
         */
        static BinaryQuadraticModel<IndexType, FloatType, DataType> from_qubo(const Quadratic<IndexType, FloatType>& Q, FloatType offset=0.0)
        {
            Linear<IndexType, FloatType> linear;
            Quadratic<IndexType, FloatType> quadratic;

            for(auto&& elem : Q){
                const auto& key = elem.first;
                const auto& value = elem.second;
                if(key.first == key.second){
                    linear[key.first] = value;
                }
                else{
                    quadratic[std::make_pair(key.first, key.second)] = value;
                }
            }

            return BinaryQuadraticModel<IndexType, FloatType, DataType>(linear, quadratic, offset, Vartype::BINARY);
        }

        /**
         * @brief Convert a binary quadratic model to Ising format.
         *
         * @return A tuple including a linear bias, a quadratic bias and an offset.
         */
        std::tuple<Linear<IndexType, FloatType>, Quadratic<IndexType, FloatType>, FloatType> to_ising()
        {
            // change vartype to spin
            BinaryQuadraticModel<IndexType, FloatType, DataType> bqm = change_vartype(Vartype::SPIN, false);

            const Linear<IndexType, FloatType>& linear = bqm.get_linear();
            const Quadratic<IndexType, FloatType>& quadratic = bqm.get_quadratic();
            FloatType offset = bqm.get_offset();
            return std::make_tuple(linear, quadratic, offset);
        }

        /**
         * @brief Create a binary quadratic model from an Ising problem.
         *
         * @param linear
         * @param quadratic
         * @param offset
         *
         * @return Binary quadratic model with vartype set to `.Vartype.SPIN`.
         */
        static BinaryQuadraticModel<IndexType, FloatType, DataType> from_ising(const Linear<IndexType, FloatType>& linear, const Quadratic<IndexType, FloatType>& quadratic, FloatType offset=0.0)
        {
            return BinaryQuadraticModel<IndexType, FloatType, DataType>(linear, quadratic, offset, Vartype::SPIN);
        }

        /**
         * @brief generate (Dense or Sparse) interaction matrix with given list of indices
         * The generated matrix will be the following triangular matrix:
         * \f[
         * \begin{pmatrix}
         * J_{0,0} & J_{0,1} & \cdots & J_{0,N-1} & h_{0}\\
         * 0 & J_{1,1} & \cdots & J_{1,N-1} & h_{1}\\
         * \vdots & \vdots & \vdots & \vdots & \vdots \\
         * 0 & 0 & \cdots & J_{N-1,N-1} & h_{N-1}\\
         * 0 & 0 & \cdots & 0 & 1 \\
         * \end{pmatrix}
         * \f]
         *
         * @param indices
         *
         * @return corresponding interaction matrix (Eigen)
         */
        Matrix interaction_matrix() const {
            return this->_quadmat;
        }

        using json = nlohmann::json;

        /**
         * @brief Convert the binary quadratic model to a dense-version serializable object
         *
         * @return An object that can be serialized (nlohmann::json)
         */
        template<typename T=DataType>
        json to_serializable(dispatch_t<T, Dense> = nullptr) const
        {
            std::string schema_version = "3.0.0-dense";
            /*
             * output sample
             * >>> bqm = dimod.BinaryQuadraticModel({'c': -1, 'd': 1}, {('a', 'd'): 2, ('b', 'e'): 5, ('a', 'c'): 3}, 0.0, dimod.BINARY)
             *
             * >>> bqm.to_serializable()
             * {'type': 'BinaryQuadraticModel', 'version': {'bqm_schema': '3.0.0-dense'}, 'use_bytes': False, 'index_type': 'uint16', 'bias_type': 'float32', 'num_variables': 5, 'variable_labels': ['a', 'b', 'c', 'd', 'e'], 'variable_type': 'BINARY', 'offset': 0.0, 'info': {}, 'biases': [0.0, 0.0, -1.0, 1.0, 0.0,...]}
             */

            //set index_dtype
            std::string index_dtype = this->get_num_variables() <= 65536UL ? "uint16" : "uint32";

            //set bias_type
            std::string bias_type;
            if(typeid(m_offset) == typeid(float))
            {
                bias_type = "float32";
            }
            else if(typeid(m_offset) == typeid(double))
            {
                bias_type = "float64";
            }
            else
            {
                throw std::runtime_error("FloatType must be float or double.");
            }

            //set variable type
            std::string variable_type;
            if(m_vartype == Vartype::SPIN)
            {
                variable_type = "SPIN";
            }
            else if(m_vartype == Vartype::BINARY)
            {
                variable_type = "BINARY";
            }
            else
            {
                throw std::runtime_error("Variable type must be SPIN or BINARY.");
            }

            //copy matrix to std::vector
            std::vector<FloatType> biases(_quadmat.data(), _quadmat.data() + _quadmat.size());

            json output;
            output["type"] = "BinaryQuadraticModel";
            output["version"] = {{"bqm_schema", schema_version}};
            output["variable_labels"] = this->get_variables();
            output["use_bytes"] = false;
            output["index_type"] = index_dtype;
            output["bias_type"] = bias_type;
            output["num_variables"] = this->get_num_variables();
            output["variable_type"] = variable_type;
            output["offset"] = m_offset;
            output["info"] = {};
            output["biases"] = biases;

            return output;
        }

        /**
         * @brief Convert the binary quadratic model to a serializable object
         *
         * @return An object that can be serialized (nlohmann::json)
         */
        template<typename T=DataType>
        json to_serializable(dispatch_t<T, Sparse> = nullptr) const
        {
            std::string schema_version = "3.0.0";
            /*
             * output sample
             * >>> bqm = dimod.BinaryQuadraticModel({'c': -1, 'd': 1}, {('a', 'd'): 2, ('b', 'e'): 5, ('a', 'c'): 3}, 0.0, dimod.BINARY)
             *
             * >>> bqm.to_serializable()
             * {'type': 'BinaryQuadraticModel', 'version': {'bqm_schema': '3.0.0'}, 'use_bytes': False, 'index_type': 'uint16', 'bias_type': 'float32', 'num_variables': 5, 'num_interactions': 3, 'variable_labels': ['a', 'b', 'c', 'd', 'e'], 'variable_type': 'BINARY', 'offset': 0.0, 'info': {}, 'linear_biases': [0.0, 0.0, -1.0, 1.0, 0.0], 'quadratic_biases': [3.0, 2.0, 5.0], 'quadratic_head': [0, 0, 1], 'quadratic_tail': [2, 3, 4]}
             */

            //set index_dtype
            std::string index_dtype = this->get_num_variables() <= 65536UL ? "uint16" : "uint32";

            //set bias_type
            std::string bias_type;
            if(typeid(m_offset) == typeid(float))
            {
                bias_type = "float32";
            }
            else if(typeid(m_offset) == typeid(double))
            {
                bias_type = "float64";
            }
            else
            {
                throw std::runtime_error("FloatType must be float or double.");
            }

            //set variable type
            std::string variable_type;
            if(m_vartype == Vartype::SPIN)
            {
                variable_type = "SPIN";
            }
            else if(m_vartype == Vartype::BINARY)
            {
                variable_type = "BINARY";
            }
            else
            {
                throw std::runtime_error("Variable type must be SPIN or BINARY.");
            }

            json output;
            output["type"] = "BinaryQuadraticModel";
            output["version"] = {{"bqm_schema", schema_version}};
            output["variable_labels"] = this->get_variables();
            output["use_bytes"] = false;
            output["index_type"] = index_dtype;
            output["bias_type"] = bias_type;
            output["num_variables"] = this->get_num_variables();
            output["variable_type"] = variable_type;
            output["offset"] = m_offset;
            output["info"] = "";

            //biases
            size_t mat_size = this->get_num_variables() + 1;
            Vector l_bias_vec = _quadmat.block(0, mat_size-1, mat_size-1, 1);
            std::vector<FloatType> l_bias(l_bias_vec.size());
            for(int i=0; i<l_bias_vec.size(); i++){
                l_bias[i] = l_bias_vec(i);
            }

            std::vector<FloatType> q_bias;
            std::vector<size_t> q_head;
            std::vector<size_t> q_tail;

            q_bias.reserve(_quadmat.nonZeros());
            q_head.reserve(_quadmat.nonZeros());
            q_tail.reserve(_quadmat.nonZeros());

            for(int k=0; k<_quadmat.outerSize(); k++){
                for(SpIter it(_quadmat, k); it; ++it){
                    size_t r        = it.row();
                    size_t c        = it.col();
                    FloatType val   = it.value();

                    if((r < mat_size-1) && (c < mat_size-1)){
                        q_bias.push_back(val);
                        q_head.push_back(r);
                        q_tail.push_back(c);
                    }

                }
            }

            output["linear_biases"] = l_bias;
            output["quadratic_biases"] = q_bias;
            output["quadratic_head"] = q_head;
            output["quadratic_tail"] = q_tail;
            output["num_interactions"] = q_bias.size();

            return output;
        }

        /**
         * @brief Create a BinaryQuadraticModel instance from a serializable object.
         *
         * @tparam IndexType_serial
         * @tparam FloatType_serial
         * @param input
         * @return BinaryQuadraticModel<IndexType_serial, FloatType_serial>
         */
        template <typename IndexType_serial = IndexType, typename FloatType_serial = FloatType, typename T=DataType>
        static BinaryQuadraticModel<IndexType_serial, FloatType_serial, DataType> from_serializable(const json &input, dispatch_t<T, Dense> = nullptr)
        {
            //extract type and version
            std::string type = input["type"];
            if(type != "BinaryQuadraticModel")
            {
                throw std::runtime_error("Type must be \"BinaryQuadraticModel\".\n");
            }
            std::string version = input["version"]["bqm_schema"];
            if(version != "3.0.0-dense")
            {
                throw std::runtime_error("bqm_schema must be 3.0.0-dense.\n");
            }

            //extract variable_type
            Vartype vartype;
            std::string variable_type = input["variable_type"];
            if(variable_type == "SPIN")
            {
                vartype = Vartype::SPIN;
            }
            else if(variable_type == "BINARY")
            {
                vartype = Vartype::BINARY;
            }
            else
            {
                throw std::runtime_error("variable_type must be SPIN or BINARY.");
            }

            //extract biases
            std::vector<IndexType_serial> variables = input["variable_labels"];
            std::vector<FloatType_serial> biases = input["biases"];
            FloatType offset = input["offset"];

            size_t mat_size = variables.size() + 1;
            Eigen::Map<Matrix> mat(biases.data(), mat_size, mat_size);

            BinaryQuadraticModel<IndexType_serial, FloatType_serial, DataType> bqm(mat, variables, offset, vartype);
            return bqm;
        }

        /**
         * @brief Create a BinaryQuadraticModel instance from a serializable object.
         *
         * @tparam IndexType_serial
         * @tparam FloatType_serial
         * @param input
         * @return BinaryQuadraticModel<IndexType_serial, FloatType_serial>
         */
        template <typename IndexType_serial = IndexType, typename FloatType_serial = FloatType, typename T=DataType>
        static BinaryQuadraticModel<IndexType_serial, FloatType_serial, DataType> from_serializable(const json &input, dispatch_t<T, Sparse> = nullptr)
        {
            //extract type and version
            std::string type = input["type"];
            if(type != "BinaryQuadraticModel")
            {
                throw std::runtime_error("Type must be \"BinaryQuadraticModel\".\n");
            }
            std::string version = input["version"]["bqm_schema"];
            if(version != "3.0.0")
            {
                throw std::runtime_error("bqm_schema must be 3.0.0.\n");
            }

            //extract variable_type
            Vartype vartype;
            std::string variable_type = input["variable_type"];
            if(variable_type == "SPIN")
            {
                vartype = Vartype::SPIN;
            }
            else if(variable_type == "BINARY")
            {
                vartype = Vartype::BINARY;
            }
            else
            {
                throw std::runtime_error("variable_type must be SPIN or BINARY.");
            }

            //extract biases
            std::vector<IndexType_serial> variables = input["variable_labels"];
            FloatType offset = input["offset"];

            std::vector<FloatType_serial> l_bias = input["linear_biases"];
            std::vector<size_t> q_head = input["quadratic_head"];
            std::vector<size_t> q_tail = input["quadratic_tail"];
            std::vector<FloatType_serial> q_bias = input["quadratic_biases"];

            //make triplets
            std::vector<Eigen::Triplet<FloatType_serial>> triplets;
            triplets.reserve(q_bias.size() + l_bias.size());

            size_t mat_size = variables.size() + 1;

            for(size_t i=0; i<l_bias.size(); i++){
                if(l_bias[i] != 0)
                    triplets.emplace_back(i, mat_size-1, l_bias[i]);
            }

            for(size_t i=0; i<q_bias.size(); i++){
                triplets.emplace_back(q_head[i], q_tail[i], q_bias[i]);
            }

            triplets.emplace_back(mat_size-1, mat_size-1, 1);

            SparseMatrix mat(mat_size, mat_size);

            mat.setFromTriplets(triplets.begin(), triplets.end());

            BinaryQuadraticModel<IndexType_serial, FloatType_serial, DataType> bqm(mat, variables, offset, vartype);
            return bqm;
        }

    };
}

#endif


/*
 * Copyright (c) 2015 Shihao Ji and Hyokun Yun. All Rights Reserved.
 *
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you under the Apache License, Version 2.0 (the
 * "License"); you may not use this file except in compliance
 * with the License.  You may obtain a copy of the License at
 *
 *  http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
 * KIND, either express or implied.  See the License for the
 * specific language governing permissions and limitations
 * under the License.
 *
 * For more information, bug reports, fixes, contact:
 *   Shihao Ji  (shihaoji@yahoo.com)
 *   Hyokun Yun (yungilbert@gmail.com)
 */
#ifndef __WORDRANK_PARAMETER_HPP
#define __WORDRANK_PARAMETER_HPP

#include <string>
#include <vector>
#include <boost/program_options.hpp>
#include "wordrank.hpp"

namespace wordrank {

using std::string;
using std::cout;
using std::cerr;
using std::endl;
using std::vector;

struct Parameter {

    string data_path_;
    double learning_rate_;
    double regularization_;
    int latent_dim_;
    int xmax_;
    scalar epsilon_;
    scalar alpha_;
    scalar beta_;
    scalar tau_;

    int max_iteration_;
    int random_seed_;
    int xi_period_;

    bool cost_eval_;

    TransformType trans_type_;
    scalar t_;
    LossType loss_type_;
    string trans_name_;
    string loss_name_;

    int numthreads_;
    int numtasks_;
    int rank_;

    string dump_prefix_;
    int dump_period_;

    int sgd_num_;

    int read_arguments(int argc, char **argv);

    friend std::ostream& operator<<(std::ostream& stream, const Parameter& param) {
        stream << param.latent_dim_ << ", " << param.regularization_ << ", "
                << param.learning_rate_ << ", " << param.xi_period_;
        return stream;
    }

};
}

int wordrank::Parameter::read_arguments(int argc, char **argv) {

    namespace po = boost::program_options;
    po::options_description desc("wordrank options:");
    desc.add_options()
            ("help", "produce help message")
            ("path", po::value<string>(&data_path_)->default_value("../data/wiki.toy"), "path of data")
            ("nthreads", po::value<int>(&numthreads_)->default_value(4), "number of threads for each MPI process")
            ("lrate", po::value<double>(&learning_rate_)->default_value(0.001), "value of learning rate parameter")
            ("reg", po::value<double>(&regularization_)->default_value(0.005), "value of regularization parameter")
            ("dim", po::value<int>(&latent_dim_)->default_value(100), "dimensionality of latent space")
            ("xmax", po::value<int>(&xmax_)->default_value(100), "cutoff value for weighting function")
            ("epsilon", po::value<scalar>(&epsilon_)->default_value(0.75), "power scaling value for weighting function")
            ("alpha", po::value<scalar>(&alpha_)->default_value(1), "alpha of the gamma distribution")
            ("beta", po::value<scalar>(&beta_)->default_value(0), "beta of the gamma distribution")
            ("tau", po::value<scalar>(&tau_)->default_value(1.0), "delta of logistic or hinge loss")
            ("iter", po::value<int>(&max_iteration_)->default_value(1000), "number of iterations")
            ("seed", po::value<int>(&random_seed_)->default_value(12345), "random number generator seed value")
            ("period", po::value<int>(&xi_period_)->default_value(100), "period of xi variable updates")
            ("cost_eval", po::bool_switch(&cost_eval_)->default_value(false), "evaluate the cost function")
            ("transform", po::value<string>(&trans_name_)->default_value("rho1"), "name of the transform (rho1, rho2, rho3)")
            ("t", po::value<scalar>(&t_)->default_value(1.5), "t value of log_t (rho3) transform function")
            ("loss", po::value<string>(&loss_name_)->default_value("logistic"), "name of the loss (logistic, hinge)")
            ("sgd_num", po::value<int>(&sgd_num_)->default_value(1), "number of SGD taken for each data point")
            ("dump_prefix", po::value<string>(&dump_prefix_)->default_value("model"), "prefix to the parameter dump file")
            ("dump_period", po::value<int>(&dump_period_)->default_value(100), "how often parameters should be dumped");

    bool flagHelp = false;
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        flagHelp = true;
    }

    if (flagHelp == true) {
        cerr << desc << endl;
        return 1;
    }

    if (data_path_.length() <= 0) {
        cerr << "data file path not specified." << endl;
        return 1;
    }

    if (trans_name_ == "rho1") {
        trans_type_ = TransformType::RHO1;
    } else if (trans_name_ == "rho2") {
        trans_type_ = TransformType::RHO2;
    } else if (trans_name_ == "rho3") {
        trans_type_ = TransformType::RHO3;
    } else {
        cerr << "illegal transform name: " << trans_name_ << endl;
        return 1;
    }

    if (loss_name_ == "logistic") {
        loss_type_ = LossType::LOGISTIC;
    } else if (loss_name_ == "hinge") {
        loss_type_ = LossType::HINGE;
    } else {
        cerr << "illegal loss name: " << loss_name_ << endl;
        return 1;
    }

    return 0;

}

#endif


// kate: replace-tabs off; indent-width 4; indent-mode normal
// vim: ts=4:sw=4:noexpandtab
/*

Copyright (c) 2010--2012,
François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
You can contact the authors at <f dot pomerleau at gmail dot com> and
<stephane at magnenat dot net>

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 <organization> 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 ETH-ASL BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#include "pointmatcher/PointMatcher.h"
#include "pointmatcher/IO.h"
#include <cassert>
#include <iostream>
#include <fstream>
#include <boost/format.hpp>
#include <boost/filesystem/path.hpp>
#include <boost/filesystem/operations.hpp>

using namespace std;
using namespace PointMatcherSupport;

void validateArgs(int argc, char *argv[]);

/**
  * Code example for ICP taking a sequence of point clouds relatively close
  * and build a map with them.
  * It assumes that: 3D point clouds are used, they were recorded in sequence
  * and they are express in sensor frame.
  */
int main(int argc, char *argv[])
{
	validateArgs(argc, argv);

	typedef PointMatcher<float> PM;
	typedef PointMatcherIO<float> PMIO;
	typedef PM::TransformationParameters TP;
	typedef PM::DataPoints DP;

	string outputFileName(argv[0]);

	// Rigid transformation
	std::shared_ptr<PM::Transformation> rigidTrans;
	rigidTrans = PM::get().REG(Transformation).create("RigidTransformation");

	// Create filters manually to clean the global map
	std::shared_ptr<PM::DataPointsFilter> densityFilter =
					PM::get().DataPointsFilterRegistrar.create(
						"SurfaceNormalDataPointsFilter",
						{
							{"knn", "10"},
							{"epsilon", "5"},
							{"keepNormals", "0"},
							{"keepDensities", "1"}
						}
                    );

	std::shared_ptr<PM::DataPointsFilter> maxDensitySubsample =
					PM::get().DataPointsFilterRegistrar.create(
						"MaxDensityDataPointsFilter",
						{{"maxDensity", toParam(30)}}
                    );
	// Main algorithm definition
	PM::ICP icp;

	// load YAML config
	ifstream ifs(argv[1]);
	validateFile(argv[1]);
	icp.loadFromYaml(ifs);

	PMIO::FileInfoVector list(argv[2]);

	PM::DataPoints mapPointCloud, newCloud;
	TP T_to_map_from_new = TP::Identity(4,4); // assumes 3D

	for(unsigned i=0; i < list.size(); i++)
	{
		cout << "---------------------\nLoading: " << list[i].readingFileName << endl;

		// It is assume that the point cloud is express in sensor frame
		newCloud = DP::load(list[i].readingFileName);

		if(mapPointCloud.getNbPoints()  == 0)
		{
			mapPointCloud = newCloud;
			continue;
		}

		// call ICP
		try
		{
			// We use the last transformation as a prior
			// this assumes that the point clouds were recorded in
			// sequence.
			const TP prior = T_to_map_from_new;

			T_to_map_from_new = icp(newCloud, mapPointCloud, prior);
		}
		catch (PM::ConvergenceError& error)
		{
			cout << "ERROR PM::ICP failed to converge: " << endl;
			cout << "   " << error.what() << endl;
			continue;
		}

		// This is not necessary in this example, but could be
		// useful if the same matrix is composed in the loop.
		T_to_map_from_new = rigidTrans->correctParameters(T_to_map_from_new);

		// Move the new point cloud in the map reference
		newCloud = rigidTrans->compute(newCloud, T_to_map_from_new);

		// Merge point clouds to map
		mapPointCloud.concatenate(newCloud);

		// Clean the map
		mapPointCloud = densityFilter->filter(mapPointCloud);
		mapPointCloud = maxDensitySubsample->filter(mapPointCloud);

		// Save the map at each iteration
		stringstream outputFileNameIter;
		outputFileNameIter << outputFileName << "_" << i << ".vtk";

		cout << "outputFileName: " << outputFileNameIter.str() << endl;
		mapPointCloud.save(outputFileNameIter.str());
	}

	return 0;
}

void validateArgs(int argc, char *argv[])
{
	if (!(argc == 3))
	{
		cerr << "Error in command line, usage " << argv[0] << " icpConfiguration.yaml listOfFiles.csv" << endl;
		cerr << endl << "Example:" << endl;
		cerr << argv[0] << " ../examples/data/default.yaml ../examples/data/carCloudList.csv" << endl;
		cerr << endl << " - or - " << endl << endl;
		cerr << argv[0] << " ../examples/data/default.yaml ../examples/data/cloudList.csv" << endl << endl;

		abort();
	}
}


#pragma  once

#include <string>
#include <vector>
#include <glm/glm.hpp>
#include <boost/filesystem.hpp>
#include "Engine/Shader.hpp"
#include "Engine/Graphic/Texture.hpp"

class Shader;

struct Vertex {

	Vertex() noexcept :
			position(glm::vec3(0.0f)),
			normal(glm::vec3(0.0f)),
			uv(glm::vec2(0.0f)) {};

	Vertex(glm::vec3 position, glm::vec3 normal, glm::vec2 uv) noexcept :
			position(position),
			normal(normal),
			uv(uv) {};

	glm::vec3		position;
	glm::vec3		normal;
	glm::vec2		uv;
};

class Mesh {
public:

	enum class eFlag { NONE = 0, TEXTURE = (1 << 0) };

	Mesh(std::vector<Vertex> vertice, std::vector<unsigned int> indice, std::vector<Texture> texture);
	Mesh(std::vector<Vertex> vertice, std::vector<unsigned int> indice);

	void				render(Shader &shader, GLenum typeOfDraw = GL_TRIANGLES) const;
	void				render(GLenum typeOfDraw = GL_TRIANGLES) const noexcept;
	unsigned int		getVAO() const;
	void				activeTexture() const;
	std::vector<unsigned int> const	&getIndice() const;
	std::vector<Vertex> const	&getVertice() const;

	~Mesh();

private:

	std::vector<Vertex>			vertice_;
	std::vector<unsigned int>	indice_;
	std::vector<Texture>		texture_;

	unsigned int				VAO;
	unsigned int				VBO;
	unsigned int				EBO;

	void	clean_();
	void 	setupMesh_();

	static bool				debug_;

	Mesh() = delete;
	Mesh(Mesh const &) = delete;
	Mesh &operator=(Mesh const &) = delete;
};


#pragma once

#include <unordered_set>
#include <boost/unordered_map.hpp>
#include <utility>
#include <tuple>
#include "types.hh"
#include <boost/functional/hash.hpp>

/*!
  @brief Generate a region automaton from a timed automaton
  @tparam NVar the number of variable in TA

  TA to RA adds states with BFS. Initial configuration is the initial states of RA. The RA contain only the states reachable from initial states.
  THIS DOES NOT MAKE INITIAL/ACCEPTING STATES!!
 */
template <int NVar>
void ta_to_ra_naive (const TimedAutomaton<NVar> &TA,RegionAutomaton &RA)
{
  using Variables = char;
  State numOfStates;

  // make all integer parts
  std::vector<std::vector<std::pair<int,int> > > integer_parts;
  integer_parts.resize(TA.max_constraints.size());
  for (int i = 0; i < integer_parts.size(); i++) {
    integer_parts[i].reserve((TA.max_constraints[i] + 1) * 2);
    std::pair<int,int> p = {0,0};
    while (p.first <= TA.max_constraints[i]) {
      integer_parts[i].push_back (p);
      if (p.first == p.second) {
        p.second++;
      } else {
        p.first++;
      }
    }
  }

  std::vector<Region> regions;
  std::vector<std::size_t> indices;
  indices.resize(integer_parts.size());
  while (1) {
    Region r;
    r.max_constraints = TA.max_constraints;
    r.frac_order.clear();
    std::vector<int> fracs;
    fracs.reserve (integer_parts.size());

    r.integer_parts.resize(integer_parts.size());
    for (std::size_t i = 0;i < integer_parts.size();i++) {
      r.integer_parts[i] = integer_parts[i][indices[i]];
      if (r.integer_parts[i].first != r.integer_parts[i].second && r.integer_parts[i].second <= r.max_constraints[i]) {
        fracs.push_back (i);
      }
    }

    // calc fracs
    if (fracs.empty()) {
      regions.push_back (r);
    } else {
      std::vector<std::vector<std::vector<int> > > c_orders = {{fracs}};
      std::unordered_set<std::vector<std::vector<int> >, boost::hash<std::vector<std::vector<int> > > > addedOrders;
      addedOrders.insert ({fracs});
      auto tmp_region = r;
      tmp_region.frac_order.push_back({fracs.begin(),fracs.end()});
      regions.push_back (std::move(tmp_region));
      while (!c_orders.empty()) {
        std::vector<std::vector<std::vector<int> > > next_orders;
        for (const auto &order: c_orders) {
          for (auto i = 0; i < order.size();i++) {
            for (auto j = 0; j < order[i].size();j++) {
              auto tmp_region = r;
              auto tmp_order = order;
              auto tmp = tmp_order[i][j];
              tmp_order[i].erase(tmp_order[i].begin()+j);
              if (i + 1 < order.size()) {
                tmp_order[i+1].push_back (tmp);
                sort(tmp_order[i+1].begin(), tmp_order[i+1].end());
              } else {
                tmp_order.push_back({tmp});
              }
              if (tmp_order[i].empty()) {
                tmp_order.erase(tmp_order.begin() + i);
              }
              // if tmp_order is new
              if (addedOrders.find (tmp_order) == addedOrders.end()) {
                addedOrders.insert (tmp_order);
                for (const auto& odr: tmp_order) {
                  tmp_region.frac_order.push_back({odr.begin(),odr.end()});
                }
                next_orders.push_back (std::move(tmp_order));
                // std::list<std::list<int> > l = {tmp_order.begin(),tmp_order.end()};
                // r.frac_order = std::move(l);
                regions.push_back (std::move(tmp_region));
              }
            }
          }
        }
        c_orders = std::move(next_orders);
      }
    }

    // increment indices
    std::size_t i;
    for (i = 0;i < indices.size();i++) {
      if(indices[i]+1 < integer_parts[i].size()) {
        indices[i]++;
        break;
      } else {
        indices[i] = 0;
      }
    }
    if (i == indices.size()) {
      break;
    }
  }

  // std::cout << "# of regions: " << regions.size() << std::endl;
  // added regions
  RA.abstractedStates.reserve (TA.edges.size() * regions.size());

  boost::unordered_map<std::tuple<TAState,std::vector< std::pair<int,int> >, std::list<std::list<int> > >, RAState> revRegionStates;
  for(TAState s = 0; s < TA.edges.size();s++) {
    for (const auto &region: regions) {
      revRegionStates[std::make_tuple(s,region.integer_parts,region.frac_order)] = RA.abstractedStates.size();
      RA.abstractedStates.push_back ({s,region});
    }
  }

  // make edges
  RA.edges.resize (TA.edges.size() * regions.size());

  for (RAState rs = 0; rs < RA.abstractedStates.size();rs++) {
    const auto &region = RA.abstractedStates[rs];
    Region now,next;
    bool ss;
    now = region.second;
    bool f = now.nextRegion (next,ss);

    next = ss ? now : next;
    if (!f && !ss)
      continue;
    do {
      now = std::move(next);
      bool too_large = true;
      const Region::Interpretation &val = now;
      for (const auto &e : TA.edges.at(region.first)) {
#ifdef DEBUG
        cout << (int)get<1>(s) << ":"
             << (int)e.source << "->"
             << (int)e.target << " "
             << (e.guard(val) ? "true" : "false")
             << endl;
#endif
        if (std::all_of(e.guard.begin(),e.guard.end(),
                        [val] (const Constraint &delta){return delta(val) == Order::EQ;})) {
          too_large = false;
          Region nregion = now;
          for (auto i : e.resetVars) {
            nregion.integer_parts[i] = {0,0};
            for (auto it0 = nregion.frac_order.begin();
                 it0 != nregion.frac_order.end(); it0++) {
              auto it = it0->begin();
              // This seems unsorted but I don't know why.
              if ((it = find (it0->begin(), it0->end(),i)) != it0->end()) {
                //                if (!(binary_search (it0->begin(), it0->end(),i))) {
                it0->erase(it);
                if (it0->empty() ) {
                  nregion.frac_order.erase(it0);
                }
                break;
              }
            }
          }

          RAState raTarget = revRegionStates[std::make_tuple(e.target, nregion.integer_parts,nregion.frac_order)];
          //          RAState raTarget = find (RA.abstractedStates.begin(),RA.abstractedStates.end(),std::make_pair(e.target,nregion)) - RA.abstractedStates.begin();
          // TODO: edges are duplicated.
          RA.edges[rs].push_back ({rs,raTarget,e.c});
        } else if (std::none_of(e.guard.begin(), e.guard.end(),
                                [val](const Constraint &delta){return delta (val) == Order::GT;})) {
          too_large = false;
        }
      }
      if (too_large) break;
    } while(now.nextRegion (next,ss));
  }
}


/*
 * GraphBLAS Template Library (GBTL), Version 3.0
 *
 * Copyright 2020 Carnegie Mellon University, Battelle Memorial Institute, and
 * Authors.
 *
 * THIS MATERIAL WAS PREPARED AS AN ACCOUNT OF WORK SPONSORED BY AN AGENCY OF
 * THE UNITED STATES GOVERNMENT.  NEITHER THE UNITED STATES GOVERNMENT NOR THE
 * UNITED STATES DEPARTMENT OF ENERGY, NOR THE UNITED STATES DEPARTMENT OF
 * DEFENSE, NOR CARNEGIE MELLON UNIVERSITY, NOR BATTELLE, NOR ANY OF THEIR
 * EMPLOYEES, NOR ANY JURISDICTION OR ORGANIZATION THAT HAS COOPERATED IN THE
 * DEVELOPMENT OF THESE MATERIALS, MAKES ANY WARRANTY, EXPRESS OR IMPLIED, OR
 * ASSUMES ANY LEGAL LIABILITY OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS,
 * OR USEFULNESS OR ANY INFORMATION, APPARATUS, PRODUCT, SOFTWARE, OR PROCESS
 * DISCLOSED, OR REPRESENTS THAT ITS USE WOULD NOT INFRINGE PRIVATELY OWNED
 * RIGHTS.
 *
 * Released under a BSD-style license, please see LICENSE file or contact
 * permission@sei.cmu.edu for full terms.
 *
 * [DISTRIBUTION STATEMENT A] This material has been approved for public release
 * and unlimited distribution.  Please see Copyright notice for non-US
 * Government use and distribution.
 *
 * This Software includes and/or makes use of the following Third-Party Software
 * subject to its own license:
 *
 * 1. Boost Unit Test Framework
 * (https://www.boost.org/doc/libs/1_45_0/libs/test/doc/html/utf.html)
 * Copyright 2001 Boost software license, Gennadiy Rozental.
 *
 * DM20-0442
 */

#include <iostream>

#include <graphblas/graphblas.hpp>

using namespace grb;

#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE matrix_build_test_suite

#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)

//****************************************************************************
BOOST_AUTO_TEST_CASE(matrix_build_test)
{
    IndexArrayType i = {0, 0, 0, 1, 1, 1, 2, 2};
    IndexArrayType j = {1, 2, 3, 0, 2, 3, 0, 1};
    std::vector<double>       v = {1, 2, 3, 4, 6, 7, 8, 9};

    std::vector<std::vector<double> > mat = {{0, 1, 2, 3},
                                             {4, 0, 6, 7},
                                             {8, 9, 0, 0}};

    Matrix<double, DirectedMatrixTag> m1(3, 4);
    m1.build(i, j, v);

    //std::cerr << m1 << std::endl;
    IndexType nnz = m1.nvals();
    BOOST_CHECK_EQUAL(nnz, i.size());

    IndexArrayType ii(nnz), jj(nnz);
    std::vector<double> val(nnz);
    m1.extractTuples(ii, jj, val);
    BOOST_CHECK_EQUAL(nnz, ii.size());
    BOOST_CHECK_EQUAL(nnz, jj.size());
    BOOST_CHECK_EQUAL(nnz, val.size());

    for (IndexType idx = 0; idx < val.size(); ++idx)
    {
        BOOST_CHECK_EQUAL(val[idx], mat[ii[idx]][jj[idx]]);
    }
}

//****************************************************************************
BOOST_AUTO_TEST_CASE(matrix_build_test_iterators)
{
    IndexArrayType i = {0, 0, 0, 1, 1, 1, 2, 2};
    IndexArrayType j = {1, 2, 3, 0, 2, 3, 0, 1};
    std::vector<double>       v = {1, 2, 3, 4, 6, 7, 8, 9};

    std::vector<std::vector<double> > mat = {{0, 1, 2, 3},
                                             {4, 0, 6, 7},
                                             {8, 9, 0, 0}};

    Matrix<double, DirectedMatrixTag> m1(3, 4);
    m1.build(i.begin(), j.begin(), v.begin(), i.size());

    //std::cerr << m1 << std::endl;
    IndexType nnz = m1.nvals();
    BOOST_CHECK_EQUAL(nnz, i.size());

    IndexArrayType ii(nnz), jj(nnz);
    std::vector<double> val(nnz);
    m1.extractTuples(ii, jj, val);
    BOOST_CHECK_EQUAL(nnz, ii.size());
    BOOST_CHECK_EQUAL(nnz, jj.size());
    BOOST_CHECK_EQUAL(nnz, val.size());

    for (IndexType idx = 0; idx < val.size(); ++idx)
    {
        BOOST_CHECK_EQUAL(val[idx], mat[ii[idx]][jj[idx]]);
    }
}

BOOST_AUTO_TEST_SUITE_END()