// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2020
// Distributed under the Boost Software License, Version 1.0.
// https://www.boost.org/LICENSE_1_0.txt
// https://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// Version: 4.0.2019.08.13

#pragma once

#include <Mathematics/Logger.h>
#include <Mathematics/Math.h>
#include <Mathematics/Array2.h>
#include <algorithm>
#include <array>
#include <cstring>

// The interpolator is for uniformly spaced (x,y)-values.  The input samples
// F must be stored in row-major order to represent f(x,y); that is,
// F[c + xBound*r] corresponds to f(x,y), where c is the index corresponding
// to x and r is the index corresponding to y.

namespace WwiseGTE
{
    template <typename Real>
    class IntpAkimaUniform2
    {
    public:
        // Construction and destruction.
        IntpAkimaUniform2(int xBound, int yBound, Real xMin, Real xSpacing,
            Real yMin, Real ySpacing, Real const* F)
            :
            mXBound(xBound),
            mYBound(yBound),
            mQuantity(xBound* yBound),
            mXMin(xMin),
            mXSpacing(xSpacing),
            mYMin(yMin),
            mYSpacing(ySpacing),
            mF(F),
            mPoly(xBound - 1, mYBound - 1)
        {
            // At least a 3x3 block of data points is needed to construct the
            // estimates of the boundary derivatives.
            LogAssert(mXBound >= 3 && mYBound >= 3 && mF != nullptr, "Invalid input.");
            LogAssert(mXSpacing > (Real)0 && mYSpacing > (Real)0, "Invalid input.");

            mXMax = mXMin + mXSpacing * static_cast<Real>(mXBound - 1);
            mYMax = mYMin + mYSpacing * static_cast<Real>(mYBound - 1);

            // Create a 2D wrapper for the 1D samples.
            Array2<Real> Fmap(mXBound, mYBound, const_cast<Real*>(mF));

            // Construct first-order derivatives.
            Array2<Real> FX(mXBound, mYBound), FY(mXBound, mYBound);
            GetFX(Fmap, FX);
            GetFY(Fmap, FY);

            // Construct second-order derivatives.
            Array2<Real> FXY(mXBound, mYBound);
            GetFXY(Fmap, FXY);

            // Construct polynomials.
            GetPolynomials(Fmap, FX, FY, FXY);
        }

        ~IntpAkimaUniform2() = default;

        // Member access.
        inline int GetXBound() const
        {
            return mXBound;
        }

        inline int GetYBound() const
        {
            return mYBound;
        }

        inline int GetQuantity() const
        {
            return mQuantity;
        }

        inline Real const* GetF() const
        {
            return mF;
        }

        inline Real GetXMin() const
        {
            return mXMin;
        }

        inline Real GetXMax() const
        {
            return mXMax;
        }

        inline Real GetXSpacing() const
        {
            return mXSpacing;
        }

        inline Real GetYMin() const
        {
            return mYMin;
        }

        inline Real GetYMax() const
        {
            return mYMax;
        }

        inline Real GetYSpacing() const
        {
            return mYSpacing;
        }

        // Evaluate the function and its derivatives.  The functions clamp the
        // inputs to xmin <= x <= xmax and ymin <= y <= ymax.  The first
        // operator is for function evaluation.  The second operator is for
        // function or derivative evaluations.  The xOrder argument is the
        // order of the x-derivative and the yOrder argument is the order of
        // the y-derivative.  Both orders are zero to get the function value
        // itself.
        Real operator()(Real x, Real y) const
        {
            x = std::min(std::max(x, mXMin), mXMax);
            y = std::min(std::max(y, mYMin), mYMax);
            int ix, iy;
            Real dx, dy;
            XLookup(x, ix, dx);
            YLookup(y, iy, dy);
            return mPoly[iy][ix](dx, dy);
        }

        Real operator()(int xOrder, int yOrder, Real x, Real y) const
        {
            x = std::min(std::max(x, mXMin), mXMax);
            y = std::min(std::max(y, mYMin), mYMax);
            int ix, iy;
            Real dx, dy;
            XLookup(x, ix, dx);
            YLookup(y, iy, dy);
            return mPoly[iy][ix](xOrder, yOrder, dx, dy);
        }

    private:
        class Polynomial
        {
        public:
            Polynomial()
            {
                for (size_t i = 0; i < 4; ++i)
                {
                    mCoeff[i].fill((Real)0);
                }
            }

            // P(x,y) = (1,x,x^2,x^3)*A*(1,y,y^2,y^3).  The matrix term A[ix][iy]
            // corresponds to the polynomial term x^{ix} y^{iy}.
            Real& A(int ix, int iy)
            {
                return mCoeff[ix][iy];
            }

            Real operator()(Real x, Real y) const
            {
                std::array<Real, 4> B;
                for (int i = 0; i <= 3; ++i)
                {
                    B[i] = mCoeff[i][0] + y * (mCoeff[i][1] + y * (mCoeff[i][2] + y * mCoeff[i][3]));
                }

                return B[0] + x * (B[1] + x * (B[2] + x * B[3]));
            }

            Real operator()(int xOrder, int yOrder, Real x, Real y) const
            {
                std::array<Real, 4> xPow;
                switch (xOrder)
                {
                case 0:
                    xPow[0] = (Real)1;
                    xPow[1] = x;
                    xPow[2] = x * x;
                    xPow[3] = x * x * x;
                    break;
                case 1:
                    xPow[0] = (Real)0;
                    xPow[1] = (Real)1;
                    xPow[2] = (Real)2 * x;
                    xPow[3] = (Real)3 * x * x;
                    break;
                case 2:
                    xPow[0] = (Real)0;
                    xPow[1] = (Real)0;
                    xPow[2] = (Real)2;
                    xPow[3] = (Real)6 * x;
                    break;
                case 3:
                    xPow[0] = (Real)0;
                    xPow[1] = (Real)0;
                    xPow[2] = (Real)0;
                    xPow[3] = (Real)6;
                    break;
                default:
                    return (Real)0;
                }

                std::array<Real, 4> yPow;
                switch (yOrder)
                {
                case 0:
                    yPow[0] = (Real)1;
                    yPow[1] = y;
                    yPow[2] = y * y;
                    yPow[3] = y * y * y;
                    break;
                case 1:
                    yPow[0] = (Real)0;
                    yPow[1] = (Real)1;
                    yPow[2] = (Real)2 * y;
                    yPow[3] = (Real)3 * y * y;
                    break;
                case 2:
                    yPow[0] = (Real)0;
                    yPow[1] = (Real)0;
                    yPow[2] = (Real)2;
                    yPow[3] = (Real)6 * y;
                    break;
                case 3:
                    yPow[0] = (Real)0;
                    yPow[1] = (Real)0;
                    yPow[2] = (Real)0;
                    yPow[3] = (Real)6;
                    break;
                default:
                    return (Real)0;
                }

                Real p = (Real)0;
                for (size_t iy = 0; iy <= 3; ++iy)
                {
                    for (size_t ix = 0; ix <= 3; ++ix)
                    {
                        p += mCoeff[ix][iy] * xPow[ix] * yPow[iy];
                    }
                }

                return p;
            }

        private:
            std::array<std::array<Real, 4>, 4> mCoeff;
        };

        // Support for construction.
        void GetFX(Array2<Real> const& F, Array2<Real>& FX)
        {
            Array2<Real> slope(mXBound + 3, mYBound);
            Real invDX = (Real)1 / mXSpacing;
            int ix, iy;
            for (iy = 0; iy < mYBound; ++iy)
            {
                for (ix = 0; ix < mXBound - 1; ++ix)
                {
                    slope[iy][ix + 2] = (F[iy][ix + 1] - F[iy][ix]) * invDX;
                }

                slope[iy][1] = (Real)2 * slope[iy][2] - slope[iy][3];
                slope[iy][0] = (Real)2 * slope[iy][1] - slope[iy][2];
                slope[iy][mXBound + 1] = (Real)2 * slope[iy][mXBound] - slope[iy][mXBound - 1];
                slope[iy][mXBound + 2] = (Real)2 * slope[iy][mXBound + 1] - slope[iy][mXBound];
            }

            for (iy = 0; iy < mYBound; ++iy)
            {
                for (ix = 0; ix < mXBound; ++ix)
                {
                    FX[iy][ix] = ComputeDerivative(slope[iy] + ix);
                }
            }
        }

        void GetFY(Array2<Real> const& F, Array2<Real>& FY)
        {
            Array2<Real> slope(mYBound + 3, mXBound);
            Real invDY = (Real)1 / mYSpacing;
            int ix, iy;
            for (ix = 0; ix < mXBound; ++ix)
            {
                for (iy = 0; iy < mYBound - 1; ++iy)
                {
                    slope[ix][iy + 2] = (F[iy + 1][ix] - F[iy][ix]) * invDY;
                }

                slope[ix][1] = (Real)2 * slope[ix][2] - slope[ix][3];
                slope[ix][0] = (Real)2 * slope[ix][1] - slope[ix][2];
                slope[ix][mYBound + 1] = (Real)2 * slope[ix][mYBound] - slope[ix][mYBound - 1];
                slope[ix][mYBound + 2] = (Real)2 * slope[ix][mYBound + 1] - slope[ix][mYBound];
            }

            for (ix = 0; ix < mXBound; ++ix)
            {
                for (iy = 0; iy < mYBound; ++iy)
                {
                    FY[iy][ix] = ComputeDerivative(slope[ix] + iy);
                }
            }
        }

        void GetFXY(Array2<Real> const& F, Array2<Real>& FXY)
        {
            int xBoundM1 = mXBound - 1;
            int yBoundM1 = mYBound - 1;
            int ix0 = xBoundM1, ix1 = ix0 - 1, ix2 = ix1 - 1;
            int iy0 = yBoundM1, iy1 = iy0 - 1, iy2 = iy1 - 1;
            int ix, iy;

            Real invDXDY = (Real)1 / (mXSpacing * mYSpacing);

            // corners
            FXY[0][0] = (Real)0.25 * invDXDY * (
                (Real)9 * F[0][0]
                - (Real)12 * F[0][1]
                + (Real)3 * F[0][2]
                - (Real)12 * F[1][0]
                + (Real)16 * F[1][1]
                - (Real)4 * F[1][2]
                + (Real)3 * F[2][0]
                - (Real)4 * F[2][1]
                + F[2][2]);

            FXY[0][xBoundM1] = (Real)0.25 * invDXDY * (
                (Real)9 * F[0][ix0]
                - (Real)12 * F[0][ix1]
                + (Real)3 * F[0][ix2]
                - (Real)12 * F[1][ix0]
                + (Real)16 * F[1][ix1]
                - (Real)4 * F[1][ix2]
                + (Real)3 * F[2][ix0]
                - (Real)4 * F[2][ix1]
                + F[2][ix2]);

            FXY[yBoundM1][0] = (Real)0.25 * invDXDY * (
                (Real)9 * F[iy0][0]
                - (Real)12 * F[iy0][1]
                + (Real)3 * F[iy0][2]
                - (Real)12 * F[iy1][0]
                + (Real)16 * F[iy1][1]
                - (Real)4 * F[iy1][2]
                + (Real)3 * F[iy2][0]
                - (Real)4 * F[iy2][1]
                + F[iy2][2]);

            FXY[yBoundM1][xBoundM1] = (Real)0.25 * invDXDY * (
                (Real)9 * F[iy0][ix0]
                - (Real)12 * F[iy0][ix1]
                + (Real)3 * F[iy0][ix2]
                - (Real)12 * F[iy1][ix0]
                + (Real)16 * F[iy1][ix1]
                - (Real)4 * F[iy1][ix2]
                + (Real)3 * F[iy2][ix0]
                - (Real)4 * F[iy2][ix1]
                + F[iy2][ix2]);

            // x-edges
            for (ix = 1; ix < xBoundM1; ++ix)
            {
                FXY[0][ix] = (Real)0.25 * invDXDY * (
                    (Real)3 * (F[0][ix - 1] - F[0][ix + 1])
                    - (Real)4 * (F[1][ix - 1] - F[1][ix + 1])
                    + (F[2][ix - 1] - F[2][ix + 1]));

                FXY[yBoundM1][ix] = (Real)0.25 * invDXDY * (
                    (Real)3 * (F[iy0][ix - 1] - F[iy0][ix + 1])
                    - (Real)4 * (F[iy1][ix - 1] - F[iy1][ix + 1])
                    + (F[iy2][ix - 1] - F[iy2][ix + 1]));
            }

            // y-edges
            for (iy = 1; iy < yBoundM1; ++iy)
            {
                FXY[iy][0] = (Real)0.25 * invDXDY * (
                    (Real)3 * (F[iy - 1][0] - F[iy + 1][0])
                    - (Real)4 * (F[iy - 1][1] - F[iy + 1][1])
                    + (F[iy - 1][2] - F[iy + 1][2]));

                FXY[iy][xBoundM1] = (Real)0.25 * invDXDY * (
                    (Real)3 * (F[iy - 1][ix0] - F[iy + 1][ix0])
                    - (Real)4 * (F[iy - 1][ix1] - F[iy + 1][ix1])
                    + (F[iy - 1][ix2] - F[iy + 1][ix2]));
            }

            // interior
            for (iy = 1; iy < yBoundM1; ++iy)
            {
                for (ix = 1; ix < xBoundM1; ++ix)
                {
                    FXY[iy][ix] = (Real)0.25 * invDXDY * (F[iy - 1][ix - 1] -
                        F[iy - 1][ix + 1] - F[iy + 1][ix - 1] + F[iy + 1][ix + 1]);
                }
            }
        }

        void GetPolynomials(Array2<Real> const& F, Array2<Real> const& FX,
            Array2<Real> const& FY, Array2<Real> const& FXY)
        {
            int xBoundM1 = mXBound - 1;
            int yBoundM1 = mYBound - 1;
            for (int iy = 0; iy < yBoundM1; ++iy)
            {
                for (int ix = 0; ix < xBoundM1; ++ix)
                {
                    // Note the 'transposing' of the 2x2 blocks (to match
                    // notation used in the polynomial definition).
                    Real G[2][2] =
                    {
                        { F[iy][ix], F[iy + 1][ix] },
                        { F[iy][ix + 1], F[iy + 1][ix + 1] }
                    };

                    Real GX[2][2] =
                    {
                        { FX[iy][ix], FX[iy + 1][ix] },
                        { FX[iy][ix + 1], FX[iy + 1][ix + 1] }
                    };

                    Real GY[2][2] =
                    {
                        { FY[iy][ix], FY[iy + 1][ix] },
                        { FY[iy][ix + 1], FY[iy + 1][ix + 1] }
                    };

                    Real GXY[2][2] =
                    {
                        { FXY[iy][ix], FXY[iy + 1][ix] },
                        { FXY[iy][ix + 1], FXY[iy + 1][ix + 1] }
                    };

                    Construct(mPoly[iy][ix], G, GX, GY, GXY);
                }
            }
        }

        Real ComputeDerivative(Real const* slope) const
        {
            if (slope[1] != slope[2])
            {
                if (slope[0] != slope[1])
                {
                    if (slope[2] != slope[3])
                    {
                        Real ad0 = std::fabs(slope[3] - slope[2]);
                        Real ad1 = std::fabs(slope[0] - slope[1]);
                        return (ad0 * slope[1] + ad1 * slope[2]) / (ad0 + ad1);
                    }
                    else
                    {
                        return slope[2];
                    }
                }
                else
                {
                    if (slope[2] != slope[3])
                    {
                        return slope[1];
                    }
                    else
                    {
                        return (Real)0.5 * (slope[1] + slope[2]);
                    }
                }
            }
            else
            {
                return slope[1];
            }
        }

        void Construct(Polynomial& poly, Real const F[2][2], Real const FX[2][2],
            Real const FY[2][2], Real const FXY[2][2])
        {
            Real dx = mXSpacing;
            Real dy = mYSpacing;
            Real invDX = (Real)1 / dx, invDX2 = invDX * invDX;
            Real invDY = (Real)1 / dy, invDY2 = invDY * invDY;
            Real b0, b1, b2, b3;

            poly.A(0, 0) = F[0][0];
            poly.A(1, 0) = FX[0][0];
            poly.A(0, 1) = FY[0][0];
            poly.A(1, 1) = FXY[0][0];

            b0 = (F[1][0] - poly(0, 0, dx, (Real)0)) * invDX2;
            b1 = (FX[1][0] - poly(1, 0, dx, (Real)0)) * invDX;
            poly.A(2, 0) = (Real)3 * b0 - b1;
            poly.A(3, 0) = ((Real)-2 * b0 + b1) * invDX;

            b0 = (F[0][1] - poly(0, 0, (Real)0, dy)) * invDY2;
            b1 = (FY[0][1] - poly(0, 1, (Real)0, dy)) * invDY;
            poly.A(0, 2) = (Real)3 * b0 - b1;
            poly.A(0, 3) = ((Real)-2 * b0 + b1) * invDY;

            b0 = (FY[1][0] - poly(0, 1, dx, (Real)0)) * invDX2;
            b1 = (FXY[1][0] - poly(1, 1, dx, (Real)0)) * invDX;
            poly.A(2, 1) = (Real)3 * b0 - b1;
            poly.A(3, 1) = ((Real)-2 * b0 + b1) * invDX;

            b0 = (FX[0][1] - poly(1, 0, (Real)0, dy)) * invDY2;
            b1 = (FXY[0][1] - poly(1, 1, (Real)0, dy)) * invDY;
            poly.A(1, 2) = (Real)3 * b0 - b1;
            poly.A(1, 3) = ((Real)-2 * b0 + b1) * invDY;

            b0 = (F[1][1] - poly(0, 0, dx, dy)) * invDX2 * invDY2;
            b1 = (FX[1][1] - poly(1, 0, dx, dy)) * invDX * invDY2;
            b2 = (FY[1][1] - poly(0, 1, dx, dy)) * invDX2 * invDY;
            b3 = (FXY[1][1] - poly(1, 1, dx, dy)) * invDX * invDY;
            poly.A(2, 2) = (Real)9 * b0 - (Real)3 * b1 - (Real)3 * b2 + b3;
            poly.A(3, 2) = ((Real)-6 * b0 + (Real)3 * b1 + (Real)2 * b2 - b3) * invDX;
            poly.A(2, 3) = ((Real)-6 * b0 + (Real)2 * b1 + (Real)3 * b2 - b3) * invDY;
            poly.A(3, 3) = ((Real)4 * b0 - (Real)2 * b1 - (Real)2 * b2 + b3) * invDX * invDY;
        }

        // Support for evaluation.
        void XLookup(Real x, int& xIndex, Real& dx) const
        {
            for (xIndex = 0; xIndex + 1 < mXBound; ++xIndex)
            {
                if (x < mXMin + mXSpacing * (xIndex + 1))
                {
                    dx = x - (mXMin + mXSpacing * xIndex);
                    return;
                }
            }

            --xIndex;
            dx = x - (mXMin + mXSpacing * xIndex);
        }

        void YLookup(Real y, int& yIndex, Real& dy) const
        {
            for (yIndex = 0; yIndex + 1 < mYBound; ++yIndex)
            {
                if (y < mYMin + mYSpacing * (yIndex + 1))
                {
                    dy = y - (mYMin + mYSpacing * yIndex);
                    return;
                }
            }

            yIndex--;
            dy = y - (mYMin + mYSpacing * yIndex);
        }

        int mXBound, mYBound, mQuantity;
        Real mXMin, mXMax, mXSpacing;
        Real mYMin, mYMax, mYSpacing;
        Real const* mF;
        Array2<Polynomial> mPoly;
    };
}