reconstruction.cpp

/*---------------------------------------------------------------------------*\
 *
 *  bitpit
 *
 *  Copyright (C) 2015-2021 OPTIMAD engineering Srl
 *
 *  -------------------------------------------------------------------------
 *  License
 *  This file is part of bitpit.
 *
 *  bitpit is free software: you can redistribute it and/or modify it
 *  under the terms of the GNU Lesser General Public License v3 (LGPL)
 *  as published by the Free Software Foundation.
 *
 *  bitpit is distributed in the hope that it will be useful, but WITHOUT
 *  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 *  FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public
 *  License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public License
 *  along with bitpit. If not, see <http://www.gnu.org/licenses/>.
 *
\*---------------------------------------------------------------------------*/
 
#include <cassert>
#include <limits>
#include "bitpit_LA.hpp"
#include "bitpit_operators.hpp"
 
#include "bitpit_private_cblas.hpp"
#include "bitpit_private_lapacke.hpp"
 
#include "reconstruction.hpp"
 
namespace bitpit {


const uint8_t ReconstructionPolynomial::MAX_DEGREE = 2;

const uint8_t ReconstructionPolynomial::MAX_DIMENSIONS = 3;

const bool ReconstructionPolynomial::ENABLE_FAST_PATH_OPTIMIZATIONS = true;

const int ReconstructionPolynomial::MAX_STACK_WORKSPACE_SIZE = 10;

const std::vector<std::vector<uint16_t>> ReconstructionPolynomial::m_countCoefficientCache = generateCountCoefficientCache();

const std::vector<std::vector<uint16_t>> ReconstructionPolynomial::m_countDegreeCoefficientCache = generateCountDegreeCoefficientCache();

std::vector<std::vector<uint16_t>> ReconstructionPolynomial::generateCountCoefficientCache()
{
    std::vector<std::vector<uint16_t>> cache(MAX_DIMENSIONS + 1, std::vector<uint16_t>(MAX_DEGREE + 1, 0));
    for (uint8_t dimensions = 0; dimensions <= MAX_DIMENSIONS; ++dimensions) {
        for (uint8_t degree = 0; degree <= MAX_DEGREE; ++degree) {
            cache[dimensions][degree] = countCoefficients(degree, dimensions);
        }
    }
 
    return cache;
}

std::vector<std::vector<uint16_t>> ReconstructionPolynomial::generateCountDegreeCoefficientCache()
{
    std::vector<std::vector<uint16_t>> cache(MAX_DIMENSIONS + 1, std::vector<uint16_t>(MAX_DEGREE + 1, 0));
    for (uint8_t dimensions = 0; dimensions <= MAX_DIMENSIONS; ++dimensions) {
        for (uint8_t degree = 0; degree <= MAX_DEGREE; ++degree) {
            cache[dimensions][degree] = countDegreeCoefficients(degree, dimensions);
        }
    }
 
    return cache;
}

uint16_t ReconstructionPolynomial::getCoefficientCount(uint8_t degree, uint8_t dimensions)
{
    assert(degree <= ReconstructionPolynomial::MAX_DEGREE);
    assert(dimensions <= ReconstructionPolynomial::MAX_DIMENSIONS);
 
    return m_countCoefficientCache[dimensions][degree];
}

const std::vector<uint16_t> & ReconstructionPolynomial::getCoefficientsCount(uint8_t dimensions)
{
    assert(dimensions <= ReconstructionPolynomial::MAX_DIMENSIONS);
 
    return m_countCoefficientCache[dimensions];
}

uint16_t ReconstructionPolynomial::countCoefficients(uint8_t degree, uint8_t dimensions)
{
    uint16_t nCoeffs = 0;
    for (int i = 0; i <= degree; ++i) {
        nCoeffs += countDegreeCoefficients(i, dimensions);
    }
 
    return nCoeffs;
}

uint16_t ReconstructionPolynomial::getDegreeCoefficientCount(uint8_t degree, uint8_t dimensions)
{
    assert(degree <= ReconstructionPolynomial::MAX_DEGREE);
    assert(dimensions <= ReconstructionPolynomial::MAX_DIMENSIONS);
 
    return m_countDegreeCoefficientCache[dimensions][degree];
}

const std::vector<uint16_t> & ReconstructionPolynomial::getDegreeCoefficientsCount(uint8_t dimensions)
{
    assert(dimensions <= ReconstructionPolynomial::MAX_DIMENSIONS);
 
    return m_countDegreeCoefficientCache[dimensions];
}

uint16_t ReconstructionPolynomial::countDegreeCoefficients(uint8_t degree, uint8_t dimensions)
{
    if (dimensions == 0) {
        return 0;
    }
 
    return static_cast<uint16_t>(utils::factorial(dimensions - 1 + degree) / utils::factorial(dimensions - 1) / utils::factorial(degree));
}

void ReconstructionPolynomial::evalPointBasisValues(uint8_t degree, uint8_t dimensions, const std::array<double, 3> &origin,
                                                    const std::array<double, 3> &point, double *csi)
{
    // Set 0-th degree coefficients
    csi[0] = 1.;
 
    // Set high degree coefficients
    if (degree >= 1) {
        int offset = 1;
        const std::array<double, 3> distance = point - origin;
 
        // Set 1-st degree coefficients
        for (int i = 0; i < dimensions; ++i) {
            csi[offset++] = distance[i];
        }
 
        // Set 2-nd degree coefficients
        if (degree >= 2) {
            for (int i = 0; i < dimensions; ++i) {
                csi[offset++] = 0.5 * distance[i] * distance[i];
            }
 
            if (dimensions >= 2) {
                csi[offset++] = distance[0] * distance[1];
 
                if (dimensions >= 3) {
                    csi[offset++] = distance[0] * distance[2];
                    csi[offset++] = distance[1] * distance[2];
                }
            }
        }
 
        // Check if all coefficients have been set
        assert(offset == ReconstructionPolynomial::getCoefficientCount(degree, dimensions));
    }
}

 * \param point is the point were the basis will be evaluated
 * \param direction is the direction of the derivative
 * \param point is the point were the basis will be evaluated
 * \param[out] dcsi on output will contain values of the basis for point
 * derivative reconstruction.
 */
void ReconstructionPolynomial::evalPointBasisDerivatives(uint8_t degree, uint8_t dimensions,  const std::array<double, 3> &origin,
                                                         const std::array<double, 3> &point, const std::array<double, 3> &direction,
                                                         double *dcsi)
{
    // Set 0-th degree coefficients
    dcsi[0] = 0.;
 
    // Set high degree coefficients
    if (degree >= 1) {
        int offset = 1;
 
        // Set 1-st degree coefficients
        for (int i = 0; i < dimensions; ++i) {
            dcsi[offset++] = direction[i];
        }
 
        // Set 2-nd degree coefficients
        if (degree >= 2) {
            const std::array<double, 3> distance = point - origin;
 
            for (int i = 0; i < dimensions; ++i) {
                dcsi[offset++] = distance[i] * direction[i];
            }
 
            if (dimensions >= 2) {
                dcsi[offset++] = distance[0] * direction[1] + distance[1] * direction[0];
 
                if (dimensions >= 3) {
                    dcsi[offset++] = distance[0] * direction[2] + distance[2] * direction[0];
                    dcsi[offset++] = distance[1] * direction[2] + distance[2] * direction[1];
                }
            }
        }
 
        // Check if all coefficients have been set
        assert(offset == ReconstructionPolynomial::getCoefficientCount(degree, dimensions));
    }
}

void ReconstructionPolynomial::evalCellBasisValues(uint8_t degree, uint8_t dimensions, const std::array<double, 3> &origin,
                                                   const Cell &cell,  const std::array<double, 3> *vertexCoords,
                                                   double *csi)
{
    // Check if cell type is supported
    //
    // Polynomials of degree 0 and 1 are supported on all type of cells, for
    // pixels and voxels also degree 2 is supported.
    //
    // Coefficients are evaluated as the volume average of the Taylor series
    // expansion coefficients. In order to support higher order degrees on
    // all type of cells, we need to implement numerical integration of the
    // expansions terms over the volume of the cell. This can be done using
    // Gauss quadrature rules and evaluating the terms on the integration
    // points using the function that evaluate the basis function on a
    // specified point.
    ElementType cellType = cell.getType();
 
    bool cellTypeSupported;
    if (cellType == ElementType::PIXEL) {
        cellTypeSupported = (degree <= 2);
    } else if (cellType == ElementType::VOXEL) {
        cellTypeSupported = (degree <= 2);
    } else {
        cellTypeSupported = (degree <= 1);
    }
 
    if (!cellTypeSupported) {
        throw std::runtime_error("Cell type not supported.");
    }
 
    // Set 0-th degree coefficients
    csi[0] = 1.;
 
    // Set high degree coefficients
    if (degree >= 1) {
        int offset = 1;
        const std::array<double, 3> distance = cell.evalCentroid(vertexCoords) - origin;
 
        // Set 1-st degree coefficients
        for (int i = 0; i < dimensions; ++i) {
            csi[offset++] = distance[i];
        }
 
        // Set 2-nd degree coefficients
        if (degree >= 2) {
            double cellSize = cell.evalSize(vertexCoords);
 
            for (int i = 0; i < dimensions; ++i) {
                csi[offset++] = 0.5 * (distance[i] * distance[i] + cellSize * cellSize / 12.);
            }
 
            if (dimensions >= 2) {
                csi[offset++] = distance[0] * distance[1];
 
                if (dimensions >= 3) {
                    csi[offset++] = distance[0] * distance[2];
                    csi[offset++] = distance[1] * distance[2];
                }
            }
        }
 
        // Check if all coefficients have been set
        assert(offset == ReconstructionPolynomial::getCoefficientCount(degree, dimensions));
    }
}

ReconstructionPolynomial::ReconstructionPolynomial()
    : m_degree(0), m_dimensions(0), m_nFields(0), m_nCoeffs(0)
{
    initialize(0, 0, {{0., 0., 0.}}, 0);
}

ReconstructionPolynomial::ReconstructionPolynomial(uint8_t degree, uint8_t dimensions,
                                                   const std::array<double, 3> &origin, int nFields)
    : m_degree(0), m_dimensions(0), m_nFields(0), m_nCoeffs(0)
{
    initialize(degree, dimensions, origin, nFields);
}

ReconstructionPolynomial::ReconstructionPolynomial(const ReconstructionPolynomial &other)
    : ReconstructionPolynomial(other.getDegree(), other.getDimensions(), other.m_origin, other.m_nFields)
{
    if (m_nFields > 0) {
        int nWeights = m_nCoeffs * m_nFields;
        std::copy(other.m_coeffs.get(), other.m_coeffs.get() + nWeights, m_coeffs.get());
    }
}

ReconstructionPolynomial & ReconstructionPolynomial::operator=(const ReconstructionPolynomial &other)
{
    ReconstructionPolynomial tmp(other);
    swap(tmp);
 
    return *this;
}

void ReconstructionPolynomial::swap(ReconstructionPolynomial &other) noexcept
{
    std::swap(other.m_degree, m_degree);
    std::swap(other.m_dimensions, m_dimensions);
    std::swap(other.m_nCoeffs, m_nCoeffs);
    std::swap(other.m_nFields, m_nFields);
    std::swap(other.m_coeffs, m_coeffs);
    std::swap(other.m_origin, m_origin);
}

void ReconstructionPolynomial::initialize(uint8_t degree, uint8_t dimensions,
                                          const std::array<double, 3> &origin,
                                          int nFields, bool release)
{
    assert(degree <= ReconstructionPolynomial::MAX_DEGREE);
    m_degree = degree;
 
    assert(dimensions <= ReconstructionPolynomial::MAX_DIMENSIONS);
    m_dimensions = dimensions;
 
    if (nFields > 0) {
        int currentStorageSize = m_nCoeffs * m_nFields;
 
        m_nFields = nFields;
        m_nCoeffs = ReconstructionPolynomial::getCoefficientCount(m_degree, m_dimensions);
 
        int storageSize = m_nCoeffs * m_nFields;
 
        bool reallocate;
        if (release) {
            reallocate = (currentStorageSize != storageSize);
        } else {
            reallocate = (currentStorageSize < storageSize);
        }
 
        if (reallocate) {
            m_coeffs = std::unique_ptr<double[]>(new double[storageSize]);
        }
    } else {
        clear(release);
    }
 
    m_origin = origin;
}

void ReconstructionPolynomial::clear(bool release)
{
    m_nFields = 0;
 
    if (release) {
        m_nCoeffs = 0;
        m_coeffs.reset();
    }
}

uint8_t ReconstructionPolynomial::getDegree() const
{
    return m_degree;
}

uint8_t ReconstructionPolynomial::getDimensions() const
{
    return m_dimensions;
}

const std::array<double, 3> & ReconstructionPolynomial::getOrigin() const
{
    return m_origin;
}

uint16_t ReconstructionPolynomial::getCoefficientCount() const
{
    return m_nCoeffs;
}

int ReconstructionPolynomial::getFieldCount() const
{
    return m_nFields;
}

const double * ReconstructionPolynomial::getCoefficients() const
{
    return m_coeffs.get();
}

double * ReconstructionPolynomial::getCoefficients()
{
    return m_coeffs.get();
}

const double * ReconstructionPolynomial::getCoefficients(int field) const
{
    const double *coefficients = m_coeffs.get() + computeFieldCoefficientsOffset(0, field);
 
    return coefficients;
}

double * ReconstructionPolynomial::getCoefficients(int field)
{
    double *coefficients = m_coeffs.get() + computeFieldCoefficientsOffset(0, field);
 
    return coefficients;
}

const double * ReconstructionPolynomial::getDegreeCoefficients(uint8_t degree, int field) const
{
    const double *coefficients = m_coeffs.get() + computeFieldCoefficientsOffset(degree, field);
 
    return coefficients;
}

double * ReconstructionPolynomial::getDegreeCoefficients(uint8_t degree, int field)
{
    double *coefficients = m_coeffs.get() + computeFieldCoefficientsOffset(degree, field);
 
    return coefficients;
}

std::size_t ReconstructionPolynomial::computeFieldCoefficientsOffset(uint8_t degree, int field) const
{
    std::size_t offset = field * getFieldCoefficientsStride() + degree;
 
    return offset;
}

std::size_t ReconstructionPolynomial::getFieldCoefficientsStride() const
{
    return m_nCoeffs;
}

void ReconstructionPolynomial::computeValue(const std::array<double, 3> &point,
                                            int field, double *value) const
{
    computeValues(m_degree, point, 1, field, value);
}

void ReconstructionPolynomial::computeValues(const std::array<double, 3> &point,
                                             double *values) const
{
    computeValues(m_degree, point, m_nFields, 0, values);
}

void ReconstructionPolynomial::computeValues(const std::array<double, 3> &point,
                                             int nFields, double *values) const
{
    computeValues(m_degree, point, nFields, 0, values);
}

void ReconstructionPolynomial::computeValues(const std::array<double, 3> &point,
                                             int nFields, int offset, double *values) const
{
    computeValues(m_degree, point, nFields, offset, values);
}

void ReconstructionPolynomial::computeValue(int degree, const std::array<double, 3> &point,
                                            int field, double *values) const
{
    computeValues(degree, point, 1, field, values);
}

void ReconstructionPolynomial::computeValues(int degree, const std::array<double, 3> &point,
                                             double *values) const
{
    computeValues(degree, point, m_nFields, 0, values);
}

void ReconstructionPolynomial::computeValues(int degree, const std::array<double, 3> &point,
                                             int nFields, double *values) const
{
    computeValues(degree, point, nFields, 0, values);
}

void ReconstructionPolynomial::computeValues(int degree, const std::array<double, 3> &point,
                                             int nFields, int offset, double *values) const
{
    assert(degree <= getDegree());
 
    // Early return if there are no field to process
    if (nFields == 0) {
        return;
    }
 
    // Early return if the polynomial is not initialized
    if (m_dimensions == 0) {
        return;
    }
 
    // Get values
    const std::size_t fieldValuesStride = 1;
    double *fieldValue = values;
    const double *fieldValueEnd = fieldValue + fieldValuesStride * nFields;
 
    // Get coefficients
    std::size_t fieldCoeffsStride = getFieldCoefficientsStride();
    const double *fieldCoeffs = m_coeffs.get() + fieldCoeffsStride * offset;
 
    // Constant polynomial
    if (ENABLE_FAST_PATH_OPTIMIZATIONS && degree == 0) {
        do {
            *fieldValue = fieldCoeffs[0];
 
            fieldCoeffs += fieldCoeffsStride;
            fieldValue  += fieldValuesStride;
        } while (fieldValue != fieldValueEnd);
 
        return;
    }
 
    // Generic polynomial
    BITPIT_CREATE_WORKSPACE(csi, double, m_nCoeffs, MAX_STACK_WORKSPACE_SIZE);
    ReconstructionPolynomial::evalPointBasisValues(degree, m_dimensions, m_origin, point, csi);
 
    do {
        *fieldValue = fieldCoeffs[0] * csi[0];
        for (int i = 1; i < m_nCoeffs; ++i) {
            *fieldValue += fieldCoeffs[i] * csi[i];
        }
 
        fieldCoeffs += fieldCoeffsStride;
        fieldValue  += fieldValuesStride;
    } while (fieldValue != fieldValueEnd);
}

void ReconstructionPolynomial::computeValueLimited(const std::array<double, 3> &point,
                                                   const double *limiters, int field,
                                                   double *values) const
{
    computeValuesLimited(m_degree, point, limiters, 1, field, values);
}

void ReconstructionPolynomial::computeValuesLimited(const std::array<double, 3> &point,
                                                    const double *limiters,
                                                    double *values) const
{
    computeValuesLimited(m_degree, point, limiters, m_nFields, 0, values);
}

void ReconstructionPolynomial::computeValuesLimited(const std::array<double, 3> &point,
                                                    const double *limiters, int nFields,
                                                    double *values) const
{
    computeValuesLimited(m_degree, point, limiters, nFields, 0, values);
}

void ReconstructionPolynomial::computeValuesLimited(const std::array<double, 3> &point,
                                                    const double *limiters, int nFields, int offset,
                                                    double *values) const
{
    computeValuesLimited(m_degree, point, limiters, nFields, offset, values);
}

void ReconstructionPolynomial::computeValueLimited(int degree, const std::array<double, 3> &point,
                                                   const double *limiters, int field,
                                                   double *values) const
{
    computeValuesLimited(degree, point, limiters, 1, field, values);
}

void ReconstructionPolynomial::computeValuesLimited(int degree, const std::array<double, 3> &point,
                                                    const double *limiters,
                                                    double *values) const
{
    computeValuesLimited(degree, point, limiters, m_nFields, 0, values);
}

void ReconstructionPolynomial::computeValuesLimited(int degree, const std::array<double, 3> &point,
                                                    const double *limiters, int nFields,
                                                    double *values) const
{
    computeValuesLimited(degree, point, limiters, nFields, 0, values);
}

void ReconstructionPolynomial::computeValuesLimited(int degree, const std::array<double, 3> &point,
                                                    const double *limiters, int nFields, int offset,
                                                    double *values) const
{
    assert(degree <= getDegree());
 
    // Early return if there are no field to process
    if (nFields == 0) {
        return;
    }
 
    // Early return if the polynomial is not initialized
    if (m_dimensions == 0) {
        return;
    }
 
    // Get values
    std::size_t fieldValuesStride = 1;
    double *fieldValue = values;
    double *fieldValueEnd = fieldValue + fieldValuesStride * nFields;
 
    // Get coefficients
    std::size_t fieldCoeffsStride = getFieldCoefficientsStride();
    const double *fieldCoeffs = m_coeffs.get() + fieldCoeffsStride * offset;
 
    // Constant polynomial
    if (ENABLE_FAST_PATH_OPTIMIZATIONS && degree == 0) {
        do {
            *fieldValue = fieldCoeffs[0];
 
            fieldCoeffs += fieldCoeffsStride;
            fieldValue  += fieldValuesStride;
        } while (fieldValue != fieldValueEnd);
 
        return;
    }
 
    // Get limiters
    std::size_t fieldLimitersStride = degree;
    const double *fieldLimiters = limiters;
 
    // Generic polynomial
    const std::vector<uint16_t> &nDegreeCoeffs = ReconstructionPolynomial::getDegreeCoefficientsCount(m_dimensions);
 
    BITPIT_CREATE_WORKSPACE(csi, double, nDegreeCoeffs[degree], MAX_STACK_WORKSPACE_SIZE);
    ReconstructionPolynomial::evalPointBasisValues(degree, m_dimensions, m_origin, point, csi);
 
    do {
        // Degree 0
        *fieldValue = fieldCoeffs[0] * csi[0];
 
        // Degrees greater than 0
        int coeffEnd = nDegreeCoeffs[0];
        for (int n = 1; n <= degree; ++n) {
            int coeffBegin = coeffEnd;
            coeffEnd = coeffBegin + nDegreeCoeffs[n];
 
            double fieldDegreeValue = 0;
            for (int i = coeffBegin; i < coeffEnd; ++i) {
                fieldDegreeValue += fieldCoeffs[i] * csi[i];
            }
 
            *fieldValue += fieldDegreeValue * fieldLimiters[n - 1];
        }
 
        fieldCoeffs   += fieldCoeffsStride;
        fieldValue    += fieldValuesStride;
        fieldLimiters += fieldLimitersStride;
    } while (fieldValue != fieldValueEnd);
}

void ReconstructionPolynomial::computeDerivative(const std::array<double, 3> &point,
                                                 const std::array<double, 3> &direction,
                                                 int field, double *derivative) const
{
    computeDerivatives(m_degree, point, direction, 1, field, derivative);
}

void ReconstructionPolynomial::computeDerivatives(const std::array<double, 3> &point,
                                                  const std::array<double, 3> &direction,
                                                  double *derivatives) const
{
    computeDerivatives(m_degree, point, direction, m_nFields, 0, derivatives);
}

void ReconstructionPolynomial::computeDerivatives(const std::array<double, 3> &point,
                                                  const std::array<double, 3> &direction,
                                                  int nFields, double *derivatives) const
{
    computeDerivatives(m_degree, point, direction, nFields, 0, derivatives);
}

void ReconstructionPolynomial::computeDerivatives(const std::array<double, 3> &point,
                                                  const std::array<double, 3> &direction,
                                                  int nFields, int offset, double *derivatives) const
{
    computeDerivatives(m_degree, point, direction, nFields, offset, derivatives);
}

void ReconstructionPolynomial::computeDerivative(int degree, const std::array<double, 3> &point,
                                                 const std::array<double, 3> &direction,
                                                 int field, double *derivative) const
{
    computeDerivatives(degree, point, direction, 1, field, derivative);
}

void ReconstructionPolynomial::computeDerivatives(int degree, const std::array<double, 3> &point,
                                                  const std::array<double, 3> &direction,
                                                  double *derivatives) const
{
    computeDerivatives(degree, point, direction, m_nFields, 0, derivatives);
}

void ReconstructionPolynomial::computeDerivatives(int degree, const std::array<double, 3> &point,
                                                  const std::array<double, 3> &direction,
                                                  int nFields, double *derivatives) const
{
    computeDerivatives(degree, point, direction, nFields, 0, derivatives);
}

void ReconstructionPolynomial::computeDerivatives(int degree, const std::array<double, 3> &point,
                                                  const std::array<double, 3> &direction,
                                                  int nFields, int offset, double *derivatives) const
{
    assert(degree <= getDegree());
 
    // Early return if there are no field to process
    if (nFields == 0) {
        return;
    }
 
    // Early return if the polynomial is not initialized
    if (m_dimensions == 0) {
        return;
    }
 
    // Get derivatives
    std::size_t fieldDerivativeStride = 1;
    double *fieldDerivative = derivatives;
    double *fieldDerivativeEnd = fieldDerivative + fieldDerivativeStride * nFields;
 
    // Constant polynomial
    if (ENABLE_FAST_PATH_OPTIMIZATIONS && degree == 0) {
        do {
            *fieldDerivative = 0.;
 
            fieldDerivative += fieldDerivativeStride;
        } while (fieldDerivative != fieldDerivativeEnd);
 
        return;
    }
 
    // Get coefficients
    std::size_t fieldCoeffsStride = getFieldCoefficientsStride();
    const double *fieldCoeffs = m_coeffs.get() + fieldCoeffsStride * offset;
 
    // Linear polynomial
    if (ENABLE_FAST_PATH_OPTIMIZATIONS && degree == 1) {
        do {
            *fieldDerivative = fieldCoeffs[1] * direction[0];
            for (int d = 1; d < m_dimensions; ++d) {
                *fieldDerivative += fieldCoeffs[d + 1] * direction[d];
            }
 
            fieldCoeffs     += fieldCoeffsStride;
            fieldDerivative += fieldDerivativeStride;
        } while (fieldDerivative != fieldDerivativeEnd);
 
        return;
    }
 
    // Generic polynomial
    BITPIT_CREATE_WORKSPACE(csi, double, m_nCoeffs, MAX_STACK_WORKSPACE_SIZE);
    ReconstructionPolynomial::evalPointBasisDerivatives(degree, m_dimensions, m_origin, point, direction, csi);
 
    do {
        *fieldDerivative = fieldCoeffs[0] * csi[0];
        for (int i = 1; i < m_nCoeffs; ++i) {
            *fieldDerivative += fieldCoeffs[i] * csi[i];
        }
 
        fieldCoeffs     += fieldCoeffsStride;
        fieldDerivative += fieldDerivativeStride;
    } while (fieldDerivative != fieldDerivativeEnd);
}

void ReconstructionPolynomial::computeDerivativeLimited(const std::array<double, 3> &point,
                                                        const std::array<double, 3> &direction,
                                                        const double *limiters, int field,
                                                        double *derivative) const
{
    computeDerivativesLimited(m_degree, point, direction, limiters, 1, field, derivative);
}

void ReconstructionPolynomial::computeDerivativesLimited(const std::array<double, 3> &point,
                                                         const std::array<double, 3> &direction,
                                                         const double *limiters,
                                                         double *derivatives) const
{
    computeDerivativesLimited(m_degree, point, direction, limiters, m_nFields, 0, derivatives);
}

void ReconstructionPolynomial::computeDerivativesLimited(const std::array<double, 3> &point,
                                                         const std::array<double, 3> &direction,
                                                         const double *limiters, int nFields,
                                                         double *derivatives) const
{
    computeDerivativesLimited(m_degree, point, direction, limiters, nFields, 0, derivatives);
}

void ReconstructionPolynomial::computeDerivativesLimited(const std::array<double, 3> &point,
                                                         const std::array<double, 3> &direction,
                                                         const double *limiters, int nFields, int offset,
                                                         double *derivatives) const
{
    computeDerivativesLimited(m_degree, point, direction, limiters, nFields, offset, derivatives);
}

void ReconstructionPolynomial::computeDerivativeLimited(int degree, const std::array<double, 3> &point,
                                                        const std::array<double, 3> &direction,
                                                        const double *limiters, int field,
                                                        double *derivative) const
{
    computeDerivativesLimited(degree, point, direction, limiters, 1, field, derivative);
}

void ReconstructionPolynomial::computeDerivativesLimited(int degree, const std::array<double, 3> &point,
                                                         const std::array<double, 3> &direction,
                                                         const double *limiters,
                                                         double *derivatives) const
{
    computeDerivativesLimited(degree, point, direction, limiters, m_nFields, 0, derivatives);
}

void ReconstructionPolynomial::computeDerivativesLimited(int degree, const std::array<double, 3> &point,
                                                         const std::array<double, 3> &direction,
                                                         const double *limiters, int nFields,
                                                         double *derivatives) const
{
    computeDerivativesLimited(degree, point, direction, limiters, nFields, 0, derivatives);
}

void ReconstructionPolynomial::computeDerivativesLimited(int degree, const std::array<double, 3> &point,
                                                         const std::array<double, 3> &direction,
                                                         const double *limiters, int nFields, int offset,
                                                         double *derivatives) const
{
    assert(degree <= getDegree());
 
    // Early return if there are no field to process
    if (nFields == 0) {
        return;
    }
 
    // Early return if the polynomial is not initialized
    if (m_dimensions == 0) {
        return;
    }
 
    // Get derivatives
    std::size_t fieldDerivativeStride = 1;
    double *fieldDerivative = derivatives;
    double *fieldDerivativeEnd = fieldDerivative + fieldDerivativeStride * nFields;
 
    // Constant polynomial
    if (ENABLE_FAST_PATH_OPTIMIZATIONS && degree == 0) {
        do {
            *fieldDerivative = 0.;
 
            fieldDerivative += fieldDerivativeStride;
        } while (fieldDerivative != fieldDerivativeEnd);
 
        return;
    }
 
    // Get coefficients
    std::size_t fieldCoeffsStride = getFieldCoefficientsStride();
    const double *fieldCoeffs = m_coeffs.get() + fieldCoeffsStride * offset;
 
    // Get limiters
    std::size_t fieldLimitersStride = degree;
    const double *fieldLimiters = limiters;
 
    // Linear polynomial
    if (ENABLE_FAST_PATH_OPTIMIZATIONS && degree == 1) {
        do {
            *fieldDerivative = fieldCoeffs[1] * direction[0];
            for (int d = 1; d < m_dimensions; ++d) {
                *fieldDerivative += fieldCoeffs[d + 1] * direction[d];
            }
 
            *fieldDerivative *= fieldLimiters[0];
 
            fieldCoeffs     += fieldCoeffsStride;
            fieldDerivative += fieldDerivativeStride;
            fieldLimiters   += fieldLimitersStride;
        } while (fieldDerivative != fieldDerivativeEnd);
 
        return;
    }
 
    // Generic polynomial
    const std::vector<uint16_t> &nDegreeCoeffs = ReconstructionPolynomial::getDegreeCoefficientsCount(m_dimensions);
 
    BITPIT_CREATE_WORKSPACE(dcsi, double, nDegreeCoeffs[degree], MAX_STACK_WORKSPACE_SIZE);
    ReconstructionPolynomial::evalPointBasisDerivatives(degree, m_dimensions, m_origin, point, direction, dcsi);
 
    do {
        *fieldDerivative = fieldCoeffs[0] * dcsi[0];
 
        int coeffEnd = nDegreeCoeffs[0];
        for (int n = 1; n <= degree; ++n) {
            int coeffBegin = coeffEnd;
            coeffEnd = coeffBegin + nDegreeCoeffs[n];
 
            double fieldDegreeDerivative = 0;
            for (int i = coeffBegin; i < coeffEnd; ++i) {
                fieldDegreeDerivative += fieldCoeffs[i] * dcsi[i];
            }
 
            *fieldDerivative += fieldLimiters[n - 1] * fieldDegreeDerivative;
        }
 
        fieldCoeffs     += fieldCoeffsStride;
        fieldDerivative += fieldDerivativeStride;
        fieldLimiters   += fieldLimitersStride;
    } while (fieldDerivative != fieldDerivativeEnd);
}

void ReconstructionPolynomial::computeGradient(const std::array<double, 3> &point,
                                               int field, std::array<double, 3> *gradient) const
{
    computeGradients(m_degree, point, 1, field, gradient);
}

void ReconstructionPolynomial::computeGradients(const std::array<double, 3> &point,
                                                std::array<double, 3> *gradients) const
{
    computeGradients(m_degree, point, m_nFields, 0, gradients);
}

void ReconstructionPolynomial::computeGradients(const std::array<double, 3> &point,
                                                int nFields, std::array<double, 3> *gradients) const
{
    computeGradients(m_degree, point, nFields, 0, gradients);
}

void ReconstructionPolynomial::computeGradients(const std::array<double, 3> &point,
                                                int nFields, int offset,
                                                std::array<double, 3> *gradients) const
{
    computeGradients(m_degree, point, nFields, offset, gradients);
}

void ReconstructionPolynomial::computeGradient(int degree, const std::array<double, 3> &point,
                                               int field, std::array<double, 3> *gradient) const
{
    computeGradients(degree, point, 1, field, gradient);
}

void ReconstructionPolynomial::computeGradients(int degree, const std::array<double, 3> &point,
                                                std::array<double, 3> *gradients) const
{
    computeGradients(degree, point, m_nFields, 0, gradients);
}

void ReconstructionPolynomial::computeGradients(int degree, const std::array<double, 3> &point,
                                                int nFields, std::array<double, 3> *gradients) const
{
    computeGradients(degree, point, nFields, 0, gradients);
}

void ReconstructionPolynomial::computeGradients(int degree, const std::array<double, 3> &point,
                                                int nFields, int offset,
                                                std::array<double, 3> *gradients) const
{
    assert(degree <= getDegree());
 
    // Early return if there are no field to process
    if (nFields == 0) {
        return;
    }
 
    // Early return if the polynomial is not initialized
    if (m_dimensions == 0) {
        return;
    }
 
    // Get gradients
    const std::size_t fieldGradientStride = 1;
    std::array<double, 3> *fieldGradient = gradients;
    const std::array<double, 3> *fieldGradientEnd = fieldGradient + fieldGradientStride * nFields;
 
    // Constant polynomial
    if (ENABLE_FAST_PATH_OPTIMIZATIONS && degree == 0) {
        do {
            *fieldGradient = {{0., 0., 0.}};
 
            fieldGradient += fieldGradientStride;
        } while (fieldGradient != fieldGradientEnd);
 
        return;
    }
 
    // Get coefficients
    const std::size_t fieldCoeffsStride = getFieldCoefficientsStride();
    const double *fieldCoeffs = m_coeffs.get() + fieldCoeffsStride * offset;
 
    // Linear polynomial
    if (ENABLE_FAST_PATH_OPTIMIZATIONS && degree == 1) {
        do {
            // Evaluate gradients
            for (int d = 0; d < m_dimensions; ++d) {
                (*fieldGradient)[d] = fieldCoeffs[1 + d];
            }
 
            // Explicitly zero unused components
            for (int d = m_dimensions; d < ReconstructionPolynomial::MAX_DIMENSIONS; ++d) {
                (*fieldGradient)[d] = 0.;
            }
 
            // Advance to the next field
            fieldCoeffs   += fieldCoeffsStride;
            fieldGradient += fieldGradientStride;
        } while (fieldGradient != fieldGradientEnd);
 
        return;
    }
 
    // Generic polynomial
    BITPIT_CREATE_WORKSPACE(dcsi, double, static_cast<std::size_t>(m_dimensions * m_nCoeffs), 3 * MAX_STACK_WORKSPACE_SIZE);
    ReconstructionPolynomial::evalPointBasisDerivatives(degree, m_dimensions, m_origin, point, {{1., 0., 0.}}, dcsi);
    ReconstructionPolynomial::evalPointBasisDerivatives(degree, m_dimensions, m_origin, point, {{0., 1., 0.}}, dcsi + m_nCoeffs);
    if (m_dimensions == 3) {
        ReconstructionPolynomial::evalPointBasisDerivatives(degree, m_dimensions, m_origin, point, {{0., 0., 1.}}, dcsi + 2 * m_nCoeffs);
    }
 
    do {
        // Evaluate gradient
        for (int d = 0; d < m_dimensions; ++d) {
            const double *dcsi_dimension = dcsi + d * m_nCoeffs;
 
            (*fieldGradient)[d] = fieldCoeffs[0] * dcsi_dimension[0];
            for (int i = 1; i < m_nCoeffs; ++i) {
                (*fieldGradient)[d] += fieldCoeffs[i] * dcsi_dimension[i];
            }
        }
 
        // Explicitly zero unused components
        for (int d = m_dimensions; d < ReconstructionPolynomial::MAX_DIMENSIONS; ++d) {
            (*fieldGradient)[d] = 0.;
        }
 
        // Advance to the next field
        fieldCoeffs   += fieldCoeffsStride;
        fieldGradient += fieldGradientStride;
    } while (fieldGradient != fieldGradientEnd);
}

void ReconstructionPolynomial::computeGradientLimited(const std::array<double, 3> &point,
                                                      const double *limiters, int field,
                                                      std::array<double, 3> *gradient) const
{
    computeGradientsLimited(m_degree, point, limiters, 1, field, gradient);
}

void ReconstructionPolynomial::computeGradientsLimited(const std::array<double, 3> &point,
                                                       const double *limiters,
                                                       std::array<double, 3> *gradients) const
{
    computeGradientsLimited(m_degree, point, limiters, m_nFields, 0, gradients);
}

void ReconstructionPolynomial::computeGradientsLimited(const std::array<double, 3> &point,
                                                       const double *limiters, int nFields,
                                                       std::array<double, 3> *gradients) const
{
    computeGradientsLimited(m_degree, point, limiters, nFields, 0, gradients);
}

void ReconstructionPolynomial::computeGradientsLimited(const std::array<double, 3> &point,
                                                       const double *limiters, int nFields, int offset,
                                                       std::array<double, 3> *gradients) const
{
    computeGradientsLimited(m_degree, point, limiters, nFields, offset, gradients);
}

void ReconstructionPolynomial::computeGradientLimited(int degree, const std::array<double, 3> &point,
                                                      const double *limiters, int field,
                                                      std::array<double, 3> *gradient) const
{
    computeGradientsLimited(degree, point, limiters, 1, field, gradient);
}

void ReconstructionPolynomial::computeGradientsLimited(int degree, const std::array<double, 3> &point,
                                                       const double *limiters,
                                                       std::array<double, 3> *gradients) const
{
    computeGradientsLimited(degree, point, limiters, m_nFields, 0, gradients);
}

void ReconstructionPolynomial::computeGradientsLimited(int degree, const std::array<double, 3> &point,
                                                       const double *limiters, int nFields,
                                                       std::array<double, 3> *gradients) const
{
    computeGradientsLimited(degree, point, limiters, nFields, 0, gradients);
}

void ReconstructionPolynomial::computeGradientsLimited(int degree, const std::array<double, 3> &point,
                                                       const double *limiters, int nFields, int offset,
                                                       std::array<double, 3> *gradients) const
{
    assert(degree <= getDegree());
 
    // Early return if there are no field to process
    if (nFields == 0) {
        return;
    }
 
    // Early return if the polynomial is not initialized
    if (m_dimensions == 0) {
        return;
    }
 
    // Get gradients
    const std::size_t fieldGradientStride = 1;
    std::array<double, 3> *fieldGradient = gradients;
    const std::array<double, 3> *fieldGradientEnd = fieldGradient + fieldGradientStride * nFields;
 
    // Constant polynomial
    if (ENABLE_FAST_PATH_OPTIMIZATIONS && degree == 0) {
        do {
            *fieldGradient = {{0., 0., 0.}};
 
            fieldGradient += fieldGradientStride;
        } while (fieldGradient != fieldGradientEnd);
 
        return;
    }
 
    // Get coefficients
    const std::size_t fieldCoeffsStride = getFieldCoefficientsStride();
    const double *fieldCoeffs = m_coeffs.get() + fieldCoeffsStride * offset;
 
    // Get limiters
    const std::size_t fieldLimitersStride = degree;
    const double *fieldLimiters = limiters;
 
    // Linear polynomial
    if (ENABLE_FAST_PATH_OPTIMIZATIONS && degree == 1) {
        do {
            // Evaluate gradients
            for (int d = 0; d < m_dimensions; ++d) {
                (*fieldGradient)[d] = fieldLimiters[0] * fieldCoeffs[1 + d];
            }
 
            // Explicitly zero unused components
            for (int d = m_dimensions; d < ReconstructionPolynomial::MAX_DIMENSIONS; ++d) {
                (*fieldGradient)[d] = 0.;
            }
 
            // Advance to the next field
            fieldCoeffs   += fieldCoeffsStride;
            fieldGradient += fieldGradientStride;
            fieldLimiters += fieldLimitersStride;
        } while (fieldGradient != fieldGradientEnd);
 
        return;
    }
 
    // Generic polynomial
    const std::vector<uint16_t> &nDegreeCoeffs = ReconstructionPolynomial::getDegreeCoefficientsCount(m_dimensions);
 
    BITPIT_CREATE_WORKSPACE(dcsi, double, static_cast<std::size_t>(m_dimensions * nDegreeCoeffs[degree]), 3 * MAX_STACK_WORKSPACE_SIZE);
    ReconstructionPolynomial::evalPointBasisDerivatives(degree, m_dimensions, m_origin, point, {{1., 0., 0.}}, dcsi);
    ReconstructionPolynomial::evalPointBasisDerivatives(degree, m_dimensions, m_origin, point, {{0., 1., 0.}}, dcsi + nDegreeCoeffs[degree]);
    if (m_dimensions == 3) {
        ReconstructionPolynomial::evalPointBasisDerivatives(degree, m_dimensions, m_origin, point, {{0., 0., 1.}}, dcsi + 2 * nDegreeCoeffs[degree]);
    }
 
    do {
        // Evaluate gradients
        for (int d = 0; d < m_dimensions; ++d) {
            const double *dcsi_dimension = dcsi + d * nDegreeCoeffs[degree];
 
            (*fieldGradient)[d] = fieldCoeffs[0] * dcsi_dimension[0];
        }
 
        int coeffEnd = nDegreeCoeffs[0];
        for (int n = 1; n <= degree; ++n) {
            double fieldsLimiter = fieldLimiters[n - 1];
 
            int coeffBegin = coeffEnd;
            coeffEnd = coeffBegin + nDegreeCoeffs[n];
            for (int i = coeffBegin; i < coeffEnd; ++i) {
                for (int d = 0; d < m_dimensions; ++d) {
                    const double *dcsi_dimension = dcsi + d * nDegreeCoeffs[degree];
 
                    (*fieldGradient)[d] += fieldsLimiter * fieldCoeffs[i] * dcsi_dimension[i];
                }
            }
        }
 
        // Explicitly zero unused components
        for (int d = m_dimensions; d < ReconstructionPolynomial::MAX_DIMENSIONS; ++d) {
            (*fieldGradient)[d] = 0.;
        }
 
        // Advance to the next field
        fieldCoeffs   += fieldCoeffsStride;
        fieldGradient += fieldGradientStride;
        fieldLimiters += fieldLimitersStride;
    } while (fieldGradient != fieldGradientEnd);
}

void ReconstructionPolynomial::display(std::ostream &out) const
{
    uint8_t dimensions = getDimensions();
 
    for (int k = 0; k < m_nFields; ++k) {
        out << " field " << k << "\n";
        for (int i = 0; i <= m_degree; ++i) {
            out << "   degree = " << i << " : " ;
 
            uint16_t nDegreeCoeffs = ReconstructionPolynomial::getDegreeCoefficientCount(i, dimensions);
            const double *degreeCoeffs = getDegreeCoefficients(i, k);
            for (int n = 0; n < nDegreeCoeffs; ++n) {
                out << degreeCoeffs[n];
                if (n != nDegreeCoeffs - 1) {
                    out << " , ";
                }
            }
 
            out << "\n";
        }
    }
}


const bool ReconstructionKernel::ENABLE_FAST_PATH_OPTIMIZATIONS = true;

const int ReconstructionKernel::MAX_STACK_WORKSPACE_SIZE = 10;

ReconstructionKernel::ReconstructionKernel()
    : m_nEquations(0), m_nCoeffs(0), m_degree(0), m_dimensions(0)
{
    initialize(0, 0, 0);
}

ReconstructionKernel::ReconstructionKernel(uint8_t degree, uint8_t dimensions, int nEquations)
    : m_nEquations(0), m_nCoeffs(0), m_degree(0), m_dimensions(0)
{
    initialize(degree, dimensions, nEquations);
}

ReconstructionKernel::ReconstructionKernel(const ReconstructionKernel &other)
    : ReconstructionKernel(other.getDegree(), other.getDimensions(), other.m_nEquations)
{
    if (m_nEquations > 0) {
        int nWeights = m_nCoeffs * m_nEquations;
        std::copy(other.m_weights.get(), other.m_weights.get() + nWeights, m_weights.get());
    }
}

ReconstructionKernel & ReconstructionKernel::operator=(const ReconstructionKernel &other)
{
    ReconstructionKernel tmp(other);
    swap(tmp);
 
    return *this;
}

void ReconstructionKernel::swap(ReconstructionKernel &other) noexcept
{
    std::swap(other.m_degree, m_degree);
    std::swap(other.m_dimensions, m_dimensions);
    std::swap(other.m_nCoeffs, m_nCoeffs);
    std::swap(other.m_nEquations, m_nEquations);
    std::swap(other.m_weights, m_weights);
}

void ReconstructionKernel::initialize(uint8_t degree, uint8_t dimensions, int nEquations, bool release)
{
    assert(degree <= ReconstructionPolynomial::MAX_DEGREE);
    m_degree = degree;
 
    assert(dimensions <= ReconstructionPolynomial::MAX_DIMENSIONS);
    m_dimensions = dimensions;
 
    int currentStorageSize = m_nCoeffs * m_nEquations;
 
    m_nCoeffs    = ReconstructionPolynomial::getCoefficientCount(m_degree, m_dimensions);
    m_nEquations = nEquations;
 
    int storageSize = m_nCoeffs * m_nEquations;
 
    bool reallocate;
    if (release) {
        reallocate = (currentStorageSize != storageSize);
    } else {
        reallocate = (currentStorageSize < storageSize);
    }
 
    if (reallocate) {
        m_weights = std::unique_ptr<double[]>(new double[storageSize]);
    }
}

uint8_t ReconstructionKernel::getDegree() const
{
    return m_degree;
}

uint8_t ReconstructionKernel::getDimensions() const
{
    return m_dimensions;
}

uint16_t ReconstructionKernel::getCoefficientCount() const
{
    return m_nCoeffs;
}

int ReconstructionKernel::getEquationCount() const
{
    return m_nEquations;
}

const double * ReconstructionKernel::getPolynomialWeights() const
{
    return m_weights.get();
}

double * ReconstructionKernel::getPolynomialWeights()
{
    return m_weights.get();
}

void ReconstructionKernel::assemblePolynomial(const std::array<double, 3> &origin,
                                              const double *values,
                                              ReconstructionPolynomial *polynomial) const
{
    uint8_t degree = getDegree();
    uint8_t dimensions = getDimensions();
 
    // Initialize the polynomial
    polynomial->initialize(degree, dimensions, origin, 1, true);
 
    // Update the polynomial
    updatePolynomial(degree, values, polynomial);
}

void ReconstructionKernel::assemblePolynomial(const std::array<double, 3> &origin,
                                              int nFields, const double **values,
                                              ReconstructionPolynomial *polynomial) const
{
    uint8_t degree = getDegree();
    uint8_t dimensions = getDimensions();
 
    // Initialize the polynomial
    polynomial->initialize(degree, dimensions, origin, nFields, true);
 
    // Update the polynomial
    updatePolynomial(degree, nFields, values, polynomial);
}

void ReconstructionKernel::assemblePolynomial(uint8_t degree, const std::array<double, 3> &origin,
                                              const double *values,
                                              ReconstructionPolynomial *polynomial) const
{
    uint8_t dimensions = getDimensions();
 
    // Initialize the polynomial
    polynomial->initialize(degree, dimensions, origin, 1, true);
 
    // Update the polynomial
    updatePolynomial(degree, values, polynomial);
}

void ReconstructionKernel::assemblePolynomial(uint8_t degree, const std::array<double, 3> &origin,
                                              int nFields, const double **values,
                                              ReconstructionPolynomial *polynomial) const
{
    uint8_t dimensions = getDimensions();
 
    // Initialize the polynomial
    polynomial->initialize(degree, dimensions, origin, nFields, true);
 
    // Update the polynomial
    updatePolynomial(degree, nFields, values, polynomial);
}

void ReconstructionKernel::updatePolynomial(const double *values,
                                            ReconstructionPolynomial *polynomial) const
{
    updatePolynomial(getDegree(), values, polynomial);
}

void ReconstructionKernel::updatePolynomial(int nFields, const double **values,
                                            ReconstructionPolynomial *polynomial) const
{
    updatePolynomial(getDegree(), nFields, values, polynomial);
}

void ReconstructionKernel::updatePolynomial(uint8_t degree, const double *values,
                                            ReconstructionPolynomial *polynomial) const
{
    assert(degree <= getDegree());
 
    int nEquations = getEquationCount();
 
    double *polynomialCoeffs = polynomial->getCoefficients();
 
    // Constant polynomial
    if (ENABLE_FAST_PATH_OPTIMIZATIONS && degree == 0) {
        // Since the polynomial weights are stored using a col-major order,
        // the weights for the constant polynomial are the first nEquation
        // values.
        const double *constantPolynomialWeights = getPolynomialWeights();
 
        polynomialCoeffs[0] = 0.;
        for (int j = 0; j < nEquations; ++j) {
            polynomialCoeffs[0] += values[j] * constantPolynomialWeights[j];
        }
 
        return;
    }
 
    // Generic polynomial
    uint8_t dimensions = getDimensions();
    int nCoeffs = ReconstructionPolynomial::getCoefficientCount(degree, dimensions);
 
    const double *polynomialWeights = getPolynomialWeights();
 
    cblas_dgemv(CBLAS_ORDER::CblasColMajor, CBLAS_TRANSPOSE::CblasTrans,
                nEquations, nCoeffs, 1., polynomialWeights, nEquations,
                values, 1, 0, polynomialCoeffs, 1);
}

void ReconstructionKernel::updatePolynomial(uint8_t degree, int nFields, const double **values,
                                            ReconstructionPolynomial *polynomial) const
{
    assert(degree <= getDegree());
 
    int nEquations = getEquationCount();
 
    // Constant polynomial
    if (ENABLE_FAST_PATH_OPTIMIZATIONS && degree == 0) {
        // Since the polynomial weights are stored using a col-major order,
        // the weights for the constant polynomial are the first nEquation
        // values.
        const double *constantPolynomialWeights = getPolynomialWeights();
        for (int k = 0; k < nFields; ++k) {
            double *fieldCoeffs = polynomial->getCoefficients(k);
 
            fieldCoeffs[0] = 0.;
            for (int j = 0; j < nEquations; ++j) {
                fieldCoeffs[0] += values[j][k] * constantPolynomialWeights[j];
            }
        }
 
        return;
    }
 
    // Generic polynomial
    BITPIT_CREATE_WORKSPACE(fieldValues, double, nEquations, MAX_STACK_WORKSPACE_SIZE);
 
    uint8_t dimensions = getDimensions();
    int nCoeffs = ReconstructionPolynomial::getCoefficientCount(degree, dimensions);
 
    const double *polynomialWeights = getPolynomialWeights();
 
    for (int k = 0; k < nFields; ++k) {
        for (int j = 0; j < nEquations; ++j) {
            fieldValues[j] = values[j][k];
        }
 
        double *fieldCoeffs = polynomial->getCoefficients(k);
 
        cblas_dgemv(CBLAS_ORDER::CblasColMajor, CBLAS_TRANSPOSE::CblasTrans,
                    nEquations, nCoeffs, 1., polynomialWeights, nEquations,
                    fieldValues, 1, 0, fieldCoeffs, 1);
    }
}

void ReconstructionKernel::computeValueWeights(const std::array<double, 3> &origin,
                                               const std::array<double, 3> &point, double *valueWeights) const
{
    computeValueLimitedWeights(getEquationCount(), getDegree(), origin, point, nullptr, valueWeights);
}

void ReconstructionKernel::computeValueWeights(uint8_t degree, const std::array<double, 3> &origin,
                                               const std::array<double, 3> &point, double *valueWeights) const
{
    computeValueLimitedWeights(getEquationCount(), degree, origin, point, nullptr, valueWeights);
}
 

void ReconstructionKernel::computeValueWeights(int nEquations, uint8_t degree, const std::array<double, 3> &origin,
                                               const std::array<double, 3> &point, double *valueWeights) const
{
   computeValueLimitedWeights(nEquations, degree, origin, point, nullptr, valueWeights);
}

void ReconstructionKernel::computeValueLimitedWeights(const std::array<double, 3> &origin, const std::array<double, 3> &point,
                                                      const double *limiters, double *valueWeights) const
{
    computeValueLimitedWeights(getEquationCount(), getDegree(), origin, point, limiters, valueWeights);
}

void ReconstructionKernel::computeValueLimitedWeights(uint8_t degree, const std::array<double, 3> &origin,
                                                      const std::array<double, 3> &point, const double *limiters,
                                                      double *valueWeights) const
{
    computeValueLimitedWeights(getEquationCount(), degree, origin, point, limiters, valueWeights);
}

void ReconstructionKernel::computeValueLimitedWeights(int nEquations, uint8_t degree, const std::array<double, 3> &origin,
                                                      const std::array<double, 3> &point, const double *limiters,
                                                      double *valueWeights) const
{
    assert(nEquations <= getEquationCount());
    assert(degree <= getDegree());
 
    // Constant polynomial
    if (ENABLE_FAST_PATH_OPTIMIZATIONS && degree == 0) {
        // Since the polynomial weights are stored using a col-major order,
        // the weights for the constant polynomial are the first nEquation
        // values.
        const double *constantPolynomialWeights = getPolynomialWeights();
        std::copy_n(constantPolynomialWeights, nEquations, valueWeights);
 
        return;
    }
 
    // Evaluate basis
    uint8_t dimensions = getDimensions();
    int nCoeffs = ReconstructionPolynomial::getCoefficientCount(degree, dimensions);
 
    BITPIT_CREATE_WORKSPACE(csi, double, nCoeffs, MAX_STACK_WORKSPACE_SIZE);
    ReconstructionPolynomial::evalPointBasisValues(degree, dimensions, origin, point, csi);
    if (limiters) {
        applyLimiter(degree, limiters, csi);
    }
 
    // Generic polynomial
    int polynomialWeightsRowCount = getEquationCount();
    const double *polynomialWeights = getPolynomialWeights();
 
    cblas_dgemv(CBLAS_ORDER::CblasColMajor, CBLAS_TRANSPOSE::CblasNoTrans,
                nEquations, nCoeffs, 1., polynomialWeights, polynomialWeightsRowCount,
                csi, 1, 0, valueWeights, 1);
}

void ReconstructionKernel::computeDerivativeWeights(const std::array<double, 3> &origin,
                                                    const std::array<double, 3> &point, const std::array<double, 3> &direction,
                                                    double *derivativeWeights) const
{
    computeDerivativeLimitedWeights(getEquationCount(), getDegree(), origin, point, direction, nullptr, derivativeWeights);
}

void ReconstructionKernel::computeDerivativeWeights(uint8_t degree, const std::array<double, 3> &origin,
                                                    const std::array<double, 3> &point, const std::array<double, 3> &direction,
                                                    double *derivativeWeights) const
{
    computeDerivativeLimitedWeights(getEquationCount(), degree, origin, point, direction, nullptr, derivativeWeights);
}

void ReconstructionKernel::computeDerivativeWeights(int nEquations, uint8_t degree, const std::array<double, 3> &origin,
                                                    const std::array<double, 3> &point, const std::array<double, 3> &direction,
                                                    double *derivativeWeights) const
{
    computeDerivativeLimitedWeights(nEquations, degree, origin, point, direction, nullptr, derivativeWeights);
}

void ReconstructionKernel::computeDerivativeLimitedWeights(const std::array<double, 3> &origin,
                                                           const std::array<double, 3> &point, const std::array<double, 3> &direction,
                                                           const double *limiters,
                                                           double *derivativeWeights) const
{
    computeDerivativeLimitedWeights(getEquationCount(), getDegree(), origin, point, direction, limiters, derivativeWeights);
}

void ReconstructionKernel::computeDerivativeLimitedWeights(uint8_t degree, const std::array<double, 3> &origin,
                                                           const std::array<double, 3> &point, const std::array<double, 3> &direction,
                                                           const double *limiters,
                                                           double *derivativeWeights) const
{
    computeDerivativeLimitedWeights(getEquationCount(), degree, origin, point, direction, limiters, derivativeWeights);
}

void ReconstructionKernel::computeDerivativeLimitedWeights(int nEquations, uint8_t degree, const std::array<double, 3> &origin,
                                                           const std::array<double, 3> &point, const std::array<double, 3> &direction,
                                                           const double *limiters,
                                                           double *derivativeWeights) const
{
    assert(nEquations <= getEquationCount());
    assert(degree <= getDegree());
 
    // Constant polynomial
    if (ENABLE_FAST_PATH_OPTIMIZATIONS && degree == 0) {
        std::fill_n(derivativeWeights, nEquations, 0.);
 
        return;
    }
 
    // Evaluate basis
    uint8_t dimensions = getDimensions();
    int nCoeffs = ReconstructionPolynomial::getCoefficientCount(degree, dimensions);
 
    BITPIT_CREATE_WORKSPACE(dcsi, double, nCoeffs, MAX_STACK_WORKSPACE_SIZE);
    ReconstructionPolynomial::evalPointBasisDerivatives(degree, dimensions, origin, point, direction, dcsi);
    if (limiters) {
        applyLimiter(degree, limiters, dcsi);
    }
 
    // Generic polynomial
    int polynomialWeightsRowCount = getEquationCount();
    const double *polynomialWeights = getPolynomialWeights();
 
    cblas_dgemv(CBLAS_ORDER::CblasColMajor, CBLAS_TRANSPOSE::CblasNoTrans,
                nEquations, nCoeffs, 1., polynomialWeights, polynomialWeightsRowCount,
                dcsi, 1, 0, derivativeWeights, 1);
}

void ReconstructionKernel::computeGradientWeights(const std::array<double, 3> &origin,
                                                  const std::array<double, 3> &point,
                                                  std::array<double, 3> *gradientWeights) const
{
    computeGradientLimitedWeights(getEquationCount(), getDegree(), origin, point, nullptr, gradientWeights);
}

void ReconstructionKernel::computeGradientWeights(uint8_t degree, const std::array<double, 3> &origin,
                                                  const std::array<double, 3> &point,
                                                  std::array<double, 3> *gradientWeights) const
{
    computeGradientLimitedWeights(getEquationCount(), degree, origin, point, nullptr, gradientWeights);
}

void ReconstructionKernel::computeGradientWeights(int nEquations, uint8_t degree, const std::array<double, 3> &origin,
                                                  const std::array<double, 3> &point,
                                                  std::array<double, 3> *gradientWeights) const
{
    computeGradientLimitedWeights(nEquations, degree, origin, point, nullptr, gradientWeights);
}

void ReconstructionKernel::computeGradientLimitedWeights(const std::array<double, 3> &origin,
                                                         const std::array<double, 3> &point, const double *limiters,
                                                         std::array<double, 3> *gradientWeights) const
{
    computeGradientLimitedWeights(getEquationCount(), getDegree(), origin, point, limiters, gradientWeights);
}

void ReconstructionKernel::computeGradientLimitedWeights(uint8_t degree, const std::array<double, 3> &origin,
                                                         const std::array<double, 3> &point, const double *limiters,
                                                         std::array<double, 3> *gradientWeights) const
{
    computeGradientLimitedWeights(getEquationCount(), degree, origin, point, limiters, gradientWeights);
}

void ReconstructionKernel::computeGradientLimitedWeights(int nEquations, uint8_t degree, const std::array<double, 3> &origin,
                                                         const std::array<double, 3> &point, const double *limiters,
                                                         std::array<double, 3> *gradientWeights) const
{
    assert(nEquations <= getEquationCount());
    assert(degree <= getDegree());
 
    // Constant polynomial
    if (ENABLE_FAST_PATH_OPTIMIZATIONS && degree == 0) {
        std::fill_n(gradientWeights, nEquations, std::array<double, 3>{0., 0., 0.});
 
        return;
    }
 
    // Generic polynomial
    uint8_t dimensions = getDimensions();
    int nCoeffs = ReconstructionPolynomial::getCoefficientCount(degree, dimensions);
 
    int polynomialWeightsRowCount = getEquationCount();
    const double *polynomialWeights = getPolynomialWeights();
 
    BITPIT_CREATE_WORKSPACE(dcsi, double, nCoeffs * dimensions, 3 * MAX_STACK_WORKSPACE_SIZE);
    for (int d = 0; d < dimensions; ++d) {
        // Select derivative direction
        std::array<double, 3> direction = {{0., 0., 0.}};
        direction[d] = 1.;
 
        // Evaluate basis derivatives
        int offset = linearalgebra::linearIndexColMajor(0, d, nCoeffs, dimensions);
 
        ReconstructionPolynomial::evalPointBasisDerivatives(degree, dimensions, origin, point, direction, dcsi + offset);
        if (limiters) {
            applyLimiter(degree, limiters, dcsi + offset);
        }
    }
 
    cblas_dgemm(CBLAS_ORDER::CblasColMajor, CBLAS_TRANSPOSE::CblasTrans, CBLAS_TRANSPOSE::CblasTrans,
                dimensions, nEquations, nCoeffs, 1., dcsi, nCoeffs, polynomialWeights, polynomialWeightsRowCount,
                0., gradientWeights[0].data(), 3);
 
    // Explicitly zero unused components
    if (dimensions != ReconstructionPolynomial::MAX_DIMENSIONS) {
        for (int j = 0; j < nEquations; ++j) {
            for (int d = dimensions; d < ReconstructionPolynomial::MAX_DIMENSIONS; ++d) {
                gradientWeights[j][d] = 0.;
            }
        }
    }
}

void ReconstructionKernel::applyLimiter(uint8_t degree, const double *limiters, double *coeffs) const
{
    if (degree < 1) {
        return;
    }
 
    uint8_t dimensions = getDimensions();
    const std::vector<uint16_t> &nDegreeCoeffs = ReconstructionPolynomial::getDegreeCoefficientsCount(dimensions);
 
    int coeffEnd = nDegreeCoeffs[0];
    for (int n = 1; n <= degree; ++n) {
        double limiterScaleFactor = limiters[n - 1];
 
        int coeffBegin = coeffEnd;
        coeffEnd = coeffBegin + nDegreeCoeffs[n];
        for (int k = coeffBegin; k < coeffEnd; ++k) {
            coeffs[k] *= limiterScaleFactor;
        }
    }
}

void ReconstructionKernel::display(std::ostream &out, double tolerance) const
{
    int nCoeffs = getCoefficientCount();
    for (int i = 0; i < nCoeffs; ++i) {
        out << i << " ";
        for (int j = 0; j < m_nEquations; ++j) {
            double weigth = m_weights[linearalgebra::linearIndexColMajor(j, i, m_nEquations, m_nCoeffs)];
            if (std::abs(weigth) < tolerance) {
                continue;
            }
 
            out << "(" << j << "," << weigth << ") ";
        }
 
        out << std::endl;
    }
}


const double ReconstructionAssembler::SVD_ZERO_THRESHOLD = 1e-14;
 

ReconstructionAssembler::ReconstructionAssembler()
{
    initialize(0, 0, false);
}

ReconstructionAssembler::ReconstructionAssembler(uint8_t degree, uint8_t dimensions)
{
    initialize(degree, dimensions, false);
}

void ReconstructionAssembler::swap(ReconstructionAssembler &other) noexcept
{
    std::swap(other.m_degree, m_degree);
    std::swap(other.m_dimensions, m_dimensions);
    std::swap(other.m_nCoeffs, m_nCoeffs);
    std::swap(other.m_constraintsOrder, m_constraintsOrder);
    std::swap(other.m_leastSquaresOrder, m_leastSquaresOrder);
    std::swap(other.m_leastSquaresScaleFactors, m_leastSquaresScaleFactors);
    std::swap(other.m_A, m_A);
    std::swap(other.m_C, m_C);
    std::swap(other.m_sigma, m_sigma);
    std::swap(other.m_U, m_U);
    std::swap(other.m_S, m_S);
    std::swap(other.m_Vt, m_Vt);
    std::swap(other.m_SVDWorkspace, m_SVDWorkspace);
    std::swap(other.m_w, m_w);
}

void ReconstructionAssembler::initialize(uint8_t degree, uint8_t dimensions, bool release)
{
    assert(degree <= ReconstructionPolynomial::MAX_DEGREE);
    m_degree = degree;
 
    assert(dimensions <= ReconstructionPolynomial::MAX_DIMENSIONS);
    m_dimensions = dimensions;
 
    m_nCoeffs = ReconstructionPolynomial::getCoefficientCount(m_degree, m_dimensions);
 
    clear(release);
}

void ReconstructionAssembler::clear(bool release)
{
    m_constraintsOrder.clear();
    m_leastSquaresOrder.clear();
    m_leastSquaresScaleFactors.clear();
 
    m_A.clear();
    m_C.clear();
 
    m_sigma.clear();
    m_U.clear();
    m_S.clear();
    m_Vt.clear();
    m_SVDWorkspace.resize(1);
    m_w.clear();
 
    if (release) {
        m_constraintsOrder.shrink_to_fit();
        m_leastSquaresOrder.shrink_to_fit();
        m_leastSquaresScaleFactors.shrink_to_fit();
 
        m_A.shrink_to_fit();
        m_C.shrink_to_fit();
 
        m_sigma.shrink_to_fit();
        m_U.shrink_to_fit();
        m_S.shrink_to_fit();
        m_Vt.shrink_to_fit();
        m_SVDWorkspace.shrink_to_fit();
        m_w.shrink_to_fit();
    }
}

uint8_t ReconstructionAssembler::getDegree() const
{
    return m_degree;
}

uint8_t ReconstructionAssembler::getDimensions() const
{
    return m_dimensions;
}

uint16_t ReconstructionAssembler::getCoefficientCount() const
{
    return m_nCoeffs;
}

int ReconstructionAssembler::countConstraints() const
{
    return m_constraintsOrder.size();
}

int ReconstructionAssembler::countLeastSquares() const
{
    return m_leastSquaresOrder.size();
}

int ReconstructionAssembler::countEquations() const
{
    int nConstraints  = countConstraints();
    int nLeastSquares = countLeastSquares();
    int nEquations    = nConstraints + nLeastSquares;
 
    return nEquations;
}

void ReconstructionAssembler::addPointValueEquation(ReconstructionType type,
                                                    const std::array<double, 3> &origin,
                                                    const std::array<double, 3> &point,
                                                    double scaleFactor)
{
    double *equationCoeffs = _addEquation(type, scaleFactor);
    ReconstructionPolynomial::evalPointBasisValues(getDegree(), getDimensions(), origin, point, equationCoeffs);
}

void ReconstructionAssembler::addPointDerivativeEquation(ReconstructionType type,
                                                         const std::array<double, 3> &origin,
                                                         const std::array<double, 3> &point,
                                                         const std::array<double, 3> &direction,
                                                         double scaleFactor)
{
    double *equationCoeffs = _addEquation(type, scaleFactor);
    ReconstructionPolynomial::evalPointBasisDerivatives(getDegree(), getDimensions(), origin, point, direction, equationCoeffs);
}

void ReconstructionAssembler::addCellAverageEquation(ReconstructionType type,
                                                     const Cell &cell,
                                                     const std::array<double, 3> &origin,
                                                     const std::array<double, 3> *vertexCoords,
                                                     double scaleFactor)
{
    double *equationCoeffs = _addEquation(type, scaleFactor);
    ReconstructionPolynomial::evalCellBasisValues(getDegree(), getDimensions(), origin, cell, vertexCoords, equationCoeffs);
}

double * ReconstructionAssembler::_addEquation(ReconstructionType type, double scaleFactor)
{
    // Update equation information
    int nEquations = countEquations();
    switch (type) {
 
    case TYPE_CONSTRAINT:
        m_constraintsOrder.emplace_back(nEquations);
        break;
 
    case TYPE_LEAST_SQUARE:
        m_leastSquaresOrder.emplace_back(nEquations);
        m_leastSquaresScaleFactors.emplace_back(scaleFactor);
        break;
 
    }
 
    // Prepare storage for equation coefficients
    int nCoeffs = getCoefficientCount();
 
    double *equationCoeffsStorage = nullptr;
    switch (type) {
 
    case TYPE_CONSTRAINT:
        m_C.resize(m_C.size() + nCoeffs);
        equationCoeffsStorage = m_C.data() + m_C.size() - nCoeffs;
        break;
 
    case TYPE_LEAST_SQUARE:
        m_A.resize(m_A.size() + nCoeffs);
        equationCoeffsStorage = m_A.data() + m_A.size() - nCoeffs;
        break;
 
    }
 
    return equationCoeffsStorage;
}

void ReconstructionAssembler::assembleKernel(ReconstructionKernel *kernel) const
{
    // Initialize reconstruction kernel
    uint8_t degree     = getDegree();
    uint8_t dimensions = getDimensions();
 
    int nEquations = countEquations();
 
    kernel->initialize(degree, dimensions, nEquations, true);
 
    // Update the kernel
    updateKernel(kernel);
}

void ReconstructionAssembler::updateKernel(ReconstructionKernel *kernel) const
{
    // Get the number of equations
    int nConstraints  = countConstraints();
    int nLeastSquares = countLeastSquares();
    int nEquations    = countEquations();
 
    // Get the number of polynomial coefficients
    int nCoeffs = getCoefficientCount();
 
    // Evaluate normalized least square scale factors
    if (nLeastSquares > 0) {
        double maxLeastSquareScaleFactor = std::abs(m_leastSquaresScaleFactors[0]);
        for (int k = 1; k < nLeastSquares; ++k) {
            maxLeastSquareScaleFactor = std::max(std::abs(m_leastSquaresScaleFactors[k]), maxLeastSquareScaleFactor);
        }
 
        m_w.resize(nLeastSquares);
        for (int k = 0; k < nLeastSquares; ++k) {
            m_w[k] = m_leastSquaresScaleFactors[k] / maxLeastSquareScaleFactor;
        }
    }
 
    // The linear-constrained are introduced in the least-squares problem
    // through Lagrange multipliers. The resulting linear system is:
    //
    // | A^t A w  C^t | |x     | = |A^t w b|
    // | C        0   | |lambda|   |d      |
    //
    // with A and C the least-squares and constraints equations respectively,
    // and b and d their corresponding RHSs. x are the coefficients of
    // the polynomial and lambda the lagrange multipliers. w are the normalized
    // east square scale factors.
    //
    // This system is denoted by S:
    //
    //     |  x   |   |A^t w  0| |b|
    // |S| |      | = |        | | |
    //     |lambda|   |0      I| |d|
    //
    // The matrices S and S^-1 are symmetric and only the upper portions are
    // computed
    int nUnknowns = nCoeffs + nConstraints;
 
    m_S.resize(nUnknowns * nUnknowns);
    for (int i = 0; i < nCoeffs; ++i) {
        for (int j = i; j < nCoeffs; ++j) {
            // Compute A^t A on the fly
            double ATA_ij = 0.;
            for (int k = 0; k < nLeastSquares; ++k) {
                int A_ki_idx = linearalgebra::linearIndexRowMajor(k, i, nLeastSquares, nCoeffs);
                int A_kj_idx = linearalgebra::linearIndexRowMajor(k, j, nLeastSquares, nCoeffs);
                ATA_ij += m_A[A_ki_idx] * m_A[A_kj_idx] * m_w[k];
            }
 
            int l = linearalgebra::linearIndexColMajor(i, j, nUnknowns, nUnknowns);
            m_S[l] = ATA_ij;
 
            int m = linearalgebra::linearIndexColMajor(j, i, nUnknowns, nUnknowns);
            m_S[m] = ATA_ij;
        }
 
        for (int j = nCoeffs; j < nUnknowns; ++j) {
            int l     = linearalgebra::linearIndexColMajor(i, j, nUnknowns, nUnknowns);
            int C_idx = linearalgebra::linearIndexRowMajor(j - nCoeffs, i, nConstraints, nCoeffs);
            m_S[l] = m_C[C_idx];
 
            int m = linearalgebra::linearIndexColMajor(j, i, nUnknowns, nUnknowns);
            m_S[m] = m_S[l];
        }
    }
 
    // Compute inverse S matrix
    // Since S may me be rank-deficit (eg if not enough neighbours are available)
    // the pseudo-inverse is used. This corresponds of computing the least-norm
    // solution of the problem.
    computePseudoInverse(nUnknowns, nUnknowns, SVD_ZERO_THRESHOLD, m_S.data());
 
    // Weights needed to evaluate the polynomial coefficients come from the
    // following equation:
    //
    // |  x   |        |A^t w  0| |b|          |b|
    // |      | = S^-1 |        | | | = S^-1 Q | |
    // |lambda|        |0      I| |d|          |d|
    //
    // Since we are interested only in x (the polynomial coefficients) only
    // the first nCoeffs rows of the matrix S^-1 Q are computed. Those values
    // are the polynomial weights.
    //
    // Weights are stored according the order in which the equations have been
    // added.
    double *weights = kernel->getPolynomialWeights();
    for (int j = 0; j < nEquations; ++j) {
        int equation;
        if (j < nLeastSquares) {
            equation = m_leastSquaresOrder[j];
        } else {
            equation = m_constraintsOrder[j - nLeastSquares];
        }
 
        for (int i = 0; i < nCoeffs; ++i) {
            double value = 0;
            for (int k = 0; k < nUnknowns; ++k) {
                int l = linearalgebra::linearIndexColMajorSymmetric(i, k, nUnknowns, nUnknowns, 'U');
                if (k < nCoeffs && j < nLeastSquares) {
                    int A_jk_idx = linearalgebra::linearIndexRowMajor(j, k, nLeastSquares, nCoeffs);
                    value += m_S[l] * m_A[A_jk_idx] * m_w[j];
                } else if ((k - nCoeffs) == (j - nLeastSquares)) {
                    value += m_S[l];
                }
            }
 
            int weightLineraIndex = linearalgebra::linearIndexColMajor(equation, i, nEquations, nCoeffs);
            weights[weightLineraIndex] = value;
        }
    }
}

void ReconstructionAssembler::computePseudoInverse(int m, int n, double zeroThreshold, double *A) const
{
    // Compute SVD
    //
    // A = U * Sigma * Vt (Equation 21)
    int k = std::min(m,n);
 
    m_sigma.resize(k);
    m_U.resize(m * k);
    m_Vt.resize(k * n);
 
    char jobU  = 'S';
    char jobVT = 'S';
 
    int workspaceSize = -1;
 
    int info;
 
    info = LAPACKE_dgesvd_work(LAPACK_COL_MAJOR, jobU, jobVT, m, n, A, m, m_sigma.data(), m_U.data(), m, m_Vt.data(), k,
                  m_SVDWorkspace.data(), workspaceSize);
 
    workspaceSize = static_cast<int>(m_SVDWorkspace[0]);
    if (workspaceSize > (int) m_SVDWorkspace.size()) {
        m_SVDWorkspace.resize(workspaceSize);
    }
 
    info = LAPACKE_dgesvd_work(LAPACK_COL_MAJOR, jobU, jobVT, m, n, A, m, m_sigma.data(), m_U.data(), m, m_Vt.data(), k,
                  m_SVDWorkspace.data(), workspaceSize);
 
    if (info > 0) {
        log::cout() << "SVD failed in ReconstructionAssembler::computePseudoInverse()" <<std::endl;
        exit(1);
    }
 
    // Inv(A) = V * Sigma^ + *U^T (Equation 22)
    //
    // u = sigma^ + *U
    // and is stored in U
    //
    // Sigma are singular values, hence they are non-negative by definition.
    // To check if a signular value is non zero, we only need to check if it's
    // greater than the defined threshold.
    for (int i = 0; i < k; ++i) {
        double sigma_plus = (m_sigma[i] > zeroThreshold) ? (1. / m_sigma[i]) : 0.;
        cblas_dscal(m, sigma_plus, &m_U[i*m], 1);
    }
 
    // Inv(A) = (Vt)^T * u^T
    cblas_dgemm(CBLAS_ORDER::CblasColMajor, CBLAS_TRANSPOSE::CblasTrans, CBLAS_TRANSPOSE::CblasTrans,
                n, m, k, 1., m_Vt.data(), k, m_U.data(), m, 0., A, n);
}


Reconstruction::Reconstruction(uint8_t degree, uint8_t dimensions)
    : ReconstructionAssembler(degree, dimensions)
{
}

void Reconstruction::swap(Reconstruction &other) noexcept
{
    ReconstructionKernel::swap(other);
    ReconstructionAssembler::swap(other);
}

void Reconstruction::initialize(uint8_t degree, uint8_t dimensions, bool release)
{
    ReconstructionAssembler::initialize(degree, dimensions, release);
}

void Reconstruction::clear(bool release)
{
    ReconstructionAssembler::clear(release);
 
    if (release) {
        ReconstructionKernel().swap(*this);
    }
}

void Reconstruction::assemble()
{
    if (ReconstructionKernel::getEquationCount() != ReconstructionAssembler::countEquations()) {
        ReconstructionAssembler::assembleKernel(this);
    } else {
        ReconstructionAssembler::updateKernel(this);
    }
}
 
}