shy_fft.h

// Copyright 2014 Emilie Gillet.
//
// Author: Emilie Gillet (emilie.o.gillet@gmail.com)
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//
// See http://creativecommons.org/licenses/MIT/ for more information.
//
// -----------------------------------------------------------------------------
//
// A modest implementation of real FFT for embedded audio applications,
// largely based on Laurent de Soras' FFTReal.
//
// Improvements:
// * No dynamic allocations.
// * No additional buffering (can use the input buffer as a workspace).
// * No big bitrev lookup table.
// * Keep the fixed size template signature, but also provide method for
//   variable size (up to the fixed size).

#ifndef STMLIB_FFT_SHY_FFT_H_
#define STMLIB_FFT_SHY_FFT_H_

#include <algorithm>
#include <cmath>

// Compile-time log 2
template <size_t x>
struct Log2
{
    enum
    {
        value = Log2<x / 2>::value + 1
    };
    typedef char CheckPowerOf2[((x & (x - 1)) == 0 ? 1 : -1)];
};

template <>
struct Log2<1>
{
    enum
    {
        value = 0
    };
};


// Bit reversal LUT size.
template <size_t>
struct BitReversalLut
{
    enum
    {
        size = 1
    };
};
template <>
struct BitReversalLut<3>
{
    enum
    {
        size = 2
    };
};
template <>
struct BitReversalLut<4>
{
    enum
    {
        size = 4
    };
};
template <>
struct BitReversalLut<5>
{
    enum
    {
        size = 8
    };
};
template <>
struct BitReversalLut<6>
{
    enum
    {
        size = 16
    };
};
template <>
struct BitReversalLut<7>
{
    enum
    {
        size = 32
    };
};
template <>
struct BitReversalLut<8>
{
    enum
    {
        size = 64
    };
};


// Typed math functions and constants.
template <typename T>
struct Math
{
    inline T pi() const;
    inline T sqrt_2_div_2() const;
    inline T cos(T x);
    inline T sin(T x);
};

template <>
struct Math<float>
{
    inline float pi() const { return 3.141592653589793f; }
    inline float sqrt_2_div_2() const { return 0.7071067811865476f; }
    inline float cos(float x) { return cosf(x); }
    inline float sin(float x) { return sinf(x); }
};

template <>
struct Math<double>
{
    inline double pi() const { return 3.141592653589793; }
    inline float  sqrt_2_div_2() const { return 0.7071067811865476; }
    inline double cos(double x) { return cos(x); }
    inline double sin(double x) { return sin(x); }
};


// Look-up table for trigonometric data.
template <typename T, size_t num_passes>
class LutPhasor
{
  public:
    LutPhasor() {}
    ~LutPhasor() {}

    void Init()
    {
        Math<T> math;

        for(size_t pass = 3; pass < num_passes; ++pass)
        {
            size_t pass_size = 1L << (pass - 1);
            T*     pass_ptr  = &trig_lut_[(1L << (pass - 1)) - 4];
            T      increment = math.pi() / (pass_size << 1);
            T      phase     = 0.0;
            for(size_t i = 0; i < pass_size; ++i)
            {
                pass_ptr[i] = math.cos(phase);
                phase += increment;
            }
        }
    }

    inline void Start(size_t pass)
    {
        size_t pass_size = 1 << (pass - 1);
        cos_ptr_         = &trig_lut_[pass_size - 4 + 1];
        sin_ptr_         = &trig_lut_[pass_size + pass_size - 4 - 1];
    }

    inline void Rotate()
    {
        ++cos_ptr_;
        --sin_ptr_;
    }

    inline T cos() const { return *cos_ptr_; }
    inline T sin() const { return *sin_ptr_; }

  private:
    T  trig_lut_[(1 << (num_passes - 1)) - 4];
    T* cos_ptr_;
    T* sin_ptr_;
};

template <typename T>
struct LutPhasor<T, 0>
{
    void Init(){};
};
template <typename T>
struct LutPhasor<T, 1>
{
    void Init(){};
};
template <typename T>
struct LutPhasor<T, 2>
{
    void Init(){};
};

template <typename T>
struct LutPhasor<T, 3>
{
    void     Init(){};
    void     Start(size_t){};
    void     Rotate(){};
    inline T cos() const { return 1.0; }
    inline T sin() const { return 0.0; }
};


// Another way of generating roots of unity.
template <typename T, size_t num_passes>
class RotationPhasor
{
  public:
    RotationPhasor() {}
    ~RotationPhasor() {}

    void Init()
    {
        Math<T> math;
        for(size_t pass = 3; pass < num_passes; ++pass)
        {
            size_t index            = (pass - 3) << 1;
            T      angle            = math.pi() / (1 << pass);
            sin_cos_lut_[index]     = math.cos(angle);
            sin_cos_lut_[index + 1] = math.sin(angle);
        }
    }

    inline void Start(size_t pass)
    {
        size_t index = (pass - 3) << 1;
        cos_ = real_ = sin_cos_lut_[index];
        sin_ = imag_ = sin_cos_lut_[index + 1];
    }

    inline void Rotate()
    {
        T temp = cos_ * real_ - sin_ * imag_;
        sin_   = cos_ * imag_ + sin_ * real_;
        cos_   = temp;
    }

    inline T cos() const { return cos_; }
    inline T sin() const { return sin_; }

  private:
    T sin_cos_lut_[(num_passes - 3) << 1];
    T cos_;
    T sin_;
    T real_;
    T imag_;
};

template <typename T>
struct RotationPhasor<T, 0>
{
    void Init(){};
};
template <typename T>
struct RotationPhasor<T, 1>
{
    void Init(){};
};
template <typename T>
struct RotationPhasor<T, 2>
{
    void Init(){};
};

template <typename T>
struct RotationPhasor<T, 3>
{
    void     Init(){};
    void     Start(size_t){};
    void     Rotate(){};
    inline T cos() const { return 1.0; }
    inline T sin() const { return 0.0; }
};

// Direct transform
template <typename T, size_t num_passes, typename Phasor>
struct DirectTransform
{
  private:
    enum
    {
        size = 1 << num_passes
    };

  public:
    void operator()(T* input, T* output, const uint8_t* bit_rev, Phasor* phasor)
    {
        T*      s;
        T*      d;
        Math<T> math;

        // First and second pass.
        d = output;
        for(size_t i = 0; i < size; i += 4)
        {
            const T* s  = input;
            size_t   r0 = num_passes <= 8
                            ? bit_rev[i >> 2]
                            : ((bit_rev[i & 0xff] << 8) | bit_rev[i >> 8])
                                  >> (16 - num_passes);
            size_t r1 = r0 + 2 * (size >> 2);
            size_t r2 = r0 + 1 * (size >> 2);
            size_t r3 = r0 + 3 * (size >> 2);

            d[1] = s[r0] - s[r1];
            d[3] = s[r2] - s[r3];
            T a  = s[r0] + s[r1];
            T b  = s[r2] + s[r3];
            d[0] = a + b;
            d[2] = a - b;
            d += 4;
        }

        // Third pass.
        s = output;
        d = input;
        for(size_t i = 0; i < size; i += 8)
        {
            T v;

            d[i]     = s[i] + s[i + 4];
            d[i + 4] = s[i] - s[i + 4];
            d[i + 2] = s[i + 2];
            d[i + 6] = s[i + 6];

            v        = (s[i + 5] - s[i + 7]) * math.sqrt_2_div_2();
            d[i + 1] = s[i + 1] + v;
            d[i + 3] = s[i + 1] - v;

            v        = (s[i + 5] + s[i + 7]) * math.sqrt_2_div_2();
            d[i + 5] = v + s[i + 3];
            d[i + 7] = v - s[i + 3];
        }

        // Remaining passes.
        for(size_t pass = 3; pass < num_passes; ++pass)
        {
            // Flip source and destination pointers
            {
                T* tmp = s;
                s      = d;
                d      = tmp;
            }

            size_t n   = 1 << pass;
            size_t n_2 = n >> 1;

            for(size_t i = 0; i < size; i += (n << 1))
            {
                T* s1r = s + i;
                T* s2r = s1r + n;
                T* dr  = d + i;
                T* di  = dr + n;

                dr[0]   = s1r[0] + s2r[0];
                di[0]   = s1r[0] - s2r[0];
                dr[n_2] = s1r[n_2];
                di[n_2] = s2r[n_2];
                T* s1i  = s1r + n_2;
                T* s2i  = s1i + n;
                phasor->Start(pass);
                for(size_t j = 1; j < n_2; ++j)
                {
                    T c = phasor->cos();
                    T s = phasor->sin();
                    T v;

                    v      = s2r[j] * c - s2i[j] * s;
                    dr[j]  = s1r[j] + v;
                    di[-j] = s1r[j] - v;

                    v         = s2r[j] * s + s2i[j] * c;
                    di[j]     = v + s1i[j];
                    di[n - j] = v - s1i[j];
                    phasor->Rotate();
                }
            }
        }

        // Annoying additional data copy step.
        if(d != output)
        {
            std::copy(&d[0], &d[size], &output[0]);
        }
    }

    // The exact same thing but with "num_passes" as a run-time argument.
    void operator()(T*             input,
                    T*             output,
                    const uint8_t* bit_rev,
                    Phasor*        phasor,
                    size_t         rt_num_passes)
    {
        T*      s;
        T*      d;
        Math<T> math;
        size_t  rt_size = 1 << rt_num_passes;
        // First and second pass.
        d = output;
        for(size_t i = 0; i < rt_size; i += 4)
        {
            const T* s  = input;
            size_t   r0 = ((bit_rev[i & 0xff] << 8) | bit_rev[i >> 8])
                        >> (16 - rt_num_passes);
            size_t r1 = r0 + 2 * (rt_size >> 2);
            size_t r2 = r0 + 1 * (rt_size >> 2);
            size_t r3 = r0 + 3 * (rt_size >> 2);

            d[1] = s[r0] - s[r1];
            d[3] = s[r2] - s[r3];
            T a  = s[r0] + s[r1];
            T b  = s[r2] + s[r3];
            d[0] = a + b;
            d[2] = a - b;
            d += 4;
        }

        // Third pass.
        s = output;
        d = input;
        for(size_t i = 0; i < rt_size; i += 8)
        {
            T v;

            d[i]     = s[i] + s[i + 4];
            d[i + 4] = s[i] - s[i + 4];
            d[i + 2] = s[i + 2];
            d[i + 6] = s[i + 6];

            v        = (s[i + 5] - s[i + 7]) * math.sqrt_2_div_2();
            d[i + 1] = s[i + 1] + v;
            d[i + 3] = s[i + 1] - v;

            v        = (s[i + 5] + s[i + 7]) * math.sqrt_2_div_2();
            d[i + 5] = v + s[i + 3];
            d[i + 7] = v - s[i + 3];
        }

        // Remaining passes.
        for(size_t pass = 3; pass < rt_num_passes; ++pass)
        {
            // Flip source and destination pointers
            {
                T* tmp = s;
                s      = d;
                d      = tmp;
            }

            size_t n   = 1 << pass;
            size_t n_2 = n >> 1;

            for(size_t i = 0; i < rt_size; i += (n << 1))
            {
                T* s1r = s + i;
                T* s2r = s1r + n;
                T* dr  = d + i;
                T* di  = dr + n;

                dr[0]   = s1r[0] + s2r[0];
                di[0]   = s1r[0] - s2r[0];
                dr[n_2] = s1r[n_2];
                di[n_2] = s2r[n_2];
                T* s1i  = s1r + n_2;
                T* s2i  = s1i + n;
                phasor->Start(pass);
                for(size_t j = 1; j < n_2; ++j)
                {
                    T c = phasor->cos();
                    T s = phasor->sin();
                    T v;

                    v      = s2r[j] * c - s2i[j] * s;
                    dr[j]  = s1r[j] + v;
                    di[-j] = s1r[j] - v;

                    v         = s2r[j] * s + s2i[j] * c;
                    di[j]     = v + s1i[j];
                    di[n - j] = v - s1i[j];
                    phasor->Rotate();
                }
            }
        }

        // Annoying additional data copy step.
        if(d != output)
        {
            std::copy(&d[0], &d[rt_size], &output[0]);
        }
    }
};

template <typename T, typename Phasor>
struct DirectTransform<T, 0, Phasor>
{
    void operator()(const T* i, T* o, T*, const uint8_t*, Phasor*)
    {
        o[0] = i[0];
    }
};

template <typename T, typename Phasor>
struct DirectTransform<T, 1, Phasor>
{
    void operator()(const T* i, T* o, T*, const uint8_t*, Phasor*)
    {
        o[0] = i[0] + i[1];
        o[1] = i[0] - i[1];
    }
};

template <typename T, typename Phasor>
struct DirectTransform<T, 2, Phasor>
{
    void operator()(const T* i, T* o, T*, const uint8_t*, Phasor*)
    {
        o[1] = i[0] - i[2];
        o[3] = i[1] - i[3];
        T a  = i[0] + i[2];
        T b  = i[1] + i[3];
        o[0] = a + b;
        o[2] = a - b;
    }
};


// Inverse transform
template <typename T, size_t num_passes, typename Phasor>
struct InverseTransform
{
  private:
    enum
    {
        size = 1 << num_passes
    };

  public:
    void operator()(T* input, T* output, const uint8_t* bit_rev, Phasor* phasor)
    {
        T*      s = (T*)(input);
        T*      d = output;
        Math<T> math;

        // Remaining passes.
        for(size_t pass = num_passes - 1; pass >= 3; --pass)
        {
            size_t n   = 1 << pass;
            size_t n_2 = n >> 1;

            for(size_t i = 0; i < size; i += (n << 1))
            {
                T* sr  = s + i;
                T* si  = sr + n;
                T* d1r = d + i;
                T* d2r = d1r + n;

                d1r[0]   = sr[0] + si[0];
                d2r[0]   = sr[0] - si[0];
                d1r[n_2] = sr[n_2] * T(2);
                d2r[n_2] = si[n_2] * T(2);

                T* d1i = d1r + n_2;
                T* d2i = d1i + n;
                phasor->Start(pass);
                for(size_t j = 1; j < n_2; ++j)
                {
                    d1r[j] = sr[j] + si[-j];
                    d1i[j] = si[j] - si[n - j];

                    T c  = phasor->cos();
                    T s  = phasor->sin();
                    T vr = sr[j] - si[-j];
                    T vi = si[j] + si[n - j];

                    d2r[j] = vr * c + vi * s;
                    d2i[j] = vi * c - vr * s;
                    phasor->Rotate();
                }
            }

            // Flip source and destination pointers for the next pass.
            if(d == output)
            {
                s = output;
                d = input;
            }
            else
            {
                s = input;
                d = output;
            }
        }

        // Copy data if necessary.
        if(d == output)
        {
            std::copy(&s[0], &s[size], &output[0]);
        }

        s = output;
        d = input;
        for(size_t i = 0; i < size; i += 8)
        {
            T vr, vi;
            d[i]     = s[i] + s[i + 4];
            d[i + 4] = s[i] - s[i + 4];
            d[i + 2] = s[i + 2] * T(2);
            d[i + 6] = s[i + 6] * T(2);
            d[i + 1] = s[i + 1] + s[i + 3];
            d[i + 3] = s[i + 5] - s[i + 7];
            vr       = s[i + 1] - s[i + 3];
            vi       = s[i + 5] + s[i + 7];
            d[i + 5] = (vr + vi) * math.sqrt_2_div_2();
            d[i + 7] = (vi - vr) * math.sqrt_2_div_2();
        }

        // First and second pass.
        s = input;
        d = output;
        for(size_t i = 0; i < size; i += 4)
        {
            size_t r0 = num_passes <= 8
                            ? bit_rev[i >> 2]
                            : ((bit_rev[i & 0xff] << 8) | bit_rev[i >> 8])
                                  >> (16 - num_passes);
            size_t r1 = r0 + 2 * (size >> 2);
            size_t r2 = r0 + 1 * (size >> 2);
            size_t r3 = r0 + 3 * (size >> 2);

            T b_0 = s[0] + s[2];
            T b_2 = s[0] - s[2];
            T b_1 = s[1] * T(2);
            T b_3 = s[3] * T(2);

            d[r0] = b_0 + b_1;
            d[r1] = b_0 - b_1;
            d[r2] = b_2 + b_3;
            d[r3] = b_2 - b_3;
            s += 4;
        }
    }

    void operator()(T*             input,
                    T*             output,
                    const uint8_t* bit_rev,
                    Phasor*        phasor,
                    size_t         rt_num_passes)
    {
        T*      s = (T*)(input);
        T*      d = output;
        Math<T> math;

        size_t rt_size = 1 << rt_num_passes;

        // Remaining passes.
        for(size_t pass = rt_num_passes - 1; pass >= 3; --pass)
        {
            size_t n   = 1 << pass;
            size_t n_2 = n >> 1;

            for(size_t i = 0; i < rt_size; i += (n << 1))
            {
                T* sr  = s + i;
                T* si  = sr + n;
                T* d1r = d + i;
                T* d2r = d1r + n;

                d1r[0]   = sr[0] + si[0];
                d2r[0]   = sr[0] - si[0];
                d1r[n_2] = sr[n_2] * T(2);
                d2r[n_2] = si[n_2] * T(2);

                T* d1i = d1r + n_2;
                T* d2i = d1i + n;
                phasor->Start(pass);
                for(size_t j = 1; j < n_2; ++j)
                {
                    d1r[j] = sr[j] + si[-j];
                    d1i[j] = si[j] - si[n - j];

                    T c  = phasor->cos();
                    T s  = phasor->sin();
                    T vr = sr[j] - si[-j];
                    T vi = si[j] + si[n - j];

                    d2r[j] = vr * c + vi * s;
                    d2i[j] = vi * c - vr * s;
                    phasor->Rotate();
                }
            }

            // Flip source and destination pointers for the next pass.
            if(d == output)
            {
                s = output;
                d = input;
            }
            else
            {
                s = input;
                d = output;
            }
        }

        // Copy data if necessary.
        if(d == output)
        {
            std::copy(&s[0], &s[rt_size], &output[0]);
        }

        s = output;
        d = input;
        for(size_t i = 0; i < rt_size; i += 8)
        {
            T vr, vi;
            d[i]     = s[i] + s[i + 4];
            d[i + 4] = s[i] - s[i + 4];
            d[i + 2] = s[i + 2] * T(2);
            d[i + 6] = s[i + 6] * T(2);
            d[i + 1] = s[i + 1] + s[i + 3];
            d[i + 3] = s[i + 5] - s[i + 7];
            vr       = s[i + 1] - s[i + 3];
            vi       = s[i + 5] + s[i + 7];
            d[i + 5] = (vr + vi) * math.sqrt_2_div_2();
            d[i + 7] = (vi - vr) * math.sqrt_2_div_2();
        }

        // First and second pass.
        s = input;
        d = output;
        for(size_t i = 0; i < rt_size; i += 4)
        {
            size_t r0 = ((bit_rev[i & 0xff] << 8) | bit_rev[i >> 8])
                        >> (16 - rt_num_passes);
            size_t r1 = r0 + 2 * (rt_size >> 2);
            size_t r2 = r0 + 1 * (rt_size >> 2);
            size_t r3 = r0 + 3 * (rt_size >> 2);

            T b_0 = s[0] + s[2];
            T b_2 = s[0] - s[2];
            T b_1 = s[1] * T(2);
            T b_3 = s[3] * T(2);

            d[r0] = b_0 + b_1;
            d[r1] = b_0 - b_1;
            d[r2] = b_2 + b_3;
            d[r3] = b_2 - b_3;
            s += 4;
        }
    }
};

template <typename T, typename Phasor>
struct InverseTransform<T, 0, Phasor>
{
    void operator()(const T* i, T* o, T*, const uint8_t*, Phasor*)
    {
        o[0] = i[0];
    }
};

template <typename T, typename Phasor>
struct InverseTransform<T, 1, Phasor>
{
    void operator()(const T* i, T* o, T*, const uint8_t*, Phasor*)
    {
        o[0] = i[0] + i[1];
        o[1] = i[0] - i[1];
    }
};

template <typename T, typename Phasor>
struct InverseTransform<T, 2, Phasor>
{
    void operator()(const T* i, T* o, T*, const uint8_t*, Phasor*)
    {
        T a = i[0] + i[2];
        T b = i[0] - i[2];

        o[0] = a + i[1] * T(2);
        o[2] = a - i[1] * T(2);
        o[1] = b + i[3] * T(2);
        o[3] = b - i[3] * T(2);
    }
};


template <typename T                               = float,
          size_t size                              = 16,
          template <typename, size_t> class Phasor = LutPhasor>
class ShyFFT
{
  public:
    enum
    {
        num_passes = Log2<size>::value,
        max_size   = size
    };

  private:
    typedef Phasor<T, num_passes> PhasorType;

  public:
    ShyFFT() {}
    ~ShyFFT() {}

    void Init()
    {
        bit_rev_[0] = 0;
        for(size_t i = 1; i < sizeof(bit_rev_); ++i)
        {
            uint8_t byte        = 0;
            uint8_t source      = i << 2;
            uint8_t destination = static_cast<uint8_t>(size >> 1);
            while(source)
            {
                if(source & 1)
                {
                    byte |= destination;
                }
                destination >>= 1;
                source >>= 1;
            }
            bit_rev_[i] = byte;
        }
        phasor_.Init();
    }

    void Direct(T* input, T* output)
    {
        DirectTransform<T, num_passes, Phasor<T, num_passes>> d;
        d(input,
          output,
          num_passes <= 8 ? &bit_rev_[0] : bit_rev_256_lut_,
          &phasor_);
    }

    void Inverse(T* input, T* output)
    {
        InverseTransform<T, num_passes, Phasor<T, num_passes>> i;
        i(input,
          output,
          num_passes <= 8 ? &bit_rev_[0] : bit_rev_256_lut_,
          &phasor_);
    }

    void Direct(T* input, T* output, size_t n)
    {
        DirectTransform<T, num_passes, Phasor<T, num_passes>> d;
        d(input, output, bit_rev_256_lut_, &phasor_, n);
    }

    void Inverse(T* input, T* output, size_t n)
    {
        InverseTransform<T, num_passes, Phasor<T, num_passes>> i;
        i(input, output, bit_rev_256_lut_, &phasor_, n);
    }


  private:
    PhasorType           phasor_;
    uint8_t              bit_rev_[BitReversalLut<num_passes>::size];
    static const uint8_t bit_rev_256_lut_[256];
};

template <typename T, size_t size, template <typename, size_t> class Phasor>
const uint8_t ShyFFT<T, size, Phasor>::bit_rev_256_lut_[256] = {
#define R2(n) n, n + 2 * 64, n + 1 * 64, n + 3 * 64
#define R4(n) R2(n), R2(n + 2 * 16), R2(n + 1 * 16), R2(n + 3 * 16)
#define R6(n) R4(n), R4(n + 2 * 4), R4(n + 1 * 4), R4(n + 3 * 4)
    R6(0),
    R6(2),
    R6(1),
    R6(3)};

#endif