/******************************************************************************

 *

 * Project:  GDAL Core

 * Purpose:  Helper code to implement overview support in different drivers.

 * Author:   Frank Warmerdam, warmerdam@pobox.com

 *

 ******************************************************************************

 * Copyright (c) 2000, Frank Warmerdam

 * Copyright (c) 2007-2010, Even Rouault <even dot rouault at spatialys.com>

 *

 * SPDX-License-Identifier: MIT

 ****************************************************************************/

#include "cpl_port.h"

#include "gdal_priv.h"

#include <cmath>

#include <cstddef>

#include <cstdlib>

#include <algorithm>

#include <complex>

#include <condition_variable>

#include <limits>

#include <list>

#include <memory>

#include <mutex>

#include <vector>

#include "cpl_conv.h"

#include "cpl_error.h"

#include "cpl_float.h"

#include "cpl_progress.h"

#include "cpl_vsi.h"

#include "cpl_worker_thread_pool.h"

#include "gdal.h"

#include "gdal_thread_pool.h"

#include "gdalwarper.h"

#include "gdal_vrt.h"

#include "vrtdataset.h"

#ifdef USE_NEON_OPTIMIZATIONS

#include "include_sse2neon.h"

#if (!defined(__aarch64__) && !defined(_M_ARM64))

#define ARM_V7

#endif

#define USE_SSE2

#include "gdalsse_priv.h"

// Restrict to 64bit processors because they are guaranteed to have SSE2,

// or if __AVX2__ is defined.

#elif defined(__x86_64) || defined(_M_X64) || defined(__AVX2__)

#define USE_SSE2

#include "gdalsse_priv.h"

#ifdef __SSE3__

#include <pmmintrin.h>

#endif

#ifdef __SSSE3__

#include <tmmintrin.h>

#endif

#ifdef __SSE4_1__

#include <smmintrin.h>

#endif

#ifdef __AVX2__

#include <immintrin.h>

#endif

#endif

// To be included after above USE_SSE2 and include gdalsse_priv.h

// to avoid build issue on Windows x86

#include "gdal_priv_templates.hpp"

/************************************************************************/

/*                       GDALResampleChunk_Near()                       */

/************************************************************************/

template <class T>

static CPLErr GDALResampleChunk_NearT(const GDALOverviewResampleArgs &args,

                                      const T *pChunk, T **ppDstBuffer)

{

    const double dfXRatioDstToSrc = args.dfXRatioDstToSrc;

    const double dfYRatioDstToSrc = args.dfYRatioDstToSrc;

    const GDALDataType eWrkDataType = args.eWrkDataType;

    const int nChunkXOff = args.nChunkXOff;

    const int nChunkXSize = args.nChunkXSize;

    const int nChunkYOff = args.nChunkYOff;

    const int nDstXOff = args.nDstXOff;

    const int nDstXOff2 = args.nDstXOff2;

    const int nDstYOff = args.nDstYOff;

    const int nDstYOff2 = args.nDstYOff2;

    const int nDstXWidth = nDstXOff2 - nDstXOff;

    /* -------------------------------------------------------------------- */

    /*      Allocate buffers.                                               */

    /* -------------------------------------------------------------------- */

    *ppDstBuffer = static_cast<T *>(

        VSI_MALLOC3_VERBOSE(nDstXWidth, nDstYOff2 - nDstYOff,

                            GDALGetDataTypeSizeBytes(eWrkDataType)));

    if (*ppDstBuffer == nullptr)

    {

        return CE_Failure;

    }

    T *const pDstBuffer = *ppDstBuffer;

    int *panSrcXOff =

        static_cast<int *>(VSI_MALLOC2_VERBOSE(nDstXWidth, sizeof(int)));

    if (panSrcXOff == nullptr)

    {

        return CE_Failure;

    }

    /* ==================================================================== */

    /*      Precompute inner loop constants.                                */

    /* ==================================================================== */

    for (int iDstPixel = nDstXOff; iDstPixel < nDstXOff2; ++iDstPixel)

    {

        int nSrcXOff = static_cast<int>(0.5 + iDstPixel * dfXRatioDstToSrc);

        if (nSrcXOff < nChunkXOff)

            nSrcXOff = nChunkXOff;

        panSrcXOff[iDstPixel - nDstXOff] = nSrcXOff;

    }

    /* ==================================================================== */

    /*      Loop over destination scanlines.                                */

    /* ==================================================================== */

    for (int iDstLine = nDstYOff; iDstLine < nDstYOff2; ++iDstLine)

    {

        int nSrcYOff = static_cast<int>(0.5 + iDstLine * dfYRatioDstToSrc);

        if (nSrcYOff < nChunkYOff)

            nSrcYOff = nChunkYOff;

        const T *const pSrcScanline =

            pChunk +

            (static_cast<size_t>(nSrcYOff - nChunkYOff) * nChunkXSize) -

            nChunkXOff;

        /* --------------------------------------------------------------------

         */

        /*      Loop over destination pixels */

        /* --------------------------------------------------------------------

         */

        T *pDstScanline =

            pDstBuffer + static_cast<size_t>(iDstLine - nDstYOff) * nDstXWidth;

        for (int iDstPixel = 0; iDstPixel < nDstXWidth; ++iDstPixel)

        {

            pDstScanline[iDstPixel] = pSrcScanline[panSrcXOff[iDstPixel]];

        }

    }

    CPLFree(panSrcXOff);

    return CE_None;

}

Unexecuted instantiation: overview.cpp:CPLErr GDALResampleChunk_NearT<unsigned char>(GDALOverviewResampleArgs const&, unsigned char const*, unsigned char**)

Unexecuted instantiation: overview.cpp:CPLErr GDALResampleChunk_NearT<unsigned short>(GDALOverviewResampleArgs const&, unsigned short const*, unsigned short**)

Unexecuted instantiation: overview.cpp:CPLErr GDALResampleChunk_NearT<unsigned int>(GDALOverviewResampleArgs const&, unsigned int const*, unsigned int**)

Unexecuted instantiation: overview.cpp:CPLErr GDALResampleChunk_NearT<unsigned long>(GDALOverviewResampleArgs const&, unsigned long const*, unsigned long**)

Unexecuted instantiation: overview.cpp:CPLErr GDALResampleChunk_NearT<std::__1::complex<double> >(GDALOverviewResampleArgs const&, std::__1::complex<double> const*, std::__1::complex<double>**)

static CPLErr GDALResampleChunk_Near(const GDALOverviewResampleArgs &args,

                                     const void *pChunk, void **ppDstBuffer,

                                     GDALDataType *peDstBufferDataType)

{

    *peDstBufferDataType = args.eWrkDataType;

    switch (args.eWrkDataType)

    {

        // For nearest resampling, as no computation is done, only the

        // size of the data type matters.

        case GDT_UInt8:

        case GDT_Int8:

        {

            CPLAssert(GDALGetDataTypeSizeBytes(args.eWrkDataType) == 1);

            return GDALResampleChunk_NearT(

                args, static_cast<const uint8_t *>(pChunk),

                reinterpret_cast<uint8_t **>(ppDstBuffer));

        }

        case GDT_Int16:

        case GDT_UInt16:

        case GDT_Float16:

        {

            CPLAssert(GDALGetDataTypeSizeBytes(args.eWrkDataType) == 2);

            return GDALResampleChunk_NearT(

                args, static_cast<const uint16_t *>(pChunk),

                reinterpret_cast<uint16_t **>(ppDstBuffer));

        }

        case GDT_CInt16:

        case GDT_CFloat16:

        case GDT_Int32:

        case GDT_UInt32:

        case GDT_Float32:

        {

            CPLAssert(GDALGetDataTypeSizeBytes(args.eWrkDataType) == 4);

            return GDALResampleChunk_NearT(

                args, static_cast<const uint32_t *>(pChunk),

                reinterpret_cast<uint32_t **>(ppDstBuffer));

        }

        case GDT_CInt32:

        case GDT_CFloat32:

        case GDT_Int64:

        case GDT_UInt64:

        case GDT_Float64:

        {

            CPLAssert(GDALGetDataTypeSizeBytes(args.eWrkDataType) == 8);

            return GDALResampleChunk_NearT(

                args, static_cast<const uint64_t *>(pChunk),

                reinterpret_cast<uint64_t **>(ppDstBuffer));

        }

        case GDT_CFloat64:

        {

            return GDALResampleChunk_NearT(

                args, static_cast<const std::complex<double> *>(pChunk),

                reinterpret_cast<std::complex<double> **>(ppDstBuffer));

        }

        case GDT_Unknown:

        case GDT_TypeCount:

            break;

    }

    CPLAssert(false);

    return CE_Failure;

}

namespace

{

// Find in the color table the entry whose RGB value is the closest

// (using quadratic distance) to the test color, ignoring transparent entries.

int BestColorEntry(const std::vector<GDALColorEntry> &entries,

                   const GDALColorEntry &test)

{

    int nMinDist = std::numeric_limits<int>::max();

    size_t bestEntry = 0;

    for (size_t i = 0; i < entries.size(); ++i)

    {

        const GDALColorEntry &entry = entries[i];

        // Ignore transparent entries

        if (entry.c4 == 0)

            continue;

        int nDist = ((test.c1 - entry.c1) * (test.c1 - entry.c1)) +

                    ((test.c2 - entry.c2) * (test.c2 - entry.c2)) +

                    ((test.c3 - entry.c3) * (test.c3 - entry.c3));

        if (nDist < nMinDist)

        {

            nMinDist = nDist;

            bestEntry = i;

        }

    }

    return static_cast<int>(bestEntry);

}

std::vector<GDALColorEntry> ReadColorTable(const GDALColorTable &table,

                                           int &transparentIdx)

{

    std::vector<GDALColorEntry> entries(table.GetColorEntryCount());

    transparentIdx = -1;

    int i = 0;

    for (auto &entry : entries)

    {

        table.GetColorEntryAsRGB(i, &entry);

        if (transparentIdx < 0 && entry.c4 == 0)

            transparentIdx = i;

        ++i;

    }

    return entries;

}

}  // unnamed  namespace

/************************************************************************/

/*                               SQUARE()                               */

/************************************************************************/

template <class T, class Tsquare = T> inline Tsquare SQUARE(T val)

{

    return static_cast<Tsquare>(val) * val;

}

Unexecuted instantiation: int SQUARE<int, int>(int)

Unexecuted instantiation: double SQUARE<double, double>(double)

Unexecuted instantiation: float SQUARE<float, float>(float)

/************************************************************************/

/*                         ComputeIntegerRMS()                          */

/************************************************************************/

// Compute rms = sqrt(sumSquares / weight) in such a way that it is the

// integer that minimizes abs(rms**2 - sumSquares / weight)

template <class T, class Twork>

inline T ComputeIntegerRMS(double sumSquares, double weight)

{

    const double sumDivWeight = sumSquares / weight;

    T rms = static_cast<T>(sqrt(sumDivWeight));

    // Is rms**2 or (rms+1)**2 closest to sumSquares / weight ?

    // Naive version:

    // if( weight * (rms+1)**2 - sumSquares < sumSquares - weight * rms**2 )

    if (static_cast<double>(static_cast<Twork>(2) * rms * (rms + 1) + 1) <

        2 * sumDivWeight)

        rms += 1;

    return rms;

}

Unexecuted instantiation: unsigned char ComputeIntegerRMS<unsigned char, int>(double, double)

Unexecuted instantiation: unsigned short ComputeIntegerRMS<unsigned short, unsigned long>(double, double)

template <class T, class Tsum> inline T ComputeIntegerRMS_4values(Tsum)

{

    CPLAssert(false);

    return 0;

}

template <> inline GByte ComputeIntegerRMS_4values<GByte, int>(int sumSquares)

{

    // It has been verified that given the correction on rms below, using

    // sqrt((float)((sumSquares + 1)/ 4)) or sqrt((float)sumSquares * 0.25f)

    // is equivalent, so use the former as it is used twice.

    const int sumSquaresPlusOneDiv4 = (sumSquares + 1) / 4;

    const float sumDivWeight = static_cast<float>(sumSquaresPlusOneDiv4);

    GByte rms = static_cast<GByte>(std::sqrt(sumDivWeight));

    // Is rms**2 or (rms+1)**2 closest to sumSquares / weight ?

    // Naive version:

    // if( weight * (rms+1)**2 - sumSquares < sumSquares - weight * rms**2 )

    // Optimized version for integer case and weight == 4

    if (static_cast<int>(rms) * (rms + 1) < sumSquaresPlusOneDiv4)

        rms += 1;

    return rms;

}

template <>

inline GUInt16 ComputeIntegerRMS_4values<GUInt16, double>(double sumSquares)

{

    const double sumDivWeight = sumSquares * 0.25;

    GUInt16 rms = static_cast<GUInt16>(std::sqrt(sumDivWeight));

    // Is rms**2 or (rms+1)**2 closest to sumSquares / weight ?

    // Naive version:

    // if( weight * (rms+1)**2 - sumSquares < sumSquares - weight * rms**2 )

    // Optimized version for integer case and weight == 4

    if (static_cast<GUInt32>(rms) * (rms + 1) <

        static_cast<GUInt32>(sumDivWeight + 0.25))

        rms += 1;

    return rms;

}

#ifdef USE_SSE2

/************************************************************************/

/*                    QuadraticMeanByteSSE2OrAVX2()                     */

/************************************************************************/

#if defined(__SSSE3__) || defined(USE_NEON_OPTIMIZATIONS)

#define sse2_hadd_epi16 _mm_hadd_epi16

#else

inline __m128i sse2_hadd_epi16(__m128i a, __m128i b)

{

    // Horizontal addition of adjacent pairs

    const auto mask = _mm_set1_epi32(0xFFFF);

    const auto horizLo =

        _mm_add_epi32(_mm_and_si128(a, mask), _mm_srli_epi32(a, 16));

    const auto horizHi =

        _mm_add_epi32(_mm_and_si128(b, mask), _mm_srli_epi32(b, 16));

    // Recombine low and high parts

    return _mm_packs_epi32(horizLo, horizHi);

}

#endif

#ifdef __AVX2__

#define set1_epi16 _mm256_set1_epi16

#define set1_epi32 _mm256_set1_epi32

#define setzero _mm256_setzero_si256

#define set1_ps _mm256_set1_ps

#define loadu_int(x) _mm256_loadu_si256(reinterpret_cast<__m256i const *>(x))

#define unpacklo_epi8 _mm256_unpacklo_epi8

#define unpackhi_epi8 _mm256_unpackhi_epi8

#define madd_epi16 _mm256_madd_epi16

#define add_epi32 _mm256_add_epi32

#define mul_ps _mm256_mul_ps

#define cvtepi32_ps _mm256_cvtepi32_ps

#define sqrt_ps _mm256_sqrt_ps

#define cvttps_epi32 _mm256_cvttps_epi32

#define packs_epi32 _mm256_packs_epi32

#define packus_epi32 _mm256_packus_epi32

#define srli_epi32 _mm256_srli_epi32

#define mullo_epi16 _mm256_mullo_epi16

#define srli_epi16 _mm256_srli_epi16

#define cmpgt_epi16 _mm256_cmpgt_epi16

#define add_epi16 _mm256_add_epi16

#define sub_epi16 _mm256_sub_epi16

#define packus_epi16 _mm256_packus_epi16

/* AVX2 operates on 2 separate 128-bit lanes, so we have to do shuffling */

/* to get the lower 128-bit bits of what would be a true 256-bit vector register

 */

inline __m256i FIXUP_LANES(__m256i x)

{

    return _mm256_permute4x64_epi64(x, _MM_SHUFFLE(3, 1, 2, 0));

}

#define store_lo(x, y)                                                         \

    _mm_storeu_si128(reinterpret_cast<__m128i *>(x),                           \

                     _mm256_extracti128_si256(FIXUP_LANES(y), 0))

#define storeu_int(x, y)                                                       \

    _mm256_storeu_si256(reinterpret_cast<__m256i *>(x), FIXUP_LANES(y))

#define hadd_epi16 _mm256_hadd_epi16

#else

#define set1_epi16 _mm_set1_epi16

#define set1_epi32 _mm_set1_epi32

#define setzero _mm_setzero_si128

#define set1_ps _mm_set1_ps

#define loadu_int(x) _mm_loadu_si128(reinterpret_cast<__m128i const *>(x))

#define unpacklo_epi8 _mm_unpacklo_epi8

#define unpackhi_epi8 _mm_unpackhi_epi8

#define madd_epi16 _mm_madd_epi16

#define add_epi32 _mm_add_epi32

#define mul_ps _mm_mul_ps

#define cvtepi32_ps _mm_cvtepi32_ps

#define sqrt_ps _mm_sqrt_ps

#define cvttps_epi32 _mm_cvttps_epi32

#define packs_epi32 _mm_packs_epi32

#define packus_epi32 GDAL_mm_packus_epi32

#define srli_epi32 _mm_srli_epi32

#define mullo_epi16 _mm_mullo_epi16

#define srli_epi16 _mm_srli_epi16

#define cmpgt_epi16 _mm_cmpgt_epi16

#define add_epi16 _mm_add_epi16

#define sub_epi16 _mm_sub_epi16

#define packus_epi16 _mm_packus_epi16

#define store_lo(x, y) _mm_storel_epi64(reinterpret_cast<__m128i *>(x), (y))

#define storeu_int(x, y) _mm_storeu_si128(reinterpret_cast<__m128i *>(x), (y))

#define hadd_epi16 sse2_hadd_epi16

#endif

template <class T>

static int

#if defined(__GNUC__)

    __attribute__((noinline))

#endif

    QuadraticMeanByteSSE2OrAVX2(int nDstXWidth, int nChunkXSize,

                                const T *&CPL_RESTRICT pSrcScanlineShiftedInOut,

                                T *CPL_RESTRICT pDstScanline)

{

    // Optimized implementation for RMS on Byte by

    // processing by group of 8 output pixels, so as to use

    // a single _mm_sqrt_ps() call for 4 output pixels

    const T *CPL_RESTRICT pSrcScanlineShifted = pSrcScanlineShiftedInOut;

    int iDstPixel = 0;

    const auto one16 = set1_epi16(1);

    const auto one32 = set1_epi32(1);

    const auto zero = setzero();

    const auto minus32768 = set1_epi16(-32768);

    constexpr int DEST_ELTS = static_cast<int>(sizeof(zero)) / 2;

    for (; iDstPixel < nDstXWidth - (DEST_ELTS - 1); iDstPixel += DEST_ELTS)

    {

        // Load 2 * DEST_ELTS bytes from each line

        auto firstLine = loadu_int(pSrcScanlineShifted);

        auto secondLine = loadu_int(pSrcScanlineShifted + nChunkXSize);

        // Extend those Bytes as UInt16s

        auto firstLineLo = unpacklo_epi8(firstLine, zero);

        auto firstLineHi = unpackhi_epi8(firstLine, zero);

        auto secondLineLo = unpacklo_epi8(secondLine, zero);

        auto secondLineHi = unpackhi_epi8(secondLine, zero);

        // Multiplication of 16 bit values and horizontal

        // addition of 32 bit results

        // [ src[2*i+0]^2 + src[2*i+1]^2 for i in range(4) ]

        firstLineLo = madd_epi16(firstLineLo, firstLineLo);

        firstLineHi = madd_epi16(firstLineHi, firstLineHi);

        secondLineLo = madd_epi16(secondLineLo, secondLineLo);

        secondLineHi = madd_epi16(secondLineHi, secondLineHi);

        // Vertical addition

        const auto sumSquaresLo = add_epi32(firstLineLo, secondLineLo);

        const auto sumSquaresHi = add_epi32(firstLineHi, secondLineHi);

        const auto sumSquaresPlusOneDiv4Lo =

            srli_epi32(add_epi32(sumSquaresLo, one32), 2);

        const auto sumSquaresPlusOneDiv4Hi =

            srli_epi32(add_epi32(sumSquaresHi, one32), 2);

        // Take square root and truncate/floor to int32

        const auto rmsLo =

            cvttps_epi32(sqrt_ps(cvtepi32_ps(sumSquaresPlusOneDiv4Lo)));

        const auto rmsHi =

            cvttps_epi32(sqrt_ps(cvtepi32_ps(sumSquaresPlusOneDiv4Hi)));

        // Merge back low and high registers with each RMS value

        // as a 16 bit value.

        auto rms = packs_epi32(rmsLo, rmsHi);

        // Round to upper value if it minimizes the

        // error |rms^2 - sumSquares/4|

        // if( 2 * (2 * rms * (rms + 1) + 1) < sumSquares )

        //    rms += 1;

        // which is equivalent to:

        // if( rms * (rms + 1) < (sumSquares+1) / 4 )

        //    rms += 1;

        // And both left and right parts fit on 16 (unsigned) bits

        const auto sumSquaresPlusOneDiv4 =

            packus_epi32(sumSquaresPlusOneDiv4Lo, sumSquaresPlusOneDiv4Hi);

        // cmpgt_epi16 operates on signed int16, but here

        // we have unsigned values, so shift them by -32768 before

        const auto mask = cmpgt_epi16(

            add_epi16(sumSquaresPlusOneDiv4, minus32768),

            add_epi16(mullo_epi16(rms, add_epi16(rms, one16)), minus32768));

        // The value of the mask will be -1 when the correction needs to be

        // applied

        rms = sub_epi16(rms, mask);

        // Pack each 16 bit RMS value to 8 bits

        rms = packus_epi16(rms, rms /* could be anything */);

        store_lo(&pDstScanline[iDstPixel], rms);

        pSrcScanlineShifted += 2 * DEST_ELTS;

    }

    pSrcScanlineShiftedInOut = pSrcScanlineShifted;

    return iDstPixel;

}

/************************************************************************/

/*                       AverageByteSSE2OrAVX2()                        */

/************************************************************************/

static int

AverageByteSSE2OrAVX2(int nDstXWidth, int nChunkXSize,

                      const GByte *&CPL_RESTRICT pSrcScanlineShiftedInOut,

                      GByte *CPL_RESTRICT pDstScanline)

{

    // Optimized implementation for average on Byte by

    // processing by group of 16 output pixels for SSE2, or 32 for AVX2

    const auto zero = setzero();

    const auto two16 = set1_epi16(2);

    const GByte *CPL_RESTRICT pSrcScanlineShifted = pSrcScanlineShiftedInOut;

    constexpr int DEST_ELTS = static_cast<int>(sizeof(zero)) / 2;

    int iDstPixel = 0;

    for (; iDstPixel < nDstXWidth - (2 * DEST_ELTS - 1);

         iDstPixel += 2 * DEST_ELTS)

    {

        decltype(setzero()) average0;

        {

            // Load 2 * DEST_ELTS bytes from each line

            const auto firstLine = loadu_int(pSrcScanlineShifted);

            const auto secondLine =

                loadu_int(pSrcScanlineShifted + nChunkXSize);

            // Extend those Bytes as UInt16s

            const auto firstLineLo = unpacklo_epi8(firstLine, zero);

            const auto firstLineHi = unpackhi_epi8(firstLine, zero);

            const auto secondLineLo = unpacklo_epi8(secondLine, zero);

            const auto secondLineHi = unpackhi_epi8(secondLine, zero);

            // Vertical addition

            const auto sumLo = add_epi16(firstLineLo, secondLineLo);

            const auto sumHi = add_epi16(firstLineHi, secondLineHi);

            // Horizontal addition of adjacent pairs, and recombine low and high

            // parts

            const auto sum = hadd_epi16(sumLo, sumHi);

            // average = (sum + 2) / 4

            average0 = srli_epi16(add_epi16(sum, two16), 2);

            pSrcScanlineShifted += 2 * DEST_ELTS;

        }

        decltype(setzero()) average1;

        {

            // Load 2 * DEST_ELTS bytes from each line

            const auto firstLine = loadu_int(pSrcScanlineShifted);

            const auto secondLine =

                loadu_int(pSrcScanlineShifted + nChunkXSize);

            // Extend those Bytes as UInt16s

            const auto firstLineLo = unpacklo_epi8(firstLine, zero);

            const auto firstLineHi = unpackhi_epi8(firstLine, zero);

            const auto secondLineLo = unpacklo_epi8(secondLine, zero);

            const auto secondLineHi = unpackhi_epi8(secondLine, zero);

            // Vertical addition

            const auto sumLo = add_epi16(firstLineLo, secondLineLo);

            const auto sumHi = add_epi16(firstLineHi, secondLineHi);

            // Horizontal addition of adjacent pairs, and recombine low and high

            // parts

            const auto sum = hadd_epi16(sumLo, sumHi);

            // average = (sum + 2) / 4

            average1 = srli_epi16(add_epi16(sum, two16), 2);

            pSrcScanlineShifted += 2 * DEST_ELTS;

        }

        // Pack each 16 bit average value to 8 bits

        const auto average = packus_epi16(average0, average1);

        storeu_int(&pDstScanline[iDstPixel], average);

    }

    pSrcScanlineShiftedInOut = pSrcScanlineShifted;

    return iDstPixel;

}

/************************************************************************/

/*                      QuadraticMeanUInt16SSE2()                       */

/************************************************************************/

#ifdef __SSE3__

#define sse2_hadd_pd _mm_hadd_pd

#else

inline __m128d sse2_hadd_pd(__m128d a, __m128d b)

{

    auto aLo_bLo =

        _mm_castps_pd(_mm_movelh_ps(_mm_castpd_ps(a), _mm_castpd_ps(b)));

    auto aHi_bHi =

        _mm_castps_pd(_mm_movehl_ps(_mm_castpd_ps(b), _mm_castpd_ps(a)));

    return _mm_add_pd(aLo_bLo, aHi_bHi);  // (aLo + aHi, bLo + bHi)

}

#endif

inline __m128d SQUARE_PD(__m128d x)

{

    return _mm_mul_pd(x, x);

}

#ifdef __AVX2__

inline __m256d SQUARE_PD(__m256d x)

{

    return _mm256_mul_pd(x, x);

}

inline __m256d FIXUP_LANES(__m256d x)

{

    return _mm256_permute4x64_pd(x, _MM_SHUFFLE(3, 1, 2, 0));

}

inline __m256 FIXUP_LANES(__m256 x)

{

    return _mm256_castpd_ps(FIXUP_LANES(_mm256_castps_pd(x)));

}

#endif

static int

QuadraticMeanUInt16SSE2(int nDstXWidth, int nChunkXSize,

                        const uint16_t *&CPL_RESTRICT pSrcScanlineShiftedInOut,

                        uint16_t *CPL_RESTRICT pDstScanline)

{

    // Optimized implementation for RMS on UInt16 by

    // processing by group of 4 output pixels.

    const uint16_t *CPL_RESTRICT pSrcScanlineShifted = pSrcScanlineShiftedInOut;

    int iDstPixel = 0;

    const auto zero = _mm_setzero_si128();

#ifdef __AVX2__

    const auto zeroDot25 = _mm256_set1_pd(0.25);

    const auto zeroDot5 = _mm256_set1_pd(0.5);

    // The first four 0's could be anything, as we only take the bottom

    // 128 bits.

    const auto permutation = _mm256_set_epi32(0, 0, 0, 0, 6, 4, 2, 0);

#else

    const auto zeroDot25 = _mm_set1_pd(0.25);

    const auto zeroDot5 = _mm_set1_pd(0.5);

#endif

    constexpr int DEST_ELTS =

        static_cast<int>(sizeof(zero) / sizeof(uint16_t)) / 2;

    for (; iDstPixel < nDstXWidth - (DEST_ELTS - 1); iDstPixel += DEST_ELTS)

    {

        // Load 8 UInt16 from each line

        const auto firstLine = _mm_loadu_si128(

            reinterpret_cast<__m128i const *>(pSrcScanlineShifted));

        const auto secondLine =

            _mm_loadu_si128(reinterpret_cast<__m128i const *>(

                pSrcScanlineShifted + nChunkXSize));

        // Detect if all of the source values fit in 14 bits.

        // because if x < 2^14, then 4 * x^2 < 2^30 which fits in a signed int32

        // and we can do a much faster implementation.

        const auto maskTmp =

            _mm_srli_epi16(_mm_or_si128(firstLine, secondLine), 14);

#if defined(__i386__) || defined(_M_IX86)

        uint64_t nMaskFitsIn14Bits = 0;

        _mm_storel_epi64(

            reinterpret_cast<__m128i *>(&nMaskFitsIn14Bits),

            _mm_packus_epi16(maskTmp, maskTmp /* could be anything */));

#else

        const auto nMaskFitsIn14Bits = _mm_cvtsi128_si64(

            _mm_packus_epi16(maskTmp, maskTmp /* could be anything */));

#endif

        if (nMaskFitsIn14Bits == 0)

        {

            // Multiplication of 16 bit values and horizontal

            // addition of 32 bit results

            const auto firstLineHSumSquare =

                _mm_madd_epi16(firstLine, firstLine);

            const auto secondLineHSumSquare =

                _mm_madd_epi16(secondLine, secondLine);

            // Vertical addition

            const auto sumSquares =

                _mm_add_epi32(firstLineHSumSquare, secondLineHSumSquare);

            // In theory we should take sqrt(sumSquares * 0.25f)

            // but given the rounding we do, this is equivalent to

            // sqrt((sumSquares + 1)/4). This has been verified exhaustively for

            // sumSquares <= 4 * 16383^2

            const auto one32 = _mm_set1_epi32(1);

            const auto sumSquaresPlusOneDiv4 =

                _mm_srli_epi32(_mm_add_epi32(sumSquares, one32), 2);

            // Take square root and truncate/floor to int32

            auto rms = _mm_cvttps_epi32(

                _mm_sqrt_ps(_mm_cvtepi32_ps(sumSquaresPlusOneDiv4)));

            // Round to upper value if it minimizes the

            // error |rms^2 - sumSquares/4|

            // if( 2 * (2 * rms * (rms + 1) + 1) < sumSquares )

            //    rms += 1;

            // which is equivalent to:

            // if( rms * rms + rms < (sumSquares+1) / 4 )

            //    rms += 1;

            auto mask =

                _mm_cmpgt_epi32(sumSquaresPlusOneDiv4,

                                _mm_add_epi32(_mm_madd_epi16(rms, rms), rms));

            rms = _mm_sub_epi32(rms, mask);

            // Pack each 32 bit RMS value to 16 bits

            rms = _mm_packs_epi32(rms, rms /* could be anything */);

            _mm_storel_epi64(

                reinterpret_cast<__m128i *>(&pDstScanline[iDstPixel]), rms);

            pSrcScanlineShifted += 2 * DEST_ELTS;

            continue;

        }

        // An approach using _mm_mullo_epi16, _mm_mulhi_epu16 before extending

        // to 32 bit would result in 4 multiplications instead of 8, but

        // mullo/mulhi have a worse throughput than mul_pd.

        // Extend those UInt16s as UInt32s

        const auto firstLineLo = _mm_unpacklo_epi16(firstLine, zero);

        const auto firstLineHi = _mm_unpackhi_epi16(firstLine, zero);

        const auto secondLineLo = _mm_unpacklo_epi16(secondLine, zero);

        const auto secondLineHi = _mm_unpackhi_epi16(secondLine, zero);

#ifdef __AVX2__

        // Multiplication of 32 bit values previously converted to 64 bit double

        const auto firstLineLoDbl = SQUARE_PD(_mm256_cvtepi32_pd(firstLineLo));

        const auto firstLineHiDbl = SQUARE_PD(_mm256_cvtepi32_pd(firstLineHi));

        const auto secondLineLoDbl =

            SQUARE_PD(_mm256_cvtepi32_pd(secondLineLo));

        const auto secondLineHiDbl =

            SQUARE_PD(_mm256_cvtepi32_pd(secondLineHi));

        // Vertical addition of squares

        const auto sumSquaresLo =

            _mm256_add_pd(firstLineLoDbl, secondLineLoDbl);

        const auto sumSquaresHi =

            _mm256_add_pd(firstLineHiDbl, secondLineHiDbl);

        // Horizontal addition of squares

        const auto sumSquares =

            FIXUP_LANES(_mm256_hadd_pd(sumSquaresLo, sumSquaresHi));

        const auto sumDivWeight = _mm256_mul_pd(sumSquares, zeroDot25);

        // Take square root and truncate/floor to int32

        auto rms = _mm256_cvttpd_epi32(_mm256_sqrt_pd(sumDivWeight));

        const auto rmsDouble = _mm256_cvtepi32_pd(rms);

        const auto right = _mm256_sub_pd(

            sumDivWeight, _mm256_add_pd(SQUARE_PD(rmsDouble), rmsDouble));

        auto mask =

            _mm256_castpd_ps(_mm256_cmp_pd(zeroDot5, right, _CMP_LT_OS));

        // Extract 32-bit from each of the 4 64-bit masks

        // mask = FIXUP_LANES(_mm256_shuffle_ps(mask, mask,

        // _MM_SHUFFLE(2,0,2,0)));

        mask = _mm256_permutevar8x32_ps(mask, permutation);

        const auto maskI = _mm_castps_si128(_mm256_extractf128_ps(mask, 0));

        // Apply the correction

        rms = _mm_sub_epi32(rms, maskI);

        // Pack each 32 bit RMS value to 16 bits

        rms = _mm_packus_epi32(rms, rms /* could be anything */);

#else

        // Multiplication of 32 bit values previously converted to 64 bit double

        const auto firstLineLoLo = SQUARE_PD(_mm_cvtepi32_pd(firstLineLo));

        const auto firstLineLoHi =

            SQUARE_PD(_mm_cvtepi32_pd(_mm_srli_si128(firstLineLo, 8)));

        const auto firstLineHiLo = SQUARE_PD(_mm_cvtepi32_pd(firstLineHi));

        const auto firstLineHiHi =

            SQUARE_PD(_mm_cvtepi32_pd(_mm_srli_si128(firstLineHi, 8)));

        const auto secondLineLoLo = SQUARE_PD(_mm_cvtepi32_pd(secondLineLo));

        const auto secondLineLoHi =

            SQUARE_PD(_mm_cvtepi32_pd(_mm_srli_si128(secondLineLo, 8)));

        const auto secondLineHiLo = SQUARE_PD(_mm_cvtepi32_pd(secondLineHi));

        const auto secondLineHiHi =

            SQUARE_PD(_mm_cvtepi32_pd(_mm_srli_si128(secondLineHi, 8)));

        // Vertical addition of squares

        const auto sumSquaresLoLo = _mm_add_pd(firstLineLoLo, secondLineLoLo);

        const auto sumSquaresLoHi = _mm_add_pd(firstLineLoHi, secondLineLoHi);

        const auto sumSquaresHiLo = _mm_add_pd(firstLineHiLo, secondLineHiLo);

        const auto sumSquaresHiHi = _mm_add_pd(firstLineHiHi, secondLineHiHi);

        // Horizontal addition of squares

        const auto sumSquaresLo = sse2_hadd_pd(sumSquaresLoLo, sumSquaresLoHi);

        const auto sumSquaresHi = sse2_hadd_pd(sumSquaresHiLo, sumSquaresHiHi);

        const auto sumDivWeightLo = _mm_mul_pd(sumSquaresLo, zeroDot25);

        const auto sumDivWeightHi = _mm_mul_pd(sumSquaresHi, zeroDot25);

        // Take square root and truncate/floor to int32

        const auto rmsLo = _mm_cvttpd_epi32(_mm_sqrt_pd(sumDivWeightLo));

        const auto rmsHi = _mm_cvttpd_epi32(_mm_sqrt_pd(sumDivWeightHi));

        // Correctly round rms to minimize | rms^2 - sumSquares / 4 |

        // if( 0.5 < sumDivWeight - (rms * rms + rms) )

        //     rms += 1;

        const auto rmsLoDouble = _mm_cvtepi32_pd(rmsLo);

        const auto rmsHiDouble = _mm_cvtepi32_pd(rmsHi);

        const auto rightLo = _mm_sub_pd(

            sumDivWeightLo, _mm_add_pd(SQUARE_PD(rmsLoDouble), rmsLoDouble));

        const auto rightHi = _mm_sub_pd(

            sumDivWeightHi, _mm_add_pd(SQUARE_PD(rmsHiDouble), rmsHiDouble));

        const auto maskLo = _mm_castpd_ps(_mm_cmplt_pd(zeroDot5, rightLo));

        const auto maskHi = _mm_castpd_ps(_mm_cmplt_pd(zeroDot5, rightHi));

        // The value of the mask will be -1 when the correction needs to be

        // applied

        const auto mask = _mm_castps_si128(_mm_shuffle_ps(

            maskLo, maskHi, (0 << 0) | (2 << 2) | (0 << 4) | (2 << 6)));

        auto rms = _mm_castps_si128(

            _mm_movelh_ps(_mm_castsi128_ps(rmsLo), _mm_castsi128_ps(rmsHi)));

        // Apply the correction

        rms = _mm_sub_epi32(rms, mask);

        // Pack each 32 bit RMS value to 16 bits

        rms = GDAL_mm_int32_to_uint16(rms);

#endif

        _mm_storel_epi64(reinterpret_cast<__m128i *>(&pDstScanline[iDstPixel]),

                         rms);

        pSrcScanlineShifted += 2 * DEST_ELTS;

    }

    pSrcScanlineShiftedInOut = pSrcScanlineShifted;

    return iDstPixel;

}

/************************************************************************/

/*                         AverageUInt16SSE2()                          */

/************************************************************************/

static int

AverageUInt16SSE2(int nDstXWidth, int nChunkXSize,

                  const uint16_t *&CPL_RESTRICT pSrcScanlineShiftedInOut,

                  uint16_t *CPL_RESTRICT pDstScanline)

{

    // Optimized implementation for average on UInt16 by

    // processing by group of 8 output pixels.

    const auto mask = _mm_set1_epi32(0xFFFF);

    const auto two = _mm_set1_epi32(2);

    const uint16_t *CPL_RESTRICT pSrcScanlineShifted = pSrcScanlineShiftedInOut;

    int iDstPixel = 0;

    constexpr int DEST_ELTS = static_cast<int>(sizeof(mask) / sizeof(uint16_t));

    for (; iDstPixel < nDstXWidth - (DEST_ELTS - 1); iDstPixel += DEST_ELTS)

    {

        __m128i averageLow;

        // Load 8 UInt16 from each line

        {

            const auto firstLine = _mm_loadu_si128(

                reinterpret_cast<__m128i const *>(pSrcScanlineShifted));

            const auto secondLine =

                _mm_loadu_si128(reinterpret_cast<__m128i const *>(

                    pSrcScanlineShifted + nChunkXSize));

            // Horizontal addition and extension to 32 bit

            const auto horizAddFirstLine = _mm_add_epi32(

                _mm_and_si128(firstLine, mask), _mm_srli_epi32(firstLine, 16));

            const auto horizAddSecondLine =

                _mm_add_epi32(_mm_and_si128(secondLine, mask),

                              _mm_srli_epi32(secondLine, 16));

            // Vertical addition and average computation

            // average = (sum + 2) >> 2

            const auto sum = _mm_add_epi32(

                _mm_add_epi32(horizAddFirstLine, horizAddSecondLine), two);

            averageLow = _mm_srli_epi32(sum, 2);

        }

        // Load 8 UInt16 from each line

        __m128i averageHigh;

        {

            const auto firstLine =

                _mm_loadu_si128(reinterpret_cast<__m128i const *>(

                    pSrcScanlineShifted + DEST_ELTS));

            const auto secondLine =

                _mm_loadu_si128(reinterpret_cast<__m128i const *>(

                    pSrcScanlineShifted + DEST_ELTS + nChunkXSize));

            // Horizontal addition and extension to 32 bit

            const auto horizAddFirstLine = _mm_add_epi32(

                _mm_and_si128(firstLine, mask), _mm_srli_epi32(firstLine, 16));

            const auto horizAddSecondLine =

                _mm_add_epi32(_mm_and_si128(secondLine, mask),

                              _mm_srli_epi32(secondLine, 16));

            // Vertical addition and average computation

            // average = (sum + 2) >> 2

            const auto sum = _mm_add_epi32(

                _mm_add_epi32(horizAddFirstLine, horizAddSecondLine), two);

            averageHigh = _mm_srli_epi32(sum, 2);

        }

        // Pack each 32 bit average value to 16 bits

        auto average = GDAL_mm_packus_epi32(averageLow, averageHigh);

        _mm_storeu_si128(reinterpret_cast<__m128i *>(&pDstScanline[iDstPixel]),

                         average);

        pSrcScanlineShifted += 2 * DEST_ELTS;

    }

    pSrcScanlineShiftedInOut = pSrcScanlineShifted;

    return iDstPixel;

}

/************************************************************************/

/*                       QuadraticMeanFloatSSE2()                       */

/************************************************************************/

#if !defined(ARM_V7)

#ifdef __SSE3__

#define sse2_hadd_ps _mm_hadd_ps

#else

inline __m128 sse2_hadd_ps(__m128 a, __m128 b)

{

    auto aEven_bEven = _mm_shuffle_ps(a, b, _MM_SHUFFLE(2, 0, 2, 0));

    auto aOdd_bOdd = _mm_shuffle_ps(a, b, _MM_SHUFFLE(3, 1, 3, 1));

    return _mm_add_ps(aEven_bEven, aOdd_bOdd);  // (aEven + aOdd, bEven + bOdd)

}

#endif

#ifdef __AVX2__

#define set1_ps _mm256_set1_ps

#define loadu_ps _mm256_loadu_ps

#define andnot_ps _mm256_andnot_ps

#define and_ps _mm256_and_ps

#define max_ps _mm256_max_ps

#define shuffle_ps _mm256_shuffle_ps

#define div_ps _mm256_div_ps