/* Copyright (C) 2017 Nikolai Pakhtusov - All Rights Reserved
 * You may use, distribute and modify this code under the
 * terms of the MIT license.
 *
 * You should have received a copy of the MIT license with
 * this file. If not, please write to: chesterlanduk@gmail.com
 */

#include <vector>
// TODO: actually it is not an experimental/optional;
// TODO: In c++17 it is just <optional> but it is not working
#include <experimental/optional>

#include <cv.h>
#include <opencv2/imgcodecs.hpp>

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

#include "help_funcs.cpp"

using std::experimental::optional;
using std::pair;
using std::numeric_limits;

using cv::Mat;
using cv::Point;

using namespace boost::numeric::ublas;

using Real = float;

#define ASSUMED_DEGREE 30

#define MINIMAL_TRIANGLE_AREA 1000

//#define DEBUG_MODE

#define TEST_MODE

enum Side {
    Right,
    Left
};

template<typename T1, typename T2>
optional<T2> operator>>(optional<T1> a, std::function< optional<T2>(T1)> f) {
    if(a) return f(*a);
    return optional<T2>{};
}

std::pair<Real, Real> findEquiationCoeffs(cv::Vec4i points){

    int x1, y1, x2, y2;

    x1 = points[0];
    y1 = points[1];
    x2 = points[2];
    y2 = points[3];

    Real k = ((Real) (y2 - y1)) /
             ((Real) (x2 - x1));
    Real b = - ((Real) (x1 * (y2 - y1)) /
                (Real) (x2 - x1))
             + y1;

    return std::make_pair(k, b);
};

// точки схода
// birds eye view

optional<pair<Real, Real>>
findIntersectionPoint(const std::vector<cv::Vec4i>& lines){

    if (lines.size() < 2){
        return std::experimental::nullopt;
    }

    // TODO: get special lines (instead of two first)
    auto [k1, b1] = findEquiationCoeffs(lines[0]);
    auto [k2, b2] = findEquiationCoeffs(lines[1]);

    // TODO: use special library for it (for example LAPACK)
    //    matrix<Real> A = identity_matrix<Real>(2);
    //    A(0, 0) = k1; A(0, 1) = b1;
    //    A(0, 0) = k1; A(0, 1) = b1;
    Real x = (b2 - b1) / (k1 - k2);
    Real y = k1 * x + b1;

    return std::make_pair(x, y);
}

void filterBadLines(std::vector<cv::Vec4i>& lines){
    auto isBadLine = [](const cv::Vec4i& line) -> bool{
        // We think it is bad when it is more then ASSUMED_DEGREE - 1
        auto [k, b] = findEquiationCoeffs(line);
//        cout << "k: " << k << " b: " << b << endl;
        double tangent = tan(M_PI * (Real)(ASSUMED_DEGREE + 1)/180);
        if (k < 0){
            if (k > -tangent){
                return true;
            }
        }
        if (k > 0){
            if (k > tangent){
                return true;
            }
        }
        return false;
//        int x1 = line[0];
//        int y1 = line[1];
//        int x2 = line[2];
//        int y2 = line[3];
//
//        int catheter1 = abs(x1 - x2);
//        int catheter2 = abs(y1 - y2);
//
//        if (catheter2 == 0){
//            return false;
//        }
////        cout << (1/sqrt(3)) << endl;
////        cout << catheter1/catheter2 << endl;
//        if ((Real)catheter2/(Real)catheter1 < (1/sqrt(3))){
//            cout << (Real)catheter1/(Real)catheter2 << endl;
//            return true;
//        }
//
//        return false;

    };

    std::remove_if(lines.begin(), lines.end(), isBadLine);

    auto isOnOneLine = [](const cv::Vec4i& line1, const cv::Vec4i& line2){
        int Ax = line1[0];
        int Ay = line1[1];
        int Bx = line1[2];
        int By = line1[3];

        int Cx1 = line2[0];
        int Cy1 = line2[1];

        int Cx2 = line2[2];
        int Cy2 = line2[3];
        // Are of triangle is: [ Ax * (By - Cy) + Bx * (Cy - Ay) + Cx * (Ay - By) ] / 2
        float area1 = (Ax * (By - Cy1) + Bx * (Cy1 - Ay) + Cx1 * (Ay - By)) / 2;
        float area2 = (Ax * (By - Cy2) + Bx * (Cy2 - Ay) + Cx2 * (Ay - By)) / 2;

        if (area1 < MINIMAL_TRIANGLE_AREA && area2 < MINIMAL_TRIANGLE_AREA){
            return true;
        }
        return false;
    };

    if (lines.size() < 2){
        throw std::exception();
    }

    auto iter = std::begin(lines) + 1;
    for(; iter != std::end(lines);) {
        if(isOnOneLine(lines[0], *iter)){
            lines.erase(iter);
        } else {
            ++iter;
            break;
        }
    }
}

Mat prepareBeforeLinesFinding(const Mat& image){
    Mat tempImg, destImg;
    cv::cvtColor(image, tempImg, cv::COLOR_BGR2BGRA);
    cv::Canny(tempImg, destImg, 1000, 10, 3);
    return destImg;
}

std::vector<cv::Vec4i> findLines(Mat& image){

    int minLineLength = 200;
    int maxLineGap = 25;

    std::vector<cv::Vec4i> lines;

    cv::HoughLinesP(image, lines, 1, M_PI/180, 100, minLineLength, maxLineGap);

    return lines;
}

Mat generateMatrix(const Mat& source, int beta_){
    int alpha_=90;
    int f_ = 500, dist_ = 500;

    Real f, dist;
    Real alpha, beta, gamma;
    beta = ((Real)beta_ - 90)*M_PI/180;
    f = (Real) f_;
    dist = (Real) dist_;

    cv::Size taille = source.size();
    Real w = (Real)taille.width, h = (Real)taille.height;

    // Projection 2D -> 3D matrix
    Mat A1 = (cv::Mat_<Real>(4,3) <<
            1, 0, -w/2,
            0, 1, -h/2,
            0, 0,    0,
            0, 0,    1);

    // Rotation matrices around the Y axe
    Mat R = (cv::Mat_<Real>(4, 4) <<
            cos(beta), 0, -sin(beta), 0,
            0,         1,          0, 0,
            sin(beta), 0,  cos(beta), 0,
            0,         0,          0, 1);

    // Translation matrix on the Z axis change dist will change the height
    Mat T = (cv::Mat_<Real>(4, 4) <<
            1, 0, 0,    0,
            0, 1, 0,    0,
            0, 0, 1, dist,
            0, 0, 0,    1);       // Camera Intrisecs matrix 3D -> 2D
    // 500 was f; f was from 1 to 2000 (magic-magic amm)
    Mat A2 = (cv::Mat_<Real>(3,4) <<
            500, 0, w/2, 0,
            0, 500, h/2, 0,
            0, 0,   1, 0);

    Mat transfo = A2 * (T * (R * A1));

    return transfo;
}

Mat transformation(const Mat& source, int beta_){

    auto transfo = generateMatrix(source, beta_);

    Mat destination;

    cv::warpPerspective(source, destination, transfo, source.size(), cv::INTER_CUBIC);

    return destination;
}

void fixImage(const Mat& image, std::function<void (Mat&, int)> callback){

    auto specialImg = prepareBeforeLinesFinding(image);
#ifdef DEBUG_MODE
    draw(specialImg);
#endif

    int defaultBeta = 90;
    int beta;

    auto lines = findLines(specialImg);
#ifdef DEBUG_MODE
    auto tempImg = image;
    for(auto&l : lines){
        line( tempImg, Point(l[0], l[1]),
              Point(l[2], l[3]), cv::Scalar(0, 255, 0), 2, 8 );
    }
    draw(tempImg);
#endif
    assert(lines.size() > 1);
    filterBadLines(lines);
#ifdef DEBUG_MODE
    for(auto&l : lines){
        line( tempImg, Point(l[0], l[1]),
              Point(l[2], l[3]), cv::Scalar(0, 255, 255), 2, 8 );
    }
    draw(tempImg);
#endif

    auto findSide = [](cv::Vec4i line, int x) -> Side {
        if(line[0] > x && line[2] > x){
            return Side::Left;
        }
        return Side::Right;
    };

    auto [x, y] = findIntersectionPoint(lines)
            .value_or(std::make_pair(0, 0));

    Side prevSide = findSide(lines[0], x);

    for (int stepUpDown = -1; stepUpDown < 2; stepUpDown += 2) {
        beta = defaultBeta;

        for (unsigned i = 0; i < ASSUMED_DEGREE; ++i) {

            beta += stepUpDown;

            std::vector<cv::Vec4i> transformedLines(lines.size());

            std::transform(std::begin(lines), std::end(lines),
                           std::begin(transformedLines),
                           [=](cv::Vec4i& line) -> cv::Vec4i {
                               std::vector<cv::Point2f> points;
                               points.emplace_back(cv::Point2f(line[0], line[1]));
                               points.emplace_back(cv::Point2f(line[2], line[3]));

                               auto m = generateMatrix(specialImg, beta); // In each iteration ??

                               std::vector<cv::Point2f> newPoints;

                               cv::perspectiveTransform(points, newPoints, m);
                               return cv::Vec4i(
                                       {
                                               int(newPoints[0].x),
                                               int(newPoints[0].y),
                                               int(newPoints[1].x),
                                               int(newPoints[1].y)
                                       }
                               );
                           }
            );
            filterBadLines(transformedLines); // TODO: ????
            auto [x, y] = findIntersectionPoint(transformedLines)
                    .value_or(std::make_pair(0, 0));

#ifdef DEBUG_MODE
            auto destination = transformation(image, beta);
            line( destination, Point(transformedLines[0][0], transformedLines[0][1]),
                  Point(transformedLines[0][2], transformedLines[0][3]), cv::Scalar(0, 255, 0), 2, 8 );
            line( destination, Point(transformedLines[1][0], transformedLines[1][1]),
                  Point(transformedLines[1][2], transformedLines[1][3]), cv::Scalar(0, 255, 0), 2, 8 );
            line( destination, Point(transformedLines[1][0], transformedLines[1][1]),
                  Point(x, y), cv::Scalar(255, 255, 0), 2, 8 );
            line( destination, Point(transformedLines[0][0], transformedLines[0][1]),
                  Point(x, y), cv::Scalar(255, 255, 0), 2, 8 );
            draw(destination);
            cout << "Current: " << x << ", beta: " << beta << endl;
#endif // DEBUG_MODE

            Side currSide = findSide(transformedLines[0], x);

            if (currSide != prevSide){
                goto RESULT;
            }

            prevSide = currSide;
        }
    }

    RESULT:

    auto destination = transformation(image, beta);
    callback(destination, beta);

}

int main(int argc, char** argv) {

#ifdef DEBUG_MODE
//    std::string imgName("../tested_data/image0.JPG");
//    std::string imgName("../data/IMG_0048.JPG");
    std::string imgName("../tested_data/IMG_0047.JPG");
    cout << "DEBUG MODE ON!" << endl;
#else
#ifdef TEST_MODE
//    std::string imgName("../tested_data/imageedit_6_2688227020.jpg");
//    std::string imgName("../data/image0.JPG");
    assert(argc == 3);

    if (strcmp(argv[1], "--data") != 0){
        cout << strcmp(argv[0], "--data") << endl;
        std::abort();
    }

    std::string imgName(argv[2]);
#else
    assert(argc == 3);

    if (strcmp(argv[1], "--data") != 0){
        cout << strcmp(argv[0], "--data") << endl;
        std::abort();
    }

    std::string imgName(argv[2]);
#endif // TEST_MODE
#endif // DEBUG_MODE
#ifdef TEST_MODE
    Mat image = getImage(imgName);

    auto printWrapped = [](Mat& , int beta) -> void {
        std::cout << beta << std::endl;
    };
    fixImage(image, printWrapped);

#else
    cout << imgName << endl;

    Mat image = getImage(imgName);

    auto drawWrapped = [](Mat& image, int i = 0) -> void {
        draw(image);
    };

    draw(image);

    fixImage(image, drawWrapped);

    return EXIT_SUCCESS;
#endif
}


//---------------------------------------------------------------------------//
//!
//! \file   Utility_SloanRadauQuadrature.hpp
//! \author Luke Kersting
//! \brief  Sloan implementation of Gauss-Radau quadrature
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_SLOAN_RADAU_QUADRATURE_HPP
#define UTILITY_SLOAN_RADAU_QUADRATURE_HPP

// Boost Includes
#include <boost/multiprecision/cpp_dec_float.hpp>

// Std Lib Includes
#include <vector>

namespace Utility{

// use extended precision to make sure moments are preserved accurately
typedef boost::multiprecision::cpp_dec_float_50 long_float;

//! The Sloan implementation of Gauss-Radau quadrature
class SloanRadauQuadrature
{

public:

  //! Constructor
  SloanRadauQuadrature(
            const std::vector<long_float>& legendre_expansion_moments );

  //! Destructor
  ~SloanRadauQuadrature()
  { /* ... */ }

  //! Find the nodes for the Radau quadrature
  void getRadauNodesAndWeights( std::vector<long_float>& nodes,
                                std::vector<long_float>& weights,
                                const int number_of_angles_wanted = 1 ) const;

  //! Find the nodes for the Radau quadrature
  void getRadauNodesAndWeights( std::vector<long double>& nodes,
                                std::vector<long double>& weights,
                                const int number_of_angles_wanted = 1 ) const;

  //! Find the nodes for the Radau quadrature
  void getRadauNodesAndWeights( std::vector<double>& nodes,
                                std::vector<double>& weights,
                                const int number_of_angles_wanted = 1 ) const;

//protected:

  // Return the Radau moments of the legendre expansion of a function, f(x)
  void getRadauMoments( std::vector<long_float>& radau_moments ) const;

  // Return the Radau moments of the legendre expansion of a function, f(x)
  void getLongRadauMoments( std::vector<long_float>& radau_moments ) const;

  // Evaluate the normalization ratio for the orthogonal polynomials, Q and x*Q
  void evaluateOrthogonalNormalizationRatio(
        std::vector<long_float>& normalization_ratios,
        const std::vector<std::vector<long_float> >& orthogonal_coefficients,
        const std::vector<long_float>& normalization_factors_N,
        const std::vector<long_float>& radau_moments,
        const int i ) const;

  // Evaluate the ith mean coefficients for orthogonal polynomial recursion relation
  long_float evaluateMeanCoefficient(
                     const std::vector<long_float>& normalization_ratios,
                     const int i ) const;

  // Evaluate the ith row of coefficients of the orthogonal polynomial Q
  void evaluateOrthogonalCoefficients(
        std::vector<std::vector<long_float> >& orthogonal_coefficients,
        const std::vector<long_float>& variances,
        const std::vector<long_float>& mean_coefficients,
        const int i ) const;

  // Evaluate the normalization factors, N_i for the orthogonal polynomial, Q
  void evaluateOrthogonalNormalizationFactor(
        std::vector<long_float>& normalization_factors_N,
        const std::vector<std::vector<long_float> >& orthogonal_coefficients,
        const std::vector<long_float>& radau_moments,
        const int i ) const;

  // Evaluate the variance of the moments of the orthogonal polynomial, Q_i
  long_float evaluateVariance(
        const std::vector<long_float>& normalization_factors_N,
        const int i ) const;

  // Evaluate the nth orthogonal polynomial at x, Q_n(x)
  long_float evaluateOrthogonalPolynomial(
        const std::vector<long_float>& variances,
        const std::vector<long_float>& mean_coefficients,
        const long_float x,
        const int i ) const;

  // Evaluate the roots of the nth orthogonal polynomial using the roots of the (n-1)th
  bool evaluateOrthogonalRoots(
        std::vector<std::vector<long_float> >& roots,
        const std::vector<long_float>& variances,
        const std::vector<long_float>& mean_coefficients,
        const int i ) const;

  // Estimate an extra (i+1)th mean coefficient for the ith orthogonal polynomial
  void estimateExtraMeanCoefficient(
        std::vector<long_float>& mean_coefficients,
        const std::vector<long_float>& variances,
        const std::vector<long_float>& normalization_factors_N,
        const std::vector<long_float>& radau_moments,
        const int i ) const;

private:

  // Shape a two-d array
  static void shapeTwoDArray( std::vector<std::vector<long_float> >& two_d_array,
                              const size_t num_rows,
                              const size_t num_cols,
                              const long_float fill_value = 0.0 );

  // Moments of the Legendre expansion of weighting function f(x)
  std::vector<long_float> d_legendre_expansion_moments;

};

} // end Utility namespace

#endif // end UTILITY_GAUSS_RADAU_QUADRATURE_KERNEL_HPP

//---------------------------------------------------------------------------//
// end Utility_SloanRadauQuadrature.hpp
//---------------------------------------------------------------------------//


/*
 * File:   mJohnsonGE.cpp
 * Author: andrasjoo
 *
 * Created on May 28, 2012, 3:43 PM
 */

#include "mJohnsonGE.h"
#include "mGenome.h"
#include <armadillo/armadillo>
#include "mGCubeAdapter.h"

namespace mmga {

    float mJohnsonGE(GAGenome & gen) {
        int sigma = ((mGenome&) gen).numberOfMultiplications();
        int delta = ((mGenome&) gen).matrixSize();
        float s1, s2;
        float err = 0;

        arma::cube a, b, c;
        mGCubeAdapter()((mGenome&) gen, a, b, c);

        for (int i = 0; i < delta; ++i) {
            for (int j = 0; j < delta; ++j) {
                for (int k = 0; k < delta; ++k) {
                    for (int l = 0; l < delta; ++l) {
                        for (int m = 0; m < delta; ++m) {
                            for (int n = 0; n < delta; ++n) {
                                s1 = (n == i) * (j == k) * (l == m);
                                s2 = 0;

                                for (int r = 0; r < sigma; ++r) {
                                    s2 += a(i, j, r) * b(k, l, r) * c(m, n, r);
                                }

                                err += (s2 - s1) * (s2 - s1);
                            }
                        }
                    }
                }
            }
        }

        return 1.0f / (1.0f + err);
    }

}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "RefNormalizationFloat32Workload.hpp"

#include "RefWorkloadUtils.hpp"
#include "TensorBufferArrayView.hpp"

#include "Profiling.hpp"

#include <armnn/Tensor.hpp>

#include <boost/log/trivial.hpp>
#include <boost/numeric/conversion/cast.hpp>

using namespace armnnUtils;

namespace armnn
{

// Helper function to compute "Within" normalization using Krichevsky 2012: Local Brightness Normalization.
static void NormalizeWithinUingLbr(const float*       inputData,
                                   float*             outputData,
                                   const TensorShape& tensorShape,
                                   uint32_t           norm_size,
                                   float              alpha,
                                   float              beta,
                                   float              kappa)
{
    const unsigned int batchSize = tensorShape[0];
    const unsigned int depth = tensorShape[1];
    const unsigned int rows = tensorShape[2];
    const unsigned int cols = tensorShape[3];

    int radius = boost::numeric_cast<int>(norm_size / 2u); /* Strong Assumption on rounding Mode */

    for (unsigned int n = 0; n < batchSize; n++)
    {
        for (unsigned int c = 0; c < depth; c++)
        {
            for (unsigned int h = 0; h < rows; h++)
            {
                for (unsigned int w = 0; w < cols; w++)
                {
                    float accumulated_scale = 0.0;
                    for (int y = -radius; y <= radius; y++)
                    {
                        for (int x = -radius; x <= radius; x++)
                        {
                            int i = boost::numeric_cast<int>(w) + x;
                            int j = boost::numeric_cast<int>(h) + y;

                            if ((i < 0) || (i >= boost::numeric_cast<int>(cols)))
                            {
                                continue;
                            }

                            if ((j < 0) || (j >= boost::numeric_cast<int>(rows)))
                            {
                                continue;
                            }

                            float inval = inputData[n * cols * rows * depth +
                                                    c * cols * rows +
                                                    boost::numeric_cast<unsigned int>(j) * cols +
                                                    boost::numeric_cast<unsigned int>(i)];

                            accumulated_scale += inval*inval;
                        }
                    }
                    outputData[n * cols * rows * depth +
                               c * cols * rows +
                               h * cols +
                               w] = inputData[n * cols * rows * depth +
                                              c * cols * rows +
                                              h * cols +
                                              w] / (powf((kappa + (accumulated_scale * alpha)), beta));
                }
            }
        }
    }
}

// Helper function to compute "Across" normalization using Krichevsky 2012: Local Brightness Normalization.
void NormalizeAcrossUingLbr(const float*       inputData,
                            float*             outputData,
                            const TensorShape& tensorShape,
                            uint32_t           norm_size,
                            float              alpha,
                            float              beta,
                            float              kappa,
                            DataLayout         dataLayout)
{
    TensorBufferArrayView<const float> input(tensorShape,
                                             inputData,
                                             dataLayout);
    TensorBufferArrayView<float> output(tensorShape,
                                        outputData,
                                        dataLayout);

    DataLayoutIndexed dataLayoutIndexed(dataLayout);

    const unsigned int batchSize = tensorShape[0];
    const unsigned int depth     = tensorShape[dataLayoutIndexed.GetChannelsIndex()];
    const unsigned int rows      = tensorShape[dataLayoutIndexed.GetHeightIndex()];
    const unsigned int cols      = tensorShape[dataLayoutIndexed.GetWidthIndex()];

    int radius = boost::numeric_cast<int>(norm_size / 2u); /* Strong Assumption on rounding Mode */

    for (unsigned int n = 0; n < batchSize; n++)
    {
        for (unsigned int c = 0; c < depth; c++)
        {
            for (unsigned int h = 0; h < rows; h++)
            {
                for (unsigned int w = 0; w < cols; w++)
                {
                    float accumulated_scale = 0.0;
                    for (int z = -radius; z <= radius; z++)
                    {
                        int k = boost::numeric_cast<int>(c) + z;

                        if ((k < 0) || (k >= boost::numeric_cast<int>(depth)))
                        {
                            continue;
                        }

                        float inval = input.Get(n, boost::numeric_cast<unsigned int>(k), h, w);

                        accumulated_scale += inval * inval;
                    }

                    float scale = kappa + (accumulated_scale * alpha);
                    scale = powf(scale, -beta);

                    output.Get(n, c, h, w) = scale * input.Get(n, c, h, w);
                }
            }
        }
    }
}

void RefNormalizationFloat32Workload::Execute() const
{
    ARMNN_SCOPED_PROFILING_EVENT(Compute::CpuRef, "RefNormalizationFloat32Workload_Execute");

    const TensorInfo& inputInfo = GetTensorInfo(m_Data.m_Inputs[0]);

    float*       outputData = GetOutputTensorDataFloat(0, m_Data);
    const float* inputData = GetInputTensorDataFloat(0, m_Data);

    if (NormalizationAlgorithmMethod::LocalBrightness == m_Data.m_Parameters.m_NormMethodType)
    {
        if (NormalizationAlgorithmChannel::Within == m_Data.m_Parameters.m_NormChannelType)
        {
            NormalizeWithinUingLbr(inputData,
                                   outputData,
                                   inputInfo.GetShape(),
                                   m_Data.m_Parameters.m_NormSize,
                                   m_Data.m_Parameters.m_Alpha,
                                   m_Data.m_Parameters.m_Beta,
                                   m_Data.m_Parameters.m_K);
        }
        else if (NormalizationAlgorithmChannel::Across == m_Data.m_Parameters.m_NormChannelType)
        {
            NormalizeAcrossUingLbr(inputData,
                                   outputData,
                                   inputInfo.GetShape(),
                                   m_Data.m_Parameters.m_NormSize,
                                   m_Data.m_Parameters.m_Alpha,
                                   m_Data.m_Parameters.m_Beta,
                                   m_Data.m_Parameters.m_K,
                                   m_Data.m_Parameters.m_DataLayout);
        }
        else
        {
            BOOST_LOG_TRIVIAL(warning) << "Illegal NORMALIZATION mode in normalization_f32";
            return;
        }
    }
    else
    {
        BOOST_LOG_TRIVIAL(warning) << "Lcr method (Jarret 2009: Local Contrast Normalization) not supported yet.";
        return;
    }
}

} //namespace armnn


#include "geometrycentral/quad_cover.h"
#include <Eigen/SparseCholesky>

// ONLY WORKS FOR MESHES WITHOUT BOUNDARY
QuadCover::QuadCover(HalfedgeMesh* m, Geometry<Euclidean>* g) : mesh(m), geom(g), phi(m), r(m), field(m),
                                                                singularities(m), branchCover(m) {
    assert(mesh->nBoundaryLoops() == 0);
}

void QuadCover::setup() {
    VertexData<double> s(mesh);
    // Compute s_i at each vertex
    for (VertexPtr v : mesh->vertices()) {
        if (v.isBoundary()) {
            s[v] = 1.0;
        } else {
            double sum = 0;
            for (HalfedgePtr he : v.outgoingHalfedges()) {
                sum += geom->angle(he.next());
            }
            s[v] = 2*M_PI / sum;
        }
    }

    // Compute transport at edges r_ij <- e^ip_ij
    for (VertexPtr v : mesh->vertices()) {
        HalfedgePtr he = v.halfedge();
        double angle = 0;
        double s_i = s[v];
        do {
            phi[he] = angle;
            angle += s_i * geom->angle(he.next());
            he = he.next().next().twin();
        } while (he != v.halfedge());
    }

    // Compute r_ij
    std::complex<double> i(0, 1);
    for (VertexPtr v : mesh->vertices()) {
        for (HalfedgePtr he : v.outgoingHalfedges()) {
            double theta_ij = phi[he];
            double theta_ji = phi[he.twin()] + M_PI;
            double rho_ij = theta_ij - theta_ji;
            r[he] = std::exp(i * n * rho_ij);
        }
    }
}

Eigen::SparseMatrix<std::complex<double>> QuadCover::assembleM() {
    size_t n = mesh->nVertices();
    Eigen::SparseMatrix<std::complex<double>> M(n,n);
    std::vector<Eigen::Triplet<std::complex<double>>> triplets;

    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (FacePtr f : mesh->faces()) {
        HalfedgePtr he_ij = f.halfedge();
        size_t i = vertexIndices[he_ij.vertex()];
        size_t j = vertexIndices[he_ij.next().vertex()];
        size_t k = vertexIndices[he_ij.prev().vertex()];

        double area = geom->area(f);
        triplets.push_back(Eigen::Triplet<std::complex<double>>(i, i, area/3.));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(j, j, area/3.));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(k, k, area/3.));
    }
    M.setFromTriplets(triplets.begin(),triplets.end());
    return M;
}

Eigen::SparseMatrix<std::complex<double>> QuadCover::assembleA() {
    size_t n = mesh->nVertices();
    Eigen::SparseMatrix<std::complex<double>> A(n,n);
    std::vector<Eigen::Triplet<std::complex<double>>> triplets;

    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (FacePtr f : mesh->faces()) {
        HalfedgePtr he_ij = f.halfedge();
        HalfedgePtr he_jk = f.halfedge().next();
        HalfedgePtr he_ki = f.halfedge().prev();

        size_t i = vertexIndices[he_ij.vertex()];
        size_t j = vertexIndices[he_jk.vertex()];
        size_t k = vertexIndices[he_ki.vertex()];

        double a = geom->cotan(he_jk);
        double b = geom->cotan(he_ki);
        double c = geom->cotan(he_ij);

        std::complex<double> r_ij = r[he_ij];
        std::complex<double> r_ji = r[he_ij.twin()];
        std::complex<double> r_jk = r[he_jk];
        std::complex<double> r_kj = r[he_jk.twin()];
        std::complex<double> r_ki = r[he_ki];
        std::complex<double> r_ik = r[he_ki.twin()];

        // row i
        triplets.push_back(Eigen::Triplet<std::complex<double>>(i,i,b + c));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(i,j,-c * r_ij));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(i,k,-b * r_ik));

        // row j
        triplets.push_back(Eigen::Triplet<std::complex<double>>(j,i,-c * r_ji));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(j,j,c + a));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(j,k,-a * r_jk));

        // row k
        triplets.push_back(Eigen::Triplet<std::complex<double>>(k,i,-b * r_ki));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(k,j,-a * r_kj));
        triplets.push_back(Eigen::Triplet<std::complex<double>>(k,k,a + b));
    }
    A.setFromTriplets(triplets.begin(),triplets.end());
    return A;
}

void QuadCover::computeSmoothestField(Eigen::SparseMatrix<std::complex<double>> M, Eigen::SparseMatrix<std::complex<double>> A) {
    // LL^T <- Cholesky(A)
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<std::complex<double>>> solver;
    solver.compute(A);

    // u <- UniformRand(-1,1)
    Eigen::MatrixXcd u = Eigen::MatrixXcd::Random(mesh->nVertices(),1);
    Eigen::MatrixXcd x;

    // inverse power iteration to find eigenvector belonging to the smallest eigenvalue
    for (int i = 0; i < nPowerIterations; i++) {
        x = solver.solve(M * u);
        std::complex<double> norm2 = (x.transpose() * M * x)(0,0);
        u = x / sqrt(norm2);
    }

    // map resulting vector to VertexData
    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (VertexPtr v : mesh->vertices()) {
        std::complex<double> c = u(vertexIndices[v],0);
        if (std::abs(c) == 0) {
            field[v] = 0;
        } else {
            field[v] = c / std::abs(c);
        }
    }
}

VertexData<std::complex<double>> QuadCover::computeCrossField() {
    std::cout << "Computing Cross Field!" << std::endl;
    // Algorithm 1 : Setup
    setup();

    // Algorithm 2 : Smoothest Field
    Eigen::SparseMatrix<std::complex<double>> M = assembleM();
    Eigen::SparseMatrix<std::complex<double>> A = assembleA();
    A = A + eps * M;
    computeSmoothestField(M,A);

    std::cout << "Done computing smoothest field!" << std::endl;
    return field;
}

FaceData<int> QuadCover::computeSingularities() {
    std::cout << "Computing Singularities!" << std::endl;
    // first, compute Omega_ijk <- arg(r_ij r_jk r_ki)
    FaceData<double> Omega(mesh);
    for (FacePtr f : mesh->faces()) {
        std::complex<double> r_ij = r[f.halfedge()];
        std::complex<double> r_jk = r[f.halfedge().next()];
        std::complex<double> r_ki = r[f.halfedge().prev()];
        Omega[f] = std::arg(r_ij * r_jk * r_ki);
    }

    // next, compute w_ij for each e_ij, such that u_j = e^iw_ij * r_ij * u_i
    // w_ij = arg(u_j / (r_ij * u_i))
    HalfedgeData<double> w(mesh);
    for (HalfedgePtr he : mesh->allHalfedges()) {
        std::complex<double> u_i = field[he.vertex()];
        std::complex<double> u_j = field[he.twin().vertex()];
        std::complex<double> r_ij = r[he];
        w[he] = std::arg(u_j * r_ij / u_i);
    }

    // finally, compute index for each triangle t
    // (1/2pi) * (w_ij + w_jk + w_ki + Omega_ijk)
    for (FacePtr f : mesh->faces()) {
        double w_ij = w[f.halfedge()];
        double w_jk = w[f.halfedge().next()];
        double w_ki = w[f.halfedge().prev()];
        double Omega_ijk = Omega[f];
        double phi = (w_ij + w_jk + w_ki - Omega_ijk) / (2.0 * M_PI);
        singularities[f] = std::round(phi);
    }
    return singularities;
}

void QuadCover::computeBranchCover() {
    std::cout<< "Computing Branch Cover!" << std::endl;
    std::complex<double> i(0, 1);
    for (FacePtr f : mesh->faces()) {
        int total = 0;
        for (HalfedgePtr he_ij : f.adjacentHalfedges()) {
            HalfedgePtr he_ji = he_ij.twin();

            std::complex<double> f_ij = std::pow(field[he_ij.vertex()], 1.0 / n);
            std::complex<double> f_ji = std::pow(field[he_ji.vertex()], 1.0 / n);

            // we need to recompute r_ij here without raising to the nth power,
            // as raising to the nth power and then taking the nth root is not always an identity operation
            double theta_ij = phi[he_ij];
            double theta_ji = phi[he_ij.twin()] + M_PI;
            double rho_ij = theta_ji - theta_ij;
            std::complex<double> r_ij = std::exp(i * rho_ij);
            std::complex<double> s_ij = f_ji / (f_ij * r_ij);
            double ang = std::arg(s_ij);

            if (ang >= -M_PI_4 && ang < M_PI_4) {
                branchCover[he_ij] = 0;
            } else if (ang >= M_PI_4 && ang < 3.0 * M_PI_4) {
                branchCover[he_ij] = 1;
                total = (total + 1) % 4;
            } else if ((ang >= 3.0 * M_PI_4 && ang <= PI) ||
                       (ang < -3.0 * M_PI_4 && ang >= -PI)) {
                branchCover[he_ij] = 2;
                total = (total + 2) % 4;
            } else {
                assert(ang >= -3.0 * M_PI_4 && ang < -M_PI_4);
                branchCover[he_ij] = 3;
                total = (total + 3) % 4;
            }
            /*
            if ( ang >= -M_PI_2 && ang < M_PI_2 ) {
                branchCover[he_ij.edge()] = 0;
            } else {
                branchCover[he_ij.edge()] = 1;
                total = (total + 1) % 2;
            }*/
        }

        if (singularities[f] != 0 && total == 0) {
            std::cout << "difference at singularity: " << total << std::endl;
        } else if (singularities[f] == 0 && total != 0) {
            std::cout << "difference at non-singularity: " << total << std::endl;
        }
    }
}

Eigen::SparseMatrix<std::complex<double>> QuadCover::buildLaplacian() {
    size_t n = mesh->nVertices();
    Eigen::SparseMatrix<std::complex<double>> L(n,n);
    std::vector<Eigen::Triplet<std::complex<double>>> triplets;

    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (VertexPtr v1 : mesh->vertices()) {
        int index1 = vertexIndices[v1];
        double sum = __DBL_EPSILON__;

        // add neighbor weights
        for (HalfedgePtr heOut : v1.outgoingHalfedges()) {
            VertexPtr v2 = heOut.twin().vertex();
            int index2 = vertexIndices[v2];
            double weight = (geom->cotan(heOut) + geom->cotan(heOut.twin())) / 2;
            sum += weight;

            if (branchCover[heOut] == 1) {
                weight *= -1;
            }
            triplets.push_back(Eigen::Triplet<std::complex<double>>(index1, index2, std::complex<double>(-weight,0)));
        }

        // add diagonal weight
        triplets.push_back(Eigen::Triplet<std::complex<double>>(index1, index1, std::complex<double>(sum,0)));
    }
    L.setFromTriplets(triplets.begin(), triplets.end());
    return L;
}

VertexData<double> QuadCover::computeOffset() {
    std::cout << "Computing Offset!" << std::endl;

    Eigen::SparseMatrix<std::complex<double>> L = buildLaplacian();
    Eigen::SparseMatrix<std::complex<double>> M = assembleM();
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<std::complex<double>>> solver;
    solver.compute(L);

    // u <- UniformRand(-1,1)
    Eigen::MatrixXcd u = Eigen::MatrixXcd::Random(mesh->nVertices(),1);
    Eigen::MatrixXcd x;

    // inverse power iteration to find eigenvector belonging to the smallest eigenvalue
    for (int i = 0; i < nPowerIterations; i++) {
        x = solver.solve(M * u);
        std::complex<double> norm2 = (x.transpose() * M * x)(0,0);
        u = x / sqrt(norm2);
    }

    // store into VertexData
    VertexData<double> offset(mesh);
    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (VertexPtr v : mesh->vertices()) {
        size_t index = vertexIndices[v];
        offset[v] = x(index,0).real();
    }

    emitTriangles(offset);
    return offset;
}

void QuadCover::emitTriangles(VertexData<double> offsets) {
    std::cout << "Emitting Triangles!" << std::endl;

    std::ofstream outfile ("branchcover.obj");
    // write vertices
    for (VertexPtr v : mesh->vertices()) {
        Vector3 pos = geom->position(v) + offsets[v] * geom->normal(v);
        Vector3 neg = geom->position(v) - offsets[v] * geom->normal(v);

        outfile << "v " << pos.x << " " << pos.y << " " << pos.z << std::endl;
        outfile << "v " << neg.x << " " << neg.y << " " << neg.z << std::endl;
    }

    // write face indices
    VertexData<size_t> vertexIndices = mesh->getVertexIndices();
    for (FacePtr f : mesh->faces()) {
        HalfedgePtr he = f.halfedge();

       for (int currSheet = 0; currSheet < 2; currSheet++) {
           outfile << "f ";
           do {
               size_t index = vertexIndices[he.vertex()];
               if (currSheet == 0) {
                   outfile << (2*index)+1 << " ";
               } else {
                   outfile << (2*index+1)+1 << " ";
               }
               currSheet = (currSheet + branchCover[he]) % 2;
               he = he.next();
           } while (he != f.halfedge());
           outfile << std::endl;
       }
    }

    outfile.close();
    std::cout << "Done!" << std::endl;
}


#include <iostream>
#include <fstream>
#include <cmath>
#include <complex>
#include <iomanip>
#include <Eigen/Dense>
#include "GaussLegendreEigen.cpp"

/*

single channel non-rel T-matrix solver

g++ -I/usr/local/include/eigen3 ...

*/

using namespace std;
using namespace Eigen;

typedef std::complex<double> Complex;
static const Complex I(0.0,1.0);
double V0,r0,mu;

double deltaExact(double k) {
  // S-wave square well phase shift
  // should be a pos del for attractive potl
  double kp = sqrt(k*k+abs(V0)*2.0*mu);
  return (-k*r0+atan(k*tan(kp*r0)/kp))*180.0/M_PI;
}

Complex SqWell(double k, double p, int ell) {
  // V = V0*theta(r-r0) S-wave only
  double SqReal;
  if (k != p) {
    SqReal = -2.0*M_PI*V0/(k*p)*(sin((k+p)*r0)/(k+p) -
                             sin((k-p)*r0)/(k-p));
  } else {
    SqReal = -2.0*M_PI*V0/(k*k)*(sin(2.0*k*r0)/(2.0*k)-r0);
  }
  return Complex(SqReal,0.0);
}

int main () {
  ofstream ofs;
  ofs.open("T.dat");
  double m1,m2;
  int Ngrid,ell;
  Complex D;

  cout << " enter m1, m2, ell " << endl;
  cin >> m1 >> m2 >> ell;
  cout << " enter V0 (<0 for attractive), r0 " << endl;
  cin >> V0 >> r0;
  cout << " enter Ngrid " << endl;
  cin >> Ngrid;

  mu = m1*m2/(m1+m2);
  VectorXd k(Ngrid+1),w(Ngrid);
  VectorXcd V(Ngrid+1),T(Ngrid+1);
  MatrixXcd M(Ngrid+1,Ngrid+1);

  GaussLegendreEigen(k,w,Ngrid,0.0,M_PI_2);   // create GL grid & weights
  for (int i=0;i<Ngrid;i++) {                                 // map grid
    w(i) = w(i)/pow(cos(k(i)),2);
    k(i) = tan(k(i));
  }
  //  test this
  double s=0.0;
  for (int i=0;i<Ngrid;i++) {
   s += w(i)*k(i)*k(i)*exp(-k(i)*k(i));
  }
  s *= 4/sqrt(M_PI);
  cout << " test integral " << setprecision(20) << s << endl;

  for (int ik=1;ik<50;ik++) {            // loop in scattering momentum
    double k0 = ik*0.04;
    k(Ngrid) = k0;                       // add  k0 to the grid
    for (int a=0;a<=Ngrid;a++) {
       V(a) = SqWell(k(a),k0,ell);
    }
    for (int a=0;a<=Ngrid;a++) {
    for (int b=0;b<=Ngrid;b++) {
      if (b == Ngrid) {
        double Dr = 0.0;
        for (int i=0;i<Ngrid;i++)
          Dr -= w(i)*k0*k0/(k0*k0-k(i)*k(i));
          D = Complex(Dr,-M_PI*k0/2.0);
      } else {
          D = Complex(w(b)*k(b)*k(b)/(k0*k0-k(b)*k(b)),0.0);
      }
      M(a,b) = -mu/(M_PI*M_PI)*SqWell(k(a),k(b),ell)*D;
      if (a == b) M(a,b) = Complex(1.0,0.0) + M(a,b);
    }} // end of a/b loops
    T = M.inverse()*V;                        // invert to obtain  T(k,k0)
    Complex Tonshell = T(Ngrid);
    double delta1 = atan(Tonshell.imag()/Tonshell.real())*180.0/M_PI;
    double delta2 = asin(-mu*k0*Tonshell.real()/M_PI)*90.0/M_PI;
    ofs << k0 << " " << delta1 << " " << delta2 << " " << deltaExact(k0) << endl;
  } // end of k0 loop
  cout << " k, delta1 , delta2, delta_exact in T.dat " << endl;
  ofs.close();
  return 0;
}


/*
 * Copyright (c) 2013-2018 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef DKA_HPP
#define DKA_HPP

#include <limits>
#include <cmath>
#include <boost/numeric/ublas/vector.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/complex.hpp>
#include <kv/constants.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T>
static T inline eval_polynomial (const ub::vector<T>& p, const T& x)
{
	int i;
	T r;
	int s = p.size();

	r = p(s-1);
	for (i=s-2; i>=0; i--) {
		r = r * x + p(i);
	}

	return r;
}

// Durand Kerner Aberth algorithm

template <class T>
bool dka(const ub::vector< kv::complex<T> >& p, ub::vector< kv::complex<T> >& x, T epsilon = std::numeric_limits<T>::epsilon())
{
	int i, j;
	int s = p.size();
	int n = s - 1;
	ub::vector< kv::complex<T> > a(s);
	kv::complex<T> c;
	ub::vector<T> b;
	T r, tmp, norm1, norm2, norm3;
	ub::vector<T> db;
	ub::vector< kv::complex<T> > dx(n);
	T pi = kv::constants<T>::pi();
	kv::complex<T> f, df;

	using std::abs;
	using std::pow;

	if (p(n).real() * p(n).real() + p(n).imag() * p(n).imag() == 0.) {
		return false;
	}

	// make equation b(r) = 0

	for (i=0; i<s; i++) a(i) = p(i);

	c = -a(n-1) / (a(n) * n);
	for (i=1; i<=n; i++) {
		for (j = n; j >= i; j--) {
			a(j-1) += a(j) * c;
		}
	}

	b.resize(s);
	for (i=0; i<n-1; i++) b(i) = -abs(a(i));
	b(n-1) = 0.;
	b(n) = abs(a(n));
	// prepare for the case where b(0)...b(n-2) are 0.
	b(0) = std::min(b(0), -std::numeric_limits<T>::epsilon());

	// initial guess for b(r) = 0
	r = 0.;
	for (i=0; i<n-1; i++) {
		tmp = pow(n * abs(b[i]/b[n]), 1.0/(n-i));
		if (tmp > r) r = tmp;
	}

	// db(r) = b'(r)
	db.resize(n);
	for (i=1; i<s; i++) db(i-1) = b(i) * i;

	// calculate radius by Newton's method for equation b(r) = 0
	while (true) {
		tmp = eval_polynomial(b, r) / eval_polynomial(db, r);
		r -= tmp;
		if (abs(tmp) < n * abs(r) * epsilon) break;
	}

	// set Aberth's initial values

	x.resize(n);
	for (i=0; i<n; i++) {
		x(i) = c + r * exp((2. * i * pi / n + pi / (2. * n)) * kv::complex<T>::i());
	}

	// max(|pi|)
	norm3 = 0.;
	for (i=0; i<s; i++) {
		norm3 = std::max(norm3, abs(p(i)));
	}

	// Durand Kerner algorithm

	while (true) {
		for (i=0; i<n; i++) {
			f = eval_polynomial(p, x(i));
			df = p(n);
			for (j=0; j<n; j++) {
				if (j == i) continue;
				df *= x(i) - x(j);
			}
			if (df.real() * df.real() + df.imag() * df.imag() == 0.) {
				dx(i) = 0.;
			} else {
				dx(i) = f / df;
			}
		}

		x -= dx;

		norm1 = 1.;
		norm2 = 0.;
		for (i=0; i<n; i++) {
			norm1 = std::max(norm1, abs(x(i)));
			norm2 = std::max(norm2, abs(dx(i)));
		}
		if (norm2 <= n * norm1 * epsilon) break;
		if (abs(f) <= n * norm3 * epsilon) break;
	}

	return true;
}

// error estimation using Smith's theorem

template <class T>
ub::vector< kv::complex< interval<T> > > smith_error(const ub::vector< kv::complex< interval<T> > >& p, const ub::vector< kv::complex<T> >& x)
{
	int i, j;
	int s = p.size();
	int n = s - 1;
	ub::vector< kv::complex< interval<T> > > x2, x3(n);
	kv::complex< interval<T> > f, df;
	T err;
	ub::vector<int> unified(n);
	bool flag;

	x2 = x;

	for (i=0; i<n; i++) {
		f = eval_polynomial(p, x2(i));
		df = p(n);
		for (j=0; j<n; j++) {
			if (j == i) continue;
			df *= x2(i) - x2(j);
		}
		if (zero_in(df.real() * df.real() + df.imag() * df.imag())) {
			err = std::numeric_limits<T>::infinity();
		} else {
			err = abs(n * f / df).upper();
		}
		x3(i).real() = x2(i).real() + err * interval<T>(-1., 1.);
		x3(i).imag() = x2(i).imag() + err * interval<T>(-1., 1.);
	}

	/* unify overlapping solutions */

	for (i=0; i<n; i++) unified(i) = -1; /* not unified */

	while (true) {
		flag = true;
		for (i=0; i<n-1; i++) {
			if (unified(i) != -1) continue;
			for (j=i; j<n; j++) {
				if (unified(j) != -1) continue;
				if (overlap(x3(i).real(), x3(j).real()) && overlap(x3(i).imag(), x3(j).imag())) {
					x3(i).real() = interval<T>::hull(x3(i).real(), x3(j).real());
					x3(i).imag() = interval<T>::hull(x3(i).imag(), x3(j).imag());
					unified(j) = i; /* x3(j) is unified to x3(i) */
					flag = false;
				}
			}
		}
		if (flag) break;
	}

	for (i=0; i<n; i++) {
		if (unified(i) != -1) x3(i) = x3(unified(i));
	}

	return x3;
}

// verified Durand Kerner Aberth

template <class T>
bool vdka(const ub::vector< kv::complex< interval<T> > >& p, ub::vector< kv::complex< interval<T> > >& result, T epsilon = std::numeric_limits<T>::epsilon())
{
	int i;
	int s = p.size();
	int n = s - 1;
	ub::vector< kv::complex< T > > p2(s), x2;

	if (zero_in(p(n).real() * p(n).real() + p(n).imag() * p(n).imag())) {
		return false;
	}

	for (i=0; i<s; i++) {
		p2(i).real() = mid(p(i).real());
		p2(i).imag() = mid(p(i).imag());
	}

	dka(p2, x2, epsilon);

	result = smith_error(p, x2);
	return true;
}

} // namespace kv

#endif // DKA_HPP


//
// Created by egrzrbr on 2019-04-26.
//

#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>
/*****************
 * Main function *
 *****************/

#include <unistd.h>
#include <time.h>
#include <iostream>

#define EIGEN_STACK_ALLOCATION_LIMIT 0

#include <Eigen/Dense>

using namespace Eigen;

/**********************************
 * Pseudo-random number generator *
 **********************************/

#define EIGEN_COUT(mat) {std::cout << #mat << " = {\n" << mat << "}" << std::endl;}

static uint64_t mat_rng[2] = {11ULL, 1181783497276652981ULL};

static inline uint64_t xorshift128plus(uint64_t s[2]) {

	uint64_t x, y;
	x = s[0], y = s[1];
	s[0] = y;
	x ^= x << 23;
	s[1] = x ^ y ^ (x >> 17) ^ (y >> 26);
	y += s[1];
	return y;
}

double mat_drand(void) {

	return (xorshift128plus(mat_rng) >> 11) * (1.0 / 9007199254740992.0);
}

template<class Matrix_t>
void mat_gen_random_ublas(Matrix_t &m) {

	ssize_t i, j;
	for (i = 0; i < m.rows(); ++i)
		for (j = 0; j < m.cols(); ++j)
			m(i, j) = mat_drand();
}

template<class Matrix_t>
void mat_arange(Matrix_t &m) {

	const ssize_t N = m.rows();
	const ssize_t M = m.cols();
	ssize_t i, j;
	for (i = 0; i < N; ++i)
		for (j = 0; j < M; ++j)
			m(i, j) = j * N + i;
}

template<typename T, size_t N, size_t TRIALS>
void test_trials(const char opt = 'r') {

	int c;
	clock_t t;

	{

		MatrixXf a(N, N), b(N, N), m(N, N);

		if (opt == 'r') {
			mat_gen_random_ublas(a);
			mat_gen_random_ublas(b);
		} else {
			mat_arange(a);
			mat_arange(b);
		}

		t = clock();
		for (int i = 0; i < TRIALS; i++)
			m = a * b;
		fprintf(stderr, "CPU time: %g\n", (double) (clock() - t) / CLOCKS_PER_SEC);

		if (N > 10) return;
		EIGEN_COUT(a)
		EIGEN_COUT(b)
		EIGEN_COUT(c)
	}

	{

		Matrix<float, N, N> a, b, c;

		if (opt == 'r') {
			mat_gen_random_ublas(a);
			mat_gen_random_ublas(b);
		} else {
			mat_arange(a);
			mat_arange(b);
		}

		t = clock();

		for (int i = 0; i < TRIALS; i++)
			c = a * b;
		fprintf(stderr, "CPU time: %g\n", (double) (clock() - t) / CLOCKS_PER_SEC);
		if (N > 10) return;
		EIGEN_COUT(a)
		EIGEN_COUT(b)
		EIGEN_COUT(c)
	}

}

template<typename T, size_t N>
void matNN_mul_matNN(const char opt = 'r') {

	std::cout << "matNN_mul_matNN" << std::endl;

	clock_t t;
	Matrix<T, N, N> a;
	Matrix<T, N, N> b;
	Matrix<T, N, N> c;

	if (opt == 'r') {
		mat_gen_random_ublas(a);
		mat_gen_random_ublas(b);
	} else {
		mat_arange(a);
		mat_arange(b);
	}

	t = clock();

	c = a * b;

	std::cout << c.size() << std::endl;
	fprintf(stderr, "CPU time: %g\n", (double) (clock() - t) / CLOCKS_PER_SEC);
	if (N > 10) return;
	EIGEN_COUT(a)
	EIGEN_COUT(b)
	EIGEN_COUT(c)
}

template<typename T, size_t N>
void vecN1_mul_vec1N(const char opt = 'r') {

	clock_t t;
	Matrix<T, N, 1> a;
	Matrix<T, 1, N> b;
	Matrix<T, N, N> c;

	if (opt == 'r') {
		mat_gen_random_ublas(a);
		mat_gen_random_ublas(b);
	} else {
		mat_arange(a);
		mat_arange(b);
	}
	t = clock();

	c = a * b;

	std::cout << c.size() << std::endl;
	fprintf(stderr, "CPU time: %g\n", (double) (clock() - t) / CLOCKS_PER_SEC);
	if (N > 10) return;
	EIGEN_COUT(a)
	EIGEN_COUT(b)
	EIGEN_COUT(c)
}

template<typename T, size_t N, size_t M>
void matNM_mul_vecM1(const char opt = 'r') {

	clock_t t;
	Matrix<T, N, M> a;
	Matrix<T, M, 1> b;
	Matrix<T, N, 1> c;

	if (opt == 'r') {
		mat_gen_random_ublas(a);
		mat_gen_random_ublas(b);
	} else {
		mat_arange(a);
		mat_arange(b);
	}

	t = clock();

	c = a * b;

	std::cout << c.size() << std::endl;
	fprintf(stderr, "CPU time: %g\n", (double) (clock() - t) / CLOCKS_PER_SEC);
	if (N > 10) return;
	EIGEN_COUT(a)
	EIGEN_COUT(b)
	EIGEN_COUT(c)
}

template<typename T, size_t N>
void vec1N_mul_vecN1(const char opt = 'r') {

	clock_t t;
	Matrix<T, 1, N> a;
	Matrix<T, N, 1> b;
	Matrix<T, 1, 1> c;

	if (opt == 'r') {
		mat_gen_random_ublas(a);
		mat_gen_random_ublas(b);
	} else {
		mat_arange(a);
		mat_arange(b);
	}

	t = clock();

	c = a * b;

	std::cout << c.size() << std::endl;
	fprintf(stderr, "CPU time: %g\n", (double) (clock() - t) / CLOCKS_PER_SEC);
	if (N > 10) return;
	EIGEN_COUT(a)
	EIGEN_COUT(b)
	EIGEN_COUT(c)
}

int main(int argc, char *argv[]) {

	constexpr int N = 500;
	constexpr int M = 400;
	constexpr int K = 10;
	clock_t t;

	matNN_mul_matNN<float, N>();
	matNM_mul_vecM1<float, N, M>();
	vecN1_mul_vec1N<float, N>();
	vec1N_mul_vecN1<float, N>();

	matNN_mul_matNN<float, 5>(0);
	matNM_mul_vecM1<float, 5, 4>(0);
	vecN1_mul_vec1N<float, 5>(0);
	vec1N_mul_vecN1<float, 5>(0);

	return 0;
}


//          Copyright Gavin Band 2008 - 2012.
// 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)

#ifndef METRO_LOG_SUM_EXP_HPP
#define METRO_LOG_SUM_EXP_HPP

#include <vector>
#include <Eigen/Core>

namespace metro {
	// Compute the log of a sum of exponentials via an algorithm that
	template< typename Matrix, typename Nonmissingness >
	double log_sum_exp( Matrix const& data, Nonmissingness const& nonmissingness ) {
		assert( data.rows() == nonmissingness.rows() ) ;
		assert( data.cols() == nonmissingness.cols() ) ;
		if( data.size() == 0 || nonmissingness.sum() == 0 ) {
			return 0.0 ;
		}
		double max_value = ( data.array() * nonmissingness.array() ).maxCoeff() ;
		if( max_value == -std::numeric_limits< double >::infinity() ) {
			return max_value ;
		}
		Eigen::MatrixXd const exponential = ( data - Eigen::MatrixXd::Constant( data.rows(), data.cols(), max_value ) ).array().exp() ;
		return max_value + std::log( ( exponential.array() * nonmissingness.array() ).sum() ) ;
	}

	// Ditto but assuming all values are present.
	template< typename Matrix >
	double log_sum_exp( Matrix const& data ) {
		if( data.size() == 0 ) {
			return 0.0 ;
		}
		double max_value = data.array().maxCoeff() ;
		if( max_value == -std::numeric_limits< double >::infinity() ) {
			return max_value ;
		}
		Eigen::MatrixXd const exponential = ( data - Eigen::MatrixXd::Constant( data.rows(), data.cols(), max_value ) ).array().exp() ;
		return max_value + std::log( exponential.array().sum() ) ;
	}

	// Ditto but assuming all values are present.
	template<>
	double log_sum_exp( std::vector< double > const& data ) ;

	void rowwise_log_sum_exp( Eigen::MatrixXd const&, Eigen::VectorXd* result ) ;
	void rowwise_log_sum_exp( Eigen::MatrixXd const&, Eigen::MatrixXd const& nonmissingness, Eigen::VectorXd* result ) ;
}

#endif


/***************************************************************************************
    File: perceptron.cpp

    Description:
      Runs an arbitrary-precision rational valued perceptron algorithm.
      This program will infinite loop if the dataset is not linearly separable.
      Reads input from stdin and outputs to stdout.

    Input: a training set
    Example:

    2     // number of training vectors
    2     // number of features per vector
    1     // label 1
    1/2   // vector 1, feature 1
    1.2   // vector 1, feature 2
    0     // label 2
    0     // vector 2, feature 1
    1%3   // vector 2, feature 2

    resulting in the following vectors:
      <1 % 1, 1 % 2, 6 % 5> and <-1 % 1, 0 % 1, 1 % 3>
      // both % & / can be used as fraction.
      // labels are transformed from {0, 1} to {-1, 1} and appended to front of vectors.

    Output:
      This program will either infinite loop or termintate after outputing a weight
      vector representing the hyperplane sperating the data (1 feature per line).

    Example output (for the above example input).
    -1 % 1
    1 % 1
    7 % 5

    which represents the following 2D line.
    y = -7/5x - 1/||<1 % 1, 7 % 5>||
 ***************************************************************************************/
#include <iostream>
#include <string>
#include <vector>
#include <limits>

#include <boost/rational.hpp>
#include <boost/multiprecision/gmp.hpp>

typedef boost::multiprecision::mpz_int Z;
typedef boost::rational<Z> Q;
typedef std::vector<Q> Qvec;

std::istream& operator >> (std::istream& ins, Q& q);
std::ostream& operator << (std::ostream& outs, const Q& q);

// Vector Operations
Q dot (const Qvec& x, const Qvec& y);
Qvec add (const Qvec& x, const Qvec& y);
Qvec mult (const Qvec& x, const Q q);
inline int sign(Q q) { return (0 < q) - (0 > q); } // 1 or -1

Qvec perceptron(const std::vector<Qvec>& features, const std::vector<int>& labels);

int main(int argc, char ** argv){
  std::size_t nvecs, nfeats;
  std::cin >> nvecs >> nfeats;

  std::vector<Qvec> vecs;
  std::vector<int>  lbls;

  for (std::size_t i = 0; i < nvecs; ++i){
    int lbl;
    std::cin >> lbl;
    lbls.push_back(lbl*2-1); //{0,1} => {-1, 1}

    std::cin.ignore(std::numeric_limits<int>::max(),'\n'); //ignore until new line

    Qvec vec(1, Q(1, 1));
    for (std::size_t j = 0; j < nfeats; ++j){
      Q q;
      std::cin >> q;
      vec.push_back(q);
    }
    vecs.push_back(vec);
  }

  Qvec w = perceptron(vecs, lbls);

  if (argc != 1){ // pretty print
    std::cout << "<";
    for (std::size_t i = 0; i < w.size(); ++i){
      std::cout << w[i];
      if (i != w.size() - 1){
        std::cout << ", ";
      }
    }
    std::cout << ">" << std::endl;
  } else {
    for (std::size_t i = 0; i < w.size(); ++i){
      std::cout << w[i] << std::endl;
    }
  }
  return 0;
}

// for now, we assume each number is on it's own line
std::istream& operator >> (std::istream& ins, Q& q){
  Z n, d;
  char c;

  std::string num;

  getline(ins, num);

  std::size_t pos;
  if ((pos = num.find('%')) != std::string::npos ||
      (pos = num.find('/')) != std::string::npos){  // fractional
    n = Z(num.substr(0, pos));
    d = Z(num.substr(pos+1));
  } else if ((pos = num.find('.')) != std::string::npos){ // floating point
    std::string N, D;
    N = num.substr(0, pos);
    D = num.substr(pos+1);
    d = 1;
    for (std::size_t i = 0; i < D.length(); ++i){ // pow doesn't work (it has limited precision)
      d *= 10;
    }
    while (D[0] == '0'){
      D = D.substr(1);
    }
    n = Z(N)*d + Z(D);
  } else { // integral
    n = Z(num);
    d = 1;
  }

  q = Q(n, d);

  return ins;
}

std::ostream& operator << (std::ostream& outs, const Q& q){
  return outs << q.numerator() << " % " << q.denominator();
}

Q dot (const Qvec& x, const Qvec& y){
  Q res(0, 1);
  for (std::size_t i = 0; i < x.size(); ++i){
    res += x[i] * y[i];
  }
  return res;
}

Qvec add (const Qvec& x, const Qvec& y){
  Qvec res(x.size());
  for (std::size_t i = 0; i < x.size(); ++i){
    res[i] = x[i] + y[i];
  }
  return res;
}

Qvec mult (const Qvec& x, const Q q){
  Qvec res(x.size());
  for (std::size_t i = 0; i < x.size(); ++i){
    res[i] = x[i] * q;
  }
  return res;
}

Qvec perceptron(const std::vector<Qvec>& features, const std::vector<int>& labels){
  Qvec weights(features[0].size(), Q(0, 1));

  size_t fuel = 0;
  bool converged;
  do {
    ++fuel;
    converged = true;

    for (std::size_t i = 0; i < features.size(); ++i){
      Q res = dot(weights, features[i]);

      if (sign(res) != labels[i] || res == Q(0,1)) { // wrong sign or res = 0.
        weights = add(weights, mult(features[i], Q(labels[i], 1)));
        converged = false;
      }
    }
  } while (!converged);
  std::cerr << "Fuel: " << fuel << std::endl;
  return weights;
}


#include <bits/stdc++.h>
#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;
using namespace std;
typedef long long ll;
typedef vector <int> vi;

int main(){
    ios_base::sync_with_stdio(false);
    cin.tie(nullptr);
    freopen("in.txt", "r", stdin);
    freopen("out.txt", "w", stdout);
    int tc; cin >> tc;
    cpp_int n, ans;
    while(tc--){
        cin >> n;
        ans = 24 + n * (-18 + n * (23 + n * (-6 + n)));
        ans /= 24;
        cout << ans << "\n";
    }
    return 0;
}

Rare Topics/Rare Algorithms/Formulas or Theorems/How Many Pieces of Land?.cpp


// Copyright (c) 2021 Graphcore Ltd. All rights reserved.

#ifndef poplibs_support_log_arithmetic_hpp
#define poplibs_support_log_arithmetic_hpp

#ifndef INCLUDE_IN_ASSEMBLER
// Conditionally avoid including functions that use multi_array
#ifndef __POPC__
#include <boost/multi_array.hpp>
#endif

namespace poplibs_support {
namespace log {

inline constexpr auto probabilityOne = 0;
inline constexpr auto probabilityZero = -65504; // Min HALF

// Given log values, perform an equivalent `linear add` operation
template <typename FPType> FPType add(const FPType a_, const FPType b_) {
  FPType a = a_ < b_ ? b_ : a_;
  FPType b = a_ < b_ ? a_ : b_;
  return a + std::log(1 + std::exp(b - a));
}

// Given log values, perform an equivalent `linear sub` operation
template <typename FPType> FPType sub(const FPType a_, const FPType b_) {
  FPType a = a_ < b_ ? b_ : a_;
  FPType b = a_ < b_ ? a_ : b_;
  return a + std::log(1 - std::exp(b - a));
}

// Given log values, perform an equivalent `linear mul` operation
template <typename FPType> FPType mul(const FPType a, const FPType b) {
  return a + b;
}
// Given log values, perform an equivalent `linear divide` operation
template <typename FPType> FPType div(const FPType a, const FPType b) {
  return a - b;
}

#ifndef __POPC__

// TODO Move out of log namespace, or do log(softmax(x)) - this is just
// softmax(x)
// Simply to save having to carefully enter values, use a softmax to
// convert them into probabilities
template <typename FPType>
boost::multi_array<FPType, 2> softMax(const boost::multi_array<FPType, 2> &in) {
  boost::multi_array<FPType, 2> out(boost::extents[in.size()][in[0].size()]);
  for (unsigned i = 0; i < in[0].size(); i++) {
    FPType sum = 0;
    for (unsigned j = 0; j < in.size(); j++) {
      sum += std::exp(in[j][i]);
    }
    for (unsigned j = 0; j < in.size(); j++) {
      out[j][i] = std::exp(in[j][i]) / sum;
    }
  }
  return out;
}

// Converted each individual element to natural log.
// Add a small constant to prevent numeric errors
template <typename FPType>
boost::multi_array<FPType, 2> log(const boost::multi_array<FPType, 2> &in) {
  boost::multi_array<FPType, 2> out(boost::extents[in.size()][in[0].size()]);
  for (unsigned i = 0; i < in.size(); i++) {
    for (unsigned j = 0; j < in[i].size(); j++) {
      out[i][j] = std::log(in[i][j] + 1e-50);
    }
  }
  return out;
}
// Find exp of each individual element
template <typename FPType>
boost::multi_array<FPType, 2> exp(const boost::multi_array<FPType, 2> &in) {
  boost::multi_array<FPType, 2> out(boost::extents[in.size()][in[0].size()]);
  for (unsigned i = 0; i < in.size(); i++) {
    for (unsigned j = 0; j < in[i].size(); j++) {
      out[i][j] = std::exp(in[i][j]);
    }
  }
  return out;
}

#endif // ifndef __POPC__
} // namespace log
} // namespace poplibs_support
#endif // ifndef INCLUDE_IN_ASSEMBLER

#define LOG_PROBABILITY_ZERO_FLOAT 0xC77FE000

#endif // poplibs_support_log_arithmetic_hpp


#include <iostream>
#include <Eigen/Dense>
#include <chrono>

#include "mesh_reader_eigen.hpp"
#include "constants_dense.hpp"
#include "newton_raphson_dense.hpp"

using Eigen::MatrixXd;
typedef Matrix<double, Dynamic, 1> VectorXd;

// Compile command:
// g++ -std=c++14 eigen_dense.cpp -O3 -o dense.o -I/usr/local/include/eigen3
// Run command:
// ./dense.o filename T(in C) eta_u(in %) eta_v(in %)

// u = Cu = concentration O2, v = Cv = concentration CO2

class F
{
  /* Returns tuple containing two functors, one for the original expression and one for the Jacobian. */

  private:
    MatrixXd& Au_;
    MatrixXd& Av_;
    MatrixXd& B_;
    MatrixXd& C_;
    VectorXd& D_;

  public:
    F(MatrixXd& Au, MatrixXd& Av,MatrixXd& B,MatrixXd& C,VectorXd& D)
    :Au_(Au), Av_(Av),B_(B), C_(C),D_(D)
    {
    }

    VectorXd operator()(VectorXd& x)
    {
      int n = Au_.rows();
      VectorXd u = x.head(n);
      VectorXd v = x.tail(n);
      VectorXd func(2*n);
      func << Au_*u + B_*Ru(u,v) + hu*(C_*u - D_*uamb),Av_*v - B_*Rv(u,v) + hv*(C_*v - D_*vamb);
      return func;
    }
};

class J
{
  /* Returns tuple containing two functors, one for the original expression and one for the Jacobian. */

  private:
    MatrixXd& Au_;
    MatrixXd& Av_;
    MatrixXd& B_;
    MatrixXd& C_;
    VectorXd& D_;

  public:
    J(MatrixXd& Au, MatrixXd& Av,MatrixXd& B,MatrixXd& C,VectorXd& D)
    :Au_(Au), Av_(Av),B_(B), C_(C),D_(D)
    {
    }

    MatrixXd operator()(VectorXd& x)
    {
      int n = Au_.rows();
      VectorXd u = x.head(n);
      VectorXd v = x.tail(n);
      MatrixXd func(2*n,2*n);
      func << Au_ + B_*dRudu(u,v) + hu*C_,B_*dRudv(u,v),-B_*dRvdu(u,v),Av_ - (B_*dRvdv(u,v)) + hv*C_;
      return func;
    }
};

int main(int argc, char *argv[])
{
  std::cout << std::endl;
  std::cout << "Setting constants ..." << std::endl;
  setConstants(atof(argv[2])+273.15,atof(argv[3])/100,atof(argv[4])/100);

  auto t1 = std::chrono::high_resolution_clock::now();
  std::string file_name = argv[1];
  std::string location = "../triangle/"+ file_name +".1";
  MatrixXd vertices = mesh::read_vertices(location+".node");
  MatrixXd triangles = mesh::read_triangles(vertices,location+".ele");
  MatrixXi boundaries = mesh::read_boundaries(vertices,location+".poly");
  auto t2 = std::chrono::high_resolution_clock::now();

  std::cout << std::endl;
  std::cout << "Input data successfully read:" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;
  std::cout << "Number of vertices :" << vertices.rows() << std::endl;
  std::cout << "Number of triangles:" << triangles.rows() <<std::endl;
  t1 = std::chrono::high_resolution_clock::now();

  /* Calculate coefficient matrix B related to the respiration kinetics,
  part of right hand side in system of nonlinear equations */

  MatrixXd B = MatrixXd::Zero(vertices.rows(), vertices.rows());
  for (unsigned t = 0; t < triangles.rows(); ++t)
  {
    int a = triangles(t, 0);
    int b = triangles(t, 1);
    int c = triangles(t, 2);
    double area = triangles(t, 3);
    B(a, a) += area*(6.*vertices(a, 0) + 2.*vertices(b, 0) + 2.*vertices(c, 0));
    B(b, a) += area*(2.*vertices(a, 0) + 2.*vertices(b, 0) + vertices(c, 0));
    B(a,b) += area*(2.*vertices(a, 0) + 2.*vertices(b, 0) + vertices(c, 0));
    B(c, a) += area*(2.*vertices(a, 0) + vertices(b, 0) + 2.*vertices(c, 0));
    B(a,c) += area*(2.*vertices(a, 0) + vertices(b, 0) + 2.*vertices(c, 0));
    B(b, b) += area*(2.*vertices(a, 0) + 6.*vertices(b, 0) + 2.*vertices(c, 0));
    B(b, c) += area*(vertices(a, 0) + 2.*vertices(b, 0) + 2.*vertices(c, 0));
    B(c,b) += area*(vertices(a, 0) + 2.*vertices(b, 0) + 2.*vertices(c, 0));
    B(c, c) += area*(2.*vertices(a, 0) + 2.*vertices(b, 0) + 6.*vertices(c, 0));
  }
  B *= (1./60.);

  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout << "B matrix successfully assembled" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Calculate the first part of the stiffness matrix in the lefthand
  side of the the nonlinear system, A */

  t1 = std::chrono::high_resolution_clock::now();
  MatrixXd A_U = MatrixXd::Zero(vertices.rows(), vertices.rows());
  MatrixXd A_V = MatrixXd::Zero(vertices.rows(), vertices.rows());
  Eigen::Matrix<double,3,2> G;
  Eigen::Matrix<double,3,3> GGT_U;
  Eigen::Matrix<double,3,3> GGT_V;
  Eigen::Matrix<double,2,2> I_U;
  Eigen::Matrix<double,2,2> I_V;
  I_U(0,0) = DU_R;
  I_U(1,0) = 0;
  I_U(0,1) = 0;
  I_U(1,1) = DU_Z;
  I_V(0,0) = DV_R;
  I_V(1,1) = DV_Z;
  I_V(1,0) = 0;
  I_V(0,1) = 0;
  for (unsigned t = 0; t < triangles.rows(); ++t)
  {
    int a = triangles(t, 0);
    int b = triangles(t, 1);
    int c = triangles(t, 2);
    double area = triangles(t, 3);
    G(0, 0) = (vertices(b, 1) - vertices(c, 1));
    G(1, 0) = (vertices(c, 1) - vertices(a, 1));
    G(2, 0) = (vertices(a, 1) - vertices(b, 1));
    G(0, 1) = (vertices(c, 0) - vertices(b, 0));
    G(1, 1) = (vertices(a, 0) - vertices(c, 0));
    G(2, 1) = (vertices(b, 0) - vertices(a, 0));
    GGT_U = (1/(2*area))*((vertices(a, 0)+vertices(b, 0)+vertices(c, 0))/6)*(G*I_U*G.transpose());
    GGT_V = (1/(2*area))*((vertices(a, 0)+vertices(b, 0)+vertices(c, 0))/6)*(G*I_V*G.transpose());
    #ifdef DEBUG
    std::cout<<"GGT_U" << std::endl << GGT_U << std::endl;
    std::cout<<"GGT_V" << std::endl << GGT_V << std::endl;
    #endif
    A_U(a, a) += GGT_U(0, 0);
    A_U(b, a) += GGT_U(1, 0);
    A_U(a, b) += GGT_U(1, 0);
    A_U(c, a) += GGT_U(2, 0);
    A_U(a, c) += GGT_U(2, 0);
    A_U(b, b) += GGT_U(1, 1);
    A_U(b, c) += GGT_U(1, 2);
    A_U(c, b) += GGT_U(1, 2);
    A_U(c, c) += GGT_U(2, 2);
    A_V(a, a) += GGT_V(0, 0);
    A_V(b, a) += GGT_V(1, 0);
    A_V(a, b) += GGT_V(1, 0);
    A_V(c, a) += GGT_V(2, 0);
    A_V(a, c) += GGT_V(2, 0);
    A_V(b, b) += GGT_V(1, 1);
    A_V(b, c) += GGT_V(1, 2);
    A_V(c, b) += GGT_V(1, 2);
    A_V(c, c) += GGT_V(2, 2);
  }

  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout << "A matrices assembled." << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Calculate the second part of the stiffness matrix (C) and the
  second part of the righthand side (D) */

  MatrixXd C = MatrixXd::Zero(vertices.rows(), vertices.rows());
  VectorXd D = VectorXd::Zero(vertices.rows());
  for (unsigned b = 0; b < boundaries.rows(); ++b)
  {
    double len = sqrt(pow(vertices(boundaries(b, 0), 0) - vertices(boundaries(b, 1), 0), 2) +
      pow(vertices(boundaries(b, 0), 1) - vertices(boundaries(b, 1), 1), 2));
    C(boundaries(b, 0), boundaries(b, 0)) += len*(vertices(boundaries(b, 0), 0)/4 + vertices(boundaries(b, 1), 0)/12);
    C(boundaries(b, 0), boundaries(b, 1)) += len*(vertices(boundaries(b, 0), 0)/12 + vertices(boundaries(b, 1), 0)/12);
    C(boundaries(b, 1), boundaries(b, 0)) += len*(vertices(boundaries(b, 0), 0)/12 + vertices(boundaries(b, 1), 0)/12);
    C(boundaries(b, 1), boundaries(b, 1)) += len*(vertices(boundaries(b, 0), 0)/12 + vertices(boundaries(b, 1), 0)/4);
    D(boundaries(b,0)) += len*(vertices(boundaries(b,0),0)/3.+vertices(boundaries(b,1),0)/6.);
    D(boundaries(b,1)) += len*(vertices(boundaries(b,0),0)/6.+vertices(boundaries(b,1),0)/3.);
  }

  t2 = std::chrono::high_resolution_clock::now();

  std::cout << std::endl;
  std::cout << "C matrix and D vector assembled." << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Solve linearized problem with ambient concentrations as input_field
  to obtain starting concentration values, then solve the nonlinear
  system with Newton-Raphson */

  t1 = std::chrono::high_resolution_clock::now();

  t1 = std::chrono::high_resolution_clock::now();
  F F_funct(A_U, A_V, B, C, D);
  J J_funct(A_U, A_V, B, C, D);
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout << "Functors are created" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;
  t1 = std::chrono::high_resolution_clock::now();

  VectorXd guess(vertices.rows()*2);
  VectorXd u_0 = (A_U+(Vmu/Kmu)*B+hu*C).colPivHouseholderQr().solve(hu*D*uamb);
  VectorXd v_0 = (A_V+hv*C).colPivHouseholderQr().solve(rq*(Vmu/Kmu)*B*u_0+hv*vamb*D);

  guess << u_0,v_0;
  t2 = std::chrono::high_resolution_clock::now();
  std::cout << std::endl;
  std::cout<< "Initial guess calculated" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  #ifdef DEBUG
  std::cout << "A_U =" << std::endl;
  std::cout << A_U << std::endl;
  std::cout << "A_V =" << std::endl;
  std::cout << A_V << std::endl;
  std::cout << "B =" << std::endl;
  std::cout << B << std::endl;
  std::cout << "C =" << std::endl;
  std::cout << C << std::endl;
  std::cout << "D =" << std::endl;
  std::cout << D << std::endl;
  std::cout << "u_0" << std::endl;
  std::cout << u_0 << std::endl;
  std::cout << "v_0" << std::endl;
  std::cout << v_0 <<std::endl;
  std::cout << "Initial function value = " << std::endl;
  std::cout << F_funct(guess) <<std::endl;
  std::cout << "Initial Jacobian = " << std::endl;
  std::cout << J_funct(guess);
  #endif

  std::cout << std::endl;
  std::cout<< "Calculating nonlinear system solution ..." << std::endl;
  t1 = std::chrono::high_resolution_clock::now();
  newton_raphson(F_funct,J_funct,guess,pow(10,-17));
  t2 = std::chrono::high_resolution_clock::now();

  std::cout<< "Numerical solution nonlinear system calculated" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;

  /* Write out the result for python matplotlib code */

  t1 = std::chrono::high_resolution_clock::now();
  int n = vertices.rows();
  VectorXd u = guess.head(n);
  VectorXd v = guess.tail(n);
  mesh::write_result(u,v);
  t2 = std::chrono::high_resolution_clock::now();

  std::cout << std::endl;
  std::cout<< "Results for u and v written out" << std::endl;
  std::cout << "This took: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t2-t1).count()
            << " milliseconds" << std::endl;
  std::cout << std::endl;

  return 0;
}


/**
 * @file advectionfv2d_main.cc
 * @brief NPDE homework AdvectionFV2D code
 * @author Philipp Egg
 * @date 21.06.2020
 * @copyright Developed at ETH Zurich
 */

#include <array>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <functional>
#include <iostream>
#include <memory>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>

#include <lf/assemble/assemble.h>
#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/io/io.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>
#include <lf/mesh/mesh.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/mesh/utils/utils.h>
#include <lf/refinement/refinement.h>

#include "advectionfv2d.h"

int main() {
#if SOLUTION
  // Define velocity field beta
  // Note that the problem description requires ||B|| <= 1
  auto beta = [](Eigen::Vector2d x) -> Eigen::Vector2d {
    return Eigen::Vector2d(-x[1], x[0]) / std::sqrt(2.0);
  };

  // Functor for initial bump
  Eigen::Vector2d x0(0.8, 0.2);
  double d = 0.2;
  auto u0 = [x0, d](Eigen::Vector2d x) -> double {
    double dist = (x - x0).norm();
    if (dist < d) {
      return std::pow(std::cos(M_PI / (2.0 * d) * dist), 2);
    } else {
      return 0.0;
    }
  };

  // Task 8-8.o
  // Generate a mesh hierarchy
  double T = 1.0;

  // TODO inconsistancy: g vs. G
  auto mesh_p = lf::mesh::test_utils::GenerateHybrid2DTestMesh(0, 1. / 3.);

  auto mesh_seq_p{
      lf::refinement::GenerateMeshHierarchyByUniformRefinemnt(mesh_p, 5)};

  std::vector<int> vector_num_cells;
  std::vector<double> vector_l2error;

  // Iterate over mesh levels starting from thrid refinement
  int num_meshes = mesh_seq_p->NumLevels();
  for (int level = 3; level < num_meshes; level++) {
    std::cout << "Computing L2Error for level: " << level << std::endl;

    // Get the current mesh
    auto cur_mesh = mesh_seq_p->getMesh(level);

    // Create a DOF Hander for the current mesh
    const lf::assemble::UniformFEDofHandler cur_dofh(
        cur_mesh, {{lf::base::RefEl::kPoint(), 0},
                   {lf::base::RefEl::kSegment(), 0},
                   {lf::base::RefEl::kTria(), 1},
                   {lf::base::RefEl::kQuad(), 1}});

    // Compute cell normals
    std::shared_ptr<lf::mesh::utils::CodimMeshDataSet<
        Eigen::Matrix<double, 2, Eigen::Dynamic>>>
        normal_vectors = AdvectionFV2D::computeCellNormals(cur_dofh.Mesh());

    // Compute adjecent cells
    std::shared_ptr<lf::mesh::utils::CodimMeshDataSet<
        std::array<const lf::mesh::Entity *, 4>>>
        adjacentCells = AdvectionFV2D::getAdjacentCellPointers(cur_dofh.Mesh());

    // Get result from simulation
    Eigen::VectorXd result = AdvectionFV2D::simulateAdvection(
        cur_dofh, beta, u0, adjacentCells, normal_vectors, T);

    // Get exact result at barycenters of cells
    Eigen::VectorXd ref_solution = AdvectionFV2D::refSolution(cur_dofh, u0, T);

    // Compute L2 error in barycenter
    double l2_error = 0;
    for (const lf::mesh::Entity *cell : cur_mesh->Entities(0)) {
      const lf::geometry::Geometry *geo_p = cell->Geometry();
      double area = lf::geometry::Volume(*geo_p);
      int idx = cur_dofh.GlobalDofIndices(*cell)[0];
      l2_error += std::pow((result[idx] - ref_solution[idx]), 2) * area;
    }
    l2_error = std::sqrt(l2_error);
    vector_num_cells.push_back(cur_dofh.NumDofs());
    vector_l2error.push_back(l2_error);
    std::cout << "L2Error at level " << level << ": " << l2_error << std::endl;

    // Writing vtk files (optional part)
    std::string sol = "sol";
    std::string ref = "ref";
    std::string f_end = ".vtk";
    std::ostringstream sol_st;
    std::ostringstream ref_st;
    std::ostringstream sol_st_file;
    std::ostringstream ref_st_file;
    sol_st << sol << level;
    ref_st << ref << level;
    sol_st_file << sol << level << f_end;
    ref_st_file << ref << level << f_end;

    lf::io::VtkWriter vtk_writer1(cur_dofh.Mesh(), sol_st_file.str());
    auto cell_data_ref =
        lf::mesh::utils::make_CodimMeshDataSet<double>(cur_dofh.Mesh(), 0);
    for (const lf::mesh::Entity *cell : cur_dofh.Mesh()->Entities(0)) {
      int row = cur_dofh.GlobalDofIndices(*cell)[0];
      cell_data_ref->operator()(*cell) = result[row];
    }
    vtk_writer1.WriteCellData(sol_st.str(), *cell_data_ref);

    lf::io::VtkWriter vtk_writer2(cur_dofh.Mesh(), ref_st_file.str());
    auto cell_data_sol =
        lf::mesh::utils::make_CodimMeshDataSet<double>(cur_dofh.Mesh(), 0);
    for (const lf::mesh::Entity *cell : cur_dofh.Mesh()->Entities(0)) {
      int row = cur_dofh.GlobalDofIndices(*cell)[0];
      cell_data_sol->operator()(*cell) = ref_solution[row];
    }
    vtk_writer2.WriteCellData(ref_st.str(), *cell_data_sol);
  }

  // Task 8-8.q
  // Compute threshold for fourth refinement level
  int level = 4;
  auto cur_mesh = mesh_seq_p->getMesh(level);

  // Create a DOF Hander for the current mesh
  const lf::assemble::UniformFEDofHandler cur_dofh(
      cur_mesh, {{lf::base::RefEl::kPoint(), 0},
                 {lf::base::RefEl::kSegment(), 0},
                 {lf::base::RefEl::kTria(), 1},
                 {lf::base::RefEl::kQuad(), 1}});

  int threshold = AdvectionFV2D::findCFLthreshold(cur_dofh, beta, T);
  int cfl_thres = int((T / AdvectionFV2D::computeHmin(cur_mesh) + 1));
  std::cout << "Threshold for level: " << level << " is: " << threshold
            << " | Threshold from CFL is: " << cfl_thres << std::endl;

  // Write Output file of Task 8-8.o and 8-8.q
  std::ofstream csv_file;
  csv_file.open("advectionfv2d.csv");
  for (int i = 0; i < vector_num_cells.size(); i++) {
    std::cout << "Cells: " << vector_num_cells.at(i)
              << " | L2Error: " << vector_l2error.at(i) << std::endl;
    csv_file << vector_num_cells.at(i) << "," << vector_l2error.at(i) << "\n";
  }
  csv_file.close();

  // Print convergence rates
  for (int i = 1; i < vector_num_cells.size(); i++) {
    double conv_rate =
        (std::log(vector_l2error[i - 1]) - std::log(vector_l2error[i])) /
        (std::log(vector_num_cells[i]) - std::log(vector_num_cells[i - 1]));
    std::cout << "Conv. Rate " << i << "-" << i + 1 << "  is: " << conv_rate
              << std::endl;
  }

  // Plot results from 8-8.o
  std::system("python3 " CURRENT_SOURCE_DIR
              "/advectionfv2d.py " CURRENT_BINARY_DIR
              "/advectionfv2d.csv " CURRENT_BINARY_DIR "/solution.eps");

  return 0;
#else
  //====================
  // Your code goes here
  //====================
  return 0;
#endif
}


// Petter Strandmark 2014
// petter.strandmark@gmail.com
//
// [1] Chambolle, A., & Pock, T. (2011). A first-order primal-dual algorithm for convex
//     problems with applications to imaging. Journal of Mathematical Imaging and Vision,
//     40(1), 120-145.
//
// [2] Pock, T., & Chambolle, A. (2011, November). Diagonal preconditioning for first
//     order primal-dual algorithms in convex optimization. In Computer Vision (ICCV),
//     2011 IEEE International Conference on (pp. 1762-1769). IEEE.
//

#include <cstdio>
#include <functional>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <string>

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

#include <first_order_solver.h>

double get_feasibility_error(const Eigen::VectorXd& x,
                             Eigen::VectorXd* temp_storage,
                             const Eigen::VectorXd& lb,
                             const Eigen::VectorXd& ub,
                             const Eigen::SparseMatrix<double>& A,
                             const Eigen::VectorXd& b,
                             const std::vector<LinearConstraintType>& constraint_types)
{
	using namespace std;
	const auto m = A.rows();
	const auto n = A.cols();
	attest(lb.rows() == n);
	attest(ub.rows() == n);
	attest(x.rows() == n);
	attest(b.rows() == m);
	attest(constraint_types.size() == m);

	double feasibility_error = 0;
	*temp_storage = A*x - b;

	for (ptrdiff_t i = 0; i < m; ++i) {
		if (constraint_types[i] == LinearConstraintType::Equality) {
			feasibility_error = max(feasibility_error, abs((*temp_storage)(i)));
		}
		else if (constraint_types[i] == LinearConstraintType::LessThan) {
			feasibility_error = max(feasibility_error, (*temp_storage)(i));
		}
		else if (constraint_types[i] == LinearConstraintType::GreaterThan) {
			feasibility_error = max(feasibility_error, -(*temp_storage)(i));
		}
	}

	for (ptrdiff_t j = 0; j < n; ++j) {
		feasibility_error = max(feasibility_error, lb(j) - x(j));
		feasibility_error = max(feasibility_error, x(j) - ub(j));
	}

	double denominator = max(x.maxCoeff(), -x.minCoeff());
	if (denominator <= 1e-10) {
		denominator = 1;
	}

	return feasibility_error / denominator;
}

// Will use x_prev as a temporary storage after
// examining it.
bool check_convergence_and_log(std::ptrdiff_t iteration,
                               const Eigen::VectorXd& x,
                               Eigen::VectorXd* x_prev,
                               const Eigen::VectorXd& y,
                               const Eigen::VectorXd& y_prev,
                               const Eigen::VectorXd& c,
                               const Eigen::VectorXd& lb,
                               const Eigen::VectorXd& ub,
                               const Eigen::SparseMatrix<double>& A,
                               const Eigen::VectorXd& b,
                               const std::vector<LinearConstraintType>& constraint_types,
                               const FirstOrderOptions& options)
{
	using namespace Eigen;
	using namespace std;

	const auto n = x.size();
	const auto m = y.size();

	bool is_converged = false;

	//cerr << "x      = " << (*x).transpose() << endl;
	//cerr << "Ax     = " << (A*(*x)).transpose() << endl;
	//cerr << "Ax - b = " << (A*(*x) - b).transpose() << endl;

	auto get_relative_change = [iteration](const Eigen::VectorXd& x, const Eigen::VectorXd& x_prev) -> double
	{
		double relative_change;
		if (iteration <= 0) {
			relative_change = std::numeric_limits<double>::quiet_NaN();
		}
		else {
			relative_change = (x - x_prev).norm() / (x.norm() + x_prev.norm());
			if (relative_change != relative_change) {
				// Both x and x_prev were null vectors.
				relative_change = 0;
			}
		}
		return relative_change;
	};

	double relative_change_x = get_relative_change(x, *x_prev);
	double relative_change_y = get_relative_change(y, y_prev);
	double feasibility_error = get_feasibility_error(x, x_prev, lb, ub, A, b, constraint_types);

	if (relative_change_x < options.tolerance && relative_change_y < options.tolerance) {
		is_converged = true;
	}

	if (options.log_function) {
		ostringstream message;
		message << setw(9);
		if (iteration == -1) {
			message << "end";
		}
		else {
			message << iteration;
		}
		message << "   "
				<< setw(15) << setprecision(6) << scientific << c.dot(x) << " "
				<< setw(12) << setprecision(3) << scientific << relative_change_x << " "
				<< setw(12) << setprecision(3) << scientific << relative_change_y << " "
				<< setw(15) << setprecision(6) << scientific << feasibility_error;
		options.log_function(message.str());
	}

	return is_converged;
}

template<typename Int>
std::string to_string_with_separator(Int input)
{
	attest(input >= 0);
	std::string s;
	auto num = std::to_string(input);
	std::reverse(begin(num), end(num));
	for (std::size_t i = 0; i < num.size(); ++i) {
		s += num[i];
		if (i % 3 == 2 && i < num.size() - 1) {
			s += ',';
		}
	}
	std::reverse(begin(s), end(s));
	return s;
}

/// Solves the linear program
///
///   minimize c·x
///   such that Ax = b,
///             l ≤ x ≤ u.
bool first_order_primal_dual_solve(Eigen::VectorXd* x_ptr,    /// Primal variables (in/out).
                                   Eigen::VectorXd* y_ptr,    /// Dual variables (in/out).
                                   const Eigen::VectorXd& c,  /// Objective function.
                                   const Eigen::VectorXd& lb, /// Lower bound on x.
                                   const Eigen::VectorXd& ub, /// Upper bound on x.
                                   const Eigen::SparseMatrix<double>& A,   /// Equality constraint matrix.
                                   const Eigen::VectorXd& b,  /// Right-hand side of constraints.
                                   const std::vector<LinearConstraintType>& constraint_types,
                                   const FirstOrderOptions& options)
{
	using namespace Eigen;
	using namespace std;

	VectorXd& x = *x_ptr;
	VectorXd& y = *y_ptr;

	const auto n = x.size();
	const auto m = y.size();
	attest(c.size() == n);
	attest(A.rows() == m);
	attest(A.cols() == n);
	attest(constraint_types.size() == m);

	VectorXd x_prev(n);
	VectorXd y_prev(m);

	//cerr << "m=" << m << ", n=" << n << endl;
	//cerr << "A = \n" << A << endl;
	//cerr << "b = \n" << b  << endl;
	//cerr << "lb = \n" << lb.transpose() << endl;
	//cerr << "ub = \n" << ub.transpose() << endl;

	const SparseMatrix<double> AT = A.transpose();

	if (options.log_function) {
		options.log_function("Problem size: " + to_string_with_separator(A.rows())
		                              + " x " + to_string_with_separator(A.cols())
		                              + " ("  + to_string_with_separator(A.nonZeros()) + " non-zeros)");
		options.log_function("   Iter         Objective     Rel. ch. x   Rel. ch. y   Infeasibility ");
		options.log_function("----------------------------------------------------------------------");
		x_prev.setConstant(std::numeric_limits<double>::quiet_NaN());
		y_prev.setConstant(std::numeric_limits<double>::quiet_NaN());
		check_convergence_and_log(0, x, &x_prev, y, y_prev, c, lb, ub, A, b, constraint_types, options);
	}

	// Compute preconditioners as in eq. (10) from [2], with alpha = 1.
	VectorXd Tvec(n);
	VectorXd Svec(m);
	Tvec.setZero();
	Svec.setZero();
	for (int k = 0; k < A.outerSize(); ++k) {
		for (SparseMatrix<double>::InnerIterator it(A, k); it; ++it) {
			auto i = it.row();
			auto j = it.col();
			auto value = abs(it.value());
			Tvec(j) += value;
			Svec(i) += value;
		}
	}

	for (ptrdiff_t j = 0; j < n; ++j) {
		Tvec(j) = 1.0 / Tvec(j);
	}

	for (ptrdiff_t i = 0; i < m; ++i) {
		Svec(i) = 1.0 / Svec(i);
	}

	size_t iteration;
	for (iteration = 1; iteration <= options.maximum_iterations; ++iteration) {
		bool should_check_convergence = options.print_interval <= 1 || iteration % options.print_interval == 1;

		x_prev = x;
		if (should_check_convergence) {
			y_prev = y;
		}

		// See eq. (18) from [2].

		x = x - Tvec.asDiagonal() * ( AT*y + c);

		for (ptrdiff_t j = 0; j < n; ++j) {
			x(j) = max(lb(j), min(ub(j), x(j)));
		}

		y = y + Svec.asDiagonal() * (A*(2 * x - x_prev) - b);

		for (size_t i = 0; i < m; ++i) {
			if (constraint_types[i] == LinearConstraintType::LessThan) {
				y(i) = max(y(i), 0.0);
			}
			else if (constraint_types[i] == LinearConstraintType::GreaterThan) {
				y(i) = min(y(i), 0.0);
			}
		}

		if (should_check_convergence) {
			if (check_convergence_and_log(iteration, x, &x_prev, y, y_prev, c, lb, ub, A, b, constraint_types, options)) {
				break;
			}
		}
	}

	check_convergence_and_log(-1, x, &x_prev, y, y_prev, c, lb, ub, A, b, constraint_types, options);

	double feasibility_error = get_feasibility_error(x, &x_prev, lb, ub, A, b, constraint_types);
	return feasibility_error < 100*options.tolerance;
}

bool EASY_IP_API first_order_admm_solve(Eigen::VectorXd* x_ptr,
                                        const Eigen::VectorXd& c,
                                        const Eigen::VectorXd& lb,
                                        const Eigen::VectorXd& ub,
                                        const Eigen::SparseMatrix<double>& A,
                                        const Eigen::VectorXd& b,
                                        const FirstOrderOptions& options)
{
	using namespace Eigen;
	using namespace std;

	VectorXd& x = *x_ptr;

	const auto n = x.size();
	const auto m = A.rows();
	attest(c.size() == n);
	attest(A.cols() == n);

	VectorXd z(n);
	z.setZero();
	VectorXd u(n);
	u.setZero();

	const double rho = options.rho;

	std::vector<LinearConstraintType> constraint_types(m, LinearConstraintType::Equality);

	vector<Triplet<double>> triplets;

	for (auto j = decltype(n)(0); j < n; ++j) {
		triplets.emplace_back(j, j, rho);
	}

	for (int k = 0; k < A.outerSize(); ++k) {
		for (SparseMatrix<double>::InnerIterator it(A, k); it; ++it) {
			auto i = it.row();
			auto j = it.col();
			auto value = it.value();

			triplets.emplace_back(j    , i + n, value);
			triplets.emplace_back(n + i, j    , value);
		}
	}

	typedef SparseMatrix<double>::Index index;
	SparseMatrix<double, ColMajor> System(index(n + m), index(n + m));
	System.setFromTriplets(triplets.begin(), triplets.end());
	System.makeCompressed();

	/*cerr << "System = " << endl << System.toDense() << endl << endl;*/

	SparseLU<SparseMatrix<double, ColMajor>, COLAMDOrdering<SparseMatrix<double, ColMajor>::Index> >  solver;
	solver.analyzePattern(System);
	solver.factorize(System);
	auto computation_info = solver.info();
	if (computation_info != Success) {
		if (computation_info == NumericalIssue) {
			throw runtime_error("Eigen::NumericalIssue");
		}
		else if (computation_info == NoConvergence) {
			throw runtime_error("Eigen::NoConvergence ");
		}
		else if (computation_info == InvalidInput) {
			throw runtime_error("Eigen::InvalidInput ");
		}
		else {
			throw runtime_error("Unknown Eigen error.");
		}
	}

	VectorXd x_prev(n);
	VectorXd z_prev(m);

	VectorXd lhs(m + n);
	VectorXd xv(m + n);

	if (options.log_function) {
		options.log_function("   Iter         Objective     Rel. ch. x   Rel. ch. z   ||Ax - b||_inf");
		options.log_function("----------------------------------------------------------------------");
		x_prev.setConstant(std::numeric_limits<double>::quiet_NaN());
		z_prev.setConstant(std::numeric_limits<double>::quiet_NaN());
		check_convergence_and_log(0, x, &x_prev, z, z_prev, c, lb, ub, A, b, constraint_types, options);
	}

	size_t iteration;
	for (iteration = 1; iteration <= options.maximum_iterations; ++iteration) {
		bool should_check_convergence = options.print_interval <= 1 || iteration % options.print_interval == 1;

		if (should_check_convergence) {
			x_prev = x;
			z_prev = z;
		}

		lhs.block(0, 0, n, 1) = -c + rho*(z - u);
		lhs.block(n, 0, m, 1) = b;
		xv = solver.solve(lhs);
		x = xv.block(0, 0, n, 1);

		z = x + u;
		for (ptrdiff_t i = 0; i < n; ++i) {
			z(i) = max(lb(i), min(ub(i), z(i)));
		}

		u = u + x - z;

		if (should_check_convergence) {
			if (check_convergence_and_log(iteration, x, &x_prev, z, z_prev, c, lb, ub, A, b, constraint_types, options)) {
				// TODO
				break;
			}
		}
	}

	double feasibility_error = get_feasibility_error(x, &x_prev, lb, ub, A, b, constraint_types);
	return feasibility_error < 100 * options.tolerance;
}

void FirstOrderProblem::check_invariants() const
{
	auto n = get_cost().size();
	attest(get_var_lb().size() == n);
	attest(get_var_ub().size() == n);

	attest(get_rows().size() == get_values().size());
	attest(get_cols().size() == get_values().size());

	auto m = get_rhs_lower().size();
	attest(get_rhs_upper().size() == m);

	attest(get_integer_variables().empty());
}

void FirstOrderProblem::convert_into_equality_constrained_problem()
{
	auto m = get_rhs_lower().size();

	size_t constraints_added = 0;
	for (size_t i = 0; i < m; ++i) {
		auto& lb = get_rhs_lower()[i];
		auto& ub = get_rhs_upper()[i];
		if (lb != ub) {
			auto slack = add_variable(IP::Real);
			set_bounds(0.0, slack, std::numeric_limits<double>::infinity());
			get_rows().push_back(int(i));
			get_cols().push_back(int(get_variable_index(slack)));

			if (ub < 1e100) {
				attest(lb <= -1e100); // Cannot convert if both lb and ub exists.
				lb = ub;
				get_values().push_back(1.0);
			}
			else {
				attest(lb > -1e100);
				ub = lb;
				get_values().push_back(-1.0);
			}
			constraints_added++;
		}
	}

	check_invariants();
}

void FirstOrderProblem::get_system_matrix(Eigen::SparseMatrix<double>* A,
                                          const FirstOrderOptions& options)
{
	using namespace Eigen;
	using namespace std;

	auto n = get_cost().size();
	auto m = get_rhs_lower().size();
	check_invariants();

	typedef unsigned int index;
	vector<Triplet<double, index>> sparse_indices;
	for (size_t ind = 0; ind < get_rows().size(); ++ind) {
		size_t i = get_rows()[ind];
		size_t j = get_cols()[ind];
		auto value = get_values()[ind];
		attest(i < m);
		attest(j < n);
		sparse_indices.emplace_back(index(i), index(j), value);
	}

	A->resize(static_cast<int>(m), static_cast<int>(n));
	A->setFromTriplets(sparse_indices.begin(), sparse_indices.end());

	check_invariants();
}

bool FirstOrderProblem::solve_first_order(const FirstOrderOptions& options)
{
	using namespace Eigen;
	using namespace std;

	SparseMatrix<double> A;
	get_system_matrix(&A, options);

	auto n = get_cost().size();
	auto m = get_rhs_lower().size();

	Map<const VectorXd> lb(get_var_lb().data(), n);
	Map<const VectorXd> ub(get_var_ub().data(), n);
	VectorXd            b(m);
	Map<const VectorXd> c(get_cost().data(), n);
	VectorXd x(n); x.setZero();
	VectorXd y(m); y.setZero();

	std::vector<LinearConstraintType> constraint_types;
	for (size_t i = 0; i < m; ++i) {
		auto& lb = get_rhs_lower()[i];
		auto& ub = get_rhs_upper()[i];
		if (lb == ub) {
			b[i] = lb;
			constraint_types.push_back(LinearConstraintType::Equality);
		}
		else if (ub < 1e100) {
			attest(lb <= -1e100); // Cannot convert if both lb and ub exists.
			b[i] = ub;
			constraint_types.push_back(LinearConstraintType::LessThan);
		}
		else {
			attest(lb > -1e100);
			b[i] = lb;
			constraint_types.push_back(LinearConstraintType::GreaterThan);
		}
	}

	bool feasible = first_order_primal_dual_solve(&x, &y, c, lb, ub, A, b, constraint_types, options);

	get_solution().resize(n);
	for (size_t j = 0; j < n; ++j) {
		get_solution()[j] = x(j);
	}

	return feasible;
}

bool FirstOrderProblem::solve_admm(const FirstOrderOptions& options)
{
	using namespace Eigen;
	using namespace std;

	auto n = get_cost().size();
	auto m = get_rhs_lower().size();

	// First, convert all inequality constraints to
	// equality constraints.
	convert_into_equality_constrained_problem();
	size_t constraints_added = get_cost().size() - n;
	n = get_cost().size();
	if (options.log_function && constraints_added > 0) {
		ostringstream sout;
		sout << constraints_added << " inequality constraints converted.";
		options.log_function(sout.str());
	}

	SparseMatrix<double> A;
	get_system_matrix(&A, options);

	Map<const VectorXd> lb(get_var_lb().data(), n);
	Map<const VectorXd> ub(get_var_ub().data(), n);
	Map<const VectorXd> b(get_rhs_upper().data(), m);
	Map<const VectorXd> c(get_cost().data(), n);
	VectorXd x(n); x.setZero();

	bool feasible = first_order_admm_solve(&x, c, lb, ub, A, b, options);

	get_solution().resize(n);
	for (size_t j = 0; j < n; ++j) {
		get_solution()[j] = x(j);
	}

	return feasible;
}


#include <hpx/hpx_init.hpp>
#include <hpx/runtime/threads/topology.hpp>
#include <boost/format.hpp>

#include <sys/time.h>

#include "matrix_block.h"

using hpx::lcos::shared_future;
using hpx::lcos::future;
using std::vector;
using std::cout;
using std::endl;
using std::chrono::high_resolution_clock;
using time_point = std::chrono::system_clock::time_point;

int blocksize;

void print(block A) {
    for(int i = 0; i < A.height; i++) {
        for(int j = 0; j < A.width; j++) {
            cout << A[i][j] << " ";
        }
        cout << endl;
    }
    cout << endl;
}

block rec_mult(block A, block B, block C);

block serial_mult(block A, block B, block C) {
    for (int i = 0; i < C.height; i++) {
        for (int j = 0; j < C.width; j++) {
            for (int k = 0; k < A.width;k++) {
                C[i][j] += A[i][k] * B[k][j];
            }
        }
    }
    return C;
}

block add_blocks(block A, block B, block result) {
    for(int i = 0; i < A.height; i++){
        for(int j = 0; j < A.width; j++) {
            result[i][j] = A[i][j] + B[i][j];
        }
    }
    return result;
}

block calc_c11(block A, block B, block C) {
    block tempC = C.block11();//scratch space
    tempC.add_scratch();
    block A11B11 = rec_mult(A.block11(), B.block11(), C.block11());
    block A12B21 = rec_mult(A.block12(), B.block21(), tempC);
    return add_blocks(A11B11, A12B21, C.block11());
}

block calc_c12(block A, block B, block C) {
    block tempC = C.block12();
    tempC.add_scratch();
    block A11B12 = rec_mult(A.block11(), B.block12(), C.block12());
    block A12B22 = rec_mult(A.block12(), B.block22(), tempC);
    return add_blocks(A11B12, A12B22, C.block12());
}

block calc_c21(block A, block B, block C) {
    block tempC = C.block21();
    tempC.add_scratch();
    block A21B11 = rec_mult(A.block21(), B.block11(), C.block21());
    block A22B21 = rec_mult(A.block22(), B.block21(), tempC);
    return add_blocks(A21B11, A22B21, C.block21());
}

block calc_c22(block A, block B, block C) {
    block tempC = C.block22();
    tempC.add_scratch();
    block A21B12 = rec_mult(A.block21(), B.block12(), C.block22());
    block A22B22 = rec_mult(A.block22(), B.block22(), tempC);
    return add_blocks(A21B12, A22B22, C.block22());
}

block rec_mult(block A, block B, block C) {
    if(C.width <= blocksize || C.height <= blocksize ) {
        return serial_mult(A, B, C);
    }
    block C11 = calc_c11(A, B, C);
    block C12 = calc_c12(A, B, C);
    block C21 = calc_c21(A, B, C);
    block C22 = calc_c22(A, B, C);

    return C;
}

int hpx_main(int argc, char **argv) {
    blocksize = 100;
    int niter = 1, N = 1000;
    time_point time1, time2;
    srand(1);
    if(argc > 1)
        N = atoi(argv[1]);
    if(argc > 2)
        blocksize = atoi(argv[2]);
    if(argc > 3)
        niter = atoi(argv[3]);
     cout << "Recursive matrix multiplication" << endl;
     cout << "size " << N << endl;
     cout << "block size " << blocksize << endl;
     cout << "Number of iterations " << niter << endl;

    block a(N);
    block b(N);
    block c(new double[N*N], N);

    time1 = high_resolution_clock::now();
    rec_mult(a, b, c);
    time2 = high_resolution_clock::now();

     auto time = std::chrono::duration_cast<std::chrono::microseconds>(time2 - time1).count();
     cout << "time "<< time << " microseconds" << endl;
    return hpx::finalize();
}

int main(int argc, char ** argv) {

    hpx::init(argc, argv);

    return 0;
}