/** \file   mri_core_kspace_filter.cpp
    \brief  Implementation kspace filter functionalities for 2D and 3D MRI parallel imaging
    \author Hui Xue
*/

#include "mri_core_kspace_filter.h"
#include "hoNDArray_elemwise.h"
#include <boost/algorithm/string.hpp>

#ifdef M_PI
    #undef M_PI
#endif // M_PI
#define M_PI 3.14159265358979323846

namespace Gadgetron
{

ISMRMRDKSPACEFILTER get_kspace_filter_type(const std::string& name)
{
    std::string name_lower(name);
    boost::algorithm::to_lower(name_lower);

    if (name_lower == "gaussian")
    {
        return ISMRMRD_FILTER_GAUSSIAN;
    }
    else if (name_lower == "hanning")
    {
        return ISMRMRD_FILTER_HANNING;
    }
    else if (name_lower == "taperedhanning")
    {
        return ISMRMRD_FILTER_TAPERED_HANNING;
    }
    else if (name_lower == "none")
    {
        return ISMRMRD_FILTER_NONE;
    }

    GERROR_STREAM("Unrecognized kspace filter name : " << name);

    return ISMRMRD_FILTER_NONE;
}

std::string get_kspace_filter_name(ISMRMRDKSPACEFILTER v)
{
    std::string name;

    switch (v)
    {
        case ISMRMRD_FILTER_GAUSSIAN:
        {
            name = "Gaussian";
            break;
        }

        case ISMRMRD_FILTER_HANNING:
        {
            name = "Hanning";
            break;
        }

        case ISMRMRD_FILTER_TAPERED_HANNING:
        {
            name = "TaperedHanning";
            break;
        }

        case ISMRMRD_FILTER_NONE:
        {
            name = "none";
            break;
        }

        default:
        {
            GERROR_STREAM("Unrecognized kspace filter type : " << v);
            name = "none";
        }
    }

    return name;
}

template<typename T>
void generate_symmetric_filter(size_t len, hoNDArray<T>& filter, ISMRMRDKSPACEFILTER filterType, double sigma, size_t width)
{
    try
    {
        if (len == 0) return;

        filter.create(len);

        if (width == 0 || width >= len) width = 1;

        size_t ii;
        if (filterType == ISMRMRD_FILTER_GAUSSIAN)
        {
            double r = -1.0*sigma*sigma / 2;

            if (len % 2 == 0)
            {
                // to make sure the zero points match and boundary of filters are symmetric
                double stepSize = 2.0 / (len - 2);
                std::vector<double> x(len - 1);

                for (ii = 0; ii<len - 1; ii++)
                {
                    x[ii] = -1 + ii*stepSize;
                }

                for (ii = 0; ii<len - 1; ii++)
                {
                    filter(ii + 1) = T(std::exp(r*(x[ii] * x[ii])));
                }

                filter(0) = T(0);
            }
            else
            {
                double stepSize = 2.0 / (len - 1);
                std::vector<double> x(len);

                for (ii = 0; ii<len; ii++)
                {
                    x[ii] = -1 + ii*stepSize;
                }

                for (ii = 0; ii<len; ii++)
                {
                    filter(ii) = T(std::exp(r*(x[ii] * x[ii])));
                }
            }
        }
        else if (filterType == ISMRMRD_FILTER_TAPERED_HANNING)
        {
            hoNDArray<T> w(width);

            for (ii = 1; ii <= width; ii++)
            {
                w(ii - 1) = T((0.5 * (1 - std::cos(2.0*M_PI*ii / (2 * width + 1)))));
            }

            // make sure the center of the filter will end up being 1:
            Gadgetron::fill(filter, T(1.0));

            if (len % 2 == 0)
            {
                for (ii = 1; ii <= width; ii++)
                {
                    filter(ii) = w(ii - 1);
                    filter(len - ii) = filter(ii);
                }

                filter(0) = T(0);
            }
            else
            {
                for (ii = 1; ii <= width; ii++)
                {
                    filter(ii - 1) = w(ii - 1);
                    filter(len - ii) = filter(ii - 1);
                }
            }
        }
        else if (filterType == ISMRMRD_FILTER_HANNING)
        {
            if (len % 2 == 0)
            {
                size_t N = len - 1;
                double halfLen = (double)((N + 1) / 2);
                for (ii = 1; ii <= halfLen; ii++)
                {
                    filter(ii) = T((0.5 * (1 - std::cos(2.0*M_PI*ii / (N + 1)))));
                }

                for (ii = (size_t)halfLen; ii<N; ii++)
                {
                    filter(ii + 1) = filter(N - ii);
                }

                filter(0) = T(0);
            }
            else
            {
                double halfLen = (double)((len + 1) / 2);
                for (ii = 1; ii <= (size_t)halfLen; ii++)
                {
                    filter(ii - 1) = T((0.5 * (1 - std::cos(2.0*M_PI*ii / (len + 1)))));
                }

                for (ii = (size_t)halfLen; ii<len; ii++)
                {
                    filter(ii) = filter(len - 1 - ii);
                }
            }
        }
        else if (filterType == ISMRMRD_FILTER_NONE)
        {
            Gadgetron::fill(filter, T(1.0));
        }
        else
        {
            GADGET_THROW("generate_symmetric_filter, unrecognized fiter type ... ");
        }

        T sos = 0.0f;
        for (ii = 0; ii<len; ii++)
        {
            sos += filter(ii)*filter(ii);
        }

        T r = T(1.0 / std::sqrt(std::abs(sos) / (len)));
        for (ii = 0; ii<len; ii++)
        {
            filter(ii) *= r;
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in generate_symmetric_filter(...) ... ");
    }
}

template EXPORTMRICORE void generate_symmetric_filter(size_t len, hoNDArray<float>& filter, ISMRMRDKSPACEFILTER filterType, double sigma, size_t width);
template EXPORTMRICORE void generate_symmetric_filter(size_t len, hoNDArray<double>& filter, ISMRMRDKSPACEFILTER filterType, double sigma, size_t width);
template EXPORTMRICORE void generate_symmetric_filter(size_t len, hoNDArray< std::complex<float> >& filter, ISMRMRDKSPACEFILTER filterType, double sigma, size_t width);
template EXPORTMRICORE void generate_symmetric_filter(size_t len, hoNDArray< std::complex<double> >& filter, ISMRMRDKSPACEFILTER filterType, double sigma, size_t width);

// ------------------------------------------------------------------------

template<typename T>
void generate_asymmetric_filter(size_t len, size_t start, size_t end, hoNDArray<T>& filter, ISMRMRDKSPACEFILTER filterType, size_t width, bool densityComp)
{
    try
    {
        if (len == 0) return;

        if (start > len - 1) start = 0;
        if (end > len - 1) end = len - 1;

        if (start > end)
        {
            start = 0;
            end = len - 1;
        }

        filter.create(len);
        Gadgetron::clear(filter);

        size_t ii;
        for (ii = start; ii <= end; ii++)
        {
            filter(ii) = T(1.0);
        }

        if (width == 0 || width >= len) width = 1;

        hoNDArray<T> w(width);

        if (filterType == ISMRMRD_FILTER_TAPERED_HANNING)
        {
            for (ii = 1; ii <= width; ii++)
            {
                w(ii - 1) = T((0.5 * (1 - std::cos(2.0*M_PI*ii / (2 * width + 1)))));
            }
        }
        else if (filterType == ISMRMRD_FILTER_NONE)
        {
            Gadgetron::fill(w, T(1.0));
        }
        else
        {
            GADGET_THROW("generate_symmetric_filter, unrecognized fiter type ... ");
        }

        if (densityComp)
        {
            size_t startSym(0), endSym(len - 1);
            find_symmetric_sampled_region(start, end, len / 2, startSym, endSym);

            if (start == 0 && end == len - 1)
            {
                for (ii = 1; ii <= width; ii++)
                {
                    filter(ii - 1) = w(ii - 1);
                    filter(len - ii) = filter(ii - 1);
                }
            }

            if (start == 0 && end<len - 1)
            {
                for (ii = 0; ii<startSym; ii++)
                {
                    filter(ii) = 2.0;
                }

                for (ii = 1; ii <= width; ii++)
                {
                    filter(ii - 1 + startSym) = T(1.0) + w(width - ii);
                    filter(end - ii + 1) = w(ii - 1);
                }
            }

            if (start>0 && end == len - 1)
            {
                for (ii = endSym + 1; ii<len; ii++)
                {
                    filter(ii) = 2.0;
                }

                for (ii = 1; ii <= width; ii++)
                {
                    filter(endSym - ii + 1) = T(1.0) + w(width - ii);
                    filter(start + ii - 1) = w(ii - 1);
                }
            }

            if (start>0 && end<len - 1)
            {
                if (start == startSym && end == endSym)
                {
                    for (ii = 1; ii <= width; ii++)
                    {
                        filter(start + ii - 1) = w(ii - 1);
                        filter(end - ii + 1) = w(ii - 1);
                    }
                }
                else if (start == startSym && end>endSym)
                {
                    for (ii = endSym + 1; ii <= end; ii++)
                    {
                        filter(ii) = 2.0;
                    }

                    for (ii = 1; ii <= width; ii++)
                    {
                        filter(end - ii + 1) = T(1.0) + w(ii - 1);
                        filter(endSym - ii + 1) = w(width - ii);
                        filter(start + ii - 1) = w(ii - 1);
                    }
                }
                else if (start<startSym && end == endSym)
                {
                    for (ii = start; ii<startSym; ii++)
                    {
                        filter(ii) = 2.0;
                    }

                    for (ii = 1; ii <= width; ii++)
                    {
                        filter(ii - 1 + start) = T(1.0) + w(ii - 1);
                        filter(ii - 1 + startSym) = w(width - ii);
                        filter(end - ii + 1) = w(ii - 1);
                    }
                }
                else
                {
                    for (ii = 1; ii <= width; ii++)
                    {
                        filter(start + ii - 1) = w(ii - 1);
                        filter(end - ii + 1) = w(ii - 1);
                    }
                }
            }
        }
        else
        {
            if (start == 0 && end == len - 1)
            {
                for (ii = 1; ii <= width; ii++)
                {
                    filter(ii - 1) = w(ii - 1);
                    filter(len - ii) = filter(ii - 1);
                }
            }

            if (start == 0 && end<len - 1)
            {
                for (ii = 1; ii <= width; ii++)
                {
                    filter(end - ii + 1) = w(ii - 1);
                }
            }

            if (start>0 && end == len - 1)
            {
                for (ii = 1; ii <= width; ii++)
                {
                    filter(start + ii - 1) = w(ii - 1);
                }
            }

            if (start>0 && end<len - 1)
            {
                for (ii = 1; ii <= width; ii++)
                {
                    filter(start + ii - 1) = w(ii - 1);
                    filter(end - ii + 1) = w(ii - 1);
                }
            }
        }

        T sos = 0.0f;
        for (ii = 0; ii<len; ii++)
        {
            sos += filter(ii)*filter(ii);
        }

        T r = (T)(1.0 / std::sqrt(std::abs(sos) / (end - start + 1))); // SNR unit filter
        for (ii = 0; ii<len; ii++)
        {
            filter(ii) *= r;
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in generate_asymmetric_filter(...) ... ");
    }
}

template EXPORTMRICORE void generate_asymmetric_filter(size_t len, size_t start, size_t end, hoNDArray<float>& filter, ISMRMRDKSPACEFILTER filterType, size_t width, bool densityComp);
template EXPORTMRICORE void generate_asymmetric_filter(size_t len, size_t start, size_t end, hoNDArray<double>& filter, ISMRMRDKSPACEFILTER filterType, size_t width, bool densityComp);
template EXPORTMRICORE void generate_asymmetric_filter(size_t len, size_t start, size_t end, hoNDArray< std::complex<float> >& filter, ISMRMRDKSPACEFILTER filterType, size_t width, bool densityComp);
template EXPORTMRICORE void generate_asymmetric_filter(size_t len, size_t start, size_t end, hoNDArray< std::complex<double> >& filter, ISMRMRDKSPACEFILTER filterType, size_t width, bool densityComp);

// ------------------------------------------------------------------------

template<typename T>
void generate_symmetric_filter_ref(size_t len, size_t start, size_t end, hoNDArray<T>& filter)
{
    try
    {
        GADGET_CHECK_THROW(len >= 2);
        GADGET_CHECK_THROW(start >= 0 && end <= len - 1 && start <= end);

        if (start == 0 && end == len - 1)
        {
            generate_symmetric_filter(len, filter, ISMRMRD_FILTER_HANNING);
            return;
        }

        size_t centerInd = len / 2;

        size_t lenFilter(0); // make a symmetric filter with zero at the center
        size_t lenFilterEnd = 2 * (end - centerInd) + 1;
        size_t lenFilterStart = 2 * (centerInd - start) + 1;

        if (start == 0 && end<len - 1)
        {
            lenFilter = lenFilterEnd;
        }
        else if (start>0 && end == len - 1)
        {
            lenFilter = lenFilterStart;
        }
        else if (start>0 && end<len - 1)
        {
            lenFilter = ((lenFilterStart<lenFilterEnd) ? lenFilterStart : lenFilterEnd);
        }
        else
        {
            GERROR_STREAM("generate_symmetric_filter_ref, invalid inputs : start - end - len ... " << start << " " << end << " " << len);
            GADGET_THROW("generate_symmetric_filter_ref, invalid inputs ... ");
        }

        GADGET_CHECK_THROW(lenFilter>0);

        hoNDArray<T> filterSym(lenFilter);
        generate_symmetric_filter(lenFilter, filterSym, ISMRMRD_FILTER_HANNING);

        filter.create(len);
        Gadgetron::clear(&filter);

        if (start == 0 && end<len - 1)
        {
            memcpy(filter.begin() + end - lenFilter + 1, filterSym.begin(), filterSym.get_number_of_bytes());
            return;
        }
        else if (start>0 && end == len - 1)
        {
            memcpy(filter.begin() + start, filterSym.begin(), filterSym.get_number_of_bytes());
            return;
        }
        else if (start>0 && end<len - 1)
        {
            if (lenFilter == lenFilterStart)
            {
                memcpy(filter.begin() + start, filterSym.begin(), filterSym.get_number_of_bytes());
            }
            else
            {
                memcpy(filter.begin() + end - lenFilter + 1, filterSym.begin(), filterSym.get_number_of_bytes());
            }

            return;
        }
        else
        {
            GERROR_STREAM("Invalid inputs : start - end - len : " << start << " " << end << " " << len);
            GADGET_THROW("generate_symmetric_filter_ref, invalid inputs : start - end - len");
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in generate_symmetric_filter_ref(...) ... ");
    }
}

template EXPORTMRICORE void generate_symmetric_filter_ref(size_t len, size_t start, size_t end, hoNDArray<float>& filter);
template EXPORTMRICORE void generate_symmetric_filter_ref(size_t len, size_t start, size_t end, hoNDArray<double>& filter);
template EXPORTMRICORE void generate_symmetric_filter_ref(size_t len, size_t start, size_t end, hoNDArray< std::complex<float> >& filter);
template EXPORTMRICORE void generate_symmetric_filter_ref(size_t len, size_t start, size_t end, hoNDArray< std::complex<double> >& filter);

// ------------------------------------------------------------------------

template <typename T>
void compute_2d_filter(const hoNDArray<T>& fx, const hoNDArray<T>& fy, hoNDArray<T>& fxy)
{
    try
    {
        size_t RO = fx.get_size(0);
        size_t E1 = fy.get_size(0);

        fxy.create(RO, E1);
        T* pFxy = fxy.begin();

        size_t x, y;

        for (y = 0; y<E1; y++)
        {
            for (x = 0; x<RO; x++)
            {
                pFxy[y*RO + x] = fx(x) * fy(y);
            }
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in compute_2d_filter(...) ... ");
    }
}

template EXPORTMRICORE void compute_2d_filter(const hoNDArray<float>& fx, const hoNDArray<float>& fy, hoNDArray<float>& fxy);
template EXPORTMRICORE void compute_2d_filter(const hoNDArray<double>& fx, const hoNDArray<double>& fy, hoNDArray<double>& fxy);
template EXPORTMRICORE void compute_2d_filter(const hoNDArray< std::complex<float> >& fx, const hoNDArray< std::complex<float> >& fy, hoNDArray< std::complex<float> >& fxy);
template EXPORTMRICORE void compute_2d_filter(const hoNDArray< std::complex<double> >& fx, const hoNDArray< std::complex<double> >& fy, hoNDArray< std::complex<double> >& fxy);

// ------------------------------------------------------------------------

void compute_2d_filter(const hoNDArray<float>& fx, const hoNDArray<float>& fy, hoNDArray< std::complex<float> >& fxy)
{
    try
    {
        size_t RO = fx.get_size(0);
        size_t E1 = fy.get_size(0);

        fxy.create(RO, E1);
        std::complex<float> * pFxy = fxy.begin();

        size_t x, y;

        for (y = 0; y<E1; y++)
        {
            for (x = 0; x<RO; x++)
            {
                pFxy[y*RO + x] = std::complex<float>(fx(x) * fy(y));
            }
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in compute_2d_filter(float) ... ");
    }
}

// ------------------------------------------------------------------------

void compute_2d_filter(const hoNDArray<double>& fx, const hoNDArray<double>& fy, hoNDArray< std::complex<double> >& fxy)
{
    try
    {
        size_t RO = fx.get_size(0);
        size_t E1 = fy.get_size(0);

        fxy.create(RO, E1);
        std::complex<double> * pFxy = fxy.begin();

        size_t x, y;

        for (y = 0; y<E1; y++)
        {
            for (x = 0; x<RO; x++)
            {
                pFxy[y*RO + x] = std::complex<double>(fx(x) * fy(y));
            }
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in compute_2d_filter(double) ... ");
    }
}

// ------------------------------------------------------------------------

template <typename T>
void compute_3d_filter(const hoNDArray<T>& fx, const hoNDArray<T>& fy, const hoNDArray<T>& fz, hoNDArray<T>& fxyz)
{
    try
    {
        size_t RO = fx.get_size(0);
        size_t E1 = fy.get_size(0);
        size_t E2 = fz.get_size(0);

        fxyz.create(RO, E1, E2);
        T* pFxyz = fxyz.begin();

        const T* px = fx.begin();
        const T* py = fy.begin();
        const T* pz = fz.begin();

        size_t x, y, z;

        T vz, vy, vx;

        for (z = 0; z<E2; z++)
        {
            vz = pz[z];
            for (y = 0; y<E1; y++)
            {
                vy = py[y];
                for (x = 0; x<RO; x++)
                {
                    vx = px[x];
                    pFxyz[x+y*RO+z*RO*E1] = (vx*vz*vy);
                }
            }
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in compute_3d_filter(...) ... ");
    }
}

template EXPORTMRICORE void compute_3d_filter(const hoNDArray<float>& fx, const hoNDArray<float>& fy, const hoNDArray<float>& fz, hoNDArray<float>& fxyz);
template EXPORTMRICORE void compute_3d_filter(const hoNDArray<double>& fx, const hoNDArray<double>& fy, const hoNDArray<double>& fz, hoNDArray<double>& fxyz);
template EXPORTMRICORE void compute_3d_filter(const hoNDArray< std::complex<float> >& fx, const hoNDArray< std::complex<float> >& fy, const hoNDArray< std::complex<float> >& fz, hoNDArray< std::complex<float> >& fxyz);
template EXPORTMRICORE void compute_3d_filter(const hoNDArray< std::complex<double> >& fx, const hoNDArray< std::complex<double> >& fy, const hoNDArray< std::complex<double> >& fz, hoNDArray< std::complex<double> >& fxyz);

// ------------------------------------------------------------------------

void compute_3d_filter(const hoNDArray<float>& fx, const hoNDArray<float>& fy, const hoNDArray<float>& fz, hoNDArray< std::complex<float> >& fxyz)
{
    try
    {
        size_t RO = fx.get_size(0);
        size_t E1 = fy.get_size(0);
        size_t E2 = fz.get_size(0);

        fxyz.create(RO, E1, E2);
        std::complex<float> * pFxyz = fxyz.begin();

        size_t x, y, z;

        for (z = 0; z<E2; z++)
        {
            for (y = 0; y<E1; y++)
            {
                for (x = 0; x<RO; x++)
                {
                    pFxyz[z*RO*E1 + y*RO + x] = std::complex<float>(fx(x)*fy(y)*fz(z));
                }
            }
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in compute_3d_filter(float) ... ");
    }
}

// ------------------------------------------------------------------------

void compute_3d_filter(const hoNDArray<double>& fx, const hoNDArray<double>& fy, const hoNDArray<double>& fz, hoNDArray< std::complex<double> >& fxyz)
{
    try
    {
        size_t RO = fx.get_size(0);
        size_t E1 = fy.get_size(0);
        size_t E2 = fz.get_size(0);

        fxyz.create(RO, E1, E2);
        std::complex<double> * pFxyz = fxyz.begin();

        size_t x, y, z;

        for (z = 0; z<E2; z++)
        {
            for (y = 0; y<E1; y++)
            {
                for (x = 0; x<RO; x++)
                {
                    pFxyz[z*RO*E1 + y*RO + x] = std::complex<double>(fx(x)*fy(y)*fz(z));
                }
            }
        }
    }
    catch (...)
    {
        GADGET_THROW("Errors in compute_3d_filter(double) ... ");
    }
}

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_RO(hoNDArray<T>& data, const hoNDArray<T>& fRO)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(0) == fRO.get_number_of_elements());
        Gadgetron::multiply(data, fRO, data);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_RO(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_RO(hoNDArray<float>& data, const hoNDArray<float>& fRO);
template EXPORTMRICORE void apply_kspace_filter_RO(hoNDArray<double>& data, const hoNDArray<double>& fRO);
template EXPORTMRICORE void apply_kspace_filter_RO(hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fRO);
template EXPORTMRICORE void apply_kspace_filter_RO(hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fRO);

template <typename T>
void apply_kspace_filter_RO(const hoNDArray<T>& data, const hoNDArray<T>& fRO, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(0) == fRO.get_number_of_elements());
        Gadgetron::multiply(data, fRO, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_RO(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_RO(const hoNDArray<float>& data, const hoNDArray<float>& fRO, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_RO(const hoNDArray<double>& data, const hoNDArray<double>& fRO, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_RO(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fRO, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_RO(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fRO, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_E1(const hoNDArray<T>& data, const hoNDArray<T>& fE1, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(1) == fE1.get_number_of_elements());

        hoNDArray<T> fRO(data.get_size(0));
        fRO.fill(T(1.0));

        hoNDArray<T> fxy;
        compute_2d_filter(fRO, fE1, fxy);

        Gadgetron::multiply(data, fxy, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_E1(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_E1(const hoNDArray<float>& data, const hoNDArray<float>& fE1, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E1(const hoNDArray<double>& data, const hoNDArray<double>& fE1, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E1(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fE1, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E1(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fE1, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_ROE1(const hoNDArray<T>& data, const hoNDArray<T>& fROE1, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(0) == fROE1.get_size(0));
        GADGET_CHECK_THROW(data.get_size(1) == fROE1.get_size(1));

        Gadgetron::multiply(data, fROE1, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_ROE1(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_ROE1(const hoNDArray<float>& data, const hoNDArray<float>& fROE1, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1(const hoNDArray<double>& data, const hoNDArray<double>& fROE1, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fROE1, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fROE1, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_ROE1(const hoNDArray<T>& data, const hoNDArray<T>& fRO, const hoNDArray<T>& fE1, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(0) == fRO.get_size(0));
        GADGET_CHECK_THROW(data.get_size(1) == fE1.get_size(0));

        hoNDArray<T> fROE1;
        compute_2d_filter(fRO, fE1, fROE1);

        Gadgetron::multiply(data, fROE1, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_ROE1(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_ROE1(const hoNDArray<float>& data, const hoNDArray<float>& fRO, const hoNDArray<float>& fE1, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1(const hoNDArray<double>& data, const hoNDArray<double>& fRO, const hoNDArray<double>& fE1, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fRO, const hoNDArray< std::complex<float> >& fE1, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fRO, const hoNDArray< std::complex<double> >& fE1, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_E2(const hoNDArray<T>& data, const hoNDArray<T>& fE2, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(2) == fE2.get_number_of_elements());

        hoNDArray<T> fRO(data.get_size(0));
        fRO.fill(T(1.0));

        hoNDArray<T> fE1(data.get_size(1));
        fE1.fill(T(1.0));

        hoNDArray<T> fxyz;
        compute_3d_filter(fRO, fE1, fE2, fxyz);

        Gadgetron::multiply(data, fxyz, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_E2(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_E2(const hoNDArray<float>& data, const hoNDArray<float>& fE2, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E2(const hoNDArray<double>& data, const hoNDArray<double>& fE2, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E2(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fE2, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E2(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fE2, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_ROE2(const hoNDArray<T>& data, const hoNDArray<T>& fRO, const hoNDArray<T>& fE2, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(0) == fRO.get_number_of_elements());
        GADGET_CHECK_THROW(data.get_size(2) == fE2.get_number_of_elements());

        hoNDArray<T> fE1(data.get_size(1));
        fE1.fill(T(1.0));

        hoNDArray<T> fxyz;
        compute_3d_filter(fRO, fE1, fE2, fxyz);

        Gadgetron::multiply(data, fxyz, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_ROE2(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_ROE2(const hoNDArray<float>& data, const hoNDArray<float>& fRO, const hoNDArray<float>& fE2, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE2(const hoNDArray<double>& data, const hoNDArray<double>& fRO, const hoNDArray<double>& fE2, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE2(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fRO, const hoNDArray< std::complex<float> >& fE2, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE2(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fRO, const hoNDArray< std::complex<double> >& fE2, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_E1E2(const hoNDArray<T>& data, const hoNDArray<T>& fE1, const hoNDArray<T>& fE2, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(1) == fE1.get_number_of_elements());
        GADGET_CHECK_THROW(data.get_size(2) == fE2.get_number_of_elements());

        hoNDArray<T> fRO(data.get_size(0));
        fRO.fill(T(1.0));

        hoNDArray<T> fxyz;
        compute_3d_filter(fRO, fE1, fE2, fxyz);

        Gadgetron::multiply(data, fxyz, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_E1E2(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_E1E2(const hoNDArray<float>& data, const hoNDArray<float>& fE1, const hoNDArray<float>& fE2, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E1E2(const hoNDArray<double>& data, const hoNDArray<double>& fE1, const hoNDArray<double>& fE2, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E1E2(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fE1, const hoNDArray< std::complex<float> >& fE2, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_E1E2(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fE1, const hoNDArray< std::complex<double> >& fE2, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_ROE1E2(const hoNDArray<T>& data, const hoNDArray<T>& fROE1E2, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(0) == fROE1E2.get_size(0));
        GADGET_CHECK_THROW(data.get_size(1) == fROE1E2.get_size(1));
        GADGET_CHECK_THROW(data.get_size(2) == fROE1E2.get_size(2));

        Gadgetron::multiply(data, fROE1E2, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_ROE1E2(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_ROE1E2(const hoNDArray<float>& data, const hoNDArray<float>& fROE1E2, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1E2(const hoNDArray<double>& data, const hoNDArray<double>& fROE1E2, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1E2(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fROE1E2, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1E2(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fROE1E2, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

template <typename T>
void apply_kspace_filter_ROE1E2(const hoNDArray<T>& data, const hoNDArray<T>& fRO, const hoNDArray<T>& fE1, const hoNDArray<T>& fE2, hoNDArray<T>& dataFiltered)
{
    try
    {
        GADGET_CHECK_THROW(data.get_size(0) == fRO.get_number_of_elements());
        GADGET_CHECK_THROW(data.get_size(1) == fE1.get_number_of_elements());
        GADGET_CHECK_THROW(data.get_size(2) == fE2.get_number_of_elements());

        hoNDArray<T> fxyz;
        compute_3d_filter(fRO, fE1, fE2, fxyz);

        Gadgetron::multiply(data, fxyz, dataFiltered);
    }
    catch (...)
    {
        GADGET_THROW("Errors in apply_kspace_filter_ROE1E2(...) ... ");
    }
}

template EXPORTMRICORE void apply_kspace_filter_ROE1E2(const hoNDArray<float>& data, const hoNDArray<float>& fRO, const hoNDArray<float>& fE1, const hoNDArray<float>& fE2, hoNDArray<float>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1E2(const hoNDArray<double>& data, const hoNDArray<double>& fRO, const hoNDArray<double>& fE1, const hoNDArray<double>& fE2, hoNDArray<double>& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1E2(const hoNDArray< std::complex<float> >& data, const hoNDArray< std::complex<float> >& fRO, const hoNDArray< std::complex<float> >& fE1, const hoNDArray< std::complex<float> >& fE2, hoNDArray< std::complex<float> >& dataFiltered);
template EXPORTMRICORE void apply_kspace_filter_ROE1E2(const hoNDArray< std::complex<double> >& data, const hoNDArray< std::complex<double> >& fRO, const hoNDArray< std::complex<double> >& fE1, const hoNDArray< std::complex<double> >& fE2, hoNDArray< std::complex<double> >& dataFiltered);

// ------------------------------------------------------------------------

void find_symmetric_sampled_region(size_t start, size_t end, size_t center, size_t& startSym, size_t& endSym)
{
    GADGET_CHECK_THROW(end >= start);
    GADGET_CHECK_THROW(center >= start);
    GADGET_CHECK_THROW(end >= center);

    size_t halfSizeStart = center - start;
    size_t halfSizeEnd = end - center;

    if (halfSizeStart > halfSizeEnd)
    {
        startSym = center - halfSizeEnd;
        endSym = center + halfSizeEnd;
    }
    else
    {
        startSym = center - halfSizeStart;
        endSym = center + halfSizeStart;
    }
}

// ------------------------------------------------------------------------

template<typename T>
void compute_filter_SNR_unit_scale_factor(const hoNDArray<T>& filter, T& scalFactor)
{
    size_t ii, len;

    len = filter.get_number_of_elements();
    if (len == 0)
    {
        scalFactor = T(1.0);
        return;
    }

    T sos(0.0);
    for (ii = 0; ii<len; ii++)
    {
        sos += filter(ii)*filter(ii);
    }

    scalFactor = (T)(1.0 / std::sqrt(std::abs(sos) / len));
}

template EXPORTMRICORE void compute_filter_SNR_unit_scale_factor(const hoNDArray<float>& filter, float& scalFactor);
template EXPORTMRICORE void compute_filter_SNR_unit_scale_factor(const hoNDArray<double>& filter, double& scalFactor);

template<> EXPORTMRICORE
void compute_filter_SNR_unit_scale_factor(const hoNDArray< std::complex<float> >& filter, std::complex<float> & scalFactor)
{
    size_t ii, len;

    len = filter.get_number_of_elements();
    if (len == 0)
    {
        scalFactor = std::complex<float>(1.0);
        return;
    }

    std::complex<float> sos(0.0);
    for (ii = 0; ii<len; ii++)
    {
        sos += filter(ii)*std::conj(filter(ii));
    }

    scalFactor = (std::complex<float>)(1.0 / std::sqrt(std::abs(sos) / len));
}

template<> EXPORTMRICORE
void compute_filter_SNR_unit_scale_factor(const hoNDArray< std::complex<double> >& filter, std::complex<double> & scalFactor)
{
    size_t ii, len;

    len = filter.get_number_of_elements();
    if (len == 0)
    {
        scalFactor = std::complex<double>(1.0);
        return;
    }

    std::complex<double> sos(0.0);
    for (ii = 0; ii<len; ii++)
    {
        sos += filter(ii)*std::conj(filter(ii));
    }

    scalFactor = (std::complex<double>)(1.0 / std::sqrt(std::abs(sos) / len));
}

// ------------------------------------------------------------------------

}


/**
    @file data.cpp
    Helper functions for saving and loading data
*/

#include <utils/data.hpp>

#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <Eigen/Dense>

/** Save matrix to .csv file. */
void Data::save_matrix(const std::string& file_name, Eigen::MatrixXd matrix) {
    const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                        Eigen::DontAlignCols, ", ", "\n");
    std::ofstream file(file_name); // open a file to write
    if (file.is_open())
    {
        file << matrix.format(CSVFormat);
        file.close();
    }
}

/** Load matrix from .csv file */
Eigen::MatrixXd Data::load_matrix(const std::string& file_name) {
    std::ifstream file(file_name); // open a file to read
    if (!file.good()) {
        std::cerr << "File: " << file_name << " does not exist" << std::endl;
        exit(1);
    }

    // Read through file and extract all data elements
    std::string row;
    int i = 0; // row counter
    std::string entry;
    std::vector<double> entries;
    while (std::getline(file, row)) {
        std::stringstream row_stream(row);
        while (std::getline(row_stream, entry, ',')) {
            entries.push_back(std::stod(entry));
        }
        i++; // increment row counter
    }

    // Convert vector into matrix of proper shape
    return Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>(entries.data(), i, entries.size() / i);
}

/** Load vector from .csv file */
Eigen::VectorXd Data::load_vector(const std::string& file_name) {
    Eigen::MatrixXd M = Data::load_matrix(file_name);
    return Eigen::Map<Eigen::VectorXd>(M.data(), M.rows());
}


#ifndef MATRIX_TEMPLATE_HXX
#define MATRIX_TEMPLATE_HXX

#include <wali/Common.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <ostream>

using namespace boost::numeric::ublas;

namespace wali {
  namespace domains {

    template<typename ElementType>
    Matrix<ElementType>::Matrix(BackingMatrix const & m)
      : m_matrix(m)
    {}

    template<typename ElementType>
    typename Matrix<ElementType>::BackingMatrix const &
    Matrix<ElementType>::matrix() const
    {
      return m_matrix;
    }

    template<typename ElementType>
    Matrix<ElementType>*
    Matrix<ElementType>::zero_raw() const
    {
      zero_matrix<value_type> m(m_matrix.size1(), m_matrix.size2());
      return new Matrix(m);
    }

    template<typename ElementType>
    Matrix<ElementType>*
    Matrix<ElementType>::one_raw() const
    {
      identity_matrix<value_type> m(m_matrix.size1(), m_matrix.size2());
      return new Matrix(m);
    }

    template<typename ElementType>
    Matrix<ElementType>*
    Matrix<ElementType>::extend_raw(Matrix * that) const
    {
      return new Matrix(prod(this->matrix(), that->matrix()));
    }

    template<typename ElementType>
    Matrix<ElementType>*
    Matrix<ElementType>::combine_raw(Matrix * that) const
    {
      return new Matrix(this->matrix() + that->matrix());
    }

    template<typename ElementType>
    bool
    Matrix<ElementType>::equal(Matrix * that) const
    {
      fast_assert(this->matrix().size1() == that->matrix().size1());
      fast_assert(this->matrix().size2() == that->matrix().size2());

      if (this->matrix().size1() != that->matrix().size1()
          || this->matrix().size2() != that->matrix().size2())
      {
        return false;
      }

      for (size_t row=0; row<matrix().size1(); ++row) {
        for (size_t col=0; col<matrix().size2(); ++col) {
          if (this->matrix()(row, col) != that->matrix()(row, col)) {
            return false;
          }
        }
      }

      return true;
    }

    template<typename ElementType>
    std::ostream &
    Matrix<ElementType>::print(std::ostream & stream) const
    {
      stream << "Matrix: " << matrix();
      return stream;
    }

    template<typename ElementType>
    sem_elem_t
    Matrix<ElementType>::one() const
    {
      return one_raw();
    }

    template<typename ElementType>
    sem_elem_t
    Matrix<ElementType>::zero() const
    {
      return zero_raw();
    }

    template<typename ElementType>
    sem_elem_t
    Matrix<ElementType>::extend(SemElem * se)
    {
      return extend_raw(down(se));
    }

    template<typename ElementType>
    sem_elem_t
    Matrix<ElementType>::combine(SemElem * se)
    {
      return combine_raw(down(se));
    }

    template<typename ElementType>
    bool
    Matrix<ElementType>::equal(SemElem * se) const
    {
      return equal(down(se));
    }

    template<typename ElementType>
    Matrix<ElementType>*
    Matrix<ElementType>::down(SemElem* se) const
    {
      Matrix* bm = dynamic_cast<Matrix*>(se);
      fast_assert(bm != NULL);
      return bm;
    }

  }
}

#endif /* MATRIX_TEMPLATE_HXX */

// Yo, Emacs!
// Local Variables:
//   c-file-style: "ellemtel"
//   c-basic-offset: 2
// End:


///////////////////////////////////////////////////////////////////////////////
// main.hpp
//
//  Copyright 2005 Eric Niebler. 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)

#include <iostream>
#include <algorithm>
#include <boost/ref.hpp>
#include <boost/bind.hpp>
#include <boost/array.hpp>
#include <boost/foreach.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>

using namespace boost;
using namespace boost::accumulators;

// Helper that uses BOOST_FOREACH to display a range of doubles
template<typename Range>
void output_range(Range const &rng)
{
    bool first = true;
    BOOST_FOREACH(double d, rng)
    {
        if(!first) std::cout << ", ";
        std::cout << d;
        first = false;
    }
    std::cout << '\n';
}

///////////////////////////////////////////////////////////////////////////////
// example1
//
//  Calculate some useful stats using accumulator_set<> and std::for_each()
//
void example1()
{
    accumulator_set<
        double
      , stats<tag::min, tag::mean(immediate), tag::sum, tag::moment<2> >
    > acc;

    boost::array<double, 4> data = {0., 1., -1., 3.14159};

    // std::for_each pushes each sample into the accumulator one at a
    // time, and returns a copy of the accumulator.
    acc = std::for_each(data.begin(), data.end(), acc);

    // The following would be equivalent, and could be more efficient
    // because it doesn't pass and return the entire accumulator set
    // by value.
    //std::for_each(data.begin(), data.end(), bind<void>(ref(acc), _1));

    std::cout << "  min""(acc)        = " << (min)(acc) << std::endl; // Extra quotes are to prevent complaints from Boost inspect tool
    std::cout << "  mean(acc)       = " << mean(acc) << std::endl;

    // since mean depends on count and sum, we can get their results, too.
    std::cout << "  count(acc)      = " << count(acc) << std::endl;
    std::cout << "  sum(acc)        = " << sum(acc) << std::endl;
    std::cout << "  moment<2>(acc)  = " << accumulators::moment<2>(acc) << std::endl;
}

///////////////////////////////////////////////////////////////////////////////
// example2
//
//  Calculate some tail statistics. This demonstrates how to specify
//  constructor and accumulator parameters. Note that the tail statistics
//  return multiple values, which are returned in an iterator_range.
//
//  It pushes data in and displays the intermediate results to demonstrate
//  how the tail statistics are updated.
void example2()
{
    // An accumulator which tracks the right tail (largest N items) and
    // some data that are covariate with them. N == 4.
    accumulator_set<
        double
      , stats<tag::tail_variate<double, tag::covariate1, right> >
    > acc(tag::tail<right>::cache_size = 4);

    acc(2.1, covariate1 = .21);
    acc(1.1, covariate1 = .11);
    acc(2.1, covariate1 = .21);
    acc(1.1, covariate1 = .11);

    std::cout << "  tail            = "; output_range(tail(acc));
    std::cout << "  tail_variate    = "; output_range(tail_variate(acc));
    std::cout << std::endl;

    acc(21.1, covariate1 = 2.11);
    acc(11.1, covariate1 = 1.11);
    acc(21.1, covariate1 = 2.11);
    acc(11.1, covariate1 = 1.11);

    std::cout << "  tail            = "; output_range(tail(acc));
    std::cout << "  tail_variate    = "; output_range(tail_variate(acc));
    std::cout << std::endl;

    acc(42.1, covariate1 = 4.21);
    acc(41.1, covariate1 = 4.11);
    acc(42.1, covariate1 = 4.21);
    acc(41.1, covariate1 = 4.11);

    std::cout << "  tail            = "; output_range(tail(acc));
    std::cout << "  tail_variate    = "; output_range(tail_variate(acc));
    std::cout << std::endl;

    acc(32.1, covariate1 = 3.21);
    acc(31.1, covariate1 = 3.11);
    acc(32.1, covariate1 = 3.21);
    acc(31.1, covariate1 = 3.11);

    std::cout << "  tail            = "; output_range(tail(acc));
    std::cout << "  tail_variate    = "; output_range(tail_variate(acc));
}

///////////////////////////////////////////////////////////////////////////////
// example3
//
//  Demonstrate how to calculate weighted statistics. This example demonstrates
//  both a simple weighted statistical calculation, and a more complicated
//  calculation where the weight statistics are calculated and stored in an
//  external weight accumulataor.
void example3()
{
    // weight == double
    double w = 1.;

    // Simple weighted calculation
    {
        // stats that depend on the weight are made external
        accumulator_set<double, stats<tag::mean>, double> acc;

        acc(0., weight = w);
        acc(1., weight = w);
        acc(-1., weight = w);
        acc(3.14159, weight = w);

        std::cout << "  mean(acc)       = " << mean(acc) << std::endl;
    }

    // Weighted calculation with an external weight accumulator
    {
        // stats that depend on the weight are made external
        accumulator_set<double, stats<tag::mean>, external<double> > acc;

        // Here's an external weight accumulator
        accumulator_set<void, stats<tag::sum_of_weights>, double> weight_acc;

        weight_acc(weight = w); acc(0., weight = w);
        weight_acc(weight = w); acc(1., weight = w);
        weight_acc(weight = w); acc(-1., weight = w);
        weight_acc(weight = w); acc(3.14159, weight = w);

        std::cout << "  mean(acc)       = " << mean(acc, weights = weight_acc) << std::endl;
    }
}

///////////////////////////////////////////////////////////////////////////////
// main
int main()
{
    std::cout << "Example 1:\n";
    example1();

    std::cout << "\nExample 2:\n";
    example2();

    std::cout << "\nExample 3:\n";
    example3();

    return 0;
}


/**
 *  testReachset.cpp
 *
 *  Test reachable set computation of the Moore-Greitzer engine model.
 *
 *  Created by Yinan Li on June 9, 2020.
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <cmath>

#include <array>
#include <boost/numeric/odeint.hpp>

#include "src/abstraction.hpp"
#include "src/system.hpp"
#include "src/csolver.h"
#include "src/matlabio.h"

typedef std::array<double, 3> state_type;

double a = 1./3.5;
double B = 2.0;
double H = 0.18;
double W = 0.25;
double lc = 8.0;
double cx = 1.0/lc;
double cy = 1.0/(4*lc*B*B);
double aH = a+H;
double H2 = H/(2.0*W*W*W);
double W2 = 3*W*W;

/* user defined dynamics */
struct mgode {
    static const int n = 3;  // system dimension
    static const int nu = 2;  // control dimension

    /* template constructor
     * @param[out] dx
     * @param[in] x
     * @param u
     */
    template<typename S>
    mgode(S *dx, const S *x, rocs::Rn u) {
	dx[0] = cx * (aH+H2*(x[0]-W)*(W2-(x[0]-W)*(x[0]-W)) - x[1]) + u[0];
	dx[1] = cy * (x[0] - x[2]*sqrt(x[1]));
	dx[2] = u[1];
    }
};
struct engine_vf {
    rocs::Rn _u;
    engine_vf(rocs::Rn u) : _u(u) {}
    void operator()(state_type &x, state_type &dxdt, double t) const {
	dxdt[0] = cx * (aH+H2*(x[0]-W)*(W2-(x[0]-W)*(x[0]-W)) - x[1]) + _u[0];
	dxdt[1] = cy * (x[0] - x[2]*sqrt(x[1]));
	dxdt[2] = _u[1];
    }
};

struct mgode2 {

    static const int n = 2;  // system dimension
    static const int nu = 2;  // control dimension

    /* template constructor
     * @param[out] dx
     * @param[in] x
     * @param u
     */
    template<typename S>
    mgode2(S *dx, const S *x, rocs::Rn u) {
	dx[0] = cx * (aH+H2*(x[0]-W)*(W2-(x[0]-W)*(x[0]-W)) - x[1]) + u[0];
	dx[1] = cy * (x[0] - u[1]*sqrt(x[1]));
    }
};
struct engine2d_ode {
    rocs::Rn _u;
    engine2d_ode(rocs::Rn u) : _u(u) {}
    void operator()(rocs::Rn &x, rocs::Rn &dxdt, double t) const {
	dxdt[0] = cx * (aH+H2*(x[0]-W)*(W2-(x[0]-W)*(x[0]-W)) - x[1]) + _u[0];
	dxdt[1] = cy * (x[0] - _u[1]*sqrt(x[1]));
    }
};

int main() {

    // /* set the state space */
    // double xlb[3]{0, 0, 0.5};
    // double xub[3]{1, 1, 0.8};

    // /* set the control values */
    // // double L = 0.0003;
    // // double ulb[2]{-5, -L};
    // // double uub[2]{5, L};
    // // double mu[2]{1, L/10};
    // double Lmu = 0.01;
    // double Lu = 0.05;
    // double ulb[2]{-Lu, -Lmu};
    // double uub[2]{Lu, Lmu};
    // double mu[2]{Lu/5, 2*Lmu/5};

    // /* set the sampling time and disturbance */
    // double t = 0.1;
    // double delta = 0.01;
    // /* parameters for computing the flow */
    // int kmax = 10;
    // double tol = 0.01;
    // double alpha = 0.5;
    // double beta = 2;
    // rocs::params controlparams(kmax, tol, alpha, beta);

    // /* define the control system */
    // rocs::CTCntlSys<mgode> engine("Moore-Greitzer", t, mgode::n, mgode::nu,
    // 				  delta, &controlparams);
    // engine.init_workspace(xlb, xub);
    // engine.init_inputset(mu, ulb, uub);
    // engine.allocate_flows();

    // /* test if reachable set covers the nominal trajectory */
    // /* compute reachable set */
    // double dt{0.001};
    // // state_type y{0.5039, 0.6605, 0.62};
    // state_type y{1.526/2, 0.5, 0.65};
    // // double e[3]{0.2, 0.5, 0.1};
    // // rocs::ivec x0 = {rocs::interval(y[0]-e[0], y[0]+e[0]),
    // // 		     rocs::interval(y[1]-e[1], y[1]+e[1]),
    // // 		     rocs::interval(y[2]-e[2], y[2]+e[2])};
    // rocs::ivec x0 = {rocs::interval(0.526, 1),
    // 		     rocs::interval(0.001, 1),
    // 		     rocs::interval(0.5, 0.8)};
    // std::vector<rocs::ivec> x(engine._ugrid._nv, rocs::ivec(3));
    // std::cout << "The initial interval: " << x0 << '\n';
    // std::cout << "The integrating time: " << t << '\n';
    // engine.get_reach_set(x, x0);

    // for (size_t i = 0; i < x.size(); ++i) {
    // 	/* integrate the nominal trajectory */
    // 	state_type y{1.526/2, 0.5, 0.65};
    // 	rocs::Rn u(engine._ugrid._data[i]);
    // 	boost::numeric::odeint::runge_kutta_cash_karp54<state_type> rk45;
    // 	boost::numeric::odeint::integrate_const(rk45, engine_vf(u),
    // 						y, 0.0, t, dt);
    // 	std::cout << "x(t)= [" << y[0] << ','<< y[1] << ',' << y[2] << "]\n";
    // 	std::cout << "R(t, x0, [" << engine._ugrid._data[i][0] << ','
    // 		  << engine._ugrid._data[i][1] << "])= ";
    // 	std::cout << x[i] <<'\n';
    // 	std::cout << "Check: ";
    // 	for (int j = 0; j < 3; ++j) {
    // 	    if (y[j] > x[i][j].getinf() && y[j] < x[i][j].getsup())
    // 		std::cout << true << ' ';
    // 	    else
    // 		std::cout << false << ' ';
    // 	}
    // 	std::cout << '\n';
    // }

    // engine.release_flows();

    /* set the state space */
    const int xdim = 2;
    double xlb[]{0.44, 0.6};
    double xub[]{0.54, 0.7};

    /* set the control values */
    // double Lmu = 0.01;
    double Lu = 0.05;
    double ulb[]{-Lu, 0.5};
    double uub[]{Lu, 0.8};
    double mu[]{Lu/5, 0.01};

    /* set the sampling time and disturbance */
    double delta = 0.01;
    /* parameters for computing the flow */
    int kmax = 10;
    double tol = 0.01;
    double alpha = 0.5;
    double beta = 2;
    rocs::params controlparams(kmax, tol, alpha, beta);

    /* define the control system */
    double t= 0.1;
    rocs::CTCntlSys<mgode2> engine("Moore-Greitzer", t, mgode2::n, mgode2::nu,
				  delta, &controlparams);
    engine.init_workspace(xlb, xub);
    engine.init_inputset(mu, ulb, uub);
    engine.allocate_flows();

    double eta[xdim]{0.00018, 0.00018};
    std::vector<rocs::ivec> x(engine._ugrid._nv, rocs::ivec(xdim));

    /**
     * Test if reachable set covers the nominal trajectory
     */
    // /* compute reachable set */
    // double dt{0.001};
    // rocs::Rn y{0.599, 0.688};
    // rocs::ivec x0 = {rocs::interval(y[0]-eta[0], y[0]+eta[0]),
    // 		     rocs::interval(y[1]-eta[1], y[1]+eta[1])};
    // std::cout << "The initial interval: " << x0 << '\n';
    // std::cout << "The integrating time: " << t << '\n';
    // engine.get_reach_set(x, x0);

    // boost::numeric::odeint::runge_kutta_cash_karp54<rocs::Rn> rk45;
    // for (size_t i = 0; i < x.size(); ++i) {
    // 	/* integrate the nominal trajectory */
    // 	rocs::Rn y{0.599, 0.688};
    // 	rocs::Rn u(engine._ugrid._data[i]);
    // 	boost::numeric::odeint::integrate_const(rk45, engine2d_ode(u),
    // 						y, 0.0, t, dt);
    // 	std::cout << "x(t)= [" << y[0] << ','<< y[1] << "]\n";
    // 	std::cout << "R(t, x0, [" << engine._ugrid._data[i][0] << ','
    // 		  << engine._ugrid._data[i][1] << "])= ";
    // 	std::cout << x[i] <<'\n';
    // 	std::cout << "Check: ";
    // 	for (int j = 0; j < xdim; ++j) {
    // 	    if (y[j] > x[i][j].getinf() && y[j] < x[i][j].getsup())
    // 		std::cout << true << ' ';
    // 	    else
    // 		std::cout << false << ' ';
    // 	}
    // 	std::cout << '\n';
    // }

    /**
     * Test if there is a box inside the goal area can
     * stay inside the goal area
     */
    rocs::abstraction< rocs::CTCntlSys<mgode2> > abst(&engine);
    abst.init_state(eta, xlb, xub);
    double e = 0.003;
    double goal[][2]{{0.5039-e, 0.5039+e}, {0.6605-e, 0.6605+e}};
    rocs::ivec G{rocs::interval(0.5039-e, 0.5039+e),
		 rocs::interval(0.6605-e, 0.6605+e)};

    double c1= eta[0]/2.0; //+1e-10;
    double c2= eta[1]/2.0; //+1e-10;
    // std::vector<rocs::ivec> y(abst._x._dim);
    rocs::Rn x_test{0.0, 0.0};
    std::cout << "Boxes in goal that stay in goal:\n";
    for(size_t i = 0; i < abst._x._nv; ++i) {
	abst._x.id_to_val(x_test, i);
	if(goal[0][0] <= (x_test[0]-c1) && (x_test[0]+c1) <= goal[0][1] &&
	   goal[1][0] <= (x_test[1]-c2) && (x_test[1]+c2) <= goal[1][1]) {
	    rocs::ivec x0{rocs::interval(x_test[0]-eta[0], x_test[0]+eta[0]),
			  rocs::interval(x_test[1]-eta[1], x_test[1]+eta[1])};
	    engine.get_reach_set(x, x0);
	    for (size_t i = 0; i < x.size(); ++i) {
		if(G.isin(x[i])) {
		    std::cout << "x=" << x[i] << ", u=" << i << '\n';
		}
	    }
	}
    }

    engine.release_flows();

    return 0;
}


/*
 * Data.hpp
 *
 *  Created on: 05 Feb 2018
 *      Author: Fabian Meyer
 */

#ifndef SLAM_DATA_HPP_
#define SLAM_DATA_HPP_

#include <Eigen/Dense>

namespace slam
{
    // 2D pose (x, y, theta)
    typedef Eigen::Vector3d Pose;
    typedef Eigen::Matrix3d PoseCov;

    // range bearing measurement (r, theta, c)
    typedef Eigen::Vector2d Measurement;

    // observation with measurement and data assoc
    typedef struct {
        Measurement m;
        size_t c;
    } Observation;

    // odometry (r1, t, r2)
    typedef Eigen::Vector3d Odometry;

    // 2D position (x, y)
    typedef Eigen::Vector2d Position;
    typedef Eigen::Matrix2d PositionCov;

    typedef struct {
        Odometry odom;
        std::vector<Observation> observ;
    } Data;

    void loadData(const std::string &filename, std::vector<Data> &data);
    void loadWorld(const std::string &filename, std::vector<Position> &landmarks);

}

#endif


#define BOOST_TEST_MODULE test_instrument

#include <boost/test/unit_test.hpp>
#include <Leg/FloatingLeg.h>
#include <Instrument/Asset.h>
#include "Instrument/Bond.h"
#include "Instrument/Swap.h"

BOOST_AUTO_TEST_SUITE(instrument_test_suite)

    BOOST_AUTO_TEST_CASE(bond_price) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        const double notional = 100;
        const double rate = 5.0 / 100;
        std::vector<boost::gregorian::date> referenceDates{};
        referenceDates.push_back(boost::gregorian::from_string("2016-04-01"));
        referenceDates.push_back(boost::gregorian::from_string("2016-10-03"));
        referenceDates.push_back(boost::gregorian::from_string("2017-04-03"));
        referenceDates.push_back(boost::gregorian::from_string("2017-10-02"));
        referenceDates.push_back(boost::gregorian::from_string("2018-04-02"));

        Actual_360 actualCalc = Actual_360();

        std::map<boost::gregorian::date, double> m_mapZeroRates{};
        m_mapZeroRates[boost::gregorian::from_string("2016-10-03")] = 0.04743305323463213;
        m_mapZeroRates[boost::gregorian::from_string("2017-04-03")] = 0.05;
        m_mapZeroRates[boost::gregorian::from_string("2017-10-02")] = 0.051;
        m_mapZeroRates[boost::gregorian::from_string("2018-04-02")] = 0.052;
        ZeroCouponCurve zeroCouponCurve{m_mapZeroRates};

        FixedLeg myLeg{notional, rate, referenceDates, actualCalc, zeroCouponCurve};
        Bond myBond{myLeg};

        double calculated_value = myBond.price();
        double theoretical_value = 9.5228417749383200;
        //BOOST_TEST_MESSAGE(" - Calculated Value: " << calculated_values);
        //BOOST_TEST_MESSAGE(" - Expected Value: " << expected_values);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-15));
    }

    BOOST_AUTO_TEST_CASE(swap_price) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        const double notional = 100000000;
        const double rate = 5.0 / 100;
        const int periodsPerYear = 2;
        std::vector<boost::gregorian::date> referenceDates{};
        referenceDates.push_back(boost::gregorian::from_string("2016-04-01"));
        referenceDates.push_back(boost::gregorian::from_string("2016-10-03"));
        referenceDates.push_back(boost::gregorian::from_string("2017-04-03"));
        referenceDates.push_back(boost::gregorian::from_string("2017-10-02"));
        referenceDates.push_back(boost::gregorian::from_string("2018-04-02"));

        Actual_360 actualCalc = Actual_360();

        std::map<boost::gregorian::date, double> m_mapZeroRates{};
        m_mapZeroRates[boost::gregorian::from_string("2016-10-03")] = 0.04743305323463213;
        m_mapZeroRates[boost::gregorian::from_string("2017-04-03")] = 0.05;
        m_mapZeroRates[boost::gregorian::from_string("2017-10-02")] = 0.051;
        m_mapZeroRates[boost::gregorian::from_string("2018-04-02")] = 0.052;
        ZeroCouponCurve zeroCouponCurve{m_mapZeroRates};

        FixedLeg payingLeg{notional, rate, referenceDates, actualCalc, zeroCouponCurve};
        FloatingLeg receivingLeg{notional, referenceDates, actualCalc, zeroCouponCurve, periodsPerYear};

        Swap mySwap{receivingLeg, payingLeg};

        double calculated_value = mySwap.price();
        double theoretical_value = 495775.94;
        //BOOST_TEST_MESSAGE(" - Calculated Value: " << calculated_values);
        //BOOST_TEST_MESSAGE(" - Expected Value: " << expected_values);
        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-2));
    }

    BOOST_AUTO_TEST_CASE(asset_getVolatility) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        const double spot_price = 50.0;
        double volatility = 62.0 / 100;

        Asset underlyingAsset{spot_price, volatility};

        double calculated_value = underlyingAsset.getVolatility();
        double theoretical_value = 0.62;

        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-2));
    }

    BOOST_AUTO_TEST_CASE(asset_getPrice) {
        BOOST_TEST_MESSAGE("using tolerances within checks.");

        const double spot_price = 50.0;
        double volatility = 62.0 / 100;

        Asset underlyingAsset{spot_price, volatility};

        double calculated_value = underlyingAsset.price();
        double theoretical_value = 50.0;

        BOOST_TEST(theoretical_value == calculated_value, boost::test_tools::tolerance(1e-2));
    }

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>
#include <iomanip>
#include <fstream>
#include <vector>
#include <numeric>

#include <boost/scope_exit.hpp>

#include <amgcl/mpi/util.hpp>
#include <amgcl_mpi.h>

#include "domain_partition.hpp"

double STDCALL constant_deflation(int, ptrdiff_t, void*) {
    return 1;
}

int main(int argc, char *argv[]) {
    int provided;
    MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
    BOOST_SCOPE_EXIT(void) {
        MPI_Finalize();
    } BOOST_SCOPE_EXIT_END

    int rank, size;
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);
    MPI_Comm_size(MPI_COMM_WORLD, &size);

    if (rank == 0)
        std::cout << "World size: " << size << std::endl;

    const ptrdiff_t n  = argc > 1 ? atoi(argv[1]) : 1024;
    const ptrdiff_t n2 = n * n;

    // Partition
    boost::array<ptrdiff_t, 2> lo = { {0, 0} };
    boost::array<ptrdiff_t, 2> hi = { {n - 1, n - 1} };

    domain_partition<2> part(lo, hi, size);
    ptrdiff_t chunk = part.size( rank );

    std::vector<ptrdiff_t> domain(size + 1);
    MPI_Allgather(
            &chunk, 1, amgcl::mpi::datatype<ptrdiff_t>(),
            &domain[1], 1, amgcl::mpi::datatype<ptrdiff_t>(),
            MPI_COMM_WORLD);
    std::partial_sum(domain.begin(), domain.end(), domain.begin());

    ptrdiff_t chunk_start = domain[rank];
    ptrdiff_t chunk_end   = domain[rank + 1];

    std::vector<ptrdiff_t> renum(n2);
    for(ptrdiff_t j = 0, idx = 0; j < n; ++j) {
        for(ptrdiff_t i = 0; i < n; ++i, ++idx) {
            boost::array<ptrdiff_t, 2> p = {{i, j}};
            std::pair<int,ptrdiff_t> v = part.index(p);
            renum[idx] = domain[v.first] + v.second;
        }
    }

    // Assemble
    std::vector<ptrdiff_t> ptr;
    std::vector<ptrdiff_t> col;
    std::vector<double>    val;
    std::vector<double>    rhs;

    ptr.reserve(chunk + 1);
    col.reserve(chunk * 5);
    val.reserve(chunk * 5);
    rhs.reserve(chunk);

    ptr.push_back(0);

    const double hinv = (n - 1);
    const double h2i  = (n - 1) * (n - 1);
    for(ptrdiff_t j = 0, idx = 0; j < n; ++j) {
        for(ptrdiff_t i = 0; i < n; ++i, ++idx) {
            if (renum[idx] < chunk_start || renum[idx] >= chunk_end) continue;

            if (j > 0)  {
                col.push_back(renum[idx - n]);
                val.push_back(-h2i);
            }

            if (i > 0) {
                col.push_back(renum[idx - 1]);
                val.push_back(-h2i - hinv);
            }

            col.push_back(renum[idx]);
            val.push_back(4 * h2i + hinv);

            if (i + 1 < n) {
                col.push_back(renum[idx + 1]);
                val.push_back(-h2i);
            }

            if (j + 1 < n) {
                col.push_back(renum[idx + n]);
                val.push_back(-h2i);
            }

            rhs.push_back(1);
            ptr.push_back( col.size() );
        }
    }

    // Setup
    amgclHandle prm    = amgcl_params_create();

    amgcl_params_sets(prm, "local.coarsening.type", "smoothed_aggregation");
    amgcl_params_sets(prm, "local.relax.type", "spai0");
    amgcl_params_sets(prm, "isolver.type", "bicgstabl");
#ifdef AMGCL_HAVE_PASTIX
    amgcl_params_sets(prm, "dsolver.type", "pastix");
#else
    amgcl_params_sets(prm, "dsolver.type", "skyline_lu");
#endif

    amgclHandle solver = amgcl_mpi_create(
            MPI_COMM_WORLD,
            chunk, ptr.data(), col.data(), val.data(),
            1, constant_deflation, NULL, prm
            );

    // Solve
    std::vector<double> x(chunk, 0);
    conv_info cnv = amgcl_mpi_solve(solver, rhs.data(), x.data());

    std::cout << "Iterations: " << cnv.iterations << std::endl
              << "Error:      " << cnv.residual   << std::endl;

    // Clean up
    amgcl_mpi_destroy(solver);
    amgcl_params_destroy(prm);

    if (n <= 4096) {
        if (rank == 0) {
            std::vector<double> X(n2);
            std::copy(x.begin(), x.end(), X.begin());

            for(int i = 1; i < size; ++i)
                MPI_Recv(&X[domain[i]], domain[i+1] - domain[i], MPI_DOUBLE, i, 42, MPI_COMM_WORLD, MPI_STATUS_IGNORE);

            std::ofstream f("out.dat", std::ios::binary);
            int m = n2;
            f.write((char*)&m, sizeof(int));
            for(ptrdiff_t i = 0; i < n2; ++i)
                f.write((char*)&X[renum[i]], sizeof(double));
        } else {
            MPI_Send(x.data(), chunk, MPI_DOUBLE, 0, 42, MPI_COMM_WORLD);
        }
    }
}


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include <Eigen/Core>
#include "gtest/gtest.h"

#include "modules/geometry/polygon.hpp"
#include "modules/geometry/line.hpp"
#include "modules/geometry/commons.hpp"
#include "modules/models/behavior/idm/idm_classic.hpp"
#include "modules/commons/params/default_params.hpp"
#include "modules/world/observed_world.hpp"
#include "modules/models/execution/interpolation/interpolate.hpp"
#include "modules/models/behavior/constant_velocity/constant_velocity.hpp"
#include "modules/models/tests/make_test_world.hpp"

using namespace modules::models::dynamic;
using namespace modules::models::execution;
using namespace modules::commons;
using namespace modules::models::behavior;
using namespace modules::world::map;
using namespace modules::models::dynamic;
using namespace modules::world;
using namespace modules::geometry;
using namespace modules::models::tests;

TEST(free_road_term, behavior_idm_classic) {
  DefaultParams params;
  BehaviorIDMClassic behavior(&params);
  const float desired_velocity = behavior.get_desired_velocity();
  const float max_acceleration = behavior.get_max_acceleration();
  const int exponent = behavior.get_exponent();

  float ego_velocity = desired_velocity, rel_distance = 7.0, velocity_difference=0.0;
  auto observed_world = make_test_observed_world(0,rel_distance, ego_velocity, velocity_difference);
  double idm_acceleration  = behavior.CalculateLongitudinalAcceleration(observed_world);

  // no vehicle is in front only free road term should give acceleration
  double desired_acceleration = max_acceleration*(1-pow(ego_velocity/desired_velocity, exponent));
  EXPECT_NEAR(idm_acceleration, desired_acceleration, 0.0001);

  ego_velocity = desired_velocity+10;
  observed_world = make_test_observed_world(0,rel_distance, ego_velocity, velocity_difference);
  idm_acceleration  = behavior.CalculateLongitudinalAcceleration(observed_world);

  // no vehicle is in front only free road term should give acceleration
  desired_acceleration = max_acceleration*(1-pow(ego_velocity/desired_velocity, exponent));
  EXPECT_NEAR(idm_acceleration, desired_acceleration, 0.0001);

  ego_velocity = desired_velocity-12;
  observed_world = make_test_observed_world(0,rel_distance, ego_velocity, velocity_difference);
  idm_acceleration  = behavior.CalculateLongitudinalAcceleration(observed_world);

  // no vehicle is in front only free road term should give acceleration
  desired_acceleration = max_acceleration*(1-pow(ego_velocity/desired_velocity, exponent));
  EXPECT_NEAR(idm_acceleration, desired_acceleration, 0.0001);
}

TEST(interaction_term, behavior_idm_classic) {
  DefaultParams params;
  BehaviorIDMClassic behavior(&params);
  const float desired_velocity = behavior.get_desired_velocity();
  const float minimum_spacing = behavior.get_minimum_spacing();
  const float desired_time_headway = behavior.get_desired_time_headway();
  const float max_acceleration = behavior.get_max_acceleration();
  const float comfortable_braking_acceleration = behavior.get_comfortable_braking_acceleration();

  // vehicle is in front, zero velocity equal desired velocity thus only interaction term
  float ego_velocity = desired_velocity, rel_distance = 7.0, velocity_difference=0.0;
  auto observed_world = make_test_observed_world(1,rel_distance, ego_velocity, velocity_difference);
  double idm_acceleration  = behavior.CalculateLongitudinalAcceleration(observed_world);
  double helper_state = minimum_spacing + ego_velocity*desired_time_headway +
               ego_velocity*velocity_difference/(2*sqrt(max_acceleration*comfortable_braking_acceleration));
  double desired_acceleration = - max_acceleration*pow(helper_state/rel_distance, 2);
  EXPECT_NEAR(idm_acceleration, desired_acceleration, 0.001);

  // velocity difference to other vehicle
  ego_velocity = desired_velocity, rel_distance = 7.0, velocity_difference=5.0;
  observed_world = make_test_observed_world(1,rel_distance, ego_velocity, velocity_difference);
  idm_acceleration  = behavior.CalculateLongitudinalAcceleration(observed_world);
  helper_state = minimum_spacing + ego_velocity*desired_time_headway +
               ego_velocity*velocity_difference/(2*sqrt(max_acceleration*comfortable_braking_acceleration));
  desired_acceleration = - max_acceleration*pow(helper_state/rel_distance, 2);
  EXPECT_NEAR(idm_acceleration, desired_acceleration, 0.001);
}

TEST(drive_free, behavior_idm_classic) {
  DefaultParams params;
  BehaviorIDMClassic behavior(&params);
  const float desired_velocity = behavior.get_desired_velocity();

  // First case, we start with the desired velocity. After num steps, we should advance
  float ego_velocity = desired_velocity, rel_distance = 7.0, velocity_difference=0.0;
  float time_step=0.2f;
  int num_steps = 10;
  WorldPtr world = make_test_world(0,rel_distance, ego_velocity, velocity_difference);

  float x_start = world->get_agents().begin()->second->get_current_state()[StateDefinition::X_POSITION];
  for (int i=0; i<num_steps; ++i ) {
    world->Step(time_step);
  }
  float x_end = world->get_agents().begin()->second->get_current_state()[StateDefinition::X_POSITION];
  float x_diff_desired = x_start + ego_velocity*num_steps*time_step - x_end;
  EXPECT_NEAR(x_diff_desired,0, 0.01);

}

TEST(drive_leading_vehicle, behavior_idm_classic) {
  DefaultParams params;
  BehaviorIDMClassic behavior(&params);
  const float desired_velocity = behavior.get_desired_velocity();
  const float minimum_spacing = behavior.get_minimum_spacing();
  const float desired_time_headway = behavior.get_desired_time_headway();

  // First case, we start with the desired velocity. After num steps, we should advance
  float ego_velocity = desired_velocity, rel_distance = 5.0, velocity_difference=10;
  float time_step=0.2f; // Very small time steps to verify differential integration character
  int num_steps = 10;
  WorldPtr world = make_test_world(1,rel_distance, ego_velocity, velocity_difference);

 /* float v_start = world->get_agents().begin()->second->get_current_state()[StateDefinition::VEL_POSITION];
  for (int i=0; i<num_steps; ++i ) {
    world->Step(time_step);
  }
  float v_end = world->get_agents().begin()->second->get_current_state()[StateDefinition::VEL_POSITION];

  EXPECT_NEAR(v_end,ego_velocity-velocity_difference, 0.01);
*/
}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#include "round.hpp"
#include "ast.hpp"
#include "parsers.hpp"
#include "types.hpp"
#include <boost/variant.hpp>
#include <stdexcept>
#include <cmath>
#include <cfenv>
namespace
{
    inline bool fisint(long double d)
    {
        long double p[2];
        p[1] = std::modf(d,p);
        return  (p[1]==0.0) ;
    }
}
namespace HT
{
    void round(PASTNode astnode, ParsersHelper& ph)
    {
        auto myParserHelper(ph);
        if (astnode->ch.size()!=2)
          throw std::runtime_error("round can only have one parameter");
        auto & secondCh = *astnode->ch.rbegin();
        ph.parse(secondCh);
        if (secondCh->token.tokenType != Complex || ! boost::get<ComplexType>(secondCh->token.info).isReal())
          throw std::runtime_error("The argument of round must be real");
        auto cast = boost::get<ComplexType>(secondCh->token.info);
        astnode->type = Simple;
        astnode->token.tokenType = Complex;
        if (cast.isRational())
        {
            bool si = cast.getRealR().getSign();
            auto rat = cast.getRealR();
            if (!si) rat = - rat;
            auto t = rat.getUp() / rat.getDown();
            auto tt = t+1;
            if (tt - rat > rat - t)
              astnode->token.info = ComplexType( t.setSign(si));
            else
              if (tt-rat!=rat-t)
                astnode->token.info = ComplexType( tt.setSign(si));
            else
              astnode->token.info = ComplexType( (t%2==0 ? t:tt).setSign(si));
        } else
        {
            long double p[2];
            p[1] = std::modf(cast.getRealD(), p);
            LOG("dex part:"<<p[1])
            if (std::fabs(p[1])!=0.5)
          astnode->token.info = ComplexType(std::roundl(cast.getRealD()));
            else
            {
                astnode->token.info = ComplexType(::fisint(p[0]/2.0)?
                            p[0] : ( p[0]<0 ? p[0]-1.0 : p[0]+1.0));
            }
        }

        astnode->remove();
    }

}


#include <boost/math/constants/constants.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <iostream>
#include <limits>
#include <cmath>
#include <iomanip>

using namespace std;
using boost::multiprecision::cpp_dec_float_50;

cpp_dec_float_50 truncate(cpp_dec_float_50 n, int precision) {
    cpp_dec_float_50 remainder = static_cast<cpp_dec_float_50>((int)floor((n - floor(n)) * precision) % precision) / static_cast<cpp_dec_float_50>(precision);
    return floor(n) + remainder;
}

int main(void) {
    int precision = 100; // as many digits as you add zeroes. 5 zeroes means precision of 5.
    cpp_dec_float_50 n =  0.02785 * 100;
    n = truncate(n + 0.5/precision, precision); // first part is remainder, floor(n) is int value truncated.
    cout << setprecision(numeric_limits<cpp_dec_float_50> ::max_digits10 + __builtin_ctz(precision)) << n << "%" << endl; // __builtin_ctz(precision) will equal the number of trailing 0, exactly the precision we need!
    return 0;
}