// Copyright 2015 XLGAMES Inc.
//
// Distributed under the MIT License (See
// accompanying file "LICENSE" or the website
// http://www.opensource.org/licenses/mit-license.php)

#define _SILENCE_CXX17_NEGATORS_DEPRECATION_WARNING		// silence warning due to removed functions in C++17 (from Eigen library)

#include "RegularNumberField.h"
#include <cmath>

#pragma warning(disable:4714)
#pragma push_macro("new")
#undef new
#include <Eigen/Dense>
#pragma pop_macro("new")

namespace XLEMath
{

///////////////////////////////////////////////////////////////////////////////////////////////////
            //   B I L I N E A R
///////////////////////////////////////////////////////////////////////////////////////////////////

    template<unsigned SamplingFlags, typename Store>
        static Float2 SampleBilinear(const VectorField2DSeparate<Store>& field, Float2 coord)
    {
        float fx = XlFloor(coord[0]);
        float fy = XlFloor(coord[1]);
        float a = coord[0] - fx, b = coord[1] - fy;
        float weights[] =
        {
            (1.f - a) * (1.f - b),
            a * (1.f - b),
            (1.f - a) * b,
            a * b
        };
        unsigned x0, x1, y0, y1;

        const auto dims = field.Dimensions();
        if (constant_expression<(SamplingFlags & RNFSample::WrapX)!=0>::result()) {
            x0 = unsigned((int(fx) + int(dims[0]))%dims[0]);
            x1 = (x0+1u)%dims[0];
        } else if (constant_expression<(SamplingFlags & RNFSample::ClampX)!=0>::result()) {
            x0 = unsigned(Clamp(fx, 0.f, float(dims[0]-1)));
            x1 = std::min(x0+1u, dims[0]-1u);
        } else {
            x0 = unsigned(fx); x1 = x0+1;
        }

        if (constant_expression<(SamplingFlags & RNFSample::WrapY)!=0>::result()) {
            y0 = unsigned((int(fy) + int(dims[1]))%dims[1]);
            y1 = (y0+1u)%dims[1];
        } else if (constant_expression<(SamplingFlags & RNFSample::ClampY)!=0>::result()) {
            y0 = unsigned(Clamp(fy, 0.f, float(dims[1]-1)));
            y1 = std::min(y0+1u, dims[1]-1u);
        } else {
            y0 = unsigned(fy); y1 = y0+1;
        }
        assert(x1 < dims[0] && y1 < dims[1]);

        float u
            = weights[0] * (*field._u)[y0*dims[0]+x0]
            + weights[1] * (*field._u)[y0*dims[0]+x1]
            + weights[2] * (*field._u)[y1*dims[0]+x0]
            + weights[3] * (*field._u)[y1*dims[0]+x1]
            ;
        float v
            = weights[0] * (*field._v)[y0*dims[0]+x0]
            + weights[1] * (*field._v)[y0*dims[0]+x1]
            + weights[2] * (*field._v)[y1*dims[0]+x0]
            + weights[3] * (*field._v)[y1*dims[0]+x1]
            ;
        return Float2(u, v);
    }

    template<unsigned SamplingFlags, typename Store>
        static float SampleBilinear(const ScalarField2D<Store>& field, Float2 coord)
    {
        float fx = XlFloor(coord[0]);
        float fy = XlFloor(coord[1]);
        float a = coord[0] - fx, b = coord[1] - fy;
        float weights[] =
        {
            (1.f - a) * (1.f - b),
            a * (1.f - b),
            (1.f - a) * b,
            a * b
        };
        unsigned x0, x1, y0, y1;

        const auto dims = field.Dimensions();
        if (constant_expression<(SamplingFlags & RNFSample::WrapX)!=0>::result()) {
            x0 = unsigned((int(fx) + int(dims[0]))%dims[0]);
            x1 = (x0+1u)%dims[0];
        } else if (constant_expression<(SamplingFlags & RNFSample::ClampX)!=0>::result()) {
            x0 = unsigned(Clamp(fx, 0.f, float(dims[0]-1)));
            x1 = std::min(x0+1u, dims[0]-1u);
        } else {
            x0 = unsigned(fx); x1 = x0+1;
        }

        if (constant_expression<(SamplingFlags & RNFSample::WrapY)!=0>::result()) {
            y0 = unsigned((int(fy) + int(dims[1]))%dims[1]);
            y1 = (y0+1u)%dims[1];
        } else if (constant_expression<(SamplingFlags & RNFSample::ClampY)!=0>::result()) {
            y0 = unsigned(Clamp(fy, 0.f, float(dims[1]-1)));
            y1 = std::min(y0+1u, dims[1]-1u);
        } else {
            y0 = unsigned(fy); y1 = y0+1;
        }
        assert(x1 < dims[0] && y1 < dims[1]);

        return
              weights[0] * (*field._u)[y0*dims[0]+x0]
            + weights[1] * (*field._u)[y0*dims[0]+x1]
            + weights[2] * (*field._u)[y1*dims[0]+x0]
            + weights[3] * (*field._u)[y1*dims[0]+x1]
            ;
    }

    template<unsigned SamplingFlags, typename Store>
        static Float3 SampleBilinear(const VectorField3DSeparate<Store>& field, Float3 coord)
    {
        float fx = XlFloor(coord[0]);
        float fy = XlFloor(coord[1]);
        float fz = XlFloor(coord[2]);
        float a = coord[0] - fx, b = coord[1] - fy, c = coord[2] - fz;
        float weights[] =
        {
            (1.f - a) * (1.f - b) * (1.f - c),
            a * (1.f - b) * (1.f - c),
            (1.f - a) * b * (1.f - c),
            a * b * (1.f - c),

            (1.f - a) * (1.f - b) * c,
            a * (1.f - b) * c,
            (1.f - a) * b * c,
            a * b * c,
        };
        unsigned x0, x1, y0, y1, z0, z1;

        const auto dims = field.Dimensions();
        if (constant_expression<(SamplingFlags & RNFSample::WrapX)!=0>::result()) {
            x0 = unsigned((int(fx) + int(dims[0]))%dims[0]);
            x1 = (x0+1u)%dims[0];
        } else if (constant_expression<(SamplingFlags & RNFSample::ClampX)!=0>::result()) {
            x0 = unsigned(Clamp(fx, 0.f, float(dims[0]-1)));
            x1 = std::min(x0+1u, dims[0]-1u);
        } else {
            x0 = unsigned(fx); x1 = x0+1;
        }

        if (constant_expression<(SamplingFlags & RNFSample::WrapY)!=0>::result()) {
            y0 = unsigned((int(fy) + int(dims[1]))%dims[1]);
            y1 = (y0+1u)%dims[1];
        } else if (constant_expression<(SamplingFlags & RNFSample::ClampY)!=0>::result()) {
            y0 = unsigned(Clamp(fy, 0.f, float(dims[1]-1)));
            y1 = std::min(y0+1u, dims[1]-1u);
        } else {
            y0 = unsigned(fy); y1 = y0+1;
        }

        if (constant_expression<(SamplingFlags & RNFSample::WrapZ)!=0>::result()) {
            z0 = unsigned((int(fz) + int(dims[2]))%dims[2]);
            z1 = (z0+1u)%dims[2];
        } else if (constant_expression<(SamplingFlags & RNFSample::ClampZ)!=0>::result()) {
            z0 = unsigned(Clamp(fz, 0.f, float(dims[2]-1)));
            z1 = std::min(z0+1u, dims[2]-1u);
        } else {
            z0 = unsigned(fz); z1 = z0+1;
        }
        assert(x1 < dims[0] && y1 < dims[1] && z1 < dims[2]);

        float u
            = weights[0] * (*field._u)[(z0*dims[1]+y0)*dims[0]+x0]
            + weights[1] * (*field._u)[(z0*dims[1]+y0)*dims[0]+x1]
            + weights[2] * (*field._u)[(z0*dims[1]+y1)*dims[0]+x0]
            + weights[3] * (*field._u)[(z0*dims[1]+y1)*dims[0]+x1]
            + weights[4] * (*field._u)[(z1*dims[1]+y0)*dims[0]+x0]
            + weights[5] * (*field._u)[(z1*dims[1]+y0)*dims[0]+x1]
            + weights[6] * (*field._u)[(z1*dims[1]+y1)*dims[0]+x0]
            + weights[7] * (*field._u)[(z1*dims[1]+y1)*dims[0]+x1]
            ;
        float v
            = weights[0] * (*field._v)[(z0*dims[1]+y0)*dims[0]+x0]
            + weights[1] * (*field._v)[(z0*dims[1]+y0)*dims[0]+x1]
            + weights[2] * (*field._v)[(z0*dims[1]+y1)*dims[0]+x0]
            + weights[3] * (*field._v)[(z0*dims[1]+y1)*dims[0]+x1]
            + weights[4] * (*field._v)[(z1*dims[1]+y0)*dims[0]+x0]
            + weights[5] * (*field._v)[(z1*dims[1]+y0)*dims[0]+x1]
            + weights[6] * (*field._v)[(z1*dims[1]+y1)*dims[0]+x0]
            + weights[7] * (*field._v)[(z1*dims[1]+y1)*dims[0]+x1]
            ;
        float w
            = weights[0] * (*field._w)[(z0*dims[1]+y0)*dims[0]+x0]
            + weights[1] * (*field._w)[(z0*dims[1]+y0)*dims[0]+x1]
            + weights[2] * (*field._w)[(z0*dims[1]+y1)*dims[0]+x0]
            + weights[3] * (*field._w)[(z0*dims[1]+y1)*dims[0]+x1]
            + weights[4] * (*field._w)[(z1*dims[1]+y0)*dims[0]+x0]
            + weights[5] * (*field._w)[(z1*dims[1]+y0)*dims[0]+x1]
            + weights[6] * (*field._w)[(z1*dims[1]+y1)*dims[0]+x0]
            + weights[7] * (*field._w)[(z1*dims[1]+y1)*dims[0]+x1]
            ;
        return Float3(u, v, w);
    }

    template<unsigned SamplingFlags, typename Store>
        static float SampleBilinear(const ScalarField3D<Store>& field, Float3 coord)
    {
        float fx = XlFloor(coord[0]);
        float fy = XlFloor(coord[1]);
        float fz = XlFloor(coord[2]);
        float a = coord[0] - fx, b = coord[1] - fy, c = coord[2] - fz;
        float weights[] =
        {
            (1.f - a) * (1.f - b) * (1.f - c),
            a * (1.f - b) * (1.f - c),
            (1.f - a) * b * (1.f - c),
            a * b * (1.f - c),

            (1.f - a) * (1.f - b) * c,
            a * (1.f - b) * c,
            (1.f - a) * b * c,
            a * b * c,
        };
        unsigned x0, x1, y0, y1, z0, z1;

        const auto dims = field.Dimensions();
        if (constant_expression<(SamplingFlags & RNFSample::WrapX)!=0>::result()) {
            x0 = unsigned((int(fx) + int(dims[0]))%dims[0]);
            x1 = (x0+1u)%dims[0];
        } else if (constant_expression<(SamplingFlags & RNFSample::ClampX)!=0>::result()) {
            x0 = unsigned(Clamp(fx, 0.f, float(dims[0]-1)));
            x1 = std::min(x0+1u, dims[0]-1u);
        } else {
            x0 = unsigned(fx); x1 = x0+1;
        }

        if (constant_expression<(SamplingFlags & RNFSample::WrapY)!=0>::result()) {
            y0 = unsigned((int(fy) + int(dims[1]))%dims[1]);
            y1 = (y0+1u)%dims[1];
        } else if (constant_expression<(SamplingFlags & RNFSample::ClampY)!=0>::result()) {
            y0 = unsigned(Clamp(fy, 0.f, float(dims[1]-1)));
            y1 = std::min(y0+1u, dims[1]-1u);
        } else {
            y0 = unsigned(fy); y1 = y0+1;
        }

        if (constant_expression<(SamplingFlags & RNFSample::WrapZ)!=0>::result()) {
            z0 = unsigned((int(fz) + int(dims[2]))%dims[2]);
            z1 = (z0+1u)%dims[2];
        } else if (constant_expression<(SamplingFlags & RNFSample::ClampZ)!=0>::result()) {
            z0 = unsigned(Clamp(fz, 0.f, float(dims[2]-1)));
            z1 = std::min(z0+1u, dims[2]-1u);
        } else {
            z0 = unsigned(fz); z1 = z0+1;
        }
        assert(x1 < dims[0] && y1 < dims[1] && z1 < dims[2]);

        return
              weights[0] * (*field._u)[(z0*dims[1]+y0)*dims[0]+x0]
            + weights[1] * (*field._u)[(z0*dims[1]+y0)*dims[0]+x1]
            + weights[2] * (*field._u)[(z0*dims[1]+y1)*dims[0]+x0]
            + weights[3] * (*field._u)[(z0*dims[1]+y1)*dims[0]+x1]
            + weights[4] * (*field._u)[(z1*dims[1]+y0)*dims[0]+x0]
            + weights[5] * (*field._u)[(z1*dims[1]+y0)*dims[0]+x1]
            + weights[6] * (*field._u)[(z1*dims[1]+y1)*dims[0]+x0]
            + weights[7] * (*field._u)[(z1*dims[1]+y1)*dims[0]+x1]
            ;
    }

///////////////////////////////////////////////////////////////////////////////////////////////////
            //   M O N O T O N I C   C U B I C
///////////////////////////////////////////////////////////////////////////////////////////////////

    static float Sign(float x) { return (x < 0.f) ? -1.f : (x==0.f) ? 0.f : 1.f; }
    static float MonotonicCubic(float xn1, float x0, float x1, float x2, float alpha)
    {
            // See also http://grmanet.sogang.ac.kr/ihm/webpapers/CLMCI.pdf
            // (Controllable Local Monotonic Cubic Interpolation in Fluid Animations)
            // for a much more expensive, but more flexible, monotonic interpolation
            // method.
            // This method reverts to linear in non-monotonic cases. The above paper
            // attempts to find a good curve even on non-monotonic cases

        float dk        = (x1 - xn1) / 2.f;
        float dk1       = (x2 - x0) / 2.f;
        float deltak    = x1 - x0;
        if (Sign(dk) != Sign(deltak) || Sign(dk1) != Sign(deltak))
            dk = dk1 = 0.f;

        float a2 = alpha * alpha;
        float a3 = alpha * a2;

            //      original math from Fedkiw has a small error in the first term
            //      (should be -2*deltak, not -delta)
        return
              (dk + dk1 - 2.f * deltak) * a3
            + (3.f * deltak - 2.f * dk - dk1) * a2
            + dk * alpha
            + x0;
    }

    template<unsigned SamplingFlags, typename Store>
        float SampleMonotonicCubic(const ScalarField2D<Store>& field, Float2 coord)
    {
        float fx = XlFloor(coord[0]);
        float fy = XlFloor(coord[1]);
        float a = coord[0] - fx, b = coord[1] - fy;

        const auto dims = field.Dimensions();
        unsigned xn1, x0, x1, x2;
        unsigned yn1, y0, y1, y2;
        if (constant_expression<(SamplingFlags & RNFSample::WrapX)!=0>::result()) {
            x0 = unsigned((int(fx) + int(dims[0]))%dims[0]);
            x1 = (x0+1u)%dims[0];
            x2 = (x1+1u)%dims[0];
            xn1 = (x0+dims[0]-1)%dims[0];
        } else {
            x0 = unsigned(Clamp(fx, 0.f, float(dims[0]-1)));
            x1 = std::min(x0+1u, dims[0]-1u);
            x2 = std::min(x1+1u, dims[0]-1u);
            xn1 = std::max(x0, 1u) - 1u;
        }

        if (constant_expression<(SamplingFlags & RNFSample::WrapY)!=0>::result()) {
            y0 = unsigned((int(fy) + int(dims[1]))%dims[1]);
            y1 = (y0+1u)%dims[1];
            y2 = (y1+1u)%dims[1];
            yn1 = (y0+dims[1]-1)%dims[1];
        } else {
            y0 = unsigned(Clamp(fy, 0.f, float(dims[1]-1)));
            y1 = std::min(y0+1u, dims[1]-1u);
            y2 = std::min(y1+1u, dims[1]-1u);
            yn1 = std::max(y0, 1u) - 1u;
        }

        float u[] =
        {
            (*field._u)[yn1*dims[0]+xn1],
            (*field._u)[yn1*dims[0]+ x0],
            (*field._u)[yn1*dims[0]+ x1],
            (*field._u)[yn1*dims[0]+ x2],

            (*field._u)[ y0*dims[0]+xn1],
            (*field._u)[ y0*dims[0]+ x0],
            (*field._u)[ y0*dims[0]+ x1],
            (*field._u)[ y0*dims[0]+ x2],

            (*field._u)[ y1*dims[0]+xn1],
            (*field._u)[ y1*dims[0]+ x0],
            (*field._u)[ y1*dims[0]+ x1],
            (*field._u)[ y1*dims[0]+ x2],

            (*field._u)[ y2*dims[0]+xn1],
            (*field._u)[ y2*dims[0]+ x0],
            (*field._u)[ y2*dims[0]+ x1],
            (*field._u)[ y2*dims[0]+ x2]
        };

        float un1 = MonotonicCubic(u[ 0], u[ 1], u[ 2], u[ 3], a);
        float u0  = MonotonicCubic(u[ 4], u[ 5], u[ 6], u[ 7], a);
        float u1  = MonotonicCubic(u[ 8], u[ 9], u[10], u[11], a);
        float u2  = MonotonicCubic(u[12], u[13], u[14], u[15], a);
        return MonotonicCubic(un1, u0, u1, u2, b);
    }

    template<unsigned SamplingFlags, typename Store>
        Float2 SampleMonotonicCubic(const VectorField2DSeparate<Store>& field, Float2 coord)
    {
        float fx = XlFloor(coord[0]);
        float fy = XlFloor(coord[1]);
        float a = coord[0] - fx, b = coord[1] - fy;

        const auto dims = field.Dimensions();
        unsigned xn1, x0, x1, x2;
        unsigned yn1, y0, y1, y2;
        if (constant_expression<(SamplingFlags & RNFSample::WrapX)!=0>::result()) {
            x0  = unsigned((int(fx) + int(dims[0]))%dims[0]);
            x1  = (x0+1u)%dims[0];
            x2  = (x1+1u)%dims[0];
            xn1 = (x0+dims[0]-1)%dims[0];
        } else {
            x0  = unsigned(Clamp(fx, 0.f, float(dims[0]-1)));
            x1  = std::min(x0+1u, dims[0]-1u);
            x2  = std::min(x1+1u, dims[0]-1u);
            xn1 = std::max(x0, 1u) - 1u;
        }

        if (constant_expression<(SamplingFlags & RNFSample::WrapY)!=0>::result()) {
            y0  = unsigned((int(fy) + int(dims[1]))%dims[1]);
            y1  = (y0+1u)%dims[1];
            y2  = (y1+1u)%dims[1];
            yn1 = (y0+dims[1]-1)%dims[1];
        } else {
            y0  = unsigned(Clamp(fy, 0.f, float(dims[1]-1)));
            y1  = std::min(y0+1u, dims[1]-1u);
            y2  = std::min(y1+1u, dims[1]-1u);
            yn1 = std::max(y0, 1u) - 1u;
        }

        float u[] =
        {
            (*field._u)[yn1*dims[0]+xn1],
            (*field._u)[yn1*dims[0]+ x0],
            (*field._u)[yn1*dims[0]+ x1],
            (*field._u)[yn1*dims[0]+ x2],

            (*field._u)[ y0*dims[0]+xn1],
            (*field._u)[ y0*dims[0]+ x0],
            (*field._u)[ y0*dims[0]+ x1],
            (*field._u)[ y0*dims[0]+ x2],

            (*field._u)[ y1*dims[0]+xn1],
            (*field._u)[ y1*dims[0]+ x0],
            (*field._u)[ y1*dims[0]+ x1],
            (*field._u)[ y1*dims[0]+ x2],

            (*field._u)[ y2*dims[0]+xn1],
            (*field._u)[ y2*dims[0]+ x0],
            (*field._u)[ y2*dims[0]+ x1],
            (*field._u)[ y2*dims[0]+ x2]
        };

        float v[] =
        {
            (*field._v)[yn1*dims[0]+xn1],
            (*field._v)[yn1*dims[0]+ x0],
            (*field._v)[yn1*dims[0]+ x1],
            (*field._v)[yn1*dims[0]+ x2],

            (*field._v)[ y0*dims[0]+xn1],
            (*field._v)[ y0*dims[0]+ x0],
            (*field._v)[ y0*dims[0]+ x1],
            (*field._v)[ y0*dims[0]+ x2],

            (*field._v)[ y1*dims[0]+xn1],
            (*field._v)[ y1*dims[0]+ x0],
            (*field._v)[ y1*dims[0]+ x1],
            (*field._v)[ y1*dims[0]+ x2],

            (*field._v)[ y2*dims[0]+xn1],
            (*field._v)[ y2*dims[0]+ x0],
            (*field._v)[ y2*dims[0]+ x1],
            (*field._v)[ y2*dims[0]+ x2]
        };

            // Unfortunately, to get correct cubic interpolation in 2D, we
            // have to do a lot of interpolations (similar to the math used
            // in refining cubic surfaces)
            //
            // We can optimise this by only doing the monotonic interpolation
            // of the U parameter in the U direction (and the V parameter in
            // the V direction). Maybe we could fall back to linear in the
            // cross-wise direction.
            //
            // It's hard to know what effect that optimisation would have on
            // the advection operation.
        float un1 = MonotonicCubic(u[ 0], u[ 1], u[ 2], u[ 3], a);
        float u0  = MonotonicCubic(u[ 4], u[ 5], u[ 6], u[ 7], a);
        float u1  = MonotonicCubic(u[ 8], u[ 9], u[10], u[11], a);
        float u2  = MonotonicCubic(u[12], u[13], u[14], u[15], a);
        float fu =  MonotonicCubic(un1, u0, u1, u2, b);

        float vn1 = MonotonicCubic(v[ 0], v[ 1], v[ 2], v[ 3], a);
        float v0  = MonotonicCubic(v[ 4], v[ 5], v[ 6], v[ 7], a);
        float v1  = MonotonicCubic(v[ 8], v[ 9], v[10], v[11], a);
        float v2  = MonotonicCubic(v[12], v[13], v[14], v[15], a);
        float fv =  MonotonicCubic(vn1, v0, v1, v2, b);
        return Float2(fu, fv);
    }

///////////////////////////////////////////////////////////////////////////////////////////////////
            //   G A T H E R   N E I G H B O R S
///////////////////////////////////////////////////////////////////////////////////////////////////

    template<typename Store>
        void GatherNeighbors(
            Float2 neighbours[9], float weights[4],
            const VectorField2DSeparate<Store>& field, Float2 coord, unsigned samplingFlags)
    {
        float fx = XlFloor(coord[0]);
        float fy = XlFloor(coord[1]);
        float a = coord[0] - fx, b = coord[1] - fy;
        weights[0] = (1.f - a) * (1.f - b);
        weights[1] = a * (1.f - b);
        weights[2] = (1.f - a) * b;
        weights[3] = a * b;

        const auto dims = field.Dimensions();
        unsigned x0, x1, y0, y1;
        unsigned xx, yx;
        if (samplingFlags & RNFSample::WrapX) {
            x0 = unsigned((int(fx) + int(dims[0]))%dims[0]);
            x1 = (x0+1u)%dims[0];
            xx = (x0+dims[0]-1)%dims[0];
        } else {
            x0 = unsigned(Clamp(fx, 0.f, float(dims[0]-1)));
            x1 = std::min(x0+1u, dims[0]-1u);
            xx = std::max(x0, 1u) - 1u;
        }

        if (samplingFlags & RNFSample::WrapY) {
            y0 = unsigned((int(fy) + int(dims[1]))%dims[1]);
            y1 = (y0+1u)%dims[1];
            yx = (y0+dims[1]-1)%dims[1];
        } else {
            y0 = unsigned(Clamp(fy, 0.f, float(dims[1]-1)));
            y1 = std::min(y0+1u, dims[1]-1u);
            yx = std::max(y0, 1u) - 1u;
        }

        neighbours[0][0] = (*field._u)[y0*dims[0]+x0];
        neighbours[1][0] = (*field._u)[y0*dims[0]+x1];
        neighbours[2][0] = (*field._u)[y1*dims[0]+x0];
        neighbours[3][0] = (*field._u)[y1*dims[0]+x1];

        neighbours[4][0] = (*field._u)[yx*dims[0]+xx];
        neighbours[5][0] = (*field._u)[yx*dims[0]+x0];
        neighbours[6][0] = (*field._u)[yx*dims[0]+x1];
        neighbours[7][0] = (*field._u)[y0*dims[0]+xx];
        neighbours[8][0] = (*field._u)[y1*dims[0]+xx];

        neighbours[0][1] = (*field._v)[y0*dims[0]+x0];
        neighbours[1][1] = (*field._v)[y0*dims[0]+x1];
        neighbours[2][1] = (*field._v)[y1*dims[0]+x0];
        neighbours[3][1] = (*field._v)[y1*dims[0]+x1];

        neighbours[4][1] = (*field._v)[yx*dims[0]+xx];
        neighbours[5][1] = (*field._v)[yx*dims[0]+x0];
        neighbours[6][1] = (*field._v)[yx*dims[0]+x1];
        neighbours[7][1] = (*field._v)[y0*dims[0]+xx];
        neighbours[8][1] = (*field._v)[y1*dims[0]+xx];
    }

    template<typename Store>
        void GatherNeighbors(
            float neighbours[8], float weights[4],
            const ScalarField2D<Store>& field, Float2 coord, unsigned samplingFlags)
    {
        float fx = XlFloor(coord[0]);
        float fy = XlFloor(coord[1]);
        float a = coord[0] - fx, b = coord[1] - fy;
        weights[0] = (1.f - a) * (1.f - b);
        weights[1] = a * (1.f - b);
        weights[2] = (1.f - a) * b;
        weights[3] = a * b;

        const auto dims = field.Dimensions();
        unsigned x0, x1, y0, y1;
        unsigned xx, yx;
        if (samplingFlags & RNFSample::WrapX) {
            x0 = unsigned((int(fx) + int(dims[0]))%dims[0]);
            x1 = (x0+1u)%dims[0];
            xx = (x0+dims[0]-1)%dims[0];
        } else {
            x0 = unsigned(Clamp(fx, 0.f, float(dims[0]-1)));
            x1 = std::min(x0+1u, dims[0]-1u);
            xx = std::max(x0, 1u) - 1u;
        }

        if (samplingFlags & RNFSample::WrapY) {
            y0 = unsigned((int(fy) + int(dims[1]))%dims[1]);
            y1 = (y0+1u)%dims[1];
            yx = (y0+dims[1]-1)%dims[1];
        } else {
            y0 = unsigned(Clamp(fy, 0.f, float(dims[1]-1)));
            y1 = std::min(y0+1u, dims[1]-1u);
            yx = std::max(y0, 1u) - 1u;
        }

        neighbours[0] = (*field._u)[y0*dims[0]+x0];
        neighbours[1] = (*field._u)[y0*dims[0]+x1];
        neighbours[2] = (*field._u)[y1*dims[0]+x0];
        neighbours[3] = (*field._u)[y1*dims[0]+x1];

        neighbours[4] = (*field._u)[yx*dims[0]+xx];
        neighbours[5] = (*field._u)[yx*dims[0]+x0];
        neighbours[6] = (*field._u)[yx*dims[0]+x1];
        neighbours[7] = (*field._u)[y0*dims[0]+xx];
        neighbours[8] = (*field._u)[y1*dims[0]+xx];
    }

    static void GatherNeighbors(float* result, size_t stride, const float* source, UInt3 base, UInt3 dims, unsigned samplingFlags)
    {
        auto x0 = base[0], y0 = base[1], z0 = base[2];
        unsigned x1, y1, z1, xx, yx, zx;
        if (samplingFlags & RNFSample::WrapX) {
            x1 = (x0+1u)%dims[0];
            xx = (x0+dims[0]-1u)%dims[0];
        } else {
            x1 = std::min(x0+1u, dims[0]-1u);
            xx = std::max(x0, 1u)-1u;
        }

        if (samplingFlags & RNFSample::WrapY) {
            y1 = (y0+1u)%dims[1];
            yx = (y0+dims[1]-1u)%dims[1];
        } else {
            y1 = std::min(y0+1u, dims[1]-1u);
            yx = std::max(y0, 1u)-1u;
        }

        if (samplingFlags & RNFSample::WrapY) {
            z1 = (z0+1u)%dims[2];
            zx = (z0+dims[2]-1u)%dims[2];
        } else {
            z1 = std::min(z0+1u, dims[2]-1u);
            zx = std::max(z0, 1u)-1u;
        }

        #define V(x,y,z) source[(z*dims[1]+y)*dims[0]+x]
        #define R(x) result[x*stride]

            // results are arranged so the first 8 are the ones used for
            // bilinear interpolation
        R( 0) = V(x0, y0, z0);
        R( 1) = V(x1, y0, z0);
        R( 2) = V(x0, y1, z0);
        R( 3) = V(x1, y1, z0);
        R( 4) = V(x0, y0, z1);
        R( 5) = V(x1, y0, z1);
        R( 6) = V(x0, y1, z1);
        R( 7) = V(x1, y1, z1);

        R( 8) = V(xx, yx, z0);
        R( 9) = V(x0, yx, z0);
        R(10) = V(x1, yx, z0);
        R(11) = V(xx, y0, z0);
        R(12) = V(xx, y1, z0);

        R(13) = V(xx, yx, z1);
        R(14) = V(x0, yx, z1);
        R(15) = V(x1, yx, z1);
        R(16) = V(xx, y0, z1);
        R(17) = V(xx, y1, z1);

        R(18) = V(xx, yx, zx);
        R(19) = V(x0, yx, zx);
        R(20) = V(x1, yx, zx);
        R(21) = V(xx, y0, zx);
        R(22) = V(x0, y0, zx);
        R(23) = V(x1, y0, zx);
        R(24) = V(xx, y1, zx);
        R(25) = V(x0, y1, zx);
        R(26) = V(x1, y1, zx);

        #undef V
        #undef R
    }

    template<typename Store>
        void GatherNeighbors(
            Float3 neighbours[27], float weights[8],
            const VectorField3DSeparate<Store>& field, Float3 coord, unsigned samplingFlags)
    {
        float fx = XlFloor(coord[0]);
        float fy = XlFloor(coord[1]);
        float fz = XlFloor(coord[2]);
        float a = coord[0] - fx, b = coord[1] - fy, c = coord[2] - fz;
        weights[0] = (1.f - a) * (1.f - b) * (1.f - c);
        weights[1] = a * (1.f - b) * (1.f - c);
        weights[2] = (1.f - a) * b * (1.f - c);
        weights[3] = a * b * (1.f - c);
        weights[4] = (1.f - a) * (1.f - b) * c;
        weights[5] = a * (1.f - b) * c;
        weights[6] = (1.f - a) * b * c;
        weights[7] = a * b * c;

        const auto dims = field.Dimensions();
        unsigned x0, y0, z0;
        if (samplingFlags & RNFSample::WrapX) {
            x0 = unsigned((int(fx) + int(dims[0]))%dims[0]);
        } else {
            x0 = unsigned(Clamp(fx, 0.f, float(dims[0]-1)));
        }

        if (samplingFlags & RNFSample::WrapY) {
            y0 = unsigned((int(fy) + int(dims[1]))%dims[1]);
        } else {
            y0 = unsigned(Clamp(fy, 0.f, float(dims[1]-1)));
        }

        if (samplingFlags & RNFSample::WrapY) {
            z0 = unsigned((int(fz) + int(dims[2]))%dims[2]);
        } else {
            z0 = unsigned(Clamp(fz, 0.f, float(dims[2]-1)));
        }

        GatherNeighbors(&neighbours[0][0], 3, &(*field._u)[0], UInt3(x0, y0, z0), dims, samplingFlags);
        GatherNeighbors(&neighbours[0][1], 3, &(*field._v)[0], UInt3(x0, y0, z0), dims, samplingFlags);
        GatherNeighbors(&neighbours[0][2], 3, &(*field._w)[0], UInt3(x0, y0, z0), dims, samplingFlags);
    }

    template<typename Store>
        void GatherNeighbors(
            float neighbours[27], float weights[8],
            const ScalarField3D<Store>& field, Float3 coord, unsigned samplingFlags)
    {
        float fx = XlFloor(coord[0]);
        float fy = XlFloor(coord[1]);
        float fz = XlFloor(coord[2]);
        float a = coord[0] - fx, b = coord[1] - fy, c = coord[2] - fz;
        weights[0] = (1.f - a) * (1.f - b) * (1.f - c);
        weights[1] = a * (1.f - b) * (1.f - c);
        weights[2] = (1.f - a) * b * (1.f - c);
        weights[3] = a * b * (1.f - c);
        weights[4] = (1.f - a) * (1.f - b) * c;
        weights[5] = a * (1.f - b) * c;
        weights[6] = (1.f - a) * b * c;
        weights[7] = a * b * c;

        const auto dims = field.Dimensions();
        unsigned x0, y0, z0;
        if (samplingFlags & RNFSample::WrapX) {
            x0 = unsigned((int(fx) + int(dims[0]))%dims[0]);
        } else {
            x0 = unsigned(Clamp(fx, 0.f, float(dims[0]-1)));
        }

        if (samplingFlags & RNFSample::WrapY) {
            y0 = unsigned((int(fy) + int(dims[1]))%dims[1]);
        } else {
            y0 = unsigned(Clamp(fy, 0.f, float(dims[1]-1)));
        }

        if (samplingFlags & RNFSample::WrapY) {
            z0 = unsigned((int(fz) + int(dims[2]))%dims[2]);
        } else {
            z0 = unsigned(Clamp(fz, 0.f, float(dims[2]-1)));
        }

        GatherNeighbors(neighbours, 1, &(*field._u)[0], UInt3(x0, y0, z0), dims, samplingFlags);
    }

///////////////////////////////////////////////////////////////////////////////////////////////////

    template<typename Store>
        float ScalarField2D<Store>::Load(Coord coord) const
    {
        assert(coord[0] < _dims[0] && coord[1] < _dims[1]);
        return (*_u)[coord[1] * _dims[0] + coord[0]];
    }

    template<typename Store>
        void ScalarField2D<Store>::Write(Coord coord, ValueType value)
    {
        assert(coord[0] < _dims[0] && coord[1] < _dims[1]);
        (*_u)[coord[1] * _dims[0] + coord[0]] = value;
        assert(std::isfinite(value) && !std::isnan(value));
    }

    template<typename Store>
    template<unsigned SamplingFlags>
        auto ScalarField2D<Store>::Sample(FloatCoord c) const -> ValueType
    {
        if (constant_expression<(SamplingFlags & RNFSample::Cubic)!=0>::result())
            return SampleMonotonicCubic<SamplingFlags>(*this, c);
        return SampleBilinear<SamplingFlags>(*this, c);
    }

    template<typename Store>
        void ScalarField2D<Store>::GatherNeighbors(ValueType neighbours[9], float weights[4], FloatCoord coord, unsigned samplingFlags) const
    {
        XLEMath::GatherNeighbors(neighbours, weights, *this, coord, samplingFlags);
    }

///////////////////////////////////////////////////////////////////////////////////////////////////

    template<typename Store>
        Float2 VectorField2DSeparate<Store>::Load(Coord coord) const
    {
        assert(coord[0] < _dims[0] && coord[1] < _dims[1]);
        return Float2(
            (*_u)[coord[1] * _dims[0] + coord[0]],
            (*_v)[coord[1] * _dims[0] + coord[0]]);
    }

    template<typename Store>
        void VectorField2DSeparate<Store>::Write(Coord coord, ValueType value)
    {
        assert(coord[0] < _dims[0] && coord[1] < _dims[1]);
        (*_u)[coord[1] * _dims[0] + coord[0]] = value[0];
        (*_v)[coord[1] * _dims[0] + coord[0]] = value[1];
        assert(std::isfinite(value[0]) && !std::isnan(value[0]));
        assert(std::isfinite(value[1]) && !std::isnan(value[1]));
    }

    template<typename Store>
    template<unsigned SamplingFlags>
        auto VectorField2DSeparate<Store>::Sample(FloatCoord c) const -> ValueType
    {
        if (constant_expression<(SamplingFlags & RNFSample::Cubic)!=0>::result())
            return SampleMonotonicCubic<SamplingFlags>(*this, c);
        return SampleBilinear<SamplingFlags>(*this, c);
    }

    template<typename Store>
        void VectorField2DSeparate<Store>::GatherNeighbors(ValueType neighbours[9], float weights[4], FloatCoord coord, unsigned samplingFlags) const
    {
        XLEMath::GatherNeighbors(neighbours, weights, *this, coord, samplingFlags);
    }

///////////////////////////////////////////////////////////////////////////////////////////////////

    template<typename Store>
        float ScalarField3D<Store>::Load(Coord coord) const
    {
        assert(coord[0] < _dims[0] && coord[1] < _dims[1]);
        return (*_u)[(coord[2] * _dims[1] + coord[1]) * _dims[0] + coord[0]];
    }

    template<typename Store>
        void ScalarField3D<Store>::Write(Coord coord, ValueType value)
    {
        assert(coord[0] < _dims[0] && coord[1] < _dims[1]);
        (*_u)[(coord[2] * _dims[1] + coord[1]) * _dims[0] + coord[0]] = value;
        assert(std::isfinite(value) && !std::isnan(value));
    }

    template<typename Store>
    template<unsigned SamplingFlags>
        auto ScalarField3D<Store>::Sample(FloatCoord c) const -> ValueType
    {
        // if (constant_expression<(SamplingFlags & RNFSample::Cubic)!=0>::result())
        //     return SampleMonotonicCubic<SamplingFlags>(*this, c);
        return SampleBilinear<SamplingFlags>(*this, c);
    }

    template<typename Store>
        void ScalarField3D<Store>::GatherNeighbors(ValueType neighbours[27], float weights[4], FloatCoord coord, unsigned samplingFlags) const
    {
        XLEMath::GatherNeighbors(neighbours, weights, *this, coord, samplingFlags);
    }

///////////////////////////////////////////////////////////////////////////////////////////////////

    template<typename Store>
        Float3 VectorField3DSeparate<Store>::Load(Coord coord) const
    {
        assert(coord[0] < _dims[0] && coord[1] < _dims[1]);
        return Float3(
            (*_u)[(coord[2] * _dims[1] + coord[1]) * _dims[0] + coord[0]],
            (*_v)[(coord[2] * _dims[1] + coord[1]) * _dims[0] + coord[0]],
            (*_w)[(coord[2] * _dims[1] + coord[1]) * _dims[0] + coord[0]]);
    }

    template<typename Store>
        void VectorField3DSeparate<Store>::Write(Coord coord, ValueType value)
    {
        assert(coord[0] < _dims[0] && coord[1] < _dims[1] && coord[2] < _dims[2]);
        (*_u)[(coord[2] * _dims[1] + coord[1]) * _dims[0] + coord[0]] = value[0];
        (*_v)[(coord[2] * _dims[1] + coord[1]) * _dims[0] + coord[0]] = value[1];
        (*_w)[(coord[2] * _dims[1] + coord[1]) * _dims[0] + coord[0]] = value[2];
        assert(std::isfinite(value[0]) && !std::isnan(value[0]));
        assert(std::isfinite(value[1]) && !std::isnan(value[1]));
    }

    template<typename Store>
    template<unsigned SamplingFlags>
        auto VectorField3DSeparate<Store>::Sample(FloatCoord c) const -> ValueType
    {
        // if (constant_expression<(SamplingFlags & RNFSample::Cubic)!=0>::result())
        //     return SampleMonotonicCubic<SamplingFlags>(*this, c);
        return SampleBilinear<SamplingFlags>(*this, c);
    }

    template<typename Store>
        void VectorField3DSeparate<Store>::GatherNeighbors(ValueType neighbours[27], float weights[4], FloatCoord coord, unsigned samplingFlags) const
    {
        XLEMath::GatherNeighbors(neighbours, weights, *this, coord, samplingFlags);
    }

///////////////////////////////////////////////////////////////////////////////////////////////////

#if COMPILER_ACTIVE == COMPILER_TYPE_MSVC
    template<typename Field>
        unsigned InstantiateField()
        {
            using SampleFn = typename Field::ValueType(Field::*)(typename Field::FloatCoord)const;
            SampleFn fns[14] = {
                &Field::Sample<0>,
                &Field::Sample<RNFSample::ClampX|RNFSample::ClampY|RNFSample::ClampZ>,
                &Field::Sample<RNFSample::WrapX |RNFSample::ClampY|RNFSample::ClampZ>,
                &Field::Sample<RNFSample::ClampX|RNFSample::WrapY |RNFSample::ClampZ>,
                &Field::Sample<RNFSample::ClampX|RNFSample::ClampY|RNFSample::WrapZ >,
                &Field::Sample<RNFSample::WrapX |RNFSample::WrapY |RNFSample::WrapZ >,
                &Field::Sample<RNFSample::WrapX |RNFSample::WrapY |RNFSample::ClampZ >,
                &Field::Sample<RNFSample::Cubic>,
                &Field::Sample<RNFSample::Cubic|RNFSample::ClampX|RNFSample::ClampY|RNFSample::ClampZ>,
                &Field::Sample<RNFSample::Cubic|RNFSample::WrapX |RNFSample::ClampY|RNFSample::ClampZ>,
                &Field::Sample<RNFSample::Cubic|RNFSample::ClampX|RNFSample::WrapY |RNFSample::ClampZ>,
                &Field::Sample<RNFSample::Cubic|RNFSample::ClampX|RNFSample::ClampY|RNFSample::WrapZ >,
                &Field::Sample<RNFSample::Cubic|RNFSample::WrapX |RNFSample::WrapY |RNFSample::WrapZ >,
                &Field::Sample<RNFSample::Cubic|RNFSample::WrapX |RNFSample::WrapY |RNFSample::ClampZ >
            };
            (void)fns;
            return 0;
        }

    static const unsigned s_i[] =
    {
        InstantiateField<ScalarField2D<Eigen::VectorXf>>(),
        InstantiateField<VectorField2DSeparate<Eigen::VectorXf>>(),
        InstantiateField<ScalarField3D<Eigen::VectorXf>>(),
        InstantiateField<VectorField3DSeparate<Eigen::VectorXf>>()
    };
#endif

    template class ScalarField2D<Eigen::VectorXf>;
    template class VectorField2DSeparate<Eigen::VectorXf>;
    template class ScalarField3D<Eigen::VectorXf>;
    template class VectorField3DSeparate<Eigen::VectorXf>;

}


// Copyright (c) 2022 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <gsl/gsl_spline2d.h>

#include <Eigen/Core>
#include <string>

#include "pyinterp/detail/gsl/interpolate2d.hpp"
#include "pyinterp/detail/math/frame.hpp"

namespace pyinterp::detail::math {

/// Bicubic interpolation
class Bicubic {
 public:
  /// Default constructor
  ///
  /// @param xr Calculation window.
  /// @param kind method of calculation
  explicit Bicubic(const Frame2D& xr, const std::string& kind)
      : interpolator_(xr.x()->size(), xr.y()->size(),
                      Bicubic::parse_interp2d_type(kind), gsl::Accelerator(),
                      gsl::Accelerator()) {}

  /// Return the interpolated value of y for a given point x
  auto interpolate(const double x, const double y, const Frame2D& xr)
      -> double {
    return interpolator_.evaluate(*(xr.x()), *(xr.y()), *(xr.q()), x, y);
  }

 private:
  /// GSL interpolator
  gsl::Interpolate2D interpolator_;

  static inline auto parse_interp2d_type(const std::string& kind)
      -> const gsl_interp2d_type* {
    if (kind == "bilinear") {
      return gsl_interp2d_bilinear;
    }
    if (kind == "bicubic") {
      return gsl_interp2d_bicubic;
    }
    throw std::invalid_argument("Invalid bicubic type: " + kind);
  }
};

}  // namespace pyinterp::detail::math


//==============================================================================
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/include/functions/log.hpp>
#include <nt2/include/functions/exp.hpp>
#include <nt2/include/functions/fastnormcdf.hpp>
#include <boost/simd/include/functions/fma.hpp>
#include <boost/simd/include/functions/fnms.hpp>
#include <boost/simd/include/functions/sqrt.hpp>
#include <boost/simd/include/functions/sqr.hpp>
#include <boost/simd/include/functions/aligned_store.hpp>
#include <boost/simd/include/functions/aligned_load.hpp>
#include <boost/simd/include/functions/divides.hpp>
#include <boost/simd/include/functions/multiplies.hpp>
#include <boost/simd/include/functions/unary_minus.hpp>
#include <boost/simd/include/functions/plus.hpp>
#include <boost/simd/include/functions/minus.hpp>
#include <boost/simd/include/constants/half.hpp>
#include <boost/simd/memory/allocator.hpp>
#include <vector>
#include <iostream>

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/metric/cycles_per_element.hpp>
#include <nt2/sdk/bench/protocol/max_duration.hpp>
#include <nt2/sdk/bench/setup/geometric.hpp>
#include <nt2/sdk/bench/stats/median.hpp>

using namespace nt2::bench;
using namespace nt2;

template <class A0>
BOOST_FORCEINLINE A0 blackandscholes(A0 const &a0, A0 const &a1, A0 const &a2, A0 const &a3, A0 const &a4)
{
  A0 da   = boost::simd::sqrt(a2);
  A0 tmp1 = nt2::log(a0/a1);
  A0 tmp2 = boost::simd::sqr(a4);
  A0 tmp4 = boost::simd::fma(tmp2,nt2::Half<A0>(),a3);
  A0 tmp3 = (tmp4*a2)/(a4*da);
  A0 ed   = nt2::exp(-a3*a2);
  A0 d1   = tmp1 + tmp3;
  A0 d2   = boost::simd::fnms(a4,da,d1);
  A0 fd1  = nt2::fastnormcdf(d1);
  A0 fd2  = nt2::fastnormcdf(d2);
  return boost::simd::fnms(a1*ed, fd2, a0*fd1);
}

template<typename T> struct blackandscholes_simd
{
  blackandscholes_simd(std::size_t n)
                    :  size_(n)
  {
    Sa.resize(size_);
    Xa.resize(size_);
    Ta.resize(size_);
    ra.resize(size_);
    va.resize(size_);
    R.resize(size_);

    for(std::size_t i = 0; i <size_; ++i)
      Sa[i] = Xa[i] = Ta[i] = ra[i] = va[i] = T(i+1);
  }

  void operator()()
  {
    using boost::simd::pack;
    using boost::simd::native;
    using boost::simd::aligned_store;
    using boost::simd::aligned_load;

    typedef native<T,BOOST_SIMD_DEFAULT_EXTENSION> type;
    std::size_t step_size_=boost::simd::meta::cardinal_of<type>::value;
    std::size_t i=0;

    while (size_-i>=step_size_)
    {
      type Sa_tmp = aligned_load<type>(&Sa[i]);
      type Xa_tmp = aligned_load<type>(&Xa[i]);
      type Ta_tmp = aligned_load<type>(&Ta[i]);
      type ra_tmp = aligned_load<type>(&ra[i]);
      type va_tmp = aligned_load<type>(&va[i]);
      aligned_store(blackandscholes(Sa_tmp, Xa_tmp, Ta_tmp, ra_tmp, va_tmp), &R[i]);
      i += step_size_;
    }
    for (; i<size_; i++)
      R[i] += blackandscholes(Sa[i], Xa[i], Ta[i], ra[i], va[i]);
  }

  friend std::ostream& operator<<(std::ostream& os, blackandscholes_simd<T> const& p)
  {
    return os << "(" << p.size() << ")";
  }

  std::size_t size() const { return size_; }

  private:
  std::vector<T, boost::simd::allocator<T> > Sa, Xa, Ta, ra, va, R;
  std::size_t size_;
};

NT2_REGISTER_BENCHMARK_TPL( blackandscholes_simd, (float) )
{
  std::size_t size_min  = args("size_min",   16);
  std::size_t size_max  = args("size_max", 4096);
  std::size_t size_step = args("size_step",   2);

  run_during_with< blackandscholes_simd<float> > ( 1.
                                                 , geometric(size_min,size_max,size_step)
                                                 , cycles_per_element<stats::median_>()
                                                 );
}


#ifndef PICSAR_MULTIPHYSICS_QUADRATURE
#define PICSAR_MULTIPHYSICS_QUADRATURE

//Should be included by all the src files of the library
#include "../qed_commons.h"

// Override BOOST_ASSERT so that an exception is thrown.
// This is used to deal with some possible numerical
// instabilities of the tanh_sinh integration method
#define BOOST_ENABLE_ASSERT_HANDLER
#include <boost/assert.hpp>
#include <boost/math/quadrature/trapezoidal.hpp>
#include <boost/math/quadrature/tanh_sinh.hpp>
#include <boost/math/quadrature/exp_sinh.hpp>
#include <boost/math/quadrature/gauss_kronrod.hpp>
#undef BOOST_ENABLE_ASSERT_HANDLER

#include <functional>
#include <limits>
#include <stdexcept>
#include <sstream>

// Override BOOST_ASSERT so that an exception is thrown.
namespace boost
{
    inline
    void assertion_failed(char const * expr,
        char const * function,
        char const * file, long line)
        {
            auto ss = std::stringstream();
            ss << "Error in " << function <<
            " (" << file << ", line " << line <<
            "): " << expr;
            throw std::runtime_error(ss.str());
        }
}
//______________________________________________________

namespace picsar{
namespace multi_physics{
namespace math{
    /**
    * This module is a wrapper around the trapezoidal,
    * gauss_kronrod, tanh_sinh & exp_sinh quadrature methods provided by the Boost library.
    * All the functions provided here accept a quadrature_algorithm template
    * parameter to choose which method will be used.
    */
    enum quadrature_algorithm {
        trapezoidal,
        tanh_sinh,
        exp_sinh,
        gauss_kronrod15,
        gauss_kronrod31,
        gauss_kronrod41,
        gauss_kronrod51,
        gauss_kronrod61
    };

    /**
    * This function performs the integration of the function f(x)
    * in the interval (a,b) using the method specified in the template parameter
    * (not usable on GPUs).
    *
    * @tparam RealType the floating point type to be used
    * @tparam QuadAlgo the quadrature method to be used
    * @param[in] f the function which should be integrated
    * @param[in] a the left boundary of the integration region
    * @param[in] b the right boundary of the integration region
    * @return the integral of f in (a,b)
    */
    template<
        typename RealType, quadrature_algorithm QuadAlgo>
    inline constexpr RealType generic_quad_a_b(
        const std::function<RealType(RealType)>& f, RealType a, RealType b)
    {
        PXRMP_INTERNAL_CONSTEXPR_IF (
            QuadAlgo == quadrature_algorithm::trapezoidal){
            return boost::math::quadrature::trapezoidal(f, a, b);
        }
        else PXRMP_INTERNAL_CONSTEXPR_IF (
            QuadAlgo == quadrature_algorithm::tanh_sinh){
            boost::math::quadrature::tanh_sinh<RealType> integrator;
            return integrator.integrate(f, a, b);
        }
        else PXRMP_INTERNAL_CONSTEXPR_IF (
            QuadAlgo == quadrature_algorithm::exp_sinh){
            boost::math::quadrature::exp_sinh<RealType> integrator;
            return integrator.integrate(f, a, b);
        }
        else PXRMP_INTERNAL_CONSTEXPR_IF (
            QuadAlgo == quadrature_algorithm::gauss_kronrod15){
            return boost::math::quadrature::gauss_kronrod<RealType, 15>
                ::integrate(f, a, b);
        }
        else PXRMP_INTERNAL_CONSTEXPR_IF (
            QuadAlgo == quadrature_algorithm::gauss_kronrod31){
            return boost::math::quadrature::gauss_kronrod<RealType, 31>
                ::integrate(f, a, b);
        }
        else PXRMP_INTERNAL_CONSTEXPR_IF (
            QuadAlgo == quadrature_algorithm::gauss_kronrod41){
            return boost::math::quadrature::gauss_kronrod<RealType, 41>
                ::integrate(f, a, b);
        }
        else PXRMP_INTERNAL_CONSTEXPR_IF (
            QuadAlgo == quadrature_algorithm::gauss_kronrod51){
            return boost::math::quadrature::gauss_kronrod<RealType, 51>
                ::integrate(f, a, b);
        }
        else PXRMP_INTERNAL_CONSTEXPR_IF (
            QuadAlgo == quadrature_algorithm::gauss_kronrod61){
            return boost::math::quadrature::gauss_kronrod<RealType, 61>
                ::integrate(f, a, b);
        }
        else
        {
            return boost::math::quadrature::trapezoidal(f, a, b);
        }
    }

    /**
    * This function performs the integration of the function f(x)
    * in the finite interval (a,b), using the "trapezoidal" quadrature method
    * (not usable on GPUs).
    *
    * @tparam RealType the floating point type to be used
    * @param[in] f the function which should be integrated
    * @param[in] a the left boundary of the integration region
    * @param[in] b the right boundary of the integration region
    * @return the integral of f in (a,b)
    */
    template<typename RealType>
    inline constexpr RealType quad_a_b(
        const std::function<RealType(RealType)>& f, RealType a, RealType b)
    {
        return generic_quad_a_b<
            RealType, quadrature_algorithm::gauss_kronrod61>(f, a, b);
    }

    /**
    * This function performs the integration of the function f(x)
    * in the finite interval (a,b) using the "tanh_sinh" quadrature method,
    * to deal with possibile singularities at the boundaries
    * (not usable on GPUs).
    *
    * @tparam RealType the floating point type to be used
    * @param[in] f the function which should be integrated
    * @param[in] a the left boundary of the integration region
    * @param[in] b the right boundary of the integration region
    * @return the integral of f in (a,b)
    */
    template<typename RealType>
    inline constexpr RealType quad_a_b_s(
        const std::function<RealType(RealType)>& f, RealType a, RealType b)
    {
         return generic_quad_a_b<RealType,
            quadrature_algorithm::tanh_sinh>(f, a, b);
    }

    /**
    * This function performs the integration of the function f(x)
    * in the semi-infinite interval (a,inf) using the "exp_sinh" quadrature method
    * (not usable on GPUs).
    *
    * @tparam RealType the floating point type to be used
    * @param[in] a the left boundary of the integration region
    * @param[in] b the right boundary of the integration region
    * @return the integral of f in (a,b)
    */
    template<typename RealType>
    PXRMP_INTERNAL_FORCE_INLINE_DECORATOR
    constexpr RealType quad_a_inf(
        const std::function<RealType(RealType)>& f, RealType a)
    {
        return generic_quad_a_b<RealType, quadrature_algorithm::exp_sinh>(
            f, a, std::numeric_limits<RealType>::infinity());
    }
}
}
}

#endif //PICSAR_MULTIPHYSICS_QUADRATURE


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_HYPOT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_HYPOT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/maxexponentm1.hpp>
#include <boost/simd/constant/minexponent.hpp>
#include <boost/simd/function/simd/abs.hpp>
#include <boost/simd/function/simd/exponent.hpp>
#include <boost/simd/function/simd/ldexp.hpp>
#include <boost/simd/function/simd/max.hpp>
#include <boost/simd/function/simd/min.hpp>
#include <boost/simd/function/simd/plus.hpp>
#include <boost/simd/function/simd/sqr.hpp>
#include <boost/simd/function/simd/sqrt.hpp>
#include <boost/simd/function/simd/unary_minus.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/logical_and.hpp>
#include <boost/simd/function/simd/logical_or.hpp>
#include <boost/simd/function/simd/is_inf.hpp>
#include <boost/simd/function/simd/is_nan.hpp>
#include <boost/simd/constant/inf.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(hypot_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
      {
        using iA0 = bd::as_integer_t<A0>;
        A0 r =  bs::abs(a0);
        A0 i =  bs::abs(a1);
        iA0 e =  exponent(bs::max(i, r));
        e = bs::min(bs::max(e,Minexponent<A0>()),Maxexponentm1<A0>());
        A0 res =  ldexp(sqrt(sqr(ldexp(r, -e))+sqr(ldexp(i, -e))), e);
        #ifndef BOOST_SIMD_NO_INVALIDS
        auto test = logical_or(logical_and(is_nan(a0), is_inf(a1)),
                              logical_and(is_nan(a1), is_inf(a0)));
        return if_else(test, Inf<A0>(), res);
        #else
        return res;
        #endif
      }
   };

} } }

#endif


//
// Created by david on 2018-11-16.
//

#include "matrix_product_stl.h"
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <memory>
#include <general/class_tic_toc.h>
#define profile_matrix_product_dense 1

// Function definitions
template <typename Scalar>
using MatrixType = Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic>;

template <typename Scalar>
using VectorType = Eigen::Matrix<Scalar,Eigen::Dynamic,1>;

template <typename Scalar>
using VectorTypeT = Eigen::Matrix<Scalar,1,Eigen::Dynamic>;

namespace stl_lu{
    std::optional<Eigen::PartialPivLU<MatrixType<double>>               >lu_real;
    std::optional<Eigen::PartialPivLU<MatrixType<std::complex<double>>> >lu_cplx;
    void reset(){
        lu_real.reset();
        lu_cplx.reset();
    }
    template<typename Scalar>
    void init(){
        if constexpr (std::is_same_v<Scalar,double>){
            stl_lu::lu_real   = Eigen::PartialPivLU<MatrixType<Scalar>>();
        }
        if constexpr (std::is_same_v<Scalar,std::complex<double>>)
            stl_lu::lu_cplx = Eigen::PartialPivLU<MatrixType<Scalar>>();
    }

}

template<typename Scalar>
void StlMatrixProduct<Scalar>::init_profiling(){
    t_factorOP = std::make_unique<class_tic_toc>(profile_matrix_product_dense, 5,"Time FactorOp");
    t_multOPv = std::make_unique<class_tic_toc>(profile_matrix_product_dense, 5,"Time MultOpv");
    t_multAx = std::make_unique<class_tic_toc>(profile_matrix_product_dense, 5,"Time MultAx");
}

template<typename Scalar>
StlMatrixProduct<Scalar>::~StlMatrixProduct(){
    stl_lu::reset();
}

// Pointer to data constructor, copies the matrix into an internal Eigen matrix.
template<typename Scalar>
StlMatrixProduct<Scalar>::StlMatrixProduct(
    const Scalar * const A_,
    const int L_,
    const bool copy_data,
    const eigutils::eigSetting::Form form_,
    const eigutils::eigSetting::Side side_):
    A_ptr(A_) ,L(L_), form(form_), side(side_)
{
    if (copy_data){
        A_stl.resize(static_cast<size_t>(L*L));
        std::copy(A_ptr,A_ptr + static_cast<size_t>(L*L), A_stl.begin());
        A_ptr = A_stl.data();
    }
    stl_lu::init<Scalar>();
    init_profiling();
}

template<typename Scalar>
void StlMatrixProduct<Scalar>::print() const {
    Eigen::Map<const MatrixType<Scalar>> A_matrix (A_ptr,L,L);
    std::cout << "A_matrix: \n" << A_matrix << std::endl;
}

template<typename Scalar>
void StlMatrixProduct<Scalar>::FactorOP()

/*  Partial pivot LU decomposition
 *  Factors P(A-sigma*I) = LU
 */
{
    if(readyFactorOp){return;}
//    lu_real_ptr = std::make_shared<LU_REAL>( LU_REAL() );
    Eigen::Map<const MatrixType<Scalar>> A_matrix (A_ptr,L,L);
    t_factorOP->tic();
    assert(readyShift and "Shift value sigma has not been set.");
    if constexpr(std::is_same_v<Scalar,double>){
        stl_lu::lu_real.value().compute(A_matrix - sigmaR * Eigen::MatrixXd::Identity(L,L));
    }
    if constexpr(std::is_same_v<Scalar,std::complex<double>>){
        Scalar sigma = std::complex<double>(sigmaR,sigmaI);
        stl_lu::lu_cplx.value().compute(A_matrix - sigma * Eigen::MatrixXd::Identity(L,L));
    }

    readyFactorOp = true;
    t_factorOP->toc();
//    std::cout << "Time Factor Op [ms]: " << std::fixed << std::setprecision(3) << t_factorOP.get_last_time_interval() * 1000 << '\n';
}

template<typename Scalar>
void StlMatrixProduct<Scalar>::MultOPv(Scalar* x_in_ptr, Scalar* x_out_ptr) {
    using namespace eigutils::eigSetting;
    assert(readyFactorOp and "FactorOp() has not been run yet.");
    t_multOPv->tic();
    switch (side){
        case Side::R: {
            Eigen::Map<VectorType<Scalar>>       x_in    (x_in_ptr,L);
            Eigen::Map<VectorType<Scalar>>       x_out   (x_out_ptr,L);
            if constexpr(std::is_same_v<Scalar,double>)
                x_out.noalias() = stl_lu::lu_real.value().solve(x_in);
            if constexpr(std::is_same_v<Scalar,std::complex<double>>)
                x_out.noalias() = stl_lu::lu_cplx.value().solve(x_in);
            break;
        }
        case Side::L: {
            Eigen::Map<VectorTypeT<Scalar>>       x_in    (x_in_ptr,L);
            Eigen::Map<VectorTypeT<Scalar>>       x_out   (x_out_ptr,L);
            if constexpr(std::is_same_v<Scalar,double>)
                x_out.noalias() = x_in *stl_lu::lu_real.value().inverse();
            if constexpr(std::is_same_v<Scalar,std::complex<double>>)
                x_out.noalias() = x_in *stl_lu::lu_cplx.value().inverse();
            break;
        }
    }
    t_multOPv->toc();
    counter++;
}

template<typename Scalar>
void StlMatrixProduct<Scalar>::MultAx(Scalar* x_in, Scalar* x_out) {
    using namespace eigutils::eigSetting;
    t_multAx->tic();
    Eigen::Map<const MatrixType<Scalar>> A_matrix (A_ptr,L,L);
    switch (form){
        case Form::NONSYMMETRIC:
            switch (side) {
                case Side::R: {
                    Eigen::Map<VectorType<Scalar>> x_vec_in (x_in,  L);
                    Eigen::Map<VectorType<Scalar>> x_vec_out(x_out, L);
                    x_vec_out.noalias() = A_matrix * x_vec_in ;
                    break;
                }
                case Side::L: {
                    Eigen::Map<VectorTypeT<Scalar>> x_vec_in(x_in, L);
                    Eigen::Map<VectorTypeT<Scalar>> x_vec_out(x_out, L);
                    x_vec_out.noalias() = x_vec_in * A_matrix;
                    break;
                }
            }
            break;
        case Form::SYMMETRIC: {
            Eigen::Map<VectorType<Scalar>> x_vec_in(x_in, L);
            Eigen::Map<VectorType<Scalar>> x_vec_out(x_out, L);
            x_vec_out.noalias() = A_matrix.template selfadjointView<Eigen::Lower>() * x_vec_in;
            break;
        }
    }
    t_multAx->toc();
    counter++;
}

// Explicit instantiations

template class StlMatrixProduct<double>;
template class StlMatrixProduct<std::complex<double>>;


#include "conex/cone_program.h"
#include "conex/equality_constraint.h"
#include "conex/linear_constraint.h"
#include "gtest/gtest.h"
#include <Eigen/Dense>

namespace conex {

using Eigen::MatrixXd;
using Eigen::VectorXd;
TEST(EqualityConstraints, Basic) {
  int num_vars = 3;
  int num_equalities = 1;
  int num_inequalities = 4;

  MatrixXd A = MatrixXd::Random(num_inequalities, num_vars);
  MatrixXd C(num_inequalities, 1);
  MatrixXd b(num_vars, 1);

  MatrixXd optimal_slack(num_inequalities, 1);
  MatrixXd optimal_dual(num_inequalities, 1);
  MatrixXd optimal_y(num_vars, 1);

  optimal_slack.setConstant(1);
  optimal_dual.setConstant(1);
  optimal_slack.topRows(num_inequalities * .5).setZero();
  optimal_dual.bottomRows(num_inequalities - num_inequalities * .5).setZero();
  optimal_y = Eigen::MatrixXd::Random(num_vars, 1);

  C = optimal_slack + A * optimal_y;

  LinearConstraint linear_inequality{A, C};

  MatrixXd eq = MatrixXd::Random(num_equalities, num_vars);
  MatrixXd eq_affine(num_equalities, 1);
  eq = Eigen::MatrixXd::Random(num_equalities, num_vars);
  eq_affine = eq * optimal_y;

  Program prog(num_vars);
  prog.AddConstraint(EqualityConstraints{eq, eq_affine});
  prog.AddConstraint(linear_inequality);

  VectorXd linear_cost(num_vars);
  linear_cost = A.transpose() * optimal_dual;

  VectorXd solution(num_vars);
  Solve(linear_cost, prog, conex::SolverConfiguration(), solution.data());

  EXPECT_NEAR((eq * solution - eq_affine).norm(), 0, 1e-5);
  EXPECT_NEAR((solution - optimal_y).norm(), 0, 1e-5);
}
}  // namespace conex


// ---------------------------------------------------------------------
//
// Copyright (c) 2021 - 2021 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

// Headers for basic SAMRAI objects
#include <BergerRigoutsos.h>
#include <CartesianGridGeometry.h>
#include <LoadBalancer.h>
#include <LocationIndexRobinBcCoefs.h>
#include <StandardTagAndInitialize.h>

// Headers for application-specific algorithm/data structure objects
#include <ibamr/AdvDiffSemiImplicitHierarchyIntegrator.h>
#include <ibamr/BrinkmanPenalizationRigidBodyDynamics.h>
#include <ibamr/INSVCStaggeredConservativeHierarchyIntegrator.h>
#include <ibamr/INSVCStaggeredHierarchyIntegrator.h>

#include <ibtk/AppInitializer.h>
#include <ibtk/CartGridFunctionSet.h>
#include <ibtk/HierarchyMathOps.h>
#include <ibtk/IBTKInit.h>
#include <ibtk/muParserCartGridFunction.h>
#include <ibtk/muParserRobinBcCoefs.h>

#include <Eigen/Geometry>

#include <ibamr/app_namespaces.h>

// Application specific includes.
#include "LevelSetInitialCondition.h"
#include "SetFluidSolidDensity.h"
#include "SetFluidSolidViscosity.h"
#include "TagLSRefinementCells.h"

// Declaring the sign of v and returning it.
inline int
sgn(double v)
{
    return ((v < 0) ? -1 : (v > 0) ? 1 : 0);
}

FoilInterface foilA, foilB;

// Struct to reset solid level set
struct SolidLevelSetResetter
{
    SolidLevelSetResetter(Pointer<AdvDiffHierarchyIntegrator> integrator,
                          Pointer<CellVariable<NDIM, double> > var,
                          Pointer<BrinkmanPenalizationRigidBodyDynamics> bp,
                          FoilInterface* foil)
        : adv_diff_integrator(integrator), ls_solid_var(var), bp_rbd(bp), ptr_foil(foil)
    {
        return;
    }

    Pointer<AdvDiffHierarchyIntegrator> adv_diff_integrator;
    Pointer<CellVariable<NDIM, double> > ls_solid_var;
    Pointer<BrinkmanPenalizationRigidBodyDynamics> bp_rbd;
    FoilInterface* ptr_foil;
};

void
reset_solid_level_set_callback_fcn(double current_time, double new_time, int /*cycle_num*/, void* ctx)
{
    SolidLevelSetResetter* resetter = static_cast<SolidLevelSetResetter*>(ctx);
    const FoilInterface& foil = *(resetter->ptr_foil);

    // Get the new centroid of the body
    const double dt = new_time - current_time;
    const Eigen::Vector3d XCOM_current = resetter->bp_rbd->getCurrentCOMPosn();
    const Eigen::Vector3d XCOM_new = XCOM_current + dt * (resetter->bp_rbd->getNewCOMTransVelocity());

    // b) Rotational matrix.
    const double theta = foil.theta_0 * std::sin(2 * M_PI * foil.freq * new_time);

    const Eigen::Vector3d rot_axis(0.0, 0.0, 1.0);
    Eigen::Quaterniond q(Eigen::AngleAxisd(theta, rot_axis));
    q.normalize();

    const Eigen::Matrix3d R_mat = q.toRotationMatrix();

    const double& R = foil.R;
    const Eigen::Vector3d& X0 = foil.X0;
    const Eigen::Vector3d& X1 = foil.X1;
    const Eigen::Vector3d& X2 = foil.X2;
    const Eigen::Vector3d& X3 = foil.X3;
    Eigen::Vector3d check1, check2, check3, check4, check5, check6;

    // Rotate and translate the top, bottom and circular surface to get new coordiantes.
    const Eigen::Vector3d X1_new = R_mat * (X1 - X0) + XCOM_new;
    const Eigen::Vector3d X2_new = R_mat * (X2 - X0) + XCOM_new;
    const Eigen::Vector3d X3_new = R_mat * (X3 - X0) + XCOM_new;

    const Eigen::Vector3d X_T_new = (X1_new + X2_new + X3_new) / 3.0;

    const double slope1 = (X3_new[1] - X1_new[1]) / (X3_new[0] - X1_new[0]);
    const double slope2 = (X3_new[1] - X2_new[1]) / (X3_new[0] - X2_new[0]);
    const double slope3 = (X2_new[0] - X1_new[0]) / (X2_new[1] - X1_new[1]);

    const double y_intercept1 = X1_new[1] - slope1 * X1_new[0];
    const double y_intercept2 = X2_new[1] - slope2 * X2_new[0];
    const double x_intercept3 = X1_new[0] - slope3 * X1_new[1];

    double distance1[2], distance2[3]; // Foil has three surfaces and 1 surface for circle.

    // Set a large value away from the solid body.
    Pointer<PatchHierarchy<NDIM> > patch_hier = resetter->adv_diff_integrator->getPatchHierarchy();
    const int hier_finest_ln = patch_hier->getFinestLevelNumber();

    VariableDatabase<NDIM>* var_db = VariableDatabase<NDIM>::getDatabase();
    const int ls_solid_idx =
        var_db->mapVariableAndContextToIndex(resetter->ls_solid_var, resetter->adv_diff_integrator->getNewContext());

    for (int ln = 0; ln <= hier_finest_ln; ++ln)
    {
        Pointer<PatchLevel<NDIM> > patch_level = patch_hier->getPatchLevel(ln);
        for (PatchLevel<NDIM>::Iterator p(patch_level); p; p++)
        {
            Pointer<Patch<NDIM> > patch = patch_level->getPatch(p());
            const Box<NDIM>& patch_box = patch->getBox();
            const Pointer<CartesianPatchGeometry<NDIM> > patch_geom = patch->getPatchGeometry();
            const double* patch_X_lower = patch_geom->getXLower();
            const hier::Index<NDIM>& patch_lower_idx = patch_box.lower();
            const double* const patch_dx = patch_geom->getDx();

            Pointer<CellData<NDIM, double> > ls_solid_data = patch->getPatchData(ls_solid_idx);
            for (Box<NDIM>::Iterator it(patch_box); it; it++)
            {
                const hier::Index<NDIM>& ci = it();
                Eigen::Vector3d coord = Eigen::Vector3d::Zero();
                for (int d = 0; d < NDIM; ++d)
                {
                    coord[d] = patch_X_lower[d] + patch_dx[d] * (static_cast<double>(ci(d) - patch_lower_idx(d)) + 0.5);
                }

                // Distance from the semi-circle.
                distance1[0] =
                    std::sqrt(std::pow((coord[0] - XCOM_new[0]), 2.0) + std::pow((coord[1] - XCOM_new[1]), 2.0)) - R;

                // Distance from top triangle surface.
                distance2[0] = std::abs(coord[1] - slope1 * coord[0] - y_intercept1) / std::sqrt(1.0 + slope1 * slope1);

                check1 = (X1_new - X3_new).cross(X_T_new - X3_new);
                check2 = (X1_new - X3_new).cross(coord - X3_new);

                distance2[0] *= (-sgn(check1[2]) * sgn(check2[2]));

                // Distance from bottom triangle surface.
                distance2[1] = std::abs(coord[1] - slope2 * coord[0] - y_intercept2) / std::sqrt(1.0 + slope2 * slope2);

                check3 = (X2_new - X3_new).cross(X_T_new - X3_new);
                check4 = (X2_new - X3_new).cross(coord - X3_new);

                distance2[1] *= (-sgn(check3[2]) * sgn(check4[2]));

                // Distance from base of triangle.
                distance2[2] = std::abs(coord[0] - slope3 * coord[1] - x_intercept3) / std::sqrt(1.0 + slope3 * slope3);

                check5 = (X2_new - X1_new).cross(X_T_new - X1_new);
                check6 = (X2_new - X1_new).cross(coord - X1_new);

                distance2[2] *= (-sgn(check5[2]) * sgn(check6[2]));

                distance1[1] =
                    std::max({ distance2[0], distance2[1], distance2[2] }); // intersection of three traingle surfaces

                (*ls_solid_data)(ci) = std::min({ distance1[0], distance1[1] }); // union of circle and triangle
            }
        }
    }

    return;
}

void
imposed_kinematics(double data_time, int /*cycle_num*/, Eigen::Vector3d& U_com, Eigen::Vector3d& W_com, void* ctx)
{
    const FoilInterface& foil = *(static_cast<FoilInterface*>(ctx));
    const double& theta_0 = foil.theta_0;
    const double& freq = foil.freq;

    U_com.setZero();
    W_com.setZero();
    W_com[2] = theta_0 * 2 * M_PI * freq * std::cos(2 * M_PI * freq * data_time);

    return;
} // imposed_kinematics

void
external_force_torque(double /*data_time*/, int /*cycle_num*/, Eigen::Vector3d& F, Eigen::Vector3d& T, void* /*ctx*/)
{
    F.setZero();
    T.setZero();
    return;
} // imposed_kinematics

/*******************************************************************************
 * For each run, the input filename and restart information (if needed) must   *
 * be given on the command line.  For non-restarted case, command line is:     *
 *                                                                             *
 *    executable <input file name>                                             *
 *                                                                             *
 * For restarted run, command line is:                                         *
 *                                                                             *
 *    executable <input file name> <restart directory> <restart number>        *
 *                                                                             *
 *******************************************************************************/
int
main(int argc, char* argv[])
{
    // Initialize PETSc, MPI, and SAMRAI.
    IBTKInit ibtk_init(argc, argv, MPI_COMM_WORLD);

    // Increase maximum patch data component indices
    SAMRAIManager::setMaxNumberPatchDataEntries(2500);

    { // cleanup dynamically allocated objects prior to shutdown

        // Parse command line options, set some standard options from the input
        // file, initialize the restart database (if this is a restarted run),
        // and enable file logging.
        Pointer<AppInitializer> app_initializer = new AppInitializer(argc, argv, "IBLevelSet.log");
        Pointer<Database> input_db = app_initializer->getInputDatabase();

        // Get various standard options set in the input file.
        const bool dump_viz_data = app_initializer->dumpVizData();
        const int viz_dump_interval = app_initializer->getVizDumpInterval();
        const bool uses_visit = dump_viz_data && !app_initializer->getVisItDataWriter().isNull();

        const bool dump_restart_data = app_initializer->dumpRestartData();
        const int restart_dump_interval = app_initializer->getRestartDumpInterval();
        const string restart_dump_dirname = app_initializer->getRestartDumpDirectory();
        if (dump_restart_data && (restart_dump_interval > 0) && !restart_dump_dirname.empty())
        {
            Utilities::recursiveMkdir(restart_dump_dirname);
        }

        const bool dump_postproc_data = app_initializer->dumpPostProcessingData();
        const int postproc_data_dump_interval = app_initializer->getPostProcessingDataDumpInterval();
        const string postproc_data_dump_dirname = app_initializer->getPostProcessingDataDumpDirectory();
        if (dump_postproc_data && (postproc_data_dump_interval > 0) && !postproc_data_dump_dirname.empty())
        {
            Utilities::recursiveMkdir(postproc_data_dump_dirname);
        }

        const bool dump_timer_data = app_initializer->dumpTimerData();
        const int timer_dump_interval = app_initializer->getTimerDumpInterval();

        // Setup solid information
        foilA.R = input_db->getDouble("R");
        foilA.mass = input_db->getDouble("MASS");

        foilA.X0[0] = input_db->getDouble("X_COM");
        foilA.X0[1] = input_db->getDouble("Y_COM");
        foilA.X1[0] = input_db->getDouble("X1");
        foilA.X1[1] = input_db->getDouble("Y1");
        foilA.X2[0] = input_db->getDouble("X2");
        foilA.X2[1] = input_db->getDouble("Y2");
        foilA.X3[0] = input_db->getDouble("X3");
        foilA.X3[1] = input_db->getDouble("Y3");

        foilA.freq = input_db->getDouble("FREQ");
        foilA.theta_0 = input_db->getDouble("THETA_0");

        // Create foilB object
        foilB = foilA;
        foilB.X0[1] -= 30 * foilB.R;
        foilB.X1[1] -= 30 * foilB.R;
        foilB.X2[1] -= 30 * foilB.R;
        foilB.X3[1] -= 30 * foilB.R;

        // Create major algorithm and data objects that comprise the
        // application.  These objects are configured from the input database
        // and, if this is a restarted run, from the restart database.
        Pointer<INSVCStaggeredHierarchyIntegrator> navier_stokes_integrator =
            new INSVCStaggeredConservativeHierarchyIntegrator(
                "INSVCStaggeredConservativeHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSVCStaggeredConservativeHierarchyIntegrator"));

        // Set up the advection diffusion hierarchy integrator
        Pointer<AdvDiffHierarchyIntegrator> adv_diff_integrator = new AdvDiffSemiImplicitHierarchyIntegrator(
            "AdvDiffSemiImplicitHierarchyIntegrator",
            app_initializer->getComponentDatabase("AdvDiffSemiImplicitHierarchyIntegrator"));

        navier_stokes_integrator->registerAdvDiffHierarchyIntegrator(adv_diff_integrator);

        Pointer<CartesianGridGeometry<NDIM> > grid_geometry = new CartesianGridGeometry<NDIM>(
            "CartesianGeometry", app_initializer->getComponentDatabase("CartesianGeometry"));
        Pointer<PatchHierarchy<NDIM> > patch_hierarchy = new PatchHierarchy<NDIM>("PatchHierarchy", grid_geometry);

        Pointer<StandardTagAndInitialize<NDIM> > error_detector =
            new StandardTagAndInitialize<NDIM>("StandardTagAndInitialize",
                                               navier_stokes_integrator,
                                               app_initializer->getComponentDatabase("StandardTagAndInitialize"));
        Pointer<BergerRigoutsos<NDIM> > box_generator = new BergerRigoutsos<NDIM>();
        Pointer<LoadBalancer<NDIM> > load_balancer =
            new LoadBalancer<NDIM>("LoadBalancer", app_initializer->getComponentDatabase("LoadBalancer"));
        Pointer<GriddingAlgorithm<NDIM> > gridding_algorithm =
            new GriddingAlgorithm<NDIM>("GriddingAlgorithm",
                                        app_initializer->getComponentDatabase("GriddingAlgorithm"),
                                        error_detector,
                                        box_generator,
                                        load_balancer);

        // Create level sets for solid interfaces.
        const string& ls_foilA = "level_set_foilA";
        Pointer<CellVariable<NDIM, double> > phi_var_foilA = new CellVariable<NDIM, double>(ls_foilA);
        const string& ls_foilB = "level_set_foilB";
        Pointer<CellVariable<NDIM, double> > phi_var_foilB = new CellVariable<NDIM, double>(ls_foilB);

        // Register the level sets with advection diffusion integrator.
        adv_diff_integrator->registerTransportedQuantity(phi_var_foilA);
        adv_diff_integrator->setDiffusionCoefficient(phi_var_foilA, 0.0);
        adv_diff_integrator->setAdvectionVelocity(phi_var_foilA,
                                                  navier_stokes_integrator->getAdvectionVelocityVariable());

        adv_diff_integrator->registerTransportedQuantity(phi_var_foilB);
        adv_diff_integrator->setDiffusionCoefficient(phi_var_foilB, 0.0);
        adv_diff_integrator->setAdvectionVelocity(phi_var_foilB,
                                                  navier_stokes_integrator->getAdvectionVelocityVariable());

        // Solid level set initial condition
        Pointer<CartGridFunction> phi_foilA_init = new LevelSetInitialCondition("phi_foilA_init", foilA);
        Pointer<CartGridFunction> phi_foilB_init = new LevelSetInitialCondition("phi_foilB_init", foilB);
        adv_diff_integrator->setInitialConditions(phi_var_foilA, phi_foilA_init);
        adv_diff_integrator->setInitialConditions(phi_var_foilB, phi_foilB_init);

        // Solid level set resetting consition
        SolidLevelSetResetter foilA_level_set_resetter(adv_diff_integrator, phi_var_foilA, /*bp_rbd*/ nullptr, &foilA);
        SolidLevelSetResetter foilB_level_set_resetter(adv_diff_integrator, phi_var_foilB, /*bp_rbd*/ nullptr, &foilB);
        adv_diff_integrator->registerIntegrateHierarchyCallback(&reset_solid_level_set_callback_fcn,
                                                                static_cast<void*>(&foilA_level_set_resetter));
        adv_diff_integrator->registerIntegrateHierarchyCallback(&reset_solid_level_set_callback_fcn,
                                                                static_cast<void*>(&foilB_level_set_resetter));

        // Fluid density and viscosity.
        Pointer<CellVariable<NDIM, double> > mu_var = new CellVariable<NDIM, double>("mu");
        Pointer<hier::Variable<NDIM> > rho_var;
        rho_var = new SideVariable<NDIM, double>("rho");
        navier_stokes_integrator->registerMassDensityVariable(rho_var);
        navier_stokes_integrator->registerViscosityVariable(mu_var);

        // Array for input into callback function
        const double rho_fluid = input_db->getDouble("RHO_F");
        SetFluidSolidDensity* ptr_setFluidSolidDensity = new SetFluidSolidDensity("SetFluidSolidDensity", rho_fluid);
        navier_stokes_integrator->registerResetFluidDensityFcn(&callSetFluidSolidDensityCallbackFunction,
                                                               static_cast<void*>(ptr_setFluidSolidDensity));

        const double mu_fluid = input_db->getDouble("MU_F");
        SetFluidSolidViscosity* ptr_setFluidSolidViscosity =
            new SetFluidSolidViscosity("SetFluidSolidViscosity", mu_fluid);
        navier_stokes_integrator->registerResetFluidViscosityFcn(&callSetFluidSolidViscosityCallbackFunction,
                                                                 static_cast<void*>(ptr_setFluidSolidViscosity));

        // Register callback function for tagging refined cells for level set data
        const double tag_value = input_db->getDouble("LS_TAG_VALUE");
        const double tag_thresh = input_db->getDouble("LS_TAG_ABS_THRESH");
        TagLSRefinementCells ls_foilA_tagger(adv_diff_integrator, phi_var_foilA, tag_value, tag_thresh);
        TagLSRefinementCells ls_foilB_tagger(adv_diff_integrator, phi_var_foilB, tag_value, tag_thresh);
        navier_stokes_integrator->registerApplyGradientDetectorCallback(&callTagSolidLSRefinementCellsCallbackFunction,
                                                                        static_cast<void*>(&ls_foilA_tagger));
        navier_stokes_integrator->registerApplyGradientDetectorCallback(&callTagSolidLSRefinementCellsCallbackFunction,
                                                                        static_cast<void*>(&ls_foilB_tagger));

        // Create Eulerian initial condition specification objects.
        if (input_db->keyExists("VelocityInitialConditions"))
        {
            Pointer<CartGridFunction> u_init = new muParserCartGridFunction(
                "u_init", app_initializer->getComponentDatabase("VelocityInitialConditions"), grid_geometry);
            navier_stokes_integrator->registerVelocityInitialConditions(u_init);
        }

        if (input_db->keyExists("PressureInitialConditions"))
        {
            Pointer<CartGridFunction> p_init = new muParserCartGridFunction(
                "p_init", app_initializer->getComponentDatabase("PressureInitialConditions"), grid_geometry);
            navier_stokes_integrator->registerPressureInitialConditions(p_init);
        }

        // Create Eulerian boundary condition specification objects (when necessary).
        const IntVector<NDIM>& periodic_shift = grid_geometry->getPeriodicShift();
        vector<RobinBcCoefStrategy<NDIM>*> u_bc_coefs(NDIM);
        if (periodic_shift.min() > 0)
        {
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                u_bc_coefs[d] = NULL;
            }
        }
        else
        {
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                ostringstream bc_coefs_name_stream;
                bc_coefs_name_stream << "u_bc_coefs_" << d;
                const string bc_coefs_name = bc_coefs_name_stream.str();

                ostringstream bc_coefs_db_name_stream;
                bc_coefs_db_name_stream << "VelocityBcCoefs_" << d;
                const string bc_coefs_db_name = bc_coefs_db_name_stream.str();

                u_bc_coefs[d] = new muParserRobinBcCoefs(
                    bc_coefs_name, app_initializer->getComponentDatabase(bc_coefs_db_name), grid_geometry);
            }
            navier_stokes_integrator->registerPhysicalBoundaryConditions(u_bc_coefs);
        }

        RobinBcCoefStrategy<NDIM>* rho_bc_coef = NULL;
        if (!(periodic_shift.min() > 0) && input_db->keyExists("DensityBcCoefs"))
        {
            rho_bc_coef = new muParserRobinBcCoefs(
                "rho_bc_coef", app_initializer->getComponentDatabase("DensityBcCoefs"), grid_geometry);
            navier_stokes_integrator->registerMassDensityBoundaryConditions(rho_bc_coef);
        }

        RobinBcCoefStrategy<NDIM>* mu_bc_coef = NULL;
        if (!(periodic_shift.min() > 0) && input_db->keyExists("ViscosityBcCoefs"))
        {
            mu_bc_coef = new muParserRobinBcCoefs(
                "mu_bc_coef", app_initializer->getComponentDatabase("ViscosityBcCoefs"), grid_geometry);
            navier_stokes_integrator->registerViscosityBoundaryConditions(mu_bc_coef);
        }

        RobinBcCoefStrategy<NDIM>* phi_bc_coef = NULL;
        if (!(periodic_shift.min() > 0) && input_db->keyExists("PhiBcCoefs"))
        {
            phi_bc_coef = new muParserRobinBcCoefs(
                "phi_bc_coef", app_initializer->getComponentDatabase("PhiBcCoefs"), grid_geometry);
        }
        adv_diff_integrator->setPhysicalBcCoef(phi_var_foilA, phi_bc_coef);

        // Configure the Brinkman penalization object to do the rigid body dynamics.
        Pointer<BrinkmanPenalizationRigidBodyDynamics> bp_rbd_A =
            new BrinkmanPenalizationRigidBodyDynamics("Airfoil_A",
                                                      phi_var_foilA,
                                                      adv_diff_integrator,
                                                      navier_stokes_integrator,
                                                      app_initializer->getComponentDatabase("BrinkmanPenalization"),
                                                      /*register_for_restart*/ true);

        Pointer<BrinkmanPenalizationRigidBodyDynamics> bp_rbd_B =
            new BrinkmanPenalizationRigidBodyDynamics("Airfoil_B",
                                                      phi_var_foilB,
                                                      adv_diff_integrator,
                                                      navier_stokes_integrator,
                                                      app_initializer->getComponentDatabase("BrinkmanPenalization"),
                                                      /*register_for_restart*/ true);

        {
            FreeRigidDOFVector free_dofs;
            free_dofs << 1, 1, 0;
            Eigen::Vector3d U_i = Eigen::Vector3d::Zero();
            Eigen::Vector3d W_i(0.0, 0.0, 2 * M_PI * foilA.freq * foilA.theta_0);
            bp_rbd_A->setSolveRigidBodyVelocity(free_dofs);
            bp_rbd_A->registerKinematicsFunction(&imposed_kinematics, &foilA);
            bp_rbd_A->registerExternalForceTorqueFunction(&external_force_torque);
            bp_rbd_A->setInitialConditions(foilA.X0, U_i, W_i, foilA.mass);
            navier_stokes_integrator->registerBrinkmanPenalizationStrategy(bp_rbd_A);
            foilA_level_set_resetter.bp_rbd = bp_rbd_A;
        }

        {
            FreeRigidDOFVector free_dofs;
            free_dofs << 1, 1, 0;
            Eigen::Vector3d U_i = Eigen::Vector3d::Zero();
            Eigen::Vector3d W_i(0.0, 0.0, 2 * M_PI * foilB.freq * foilB.theta_0);
            bp_rbd_B->setSolveRigidBodyVelocity(free_dofs);
            bp_rbd_B->registerKinematicsFunction(&imposed_kinematics, &foilB);
            bp_rbd_B->registerExternalForceTorqueFunction(&external_force_torque);
            bp_rbd_B->setInitialConditions(foilB.X0, U_i, W_i, foilB.mass);
            navier_stokes_integrator->registerBrinkmanPenalizationStrategy(bp_rbd_B);
            foilB_level_set_resetter.bp_rbd = bp_rbd_B;
        }

        // Set up visualization plot file writers.
        Pointer<VisItDataWriter<NDIM> > visit_data_writer = app_initializer->getVisItDataWriter();
        if (uses_visit)
        {
            navier_stokes_integrator->registerVisItDataWriter(visit_data_writer);
        }

        // Initialize hierarchy configuration and data on all patches.
        navier_stokes_integrator->initializePatchHierarchy(patch_hierarchy, gridding_algorithm);

        // Deallocate initialization objects.
        app_initializer.setNull();

        // Print the input database contents to the log file.
        plog << "Input database:\n";
        input_db->printClassData(plog);

        // Write out initial visualization data.
        int iteration_num = navier_stokes_integrator->getIntegratorStep();
        double loop_time = navier_stokes_integrator->getIntegratorTime();

        if (dump_viz_data)
        {
            pout << "\n\nWriting visualization files...\n\n";
            if (uses_visit)
            {
                navier_stokes_integrator->setupPlotData();
                visit_data_writer->writePlotData(patch_hierarchy, iteration_num, loop_time);
            }
        }

        // Open streams to save position and velocity of the structure.
        ofstream rbd_foilA_stream, ft_foilA_stream, rbd_foilB_stream, ft_foilB_stream;
        if (SAMRAI_MPI::getRank() == 0)
        {
            rbd_foilA_stream.open("rbd.foilA", ios_base::out | ios_base::app);
            ft_foilA_stream.open("hydro_force_torque.foilA", ios_base::out | std::ios_base::app);

            rbd_foilB_stream.open("rbd.foilB", ios_base::out | ios_base::app);
            ft_foilB_stream.open("hydro_force_torque.foilB", ios_base::out | std::ios_base::app);
        }

        // Main time step loop.
        double loop_time_end = navier_stokes_integrator->getEndTime();
        double dt = 0.0;
        while (!MathUtilities<double>::equalEps(loop_time, loop_time_end) && navier_stokes_integrator->stepsRemaining())
        {
            iteration_num = navier_stokes_integrator->getIntegratorStep();
            loop_time = navier_stokes_integrator->getIntegratorTime();

            pout << "\n";
            pout << "+++++++++++++++++++++++++++++++++++++++++++++++++++\n";
            pout << "At beginning of timestep # " << iteration_num << "\n";
            pout << "Simulation time is " << loop_time << "\n";

            dt = navier_stokes_integrator->getMaximumTimeStepSize();
            pout << "Advancing hierarchy with timestep size dt = " << dt << "\n";
            navier_stokes_integrator->advanceHierarchy(dt);
            loop_time += dt;

            pout << "\n";
            pout << "At end       of timestep # " << iteration_num << "\n";
            pout << "Simulation time is " << loop_time << "\n";
            pout << "+++++++++++++++++++++++++++++++++++++++++++++++++++\n";
            pout << "\n";

            // At specified intervals, write visualization and restart files,
            // and print out timer data.
            iteration_num += 1;
            const bool last_step = !navier_stokes_integrator->stepsRemaining();
            if (dump_viz_data && uses_visit && (iteration_num % viz_dump_interval == 0 || last_step))
            {
                pout << "Writing visualization files...\n\n";
                navier_stokes_integrator->setupPlotData();
                visit_data_writer->writePlotData(patch_hierarchy, iteration_num, loop_time);
            }
            if (dump_restart_data && (iteration_num % restart_dump_interval == 0 || last_step))
            {
                pout << "Writing restart files...\n\nn";
                RestartManager::getManager()->writeRestartFile(restart_dump_dirname, iteration_num);
            }
            if (dump_timer_data && (iteration_num % timer_dump_interval == 0 || last_step))
            {
                pout << "Writing timer data...\n\n";
                TimerManager::getManager()->print(plog);
            }

            if (SAMRAI_MPI::getRank() == 0)
            {
                {
                    const Eigen::Vector3d& rbd_posn = bp_rbd_A->getCurrentCOMPosn();
                    const Eigen::Vector3d& rbd_trans_vel = bp_rbd_A->getCurrentCOMTransVelocity();

                    rbd_foilA_stream.precision(12);
                    rbd_foilA_stream.setf(ios::fixed, ios::floatfield);
                    rbd_foilA_stream << loop_time << "\t" << rbd_posn[0] << "\t" << rbd_posn[1] << "\t"
                                     << rbd_trans_vel[0] << "\t" << rbd_trans_vel[1] << std::endl;

                    Eigen::Vector3d hydro_force_pressure, hydro_force_viscous, hydro_torque_pressure,
                        hydro_torque_viscous;
                    bp_rbd_A->getHydrodynamicForceTorque(
                        hydro_force_pressure, hydro_force_viscous, hydro_torque_pressure, hydro_torque_viscous);
                    ft_foilA_stream.precision(12);
                    ft_foilA_stream.setf(ios::fixed, ios::floatfield);
                    ft_foilA_stream << loop_time << "\t" << hydro_force_viscous[0] << "\t" << hydro_force_viscous[1]
                                    << "\t" << hydro_force_pressure[0] << "\t" << hydro_force_pressure[1] << std::endl;
                }

                {
                    const Eigen::Vector3d& rbd_posn = bp_rbd_B->getCurrentCOMPosn();
                    const Eigen::Vector3d& rbd_trans_vel = bp_rbd_B->getCurrentCOMTransVelocity();

                    rbd_foilB_stream.precision(12);
                    rbd_foilB_stream.setf(ios::fixed, ios::floatfield);
                    rbd_foilB_stream << loop_time << "\t" << rbd_posn[0] << "\t" << rbd_posn[1] << "\t"
                                     << rbd_trans_vel[0] << "\t" << rbd_trans_vel[1] << std::endl;

                    Eigen::Vector3d hydro_force_pressure, hydro_force_viscous, hydro_torque_pressure,
                        hydro_torque_viscous;
                    bp_rbd_B->getHydrodynamicForceTorque(
                        hydro_force_pressure, hydro_force_viscous, hydro_torque_pressure, hydro_torque_viscous);
                    ft_foilB_stream.precision(12);
                    ft_foilB_stream.setf(ios::fixed, ios::floatfield);
                    ft_foilB_stream << loop_time << "\t" << hydro_force_viscous[0] << "\t" << hydro_force_viscous[1]
                                    << "\t" << hydro_force_pressure[0] << "\t" << hydro_force_pressure[1] << std::endl;
                }
            }
        }

        // Close the logging streams.
        if (SAMRAI_MPI::getRank() == 0)
        {
            rbd_foilA_stream.close();
            ft_foilA_stream.close();
            rbd_foilB_stream.close();
            ft_foilB_stream.close();
        }

        // Delete dumb pointers.
        for (unsigned int d = 0; d < NDIM; ++d) delete u_bc_coefs[d];
        delete ptr_setFluidSolidDensity;
        delete ptr_setFluidSolidViscosity;
        delete rho_bc_coef;
        delete mu_bc_coef;
        delete phi_bc_coef;

    } // cleanup dynamically allocated objects prior to shutdown

    SAMRAIManager::shutdown();
} // main


//  Copyright (c) 2006 Xiaogang Zhang
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_BESSEL_JN_HPP
#define BOOST_MATH_BESSEL_JN_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/special_functions/detail/bessel_j0.hpp>
#include <boost/math/special_functions/detail/bessel_j1.hpp>
#include <boost/math/special_functions/detail/bessel_jy.hpp>
#include <boost/math/special_functions/detail/bessel_jy_asym.hpp>
#include <boost/math/special_functions/detail/bessel_jy_series.hpp>

// Bessel function of the first kind of integer order
// J_n(z) is the minimal solution
// n < abs(z), forward recurrence stable and usable
// n >= abs(z), forward recurrence unstable, use Miller's algorithm

namespace boost { namespace math { namespace detail{

template <typename T, typename Policy>
T bessel_jn(int n, T x, const Policy& pol)
{
    T value(0), factor, current, prev, next;

    BOOST_MATH_STD_USING

    //
    // Reflection has to come first:
    //
    if (n < 0)
    {
        factor = static_cast<T>((n & 0x1) ? -1 : 1);  // J_{-n}(z) = (-1)^n J_n(z)
        n = -n;
    }
    else
    {
        factor = 1;
    }
    if(x < 0)
    {
        factor *= (n & 0x1) ? -1 : 1;  // J_{n}(-z) = (-1)^n J_n(z)
        x = -x;
    }
    //
    // Special cases:
    //
    if(asymptotic_bessel_large_x_limit(T(n), x))
       return factor * asymptotic_bessel_j_large_x_2<T>(T(n), x, pol);
    if (n == 0)
    {
        return factor * bessel_j0(x);
    }
    if (n == 1)
    {
        return factor * bessel_j1(x);
    }

    if (x == 0)                             // n >= 2
    {
        return static_cast<T>(0);
    }

    BOOST_ASSERT(n > 1);
    T scale = 1;
    if (n < abs(x))                         // forward recurrence
    {
        prev = bessel_j0(x);
        current = bessel_j1(x);
        policies::check_series_iterations<T>("boost::math::bessel_j_n<%1%>(%1%,%1%)", n, pol);
        for (int k = 1; k < n; k++)
        {
            T fact = 2 * k / x;
            //
            // rescale if we would overflow or underflow:
            //
            if((fabs(fact) > 1) && ((tools::max_value<T>() - fabs(prev)) / fabs(fact) < fabs(current)))
            {
               scale /= current;
               prev /= current;
               current = 1;
            }
            value = fact * current - prev;
            prev = current;
            current = value;
        }
    }
    else if((x < 1) || (n > x * x / 4) || (x < 5))
    {
       return factor * bessel_j_small_z_series(T(n), x, pol);
    }
    else                                    // backward recurrence
    {
        T fn; int s;                        // fn = J_(n+1) / J_n
        // |x| <= n, fast convergence for continued fraction CF1
        boost::math::detail::CF1_jy(static_cast<T>(n), x, &fn, &s, pol);
        prev = fn;
        current = 1;
        // Check recursion won't go on too far:
        policies::check_series_iterations<T>("boost::math::bessel_j_n<%1%>(%1%,%1%)", n, pol);
        for (int k = n; k > 0; k--)
        {
            T fact = 2 * k / x;
            if((fabs(fact) > 1) && ((tools::max_value<T>() - fabs(prev)) / fabs(fact) < fabs(current)))
            {
               prev /= current;
               scale /= current;
               current = 1;
            }
            next = fact * current - prev;
            prev = current;
            current = next;
        }
        value = bessel_j0(x) / current;       // normalization
        scale = 1 / scale;
    }
    value *= factor;

    if(tools::max_value<T>() * scale < fabs(value))
       return policies::raise_overflow_error<T>("boost::math::bessel_jn<%1%>(%1%,%1%)", 0, pol);

    return value / scale;
}

}}} // namespaces

#endif // BOOST_MATH_BESSEL_JN_HPP


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2014 Olga Diamanti <olga.diam@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.

#include "polyroots.h"
#include <Eigen/Eigenvalues>

template <typename S, typename T>
IGL_INLINE void igl::polyRoots(Eigen::Matrix<S, Eigen::Dynamic,1> &polyCoeff, //real or comples coefficients
                          Eigen::Matrix<std::complex<T>, Eigen::Dynamic,1> &roots // complex roots (double or float)
)
{
  //  degree
  int n = polyCoeff.rows() - 1;

  Eigen::Matrix<S, Eigen::Dynamic, 1> d (n,1);
  d = polyCoeff.tail(n)/polyCoeff(0);

  Eigen::Matrix<S, Eigen::Dynamic, Eigen::Dynamic> I; I.setIdentity(n-1,n-1);
  Eigen::Matrix<S, Eigen::Dynamic, 1> z; z.setZero(n-1,1);

  Eigen::Matrix<S, Eigen::Dynamic, Eigen::Dynamic> a(n,n);
  a<<-d.transpose(),I,z;
  roots = a.eigenvalues();

}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::polyRoots<std::complex<double>, double>(Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1>&, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1>&);
template void igl::polyRoots<double, double>(Eigen::Matrix<double, -1, 1, 0, -1, 1>&, Eigen::Matrix<std::complex<double>, -1, 1, 0, -1, 1>&);
#endif


#include <replay/planar_direction.hpp>
#include <boost/math/constants/constants.hpp>
#include <replay/matrix2.hpp>
#include <replay/vector_math.hpp>

using namespace replay;

planar_direction planar_direction::move(planar_direction from, planar_direction to, float max_angle_delta)
{
    auto const relative_move = (to - from).normalized();

    if (std::abs(relative_move.angle()) < max_angle_delta)
        return to;

    return planar_direction{ from.angle() + math::clamp_absolute(relative_move.angle(), max_angle_delta) };
}

planar_direction planar_direction::normalized() const
{
    auto angle = m_angle;
    constexpr auto pi = boost::math::constants::pi<float>();
    constexpr auto two_pi = boost::math::constants::two_pi<float>();

    while (angle > pi)
        angle -= two_pi;
    while (angle <= -pi)
        angle += two_pi;
    return planar_direction(angle);
}

planar_direction planar_direction::average(planar_direction from, planar_direction to)
{
    return planar_direction::from_vector(from.as_vector() + to.as_vector());
}