/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file boost/numeric/ublasx/operation/diag.hpp
 *
 * \brief The \c diag operation.
 *
 * The \c diag operation takes inspiration from the \e diag MATLAB's function
 * and the \e DiagonalMatrix Mathematica's function.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2009, Marco Guazzone
 *
 * 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 BOOST_NUMERIC_UBLAS_DIAG_HPP
#define BOOST_NUMERIC_UBLAS_DIAG_HPP

#include <boost/numeric/ublasx/container/generalized_diagonal_matrix.hpp>
#include <boost/numeric/ublas/fwd.hpp>
#include <boost/numeric/ublasx/proxy/matrix_diagonal.hpp>
#include <boost/numeric/ublas/traits.hpp>

namespace boost { namespace numeric { namespace ublasx {

using namespace ::boost::numeric::ublas;

/**
 * \brief A traits class for vector-to-diagonal-matrix transformation.
 * \tparam VectorT A model of VectorExpression.
 * \tparam Layout A matrix layout type.
 */
template <typename VectorT, typename LayoutT>
struct vector_matrix_diag_traits
{
    typedef typename vector_traits<VectorT>::value_type value_type;
    typedef typename vector_traits<VectorT>::difference_type difference_type;
    typedef typename vector_traits<VectorT>::size_type size_type;
    typedef LayoutT layout_type;
    typedef typename VectorT::array_type array_type; //FIXME: not in vector_traits
    typedef generalized_diagonal_matrix<value_type, layout_type, array_type> result_type;
};

/**
 * \brief A traits class for vector-to-diagonal-matrix transformation.
 * \tparam VectorExprT A model of VectorExpression.
 * \tparam Layout A matrix layout type.
 */
template <typename VectorExprT, typename LayoutT>
struct vector_matrix_diag_traits<vector_expression<VectorExprT>, LayoutT>
{
    typedef typename vector_traits<VectorExprT>::value_type value_type;
    typedef typename vector_traits<VectorExprT>::difference_type difference_type;
    typedef typename vector_traits<VectorExprT>::size_type size_type;
    typedef LayoutT layout_type;
    typedef typename VectorExprT::array_type array_type; //FIXME: not in vector_traits
    typedef generalized_diagonal_matrix<value_type, layout_type, array_type> result_type;
};

/**
 * \brief Create a square matrix of order \f$n+abs(k)\f$, with the elements of
 *  \a v on the \a k-th diagonal.
 * \tparam VectorT A model of VectorExpression.
 * \tparam LayoutT The layout type of the resulting matrix (e.g., row_major).
 * \param v A vector expression.
 * \param k The offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \param l The matrix layout.
 * \return A square diagonal matrix with the elements of \a v on the \a k-th
 *  diagonal.
 */
template <typename VectorT, typename LayoutT>
BOOST_UBLAS_INLINE
typename vector_matrix_diag_traits<VectorT,LayoutT>::result_type diag(vector_expression<VectorT>& v, typename vector_matrix_diag_traits<VectorT,LayoutT>::difference_type k=0, LayoutT /*l*/=LayoutT())
{
    typedef vector_matrix_diag_traits<VectorT,LayoutT> traits;
    typedef typename traits::size_type size_type;
    typedef typename traits::array_type array_type;
    typedef typename traits::result_type result_type;

    size_type d(k > 0 ? k : -k);

    return result_type(
            v().size() + d,
            k,
            array_type(v().data())
    );
}

/**
 * \brief Create a square matrix of order \f$n+abs(k)\f$, with the elements of
 *  \a v on the \a k-th diagonal.
 * \tparam VectorT A model of VectorExpression.
 * \tparam LayoutT The layout type of the resulting matrix (e.g., row_major).
 * \param v A vector expression.
 * \param k The offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \param l The matrix layout.
 * \return A square diagonal matrix with the elements of \a v on the \a k-th
 *  diagonal.
 *
 * Variant for const reference to vectors.
 */
template <typename VectorT, typename LayoutT>
BOOST_UBLAS_INLINE
typename vector_matrix_diag_traits<VectorT const,LayoutT>::result_type diag(vector_expression<VectorT> const& v, typename vector_matrix_diag_traits<VectorT,LayoutT>::difference_type k=0, LayoutT /*l*/=LayoutT())
{
    typedef vector_matrix_diag_traits<VectorT const,LayoutT> traits;
    typedef typename traits::size_type size_type;
    typedef typename traits::array_type array_type;
    typedef typename traits::result_type result_type;

    size_type d(k > 0 ? k : -k);

    return result_type(
            v().size() + d,
            k,
            array_type(v().data())
    );
}

/**
 * \brief create a square matrix of order \f$n+abs(k)\f$, with the elements of
 *  \a v on the \a k-th diagonal.
 * \tparam vectort a model of vectorexpression.
 * \param v a vector expression.
 * \param k the offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \return a square diagonal matrix with the elements of \a v on the \a k-th
 *  diagonal and with a row-major layout.
 */
template <typename VectorT>
BOOST_UBLAS_INLINE
typename vector_matrix_diag_traits<VectorT,row_major>::result_type diag(vector_expression<VectorT>& v, typename vector_matrix_diag_traits<VectorT,row_major>::difference_type k=0)
{
    typedef vector_matrix_diag_traits<VectorT,row_major> traits;
    typedef typename traits::size_type size_type;
    typedef typename traits::array_type array_type;
    typedef typename traits::result_type result_type;

    size_type d(k > 0 ? k : -k);

    return result_type(
            v().size() + d,
            k,
            array_type(v().data())
    );
}

/**
 * \brief create a square matrix of order \f$n+abs(k)\f$, with the elements of
 *  \a v on the \a k-th diagonal.
 * \tparam vectort a model of vectorexpression.
 * \param v a vector expression.
 * \param k the offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \return a square diagonal matrix with the elements of \a v on the \a k-th
 *  diagonal and with a row-major layout.
 *
 * Variant for const reference to vectors.
 */
template <typename VectorT>
BOOST_UBLAS_INLINE
typename vector_matrix_diag_traits<VectorT const,row_major>::result_type diag(vector_expression<VectorT> const& v, typename vector_matrix_diag_traits<VectorT,row_major>::difference_type k=0)
{
    typedef vector_matrix_diag_traits<VectorT const,row_major> traits;
    typedef typename traits::size_type size_type;
    typedef typename traits::array_type array_type;
    typedef typename traits::result_type result_type;

    size_type d(k > 0 ? k : -k);

    return result_type(
            v().size() + d,
            k,
            array_type(v().data())
    );
}

/**
 * \brief Create a \a size1 by \a size2 matrix with the elements of
 *  \a v on the \a k-th diagonal.
 * \tparam VectorT A model of VectorExpression.
 * \tparam LayoutT The layout type of the resulting matrix (e.g., row_major).
 * \param v A vector expression. If too long it will be truncated.
 * \param size1 The number of rows of the resulting matrix.
 * \param size2 The number of columns of the resulting matrix.
 * \param k The offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \param l The matrix layout.
 * \return A rectangular diagonal matrix with the elements of \a v on the
 *  \a k-th diagonal.
 */
template <typename VectorT, typename LayoutT>
BOOST_UBLAS_INLINE
typename vector_matrix_diag_traits<VectorT,LayoutT>::result_type diag(vector_expression<VectorT>& v, typename vector_matrix_diag_traits<VectorT,LayoutT>::size_type size1, typename vector_matrix_diag_traits<VectorT,LayoutT>::size_type size2, typename vector_matrix_diag_traits<VectorT,LayoutT>::difference_type k=0, LayoutT /*l*/=LayoutT())
{
    typedef vector_matrix_diag_traits<VectorT,row_major> traits;
    typedef typename traits::array_type array_type;
    typedef typename traits::result_type result_type;

    return result_type(
            size1,
            size2,
            k,
            array_type(v().data())
    );
}

/**
 * \brief Create a \a size1 by \a size2 matrix with the elements of
 *  \a v on the \a k-th diagonal.
 * \tparam VectorT A model of VectorExpression.
 * \tparam LayoutT The layout type of the resulting matrix (e.g., row_major).
 * \param v A vector expression. If too long it will be truncated.
 * \param size1 The number of rows of the resulting matrix.
 * \param size2 The number of columns of the resulting matrix.
 * \param k The offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \param l The matrix layout.
 * \return A rectangular diagonal matrix with the elements of \a v on the
 *  \a k-th diagonal.
 *
 * Variant for const reference to vectors.
 */
template <typename VectorT, typename LayoutT>
BOOST_UBLAS_INLINE
typename vector_matrix_diag_traits<VectorT const,LayoutT>::result_type diag(vector_expression<VectorT> const& v, typename vector_matrix_diag_traits<VectorT,LayoutT>::size_type size1, typename vector_matrix_diag_traits<VectorT,LayoutT>::size_type size2, typename vector_matrix_diag_traits<VectorT,LayoutT>::difference_type k=0, LayoutT /*l*/=LayoutT())
{
    typedef vector_matrix_diag_traits<VectorT const,row_major> traits;
    typedef typename traits::array_type array_type;
    typedef typename traits::result_type result_type;

    return result_type(
            size1,
            size2,
            k,
            array_type(v().data())
    );
}

/**
 * \brief Create a \a size1 by \a size2 matrix with the elements of
 *  \a v on the \a k-th diagonal.
 * \tparam VectorT A model of VectorExpression.
 * \param v A vector expression. If too long it will be truncated.
 * \param size1 The number of rows of the resulting matrix.
 * \param size2 The number of columns of the resulting matrix.
 * \param k The offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \return A rectangular diagonal matrix with the elements of \a v on the
 *  \a k-th diagonal and with a row-major layout.
 */
template <typename VectorT>
BOOST_UBLAS_INLINE
typename vector_matrix_diag_traits<VectorT,row_major>::result_type diag(vector_expression<VectorT>& v, typename vector_matrix_diag_traits<VectorT,row_major>::size_type size1, typename vector_matrix_diag_traits<VectorT,row_major>::size_type size2, typename vector_matrix_diag_traits<VectorT,row_major>::difference_type k=0)
{
    typedef vector_matrix_diag_traits<VectorT,row_major> traits;
    typedef typename traits::array_type array_type;
    typedef typename traits::result_type result_type;

    return result_type(
            size1,
            size2,
            k,
            array_type(v().data())
    );
}

/**
 * \brief Create a \a size1 by \a size2 matrix with the elements of
 *  \a v on the \a k-th diagonal.
 * \tparam VectorT A model of VectorExpression.
 * \param v A vector expression. If too long it will be truncated.
 * \param size1 The number of rows of the resulting matrix.
 * \param size2 The number of columns of the resulting matrix.
 * \param k The offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \return A rectangular diagonal matrix with the elements of \a v on the
 *  \a k-th diagonal and with a row-major layout.
 *
 * Variant for const reference to vectors.
 */
template <typename VectorT>
BOOST_UBLAS_INLINE
typename vector_matrix_diag_traits<VectorT const,row_major>::result_type diag(vector_expression<VectorT> const& v, typename vector_matrix_diag_traits<VectorT,row_major>::size_type size1, typename vector_matrix_diag_traits<VectorT,row_major>::size_type size2, typename vector_matrix_diag_traits<VectorT,row_major>::difference_type k=0)
{
    typedef vector_matrix_diag_traits<VectorT const,row_major> traits;
    typedef typename traits::array_type array_type;
    typedef typename traits::result_type result_type;

    return result_type(
            size1,
            size2,
            k,
            array_type(v().data())
    );
}

//[FIXME]: The two functions below are commented for problems on overloading:
// the ambiguity is caused by difference_type, size_type and LayoutT.
///**
// * \brief Create a square matrix of order \f$n+abs(k)\f$, with the elements of
// *  \a v on the \a k-th diagonal.
// * \tparam VectorT A model of VectorExpression.
// * \tparam LayoutT The layout type of the resulting matrix (e.g., row_major).
// * \param v A vector expression. If longer than \a size it will be truncated.
// * \param size The size of the resulting matrix.
// * \param k The offset from the main diagonal:
// *  - \a k = 0 represents the main diagonal,
// *  - \a k > 0 is the offset above the main diagonal,
// *  - \a k < 0 is the offset below the main diagonal.
// * \param l The matrix layout.
// *  .
// * \return A square diagonal matrix with the elements of \a v on the \a k-th
// *  diagonal.
// */
//template <typename VectorT, typename LayoutT>
//BOOST_UBLAS_INLINE
//typename vector_matrix_diag_traits<VectorT,LayoutT>::result_type diag(vector_expression<VectorT>& v, typename vector_matrix_diag_traits<VectorT,LayoutT>::size_type size, typename vector_matrix_diag_traits<VectorT,LayoutT>::difference_type k, LayoutT /*l*//*=LayoutT()*/)
//{
//  typedef vector_matrix_diag_traits<VectorT,LayoutT> traits;
//  typedef typename traits::size_type size_type;
//  typedef typename traits::array_type array_type;
//  typedef typename traits::result_type result_type;
//
//  return result_type(
//          size,
//          k,
//          array_type(v().data())
//  );
//}

///**
// * \brief Create a square matrix of order \f$n+abs(k)\f$, with the elements of
// *  \a v on the \a k-th diagonal.
// * \tparam VectorT A model of VectorExpression.
// * \param v A vector expression. If longer than \a size it will be truncated.
// * \param size The size of the resulting matrix.
// * \param k The offset from the main diagonal:
// *  - \a k = 0 represents the main diagonal,
// *  - \a k > 0 is the offset above the main diagonal,
// *  - \a k < 0 is the offset below the main diagonal.
// *  .
// * \return A square diagonal matrix with the elements of \a v on the \a k-th
// *  diagonal and with a row-major layout.
// */
//template <typename VectorT>
//BOOST_UBLAS_INLINE
//typename vector_matrix_diag_traits<VectorT,row_major>::result_type diag(vector_expression<VectorT>& v, typename vector_matrix_diag_traits<VectorT,row_major>::size_type size, typename vector_matrix_diag_traits<VectorT,row_major>::difference_type k)
//{
//  typedef vector_matrix_diag_traits<VectorT,row_major> traits;
//  typedef typename traits::size_type size_type;
//  typedef typename traits::array_type array_type;
//  typedef typename traits::result_type result_type;
//
//  return result_type(
//          size,
//          k,
//          array_type(v().data())
//  );
//}
//[/FIXME]

/**
 * \brief Create a view of the \a k-th diagonal of a matrix.
 * \tparam MatrixT A model of MatrixExpression.
 * \param me A matrix expression from which taking the \a k-th diagonal.
 * \param k The offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \return A view of the \a k-th diagonal of matrix \a me.
 */
template<typename MatrixT>
BOOST_UBLAS_INLINE
matrix_diagonal<MatrixT> diag(matrix_expression<MatrixT>& me, typename MatrixT::difference_type k=0)
{
    return matrix_diagonal<MatrixT>(me(), k);
}

/**
 * \brief Create an unmutable view of the \a k-th diagonal of a matrix.
 * \tparam MatrixT A model of MatrixExpression.
 * \param me A matrix expression from which taking the \a k-th diagonal.
 * \param k The offset from the main diagonal:
 *  - \a k = 0 represents the main diagonal,
 *  - \a k > 0 is the offset above the main diagonal,
 *  - \a k < 0 is the offset below the main diagonal.
 *  .
 *  Default to zero.
 * \return An unmutable view of the \a k-th diagonal of matrix \a me.
 */
template<typename MatrixT>
BOOST_UBLAS_INLINE
matrix_diagonal<MatrixT const> const diag(matrix_expression<MatrixT> const& me, typename MatrixT::difference_type k=0)
{
    return matrix_diagonal<MatrixT const>(me(), k);
}

}}} // Namespace boost::numeric::ublasx

#endif // BOOST_NUMERIC_UBLASX_OPERATION_DIAG_HPP


#include <iostream>
#include <cmath>
#include <vector>
#include <math.h>
#include <fstream>
#include "GeneralizedHeat.hpp"
#include "APDE.hpp"
#include "PDE_Q2.hpp"
#include "originalPDE.hpp"
#include "EllipticPDE_Q1.hpp"
#include "EllipticPDE2.hpp"
#include "Nonanalytic_1.hpp"
#include <string>
#include <boost/math/quadrature/gauss.hpp>
using namespace std;
using namespace boost::math::quadrature;
//const double M_PI = 2*acos(0);

void printTime(SpaceMesh a_smesh, TimeMesh a_tmesh);

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

SpaceMesh smesh;
smesh.GenerateDefaultSpaceMesh();
smesh.GloballyBisectSpaceMesh();
smesh.GloballyBisectSpaceMesh();
//smesh.GloballyBisectSpaceMesh();
//smesh.GloballyBisectSpaceMesh();

TimeMesh tmesh;
tmesh.GenerateUniformTimeMesh(pow(smesh.meshsize(), 1), 1.0);

PDE_Q2 anotherpde;
originalPDE firstpde;
EllipticPDE_Q1 Q1;
EllipticPDE2 elliptic2;
Nonanalytic NA_1;

GeneralHeat genheat;
smesh.PrintSpaceNodes();
genheat.SetSpaceTimeMesh(smesh, tmesh, NA_1);
//genheat.StationaryHeatEquation();
genheat.SolveWithBCs();
printTime(smesh, tmesh);
genheat.PrintSolution();

//genheat.GlobalSpaceError();

//for (int i=0; i<6; i++)
//{
//    genheat.SetSpaceTimeMesh(smesh, tmesh, elliptic2);
//    genheat.StationaryHeatEquation();
//    std::cout<< smesh.meshsize() + 1 <<"\n";
//    genheat.UnitTest1();
//    smesh.GloballyBisectSpaceMesh();
//    tmesh.GenerateUniformTimeMesh(pow(smesh.meshsize()+1, 2), 1.0);
//}

}

void printTime(SpaceMesh a_smesh, TimeMesh a_tmesh)
{
    ofstream myfile2;
    myfile2.open ("Y.csv");
    for(int j=0;j<a_tmesh.NumberOfTimeSteps()+1;j++ )
    {
    for(int i = 0; i<a_smesh.meshsize()+1; i++)
    {
        myfile2 << a_tmesh.ReadTimeStep(j) << ", ";
    }
    myfile2 << "\n";
    }
    myfile2.close();
}

Solver class generalised for all boundary conditions/driver.cpp


//
// Created by mmath on 5/28/17.
//

#ifndef PYSCAN_POINT_HPP
#define PYSCAN_POINT_HPP
#include <string>
#include <cstring>
#include <cmath>
#include <vector>
#include <iostream>
#include <array>
#include <cassert>
#include <sstream>
#include <functional>
#include <unordered_map>
#include <queue>
#include <optional>

#include "Utilities.hpp"

//#include <boost/math/special_functions/next.hpp>

namespace pyscan {

    template<typename tuple_t>
    constexpr auto get_array_from_tuple(tuple_t&& tuple) {
        constexpr auto get_array = [](auto&& ... x){
            return std::array{std::forward<decltype(x)>(x) ... , 1.0};
        };
        return std::apply(get_array, std::forward<tuple_t>(tuple));
    }

    // Coordiates in homgenous space.
    template<int dim = 2>
    class Point {
    public:

        template <class ...Coords, std::enable_if_t<(sizeof...(Coords) == dim + 1)>* = nullptr>
        explicit Point(Coords... rest) : coords{rest...} {
            static_assert(sizeof...(rest) == dim + 1, "Wrong number of arguments for the point type.");
        }

        template <class ...Coords, std::enable_if_t<(sizeof...(Coords) == dim)>* = nullptr>
        explicit Point(std::tuple<Coords...> c) : coords(get_array_from_tuple(c)) {}

       /* Point<dim>& operator=(Point<dim> const& pt) {
            std::copy(coords.begin(), coords.end(), pt.coords.begin());
            return *this;
        }*/

        Point() {
            coords.fill(0.0);
        }
        virtual ~Point() = default;

        friend std::ostream &operator<<(std::ostream &os, Point const &pt) {
            os << "pyscan::Point<" << dim << ">(";
            for (size_t d = 0; d < dim; d++ ) {
                os << pt.coords[d] << ", ";
            }
            os << pt.coords[dim];
            os << ")";
            return os;
        }

        friend Point<dim> operator*(Point<dim> pt, double val) {
            Point<dim> res;
            for (size_t i = 0; i < dim; ++i) {
                res.coords[i] = pt.coords[i] * val;
            }
            res.coords[dim] = pt.coords[dim];
            return res;
        }

        friend Point<dim> operator*(double val, Point<dim> pt) {
            return operator*(pt, val);
        }

        Point<dim> flip_orientation() const {
            //Flips the orientation
            Point<dim> p_out;
            for (size_t j = 0; j < dim + 1; j++) {
                p_out.coords[j] = -coords[j];
            }
            return p_out;
        }

        Point<dim> orient_up(int i) const {
            // Always orient these down by the second to last coordinate.
            // In 2d this ensures that y is oriented down.
            // In 3d this ensures that z is oriented down.
            Point<dim> p_out;
            assert(i < dim + 1);
            double orientation = std::copysign(1.0, coords[i]);
            for (size_t j = 0; j < dim + 1; j++) {
                p_out.coords[j] = orientation * coords[j];
            }
            return p_out;
        }

        Point<dim> orient_down(int i) const {
            // Always orient these down by the second to last coordinate.
            // In 2d this ensures that y is oriented down.
            // In 3d this ensures that z is oriented down.
            Point<dim> p_out;
            assert(i < dim + 1);
            double orientation = std::copysign(1.0, coords[i]);
            for (size_t j = 0; j < dim + 1; j++) {
                p_out.coords[j] = -orientation * coords[j];
            }
            return p_out;
        }

        inline double& operator[](size_t i) {
            assert(i < dim + 1);
            return coords[i];
        }

        inline double get_coord(size_t i) const {
            return coords[i];
        }

        inline double operator[](size_t i) const {
            assert(i < dim + 1);
            return coords[i];
        }

        inline double operator()(size_t i) const {
            assert(i < dim);
            return coords[i] / coords[dim];
        }

        virtual std::string str() const {
            std::stringstream ss;
            ss << *this;
            return ss.str();
        }

        bool operator==(const Point<dim> &pt) const {
            for (size_t i = 0; i < dim + 1; ++i) {
                if (coords[i] != pt.coords[i])
                    return false;
            }
            return true;
        }

        inline double evaluate(const Point<dim> &other) const {
            double res = 0.0;
            for (size_t i = 0; i < dim + 1; ++i) {
                res += coords[i] * other.coords[i];
            }
            return res; //* std::copysign(1.0, other[dim]);
        }

        inline Point<dim> direction(const Point<dim> & other) const {
            auto pt = other.operator-(*this);
            return pt.normalize();
        }

        inline double square_dist(const Point<dim> &p) const {
            double res = 0.0;
            for (size_t i = 0; i < dim; ++i) {
                double tmp = (p[i] / p[dim] - coords[i] / coords[dim]);
                res += tmp * tmp;
            }
            return res;
        }

        inline Point<dim> on_segment(Point<dim> const& p2, double alpha) {
            /*
             * Returns a point between this point and the second point that is alpha this + (1 - alpha) p2;
             */
            return alpha * (*this) + (1 - alpha) * p2;
        }

        inline double dist(const Point<dim> &p) const {
            return sqrt(square_dist(p));
        }

        Point<dim> normalize() const {
            Point<dim> res;
            double magnitude = sqrt(pdot(*this)) / coords[dim];
            for (size_t i = 0; i < dim; ++i) {
                res.coords[i] = coords[i] / magnitude;
            }
            res.coords[dim] = 1.0;
            return res;
        }

        Point<dim> operator-(const Point<dim>& other) const {
            Point<dim> res;
            for (size_t i = 0; i < dim; ++i) {
                res.coords[i] = coords[i] * other[dim] - other[i] * coords[dim];
            }
            res.coords[dim] = other[dim] * coords[dim];
            return res;
        }

        Point<dim> operator+(const Point<dim>& other) const {
            Point<dim> res;
            for (size_t i = 0; i < dim; ++i) {
                res.coords[i] = coords[i] * other[dim] + other[i] * coords[dim];
            }
            res.coords[dim] = other[dim] * coords[dim];
            return res;
        }

        inline double square_dist(const Point<dim>& begin, const Point<dim>& end) const {
            auto v = end - begin;
            auto w = *this - begin;
            double c1 = w.pdot(v);
            if (util::alte(c1, 0.0)) return square_dist(begin);
            double c2 = v.pdot(v);
            if (util::alte(c2, c1)) return square_dist(end);
            double b = c1 / c2;
            auto pb = begin + v * b;
            return square_dist(pb);
        }

        inline double pdot(const Point<dim>& other) const {
            /*
             * Takes the dot product between these points as if they are vectors
             */
            double res = 0.0;
            for (size_t i = 0; i < dim; ++i) {
                res += coords[i] * other.coords[i];
            }
            //If this is approximately 0 then the points exist at infinity and this operation doesn't make sense.
            assert(!util::aeq(coords[dim] * other[dim], 0.0));
            return res / (coords[dim] * other[dim]);
        }

        inline bool approx_eq(Point<dim> const& p) const {

            double res = 0.0;
            for (size_t i = 0; i < dim; ++i) {
                double tmp = (p[i] * coords[dim] - coords[i] * p[dim]);
                res += std::abs(tmp);
            }
            return util::aeq(res, 0.0);
        }

        inline bool above_closed(const Point<dim> &p) const {
            return util::alte(0.0, evaluate(p));
        }

        inline bool below_closed(const Point<dim> &p) const {
            return util::alte(evaluate(p), 0.0);
        }

        inline bool above(const Point<dim> &p) const {
            return util::alt(0.0, evaluate(p));
        }

        inline bool below(const Point<dim> &p) const {
            return util::alt(evaluate(p), 0.0);
        }

        inline bool crosses( const Point<dim>& p1, const Point<dim>& p2) const {
            /*
             * Checks if this line is crossed by this line segment between p1
             */
            auto or1 = p1.orient_up(dim);
            auto or2 = p2.orient_up(dim);
            return (above(or1) && below(or2)) || (below(or1) && above(or2));
        }

        inline bool parallel_lte( const Point<dim>& l1) const {
            /*
             * Checks to see if this line is less than or equal to the line l1 assuming that l1 is parallel to
             * this.
             */
            double norm = 1.0;
            for (size_t i = 0; i < dim; ++i) {
                if (!util::aeq(l1[i], 0)) {
                    norm = coords[i] / l1[i];
                    break;
                }
            }
            return util::alte(l1[dim] * norm, coords[dim]);
        }

        inline bool parallel_lt( const Point<dim>& l1) const {
            /*
             * Checks to see if this line is less than or equal to the line l1 assuming that l1 is parallel to
             * this.
             */
            double norm = 1.0;
            for (size_t i = 0; i < dim; ++i) {
                if (!util::aeq(l1[i], 0)) {
                    norm = coords[i] / l1[i];
                    break;
                }
            }
            return util::alt(l1[dim] * norm, coords[dim]);
        }

    protected:
        std::array<double, dim + 1> coords;
    };

    template<int dim = 2>
    class WPoint : public Point<dim> {
    public:
        template<typename ...Coords>
        explicit WPoint(double weight, Coords... rest)
                : Point<dim>(rest...), weight(weight) {
                }

        WPoint()
                : Point<dim>(), weight(0.0) {}
        virtual ~WPoint() = default;

        friend std::ostream &operator<<(std::ostream &os, WPoint const &pt) {
            os << "WPoint(" << pt.get_weight() << ", ";
            for (auto &el: pt.coords) {
                os << el << ", ";
            }
            os << ")";
            return os;
        }

        inline double get_weight() const {
            return weight;
        }

        virtual void set_weight(double w) {
            weight = w;
        }

    protected:
        double weight;
    };

    template<int dim = 2>
    class LPoint : public WPoint<dim> {
    public:
        template<typename ...Coords>
        LPoint(size_t label, double weight, Coords... rest)
                : WPoint<dim>(weight, rest...), label(label) {}

        LPoint()
                : WPoint<dim>(), label(0) {}

        virtual ~LPoint() = default;

        inline size_t get_label() const {
            return label;
        }

        friend std::ostream &operator<<(std::ostream &os, LPoint const &pt) {
            os << "LPoint(" << pt.label << ", " << pt.get_weight() << ", ";
            for (auto &el: pt.coords) {
                os << el << ", ";
            }
            os << ")";
            return os;
        }

        virtual void set_label(size_t l) {
            label = l;
        }

    protected:
        size_t label;
    };

    Point<2> correct_orientation(const Point<2>& pivot, const Point<2>& p);

    Point<2> intersection(const Point<2> &p1, const Point<2> &p2);

    std::tuple<double, double, double> normal(const Point<3> &p1, const Point<3> &p2, const Point<3> &p3);

    bool is_parallel(const Point<2> &l1, const Point<2> &l2);

    Point<3> cross_product(const Point<3>& p1, const Point<3>& p2);

    bool crosses_segment(const Point<2> &p1, const Point<2> &p2, const Point<2> &q1, const Point<2> &q2);

    //Return the two points on the line that are equidistance to some other point.
//    std::tuple<Point<2>, Point<2>> chord_pts(const Point<2> &line, const Point<2> &origin, double dist);

    using pt2_t = Point<2>;
    using wpt2_t = WPoint<2>;
    using lpt2_t = LPoint<2>;
    using point_list_t = std::vector<pt2_t>;
    using point_it_t = point_list_t::iterator;
    using cpoint_it_t = point_list_t::const_iterator;

    using weight_list_t = std::vector<double>;
    using weight_it_t = weight_list_t::iterator;
    using pt3_t = Point<3>;
    using wpt3_t = WPoint<3>;
    using lpt3_t = LPoint<3>;
    using point3_list_t = std::vector<pt3_t>;
    using point3_it_t = point3_list_t::iterator;
    using label_list_t = std::vector<size_t>;
    using wpoint_list_t = std::vector<WPoint<2>>;
    using wpoint_it_t = wpoint_list_t::iterator;
    using cwpoint_it_t = wpoint_list_t::const_iterator;

    using lpoint_list_t = std::vector<LPoint<2>>;
    using lpoint_it_t = lpoint_list_t::iterator;
    using clpoint_it_t = lpoint_list_t::const_iterator;

    using wpoint3_list_t = std::vector<WPoint<3>>;
    using lpoint3_list_t = std::vector<LPoint<3>>;
    using lpoint3_it_t = lpoint3_list_t::iterator;

    using discrepancy_func_t = std::function<double(double, double, double, double)>;

    template <typename T>
    void remove_duplicates(std::vector<T>& pts) {

        auto pts_end = std::remove_if(pts.begin(), pts.end(), [] (T const& pt){
            return util::aeq(pt[2], 0.0) || std::isnan(pt(0)) || std::isnan(pt(1)) || std::isinf(pt(0)) || std::isinf(pt(1));
        });
        std::sort(pts.begin(), pts_end, [](T const& p1, T const& p2){
            return p1(0) < p2(0);
        });

        //This is a version of unique that should be defined for non transitive relationships.
        auto new_end = pts.end() - 1;
        if (pts.size() <= 1) {
            return;
        }
        for (auto pt_b = pts.end() - 2; ;pt_b--) {
            if ((pt_b + 1)->approx_eq(*pt_b)) {
                std::swap(*(pt_b + 1), *pt_b);
                std::swap(*(pt_b + 1), *new_end);
                new_end--;
            }
            if (pt_b == pts.begin()) {
                break;
            }
        }
        pts.erase(new_end + 1, pts.end());
    }

    inline bool cmpX(Point<2> const& p1, Point<2> const& p2) {
        return p1(0) < p2(0);
    }

    inline bool cmpY(Point<2> const& p1, Point<2> const& p2) {
        return p1(1) < p2(1);
    }

    using bbox_t = std::tuple<double, double, double, double>;

    template<typename Pt>
    std::optional<bbox_t> bbox(std::vector<Pt> const& pts) {
        if (pts.empty()) {
            return std::nullopt;
        }
        auto [mnx, mxx] = std::minmax_element(pts.begin(), pts.end(), cmpX);
        auto [mny, mxy] = std::minmax_element(pts.begin(), pts.end(), cmpY);
        return std::make_tuple((*mnx)(0), (*mny)(1), (*mxx)(0), (*mxy)(1));
    }

    template<typename Pt, typename ...Args>
    std::optional<bbox_t> bbox(std::vector<Pt> const& pts, Args ...rest) {
        if (!pts.empty()) {
            auto opt_bbox = bbox(rest...);
            auto opt_bbox2 = bbox(pts);
            if (opt_bbox.has_value() && opt_bbox2.has_value()) {
                auto[mnx1, mny1, mxx1, mxy1] = opt_bbox.value();
                auto[mnx2, mny2, mxx2, mxy2] = opt_bbox2.value();
                return std::make_tuple(std::min(mnx1, mnx2), std::min(mny1, mny2), std::max(mxx1, mxx2),
                                       std::max(mxy1, mxy2));
            } else if (opt_bbox.has_value()) {
                return opt_bbox;
            } else {
                return opt_bbox2;
            }
        } else {
            return bbox(rest...);
        }
    }

//    template <int dim, class URNG>
//    wpoint_list_t weighted_random_sample_wor(wpoint_list_t& arr, URNG&& g, size_t sample_size) {
//        std::priority_queue<double, WPoint<dim>> queue_els;
//
//        sample_size = std::min(arr.size(), sample_size);
//        for (size_t i = 0; i < sample_size; ++i) {
//            std::uniform_int_distribution<decltype(i)> d(i, arr.size() - 1);
//            std::swap (arr[i], arr[d(g)]);
//        }
//        return Vec(arr.begin(), arr.begin() + sample_size);
//    }

}

#endif //PYSCAN_POINT_HPP


#pragma once
#include <vector>
#include <memory>
#include <Eigen/Sparse>
#include "../DerivedPtrHolder.hh"

namespace kt84 {

template <int N>
struct LaplaceDirect_VertexTraits {
    struct Data {
        bool is_fixed;
        Eigen::Matrix<double, N, 1> value;
        Eigen::Matrix<double, N, 1> laplacian;

        Data()
            : is_fixed()
            , value    (Eigen::Matrix<double, N, 1>::Zero())
            , laplacian(Eigen::Matrix<double, N, 1>::Zero())
        {}
    } laplaceDirect;
};

struct LaplaceDirect_HalfedgeTraits {
    double laplaceDirect_weight;
    LaplaceDirect_HalfedgeTraits()
        : laplaceDirect_weight(1)
    {}
};

template <class TMeshBase, class TMesh, int N>
struct LaplaceDirect : public DerivedPtrHolder<TMesh, LaplaceDirect<TMeshBase, TMesh, N>> {
    typedef Eigen::Matrix<double,  N, 1> Value ;
    typedef Eigen::Matrix<double, -1, N> Vector;
    typedef Eigen::SparseMatrix <double> Matrix;

    double laplaceDirect_constraintWeight;
    Matrix L;
    std::shared_ptr<Eigen::SimplicialCholesky<Matrix>> solver;

    LaplaceDirect()
        : laplaceDirect_constraintWeight(1000.0)            // better to set large value as default? not really sure...
    {}

    void laplaceDirect_factorize() {
        TMesh* mesh = get_mesh();

        int nv = mesh->n_vertices();

        typedef Eigen::Triplet<double> Triplet;
        std::vector<Triplet> triplets;
        for (int i = 0; i < nv; ++i) {
            auto v = mesh->vertex_handle(i);

            triplets.push_back(Triplet(i, i, 1));

            double weight_sum = 0;
            for (auto h = mesh->voh_iter(v); h.is_valid(); ++h)
                weight_sum += mesh->data(*h).laplaceDirect_weight;

            for (auto h = mesh->voh_iter(v); h.is_valid(); ++h) {
                auto w = mesh->to_vertex_handle(*h);
                double weight = mesh->data(*h).laplaceDirect_weight / weight_sum;

                triplets.push_back(Triplet(i, w.idx(), -weight));
            }

            if (mesh->data(v).laplaceDirect.is_fixed)
                triplets.push_back(Triplet(i, i, laplaceDirect_constraintWeight));
        }

        L.resize(nv, nv);
        L.setFromTriplets(triplets.begin(), triplets.end());

        solver.reset(new Eigen::SimplicialCholesky<Matrix>(L.transpose() * L));
    }

    void laplaceDirect_solve() {
        TMesh* mesh = get_mesh();

        int nv = mesh->n_vertices();

        if (!solver || L.rows() != nv)
            laplaceDirect_factorize();

        // set right hand side
        Vector b(nv, N);
        for (int i = 0; i < nv; ++i) {
            auto& vdata = mesh->data(mesh->vertex_handle(i)).laplaceDirect;

            b.row(i) = vdata.laplacian.transpose();

            if (vdata.is_fixed)
                b.row(i) += laplaceDirect_constraintWeight * vdata.value.transpose();
        }

        // solve!
        Vector x = solver->solve(L.transpose() * b);

        // copy result to Data::value
        for (int i = 0; i < nv; ++i) {
            auto& vdata = mesh->data(mesh->vertex_handle(i)).laplaceDirect;

            if (!vdata.is_fixed)
                vdata.value = x.row(i).transpose();
        }
    }

    void laplaceDirect_set_laplacian_from_value() {
        TMesh* mesh = get_mesh();

        for (auto v : mesh->vertices()) {
            auto& vdata = mesh->data(v).laplaceDirect;

            vdata.laplacian = vdata.value;

            double weight_sum = 0;
            for (auto h = mesh->voh_iter(v); h.is_valid(); ++h)
                weight_sum += mesh->data(*h).laplaceDirect_weight;

            for (auto h = mesh->voh_iter(v); h.is_valid(); ++h) {
                auto w = mesh->to_vertex_handle(*h);
                auto& wdata = mesh->data(w).laplaceDirect;
                double weight = mesh->data(*h).laplaceDirect_weight / weight_sum;

                vdata.laplacian -= weight * wdata.value;
            }
        }
    }
private:
    TMesh* get_mesh() const { return DerivedPtrHolder<TMesh, LaplaceDirect<TMeshBase, TMesh, N>>::derived_ptr; }
};

}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <array>
#include <boost/optional.hpp>
#include <cstddef>
#include <limits>

#include "DataStructures/Tensor/TypeAliases.hpp"
#include "Domain/OrientationMap.hpp"
#include "Utilities/TypeTraits.hpp"

namespace PUP {
class er;
}  // namespace PUP

namespace CoordinateMaps {

/*!
 * \ingroup CoordinateMapsGroup
 *
 * \brief Three dimensional map from the cube to a wedge.
 * \image html Shell.png "A shell can be constructed out of six wedges."
 *
 * \details The mapping that goes from a reference cube to a three-dimensional
 *  wedge centered on a coordinate axis covering a volume between an inner
 *  surface and outer surface. Each surface can be given a curvature
 *  between flat (a sphericity of 0) or spherical (a sphericity of 1).
 *
 *  The first two logical coordinates correspond to the two angular coordinates,
 *  and the third to the radial coordinate.
 *
 *  The Wedge3D map is constructed by linearly interpolating between a bulged
 *  face of radius `radius_of_inner_surface` to a bulged face of
 *  radius `radius_of_outer_surface`, where the radius of each bulged face
 *  is defined to be the radius of the sphere circumscribing the bulge.
 *
 *  We make a choice here as to whether we wish to use the logical coordinates
 *  parameterizing these surface as they are, in which case we have the
 *  equidistant choice of coordinates, or whether to apply a tangent map to them
 *  which leads us to the equiangular choice of coordinates. In terms of the
 *  logical coordinates, the equiangular coordinates are:
 *
 *  \f[\textrm{equiangular xi} : \Xi(\xi) = \textrm{tan}(\xi\pi/4)\f]
 *
 *  \f[\textrm{equiangular eta}  : \mathrm{H}(\eta) = \textrm{tan}(\eta\pi/4)\f]
 *
 *  With derivatives:
 *
 *  \f[\Xi'(\xi) = \frac{\pi}{4}(1+\Xi^2)\f]
 *
 *  \f[\mathrm{H}'(\eta) = \frac{\pi}{4}(1+\mathrm{H}^2)\f]
 *
 *  The equidistant coordinates are:
 *
 *  \f[ \textrm{equidistant xi}  : \Xi = \xi\f]
 *
 *  \f[ \textrm{equidistant eta}  : \mathrm{H} = \eta\f]
 *
 *  with derivatives:
 *
 *  <center>\f$\Xi'(\xi) = 1\f$, and \f$\mathrm{H}'(\eta) = 1\f$</center>
 *
 *  We also define the variable \f$\rho\f$, given by:
 *
 *  \f[\textrm{rho} : \rho = \sqrt{1+\Xi^2+\mathrm{H}^2}\f]
 *
 *  ### The Spherical Face Map
 *  The surface map for the spherical face of radius \f$R\f$ lying in the
 * \f$+z\f$
 *  direction in either choice of coordinates is then given by:
 *
 *  \f[\vec{\sigma}_{spherical}: \vec{\xi} \rightarrow \vec{x}(\vec{\xi})\f]
 *  Where
 *  \f[
 *  \vec{x}(\xi,\eta) =
 *  \begin{bmatrix}
 *  x(\xi,\eta)\\
 *  y(\xi,\eta)\\
 *  z(\xi,\eta)\\
 *  \end{bmatrix}  = \frac{R}{\rho}
 *  \begin{bmatrix}
 *  \Xi\\
 *  \mathrm{H}\\
 *  1\\
 *  \end{bmatrix}\f]
 *
 *  ### The Bulged Face Map
 *  The bulged surface is itself constructed by linearly interpolating between
 *  a cubical face and a spherical face. The surface map for the cubical face
 *  of side length \f$2L\f$ lying in the \f$+z\f$ direction is given by:
 *
 *  \f[\vec{\sigma}_{cubical}: \vec{\xi} \rightarrow \vec{x}(\vec{\xi})\f]
 *  Where
 *  \f[
 *  \vec{x}(\xi,\eta) =
 *  \begin{bmatrix}
 *  x(\xi,\eta)\\
 *  y(\xi,\eta)\\
 *  L\\
 *  \end{bmatrix}  = L
 *  \begin{bmatrix}
 *  \Xi\\
 *  \mathrm{H}\\
 *  1\\
 *  \end{bmatrix}\f]
 *
 *  To construct the bulged map we interpolate between this cubical face map
 *  and a spherical face map of radius \f$R\f$, with the
 *  interpolation parameter being \f$s\f$. The surface map for the bulged face
 *  lying in the \f$+z\f$ direction is then given by:
 *
 *  \f[\vec{\sigma}_{bulged}(\xi,\eta) = {(1-s)L + \frac{sR}{\rho}}
 *  \begin{bmatrix}
 *  \Xi\\
 *  \mathrm{H}\\
 *  1\\
 *  \end{bmatrix}\f]
 *
 *  We constrain L by demanding that the spherical face circumscribe the cube.
 *  With this condition, we have \f$L = R/\sqrt3\f$.
 *  \note This differs from the choice in SpEC where it is demanded that the
 *  surfaces touch at the center, which leads to \f$L = R\f$.
 *
 *  ### The Full Volume Map
 *  The final map for the wedge which lies along the \f$+z\f$ is obtained by
 *  interpolating between the two surfaces with the
 *  interpolation parameter being the logical coordinate \f$\zeta\f$. This
 *  results in:
 *
 *  \f[\vec{x}(\xi,\eta,\zeta) =
 *  \frac{1}{2}\left\{(1-\zeta)\Big[(1-s_{inner})\frac{R_{inner}}{\sqrt 3}
 *   + s_{inner}\frac{R_{inner}}{\rho}\Big] +
 *  (1+\zeta)\Big[(1-s_{outer})\frac{R_{outer}}{\sqrt 3} +s_{outer}
 *  \frac{R_{outer}}{\rho}\Big] \right\}\begin{bmatrix}
 *  \Xi\\
 *  \mathrm{H}\\
 *  1\\
 *  \end{bmatrix}\f]
 *
 *  We will define the variables \f$F(\zeta)\f$ and \f$S(\zeta)\f$, the frustum
 * and sphere factors: \f[F(\zeta) = F_0 + F_1\zeta\f] \f[S(\zeta) = S_0 +
 * S_1\zeta\f]
 *  Where \f{align*}F_0 &= \frac{1}{2} \big\{ (1-s_{outer})R_{outer} +
 * (1-s_{inner})R_{inner}\big\}\\
 *  F_1 &= \partial_{\zeta} F = \frac{1}{2} \big\{ (1-s_{outer})R_{outer} -
 * (1-s_{inner})R_{inner}\big\}\\
 *  S_0 &= \frac{1}{2} \big\{ s_{outer}R_{outer} + s_{inner}R_{inner}\big\}\\
 *  S_1 &= \partial_{\zeta} S = \frac{1}{2} \big\{ s_{outer}R_{outer} -
 * s_{inner}R_{inner}\big\}\f}
 *
 *  The map can then be rewritten as:
 * \f[\vec{x}(\xi,\eta,\zeta) = \left\{\frac{F(\zeta)}{\sqrt 3} +
 * \frac{S(\zeta)}{\rho}\right\}\begin{bmatrix}
 *  \Xi\\
 *  \mathrm{H}\\
 *  1\\
 *  \end{bmatrix}\f]
 *
 *  We provide some common derivatives:
 *  \f[\partial_{\xi}z = \frac{-S(\zeta)\Xi\Xi'}{\rho^3}\f]
 *  \f[\partial_{\eta}z = \frac{-S(\zeta)\mathrm{H}\mathrm{H}'}{\rho^3}\f]
 * \f[\partial_{\zeta}z = \frac{F'}{\sqrt 3} + \frac{S'}{\rho}\f]
 *  The Jacobian then is: \f[J =
 *  \begin{bmatrix}
 *  \Xi'z + \Xi\partial_{\xi}z & \Xi\partial_{\eta}z & \Xi\partial_{\zeta}z \\
 *  \mathrm{H}\partial_{\xi}z & \mathrm{H}'z +
 *  \mathrm{H}\partial_{\eta}z & \mathrm{H}\partial_{\zeta}z\\
 *   \partial_{\xi}z&\partial_{\eta}z &\partial_{\zeta}z \\
 *  \end{bmatrix}
 *  \f]
 *
 *  A common factor that shows up in the inverse jacobian is:
 *  \f[ T:= \frac{S(\zeta)}{(\partial_{\zeta}z)\rho^3}\f]
 *
 *  The inverse Jacobian then is: \f[J^{-1} =
 *  \frac{1}{z}\begin{bmatrix}
 *  \Xi'^{-1} & 0 & -\Xi\Xi'^{-1}\\
 *  0 & \mathrm{H}'^{-1} & -\mathrm{H}\mathrm{H}'^{-1}\\
 *  T\Xi &
 *  T\mathrm{H} &
 *  T + F(\partial_{\zeta}z)^{-1}/\sqrt 3\\
 *  \end{bmatrix}
 *  \f]
 *
 *  ### Changing the radial distribution of the gridpoints
 *  By default, Wedge3D linearly distributes its gridpoints in the radial
 *  direction. An exponential distribution of gridpoints can be obtained by
 *  linearly interpolating in the logarithm of the radius, in order to obtain
 *  a relatively higher resolution at smaller radii. Since this is a radial
 *  rescaling of Wedge3D, this option is only supported for fully spherical
 *  wedges with `sphericity_inner` = `sphericity_outer` = 1.
 *
 *  The linear interpolation done is:
 *  \f[
 *  \log r = \frac{1-\zeta}{2}\log R_{inner} +
 *  \frac{1+\zeta}{2}\log R_{outer}
 *  \f]
 *
 *  The map then is:
 *  \f[\vec{x}(\xi,\eta,\zeta) =
 *  \frac{\sqrt{R_{inner}^{1-\zeta}R_{outer}^{1+\zeta}}}{\rho}\begin{bmatrix}
 *  \Xi\\
 *  \mathrm{H}\\
 *  1\\
 *  \end{bmatrix}\f]
 *
 *  The jacobian simplifies similarly.
 *
 */
class Wedge3D {
 public:
  static constexpr size_t dim = 3;
  enum class WedgeHalves {
    /// Use the entire wedge
    Both,
    /// Use only the upper logical half
    UpperOnly,
    /// Use only the lower logical half
    LowerOnly
  };

  /*!
   * Constructs a 3D wedge.
   * \param radius_inner Distance from the origin to one of the
   * corners which lie on the inner surface.
   * \param radius_outer Distance from the origin to one of the
   * corners which lie on the outer surface.
   * \param orientation_of_wedge The orientation of the desired wedge relative
   * to the orientation of the default wedge which is a wedge that has its
   * curved surfaces pierced by the upper-z axis. The logical xi and eta
   * coordinates point in the cartesian x and y directions, respectively.
   * \param sphericity_inner Value between 0 and 1 which determines
   * whether the inner surface is flat (value of 0), spherical (value of 1) or
   * somewhere in between
   * \param sphericity_outer Value between 0 and 1 which determines
   * whether the outer surface is flat (value of 0), spherical (value of 1) or
   * somewhere in between
   * \param with_equiangular_map Determines whether to apply a tangent function
   * mapping to the logical coordinates (for `true`) or not (for `false`).
   * \param halves_to_use Determines whether to use the logical xi
   * coordinates in the [0,1] interval (value of `UpperOnly`) of the full wedge,
   * the coordinates in the [-1,0] interval (value of `LowerOnly`) of the
   * full wedge, or the full wedge entirely (value of `Both`). Half wedges are
   * currently only useful in constructing domains for binary systems.
   * \param with_logarithmic_map Determines whether to apply an exponential
   * function mapping to the "sphere factor", the effect of which is to
   * distribute the radial gridpoints logarithmically in physical space.
   */
  Wedge3D(double radius_inner, double radius_outer,
          OrientationMap<3> orientation_of_wedge, double sphericity_inner,
          double sphericity_outer, bool with_equiangular_map,
          WedgeHalves halves_to_use = WedgeHalves::Both,
          bool with_logarithmic_map = false) noexcept;

  Wedge3D() = default;
  ~Wedge3D() = default;
  Wedge3D(Wedge3D&&) = default;
  Wedge3D(const Wedge3D&) = default;
  Wedge3D& operator=(const Wedge3D&) = default;
  Wedge3D& operator=(Wedge3D&&) = default;

  template <typename T>
  std::array<tt::remove_cvref_wrap_t<T>, 3> operator()(
      const std::array<T, 3>& source_coords) const noexcept;

  /// For a \f$+z\f$-oriented `Wedge3D`, returns invalid if \f$z<=0\f$
  /// or if \f$(x,y,z)\f$ is on or outside the cone defined
  /// by \f$(x^2/z^2 + y^2/z^2+1)^{1/2} = -S/F\f$, where
  /// \f$S = \frac{1}{2}(s_1 r_1 - s_0 r_0)\f$ and
  /// \f$F = \frac{1}{2\sqrt{3}}((1-s_1) r_1 - (1-s_0) r_0)\f$.
  /// Here \f$s_0,s_1\f$ and \f$r_0,r_1\f$ are the specified sphericities
  /// and radii of the inner and outer \f$z\f$ surfaces.  The map is singular on
  /// the cone and on the xy plane.
  boost::optional<std::array<double, 3>> inverse(
      const std::array<double, 3>& target_coords) const noexcept;

  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame> jacobian(
      const std::array<T, 3>& source_coords) const noexcept;

  template <typename T>
  tnsr::Ij<tt::remove_cvref_wrap_t<T>, 3, Frame::NoFrame> inv_jacobian(
      const std::array<T, 3>& source_coords) const noexcept;

  // clang-tidy: google runtime references
  void pup(PUP::er& p) noexcept;  // NOLINT

 private:
  // factors out calculation of z needed for mapping and jacobian
  template <typename T>
  tt::remove_cvref_wrap_t<T> default_physical_z(const T& zeta,
                                                const T& one_over_rho) const
      noexcept;
  friend bool operator==(const Wedge3D& lhs, const Wedge3D& rhs) noexcept;

  double radius_inner_{std::numeric_limits<double>::signaling_NaN()};
  double radius_outer_{std::numeric_limits<double>::signaling_NaN()};
  OrientationMap<3> orientation_of_wedge_{};
  double sphericity_inner_{std::numeric_limits<double>::signaling_NaN()};
  double sphericity_outer_{std::numeric_limits<double>::signaling_NaN()};
  bool with_equiangular_map_ = false;
  WedgeHalves halves_to_use_ = WedgeHalves::Both;
  bool with_logarithmic_map_ = false;
  double scaled_frustum_zero_{std::numeric_limits<double>::signaling_NaN()};
  double sphere_zero_{std::numeric_limits<double>::signaling_NaN()};
  double scaled_frustum_rate_{std::numeric_limits<double>::signaling_NaN()};
  double sphere_rate_{std::numeric_limits<double>::signaling_NaN()};
};
bool operator!=(const Wedge3D& lhs, const Wedge3D& rhs) noexcept;
}  // namespace CoordinateMaps


/*
 * Copyright 2019 Xilinx, Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

/**
 *  @brief Kernel implementation file
 *
 *  $DateTime: 2019/04/09 12:00:00 $
 */

#ifndef _HLS_FD_SOLVER_H_
#define _HLS_FD_SOLVER_H_
#include "ap_int.h"
#include "xf_fintech/spmv.hpp"
#include "xf_fintech/dimv.hpp"
#include "xf_fintech/trsv.hpp"
#ifndef __SYNTHESIS__
#include <assert.h>
#include <boost/algorithm/string.hpp>
#include <cmath>
#include <fstream>
#include <iostream>
#include <vector>
#endif

#define PRAGMA_SUB(x) _Pragma(#x)
#define DO_PRAGMA(x) PRAGMA_SUB(x)

namespace xf {
namespace fintech {
namespace internal {

/// @brief Get V-inner index from an S ordered vector
inline unsigned int V2S(const unsigned int index, const unsigned int m1, const unsigned int m2) {
    unsigned int row = index % m2;
    unsigned int col = index / m2;
    return row * m1 + col;
}

/// @brief Get S-inner index from an V ordered vector
inline unsigned int S2V(const unsigned int index, const unsigned int m1, const unsigned int m2) {
    unsigned int row = index % m1;
    unsigned int col = index / m1;
    return row * m2 + col;
}

/// @brief Class to encapsulate the Finite Difference engine components
template <typename DT,
          unsigned int M_SIZE,
          unsigned int LOG2_M_SIZE,
          unsigned int A_SIZE,
          unsigned int MEM_WIDTH,
          unsigned int DIM2_SIZE1 = 3,
          unsigned int DIM2_SIZE2 = 5>
class Solver {
   public:
    /// @brief default constructor
    Solver() {
#pragma HLS inline
    }
    /// @brief Copy and reorder S ordered vector into V inner form
    /// @param[in]  v_in  Vector representing array of size m1 x m2 flattened in S
    /// inner form
    /// @param[in]  m1   Size of array [0..m1-1] in S direction
    /// @param[in]  m2   Size of array [0..m2-1] in V direction
    /// @param[out] v_out Vector representing array of size m1 x m2 flattened in V
    /// inner form
    void reorderS2V(const DT v_in[M_SIZE], DT v_out[M_SIZE], const unsigned int m1, const unsigned int m2) {
#pragma HLS ARRAY_PARTITION variable = v_out cyclic factor = MEM_WIDTH dim = 1

        for (unsigned int i = 0; i < M_SIZE; ++i) {
#pragma HLS UNROLL factor = MEM_WIDTH
            v_out[S2V(i, m1, m2)] = v_in[i];
        }
    }

    /// @brief Copy and reorder V ordered vector into S inner form
    /// @param[in]  v_in  Vector representing array of size m1 x m2 flattened in V
    /// inner form
    /// @param[in]  m1   Size of array [0..m1] in S direction
    /// @param[in]  m2   Size of array [0..m2] in V direction
    /// @param[out] v_out Vector representing array of size m1 x m2 flattened in S
    /// inner form
    void reorderV2S(const DT v_in[M_SIZE], DT v_out[M_SIZE], const unsigned int m1, const unsigned int m2) {
#pragma HLS ARRAY_PARTITION variable = v_out cyclic factor = MEM_WIDTH dim = 1

        for (unsigned int i = 0; i < M_SIZE; ++i) {
#pragma HLS UNROLL factor = MEM_WIDTH
            v_out[V2S(i, m1, m2)] = v_in[i];
        }
    }

    /// @brief Utility function to copy a vector
    void CopyVector(const DT v_in[M_SIZE], DT v_out[M_SIZE]) {
        for (unsigned int i = 0; i < M_SIZE; ++i) {
            v_out[i] = v_in[i];
        }
    }

    /// @brief Utility function to add two vectors
    void vectorAdd(const DT v_in0[M_SIZE], const DT v_in1[M_SIZE], DT v_out[M_SIZE]) {
#pragma HLS ARRAY_PARTITION variable = v_out cyclic factor = MEM_WIDTH dim = 1

        for (int i = 0; i < M_SIZE; ++i) {
#pragma HLS UNROLL factor = MEM_WIDTH
            v_out[i] = v_in0[i] + v_in1[i];
        }
    }

    /// @brief Utility function to subtract two vectors
    void vectorSub(const DT v_in0[M_SIZE], const DT v_in1[M_SIZE], DT v_out[M_SIZE]) {
#pragma HLS ARRAY_PARTITION variable = v_out cyclic factor = MEM_WIDTH dim = 1

        for (int i = 0; i < M_SIZE; ++i) {
#pragma HLS UNROLL factor = MEM_WIDTH
            v_out[i] = v_in0[i] - v_in1[i];
        }
    }

    /// @brief Wrapper to PCR tridiagonal solver
    /// @details Solves tridiagonal linear system
    /// @param[in]  X1    M x 3 array holding lower/main/upper diagonals of X1
    /// tridiagonal matrix
    /// @param[in]  rhs   Right hand side of linear system to be solved
    /// @param[in]  m1    Size of array [0..m1-1] in S direction
    /// @param[in]  m2    Size of array [0..m2-1] in V direction
    /// @param[out] v_out  Solution vector representing array of size m1 x m2
    /// flattened in V inner form
    void triDiagSovlerPCR(
        DT X1[M_SIZE][DIM2_SIZE1], DT rhs[M_SIZE], DT v_out[M_SIZE], const unsigned int m1, const unsigned int m2) {
        DT r[M_SIZE];
        DT a[M_SIZE];
        DT b[M_SIZE];
        DT c[M_SIZE];
#pragma HLS RESOURCE variable = r core = RAM_2P_BRAM
#pragma HLS RESOURCE variable = a core = RAM_2P_BRAM
#pragma HLS RESOURCE variable = b core = RAM_2P_BRAM
#pragma HLS RESOURCE variable = c core = RAM_2P_BRAM
        DO_PRAGMA(HLS array_partition variable = r cyclic factor = FD_NUM_PCR)
        DO_PRAGMA(HLS array_partition variable = a cyclic factor = FD_NUM_PCR)
        DO_PRAGMA(HLS array_partition variable = b cyclic factor = FD_NUM_PCR)
        DO_PRAGMA(HLS array_partition variable = c cyclic factor = FD_NUM_PCR)

        // Working copies of diagonals as solver will overwrite them
        for (int i = 0; i < M_SIZE; i++) {
            //#pragma HLS pipeline
            r[i] = rhs[i];
            a[i] = X1[i][0];
            b[i] = X1[i][1];
            c[i] = X1[i][2];
        }

        // Call the PCR tridiagonal solver
        xf::fintech::trsvCore<DT, M_SIZE, LOG2_M_SIZE, FD_NUM_PCR>(a, b, c, r);

        // Final division for result
        for (int k = 0; k < M_SIZE; k++) {
            // Note that v_out is written in V inner order using the S2V function
            v_out[S2V(k, m1, m2)] = r[k] / b[k];
        }
    }

    /// @brief Solve pentadiagonal form linear system
    /// @details This is a highly serial algorithm and is the bottleneck in this
    /// solver
    /// Unfortunately due to the formulation used by In 'T Hout & Foulon, the
    /// pentadiagonal array contains diagonals
    /// which are not fully populated.  This causes the common parallel
    /// pentadiagonal systems to fail due to
    /// divide-by-zero errors or similar.
    /// @param[in]  A          M x 5 array holding lower/lower/main/upper/upper
    /// diagonals of X1 tridiagonal matrix
    /// @param[in]  rhs        Right hand side of linear system to be solved
    /// @param[in]  precompute Flag to indicate the scaling factors should be
    /// computed
    /// @param[out] v_out       Solution vector representing array of size m1 x m2
    /// flattened in V inner form
    void pendaDiagSovler(DT A[M_SIZE][DIM2_SIZE2], DT rhs[M_SIZE], DT v_out[M_SIZE], bool precompute) {
        // These are preprocessed versions of the diagonals.
        // For a given array these are constant so they are calculated once and
        // cached (by marking as static) and using precompute variable.
        static DT d[M_SIZE];
        static DT a[M_SIZE];
        static DT e[M_SIZE];
        static DT c[M_SIZE];
        static DT f[M_SIZE];

        static DT xmult0[M_SIZE];
        static DT xmult1[M_SIZE];

        static DT xmult_r;

        static DT d_inv[M_SIZE];

        // Not static as this changes each time
        DT r[M_SIZE];

        // Working variable
        DT xmult = 0.0;

        // One time precomputation of the vectors
        if (precompute) {
            // Main diagonal
            for (unsigned int i = 0; i < M_SIZE; ++i) {
                d[i] = A[i][2];
            }

            // First upper/lower, dropping padded zeros
            for (unsigned int i = 0; i < M_SIZE - 1; ++i) {
                a[i] = A[i + 1][1];
                c[i] = A[i][3];
            }

            // Second upper/lower, dropping padded zeros
            for (unsigned int i = 0; i < M_SIZE - 2; ++i) {
                e[i] = A[i + 2][0];
                f[i] = A[i][4];
            }

            // Scaling factors
            for (unsigned int i = 1; i < (M_SIZE - 1); ++i) {
                xmult = a[i - 1] / d[i - 1];
                d[i] = d[i] - xmult * c[i - 1];
                c[i] = c[i] - xmult * f[i - 1];
                xmult0[i] = xmult;
                xmult = e[i - 1] / d[i - 1];
                a[i] = a[i] - xmult * c[i - 1];
                d[i + 1] = d[i + 1] - xmult * f[i - 1];
                xmult1[i] = xmult;
            }

            // d manipulation
            xmult_r = a[M_SIZE - 2] / d[M_SIZE - 2];
            d[M_SIZE - 1] = d[M_SIZE - 1] - xmult_r * c[M_SIZE - 2];

            // Invert d (one time operation and allows multiplies to be used later on)
            for (unsigned int i = 0; i < M_SIZE; ++i) {
                d_inv[i] = 1.0 / d[i];
            }
        }

        // Input vector changes each time
        for (unsigned int i = 0; i < M_SIZE; ++i) {
            r[i] = rhs[i];
        }

        // Apply the scaling factors
        for (unsigned int i = 1; i < (M_SIZE - 1); ++i) {
            r[i] = r[i] - xmult0[i] * r[i - 1];
            r[i + 1] = r[i + 1] - xmult1[i] * r[i - 1];
        }

        // Back solve
        v_out[M_SIZE - 1] = (r[M_SIZE - 1] - xmult_r * r[M_SIZE - 2]) * d_inv[M_SIZE - 1];
        v_out[M_SIZE - 2] = (r[M_SIZE - 2] - c[M_SIZE - 2] * v_out[M_SIZE - 1]) * d_inv[M_SIZE - 2];
        unsigned int i = M_SIZE - 3;
        do {
            v_out[i] = (r[i] - f[i] * v_out[i + 2] - c[i] * v_out[i + 1]) * d_inv[i];
        } while (i-- > 0);
    }
};

/// @brief Utility class to encapsulate the multiplier elements
template <typename DT,
          unsigned int MEM_WIDTH,
          unsigned int INDEX_WIDTH,
          unsigned int A_SIZE,
          unsigned int M_SIZE,
          unsigned int LOG2_M_SIZE,
          unsigned int DIM2_SIZE1 = 3,
          unsigned int DIM2_SIZE2 = 5>
class StreamWrapper {
   private:
    static const unsigned int M_SIZE_BLOCKS = M_SIZE / MEM_WIDTH;

   public:
    typedef xf::fintech::blas::WideType<DT, MEM_WIDTH> WideDataType;
    typedef hls::stream<WideDataType> WideStreamType;

   public:
    /// @brief default constructor
    StreamWrapper() {
#pragma HLS inline
    }
    /// @brief Computes multiplication of tridiagonal matrix by a vector
    /// @param[in]  A1         M x 3 array holding lower/main/upper diagonals of
    /// A1 tridiagonal matrix
    /// @param[in]  u          Vector to be multiplied
    /// @param[out] rhs1_tmp0 Multiplication result
    /// @param[out] u_out     Stream form of U to pass to pentadiagonal multiplier
    void streamDimv3(DT A1[M_SIZE][DIM2_SIZE1], DT u[M_SIZE], DT rhs1_tmp0[M_SIZE], WideStreamType& u_out) {
        for (unsigned int i = 0; i < M_SIZE_BLOCKS; ++i) {
#pragma HLS PIPELINE
            WideDataType val;
#pragma HLS ARRAY_PARTITION variable = val complete
            for (unsigned int j = 0; j < MEM_WIDTH; ++j) {
                val[j] = u[i * MEM_WIDTH + j];
            }
            u_out.write(val);
        }
        xf::fintech::blas::dimv<DT, M_SIZE, DIM2_SIZE1, MEM_WIDTH>(A1, u, M_SIZE, rhs1_tmp0);
    }

    /// @brief Computes multiplication of pentadiagonal matrix by a vector
    /// @param[in]  A2         M x 5 array holding lower/main/upper diagonals of
    /// A2 tridiagonal matrix
    /// @param[in]  u_in      Vector to be multiplied [stream format]
    /// @param[in]  m1         Size of array [0..m1-1] in S direction
    /// @param[in]  m2         Size of array [0..m2-1] in V direction
    /// @param[out] rhs2_tmp0 Multiplication result
    /// @param[out] u_out     Vector output [stream format]
    void streamDimv5(DT A2[M_SIZE][DIM2_SIZE2],
                     WideStreamType& u_in,
                     unsigned int m1,
                     unsigned int m2,
                     DT rhs2_tmp0[M_SIZE],
                     WideStreamType& u_out) {
        DT u_r0[M_SIZE]; // S-inner
        DT u_r1[M_SIZE]; // V-inner
#pragma HLS ARRAY_PARTITION variable = u_r0 cyclic factor = MEM_WIDTH dim = 1
#pragma HLS ARRAY_PARTITION variable = u_r1 cyclic factor = MEM_WIDTH dim = 1

        for (unsigned int i = 0; i < M_SIZE_BLOCKS; ++i) {
#pragma HLS PIPELINE
            WideDataType val;
#pragma HLS ARRAY_PARTITION variable = val complete
            val = u_in.read();
            u_out.write(val);
            for (unsigned int j = 0; j < MEM_WIDTH; ++j) {
                u_r0[i * MEM_WIDTH + j] = val[j];
            }
        }
        // S2V conversion
        Solver<DT, M_SIZE, LOG2_M_SIZE, A_SIZE, MEM_WIDTH> solver;
        solver.reorderS2V(u_r0, u_r1, m1, m2);
        xf::fintech::blas::dimv<DT, M_SIZE, DIM2_SIZE2, MEM_WIDTH>(A2, u_r1, M_SIZE, rhs2_tmp0);
    }

    /// @brief Computes multiplication of sparse matrix by vector plus a constant
    /// @param[in]  A          Sparse matrix value
    /// @param[in]  Ar         Sparse matrix row
    /// @param[in]  Ac         Sparse matrix column
    /// @param[in]  u_in      Vector to be multiplied [stream format]
    /// @param[in]  b          Vector to be added after sparse-mult stage
    /// @param[in]  Annz       Number of non-zeros in sparse matrix (how many
    /// elements of A/Ar/Ac are valid)
    /// @param[in]  M          Matrix M-size === (m1+1) x (m2+1)
    /// @param[out] y0         Result of mult-add in flattened S-inner form
    void streamSparseMultAdd(DT A[A_SIZE],
                             unsigned int Ar[A_SIZE],
                             unsigned int Ac[A_SIZE],
                             WideStreamType& u_in,
                             DT b[M_SIZE],
                             unsigned int Annz,
                             unsigned int M,
                             DT y0[M_SIZE]) {
#pragma HLS ARRAY_PARTITION variable = y0 cyclic factor = MEM_WIDTH dim = 1
        DT u[M_SIZE];
#pragma HLS ARRAY_PARTITION variable = u cyclic factor = MEM_WIDTH dim = 1
        DT y0_tmp1[M_SIZE];
#pragma HLS ARRAY_PARTITION variable = y0_tmp1 cyclic factor = MEM_WIDTH dim = 1

        for (unsigned int i = 0; i < M_SIZE_BLOCKS; ++i) {
#pragma HLS PIPELINE
            WideDataType val = u_in.read();
#pragma HLS ARRAY_PARTITION variable = val complete
            for (unsigned int j = 0; j < MEM_WIDTH; ++j) {
                u[i * MEM_WIDTH + j] = val[j];
            }
        }
        xf::fintech::blas::Spmv<DT, MEM_WIDTH, INDEX_WIDTH, M_SIZE, M_SIZE, A_SIZE> spmv;
        spmv.sparseMultAdd(A, Ar, Ac, u, b, y0_tmp1, Annz, M);
        unsigned int vec_blocks = M / MEM_WIDTH;
        for (unsigned int i = 0; i < vec_blocks; ++i) {
#pragma HLS PIPELINE
            for (unsigned int j = 0; j < MEM_WIDTH; ++j) {
                y0[i * MEM_WIDTH + j] = u[i * MEM_WIDTH + j] + y0_tmp1[i * MEM_WIDTH + j];
            }
        }
    }

    /// @brief Wrapper function to combine multipliers into a dataflow region and
    /// allow parallelization
    void parallelBlocks(DT A[A_SIZE],
                        unsigned int Ar[A_SIZE],
                        unsigned int Ac[A_SIZE],
                        DT u[M_SIZE],
                        DT b[M_SIZE],
                        unsigned int Annz,
                        unsigned int M,
                        DT A1[M_SIZE][DIM2_SIZE1],
                        DT A2[M_SIZE][DIM2_SIZE2],
                        unsigned int m1,
                        unsigned int m2,
                        DT y0[M_SIZE],
                        DT rhs1_tmp0[M_SIZE],
                        DT rhs2_tmp0[M_SIZE]) {
        WideStreamType u0;
        WideStreamType u1;
#pragma HLS DATAFLOW
        streamDimv3(A1, u, rhs1_tmp0, u0);
        streamDimv5(A2, u0, m1, m2, rhs2_tmp0, u1);
        streamSparseMultAdd(A, Ar, Ac, u1, b, Annz, M, y0);
    }
};

} // end of internal namespace block

/// @brief Top level callable function to perform the Douglas ADI method
/// @details This function creates the solver/stream wrapper objects and connects
/// them up
/// It also provides the extra connectivity for the non-streaming blocks
/// @param[in]  A          Sparse matrix value
/// @param[in]  Ar         Sparse matrix row
/// @param[in]  Ac         Sparse matrix column
/// @param[in]  Annz       Number of non-zeros in sparse matrix (how many
/// elements of A/Ar/Ac are valid)
/// @param[in]  A1         Tridiagonal matrix stored as three vectors
/// lower/main/upper
/// @param[in]  A2         Pentadiagonal matrix stored as five vectors
/// lower/lower/main/upper/upper
/// @param[in]  X1         Tridiagonal matrix stored as three vectors
/// lower/main/upper
/// @param[in]  X2         Pentadiagonal matrix stored as five vectors
/// lower/lower/main/upper/upper
/// @param[in]  b          Boundary condition vector
/// @param[in]  u0         Initial condition (payoff condition for a call
/// option)
/// @param[in]  M1         Size of array [0..M1] in S direction
/// @param[in]  M2         Size of array [0..M2] in V direction
/// @param[in]  N          Iteration count
/// @param[out] u          Calculated price grid
template <typename DT,
          unsigned int MEM_WIDTH,
          unsigned int INDEX_WIDTH,
          unsigned int A_SIZE,
          unsigned int M_SIZE,
          unsigned int LOG2_M_SIZE,
          unsigned int DIM2_SIZE1,
          unsigned int DIM2_SIZE2>
void FdDouglas(DT A[A_SIZE],
               unsigned int Ar[A_SIZE],
               unsigned int Ac[A_SIZE],
               unsigned int Annz,
               DT A1[M_SIZE][DIM2_SIZE1],
               DT A2[M_SIZE][DIM2_SIZE2],
               DT X1[M_SIZE][DIM2_SIZE1],
               DT X2[M_SIZE][DIM2_SIZE2],
               DT b[M_SIZE],
               DT u0[M_SIZE],
               unsigned int M1,
               unsigned int M2,
               unsigned int N,
               DT u[M_SIZE]) {
    DT y0[M_SIZE];
#pragma HLS ARRAY_PARTITION variable = y0 cyclic factor = MEM_WIDTH dim = 1
    DT rhs1[M_SIZE];
    DT y1[M_SIZE];
    DT rhs2[M_SIZE];
    DT y2[M_SIZE];

    DT rhs1_tmp0[M_SIZE];
    DT rhs2_tmp0[M_SIZE];

    internal::Solver<DT, M_SIZE, LOG2_M_SIZE, A_SIZE, MEM_WIDTH, DIM2_SIZE1, DIM2_SIZE2> solver;
    internal::StreamWrapper<DT, MEM_WIDTH, INDEX_WIDTH, A_SIZE, M_SIZE, LOG2_M_SIZE> stream_wrapper;

    solver.CopyVector(u0, u);

    // Perform the Douglas iteration
    // After N iterations the u vector will hold the output
    for (int i = 0; i < N; ++i) {
        // These operations are performed in parallel with a stream passing vector
        // data between them
        stream_wrapper.parallelBlocks(A, Ar, Ac, u, b, Annz, M_SIZE, A1, A2, M1, M2, y0, rhs1_tmp0, rhs2_tmp0);

        // These operations depending on the previous one completing and run
        // sequentially
        solver.vectorSub(y0, rhs1_tmp0, rhs1);
        solver.triDiagSovlerPCR(X1, rhs1, y1, M1, M2);
        solver.vectorSub(y1, rhs2_tmp0, rhs2);
        solver.pendaDiagSovler(X2, rhs2, y2, i == 0);

        // Vector u has to be reordered back to S-inner form
        solver.reorderV2S(y2, u, M1, M2);
    }
}
}
} // End of namespace xf::fintech

#endif