// BSD 3-Clause License

// Copyright (c) 2021, Chenyu
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:

// 1. Redistributions of source code must retain the above copyright notice,
// this
//    list of conditions and the following disclaimer.

// 2. 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.

// 3. Neither the name of the copyright holder nor the names of its
//    contributors may be used to endorse or promote products derived from
//    this software without specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

// edited by Steffen Urban (urbste@googlemail.com), August 2021

#include "theia/math/rank_restricted_sdp_solver.h"

#include <glog/logging.h>
#include <limits>

#include <Eigen/Eigenvalues>
#include <Eigen/QR>

namespace theia {
namespace math {

RankRestrictedSDPSolver::RankRestrictedSDPSolver(const size_t n,
                                                 const size_t block_dim)
    : RankRestrictedSDPSolver(n, block_dim, math::SDPSolverOptions()) {}

RankRestrictedSDPSolver::RankRestrictedSDPSolver(
    const size_t n,
    const size_t block_dim,
    const math::SDPSolverOptions& options)
    : BCMSDPSolver(n, block_dim, options), rank_(3) {
  Y_ = Eigen::MatrixXd::Zero(rank_, dim_ * n_);
}

void RankRestrictedSDPSolver::Solve(math::Summary& summary) {
  Eigen::MatrixXd G = Eigen::MatrixXd::Zero(rank_, dim_ * n_);

#pragma omp parallel for num_threads(sdp_solver_options_.num_threads)
  // Compute inital G according to Equ.(3)
  for (size_t i = 0; i < n_; i++) {
    const std::vector<size_t>& adjs = adj_edges_[i];
    for (auto j : adjs) {
      G.block(0, i * dim_, rank_, dim_) +=
          Y_.block(0, j * dim_, rank_, dim_) *
          Q_.block(j * dim_, i * dim_, dim_, dim_);
    }
  }

  double prev_func_val = std::numeric_limits<double>::max();
  double cur_func_val = this->EvaluateFuncVal();
  double duration = 0.0;
  double error = 0.0;

  summary.begin_time = std::chrono::high_resolution_clock::now();
  while (summary.total_iterations_num < sdp_solver_options_.max_iterations) {
    if (sdp_solver_options_.verbose) {
      LogToStd(summary.total_iterations_num,
               prev_func_val,
               cur_func_val,
               error,
               duration);
    }

    if (IsConverge(prev_func_val,
                   cur_func_val,
                   sdp_solver_options_.tolerance,
                   &error)) {
      break;
    }

    for (size_t i = 0; i < n_; i++) {
      const Eigen::MatrixXd G_block = G.block(0, i * dim_, rank_, dim_);
      Eigen::JacobiSVD<Eigen::MatrixXd> jacobi_svd(
          -G_block, Eigen::ComputeThinU | Eigen::ComputeThinV);

      const Eigen::MatrixXd prev_Y = Y_.block(0, i * dim_, rank_, dim_).eval();
      Y_.block(0, i * dim_, rank_, dim_) =
          jacobi_svd.matrixU() * jacobi_svd.matrixV().transpose();

      const std::vector<size_t>& adjs = adj_edges_[i];
#pragma omp parallel for num_threads(sdp_solver_options_.num_threads)
      for (size_t idx = 0; idx < adjs.size(); ++idx) {
        const size_t j = adjs[idx];
        G.block(0, j * dim_, rank_, dim_) +=
            (Y_.block(0, i * dim_, rank_, dim_) - prev_Y) *
            Q_.block(i * dim_, j * dim_, dim_, dim_);
      }
    }

    summary.total_iterations_num++;
    duration = summary.Duration();

    // Update function value
    prev_func_val = cur_func_val;
    cur_func_val = this->EvaluateFuncVal();
  }

  summary.total_iterations_num++;
  if (sdp_solver_options_.verbose) {
    LogToStd(summary.total_iterations_num,
             prev_func_val,
             cur_func_val,
             error,
             duration);
  }
}

Eigen::MatrixXd RankRestrictedSDPSolver::GetSolution() const {
  Eigen::MatrixXd solution = Eigen::MatrixXd(dim_, dim_ * n_);
  for (size_t i = 0; i < n_; i++) {
    solution.block(0, dim_ * i, dim_, dim_) =
        Y_.block(0, 0, rank_, dim_).transpose() *
        Y_.block(0, dim_ * i, rank_, dim_);
  }
  return solution;
}

const Eigen::MatrixXd& RankRestrictedSDPSolver::GetYStar() const { return Y_; }

const Eigen::MatrixXd RankRestrictedSDPSolver::ComputeQYt(
    const Eigen::MatrixXd& Y) const {
  return Q_ * Y.transpose();
}

Eigen::MatrixXd RankRestrictedSDPSolver::ComputeLambdaMatrix() const {
  const Eigen::MatrixXd QYt = this->ComputeQYt(Y_);

  const size_t rank = Y_.rows();
  // \Lambda = \SymblockDiag(Q * Y^T * Y).
  Eigen::MatrixXd Lambda = Eigen::MatrixXd::Zero(dim_, n_ * dim_);

#pragma omp parallel for num_threads(sdp_solver_options_.num_threads)
  for (size_t i = 0; i < n_; ++i) {
    Eigen::MatrixXd P =
        QYt.block(i * dim_, 0, dim_, rank) * Y_.block(0, i * dim_, rank, dim_);
    Lambda.block(0, i * dim_, dim_, dim_) = 0.5 * (P + P.transpose());
  }

  return Lambda;
}

double RankRestrictedSDPSolver::EvaluateFuncVal() const {
  // tr(Q * Y^T * Y) = tr(Y * Q * Y^T)
  // return (Q_ * Y_.transpose() * Y_).trace();
  return (Y_ * (Q_ * Y_.transpose())).trace();
}

double RankRestrictedSDPSolver::EvaluateFuncVal(
    const Eigen::MatrixXd& Y) const {
  return (Y * (Q_ * Y.transpose())).trace();
}

void RankRestrictedSDPSolver::AugmentRank() {
  rank_++;
  Y_.conservativeResize(Y_.rows() + 1, Y_.cols());
  Y_.row(Y_.rows() - 1).setZero();
}

void RankRestrictedSDPSolver::SetOptimalY(const Eigen::MatrixXd& Y) {
  CHECK_EQ(Y.rows(), Y_.rows());
  CHECK_EQ(Y.cols(), Y_.cols());
  Y_ = Y;
}

size_t RankRestrictedSDPSolver::CurrentRank() const { return rank_; }

Eigen::MatrixXd RankRestrictedSDPSolver::Project(
    const Eigen::MatrixXd& A) const {
  // We use a generalization of the well-known SVD-based projection for the
  // orthogonal and special orthogonal groups; see for example Proposition 7
  // in the paper "Projection-Like Retractions on Matrix Manifolds" by Absil
  // and Malick.

  Eigen::MatrixXd P(rank_, dim_ * n_);

#pragma omp parallel for num_threads(sdp_solver_options_.num_threads)
  for (size_t i = 0; i < n_; ++i) {
    // Compute the (thin) SVD of the ith block of A
    Eigen::JacobiSVD<Eigen::MatrixXd> SVD(
        A.block(0, i * dim_, rank_, dim_),
        Eigen::ComputeThinU | Eigen::ComputeThinV);

    // Set the ith block of P to the SVD-based projection of the ith block of A
    P.block(0, i * dim_, rank_, dim_) =
        SVD.matrixU() * SVD.matrixV().transpose();
  }
  return P;
}

Eigen::MatrixXd RankRestrictedSDPSolver::Retract(
    const Eigen::MatrixXd& Y, const Eigen::MatrixXd& V) const {
  // We use projection-based retraction, as described in "Projection-Like
  // Retractions on Matrix Manifolds" by Absil and Malick.
  return Project(Y + V);
}

Eigen::MatrixXd RankRestrictedSDPSolver::EuclideanGradient(
    const Eigen::MatrixXd& Y) const {
  return 2.0 * (Q_ * Y.transpose()).transpose();
}

Eigen::MatrixXd RankRestrictedSDPSolver::RiemannianGradient(
    const Eigen::MatrixXd& Y, const Eigen::MatrixXd& nablaF_Y) const {
  return TangentSpaceProjection(Y, nablaF_Y);
}

Eigen::MatrixXd RankRestrictedSDPSolver::RiemannianGradient(
    const Eigen::MatrixXd& Y) const {
  return TangentSpaceProjection(Y, EuclideanGradient(Y));
}

Eigen::MatrixXd RankRestrictedSDPSolver::TangentSpaceProjection(
    const Eigen::MatrixXd& Y, const Eigen::MatrixXd& dotY) const {
  return dotY - SymBlockDiagProduct(Y, Y, dotY);
}

Eigen::MatrixXd RankRestrictedSDPSolver::SymBlockDiagProduct(
    const Eigen::MatrixXd& A,
    const Eigen::MatrixXd& B,
    const Eigen::MatrixXd& C) const {
  // Preallocate result matrix
  Eigen::MatrixXd R(rank_, dim_ * n_);

#pragma omp parallel for num_threads(sdp_solver_options_.num_threads)
  for (size_t i = 0; i < n_; ++i) {
    // Compute block product Bi' * Ci.
    Eigen::MatrixXd P = B.block(0, i * dim_, rank_, dim_).transpose() *
                        C.block(0, i * dim_, rank_, dim_);
    // Symmetrize this block.
    Eigen::MatrixXd S = 0.5 * (P + P.transpose());
    // Compute Ai * S and set corresponding block of R.
    R.block(0, i * dim_, rank_, dim_) = A.block(0, i * dim_, rank_, dim_) * S;
  }
  return R;
}

Eigen::MatrixXd RankRestrictedSDPSolver::Precondition(
    const Eigen::MatrixXd& Y, const Eigen::MatrixXd& dotY) const {
  switch (sdp_solver_options_.preconditioner_type) {
    case PreconditionerType::None:
      return dotY;
      break;
    case PreconditionerType::JACOBI:
      /* code */
      break;
    case PreconditionerType::INCOMPLETE_CHOLESKY:
      /* code */
      break;
    case PreconditionerType::REGULARIZED_CHOLESKY:
    default:
      break;
  }
}

}  // namespace math
}  // namespace theia


#include <armadillo>
#include <cmath>
#include <emissions.hpp>
#include <HSMM.hpp>
#include <iostream>
#include <vector>

using namespace arma;
using namespace hsmm;
using namespace std;

#define NDIM 2
#define NDUR 70
#define MINDUR 30
#define NOISE_STDDEV 0.05

double gaussianlogpdf_(double x, double mu, double sigma) {
    double ret = ((x - mu)*(x - mu)) / (-2*sigma*sigma);
    ret = ret - log(sqrt(2 * M_PI) * sigma);
    return ret;
}

mat fieldToMat(int njoints, field<mat> &samples) {
    mat ret(njoints, samples.n_elem);
    for(int i = 0; i < samples.n_elem; i++)
        ret.col(i) = samples(i);
    return ret;
}

void fillDurationMatrix(mat& duration, const ivec& centers) {
    assert(centers.n_elem == duration.n_rows);
    duration.zeros();
    //vec pmf_pattern = {0.1, 0.2, 0.4, 0.2, 0.1};
    vec pmf_pattern(20);
    pmf_pattern.fill(1.0/pmf_pattern.n_elem);
    for(int i = 0; i < centers.n_elem; i++) {
        int m = centers(i);
        int half = pmf_pattern.n_elem / 2;
        for(int j = 0; j < pmf_pattern.n_elem; j++)
            duration(i, m - half + j) = pmf_pattern(j);
    }
}

class ToyEmission : public AbstractEmissionOnlineSetting {
    public:
        ToyEmission(vector<function<vec(double)>> states, int ndim,
                double noise_stddev) : states_(states),
                AbstractEmissionOnlineSetting(states.size(), ndim) {
            output_gaussian_stddev_ = ones<mat>(getDimension(),
                    getNumberStates()) * noise_stddev;
        }

        ToyEmission* clone() const {
            return new ToyEmission(*this);
        }

        vec getSampleLocations(int length) const {
            return linspace<vec>(0, 1.0, length);
        }

        double loglikelihood(int state, const field<mat>& obs) const {
            vec t = getSampleLocations(obs.n_elem);
            double ret = 0;
            for(int i = 0; i < obs.n_elem; i++) {
                // Making sure all the missing obs are at the end.
                // Other missing obs patterns are not supported yet.
                if (obs(i).is_empty()) {
                    for(int j = i; j < obs.n_elem; j++)
                        assert(obs(j).is_empty());
                    break;
                }
                vec diff = obs(i) - states_.at(state)(t(i));
                for(int j = 0; j < diff.n_elem; j++)
                    ret += gaussianlogpdf_(diff(j), 0,
                            output_gaussian_stddev_(j, state));
            }
            return ret;
        }

        void reestimate(int min_duration, const field<cube>& meta,
                const field<field<mat>>& mobs ) {
            throw logic_error("Not implemented yet");
        }

        field<mat> sampleFromState(int state, int nsegments,
                mt19937& rng) const {
            vec t = getSampleLocations(nsegments);
            field<mat> ret(nsegments);
            for(int i = 0; i < nsegments; i++) {
                ret(i) = randn<vec>(getDimension());
                ret(i) = ret(i) % output_gaussian_stddev_.col(state);
                ret(i) = ret(i) + states_.at(state)(t(i));
            }
            return ret;
        }

        field<mat> sampleNextObsGivenPastObs(int state, int seg_dur,
                const field<mat>& past_obs, mt19937 &rng) const {
            throw logic_error("Not implemented yet");
        }

    private:
        vector<function<vec(double)>> states_;
        mat output_gaussian_stddev_;
};

vec halfsine1(double t) {
    assert(t <= 1.0 && t >= 0);
    t = t * M_PI;
    vec ret = {sin(t), -sin(t)};
    return ret;
}

vec halfsine2(double t) {
    assert(t <= 1.0 && t >= 0);
    t = t * M_PI;
    vec ret = {-sin(t), sin(t)};
    return ret;
}

vec halfsine3(double t) {
    assert(t <= 1.0 && t >= 0);
    t = t * M_PI;
    vec ret = {0.5*sin(t), 0.25*sin(t)};
    return ret;
}

vec halfsine4(double t) {
    assert(t <= 1.0 && t >= 0);
    t = t * M_PI;
    vec ret = {-0.25*sin(t), -0.5*sin(t)};
    return ret;
}

int main(int arc, char* argv[]) {
    vector<function<vec(double)>> states = {halfsine1, halfsine2,
            halfsine3, halfsine4};
    shared_ptr<ToyEmission> ptr_emission(new ToyEmission(states, NDIM,
            NOISE_STDDEV));
    vec pi(states.size());
    mat transition(states.size(), states.size());
    mat duration(states.size(), NDUR, fill::zeros);

    // Init the parameters.
    pi.fill(1.0 / states.size());
    transition.fill(1.0 / (states.size() - 1));
    transition.diag().zeros();
    ivec duration_centers = {15, 30, 45, 60};
    fillDurationMatrix(duration, duration_centers);

    OnlineHSMM online_toy_model(static_pointer_cast<
            AbstractEmissionOnlineSetting>(ptr_emission), transition, pi,
            duration, MINDUR);
    int nsegments = 12;
    ivec hs, hd;
    field<mat> toy_seq = online_toy_model.sampleSegments(nsegments, hs, hd);
    int nobs = toy_seq.n_elem;
    imat vit_mat = join_horiz(hs, hd);
    cout << "vit file" << endl << vit_mat << endl;

    // Online inference.
    mat state_marginals(states.size(), nobs);
    mat runlength_marginals(MINDUR + NDUR, nobs);
    mat implicit_remaining_marginals(MINDUR + NDUR, nobs);
    mat duration_marginals(NDUR, nobs);
    mat implicit_duration_marginals(NDUR, nobs);
    for(int c = 0; c < nobs; c++) {
        cout << "time step: " << c << endl;
        online_toy_model.addNewObservation(toy_seq(c));
        state_marginals.col(c) = online_toy_model.getStateMarginal();
        runlength_marginals.col(c) = online_toy_model.getRunlengthMarginal();
        duration_marginals.col(c) = online_toy_model.getDurationMarginal();
        implicit_remaining_marginals.col(c) = online_toy_model.
                getImplicitResidualTimeMarginal();
        //implicit_duration_marginals.col(c) = online_toy_model.
        //        getImplicitDurationMarginal();
    }

    // Saving everything.
    string prefix = "/local_data/dagudelo/paper_synth_experiment/";
    mat output = fieldToMat(NDIM, toy_seq);
    output.save(prefix + "paper_synth_obs.txt", raw_ascii);
    vit_mat.save(prefix + "paper_synth_gt_vit_seq.txt", raw_ascii);
    state_marginals.save(prefix + "paper_synth_mstates.txt", raw_ascii);
    runlength_marginals.save(prefix + "paper_synth_runlength.txt", raw_ascii);
    duration_marginals.save(prefix + "paper_synth_duration.txt", raw_ascii);
    implicit_remaining_marginals.save(prefix + "paper_synth_remaining.txt",
            raw_ascii);
    //implicit_duration_marginals.save(prefix + "paper_synth_impli_duration.txt",
    //        raw_ascii);
    return 0;
}


#include <iostream>
#include <cfloat>

#include <armadillo>

#include <glm/models/glm_model.hpp>
#include <glm/models/links/glm_link.hpp>
#include <glm/irls.hpp>
#include <dcdflib/libdcdf.hpp>

using namespace arma;

void
set_missing_to_zero(const uvec &missing, vec &w)
{
    for(int i = 0; i < w.size( ); i++)
    {
        if( missing[ i ] == 1 )
        {
            w[ i ] = 0.0;
        }
    }
}

vec
chi_square_cdf(const vec &x, unsigned int df)
{
    vec p = ones<vec>( x.n_elem );
    for(int i = 0; i < x.n_elem; i++)
    {
        p[ i ] = chi_square_cdf( x[ i ], df );
    }

    return p;
}

vec
weighted_least_squares(const mat &X, const vec &y, const vec &w, bool fast_inversion)
{
    /* A = sqrt( w ) * X */
    mat A = diagmat( sqrt( w ) ) * X;

    /* ty = sqrt( w ) * y */
    vec ty = y % sqrt( w );

    vec beta;
    if( fast_inversion )
    {
        if( solve( beta, A, ty, solve_opts::fast + solve_opts::no_approx ) )
        {
            return beta;
        }
        else
        {
            return vec( );
        }
    }
    else
    {
        if( solve( beta, A, ty ) )
        {
            return beta;
        }
        else
        {
            return vec( );
        }
    }
}

vec
compute_z(const vec &eta, const vec &mu, const vec &mu_eta, const vec &y)
{
    return eta + mu_eta % ( y - mu );
}

vec
compute_w(const vec &var, const vec& mu_eta)
{
    return 1.0 / ( var % ( mu_eta % mu_eta ) );
}

vec
init_beta(const mat &X, const vec&y, const uvec &missing, const glm_model &model, bool fast_inversion = false)
{
    vec eta = model.get_link( ).eta( (y + 0.5) / 3.0 );

    return weighted_least_squares( X, eta, ones<vec>( missing.n_elem ) - missing, fast_inversion );
}

vec
irls(const mat &X, const vec &y, const uvec &missing, const glm_model &model, glm_info &output, bool fast_inversion)
{
    const glm_link &link = model.get_link( );
    vec b = init_beta( X, y, missing, model );
    vec w( X.n_rows );
    vec z( X.n_rows );
    vec eta = X * b;
    vec mu = link.mu( eta );

    vec mu_eta = link.mu_eta( mu );

    int num_iter = 0;
    double old_logl = -DBL_MAX;
    double logl = model.likelihood( mu, y, missing );
    bool invalid_mu = false;
    bool inverse_fail = false;
    vec b_old = b;
    bool first_attempt = true;
    while( num_iter < IRLS_MAX_ITERS && ! ( fabs( logl - old_logl ) / ( 0.1 + fabs( logl ) ) < IRLS_TOLERANCE ) )
    {
        w = compute_w( model.var( mu ), mu_eta );
        z = compute_z( eta, mu, mu_eta, y );
        set_missing_to_zero( missing, w );
        b = weighted_least_squares( X, z, w, fast_inversion );
        if( b.n_elem <= 0 )
        {
            inverse_fail = true;
            break;
        }

compute_eta:
        eta = X * b;
        mu = link.mu( eta );
        mu_eta = link.mu_eta( mu );

        if( !model.valid_mu( mu ) )
        {
            if( first_attempt )
            {
                /* Try a smaller step */
                b = 0.5*b_old + 0.5*b;
                first_attempt = false;
                goto compute_eta;
            }
            else
            {
                invalid_mu = true;
                break;
            }
        }

        old_logl = logl;
        b_old = b;
        logl = model.likelihood( mu, y, missing );

        num_iter++;
    }

    if( num_iter < IRLS_MAX_ITERS && !invalid_mu && !inverse_fail )
    {
        mat I = X.t( ) * diagmat( w ) * X;
        mat C;
        if( I.is_finite( ) && inv( C, I ) )
        {
            float dispersion = model.dispersion( mu, y, missing, b.n_elem );
            output.se_beta = sqrt( model.dispersion( mu, y, missing, dispersion ) * diagvec( C ) );
            output.num_iters = num_iter;
            output.converged = true;
            output.success = true;
            output.mu = mu;
            output.logl = model.likelihood( mu, y, missing, dispersion );

            vec wald_z = b / output.se_beta;
            vec chi2_value = wald_z % wald_z;
            output.p_value = -1.0 * ones<vec>( chi2_value.n_elem );
            for(int i = 0; i < chi2_value.n_elem; i++)
            {
                try
                {
                    output.p_value[ i ] = 1.0 - chi_square_cdf( chi2_value[ i ], 1 );
                }
                catch(bad_domain_value &e)
                {
                    continue;
                }
            }
        }
        else
        {
            output.success = false;
        }
    }
    else
    {
        output.num_iters = num_iter;
        output.converged = false;
        output.success = false;
    }

    return b;
}

vec
irls(const mat &X, const vec &y, const glm_model &model, glm_info &output, bool fast_inversion)
{
    uvec missing = zeros<uvec>( y.n_elem );
    return irls( X, y, missing, model, output );
}


/**
 * @file IndexGraph.hpp
 * @author bwu
 * @brief Model of index graph concept and graph related algorithms
 * @version 0.1
 * @date 2022-02-22
 */
#ifndef GENERIC_TOPOLOGY_INDEXGRAPH_HPP
#define GENERIC_TOPOLOGY_INDEXGRAPH_HPP
#include "generic/common/Exception.hpp"
#include "generic/common/Archive.hpp"
#include "Common.hpp"
#include <boost/graph/connected_components.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/functional/hash.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/visitors.hpp>
#include <unordered_set>

namespace generic  {
namespace topology {

///@brief represents model of undirected index edge concept
struct UndirectedIndexEdge
{
    ///@brief constructs an invalid edge
    UndirectedIndexEdge()
    {
        m_vertices = std::make_pair(noIndex, noIndex);
    }

    ///@brief constructs an undirected edge with vertex `iv1` and `iv2`
    UndirectedIndexEdge(index_t iv1, index_t iv2)
    {
       SetVertices(iv1, iv2);
    }

    ///@brief checks if this edge equals to `e`
    bool operator==(const UndirectedIndexEdge & e) const { return m_vertices == e.m_vertices; }
    ///@brief checks if this edge not equals to `e`
    bool operator!=(const UndirectedIndexEdge & e) const { return !(*this == e); }

    ///@brief set two vertices index of this edges
    void SetVertices(index_t iv1, index_t iv2) { m_vertices.first = std::min(iv1, iv2); m_vertices.second = std::max(iv1, iv2); }

    ///@brief returns the smaller vertex index of this edge
    index_t v1() const { return m_vertices.first ; }
    ///@brief returns the bigger vertex index of this edge
    index_t v2() const { return m_vertices.second; }

    ///@brief chechks if this edge contains vertex `iv`
    bool hasVertex(index_t iv) const { return iv == m_vertices.first || iv == m_vertices.second; }

private:
    std::pair<index_t, index_t> m_vertices;

#ifdef BOOST_SERIALIZATION_SUPPORT
private:
    friend class boost::serialization::access;
    template <typename Archive>
    void serialize(Archive & ar, const unsigned int)
    {
        ar & m_vertices;
    }
#endif
};

struct UndirectedIndexEdgeHash
{
    size_t operator() (const UndirectedIndexEdge & edge) const noexcept
    {
        size_t seed(0);
        boost::hash_combine(seed, edge.v1());
        boost::hash_combine(seed, edge.v2());
        return seed;
    }
};

struct UndirectedIndexEdgeCompare
{
    bool operator() (const UndirectedIndexEdge & e1, const UndirectedIndexEdge & e2) const noexcept { return e1 == e2; }
};

using UndirectedIndexEdgeSet = std::unordered_set<UndirectedIndexEdge, UndirectedIndexEdgeHash, UndirectedIndexEdgeCompare>;

template <typename T>
using UndirectedIndexEdgeMap = std::unordered_map<UndirectedIndexEdge, T, UndirectedIndexEdgeHash, UndirectedIndexEdgeCompare>;

///@brief represents model of sparse undirect index graph concept
using SparseIndexGraph = boost::adjacency_list<
                         boost::setS,
                         boost::vecS,
                         boost::undirectedS,
                         boost::property<boost::vertex_index_t, index_t >
                         >;

using SIGEdge = boost::graph_traits<SparseIndexGraph>::edge_descriptor;
using SIGEdgeIter = boost::graph_traits<SparseIndexGraph>::edge_iterator;
using SIGVertex = boost::graph_traits<SparseIndexGraph>::vertex_descriptor;

inline void AddEdge(index_t i, index_t j, SparseIndexGraph & g)
{
    boost::add_edge(i, j, g);
}

inline std::pair<SIGEdgeIter, SIGEdgeIter> Edges(const SparseIndexGraph & g)
{
    return boost::edges(g);
}

inline SIGVertex Source(const SIGEdge & e, const SparseIndexGraph & g)
{
    return boost::source(e, g);
}

inline SIGVertex Target(const SIGEdge & e, const SparseIndexGraph & g)
{
    return boost::target(e, g);
}

/**
 * @brief gets connected component by BFS
 *
 * @param[in] g the connection graph
 * @param[in] v the source vertex
 * @param[out] c contains the vertices connected to source v
 */
inline void ConnectedComponent(const SparseIndexGraph & g, const index_t v, std::list<index_t> & c)
{
    using namespace boost;
    GENERIC_ASSERT(v < num_vertices(g))

    class BFSVisitor : public default_bfs_visitor
    {
    public:
        std::list<index_t> & visited;
        BFSVisitor(std::list<index_t> & _visited) : visited(_visited) {}
        void discover_vertex(index_t s, const SparseIndexGraph &) { visited.push_back(s); }
    };

    c.clear();
    BFSVisitor vis(c);
    breadth_first_search(g, v, visitor(vis));
}

/**
 * @brief gets all connected components from graph
 *
 * @param[in] g the connection graph
 * @param[out] cc connected components
 */
inline void ConnectedComponents(const SparseIndexGraph & g, std::vector<std::list<index_t> > & cc)
{
    using namespace boost;
    std::vector<index_t> c(num_vertices(g));
    auto numComp = connected_components(g, make_iterator_property_map(c.begin(), get(vertex_index, g)));

    cc.clear();
    cc.resize(numComp);
    for(size_t i = 0; i < c.size(); ++i)
        cc[c[i]].push_back(i);
}

inline UndirectedIndexEdgeSet EdgeSet(const SparseIndexGraph & g)
{
    UndirectedIndexEdgeSet edgeSet;
    auto [begin, end] = topology::Edges(g);
    for(auto iter = begin; iter != end; ++iter){
        auto u = topology::Source(*iter, g);
        auto v = topology::Target(*iter, g);
        edgeSet.insert(UndirectedIndexEdge{u, v});
    }
    return edgeSet;
}

}//namespace topology
}//namespace generic
#endif//GENERIC_TOPOLOGY_INDEXGRAPH_HPP


/*
 * util.hh
 *
 *  Created on: May 3, 2015
 *      Author: sunayana_ghosh
 */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2015-2015 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

//In this file we implement some of the utility functions necessary for
//the classes WorkModel, ErrorEstimate and the main integrate function of the class EulerSDC.

#ifndef KASKADE_TIMESTEPPING_EULERSDC_UTIL_HH_
#define KASKADE_TIMESTEPPING_EULERSDC_UTIL_HH_

#include "norm.hh"

#include <cmath>          //std::abs
#include <functional>     //std::function
#include <vector>

#include <boost/timer/timer.hpp>

#include "dune/common/dynmatrix.hh"
#include "dune/common/dynvector.hh"

#include "fem/fixdune.hh"
#include "timestepping/sdc.hh"

namespace Kaskade {
  /**
   * \ingroup eulerSDC
   * \brief Some utility functions.
   */
//=================================================================================================
//  Utility function to find norm of a difference of vector of two Vectors
//=================================================================================================

/**
 * \ingroup eulerSDC
 * \brief Function to compute the norm of the difference of two vector of Vectors.
 *
 * @param y0  : A vector of Vectors, usually of type std::vector<Vector>
 * @param y1  : A vector of Vectors, usually of type std::vector<Vector>
 * @param norm  : A type of norm, in this case an object of type Kaskade::Norm
 * @return An object of type Vector::value_type is returned, it is usually a double.
 */
template<class Vector, class Norm>
typename Vector::value_type normVecDiff(std::vector<Vector> const& y0, std::vector<Vector> const& y1, Norm norm);

//=================================================================================================
//  Implementation for single sdc iteration step based on explicit Euler method.
//=================================================================================================
 /**
  * \ingroup eulerSDC
  * This function performs one spectral defect correction (SDC) iteration for a system of ODE's of the form:
  * \f$ y'(t) = f(y(t)) \f$, \f$ y(0) = y_0\f$ and \f$ t \in [0, T] \f$,
  * using the explicit Euler method on a given time grid. Here SDC is interpreted as fixed point iteration.
  * Given an approximate solution \f$ y^{[j]} \in \mathbb{P}_N \f$, the error function
  * \f[ \delta^{[j]} = y - y^{[j]} \f]
  * satisfies the defect equation
  * \f[ \delta^{[j]'}(t) = y'(t) - y^{[j]'}(t) = f(y(t)) - y^{[j]'}(t). \f]
  * The equivalent Picard equation is
  * \f[ \delta^{[j]}(t) = \int_{\tau=0}^t \left( f(y^{[j]}(\tau)+\delta^{[j]}(\tau)) - y^{[j]'}(\tau) \,d\tau\right)\f]
  * Evaluated at the grid nodes \f$ t_i\f$ we obtain
  * \f{eqnarray*}{
  * \delta^{[j]}_i & = & \delta^{[j]}_{i-1} + \int_{\tau=t_{i-1}}^{t_i} \left( f(y^{[j]}(\tau)+\delta^{[j]}(\tau)) - {y^{[j]}}'(\tau) \right) \,d\tau \\
  *                & = & \delta^{[j]}_{i-1} + \int_{\tau=t_{i-1}}^{t_i} \left( f(y^{[j]}(\tau)+\delta^{[j]}(\tau)) - f(y^{[j]}(\tau))\right) \,d\tau
  *               + \int_{\tau=t_{i-1}}^{t_i} \hspace{-0.5em}f(y^{[j]}(\tau)) \,d\tau - ( y^{[j]}_i-y^{[j]}_{i-1})
  * \f}
  * starting at \f$ \delta_0^{[j]} = 0. \f$ Using left looking rectangular rule for approximating the first integral, and the canonical quadrature by
  * polynomial interpolation on the nodes \f$ t_1,\ldots t_N \f$ for second integral, approximate values \f$ \hat{\delta}_i^{[j]} \f$ for
  * \f$ \delta_i^{[j]} \f$ can be evaluated. With the left looking rectangular rule, we obtain the explicit scheme
  * \f{eqnarray*}{
  *  \hat\delta^{[j]}_i & = &  \hat\delta^{[j]}_{i-1}
  * + (t_i-t_{i-1}) \left( f(y^{[j]}_{i-1}+\hat{\delta}^{[j]}_{i-1}) - f(y^{[j]}_{i-1})\right)
  * +  \sum_{k=1}^N S_{ik} f(y^{[j]}_k) - y^{[j]}_i+y^{[j]}_{i-1},
  * \f}
  * where the entries of the spectral quadrature matrix \f$ S \in \mathbb{R}^{N \times N} \f$ are defined in terms of the Lagrange polynomials
  * \f$ L_k \in \mathbb{P}_N \f$ satisfying \f$ L_k(t_i) = \delta_{ik} \f$ as
  * \f{eqnarray*}{
  *   S_{ik} & = & \int_{\tau=t_{i-1}}^{t_i} L_k(\tau) \,d\tau, \quad i,k =1,\dots, N.
  * \f}
  * An improved approximation \f$ y^{[j+1]} \f$ is then obtained by polynomial interpolation of \f$ \hat \delta^{[j]}_i \f$,
  * \f{eqnarray*}{
  * y^{[j+1]} = y^{[j]} + \hat\delta^{[j]}.
  * \f}
  *
  *\tparam Vector  a vector type, usually Dune::DenseVector.
  *
  * \param[in] grid the collocation time grid
  * \param[in] rhsFunc is the function which represents the right hand side \f$ f \f$ and is used in the computation of \f$ \delta y \f$.
  * \param[out] yi stores the current iterate of the solution \f$ y \f$.
  * \param[out] dyi is the approximate correction \f$ \delta y \f$
  * \param[in] verbose a boolean used to print out the iterates of \f$ y \f$.
  *
  */

  template<class Vector>
  void sdcExplicitEulerIterationStep(Kaskade::SDCTimeGrid const& grid,
                                     std::function<Vector(typename Vector::value_type, Vector const&)> rhsFunc,
                                     std::vector<Vector> & yi, std::vector<Vector> & dyi,
                                     bool verbose = false)
  {
    //extract time points from the grid
    auto const& pts = grid.points();
    //extracting values of the integration matrix corresponding to the time grid.
    auto const& integ = grid.integrationMatrix();
    //number of intervals
    int const n = pts.size() - 1;

    //including starting point for yi and dyi
    assert(yi.size() == n+1);
    assert(dyi.size() == n+1);

    //initialize all elements of dyi to zero.
    for (int i = 0; i <= n; ++i)
    {
      dyi[i] = Vector(0.0);
    }

    //declare Vector total to be used inside the for loop
    Vector total(0.0);

    //the SDC step for explicit Euler
    //perform n Euler steps
    boost::timer::cpu_timer timer;
    for (auto i = 1; i <= n; ++i)
    {
      //Initialize all the entries of total to zero
      total = Vector(0.0);
      //computation of the Lagrange interpolation
      for (int k = 0; k <= n; ++k)
      {
        total += integ[i-1][k] * rhsFunc(pts[k], yi[k]);
      }
      //approximate correction computation
      dyi[i] = dyi[i-1] + (pts[i] - pts[i-1]) *(rhsFunc(pts[i-1],yi[i-1]+dyi[i-1]) - rhsFunc(pts[i-1],yi[i-1])) + total - yi[i] + yi[i-1];
    }

    //Compute the next iteration of yi.
    for (auto i = 0; i <= n; ++i)
    {
      yi[i] += dyi[i];
      if (verbose)
        std::cout << "yi[" << i << "] = " << yi[i] << "\n";
    }

  }

  //=================================================================================================
   //  Implementation for single inexact sdc iteration step based on explicit Euler method.
   //=================================================================================================

   template<class Vector, class RealVector>
   void inexactSDCExplicitEulerIterationStep(Kaskade::SDCTimeGrid const& grid,
                                             std::function<Vector(typename Vector::value_type, Vector const&)> rhsFunc,
                                             RealVector const& toleranceVector, typename Vector::value_type rho,
                                             Kaskade::NormType norm_t,
                                             std::vector<Vector> & yi, std::vector<Vector> & dyi,
                                             bool verbose = false)
     {
       //extract time points from the grid
       auto const& tpts = grid.points();
       //extracting values of the integration matrix corresponding to the time grid.
       auto const& integ = grid.integrationMatrix();
       //number of intervals
       unsigned int const n = tpts.size() - 1;

       //including starting point for yi and dyi
       assert(yi.size() == n+1);
       assert(dyi.size() == n+1);

       //initialize all elements of dyi to zero.
       for (auto i = 0u; i <= n; ++i)
       {
         dyi[i] = Vector(0.0);
       }

       //declare Vector total to be used inside the for loop
       Vector total(0.0);

       //convert the toleranceVector which is a vector of doubles to a vector of Vectors.
       std::vector<Vector> errVector(n+1, Vector(0.0));
       //depending on the normType fill in err vector. This step is taken since the tolerance vector is a vector of scalars
       switch(norm_t)
       {
       case Kaskade::NormType::ONE_NORM:
       {
         auto sz = dyi[0].size();
         for (auto i = 1u; i <= n; ++i)
           errVector[i] = Vector(toleranceVector[i]/sz);
         break;
       }
       case Kaskade::NormType::MAX_NORM:
       {
         for (auto i = 1u; i <= n; ++i)
           errVector[i] = Vector(toleranceVector[i]);
         break;
        }
       }

       //the perturbed SDC step for explicit Euler
       //perform n Euler steps
       boost::timer::cpu_timer timer;
       for (auto i = 1u; i <= n; ++i)
       {
         //Initialize all the entries of total to zero
         total = Vector(0.0);

         //computation of the Lagrange interpolation
         for (auto k = 0u; k <= n; ++k)
         {
           total += integ[i-1][k] * (rhsFunc(tpts[k], yi[k]) + errVector[k]);
         }
         //approximate correction computation
         dyi[i] = dyi[i-1] + (tpts[i] - tpts[i-1]) *((rhsFunc(tpts[i-1],yi[i-1]+dyi[i-1]) - rhsFunc(tpts[i-1],yi[i-1]))- (1 - rho)*errVector[i-1]) + total - yi[i] + yi[i-1];
       }

       //Compute the next iteration of yi.
       for (auto i = 0u; i <= n; ++i)
       {
         yi[i] += dyi[i];
         if (verbose)
           std::cout << "yi[" << i << "] = " << yi[i] << "\n";
       }
     }

  //===================================================================================================
  //   Implementation of the abstract base class SDCUtil for all utility methods depending on the norms.
  //===================================================================================================

  template <class Vector, class Norm>
  class SDCUtil
  {
  public:
    using field_type = typename Vector::value_type;
    using RealVector = Dune::DynamicVector<double>;
    using RealMatrix = Dune::DynamicMatrix<double>;

    //virtual function
    /**
     * \ingroup eulerSDC
     * This function computes an estimate of the SDC contraction factor (\f$ \rho \f$) given three consecutive iterations of \f$ y \f$ and
     * depends on the given norm.
     * The estimate of \f$ \rho \f$ is given by:
     * \f{eqnarray*}{
     *  \rho & = & \frac{\|y^{[j+1]} - y^{[j]}\|}{\|y^{[j]} - y^{[j-1]}\| + \|y^{[j+1]} - y^{[j]}\|}
     * \f}
     *
     * \param[in] yPrev a Vector denoting previous iterate of \f$ y \f$.
     * \param[in] yCurrent a Vector denoting current iterate of \f$ y \f$.
     * \param[in] yNext a Vector denoting next iterate of \f$ y \f$   as obtained from SDC iteration step.
     * \param[in] norm an object of abstract base class type Norm, where we can provide the specific norm we consider for a problem.
     *
     * \return \f$ \rho \f$ an estimate of the SDC contraction factor. Return type is Vector::field_type
     */
    field_type sdcContractionFactor(std::vector<Vector> const& yPrev,
                                    std::vector<Vector> const& yCurrent,
                                    std::vector<Vector> const& yNext,
                                    Norm& norm);

    //pure virtual functions
    /**
     * \ingroup eulerSDC
     * The function definition changes with the associated norm. For derivation details for the vectors \f$ \alpha \f$
     * and \f$ \Gamma \f$ see M. Weiser, S. Ghosh: Adaptive inexact SDC Methods.
     * The vector \f$ \alpha \f$ is given by \f$  \alpha = \{\alpha_1, \ldots , \alpha_N \}\f$, where
     * \f{eqnarray*}{
     * \alpha_i & = & \sum_{k=1}^N |S_{ki}| + (1 - \delta_{i,N})\,(t_{i+1} - t_i)\,(1 + \rho),
     * \f}
     * this is computed in case of \f$ 1\f$-norm and in the case of \f$ \max\f$-norm we have
     * \f$ \Gamma = \{\Gamma_1, \ldots, \Gamma_N \} \f$, where
     *  \f{eqnarray*}{
     *    \Gamma_i & = & \max_{1\leq n \leq N} \alpha_{n,i}, \textrm{ such that } \\
     *    \alpha_{n,i} & = & |S_{ni}| + \delta_{i,n-1}\,(t_i - t_{i-1})\,(1+\rho)
     *  \f}
     * @param[in] timePoints : Time points generated using the points() method of SDCTimeGrid.
     * @param[in] integrationMatrix : Integration matrix generated using the integrationMatrix() method of SDCTimeGrid.
     *                            Computed once for a given time grid.
     * @param[in] yPrev : Estimate of exact solution \f$ y \f$ using SDC iteration step
     * @param[in] yCurrent : The following estimate of \f$ y \f$ using SDC iteration step
     * @param[in] yNext : The next estimate of \f$ y \f$ after yCurrent using SDC iteration step
     *
     * \return Reference to a Vector, where the vector is \f$ \alpha \f$ or \f$ \Gamma \f$ depending on the norm.
     */

    virtual RealVector const& computeAlphaVec(RealVector const& timePoints,
                                              RealMatrix const& integrationMatrix,
                                              std::vector<Vector> const& yPrev,
                                              std::vector<Vector> const& yCurrent,
                                              std::vector<Vector> const& yNext) = 0;

    //virtual destructor
    virtual ~SDCUtil(){}
  };

  //===================================================================================================
  //   Implementation of the derived class SDCUtilOneNorm w.r.t. 1-norm.
  //===================================================================================================
  template<class Vector, class Norm>
  class SDCUtilOneNorm : public SDCUtil<Vector, Norm>
  {
  public:
    using field_type = typename Vector::value_type;
    using RealVector = Dune::DynamicVector<double>;
    using RealMatrix = Dune::DynamicMatrix<double>;

    /**
     *
     * @param timePts : Represents a vector of time points
     * @param integrationMatrix
     * @param yPrev
     * @param yCurrent
     * @param yNext
     * @return
     */

    virtual RealVector const& computeAlphaVec(RealVector const& timePts,
                                              RealMatrix const& integrationMatrix,
                                              std::vector<Vector> const& yPrev,
                                              std::vector<Vector> const& yCurrent,
                                              std::vector<Vector> const& yNext)
    {
      //number of collocation points
      auto nCollocationPts = timePts.size();
      alphaVec = RealVector(nCollocationPts-1, 0.0);

      //create one norm object
      Kaskade::OneNorm<Vector> on;
      //compute sdc contraction rate
      auto rho = Kaskade::SDCUtil<Vector, Norm>::sdcContractionFactor(yPrev, yCurrent, yNext, on);
      for (auto i = 0u; i < nCollocationPts-1; ++i)
      {
        field_type total = 0.0;
        for (auto k = 0u; k < nCollocationPts-1; ++k)
          total += std::abs(integrationMatrix[k][i]);
        if (i != nCollocationPts-2)
          alphaVec[i] = (total + (timePts[i+1] - timePts[i]) * (1 + rho));
        else
          alphaVec[i] = total;
      }
      return alphaVec;
    }

    virtual ~SDCUtilOneNorm(){}

  private:
    RealVector alphaVec;
    RealMatrix alphaMat;
  };

  //===================================================================================================
  //   Implementation of the derived class SDCUtilMaxNorm w.r.t. max-norm.
  //===================================================================================================
  template<class Vector, class Norm>
    class SDCUtilMaxNorm : public SDCUtil<Vector, Norm>
    {
    public:
      using field_type = typename Vector::value_type;
      using RealVector = Dune::DynamicVector<double>;
      using RealMatrix = Dune::DynamicMatrix<double>;

      //TODO: Doc me
      /**
       *
       * @param timePts
       * @param integrationMatrix
       * @param yPrev
       * @param yCurrent
       * @param yNext
       * @return
       */
      virtual RealVector const& computeAlphaVec(RealVector const& timePts,
                                                RealMatrix const& integrationMatrix,
                                                std::vector<Vector> const& yPrev,
                                                std::vector<Vector> const& yCurrent,
                                                std::vector<Vector> const& yNext)
      {
        //number of collocation points.
        auto nCollocationPts = timePts.size();
        gamma = RealVector(nCollocationPts, 0.0);

        //initialize the matrix alpha with size (nCollocationPts - 1) x nCollocationPts
        auto alphaMat = computeAlphaMat(timePts, integrationMatrix, yPrev, yCurrent, yNext, nCollocationPts);
        //Compute the vector Gamma.
        for (int i = 0; i < nCollocationPts -1; ++i)
        {
          for (int j = 0; j < nCollocationPts; ++j)
          {
            if (alphaMat[i][j] > gamma[j])
              gamma[j] = alphaMat[i][j];
          }
        }
        return gamma;
       }

      virtual ~SDCUtilMaxNorm() {};
    private:
      RealVector gamma;
      RealMatrix alphaMat;

      /**
       *
       * @param timePts
       * @param integrationMatrix
       * @param yPrev
       * @param yCurrent
       * @param yNext
       * @param nCollocationPts
       * @return
       */
      RealMatrix const& computeAlphaMat(RealVector const& timePts,
                                        RealMatrix const& integrationMatrix,
                                        std::vector<Vector> const& yPrev,
                                        std::vector<Vector> const& yCurrent,
                                        std::vector<Vector> const& yNext,
                                        int nCollocationPts);

    };

  //=================================================================================================
  //   FUNCTION DEFINITION BEGINS FROM HERE
  //=================================================================================================

  //=================================================================================================
  //  Utility function to find norm of a difference of vector of two Vectors
  //=================================================================================================

  template<class Vector, class Norm>
  typename Vector::value_type normVecDiff(std::vector<Vector> const& y0, std::vector<Vector> const& y1, Norm norm)
   {
     //Initialize the all the entries of dy to 0.0
     std::vector<Vector> dy(y0.size(), Vector(0.0));
     for (auto i = 0u; i < y0.size(); ++i)
     {
       dy[i] = y1[i] - y0[i];
     }
     return norm.value(dy);
   }

  //===================================================================================================
  //   Implementation of the abstract base class SDCUtil for all utility methods depending on the norms.
  //===================================================================================================

  template <class Vector, class Norm>
  typename Vector::value_type SDCUtil<Vector, Norm>::sdcContractionFactor(std::vector<Vector> const& yPrev,
                                                                          std::vector<Vector> const& yCurrent,
                                                                          std::vector<Vector> const& yNext,
                                                                          Norm& norm)
   {
    size_t n = yPrev.size();
    //Compute the differences: yPrev - yCurrent and yCurrent - yNext
    std::vector<Vector> y01(n, Vector(0.0));
    std::vector<Vector> y12(n, Vector(0.0));
    for (auto i = 0u; i < n; ++i)
    {
      y01[i] = yCurrent[i] - yPrev[i] ;
      y12[i] = yNext[i] - yCurrent[i];
    }
    auto val01 = norm.value(y01);
    auto val12 = norm.value(y12);
    auto beta = val12/(val01 + val12);
    auto val = val12/val01;
    if (val > 1 || val != val) //check if val > 1 or val is NaN.
    {
      if (beta != beta) //check if beta = NaN
        val = 0.70;
      else
        val = beta;
    }
    return val;
   }

  //===================================================================================================
  //   Implementation of the private method computeAlphaMat for the derived class SDCUtilMaxNorm
  //===================================================================================================
  using RealVector = Dune::DynamicVector<double>;
  using RealMatrix = Dune::DynamicMatrix<double>;

  template<class Vector, class Norm>
  RealMatrix const& SDCUtilMaxNorm<Vector, Norm>::computeAlphaMat(RealVector const& timePts,
                                                                  RealMatrix const& integrationMatrix,
                                                                  std::vector<Vector> const& yPrev,
                                                                  std::vector<Vector> const& yCurrent,
                                                                  std::vector<Vector> const& yNext,
                                                                  int nCollocationPts)
    {
    //create max norm object
    Kaskade::MaxNorm<Vector> mn;
    //compute sdc contraction rate
    auto rho = Kaskade::SDCUtil<Vector, Norm>::sdcContractionFactor(yPrev, yCurrent, yNext, mn);
    alphaMat = RealMatrix(nCollocationPts-1, nCollocationPts, 0.0);
    for (auto i = 0; i < nCollocationPts-1; ++i)
    {
      for (auto k = 0; k < nCollocationPts; ++k)
      {
        if (i == k)
          alphaMat[i][k] = std::abs(integrationMatrix[i][k]) + (timePts[i+1] - timePts[i]) * (1 + rho);
        else
          alphaMat[i][k] = std::abs(integrationMatrix[i][k]);
      }
    }
    return alphaMat;
    }

}//end namespace Kaskade

#endif /* KASKADE_TIMESTEPPING_EULERSDC_UTIL_HH_ */


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SCALAR_REM_PIO2_MEDIUM_HPP_INCLUDED
#define NT2_TOOLBOX_TRIGONOMETRIC_FUNCTIONS_SCALAR_REM_PIO2_MEDIUM_HPP_INCLUDED
#include <nt2/toolbox/trigonometric/functions/rem_pio2_medium.hpp>
#include <nt2/toolbox/trigonometric/constants.hpp>
#include <nt2/include/functions/scalar/round.hpp>
#include <nt2/include/functions/scalar/fast_toint.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <boost/fusion/tuple.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_pio2_medium_, tag::cpu_,
                             (A0),
                             (scalar_ < floating_<A0> > )
                             )
  {
    typedef boost::fusion::tuple<A0,A0,nt2::int32_t>           result_type;

    inline result_type operator()(A0 const& a0) const
    {
      result_type res;
      boost::fusion::at_c<2>(res) =
      nt2::rem_pio2_medium(a0,
                           boost::fusion::at_c<0>(res),
                           boost::fusion::at_c<1>(res)
                          );
      return res;
    }
  };

  /////////////////////////////////////////////////////////////////////////////
  // reference based Implementation when real
  /////////////////////////////////////////////////////////////////////////////
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::rem_pio2_medium_, tag::cpu_,
                             (A0),
                             (scalar_ < floating_<A0> > )
                             (scalar_ < floating_<A0> > )
                             (scalar_ < floating_<A0> > )
                             )
  {
    typedef typename meta::as_integer<A0>::type result_type;
    inline result_type operator()(A0 const& t, A0 & xr, A0& xc) const
    {
      const A0 fn = nt2::round(t*Invpio_2<A0>());
      A0 r  = t-fn*Pio2_1<A0>();
      A0 w  = fn*Pio2_1t<A0>();
      A0 t2 = r;
      w  = fn*Pio2_2<A0>();
      r  = t2-w;
      w  = fn*Pio2_2t<A0>()-((t2-r)-w);
      t2 = r;
      w  = fn*Pio2_3<A0>();
      r  = t2-w;
      w  = fn*Pio2_3t<A0>()-((t2-r)-w);
      xr = r-w;
      xc = (r-xr)-w;
      return  fast_toint(fn)&3;
    }
  };
} }
#endif


#include <vector>
#include <algorithm>

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

#include "MODI.hpp"
#include "Helper.hpp"

namespace ublas = boost::numeric::ublas;
using namespace std;

namespace t_simplex
{
	/**********************
	Vogel's initialization method
	**********************/
	void _initVogel(
		vector<double> &bi,
		vector<double> dj,
		TsBasic * basicsEnd,
		TsBasic ** facBasics,
		TsBasic ** cusBasics,
		bool ** isBasic,
		int m,
		int n,
		ublas::matrix<double> &cij,
		double &_tsMaxW)
	{
		int i, j;
		TsVogPen *srcPens = NULL;
		TsVogPen *snkPens = NULL;
		TsVogPen *pitra = NULL, *pitrb = NULL;  //iterators
		TsVogPen *maxPen = NULL;
		TsVogPen srcPenHead, snkPenHead;
		bool maxIsSrc = false;
		double lowVal = 0.0;

		try
		{
			srcPens = new TsVogPen[m];
			snkPens = new TsVogPen[n];
		}
		catch (std::bad_alloc)
		{
			delete[] srcPens;
			delete[] snkPens;
			throw;
		}

		srcPenHead.next = pitra = srcPens;
		for (i = 0; i < m; i++)
		{
			pitra->i = i;
			pitra->next = pitra + 1;
			pitra->prev = pitra - 1;
			pitra->one = pitra->two = 0;
			pitra->oneCost = pitra->twoCost = TSINFINITY;
			pitra++;
		}
		(--pitra)->next = NULL;
		srcPens[0].prev = &srcPenHead;

		snkPenHead.next = pitra = snkPens;
		for (i = 0; i < n; i++)
		{
			pitra->i = i;
			pitra->next = pitra + 1;
			pitra->prev = pitra - 1;
			pitra->one = pitra->two = 0;
			pitra->oneCost = pitra->twoCost = TSINFINITY;
			pitra++;
		}
		(--pitra)->next = NULL;
		snkPens[0].prev = &snkPenHead;

		for (pitra = srcPenHead.next, i = 0; pitra != NULL; pitra = pitra->next, i++)
		{
			for (pitrb = snkPenHead.next, j = 0; pitrb != NULL; pitrb = pitrb->next, j++)
			{
				//initialize Source Penalties;
				addPenalty(pitra, cij(i, j), j);
				addPenalty(pitrb, cij(i, j), i);
			}
		}

		while (srcPenHead.next != NULL && snkPenHead.next != NULL)
		{
			maxIsSrc = true;
			for (maxPen = pitra = srcPenHead.next; pitra != NULL; pitra = pitra->next)
				if ((pitra->twoCost - pitra->oneCost) > (maxPen->twoCost - maxPen->oneCost))
					maxPen = pitra;

			for (pitra = snkPenHead.next; pitra != NULL; pitra = pitra->next)
				if ((pitra->twoCost - pitra->oneCost) > (maxPen->twoCost - maxPen->oneCost))
				{
					maxPen = pitra;
					maxIsSrc = false;
				}

			if (maxIsSrc)
			{
				i = maxPen->i;
				j = maxPen->one;
			}
			else
			{
				j = maxPen->i;
				i = maxPen->one;
			}

			if (dj[j] - bi[i] > _tsMaxW * TSEPSILON
				|| (srcPenHead.next->next != NULL
				&& fabs(dj[i] - bi[j]) < _tsMaxW * TSEPSILON))
			{
				//delete source
				lowVal = bi[i];
				maxPen = srcPens + i;
				maxPen->prev->next = maxPen->next;
				if (maxPen->next != NULL)
					maxPen->next->prev = maxPen->prev;

				for (pitra = snkPenHead.next; pitra != NULL; pitra = pitra->next)
				{
					if (pitra->one == i || pitra->two == i)
					{
						pitra->oneCost = TSINFINITY;
						pitra->twoCost = TSINFINITY;
						for (pitrb = srcPenHead.next; pitrb != NULL; pitrb = pitrb->next)
							addPenalty(pitra, cij(pitrb->i, pitra->i), pitrb->i);
					}
				}
			}
			else
			{
				//delete sink
				lowVal = dj[j];
				maxPen = snkPens + j;
				maxPen->prev->next = maxPen->next;
				if (maxPen->next != NULL)
					maxPen->next->prev = maxPen->prev;

				for (pitra = srcPenHead.next; pitra != NULL; pitra = pitra->next)
				{
					if (pitra->one == j || pitra->two == j) {
						pitra->oneCost = TSINFINITY;
						pitra->twoCost = TSINFINITY;
						for (pitrb = snkPenHead.next; pitrb != NULL; pitrb = pitrb->next)
							addPenalty(pitra, cij(pitra->i, pitrb->i), pitrb->i);
					}
				}
			}

			bi[i] -= lowVal;
			dj[j] -= lowVal;

			isBasic[i][j] = 1;
			basicsEnd->val = lowVal;
			basicsEnd->i = i;
			basicsEnd->j = j;

			basicsEnd->nextCus = cusBasics[j];
			if (cusBasics[j] != NULL) cusBasics[j]->prevCus = basicsEnd;
			basicsEnd->nextFac = facBasics[i];
			if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

			facBasics[i] = basicsEnd;
			basicsEnd->prevCus = NULL;
			cusBasics[j] = basicsEnd;
			basicsEnd->prevFac = NULL;

			basicsEnd++;
		}

		delete[] srcPens;
		srcPens = NULL;
		delete[] snkPens;
		snkPens = NULL;
	}

	/**********************
	NW-Corner initialization method
	**********************/
	void _initNW(
		double *bi,
		double *dj,
		TsBasic * basicsEnd,
		TsBasic ** facBasics,
		TsBasic ** cusBasics,
		bool ** isBasic,
		int m,
		int n)
	{
		unsigned int i = 0, j = 0;
		double lowVal = 0.0;

		while (i < m && j < n)
		{
			// More capacity than Demand
			if (bi[i] >= dj[j] && dj[j] != 0.0)
			{
				lowVal = dj[j];
				bi[i] -= dj[j];
				dj[j] = 0.0;

				isBasic[i][j] = 1;
				basicsEnd->val = lowVal;
				basicsEnd->i = i;
				basicsEnd->j = j;

				basicsEnd->nextCus = cusBasics[j];
				if (cusBasics[j] != NULL) cusBasics[j]->prevCus = basicsEnd;
				basicsEnd->nextFac = facBasics[i];
				if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

				facBasics[i] = basicsEnd;
				basicsEnd->prevCus = NULL;
				cusBasics[j] = basicsEnd;
				basicsEnd->prevFac = NULL;

				basicsEnd++;

				// Test on Degeneration
				if (bi[i] == 0.0 && dj[j] == 0.0  && i < m - 1 && j < n - 1)
				{
					isBasic[i][j + 1] = 1;
					basicsEnd->val = lowVal;
					basicsEnd->i = i;
					basicsEnd->j = j + 1;

					basicsEnd->nextCus = cusBasics[j + 1];
					if (cusBasics[j + 1] != NULL) cusBasics[j + 1]->prevCus = basicsEnd;
					basicsEnd->nextFac = facBasics[i];
					if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

					facBasics[i] = basicsEnd;
					basicsEnd->prevCus = NULL;
					cusBasics[j + 1] = basicsEnd;
					basicsEnd->prevFac = NULL;

					basicsEnd++;

					i++;
					j++;
				}
				else
				{
					// Skip to next Customer
					j++;
				}
			}
			// Less Capacity than Demand
			else if (bi[i] < dj[j] && bi[i] != 0.0)
			{
				lowVal = bi[i];
				dj[j] -= bi[i];
				bi[i] = 0.0;

				isBasic[i][j] = 1;
				basicsEnd->val = lowVal;
				basicsEnd->i = i;
				basicsEnd->j = j;

				basicsEnd->nextCus = cusBasics[j];
				if (cusBasics[j] != NULL) cusBasics[j]->prevCus = basicsEnd;
				basicsEnd->nextFac = facBasics[i];
				if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

				facBasics[i] = basicsEnd;
				basicsEnd->prevCus = NULL;
				cusBasics[j] = basicsEnd;
				basicsEnd->prevFac = NULL;

				basicsEnd++;

				// Test on Degeneration
				if (bi[i] == 0.0 && dj[j] == 0.0 && i < m - 1 && j < n - 1)
				{
					isBasic[i][j+1] = 1;
					basicsEnd->val = lowVal;
					basicsEnd->i = i;
					basicsEnd->j = j+1;

					basicsEnd->nextCus = cusBasics[j+1];
					if (cusBasics[j+1] != NULL) cusBasics[j+1]->prevCus = basicsEnd;
					basicsEnd->nextFac = facBasics[i];
					if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

					facBasics[i] = basicsEnd;
					basicsEnd->prevCus = NULL;
					cusBasics[j+1] = basicsEnd;
					basicsEnd->prevFac = NULL;

					basicsEnd++;

					i++;
					j++;
				}
				else
				{
					// Skip to next Customer
					i++;
				}

			}
		}
	}

	/**********************
	LCM initialization method
	**********************/
	void _initLCM(
		double *bi,
		double *dj,
		TsBasic *basicsEnd,
		TsBasic **facBasics,
		TsBasic **cusBasics,
		bool ** isBasic,
		int m,
		int n,
		ublas::matrix<unsigned int> &cij_column_sorted,
		int *facilities)
	{
		unsigned int i = 0, j = 0;
		unsigned int indx_i = 0;
		double lowVal = 0.0;

		while (j < cij_column_sorted.size2())
		{
			indx_i = cij_column_sorted(i, j);

			// Can shift Capacity to fullfill Customer Demand
			if (bi[indx_i] != 0.0 && dj[j] != 0.0)
			{
				// More capacity than Demand
				if (bi[i] >= dj[j] && dj[j] != 0.0)
				{
					lowVal = dj[j];
					bi[i] -= dj[j];
					dj[j] = 0.0;

					isBasic[i][j] = 1;
					basicsEnd->val = lowVal;
					basicsEnd->i = i;
					basicsEnd->j = j;

					basicsEnd->nextCus = cusBasics[j];
					if (cusBasics[j] != NULL) cusBasics[j]->prevCus = basicsEnd;
					basicsEnd->nextFac = facBasics[i];
					if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

					facBasics[i] = basicsEnd;
					basicsEnd->prevCus = NULL;
					cusBasics[j] = basicsEnd;
					basicsEnd->prevFac = NULL;

					basicsEnd++;

					// Skip to next Customer
					j++;
					i = 0;
				}
				// Less Capacity than Demand
				else if (bi[i] < dj[j] && bi[i] != 0.0)
				{
					lowVal = bi[i];
					dj[j] -= bi[i];
					bi[i] = 0.0;

					isBasic[i][j] = 1;
					basicsEnd->val = lowVal;
					basicsEnd->i = i;
					basicsEnd->j = j;

					basicsEnd->nextCus = cusBasics[j];
					if (cusBasics[j] != NULL) cusBasics[j]->prevCus = basicsEnd;
					basicsEnd->nextFac = facBasics[i];
					if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

					facBasics[i] = basicsEnd;
					basicsEnd->prevCus = NULL;
					cusBasics[j] = basicsEnd;
					basicsEnd->prevFac = NULL;

					basicsEnd++;

					// Skip to next Customer
					i++;
				}
			}
		}

		// If its in the fictional Column
		if (n > cij_column_sorted.size2() && j == cij_column_sorted.size2())
		{
			for (i = 0; i != cij_column_sorted.size1(); ++i)
			{
				// If Capacity is not empty ship to customer
				if (bi[i] != 0.0)
				{
					lowVal = bi[i];
					bi[i] = 0.0;

					isBasic[i][j] = 1;
					basicsEnd->val = lowVal;
					basicsEnd->i = i;
					basicsEnd->j = j;

					basicsEnd->nextCus = cusBasics[j];
					if (cusBasics[j] != NULL) cusBasics[j]->prevCus = basicsEnd;
					basicsEnd->nextFac = facBasics[i];
					if (facBasics[i] != NULL) facBasics[i]->prevFac = basicsEnd;

					facBasics[i] = basicsEnd;
					basicsEnd->prevCus = NULL;
					cusBasics[j] = basicsEnd;
					basicsEnd->prevFac = NULL;

					basicsEnd++;
				}
			}
		}
	}
}


#ifndef STAN_OPTIMIZATION_BFGS_LINESEARCH_HPP
#define STAN_OPTIMIZATION_BFGS_LINESEARCH_HPP

#include <boost/math/special_functions/fpclassify.hpp>
#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <string>
#include <limits>

namespace stan {
  namespace optimization {
    /**
     * Find the minima in an interval [loX, hiX] of a cubic function which
     * interpolates the points, function values and gradients provided.
     *
     * Implicitly, this function constructs an interpolating polynomial
     *     g(x) = a_3 x^3 + a_2 x^2 + a_1 x + a_0
     * such that g(0) = 0, g(x1) = f1, g'(0) = df0, g'(x1) = df1 where
     *     g'(x) = 3 a_3 x^2 + 2 a_2 x + a_1
     * is the derivative of g(x).  It then computes the roots of g'(x) and
     * finds the minimal value of g(x) on the interval [loX,hiX] including
     * the end points.
     *
     * This function implements the full parameter version of CubicInterp().
     *
     * @param df0 First derivative value, f'(x0)
     * @param x1 Second point
     * @param f1 Second function value, f(x1)
     * @param df1 Second derivative value, f'(x1)
     * @param loX Lower bound on the interval of solutions
     * @param hiX Upper bound on the interval of solutions
     **/
    template<typename Scalar>
    Scalar CubicInterp(const Scalar &df0,
                       const Scalar &x1, const Scalar &f1, const Scalar &df1,
                       const Scalar &loX, const Scalar &hiX) {
      const Scalar c3((-12*f1 + 6*x1*(df0 + df1))/(x1*x1*x1));
      const Scalar c2(-(4*df0 + 2*df1)/x1 + 6*f1/(x1*x1));
      const Scalar &c1(df0);

      const Scalar t_s = std::sqrt(c2*c2 - 2.0*c1*c3);
      const Scalar s1 = - (c2 + t_s)/c3;
      const Scalar s2 = - (c2 - t_s)/c3;

      Scalar tmpF;
      Scalar minF, minX;

      // Check value at lower bound
      minF = loX*(loX*(loX*c3/3.0 + c2)/2.0 + c1);
      minX = loX;

      // Check value at upper bound
      tmpF = hiX*(hiX*(hiX*c3/3.0 + c2)/2.0 + c1);
      if (tmpF < minF) {
        minF = tmpF;
        minX = hiX;
      }

      // Check value of first root
      if (loX < s1 && s1 < hiX) {
        tmpF = s1*(s1*(s1*c3/3.0 + c2)/2.0 + c1);
        if (tmpF < minF) {
          minF = tmpF;
          minX = s1;
        }
      }

      // Check value of second root
      if (loX < s2 && s2 < hiX) {
        tmpF = s2*(s2*(s2*c3/3.0 + c2)/2.0 + c1);
        if (tmpF < minF) {
          minF = tmpF;
          minX = s2;
        }
      }

      return minX;
    }

    /**
     * Find the minima in an interval [loX, hiX] of a cubic function which
     * interpolates the points, function values and gradients provided.
     *
     * Implicitly, this function constructs an interpolating polynomial
     *     g(x) = a_3 x^3 + a_2 x^2 + a_1 x + a_0
     * such that g(x0) = f0, g(x1) = f1, g'(x0) = df0, g'(x1) = df1 where
     *     g'(x) = 3 a_3 x^2 + 2 a_2 x + a_1
     * is the derivative of g(x).  It then computes the roots of g'(x) and
     * finds the minimal value of g(x) on the interval [loX,hiX] including
     * the end points.
     *
     * @param x0 First point
     * @param f0 First function value, f(x0)
     * @param df0 First derivative value, f'(x0)
     * @param x1 Second point
     * @param f1 Second function value, f(x1)
     * @param df1 Second derivative value, f'(x1)
     * @param loX Lower bound on the interval of solutions
     * @param hiX Upper bound on the interval of solutions
     **/
    template<typename Scalar>
    Scalar CubicInterp(const Scalar &x0, const Scalar &f0, const Scalar &df0,
                       const Scalar &x1, const Scalar &f1, const Scalar &df1,
                       const Scalar &loX, const Scalar &hiX) {
      return x0 + CubicInterp(df0, x1-x0, f1-f0, df1, loX-x0, hiX-x0);
    }

    /**
     * An internal utility function for implementing WolfeLineSearch()
     **/
    template<typename FunctorType, typename Scalar, typename XType>
    int WolfLSZoom(Scalar &alpha, XType &newX, Scalar &newF, XType &newDF,
                   FunctorType &func,
                   const XType &x, const Scalar &f, const Scalar &dfp,
                   const Scalar &c1dfp, const Scalar &c2dfp, const XType &p,
                   Scalar alo, Scalar aloF, Scalar aloDFp,
                   Scalar ahi, Scalar ahiF, Scalar ahiDFp,
                   const Scalar &min_range) {
      Scalar d1, d2, newDFp;
      int itNum(0);

      while (1) {
        itNum++;

        if (std::fabs(alo-ahi) < min_range)
          return 1;

        if (itNum%5 == 0) {
          alpha = 0.5*(alo+ahi);
        } else {
          // Perform cubic interpolation to determine next point to try
          d1 = aloDFp + ahiDFp - 3*(aloF-ahiF)/(alo-ahi);
          d2 = std::sqrt(d1*d1 - aloDFp*ahiDFp);
          if (ahi < alo)
            d2 = -d2;
          alpha = ahi
            - (ahi - alo) * (ahiDFp + d2 - d1) / (ahiDFp - aloDFp + 2*d2);
          if (!boost::math::isfinite(alpha) ||
              alpha < std::min(alo, ahi) + 0.01 * std::fabs(alo - ahi) ||
              alpha > std::max(alo, ahi) - 0.01 * std::fabs(alo - ahi))
            alpha = 0.5 * (alo + ahi);
        }

        newX = x + alpha * p;
        while (func(newX, newF, newDF)) {
          alpha = 0.5 * (alpha + std::min(alo, ahi));
          if (std::fabs(std::min(alo, ahi) - alpha) < min_range)
            return 1;
          newX = x + alpha * p;
        }
        newDFp = newDF.dot(p);
        if (newF > (f + alpha * c1dfp) || newF >= aloF) {
          ahi = alpha;
          ahiF = newF;
          ahiDFp = newDFp;
        } else {
          if (std::fabs(newDFp) <= -c2dfp)
            break;
          if (newDFp*(ahi-alo) >= 0) {
            ahi = alo;
            ahiF = aloF;
            ahiDFp = aloDFp;
          }
          alo = alpha;
          aloF = newF;
          aloDFp = newDFp;
        }
      }
      return 0;
    }

    /**
     * Perform a line search which finds an approximate solution to:
     * \f[
     *       \min_\alpha f(x_0 + \alpha p)
     * \f]
     * satisfying the strong Wolfe conditions:
     *  1) \f$ f(x_0 + \alpha p) \leq f(x_0) + c_1 \alpha p^T g(x_0) \f$
     *  2) \f$ \vert p^T g(x_0 + \alpha p) \vert \leq c_2 \vert p^T g(x_0) \vert \f$
     * where \f$g(x) = \frac{\partial f}{\partial x}\f$ is the gradient of f(x).
     *
     * @tparam FunctorType A type which supports being called as
     *        ret = func(x,f,g)
     * where x is the input point, f and g are the function value and
     * gradient at x and ret is non-zero if function evaluation fails.
     *
     * @param func Function which is being minimized.
     *
     * @param alpha First value of \f$ \alpha \f$ to try.  Upon return this
     * contains the final value of the \f$ \alpha \f$.
     *
     * @param x1 Final point, equal to \f$ x_0 + \alpha p \f$.
     *
     * @param f1 Final point function value, equal to \f$ f(x_0 + \alpha p) \f$.
     *
     * @param gradx1 Final point gradient, equal to \f$ g(x_0 + \alpha p) \f$.
     *
     * @param p Search direction.  It is assumed to be a descent direction such
     * that \f$ p^T g(x_0) < 0 \f$.
     *
     * @param x0 Value of starting point, \f$ x_0 \f$.
     *
     * @param f0 Value of function at starting point, \f$ f(x_0) \f$.
     *
     * @param gradx0 Value of function gradient at starting point,
     *    \f$ g(x_0) \f$.
     *
     * @param c1 Parameter of the Wolfe conditions. \f$ 0 < c_1 < c_2 < 1 \f$
     * Typically c1 = 1e-4.
     *
     * @param c2 Parameter of the Wolfe conditions. \f$ 0 < c_1 < c_2 < 1 \f$
     * Typically c2 = 0.9.
     *
     * @param minAlpha Smallest allowable step-size.
     *
     * @param maxLSIts Maximum number line search iterations.
     *
     * @param maxLSRestarts Maximum number of times line search will
     * restart with \f$ f() \f$ failing.
     *
     * @return Returns zero on success, non-zero otherwise.
     **/
    template<typename FunctorType, typename Scalar, typename XType>
    int WolfeLineSearch(FunctorType &func,
                        Scalar &alpha,
                        XType &x1, Scalar &f1, XType &gradx1,
                        const XType &p,
                        const XType &x0, const Scalar &f0, const XType &gradx0,
                        const Scalar &c1, const Scalar &c2,
                        const Scalar &minAlpha, const Scalar &maxLSIts,
                        const Scalar &maxLSRestarts) {
      const Scalar dfp(gradx0.dot(p));
      const Scalar c1dfp(c1*dfp);
      const Scalar c2dfp(c2*dfp);

      Scalar alpha0(minAlpha);
      Scalar alpha1(alpha);

      Scalar prevF(f0);
      XType prevDF(gradx0);
      Scalar prevDFp(dfp);
      Scalar newDFp;

      int retCode = 0, nits = 0, lsRestarts = 0, ret;

      while (1) {
        if (nits >= maxLSIts) {
          retCode = 1;
          break;
        }

        x1.noalias() = x0 + alpha1 * p;
        ret = func(x1, f1, gradx1);
        if (ret != 0) {
          if (lsRestarts >= maxLSRestarts) {
            retCode = 1;
            break;
          }

          alpha1 = 0.5 * (alpha0 + alpha1);
          lsRestarts++;
          continue;
        }
        lsRestarts = 0;

        newDFp = gradx1.dot(p);
        if ((f1 > f0 + alpha * c1dfp) || (f1 >= prevF && nits > 0)) {
          retCode = WolfLSZoom(alpha, x1, f1, gradx1,
                               func,
                               x0, f0, dfp,
                               c1dfp, c2dfp, p,
                               alpha0, prevF, prevDFp,
                               alpha1, f1, newDFp,
                               1e-16);
          break;
        }
        if (std::fabs(newDFp) <= -c2dfp) {
          alpha = alpha1;
          break;
        }
        if (newDFp >= 0) {
          retCode = WolfLSZoom(alpha, x1, f1, gradx1,
                               func,
                               x0, f0, dfp,
                               c1dfp, c2dfp, p,
                               alpha1, f1, newDFp,
                               alpha0, prevF, prevDFp,
                               1e-16);
          break;
        }

        alpha0 = alpha1;
        prevF = f1;
        std::swap(prevDF, gradx1);
        prevDFp = newDFp;

        alpha1 *= 10.0;

        nits++;
      }
      return retCode;
    }
  }
}

#endif


//
// Created by yanhang on 4/21/16.
//

#include "warping.h"
#include "gridenergy.h"
#include "utility.h"
#include <Eigen/Sparse>
#include <Eigen/SPQRSupport>
#include <fstream>

using namespace std;
using namespace Eigen;
using namespace cv;

namespace substab{

	GridWarpping::GridWarpping(const int w, const int h, const int gw, const int gh) : width(w), height(h), gridW(gw), gridH(gh) {
		blockW = (double) width / gridW;
		blockH = (double) height / gridH;
		gridLoc.resize((size_t) (gridW + 1) * (gridH + 1));
		for (auto x = 0; x <= gridW; ++x) {
			for (auto y = 0; y <= gridH; ++y) {
				gridLoc[y * (gridW + 1) + x] = Eigen::Vector2d(blockW * x, blockH * y);
				if (x == gridW)
					gridLoc[y * (gridW + 1) + x][0] -= 1.1;
				if (y == gridH)
					gridLoc[y * (gridW + 1) + x][1] -= 1.1;
			}
		}
	}

	void GridWarpping::getGridIndAndWeight(const Eigen::Vector2d &pt, Eigen::Vector4i &ind,
										   Eigen::Vector4d &w) const {
		CHECK_LE(pt[0], width - 1);
		CHECK_LE(pt[1], height - 1);
		int x = (int) floor(pt[0] / blockW);
		int y = (int) floor(pt[1] / blockH);

		//////////////
		// 1--2
		// |  |
		// 4--3
		/////////////
		ind = Vector4i(y * (gridW + 1) + x, y * (gridW + 1) + x + 1, (y + 1) * (gridW + 1) + x + 1,
					   (y + 1) * (gridW + 1) + x);

		const double &xd = pt[0];
		const double &yd = pt[1];
		const double xl = gridLoc[ind[0]][0];
		const double xh = gridLoc[ind[2]][0];
		const double yl = gridLoc[ind[0]][1];
		const double yh = gridLoc[ind[2]][1];

		w[0] = (xh - xd) * (yh - yd);
		w[1] = (xd - xl) * (yh - yd);
		w[2] = (xd - xl) * (yd - yl);
		w[3] = (xh - xd) * (yd - yl);

		double s = w[0] + w[1] + w[2] + w[3];
		CHECK_GT(s, 0) << pt[0] << ' '<< pt[1];
		w = w / s;

		Vector2d pt2 =
				gridLoc[ind[0]] * w[0] + gridLoc[ind[1]] * w[1] + gridLoc[ind[2]] * w[2] + gridLoc[ind[3]] * w[3];
		double error = (pt2 - pt).norm();
		CHECK_LT(error, 0.0001) << pt[0] << ' ' << pt[1] << ' ' << pt2[0] << ' ' << pt2[1];
	}

	void GridWarpping::visualizeGrid(const std::vector<Eigen::Vector2d>& grid, cv::Mat &img) const {
		CHECK_EQ(grid.size(), gridLoc.size());
		CHECK_EQ(img.cols, width);
		CHECK_EQ(img.rows, height);
		//img = Mat(height, width, CV_8UC3, Scalar(0,0,0));
		for(auto gy=0; gy<gridH; ++gy) {
			for (auto gx = 0; gx < gridW; ++gx){
				const int gid1 = gy * (gridW+1) + gx;
				const int gid2 = (gy+1) * (gridW+1) + gx;
				const int gid3 = (gy+1)*(gridW+1)+gx+1;
				const int gid4= gy * (gridW+1) + gx+1;
				if(grid[gid1][0] > 0 && grid[gid2][0] > 0)
					cv::line(img, cv::Point(grid[gid1][0], grid[gid1][1]), cv::Point(grid[gid2][0], grid[gid2][1]), Scalar(255,255,255));
				if(grid[gid2][0] > 0 && grid[gid3][0] > 0)
					cv::line(img, cv::Point(grid[gid2][0], grid[gid2][1]), cv::Point(grid[gid3][0], grid[gid3][1]), Scalar(255,255,255));
				if(grid[gid3][0] > 0 && grid[gid4][0] > 0)
					cv::line(img, cv::Point(grid[gid3][0], grid[gid3][1]), cv::Point(grid[gid4][0], grid[gid4][1]), Scalar(255,255,255));
				if(grid[gid4][0] > 0 && grid[gid1][0] > 0)
					cv::line(img, cv::Point(grid[gid4][0], grid[gid4][1]), cv::Point(grid[gid1][0], grid[gid1][1]), Scalar(255,255,255));
			}
		}
	}

	void GridWarpping::computeSimilarityWeight(const cv::Mat &input, std::vector<double>& saliency) const {
		saliency.resize((size_t)(gridW*gridH));
		for(auto y=0; y<gridH; ++y){
			for(auto x=0; x<gridW; ++x){
				const int gid = gridInd(x,y);
				vector<vector<double> > pixs(3);
				for(auto x1=(int)gridLoc[gridInd(x,y)][0]; x1<gridLoc[gridInd(x+1,y+1)][0]; ++x1){
					for(auto y1=(int)gridLoc[gridInd(x,y)][1]; y1<gridLoc[gridInd(x+1,y+1)][1]; ++y1){
						Vec3b pix = input.at<Vec3b>(y1,x1);
						pixs[0].push_back((double)pix[0] / 255.0);
						pixs[1].push_back((double)pix[1] / 255.0);
						pixs[2].push_back((double)pix[2] / 255.0);
					}
				}
				Vector3d vars(math_util::variance(pixs[0]),math_util::variance(pixs[1]),math_util::variance(pixs[2]));
				saliency[gid] = vars.norm();
			}
		}
	}

	void GridWarpping::warpImageCloseForm(const cv::Mat &input, cv::Mat &output, const vector<Vector2d>& pts1, const vector<Vector2d>& pts2, const int id) const {
		CHECK_EQ(pts1.size(), pts2.size());

		char buffer[1024] = {};

		vector<Vector2d> resGrid(gridLoc.size());
		const int kDataTerm = (int)pts1.size() * 2;
		const int kSimTerm = (gridW-1)*(gridH-1)*8;
		const int kVar = (int)gridLoc.size() * 2;

		vector<Eigen::Triplet<double> > triplets;
		VectorXd B(kDataTerm+kSimTerm);
		//add data constraint
		const double wdata = 1.0;
		const double wsimilarity = 20;
		int cInd = 0;
		for(auto i=0; i<pts2.size(); ++i) {
			if (pts2[i][0] < 0 || pts2[i][1] < 0 || pts2[i][0] >= width - 1 || pts2[i][1] >= height - 1)
				continue;
			Vector4i indRef;
			Vector4d bwRef;
			CHECK_LT(cInd + 1, B.rows());

			getGridIndAndWeight(pts2[i], indRef, bwRef);
			for (auto j = 0; j < 4; ++j) {
				CHECK_LT(indRef[j]*2+1, kVar);
				triplets.push_back(Triplet<double>(cInd, indRef[j] * 2, wdata * bwRef[j]));
				triplets.push_back(Triplet<double>(cInd + 1, indRef[j] * 2 + 1, wdata * bwRef[j]));
			}
			B[cInd] = wdata * pts1[i][0];
			B[cInd + 1] = wdata * pts1[i][1];
			cInd += 2;
		}

		// Mat inputOutput = input.clone();
		// visualizeGrid(gridLoc, inputOutput);
		// for(const auto& pt: pts1)
		// 	cv::circle(inputOutput, cv::Point2d(pt[0], pt[1]), 1, Scalar(0,0,255), 2);
		// sprintf(buffer, "vis_input%05d.jpg", id);
		// imwrite(buffer, inputOutput);

//		vector<double> saliency;
//		computeSimilarityWeight(input, saliency);

		auto getLocalCoord = [](const Vector2d& p1, const Vector2d& p2, const Vector2d& p3){
			Vector2d axis1 = p3 - p2;
			Vector2d axis2(-1*axis1[1], axis1[0]);
			Vector2d v = p1 - p2;
			return Vector2d(v.dot(axis1)/axis1.squaredNorm(), v.dot(axis2)/axis2.squaredNorm());
		};
		{
			//test for local coord
//			Vector2d p1(0,0), p2(0,-1), p3(1,0);
//			Vector2d uv = getLocalCoord(p1,p2,p3);
//			printf("(%.2f,%.2f)\n", uv[0], uv[1]);
		}
		for(auto y=1; y< gridH; ++y) {
			for (auto x = 1; x < gridW; ++x) {
				vector<Vector2i> gids{
						Vector2i(gridInd(x - 1, y), gridInd(x, y - 1)),
						Vector2i(gridInd(x, y - 1), gridInd(x + 1, y)),
						Vector2i(gridInd(x + 1, y), gridInd(x, y + 1)),
						Vector2i(gridInd(x, y + 1), gridInd(x - 1, y))
				};
				const int cgid = gridInd(x, y);
//				printf("-----------------------\n");
				for (const auto &gid: gids) {
					Vector2d refUV = getLocalCoord(gridLoc[cgid], gridLoc[gid[0]], gridLoc[gid[1]]);
//					printf("(%.2f,%.2f),(%.2f,%.2f),(%.2f,%.2f), u:%.2f,v:%.2f\n", gridLoc[cgid][0], gridLoc[cgid][1],
//						   gridLoc[gid[0]][0], gridLoc[gid[0]][1], gridLoc[gid[1]][0], gridLoc[gid[1]][1], refUV[0],
//						   refUV[1]);
					//x coordinate
					triplets.push_back(Triplet<double>(cInd, cgid * 2, wsimilarity));
					triplets.push_back(Triplet<double>(cInd, gid[0]*2, -1 * wsimilarity));
					triplets.push_back(Triplet<double>(cInd, gid[0] * 2, refUV[0] * wsimilarity));
					triplets.push_back(Triplet<double>(cInd, gid[1] * 2, -1 * refUV[0] * wsimilarity));

					triplets.push_back(Triplet<double>(cInd, gid[0] * 2 + 1, -1 * refUV[1] * wsimilarity));
					triplets.push_back(Triplet<double>(cInd, gid[1] * 2 + 1, refUV[1] * wsimilarity));
					B[cInd] = 0;

					//y coordinate
					triplets.push_back(Triplet<double>(cInd + 1, cgid * 2 + 1, wsimilarity));
					triplets.push_back(Triplet<double>(cInd + 1, gid[0] * 2 + 1, -1 * wsimilarity));
					triplets.push_back(Triplet<double>(cInd + 1, gid[0] * 2 + 1, refUV[0] * wsimilarity));
					triplets.push_back(Triplet<double>(cInd + 1, gid[1] * 2 + 1, -1 * refUV[0] * wsimilarity));
					triplets.push_back(Triplet<double>(cInd + 1, gid[0] * 2, refUV[1] * wsimilarity));
					triplets.push_back(Triplet<double>(cInd + 1, gid[1] * 2, -1 * refUV[1] * wsimilarity));
					B[cInd + 1] = 0;
					cInd += 2;
				}
			}
		}

//		const double wregular = 0.1;
//		for(auto x=0; x<=gridW; ++x){
//			for(auto y=0; y<=gridH; ++y){
//				int gid = gridInd(x,y);
//				triplets.push_back(Triplet<double>(cInd, gid*2, wregular));
//				triplets.push_back(Triplet<double>(cInd+1, gid*2+1, wregular));
//				B[cInd] = wregular * gridLoc[gid][0];
//				B[cInd+1] = wregular * gridLoc[gid][1];
//				cInd +=2;
//			}
//		}
		CHECK_LE(cInd, kDataTerm+kSimTerm);
		SparseMatrix<double> A(cInd, kVar);
		A.setFromTriplets(triplets.begin(), triplets.end());

		Eigen::SPQR<SparseMatrix<double> > solver(A);
		VectorXd res = solver.solve(B.block(0,0,cInd,1));
		CHECK_EQ(res.rows(), kVar);

		vector<Vector2d> vars(gridLoc.size());
		for(auto i=0; i<vars.size(); ++i){
			vars[i][0] = res[2*i];
			vars[i][1] = res[2*i+1];
		}

		output = Mat(height, width, CV_8UC3, Scalar::all(0));
		for(auto y=0; y<height; ++y){
			for(auto x=0; x<width; ++x){
				Vector4i ind;
				Vector4d w;
				getGridIndAndWeight(Vector2d(x,y), ind, w);
				Vector2d pt(0,0);
				for(auto i=0; i<4; ++i){
					pt[0] += vars[ind[i]][0] * w[i];
					pt[1] += vars[ind[i]][1] * w[i];
				}
				if(pt[0] < 0 || pt[1] < 0 || pt[0] > width - 1 || pt[1] > height - 1)
					continue;
				Vector3d pixO = interpolation_util::bilinear<uchar,3>(input.data, input.cols, input.rows, pt);
				output.at<Vec3b>(y,x) = Vec3b((uchar) pixO[0], (uchar)pixO[1], (uchar)pixO[2]);
			}
		}

		// Mat outputOutput = input.clone();
		// visualizeGrid(vars, outputOutput);
		// for(const auto& pt: pts2)
		// 	cv::circle(outputOutput, cv::Point2d(pt[0], pt[1]), 1, Scalar(0,0,255), 2);
		// sprintf(buffer, "vis_output%05d.jpg", id);
		// imwrite(buffer, outputOutput);
	}

}//namespace substablas