/**
 * MIT License
 *
 * Copyright (c) 2018 Parsiad Azimzadeh
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#ifndef MFHOWARDS_BELLMAN_EQ_FROM_LAMBDAS_HPP
#define MFHOWARDS_BELLMAN_EQ_FROM_LAMBDAS_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/Sparse>

#include <functional>       // std::function
#include <initializer_list> // std::initializer_list
#include <limits>           // std::numeric_limits
#include <utility>          // std::forward

namespace mfhowards {
namespace {
template <typename CtrlT> class bellman_eq_from_lambdas;
}
} // namespace mfhowards

namespace Eigen {
namespace internal {
template <typename CtrlT>
struct traits<mfhowards::bellman_eq_from_lambdas<CtrlT>>
    : public Eigen::internal::traits<Eigen::SparseMatrix<double>> {};
} // namespace internal
} // namespace Eigen

namespace mfhowards {
namespace {
template <typename CtrlT>
class bellman_eq_from_lambdas
    : public Eigen::EigenBase<bellman_eq_from_lambdas<CtrlT>> {

public:
  typedef double Scalar;
  typedef double RealScalar;
  typedef int StorageIndex;

  enum {
    ColsAtCompileTime = Eigen::Dynamic,
    MaxColsAtCompileTime = Eigen::Dynamic,
    IsRowMajor = false
  };

private:
  typedef std::function<Scalar(int, int, CtrlT)> FA;
  typedef std::function<Scalar(int, CtrlT)> Fb;

  int n;
  std::vector<CtrlT> ctrl_set;
  FA A;
  Fb b;
  CtrlT *curr_ctrls;

  template <typename Rhs>
  using Helper = Eigen::Product<bellman_eq_from_lambdas<CtrlT>, Rhs,
                                Eigen::AliasFreeProduct>;

public:
  int rows() const { return n; }
  int cols() const { return n; }

  template <typename Rhs>
  Helper<Rhs> operator*(const Eigen::MatrixBase<Rhs> &x) const {
    return Helper<Rhs>(*this, x.derived());
  }

  template <typename T>
  bellman_eq_from_lambdas(const int n, T &&ctrl_set, FA A, Fb b)
      : ctrl_set(std::forward<T>(ctrl_set)) {
    this->n = n;
    this->A = A;
    this->b = b;
    curr_ctrls = new CtrlT[n];
  }

  bellman_eq_from_lambdas(const int n, std::initializer_list<CtrlT> list, FA A,
                          Fb b)
      : bellman_eq_from_lambdas(n, std::vector<CtrlT>(list), A, b) {}

  ~bellman_eq_from_lambdas() { delete[] curr_ctrls; }

  bellman_eq_from_lambdas(const bellman_eq_from_lambdas &) = delete;
  bellman_eq_from_lambdas &operator=(const bellman_eq_from_lambdas &) = delete;

  void improve(const Eigen::VectorXd &x) {
    Eigen::VectorXd best =
        Eigen::VectorXd::Ones(rows()) * std::numeric_limits<double>::infinity();
    for (auto c : ctrl_set) {
      for (int i = 0; i < rows(); ++i) {
        double tmp = 0;
        for (int j = 0; j < cols(); ++j) {
          tmp += A(i, j, c) * x(j) - b(i, c);
        }
        if (tmp < best(i)) {
          best(i) = tmp;
          curr_ctrls[i] = c;
        }
      }
    }
  }

  Eigen::VectorXd rhs() const {
    Eigen::VectorXd rhs(rows());
    for (int i = 0; i < rows(); ++i) {
      rhs(i) = b(i, curr_ctrls[i]);
    }
    return rhs;
  }

  template <typename, typename, typename, typename, int>
  friend class Eigen::internal::generic_product_impl;
};
} // namespace
} // namespace mfhowards

namespace Eigen {
namespace internal {
template <typename CtrlT, typename Rhs>
struct generic_product_impl<mfhowards::bellman_eq_from_lambdas<CtrlT>, Rhs,
                            SparseShape, DenseShape, GemvProduct>
    : generic_product_impl_base<
          mfhowards::bellman_eq_from_lambdas<CtrlT>, Rhs,
          generic_product_impl<mfhowards::bellman_eq_from_lambdas<CtrlT>,
                               Rhs>> {

  typedef
      typename Product<mfhowards::bellman_eq_from_lambdas<CtrlT>, Rhs>::Scalar
          Scalar;

  template <typename Dest>
  static void
  scaleAndAddTo(Dest &dst, const mfhowards::bellman_eq_from_lambdas<CtrlT> &lhs,
                const Rhs &rhs, const Scalar &) {
    for (int i = 0; i < lhs.rows(); ++i) {
      for (int j = 0; j < lhs.cols(); ++j) {
        dst(i) += lhs.A(i, j, lhs.curr_ctrls[i]) * rhs(j);
      }
    }
  }
};
} // namespace internal
} // namespace Eigen

#endif


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file bounded_degree_mst.hpp
 * @brief
 * @author Piotr Godlewski
 * @version 1.0
 * @date 2013-06-03
 */
#ifndef PAAL_BOUNDED_DEGREE_MST_HPP
#define PAAL_BOUNDED_DEGREE_MST_HPP

#include "paal/iterative_rounding/bounded_degree_min_spanning_tree/bounded_degree_mst_oracle.hpp"
#include "paal/iterative_rounding/ir_components.hpp"
#include "paal/iterative_rounding/iterative_rounding.hpp"
#include "paal/lp/lp_row_generation.hpp"

#include <boost/bimap.hpp>
#include <boost/range/as_array.hpp>
#include <boost/graph/connected_components.hpp>

namespace paal {
namespace ir {

namespace {
struct bounded_degree_mst_compare_traits {
    static const double EPSILON;
};

const double bounded_degree_mst_compare_traits::EPSILON = 1e-10;
}

/**
 * @class bounded_degree_mst
 * @brief The class for solving the Bounded Degree MST problem using Iterative
* Rounding.
 *
 * @tparam Graph input graph
 * @tparam DegreeBounds map from Graph vertices to degree bounds
 * @tparam CostMap map from Graph edges to costs
 * @tparam VertexIndex map from Graph vertices to indices
 * @tparam SpanningTreeOutputIterator
 * @tparam Oracle separation oracle
 */
template <typename Graph, typename DegreeBounds, typename CostMap,
          typename VertexIndex, typename SpanningTreeOutputIterator,
          typename Oracle = paal::lp::random_violated_separation_oracle>
class bounded_degree_mst {
  public:
    /**
     * Constructor.
     */
    bounded_degree_mst(const Graph & g, const DegreeBounds & deg_bounds,
                    CostMap cost_map, VertexIndex index,
                    SpanningTreeOutputIterator result_spanning_tree, Oracle oracle = Oracle{}) :
              m_g(g), m_cost_map(cost_map), m_index(index), m_deg_bounds(deg_bounds),
              m_result_spanning_tree(result_spanning_tree),
              m_compare(bounded_degree_mst_compare_traits::EPSILON),
              m_oracle(oracle)
    {}

    using Edge = typename boost::graph_traits<Graph>::edge_descriptor;
    using Vertex = typename boost::graph_traits<Graph>::vertex_descriptor;

    using EdgeMap = boost::bimap<Edge, lp::col_id>;
    using VertexMap = std::unordered_map<lp::row_id, Vertex>;

    using EdgeMapOriginal = std::vector<std::pair<lp::col_id, Edge>>;

    using ErrorMessage = boost::optional<std::string>;

    /**
     * Checks if the input graph is connected.
     */
    ErrorMessage check_input_validity() {
        // Is g connected?
        std::vector<int> components(num_vertices(m_g));
        int num = boost::connected_components(m_g, &components[0]);

        if (num > 1) {
            return ErrorMessage{ "The graph is not connected." };
        }

        return ErrorMessage{};
    }

    /**
     * @brief
     *
     * @tparam LP
     * @param lp
     *
     * @return
     */
    template <typename LP>
    auto get_find_violation(LP & lp) {
        using candidate = bdmst_violation_checker::Candidate;
        return m_oracle([&](){return m_violation_checker.get_violation_candidates(*this, lp);},
                        [&](candidate c){return m_violation_checker.check_violation(c, *this);},
                        [&](candidate c){return m_violation_checker.add_violated_constraint(c, *this, lp);});
    }

    /**
     * Returns the input graph.
     */
    const Graph &get_graph() const { return m_g; }

    /**
     * Returns the vertex index.
     */
    const VertexIndex &get_index() const { return m_index; }

    /**
     * Removes an LP column and the graph edge corresponding to it.
     */
    void remove_column(lp::col_id col_id) {
        auto ret = m_edge_map.right.erase(col_id);
        assert(ret == 1);
    }

    /**
     * Binds a graph edge to a LP column.
     */
    void bind_edge_to_col(Edge e, lp::col_id col) {
        m_edge_map_original.push_back(
            typename EdgeMapOriginal::value_type(col, e));
        m_edge_map.insert(typename EdgeMap::value_type(e, col));
    }

    /**
     * Returns the cost of a given edge.
     */
    decltype(get(std::declval<CostMap>(), std::declval<Edge>()))
        get_cost(Edge e) {
        return get(m_cost_map, e);
    }

    /**
     * Returns the degree bound of a vertex.
     */
    decltype(std::declval<DegreeBounds>()(get(std::declval<VertexIndex>(),
                                              std::declval<Vertex>())))
        get_degree_bound(Vertex v) {
        return m_deg_bounds(get(m_index, v));
    }

    /**
     * Returns the LP column corresponding to an edge, if it wasn't deleted from
     * the LP.
     */
    boost::optional<lp::col_id> edge_to_col(Edge e) const {
        auto i = m_edge_map.left.find(e);
        if (i != m_edge_map.left.end()) {
            return i->second;
        } else {
            return boost::none;
        }
    }

    /**
     * Returns a bimap between edges and LP column IDs.
     */
    const EdgeMap &get_edge_map() const { return m_edge_map; }

    /**
     * Returns a mapping between LP column IDs and edges in the original graph.
     */
    const EdgeMapOriginal &get_original_edges_map() const {
        return m_edge_map_original;
    }

    /**
     * Adds an edge to the result spanning tree.
     */
    void add_to_result_spanning_tree(Edge e) {
        *m_result_spanning_tree = e;
        ++m_result_spanning_tree;
    }

    /**
     * Returns the double comparison object.
     */
    utils::compare<double> get_compare() const {
        return m_compare;
    }

    /**
     * Binds a graph vertex to an LP row.
     */
    void bind_vertex_to_row(Vertex v, lp::row_id row) {
        m_vertex_map.insert(typename VertexMap::value_type(row, v));
    }

    /**
     * Unbinds the graph vertex from its corresponding (deleted) LP row.
     */
    void remove_row(lp::row_id row_id) {
        auto ret = m_vertex_map.erase(row_id);
        assert(ret == 1);
    }

    /**
     * Returns the graph vertex corresponding to a given LP row,
     *        unless the row doen't correspond to any vertex.
     */
    boost::optional<Vertex> row_to_vertex(lp::row_id row) {
        auto i = m_vertex_map.find(row);
        if (i != m_vertex_map.end()) {
            return i->second;
        } else {
            return boost::none;
        }
    }

  private:
    Edge col_to_edge(lp::col_id col) {
        auto i = m_edge_map.right.find(col);
        assert(i != m_edge_map.right.end());
        return i->second;
    }

    const Graph &m_g;
    CostMap m_cost_map;
    VertexIndex m_index;
    const DegreeBounds &m_deg_bounds;
    SpanningTreeOutputIterator m_result_spanning_tree;
    bdmst_violation_checker m_violation_checker;

    EdgeMapOriginal m_edge_map_original;
    EdgeMap m_edge_map;
    VertexMap m_vertex_map;

    const utils::compare<double>   m_compare;

    Oracle m_oracle;
};

namespace detail {
/**
 * @brief Creates a bounded_degree_mst object. Non-named version.
 *
 * @tparam Oracle
 * @tparam Graph
 * @tparam DegreeBounds
 * @tparam CostMap
 * @tparam VertexIndex
 * @tparam SpanningTreeOutputIterator
 * @param g
 * @param degBoundMap
 * @param cost_map
 * @param vertex_index
 * @param result_spanning_tree
 * @param oracle
 *
 * @return bounded_degree_mst object
 */
template <typename Oracle = lp::random_violated_separation_oracle,
          typename Graph,
          typename DegreeBounds, typename CostMap, typename VertexIndex,
          typename SpanningTreeOutputIterator>
bounded_degree_mst<Graph, DegreeBounds, CostMap, VertexIndex, SpanningTreeOutputIterator, Oracle>
make_bounded_degree_mst(const Graph & g, const DegreeBounds & deg_bounds,
                      CostMap cost_map, VertexIndex vertex_index,
                      SpanningTreeOutputIterator result_spanning_tree,
                      Oracle oracle = Oracle()) {
    return bounded_degree_mst<Graph, DegreeBounds, CostMap, VertexIndex,
                SpanningTreeOutputIterator, Oracle>(g, deg_bounds, cost_map, vertex_index,
                    result_spanning_tree, oracle);
}
} // detail

/**
 * Creates a bounded_degree_mst object. Named version.
 * The returned object can be used to check input validity or to get a lower
* bound on the
 * optimal solution cost.
 *
 * @tparam Oracle
 * @tparam Graph
 * @tparam DegreeBounds
 * @tparam SpanningTreeOutputIterator
 * @tparam P
 * @tparam T
 * @tparam R
 * @param g
 * @param deg_bounds
 * @param params
 * @param result_spanning_tree
 * @param oracle
 *
 * @return bounded_degree_mst object
 */
template <typename Oracle = lp::random_violated_separation_oracle,
          typename Graph,
          typename DegreeBounds, typename SpanningTreeOutputIterator,
          typename P, typename T, typename R>
auto
make_bounded_degree_mst(const Graph & g,
                      const DegreeBounds & deg_bounds,
                      const boost::bgl_named_params<P, T, R> & params,
                      SpanningTreeOutputIterator result_spanning_tree,
                      Oracle oracle = Oracle())
        -> bounded_degree_mst<Graph, DegreeBounds,
                decltype(choose_const_pmap(get_param(params, boost::edge_weight), g, boost::edge_weight)),
                decltype(choose_const_pmap(get_param(params, boost::vertex_index), g, boost::vertex_index)),
                SpanningTreeOutputIterator, Oracle> {

    return detail::make_bounded_degree_mst(g, deg_bounds,
                choose_const_pmap(get_param(params, boost::edge_weight), g, boost::edge_weight),
                choose_const_pmap(get_param(params, boost::vertex_index), g, boost::vertex_index),
                result_spanning_tree, oracle);
}

/**
 * Creates a bounded_degree_mst object. All default parameters.
 * The returned object can be used to check input validity or to get a lower
* bound on the
 * optimal solution cost.
 *
 * @tparam Oracle
 * @tparam Graph
 * @tparam DegreeBounds
 * @tparam SpanningTreeOutputIterator
 * @param g
 * @param deg_bounds
 * @param result_spanning_tree
 * @param oracle
 *
 * @return bounded_degree_mst object
 */
template <typename Oracle = lp::random_violated_separation_oracle,
          typename Graph, typename DegreeBounds, typename SpanningTreeOutputIterator>
auto
make_bounded_degree_mst(const Graph & g, const DegreeBounds & deg_bounds,
                      SpanningTreeOutputIterator result_spanning_tree,
                      Oracle oracle = Oracle()) ->
        decltype(make_bounded_degree_mst(g, deg_bounds, boost::no_named_parameters(), result_spanning_tree, oracle)) {
    return make_bounded_degree_mst(g, deg_bounds, boost::no_named_parameters(), result_spanning_tree, oracle);
}

/**
 * Round Condition of the IR Bounded Degree MST algorithm.
 */
struct bdmst_round_condition {
    /**
     * Constructor. Takes epsilon used in double comparison.
     */
    bdmst_round_condition(double epsilon =
                              bounded_degree_mst_compare_traits::EPSILON)
        : m_round_zero(epsilon) {}

    /**
     * Checks if a given column of the LP can be rounded to 0.
     * If the column is rounded, the corresponding edge is removed from the
     * graph.
     */
    template <typename Problem, typename LP>
    boost::optional<double> operator()(Problem &problem, const LP &lp,
                                       lp::col_id col) {
        auto ret = m_round_zero(problem, lp, col);
        if (ret) {
            problem.remove_column(col);
        }
        return ret;
    }

  private:
    round_condition_equals<0> m_round_zero;
};

/**
 * Relax Condition of the IR Bounded Degree MST algorithm.
 */
struct bdmst_relax_condition {
    /**
     * Checks if a given row of the LP corresponds to a degree bound and can be
     * relaxed.
     * If the row degree is not greater than the corresponding degree bound + 1,
     * it is relaxed
     * and the degree bound is deleted from the problem.
     */
    template <typename Problem, typename LP>
    bool operator()(Problem &problem, const LP &lp, lp::row_id row) {
        auto vertex = problem.row_to_vertex(row);
        if (vertex) {
            auto ret = (lp.get_row_degree(row) <=
                        problem.get_degree_bound(*vertex) + 1);
            if (ret) {
                problem.remove_row(row);
            }
            return ret;
        } else
            return false;
    }
};

/**
 * Initialization of the IR Bounded Degree MST algorithm.
 */
struct bdmst_init {
    /**
     * Initializes the LP: variables for edges, degree bound constraints
     * and constraint for all edges.
     */
    template <typename Problem, typename LP>
    void operator()(Problem &problem, LP &lp) {
        lp.set_lp_name("bounded degree minimum spanning tree");
        lp.set_optimization_type(lp::MINIMIZE);

        add_variables(problem, lp);
        add_degree_bound_constraints(problem, lp);
        add_all_set_equality(problem, lp);
    }

  private:
    /**
     * Adds a variable to the LP for each edge in the input graph.
     * Binds the LP columns to edges.
     */
    template <typename Problem, typename LP>
    void add_variables(Problem & problem, LP & lp) {
        for (auto e : boost::as_array(edges(problem.get_graph()))) {
            auto col = lp.add_column(problem.get_cost(e), 0, 1);
            problem.bind_edge_to_col(e, col);
        }
    }

    /**
     * Adds a degree bound constraint to the LP for each vertex in the input
     * graph
     * and binds vertices to rows.
     */
    template <typename Problem, typename LP>
    void add_degree_bound_constraints(Problem &problem, LP &lp) {
        auto const &g = problem.get_graph();

        for (auto v : boost::as_array(vertices(g))) {
            lp::linear_expression expr;

            for (auto e : boost::as_array(out_edges(v, g))) {
                expr += *(problem.edge_to_col(e));
            }

            auto row =
                lp.add_row(std::move(expr) <= problem.get_degree_bound(v));
            problem.bind_vertex_to_row(v, row);
        }
    }

    /**
     * Adds an equality constraint to the LP for the set of all edges in the
     * input graph.
     */
    template <typename Problem, typename LP>
    void add_all_set_equality(Problem &problem, LP &lp) {
        lp::linear_expression expr;
        for (auto col : lp.get_columns()) {
            expr += col;
        }
        lp.add_row(std::move(expr) == num_vertices(problem.get_graph()) - 1);
    }
};

/**
 * Set Solution component of the IR Bounded Degree MST algorithm.
 */
struct bdmst_set_solution {
    /**
     * Constructor. Takes epsilon used in double comparison.
     */
    bdmst_set_solution(double epsilon =
                           bounded_degree_mst_compare_traits::EPSILON)
        : m_compare(epsilon) {}

    /**
     * Creates the result spanning tree form the LP (all edges corresponding to
     * columns with value 1).
     */
    template <typename Problem, typename GetSolution>
    void operator()(Problem & problem, const GetSolution & solution) {
        for (auto col_and_edge : problem.get_original_edges_map()) {
            if (m_compare.e(solution(col_and_edge.first), 1)) {
                problem.add_to_result_spanning_tree(col_and_edge.second);
            }
        }
    }

private:
    const utils::compare<double>   m_compare;
};

template <typename Init = bdmst_init,
          typename RoundCondition = bdmst_round_condition,
          typename RelaxContition = bdmst_relax_condition,
          typename SetSolution = bdmst_set_solution,
          typename SolveLPToExtremePoint = ir::row_generation_solve_lp<>,
          typename ResolveLpToExtremePoint = ir::row_generation_solve_lp<>>
using bdmst_ir_components =
    IRcomponents<Init, RoundCondition, RelaxContition, SetSolution,
                 SolveLPToExtremePoint, ResolveLpToExtremePoint>;

namespace detail {
/**
 * @brief Solves the Bounded Degree MST problem using Iterative Rounding.
* Non-named version.
 *
 * @tparam Oracle
 * @tparam Graph
 * @tparam DegreeBounds
 * @tparam CostMap
 * @tparam VertexIndex
 * @tparam SpanningTreeOutputIterator
 * @tparam IRcomponents
 * @tparam Visitor
 * @param g
 * @param degBoundMap
 * @param cost_map
 * @param vertex_index
 * @param result_spanning_tree
 * @param components
 * @param oracle
 * @param visitor
 *
 * @return solution status
 */
template <typename Oracle = lp::random_violated_separation_oracle,
          typename Graph,
          typename DegreeBounds, typename CostMap, typename VertexIndex,
          typename SpanningTreeOutputIterator,
          typename IRcomponents = bdmst_ir_components<>,
          typename Visitor = trivial_visitor>
IRResult bounded_degree_mst_iterative_rounding(
        const Graph & g,
        const DegreeBounds & deg_bounds,
        CostMap cost_map,
        VertexIndex vertex_index,
        SpanningTreeOutputIterator result_spanning_tree,
        IRcomponents components = IRcomponents(),
        Oracle oracle = Oracle(),
        Visitor visitor = Visitor()) {

    auto bdmst = make_bounded_degree_mst(g, deg_bounds, cost_map, vertex_index, result_spanning_tree, oracle);
    return solve_iterative_rounding(bdmst, std::move(components), std::move(visitor));
}
} // detail

/**
 * @brief Solves the Bounded Degree MST problem using Iterative Rounding. Named
* version.
 *
 * @tparam Oracle
 * @tparam Graph
 * @tparam DegreeBounds
 * @tparam SpanningTreeOutputIterator
 * @tparam IRcomponents
 * @tparam Visitor
 * @tparam P
 * @tparam T
 * @tparam R
 * @param g
 * @param deg_bounds
 * @param result_spanning_tree
 * @param params
 * @param components
 * @param oracle
 * @param visitor
 *
 * @return solution status
 */
template <typename Oracle = lp::random_violated_separation_oracle,
          typename Graph,
          typename DegreeBounds, typename SpanningTreeOutputIterator,
          typename IRcomponents = bdmst_ir_components<>,
          typename Visitor = trivial_visitor, typename P, typename T,
          typename R>
IRResult bounded_degree_mst_iterative_rounding(
            const Graph & g,
            const DegreeBounds & deg_bounds,
            const boost::bgl_named_params<P, T, R> & params,
            SpanningTreeOutputIterator result_spanning_tree,
            IRcomponents components = IRcomponents(),
            Oracle oracle = Oracle(),
            Visitor visitor = Visitor()) {

        return detail::bounded_degree_mst_iterative_rounding(g, deg_bounds,
                    choose_const_pmap(get_param(params, boost::edge_weight), g, boost::edge_weight),
                    choose_const_pmap(get_param(params, boost::vertex_index), g, boost::vertex_index),
                    std::move(result_spanning_tree), std::move(components),
                    std::move(oracle), std::move(visitor));
}

/**
 * @brief Solves the Bounded Degree MST problem using Iterative Rounding. All
* default parameters.
 *
 * @tparam Oracle
 * @tparam Graph
 * @tparam DegreeBounds
 * @tparam SpanningTreeOutputIterator
 * @tparam IRcomponents
 * @tparam Visitor
 * @param g
 * @param deg_bounds
 * @param result_spanning_tree
 * @param components
 * @param oracle
 * @param visitor
 *
 * @return solution status
 */
template <typename Oracle = lp::random_violated_separation_oracle, typename Graph,
          typename DegreeBounds, typename SpanningTreeOutputIterator,
          typename IRcomponents = bdmst_ir_components<>,
          typename Visitor = trivial_visitor>
IRResult bounded_degree_mst_iterative_rounding(
            const Graph & g,
            const DegreeBounds & deg_bounds,
            SpanningTreeOutputIterator result_spanning_tree,
            IRcomponents components = IRcomponents(),
            Oracle oracle = Oracle(),
            Visitor visitor = Visitor()) {

        return bounded_degree_mst_iterative_rounding(g, deg_bounds,
                    boost::no_named_parameters(), std::move(result_spanning_tree),
                    std::move(components), std::move(oracle), std::move(visitor));
}

} //! ir
} //! paal
#endif // PAAL_BOUNDED_DEGREE_MST_HPP


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG, www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also tools/license/license.mtl.txt in the distribution.
//
// Author Marc Hartung

#ifndef MTL_MATRIX_EIGENVALUE_INCLUDE
#define MTL_MATRIX_EIGENVALUE_INCLUDE

#include <cmath>
#include <complex>
#include <utility>

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/operation/qr_givens.hpp>
#include <boost/numeric/mtl/operation/misc.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>

namespace mtl { namespace mat {

/// Solver class for general eigenvalues
/** Not yet tested for complex matrices. **/
template <typename Matrix>
class eigenvalue_solver {

    typedef typename Collection<Matrix>::value_type   value_type;
    typedef typename Collection<Matrix>::size_type    size_type;

public:

    /** \brief Constructor needs a sqare-matrix as input
     * \param MTL-Matrix type
     *
     */
    eigenvalue_solver(const Matrix& IN) : ncols(num_cols(IN)), nrows(num_rows(IN)) {
	zero= math::zero(IN[0][0]);
	one= math::one(IN[0][0]);
	R = hessenberg(IN);
	I = Matrix(ncols,nrows);
	I = one;

	//Standartwert Initialisierung:

	maxIteration = 20*ncols*ncols;
	eps = 1.0e-8;
    }

    /** \brief Changes the zero-tolerance
     *  \param value_type of allowed distance to zero
     */
    void setTolerance(value_type in) {
	eps = in;
    }

    /** \brief Changes the number of allowed iterations
     * \param size_type variable of maximum allowed iterations
     */
    void setMaxIteration(size_type in) {
	maxIteration = in;
    }

    /** \brief Starts the calculation of eigenvalues.
     */
    void calc() {
	value_type singleCont;
	std::complex<value_type> compCont;
	size_type size, allIt = 0;
	for(size_type i=ncols-1;i>0 && i<ncols && allIt<=maxIteration;i--) {
	    size = i+1; // verkleinert die Matrix-Dimensionen
	    irange r(0,size);
	    singleCont = std::abs(R[i][i-1])+1;
	    compCont = getComplex2x2EW(i) + 1;
	    while( allIt < maxIteration  && std::abs(R[i][i-1])>eps) {
		if(isRealEW(i)) {
		    if(std::abs(R[i][i-1])<singleCont) { //SingleShift
			singleCont = std::abs(R[i][i-1]);
			singleShift(r);
		    }
		    else { //DoubleShift
			singleCont = std::abs(R[i][i-1]);
			doubleShift(r);
		    }
		    allIt++;
		}
		else {
		    if(std::abs(compCont-getComplex2x2EW(i))>eps) {
			compCont = getComplex2x2EW(i);
			doubleShift(r);
			allIt++;
		    }
		    else {
			i--;
			break;
		    }
		}
 	    }
	}
    }

    /** \brief Returner for the calculated eigenvalues
     *
     * Before using get_eigenvalues() you have to use calc()!
     * \param dense_vector<complex<value_type>> of eigenvalues
     */
    dense_vector<std::complex<value_type> > get_eigenvalues()
    {
	using mtl::conj;
	dense_vector<std::complex<double> > res(ncols, 0.0);
	size_type i;
	for(i=ncols-1;i>0 && i<ncols;i--) {
	    if(std::abs(R[i][i-1])<eps || isRealEW(i)) { // wenn ein reeller EW
		res[i] = R[i][i];
	    }
	    else { // wenn zwei konjungiert komplexe EW

		std::complex<value_type> ews = getComplex2x2EW(i);
		res[i-1] = ews;
		res[i] = conj(ews);
		i--;
	    }
	}
	if(i==0) { // zur Vermeidung von Zugriffsfehler
	    res[0] = R[0][0];
	}
	return res;
    }

private:
    Matrix R, I;
    size_type ncols, nrows, maxIteration;
    value_type zero, one, eps;

    bool isRealEW(size_type k) {
	if(std::abs(sqrt(square(R[k-1][k-1]+R[k][k])/4.0+R[k-1][k]*R[k][k-1]-R[k-1][k-1]*R[k][k])) >= 0.0) {
	    return true;
	}
	return false;
    }

    /** \brief Calculates real eigenvalues of a 2x2-matrix defined by col/row k arround the diagonal of the input-matrix
     *
     * First entry of the pair is the eigenvalue closer to the (kxk)-entry (Wilkinson-shift for single shifting)
     */
    std::pair<value_type, value_type> get2x2EW(const size_type k) {
	std::pair<value_type, value_type> res;
	value_type front, back, comparator;

	front = (R[k-1][k-1]+R[k][k])/2.0;
	back = sqrt(square(R[k-1][k-1]+R[k][k])/4.0+R[k-1][k]*R[k][k-1]-R[k-1][k-1]*R[k][k]);
	comparator = R[k][k]-front;

	if( std::abs(comparator-back) < std::abs(comparator+back) ) {
	    res.first = front+back;
	    res.second = front-back;
	}
	else {
	    res.first = front-back;
	    res.second = front+back;
	}
	return res;
    }

    /** \brief Calculates a complex eigenvalue of a 2x2-matrix defined by col/row k arround the diagonal of the input-matrix
     *
     */
    std::complex<value_type> getComplex2x2EW(const size_type k) {
	std::complex<value_type> res;
	res = square(R[k-1][k-1]+R[k][k])/4.0+R[k-1][k]*R[k][k-1]-R[k-1][k-1]*R[k][k];
	res = sqrt(res);
	res += (R[k-1][k-1]+R[k][k])/2.0;
	return res;
    }

    /** \brief Performes a double shift for submatrix defined by range r
     */

    void doubleShift(irange r)
    {
	using mtl::conj;
	value_type s,t;

	if(isRealEW(r.finish()-1)) {
	    std::pair<value_type, value_type> ews = get2x2EW(r.finish()-1);
	    s = ews.first+ews.second;
	    t = ews.first*ews.second;
	}
	else {
	    std::complex<value_type> ew = getComplex2x2EW(r.finish()-1);
	    s = 2.0*ew.real();
	    t = std::abs(ew*conj(ew));
	}

	Matrix RIter(R[r][r]*R[r][r] - s*R[r][r] + t*I[r][r]);

	qr_givens_solver<Matrix> QR(RIter);
	QR.setTolerance(eps);
	QR.calc();

	RIter = R[r][r];
	R[r][r] = (QR.getQ()) * RIter * trans(QR.getQ());

    }

     /** \brief Performes a single shift for submatrix defined by range r
     */
    void singleShift(irange r) {
	value_type sh = get2x2EW(r.finish()-1).first;
	if(!(std::abs(sh) >= 0.0)) {
	    sh = R[r.finish()-1][r.finish()-1];
	}
	Matrix RIter(R[r][r] - sh*I[r][r]);		//Shift wird abgezogen

	qr_givens_solver<Matrix> QR(RIter);		//QR-Zerlegung
	QR.setTolerance(eps);
	QR.calc();

	R[r][r] = ((QR.getR())*trans(QR.getQ())) + sh*I[r][r];	//QR-Iteration mit dem Rückshift

    }

};

/// Calculation of eigenvalues of general matrix A
/** Not yet tested for complex matrices. **/
template <typename Matrix>
dense_vector<std::complex<typename Collection<Matrix>::value_type> >
inline eigenvalues(const Matrix& A)
{
    eigenvalue_solver<Matrix> s(A);
    s.calc();
    return s.get_eigenvalues();
}

}} // namespace mtl::matrix

#endif // MTL_MATRIX_EIGENVALUE_INCLUDE


/* Copyright (c) 2017, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
// g++ -Wall -std=c++1z -I /usr/include/eigen3/ main.cpp -o test
#include <random>
#include <cmath>
#include <iostream>
#include <Eigen/Dense>
#include "vmf.hpp"
#include "vmfPrior.hpp"
#include "sample.hpp"

int main() {

  std::mt19937 rnd(1);

  vMF<float,3> vmfA(Eigen::Vector3f(0,0,1), 100);
  vMF<float,3> vmfB(Eigen::Vector3f(0,1,0), 100);
  vMF<float,3> vmfC(Eigen::Vector3f(1,0,0), 100);
  vMF<float,3> vmfD(Eigen::Vector3f(-1,0,0),100);
  vMF<float,3> vmfO(Eigen::Vector3f(1,0,0),10);

  size_t N=30;
  std::vector<std::vector<Eigen::Vector3f>> x;
  for (size_t i=0; i<N; ++i) {
    x.push_back(std::vector<Eigen::Vector3f>());
    for (size_t j=0; j<N; ++j) {
      if (i<N/2 && j<N/2)
        x[i].push_back(vmfA.sample(rnd));
      if (i>=N/2 && j<N/2)
        x[i].push_back(vmfB.sample(rnd));
      if (i>=N/2 && j>=N/2)
        x[i].push_back(vmfC.sample(rnd));
      if (i<N/2 && j>=N/2)
        x[i].push_back(vmfD.sample(rnd));
      if (N/3 < i && i< 2*N/3 && N/3 < j && j < 2*N/3)
        x[i].back() = vmfO.sample(rnd);
    }
  }
  std::cout << "have " << x.size() << " input data" << std::endl;

  std::vector<Eigen::Vector3f> xSum(1, Eigen::Vector3f::Zero());
  std::vector<std::vector<uint32_t>> z(x.size());
  for (size_t i=0; i<x.size(); ++i)
    for (size_t j=0; j<x[i].size(); ++j)  {
      xSum[0] += x[i][j];
      z[i].push_back(0);
    }
  std::vector<float> counts(1, x.size()*x.size());
  std::vector<vMF<float,3>> vmfs;
  vMFprior<float> base(Eigen::Vector3f(0,0,1), .1, 0.0);
  float logAlpha = log(10.);
  float lambda = .1;

  vmfs.push_back(base.sample(rnd));
  for (size_t it=0; it<10000; ++it) {
    // sample labels | parameters
    size_t K = vmfs.size();
    for (size_t i=0; i<x.size(); ++i) {
      for (size_t j=0; j<x[i].size(); ++j) {
        Eigen::VectorXf logPdfs(K+1);
        Eigen::VectorXf pdfs(K+1);

        Eigen::VectorXf neighNs = Eigen::VectorXf::Zero(K);
        if (i+1<N) neighNs[z[i+1][j]] ++;
        if (i>=1)  neighNs[z[i-1][j]] ++;
        if (j+1<N) neighNs[z[i][j+1]] ++;
        if (j>=1)  neighNs[z[i][j-1]] ++;

//        if (i+1<N) neighNs[z[i+1][j]] += x[i+1][j].dot(x[i][j]);
//        if (i>=1)  neighNs[z[i-1][j]] += x[i-1][j].dot(x[i][j]);
//        if (j+1<N) neighNs[z[i][j+1]] += x[i][j+1].dot(x[i][j]);
//        if (j>=1)  neighNs[z[i][j-1]] += x[i][j-1].dot(x[i][j]);

//        if (i+1<N) neighNs[z[i+1][j]] += vmfs[z[i+1][j]].mu_.dot(x[i][j]);
//        if (i>=1)  neighNs[z[i-1][j]] += vmfs[z[i-1][j]].mu_.dot(x[i][j]);
//        if (j+1<N) neighNs[z[i][j+1]] += vmfs[z[i][j+1]].mu_.dot(x[i][j]);
//        if (j>=1)  neighNs[z[i][j-1]] += vmfs[z[i][j-1]].mu_.dot(x[i][j]);

//        if (i+1<N) neighNs[z[i+1][j]] += vmfs[z[i+1][j]].mu_.dot(vmfs[z[i][j]].mu_);
//        if (i>=1)  neighNs[z[i-1][j]] += vmfs[z[i-1][j]].mu_.dot(vmfs[z[i][j]].mu_);
//        if (j+1<N) neighNs[z[i][j+1]] += vmfs[z[i][j+1]].mu_.dot(vmfs[z[i][j]].mu_);
//        if (j>=1)  neighNs[z[i][j-1]] += vmfs[z[i][j-1]].mu_.dot(vmfs[z[i][j]].mu_);

        for (size_t k=0; k<K; ++k) {
          logPdfs[k] = lambda*(neighNs[k]-4);
          if (z[i][j] == k) {
            // TODO what if last in cluster
            logPdfs[k] += log(counts[k]-1)+vmfs[k].logPdf(x[i][j]);
          } else {
            logPdfs[k] += log(counts[k])+vmfs[k].logPdf(x[i][j]);
          }
        }
        logPdfs[K] = logAlpha + base.logMarginal(x[i][j]);
        logPdfs = logPdfs.array() - logSumExp<float>(logPdfs);
        pdfs = logPdfs.array().exp();
        size_t zPrev = z[i][j];
        z[i][j] = sampleDisc(pdfs, rnd);
        //      std::cout << z[i] << " " << K << ": " << pdfs.transpose() << std::endl;
        if (z[i][j] == K) {
          vmfs.push_back(base.posterior(x[i][j],1).sample(rnd));
          counts.push_back(0);
          xSum.push_back(Eigen::Vector3f::Zero());
          K++;
        }
        if (zPrev != z[i][j]) {
          counts[zPrev] --;
          counts[z[i][j]] ++;
          xSum[zPrev] -= x[i][j];
          xSum[z[i][j]] += x[i][j];
        }
      }
    }
//    std::cout << "sample parameters" << std::endl;
    // sample parameters | labels
//    for (size_t i=0; i<x.size(); ++i) {
//      xSum[z[i]] += x[i]; // TODO: can fold in above as well
//    }
    for (size_t k=0; k<K; ++k) {
      if (counts[k] > 0) {
        vmfs[k] = base.posterior(xSum[k],counts[k]).sample(rnd);
      }
    }
    std::cout << "counts " << K << ": ";
    for (size_t k=0; k<K; ++k) if (counts[k] > 0) std::cout << counts[k] << " ";
    std::cout << "\ttaus: " ;
    for (size_t k=0; k<K; ++k) if (counts[k] > 0) std::cout << vmfs[k].tau_ << " ";
    std::cout << std::endl;
  }
  return 0;
}


#include "NoiseModel2.h"
#include <iostream>
#include <DNest4/code/Distributions/Cauchy.h>
#include <DNest4/code/Utils.h>
#include <Eigen/Sparse>

namespace CorrelatedNoise
{

NoiseModel2::NoiseModel2(int _ny, int _nx)
:ny(_ny)
,nx(_nx)
,n(ny*nx)
{

}

void NoiseModel2::from_prior(DNest4::RNG& rng)
{
    DNest4::Cauchy cauchy(0.0, 5.0);

    // Fairly generic priors
    do
    {
        coeff0 = cauchy.generate(rng);
    }while(std::abs(coeff0) >= 100.0);
    coeff0 = exp(coeff0);

    do
    {
        coeff1 = cauchy.generate(rng);
    }while(std::abs(coeff1) >= 100.0);
    coeff1 = exp(coeff1);

    correlation_logit = 5.0*rng.randn();
}

double NoiseModel2::perturb(DNest4::RNG& rng)
{
    double logH = 0.0;

    DNest4::Cauchy cauchy(0.0, 5.0);

    int which = rng.rand_int(3);

    if(which == 0)
    {
        coeff0 = log(coeff0);
        logH += cauchy.perturb(coeff0, rng);
        if(std::abs(coeff0) >= 100.0)
        {
            coeff0 = 1.0;
            return -1E300;
        }
        coeff0 = exp(coeff0);
    }
    else if(which == 1)
    {
        coeff1 = log(coeff1);
        logH += cauchy.perturb(coeff1, rng);
        if(std::abs(coeff1) >= 100.0)
        {
            coeff1 = 1.0;
            return -1E300;
        }
        coeff1 = exp(coeff1);
    }
    else
    {
        logH -= -0.5*pow(correlation_logit/5.0, 2);
        correlation_logit += 5.0*rng.randh();
        logH += -0.5*pow(correlation_logit/5.0, 2);
    }

    return logH;
}

// More complete log likelihood
double NoiseModel2::log_likelihood(const Eigen::MatrixXd& data,
                                   const Eigen::MatrixXd& model,
                                   const Eigen::MatrixXd& sigma_map) const
{
    // Convert from logit
    double alpha = 0.25*exp(correlation_logit)/(1.0 + exp(correlation_logit));

    // Find min of model
    double min = 1E300;
    for(int i=0; i<ny; ++i)
        for(int j=0; j<nx; ++j)
            if(model(i, j) < min)
                min = model(i, j);

    // Flatten data and turn it into standardised residuals
    Eigen::VectorXd ys(n);
    int k = 0;
    double extra_log_determinant = 0.0;
    double sd;
    int num_non_masked = 0;
    for(int i=0; i<ny; ++i)
    {
        for(int j=0; j<nx; ++j)
        {
            sd = sqrt(coeff0*coeff0 + coeff1*(model(i, j) - min)
                                    + pow(sigma_map(i, j), 2));
            if(sigma_map(i, j) < 1E100)
            {
                ys(k++) = (data(i, j) - model(i, j))/sd;
                extra_log_determinant += 2*log(sd);
                ++num_non_masked;
            }
            else
            {
                // Masked pixels
                ys(k++) = 0.0;
            }
        }
    }

    double logL = -0.5*num_non_masked*log(2.0*M_PI) - 0.5*extra_log_determinant;

    std::vector<Eigen::Triplet<double>> triplets;
    int k1, k2;
    for(int i=0; i<ny; ++i)
    {
        for(int j=0; j<nx; ++j)
        {
            // Here
            k1 = j + i*nx;
            triplets.emplace_back(k1, k1, 1.0);

            // Pixel up
            if(i > 0)
            {
                k2 = j + (i-1)*nx;
//                std::cout << k1 << ' ' << k2 << std::endl;
                triplets.emplace_back(k1, k2, -alpha);
            }

            // Pixel down
            if(i < ny - 1)
            {
                k2 = j + (i+1)*nx;
//                std::cout << k1 << ' ' << k2 << std::endl;
                triplets.emplace_back(k1, k2, -alpha);
            }

            // Pixel left
            if(j > 0)
            {
                k2 = (j-1) + i*nx;
//                std::cout << k1 << ' ' << k2 << std::endl;
                triplets.emplace_back(k1, k2, -alpha);
            }

            // Pixel right
            if(j < nx - 1)
            {
                k2 = (j+1) + i*nx;
//                std::cout << k1 << ' ' << k2 << std::endl;
                triplets.emplace_back(k1, k2, -alpha);
            }

        }
    }

    // Make the sparse precision matrix
    Eigen::SparseMatrix<double> sparse_mat(n, n);
    sparse_mat.setFromTriplets(triplets.begin(), triplets.end());

    // Term in the exponential
    logL += -0.5*ys.transpose()*sparse_mat*ys;

    // Use LDLT for log determinant
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> ldlt;
    ldlt.compute(sparse_mat);
    Eigen::VectorXd D = ldlt.vectorD();

    // log det of C, not of anything else!!!
    double log_det = 0.0;
    for(int i=0; i<n; ++i)
        log_det += -log(D(i));
    logL += -0.5*log_det;

    if(std::isnan(logL) || std::isinf(logL))
        logL = -1E300;

    return logL;
}

void NoiseModel2::print(std::ostream& out) const
{
    out << coeff0 << ' ' << coeff1 << ' ' << correlation_logit;
}

std::string NoiseModel2::description()
{
    return "coeff0, coeff1, correlation_logit, ";
}

std::ostream& operator << (std::ostream& out, const NoiseModel2& m)
{
    m.print(out);
    return out;
}

} // namespace CorrelatedNoise


/* Siconos-Kernel, Copyright INRIA 2005-2012.
 * Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 * Siconos is a free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * Siconos is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with Siconos; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * Contact: Vincent ACARY, siconos-team@lists.gforge.inria.fr
 */

#ifndef __determinant__
#define __determinant__

#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>

namespace ublas = boost::numeric::ublas;

template<class matrix_T>
double determinant(ublas::matrix_expression<matrix_T> const& mat_r)
{
  double det = 1.0;

  matrix_T mLu(mat_r());
  ublas::permutation_matrix<std::size_t> pivots(mat_r().size1());

  int is_singular = lu_factorize(mLu, pivots);

  if (!is_singular)
  {
    for (std::size_t i=0; i < pivots.size(); ++i)
    {
      if (pivots(i) != i)
        det *= -1.0;

      det *= mLu(i,i);
    }
  }
  else
    det = 0.0;

  return det;
}

#endif


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/
#include <boost/make_shared.hpp>
#include <ql/math/interpolations/bilinearinterpolation.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/yield/forwardcurve.hpp>
#include <qle/termstructures/blackvariancesurfacemoneyness.hpp>

using namespace std;

namespace QuantExt {

BlackVarianceSurfaceMoneyness::BlackVarianceSurfaceMoneyness(
    const Calendar& cal, const Handle<Quote>& spot, const std::vector<Time>& times, const std::vector<Real>& moneyness,
    const std::vector<std::vector<Handle<Quote> > >& blackVolMatrix, const DayCounter& dayCounter, bool stickyStrike)
    : BlackVarianceTermStructure(0, cal), stickyStrike_(stickyStrike), spot_(spot), dayCounter_(dayCounter),
      moneyness_(moneyness), quotes_(blackVolMatrix) {

    QL_REQUIRE(times.size() == blackVolMatrix.front().size(), "mismatch between times vector and vol matrix colums");
    QL_REQUIRE(moneyness_.size() == blackVolMatrix.size(), "mismatch between moneyness vector and vol matrix rows");

    QL_REQUIRE(times[0] >= 0, "cannot have times[0] < 0");

    if (stickyStrike) {
        // we don't want to know if the spot has changed - we take a copy here
        spot_ = Handle<Quote>(boost::make_shared<SimpleQuote>(spot->value()));
    } else {
        registerWith(spot_);
    }

    Size j, i;
    // internally times_ is one bigger than the input "times"
    times_ = std::vector<Time>(times.size() + 1);
    times_[0] = 0.0;
    variances_ = Matrix(moneyness_.size(), times.size() + 1);
    for (i = 0; i < moneyness_.size(); i++) {
        variances_[i][0] = 0.0;
    }
    for (j = 1; j <= times.size(); j++) {
        times_[j] = times[j - 1];
        QL_REQUIRE(times_[j] > times_[j - 1], "dates must be sorted unique!");
        for (i = 0; i < moneyness_.size(); i++) {
            variances_[i][j] = 0.0;
            registerWith(blackVolMatrix[i][j - 1]);
        }
    }

    varianceSurface_ =
        Bilinear().interpolate(times_.begin(), times_.end(), moneyness_.begin(), moneyness_.end(), variances_);
    notifyObservers();
}

void BlackVarianceSurfaceMoneyness::update() {
    TermStructure::update();
    LazyObject::update();
}

void BlackVarianceSurfaceMoneyness::performCalculations() const {
    for (Size j = 1; j < variances_.columns(); j++) {
        for (Size i = 0; i < variances_.rows(); i++) {
            Real vol = quotes_[i][j - 1]->value();
            variances_[i][j] = times_[j] * vol * vol;
        }
    }
    varianceSurface_.update();
}

Real BlackVarianceSurfaceMoneyness::blackVarianceImpl(Time t, Real strike) const {

    calculate();

    if (t == 0.0)
        return 0.0;

    return blackVarianceMoneyness(t, moneyness(t, strike));
}

Real BlackVarianceSurfaceMoneyness::blackVarianceMoneyness(Time t, Real m) const {
    if (t <= times_.back())
        return varianceSurface_(t, m, true);
    else
        return varianceSurface_(times_.back(), m, true) * t / times_.back();
}

BlackVarianceSurfaceMoneynessSpot::BlackVarianceSurfaceMoneynessSpot(
    const Calendar& cal, const Handle<Quote>& spot, const std::vector<Time>& times, const std::vector<Real>& moneyness,
    const std::vector<std::vector<Handle<Quote> > >& blackVolMatrix, const DayCounter& dayCounter, bool stickyStrike)
    : BlackVarianceSurfaceMoneyness(cal, spot, times, moneyness, blackVolMatrix, dayCounter, stickyStrike) {}

Real BlackVarianceSurfaceMoneynessSpot::moneyness(Time, Real strike) const {
    if (strike == Null<Real>() || strike == 0)
        return 1.0;
    else
        return strike / spot_->value();
}

BlackVarianceSurfaceMoneynessForward::BlackVarianceSurfaceMoneynessForward(
    const Calendar& cal, const Handle<Quote>& spot, const std::vector<Time>& times, const std::vector<Real>& moneyness,
    const std::vector<std::vector<Handle<Quote> > >& blackVolMatrix, const DayCounter& dayCounter,
    const Handle<YieldTermStructure>& forTS, const Handle<YieldTermStructure>& domTS, bool stickyStrike)
    : BlackVarianceSurfaceMoneyness(cal, spot, times, moneyness, blackVolMatrix, dayCounter, stickyStrike),
      forTS_(forTS), domTS_(domTS) {

    if (!stickyStrike) {
        QL_REQUIRE(!forTS_.empty(), "foreign discount curve required for atmf surface");
        QL_REQUIRE(!domTS_.empty(), "foreign discount curve required for atmf surface");
        registerWith(forTS_);
        registerWith(domTS_);
    } else {
        for (Size i = 0; i < times_.size(); i++) {
            Time t = times_[i];
            Real fwd = spot_->value() * forTS_->discount(t) / domTS_->discount(t);
            forwards_.push_back(fwd);
        }
        forwardCurve_ = Linear().interpolate(times_.begin(), times_.end(), forwards_.begin());
    }
}

Real BlackVarianceSurfaceMoneynessForward::moneyness(Time t, Real strike) const {
    Real fwd;
    if (strike == Null<Real>() || strike == 0)
        return 1.0;
    else {
        if (stickyStrike_)
            fwd = forwardCurve_(t, true);
        else
            fwd = spot_->value() * forTS_->discount(t) / domTS_->discount(t);
        return strike / fwd;
    }
}

} // namespace QuantExt


#include "TimeIntegrator.h"

#include <exception>
#include <Eigen/Dense>
#include <cinder/Log.h>

namespace ar
{

    void TimeIntegrator_Newmark1::performStep(real deltaT, const VectorX & mass, const MatrixX & damping, const MatrixX & stiffness, const VectorX & load)
    {
        assertSize(mass);
        assertSize(damping);
        assertSize(stiffness);
        assertSize(load);

        prevU = currentU;
        prevUDot = currentUDot;

        MatrixX massMatrix = mass.asDiagonal();
        MatrixX A = getMatrixPartA(stiffness, damping, mass, load, deltaT, theta);
        VectorX b
            = getMatrixPartB(stiffness, damping, mass, load, deltaT, theta) * prevU
            + getMatrixPartC(stiffness, damping, mass, load, deltaT, theta) * prevUDot
            + getMatrixPartD(stiffness, damping, mass, load, deltaT, theta);
        currentU = solveDense(A, b);

        currentUDot = computeUdot(currentU, prevU, prevUDot, deltaT, theta);
    }

    void TimeIntegrator_Newmark2::performStep(real deltaT, const VectorX & mass, const MatrixX & damping, const MatrixX & stiffness, const VectorX & load)
    {
        assertSize(mass);
        assertSize(damping);
        assertSize(stiffness);
        assertSize(load);

        prevU = currentU;
        prevUDot = currentUDot;
        prevUDotDot = currentUDotDot;

        CI_LOG_V("prevU: " << prevU.transpose());
        CI_LOG_V("prevUDot: " << prevUDot.transpose());
        CI_LOG_V("prevUDotDot: " << prevUDotDot.transpose());

        MatrixX massMatrix = mass.asDiagonal();
        MatrixX A = (4 / (deltaT*deltaT)) * massMatrix + (2 / deltaT) * damping + stiffness;
        VectorX b = load
            + massMatrix * ((4 / (deltaT*deltaT)) * (prevU + prevUDot) + prevUDotDot)
            + damping * ((2 / deltaT) * prevU + prevUDot);
        currentU = solveDense(A, b);
        CI_LOG_V("A:\n" << A);
        CI_LOG_V("b:\n" << b.transpose());
        CI_LOG_V("solution:\n" << currentU.transpose());

        currentUDot = (2 / deltaT) * (currentU - prevU) - prevUDot;
        currentUDotDot = (4 / (deltaT*deltaT)) * (currentU - prevU - deltaT * prevUDot) - prevUDotDot;
    }

    void TimeIntegrator_ExplicitCentralDifferences::performStep(real deltaT, const VectorX & mass, const MatrixX & damping, const MatrixX & stiffness, const VectorX & load)
    {
        assertSize(mass);
        assertSize(damping);
        assertSize(stiffness);
        assertSize(load);

        prevPrevU = prevU;
        prevU = currentU;

        MatrixX massMatrix = mass.asDiagonal();
        MatrixX A = (1 / (deltaT*deltaT)) * massMatrix + (1 / (2 * deltaT)) * damping;
        VectorX b = load
            + ((2 / (deltaT*deltaT)) * massMatrix - stiffness) * prevU
            + ((-1 / (deltaT*deltaT)) * massMatrix + (1 / (2 * deltaT)) * damping) * prevPrevU;
        currentU = solveDense(A, b);
    }

    void TimeIntegrator_ImplicitLinearAcceleration::performStep(real deltaT, const VectorX & mass, const MatrixX & damping, const MatrixX & stiffness, const VectorX & load)
    {
        assertSize(mass);
        assertSize(damping);
        assertSize(stiffness);
        assertSize(load);

        prevU = currentU;
        prevUDot = currentUDot;
        prevUDotDot = currentUDotDot;

        MatrixX massMatrix = mass.asDiagonal();
        MatrixX A = (massMatrix + (deltaT/2)*damping + (deltaT*deltaT/6)*stiffness);
        VectorX b = ((-deltaT / 2) * damping - (deltaT*deltaT / 3) *  stiffness) * prevUDotDot
            - stiffness * prevU
            - damping * prevUDot
            - deltaT * stiffness * prevUDot
            + load;
        currentUDotDot = solveDense(A, b);

        currentUDot = prevUDot + (deltaT / 2) * (currentUDotDot + prevUDotDot);
        currentU = prevU + deltaT * prevUDot + (deltaT*deltaT / 6)*(currentUDotDot + 2 * prevUDotDot);
    }

    void TimeIntegrator_Newmark3::performStep(real deltaT, const VectorX & mass, const MatrixX & damping, const MatrixX & stiffness, const VectorX & load)
    {
        assertSize(mass);
        assertSize(damping);
        assertSize(stiffness);
        assertSize(load);

        prevU = currentU;
        prevUDot = currentUDot;
        prevUDotDot = currentUDotDot;

        MatrixX massMatrix = mass.asDiagonal();
        MatrixX A = ((6 / (deltaT*deltaT))*massMatrix + (3 / deltaT)*damping + stiffness);
        VectorX b = load
            + (3 * massMatrix + (deltaT / 2) * damping) * prevUDotDot
            + ((6 / deltaT) * massMatrix + 3 * damping) * prevUDot;
        VectorX diffX = solveDense(A, b);

        currentU = prevU + diffX;
        currentUDot = -2 * prevUDot - (deltaT / 2)*prevUDotDot + (3 / deltaT) * diffX;
        MatrixX invMassMatrix = mass.cwiseInverse().asDiagonal();
        currentUDotDot = -invMassMatrix * (damping * currentUDot + stiffness * currentU - load);
    }

    void TimeIntegrator_HHTalpha::performStep(real deltaT, const VectorX & mass, const MatrixX & damping, const MatrixX & stiffness, const VectorX & load)
    {
        assertSize(mass);
        assertSize(damping);
        assertSize(stiffness);
        assertSize(load);

        prevU = currentU;
        prevUDot = currentUDot;
        prevUDotDot = currentUDotDot;

        MatrixX massMatrix = mass.asDiagonal();
        MatrixX A = massMatrix + (deltaT * (1 - alpha) * gamma) * damping + (deltaT * deltaT * (1 - alpha) * beta) * stiffness;
        VectorX b = load
            - ((deltaT * (1 - alpha) * (1 - gamma)) * damping + ((deltaT * deltaT) * (1 - alpha) * (0.5f - gamma)) * stiffness) * prevUDotDot
            - (damping + (deltaT * (1 - alpha)) * stiffness) * prevUDot
            - stiffness * prevU;
        currentUDotDot = solveDense(A, b);

        currentU = prevU + deltaT * prevUDot + deltaT * deltaT*((0.5f - beta)*prevUDotDot + beta * currentUDotDot);
        currentUDot = prevUDot + deltaT * ((1 - gamma)*prevUDotDot + gamma * currentUDotDot);
    }

}