///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2015 Edouard Griffiths, F4EXB                                   //
//                                                                               //
// This program is free software; you can redistribute it and/or modify          //
// it under the terms of the GNU General Public License as published by          //
// the Free Software Foundation as version 3 of the License, or                  //
//                                                                               //
// This program is distributed in the hope that it will be useful,               //
// but WITHOUT ANY WARRANTY; without even the implied warranty of                //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the                  //
// GNU General Public License V3 for more details.                               //
//                                                                               //
// You should have received a copy of the GNU General Public License             //
// along with this program. If not, see .          //
///////////////////////////////////////////////////////////////////////////////////
#ifndef INCLUDE_GPL_DSP_DECIMATORS_H_
#define INCLUDE_GPL_DSP_DECIMATORS_H_
#include "dsp/dsptypes.h"
#ifdef USE_SSE4_1
#include "dsp/inthalfbandfiltereo1.h"
#else
#include "dsp/inthalfbandfilterdb.h"
#endif
#define DECIMATORS_HB_FILTER_ORDER 64
template
struct decimation_shifts
{
    static const uint pre1   = 0;
    static const uint pre2   = 0;
    static const uint post2  = 0;
    static const uint pre4   = 0;
    static const uint post4  = 0;
    static const uint pre8   = 0;
    static const uint post8  = 0;
    static const uint pre16  = 0;
    static const uint post16 = 0;
    static const uint pre32  = 0;
    static const uint post32 = 0;
    static const uint pre64  = 0;
    static const uint post64 = 0;
};
template<>
struct decimation_shifts<16, 16>
{
    static const uint pre1   = 0;
    static const uint pre2   = 0;
    static const uint post2  = 1;
    static const uint pre4   = 0;
    static const uint post4  = 2;
    static const uint pre8   = 0;
    static const uint post8  = 3;
    static const uint pre16  = 0;
    static const uint post16 = 4;
    static const uint pre32  = 0;
    static const uint post32 = 5;
    static const uint pre64  = 0;
    static const uint post64 = 6;
};
template<>
struct decimation_shifts<16, 12>
{
    static const uint pre1   = 4;
    static const uint pre2   = 3;
    static const uint post2  = 0;
    static const uint pre4   = 2;
    static const uint post4  = 0;
    static const uint pre8   = 1;
    static const uint post8  = 0;
    static const uint pre16  = 0;
    static const uint post16 = 0;
    static const uint pre32  = 0;
    static const uint post32 = 1;
    static const uint pre64  = 0;
    static const uint post64 = 2;
};
template<>
struct decimation_shifts<16, 8>
{
    static const uint pre1   = 6;
    static const uint pre2   = 5;
    static const uint post2  = 0;
    static const uint pre4   = 4;
    static const uint post4  = 0;
    static const uint pre8   = 3;
    static const uint post8  = 0;
    static const uint pre16  = 2;
    static const uint post16 = 0;
    static const uint pre32  = 1;
    static const uint post32 = 0;
    static const uint pre64  = 0;
    static const uint post64 = 0;
};
template
class Decimators
{
public:
    // interleaved I/Q input buffer
	void decimate1(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate2_u(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate2_inf(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate2_sup(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate2_cen(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate4_inf(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate4_sup(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate4_cen(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate8_inf(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate8_sup(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate8_cen(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate16_inf(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate16_sup(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate16_cen(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate32_inf(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate32_sup(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate32_cen(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate64_inf(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate64_sup(SampleVector::iterator* it, const T* buf, qint32 len);
	void decimate64_cen(SampleVector::iterator* it, const T* buf, qint32 len);
	// separate I and Q input buffers
    void decimate1(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate2_u(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate2_inf(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate2_sup(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate2_cen(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate4_inf(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate4_sup(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate4_cen(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate8_inf(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate8_sup(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate8_cen(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate16_inf(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate16_sup(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate16_cen(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate32_inf(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate32_sup(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate32_cen(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate64_inf(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate64_sup(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
    void decimate64_cen(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len);
private:
#ifdef USE_SSE4_1
    IntHalfbandFilterEO1 m_decimator2;  // 1st stages
    IntHalfbandFilterEO1 m_decimator4;  // 2nd stages
    IntHalfbandFilterEO1 m_decimator8;  // 3rd stages
    IntHalfbandFilterEO1 m_decimator16; // 4th stages
    IntHalfbandFilterEO1 m_decimator32; // 5th stages
    IntHalfbandFilterEO1 m_decimator64; // 6th stages
#else
	IntHalfbandFilterDB m_decimator2;  // 1st stages
	IntHalfbandFilterDB m_decimator4;  // 2nd stages
	IntHalfbandFilterDB m_decimator8;  // 3rd stages
	IntHalfbandFilterDB m_decimator16; // 4th stages
	IntHalfbandFilterDB m_decimator32; // 5th stages
	IntHalfbandFilterDB m_decimator64; // 6th stages
#endif
};
template
void Decimators::decimate1(SampleVector::iterator* it, const T* buf, qint32 len)
{
	qint32 xreal, yimag;
	for (int pos = 0; pos < len - 1; pos += 2)
	{
		xreal = buf[pos+0];
		yimag = buf[pos+1];
		(**it).setReal(xreal << decimation_shifts::pre1); // Valgrind optim (2 - comment not repeated)
		(**it).setImag(yimag << decimation_shifts::pre1);
		++(*it); // Valgrind optim (comment not repeated)
	}
}
template
void Decimators::decimate1(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len)
{
    qint32 xreal, yimag;
    for (int pos = 0; pos < len; pos += 1)
    {
        xreal = bufI[pos];
        yimag = bufQ[pos];
        (**it).setReal(xreal << decimation_shifts::pre1); // Valgrind optim (2 - comment not repeated)
        (**it).setImag(yimag << decimation_shifts::pre1);
        ++(*it); // Valgrind optim (comment not repeated)
    }
}
template
void Decimators::decimate2_u(SampleVector::iterator* it, const T* buf, qint32 len)
{
	qint32 xreal, yimag;
	for (int pos = 0; pos < len - 7; pos += 8)
	{
		xreal = (buf[pos+0] - buf[pos+3]) << decimation_shifts::pre2;
		yimag = (buf[pos+1] + buf[pos+2] - 255) << decimation_shifts::pre2;
		(**it).setReal(xreal >> decimation_shifts::post2);
		(**it).setImag(yimag >> decimation_shifts::post2);
		++(*it);
		xreal = (buf[pos+7] - buf[pos+4]) << decimation_shifts::pre2;
		yimag = (255 - buf[pos+5] - buf[pos+6]) << decimation_shifts::pre2;
		(**it).setReal(xreal >> decimation_shifts::post2);
		(**it).setImag(yimag >> decimation_shifts::post2);
		++(*it);
	}
}
template
void Decimators::decimate2_u(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len)
{
    qint32 xreal, yimag;
    for (int pos = 0; pos < len - 3; pos += 4)
    {
        // 0: I[0] 1: Q[0] 2: I[1] 3: Q[1]
        xreal = (bufI[pos] - bufQ[pos+1]) << decimation_shifts::pre2;
        yimag = (bufQ[pos] + bufI[pos+1] - 255) << decimation_shifts::pre2;
        (**it).setReal(xreal >> decimation_shifts::post2);
        (**it).setImag(yimag >> decimation_shifts::post2);
        ++(*it);
        // 4: I[2] 5: Q[2] 6: I[3] 7: Q[3]
        xreal = (bufQ[pos+3] - bufI[pos+2]) << decimation_shifts::pre2;
        yimag = (255 - bufQ[pos+2] - bufI[pos+3]) << decimation_shifts::pre2;
        (**it).setReal(xreal >> decimation_shifts::post2);
        (**it).setImag(yimag >> decimation_shifts::post2);
        ++(*it);
    }
}
template
void Decimators::decimate2_inf(SampleVector::iterator* it, const T* buf, qint32 len)
{
	qint32 xreal, yimag;
	for (int pos = 0; pos < len - 7; pos += 8)
	{
		xreal = (buf[pos+0] - buf[pos+3]) << decimation_shifts::pre2;
		yimag = (buf[pos+1] + buf[pos+2]) << decimation_shifts::pre2;
		(**it).setReal(xreal >> decimation_shifts::post2);
		(**it).setImag(yimag >> decimation_shifts::post2);
		++(*it);
		xreal = (buf[pos+7] - buf[pos+4]) << decimation_shifts::pre2;
		yimag = (- buf[pos+5] - buf[pos+6]) << decimation_shifts::pre2;
		(**it).setReal(xreal >> decimation_shifts::post2);
		(**it).setImag(yimag >> decimation_shifts::post2);
		++(*it);
	}
}
template
void Decimators::decimate2_inf(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len)
{
    qint32 xreal, yimag;
    for (int pos = 0; pos < len - 3; pos += 4)
    {
        // 0: I[0] 1: Q[0] 2: I[1] 3: Q[1]
        xreal = (bufI[pos] - bufQ[pos+1]) << decimation_shifts::pre2;
        yimag = (bufQ[pos] + bufI[pos+1]) << decimation_shifts::pre2;
        (**it).setReal(xreal >> decimation_shifts::post2);
        (**it).setImag(yimag >> decimation_shifts::post2);
        ++(*it);
        // 4: I[2] 5: Q[2] 6: I[3] 7: Q[3]
        xreal = (bufQ[pos+3] - bufI[pos+2]) << decimation_shifts::pre2;
        yimag = (- bufQ[pos+2] - bufI[pos+3]) << decimation_shifts::pre2;
        (**it).setReal(xreal >> decimation_shifts::post2);
        (**it).setImag(yimag >> decimation_shifts::post2);
        ++(*it);
    }
}
template
void Decimators::decimate2_sup(SampleVector::iterator* it, const T* buf, qint32 len)
{
	qint32 xreal, yimag;
	for (int pos = 0; pos < len - 7; pos += 8)
	{
		xreal = (buf[pos+1] - buf[pos+2]) << decimation_shifts::pre2;
		yimag = (- buf[pos+0] - buf[pos+3]) << decimation_shifts::pre2;
		(**it).setReal(xreal >> decimation_shifts::post2);
		(**it).setImag(yimag >> decimation_shifts::post2);
		++(*it);
		xreal = (buf[pos+6] - buf[pos+5]) << decimation_shifts::pre2;
		yimag = (buf[pos+4] + buf[pos+7]) << decimation_shifts::pre2;
		(**it).setReal(xreal >> decimation_shifts::post2);
		(**it).setImag(yimag >> decimation_shifts::post2);
		++(*it);
	}
}
template
void Decimators::decimate2_sup(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len)
{
    qint32 xreal, yimag;
    for (int pos = 0; pos < len - 3; pos += 4)
    {
        // 0: I[0] 1: Q[0] 2: I[1] 3: Q[1]
        xreal = (bufQ[pos] - bufI[pos+1]) << decimation_shifts::pre2;
        yimag = (- bufI[pos] - bufQ[pos+1]) << decimation_shifts::pre2;
        (**it).setReal(xreal >> decimation_shifts::post2);
        (**it).setImag(yimag >> decimation_shifts::post2);
        ++(*it);
        // 4: I[2] 5: Q[2] 6: I[3] 7: Q[3]
        xreal = (bufI[pos+3] - bufQ[pos+2]) << decimation_shifts::pre2;
        yimag = (bufI[pos+2] + bufQ[pos+3]) << decimation_shifts::pre2;
        (**it).setReal(xreal >> decimation_shifts::post2);
        (**it).setImag(yimag >> decimation_shifts::post2);
        ++(*it);
    }
}
template
void Decimators::decimate4_inf(SampleVector::iterator* it, const T* buf, qint32 len)
{
	qint32 xreal, yimag;
	for (int pos = 0; pos < len - 7; pos += 8)
	{
		xreal = (buf[pos+0] - buf[pos+3] + buf[pos+7] - buf[pos+4]) << decimation_shifts::pre4;
		yimag = (buf[pos+1] - buf[pos+5] + buf[pos+2] - buf[pos+6]) << decimation_shifts::pre4;
		(**it).setReal(xreal >> decimation_shifts::post4);
		(**it).setImag(yimag >> decimation_shifts::post4);
		++(*it);
	}
}
template
void Decimators::decimate4_inf(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len)
{
    qint32 xreal, yimag;
    for (int pos = 0; pos < len - 3; pos += 4)
    {
        xreal = (bufI[pos] - bufQ[pos+1] + bufQ[pos+3] - bufI[pos+2]) << decimation_shifts::pre4;
        yimag = (bufQ[pos] - bufQ[pos+2] + bufI[pos+1] - bufI[pos+3]) << decimation_shifts::pre4;
        (**it).setReal(xreal >> decimation_shifts::post4);
        (**it).setImag(yimag >> decimation_shifts::post4);
        ++(*it);
    }
}
template
void Decimators::decimate4_sup(SampleVector::iterator* it, const T* buf, qint32 len)
{
	// Sup (USB):
	//            x  y   x  y   x   y  x   y  / x -> 1,-2,-5,6 / y -> -0,-3,4,7
	// [ rotate:  1, 0, -2, 3, -5, -4, 6, -7]
	// Inf (LSB):
	//            x  y   x  y   x   y  x   y  / x -> 0,-3,-4,7 / y -> 1,2,-5,-6
	// [ rotate:  0, 1, -3, 2, -4, -5, 7, -6]
	qint32 xreal, yimag;
	for (int pos = 0; pos < len - 7; pos += 8)
	{
		xreal = (buf[pos+1] - buf[pos+2] - buf[pos+5] + buf[pos+6]) << decimation_shifts::pre4;
		yimag = (- buf[pos+0] - buf[pos+3] + buf[pos+4] + buf[pos+7]) << decimation_shifts::pre4;
		(**it).setReal(xreal >> decimation_shifts::post4);
		(**it).setImag(yimag >> decimation_shifts::post4);
		++(*it);
	}
}
template
void Decimators::decimate4_sup(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len)
{
    qint32 xreal, yimag;
    for (int pos = 0; pos < len - 3; pos += 4)
    {
        xreal = (bufQ[pos] - bufI[pos+1] - bufQ[pos+2] + bufI[pos+3]) << decimation_shifts::pre4;
        yimag = (- bufI[pos] - bufQ[pos+1] + bufI[pos+2] + bufQ[pos+3]) << decimation_shifts::pre4;
        (**it).setReal(xreal >> decimation_shifts::post4);
        (**it).setImag(yimag >> decimation_shifts::post4);
        ++(*it);
    }
}
template
void Decimators::decimate8_inf(SampleVector::iterator* it, const T* buf, qint32 len)
{
	qint32 xreal[2], yimag[2];
	for (int pos = 0; pos < len - 15; pos += 8)
	{
		xreal[0] = (buf[pos+0] - buf[pos+3] + buf[pos+7] - buf[pos+4]) << decimation_shifts::pre8;
		yimag[0] = (buf[pos+1] - buf[pos+5] + buf[pos+2] - buf[pos+6]) << decimation_shifts::pre8;
		pos += 8;
		xreal[1] = (buf[pos+0] - buf[pos+3] + buf[pos+7] - buf[pos+4]) << decimation_shifts::pre8;
		yimag[1] = (buf[pos+1] - buf[pos+5] + buf[pos+2] - buf[pos+6]) << decimation_shifts::pre8;
		m_decimator2.myDecimate(xreal[0], yimag[0], &xreal[1], &yimag[1]);
		(**it).setReal(xreal[1] >> decimation_shifts::post8);
		(**it).setImag(yimag[1] >> decimation_shifts::post8);
		++(*it);
	}
}
template
void Decimators::decimate8_inf(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len)
{
    qint32 xreal[2], yimag[2];
    for (int pos = 0; pos < len - 7; pos += 4)
    {
        xreal[0] = (bufI[pos] - bufQ[pos+1] + bufQ[pos+3] - bufI[pos+2]) << decimation_shifts::pre8;
        yimag[0] = (bufQ[pos] - bufQ[pos+2] + bufI[pos+1] - bufI[pos+3]) << decimation_shifts::pre8;
        pos += 4;
        xreal[1] = (bufI[pos] - bufQ[pos+1] + bufQ[pos+3] - bufI[pos+2]) << decimation_shifts::pre8;
        yimag[1] = (bufQ[pos] - bufQ[pos+2] + bufI[pos+1] - bufQ[pos+3]) << decimation_shifts::pre8;
        m_decimator2.myDecimate(xreal[0], yimag[0], &xreal[1], &yimag[1]);
        (**it).setReal(xreal[1] >> decimation_shifts::post8);
        (**it).setImag(yimag[1] >> decimation_shifts::post8);
        ++(*it);
    }
}
template
void Decimators::decimate8_sup(SampleVector::iterator* it, const T* buf, qint32 len)
{
	qint32 xreal[2], yimag[2];
	for (int pos = 0; pos < len - 15; pos += 8)
	{
		xreal[0] = (buf[pos+1] - buf[pos+2] - buf[pos+5] + buf[pos+6]) << decimation_shifts::pre8;
		yimag[0] = (- buf[pos+0] - buf[pos+3] + buf[pos+4] + buf[pos+7]) << decimation_shifts::pre8;
		pos += 8;
		xreal[1] = (buf[pos+1] - buf[pos+2] - buf[pos+5] + buf[pos+6]) << decimation_shifts::pre8;
		yimag[1] = (- buf[pos+0] - buf[pos+3] + buf[pos+4] + buf[pos+7]) << decimation_shifts::pre8;
		m_decimator2.myDecimate(xreal[0], yimag[0], &xreal[1], &yimag[1]);
		(**it).setReal(xreal[1] >> decimation_shifts::post8);
		(**it).setImag(yimag[1] >> decimation_shifts::post8);
		++(*it);
	}
}
template
void Decimators::decimate8_sup(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len)
{
    qint32 xreal[2], yimag[2];
    for (int pos = 0; pos < len - 7; pos += 4)
    {
        xreal[0] = (bufQ[pos]   - bufI[pos+1] - bufQ[pos+2] + bufI[pos+3]) << decimation_shifts::pre8;
        yimag[0] = (- bufI[pos] - bufQ[pos+1] + bufI[pos+2] + bufQ[pos+3]) << decimation_shifts::pre8;
        pos += 4;
        xreal[1] = (bufQ[pos]   - bufI[pos+1] - bufQ[pos+2] + bufI[pos+3]) << decimation_shifts::pre8;
        yimag[1] = (- bufI[pos] - bufQ[pos+1] + bufI[pos+2] + bufQ[pos+3]) << decimation_shifts::pre8;
        m_decimator2.myDecimate(xreal[0], yimag[0], &xreal[1], &yimag[1]);
        (**it).setReal(xreal[1] >> decimation_shifts::post8);
        (**it).setImag(yimag[1] >> decimation_shifts::post8);
        ++(*it);
    }
}
template
void Decimators::decimate16_inf(SampleVector::iterator* it, const T* buf, qint32 len)
{
	// Offset tuning: 4x downsample and rotate, then
	// downsample 4x more. [ rotate:  0, 1, -3, 2, -4, -5, 7, -6]
	qint32 xreal[4], yimag[4];
	for (int pos = 0; pos < len - 31; )
	{
		for (int i = 0; i < 4; i++)
		{
			xreal[i] = (buf[pos+0] - buf[pos+3] + buf[pos+7] - buf[pos+4]) << decimation_shifts::pre16;
			yimag[i] = (buf[pos+1] - buf[pos+5] + buf[pos+2] - buf[pos+6]) << decimation_shifts::pre16;
			pos += 8;
		}
		m_decimator2.myDecimate(xreal[0], yimag[0], &xreal[1], &yimag[1]);
		m_decimator2.myDecimate(xreal[2], yimag[2], &xreal[3], &yimag[3]);
		m_decimator4.myDecimate(xreal[1], yimag[1], &xreal[3], &yimag[3]);
		(**it).setReal(xreal[3] >> decimation_shifts::post16);
		(**it).setImag(yimag[3] >> decimation_shifts::post16);
		++(*it);
	}
}
template
void Decimators::decimate16_inf(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len)
{
    // Offset tuning: 4x downsample and rotate, then
    // downsample 4x more. [ rotate:  0, 1, -3, 2, -4, -5, 7, -6]
    qint32 xreal[4], yimag[4];
    for (int pos = 0; pos < len - 15; )
    {
        for (int i = 0; i < 4; i++)
        {
            xreal[i] = (bufI[pos] - bufI[pos+1] + bufQ[pos+3] - bufI[pos+2]) << decimation_shifts::pre16;
            yimag[i] = (bufQ[pos] - bufQ[pos+2] + bufI[pos+1] - bufI[pos+3]) << decimation_shifts::pre16;
            pos += 4;
        }
        m_decimator2.myDecimate(xreal[0], yimag[0], &xreal[1], &yimag[1]);
        m_decimator2.myDecimate(xreal[2], yimag[2], &xreal[3], &yimag[3]);
        m_decimator4.myDecimate(xreal[1], yimag[1], &xreal[3], &yimag[3]);
        (**it).setReal(xreal[3] >> decimation_shifts::post16);
        (**it).setImag(yimag[3] >> decimation_shifts::post16);
        ++(*it);
    }
}
template
void Decimators::decimate16_sup(SampleVector::iterator* it, const T* buf, qint32 len)
{
	// Offset tuning: 4x downsample and rotate, then
	// downsample 4x more. [ rotate:  1, 0, -2, 3, -5, -4, 6, -7]
	qint32 xreal[4], yimag[4];
	for (int pos = 0; pos < len - 31; )
	{
		for (int i = 0; i < 4; i++)
		{
			xreal[i] = (buf[pos+1] - buf[pos+2] - buf[pos+5] + buf[pos+6]) << decimation_shifts::pre16;
			yimag[i] = (buf[pos+4] + buf[pos+7] - buf[pos+0] - buf[pos+3]) << decimation_shifts::pre16;
			pos += 8;
		}
		m_decimator2.myDecimate(xreal[0], yimag[0], &xreal[1], &yimag[1]);
		m_decimator2.myDecimate(xreal[2], yimag[2], &xreal[3], &yimag[3]);
		m_decimator4.myDecimate(xreal[1], yimag[1], &xreal[3], &yimag[3]);
		(**it).setReal(xreal[3] >> decimation_shifts::post16);
		(**it).setImag(yimag[3] >> decimation_shifts::post16);
		++(*it);
	}
}
template
void Decimators::decimate16_sup(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len)
{
    // Offset tuning: 4x downsample and rotate, then
    // downsample 4x more. [ rotate:  1, 0, -2, 3, -5, -4, 6, -7]
    qint32 xreal[4], yimag[4];
    for (int pos = 0; pos < len - 15; )
    {
        for (int i = 0; i < 4; i++)
        {
            xreal[i] = (bufQ[pos+0] - bufI[pos+1] - bufQ[pos+2] + bufI[pos+3]) << decimation_shifts::pre16;
            yimag[i] = (bufI[pos+2] + bufQ[pos+3] - bufI[pos+0] - bufQ[pos+1]) << decimation_shifts::pre16;
            pos += 4;
        }
        m_decimator2.myDecimate(xreal[0], yimag[0], &xreal[1], &yimag[1]);
        m_decimator2.myDecimate(xreal[2], yimag[2], &xreal[3], &yimag[3]);
        m_decimator4.myDecimate(xreal[1], yimag[1], &xreal[3], &yimag[3]);
        (**it).setReal(xreal[3] >> decimation_shifts::post16);
        (**it).setImag(yimag[3] >> decimation_shifts::post16);
        ++(*it);
    }
}
template
void Decimators::decimate32_inf(SampleVector::iterator* it, const T* buf, qint32 len)
{
	qint32 xreal[8], yimag[8];
	for (int pos = 0; pos < len - 63; )
	{
		for (int i = 0; i < 8; i++)
		{
			xreal[i] = (buf[pos+0] - buf[pos+3] + buf[pos+7] - buf[pos+4]) << decimation_shifts::pre32;
			yimag[i] = (buf[pos+1] - buf[pos+5] + buf[pos+2] - buf[pos+6]) << decimation_shifts::pre32;
			pos += 8;
		}
		m_decimator2.myDecimate(xreal[0], yimag[0], &xreal[1], &yimag[1]);
		m_decimator2.myDecimate(xreal[2], yimag[2], &xreal[3], &yimag[3]);
		m_decimator2.myDecimate(xreal[4], yimag[4], &xreal[5], &yimag[5]);
		m_decimator2.myDecimate(xreal[6], yimag[6], &xreal[7], &yimag[7]);
		m_decimator4.myDecimate(xreal[1], yimag[1], &xreal[3], &yimag[3]);
		m_decimator4.myDecimate(xreal[5], yimag[5], &xreal[7], &yimag[7]);
		m_decimator8.myDecimate(xreal[3], yimag[3], &xreal[7], &yimag[7]);
		(**it).setReal(xreal[7] >> decimation_shifts::post32);
		(**it).setImag(yimag[7] >> decimation_shifts::post32);
		++(*it);
	}
}
template
void Decimators::decimate32_inf(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len)
{
    qint32 xreal[8], yimag[8];
    for (int pos = 0; pos < len - 31; )
    {
        for (int i = 0; i < 8; i++)
        {
            xreal[i] = (bufI[pos+0] - bufQ[pos+1] + bufQ[pos+3] - bufI[pos+2]) << decimation_shifts::pre32;
            yimag[i] = (bufQ[pos+0] - bufQ[pos+2] + bufI[pos+1] - bufI[pos+3]) << decimation_shifts::pre32;
            pos += 4;
        }
        m_decimator2.myDecimate(xreal[0], yimag[0], &xreal[1], &yimag[1]);
        m_decimator2.myDecimate(xreal[2], yimag[2], &xreal[3], &yimag[3]);
        m_decimator2.myDecimate(xreal[4], yimag[4], &xreal[5], &yimag[5]);
        m_decimator2.myDecimate(xreal[6], yimag[6], &xreal[7], &yimag[7]);
        m_decimator4.myDecimate(xreal[1], yimag[1], &xreal[3], &yimag[3]);
        m_decimator4.myDecimate(xreal[5], yimag[5], &xreal[7], &yimag[7]);
        m_decimator8.myDecimate(xreal[3], yimag[3], &xreal[7], &yimag[7]);
        (**it).setReal(xreal[7] >> decimation_shifts::post32);
        (**it).setImag(yimag[7] >> decimation_shifts::post32);
        ++(*it);
    }
}
template
void Decimators::decimate32_sup(SampleVector::iterator* it, const T* buf, qint32 len)
{
	qint32 xreal[8], yimag[8];
	for (int pos = 0; pos < len - 63; )
	{
		for (int i = 0; i < 8; i++)
		{
			xreal[i] = (buf[pos+1] - buf[pos+2] - buf[pos+5] + buf[pos+6]) << decimation_shifts::pre32;
			yimag[i] = (buf[pos+4] + buf[pos+7] - buf[pos+0] - buf[pos+3]) << decimation_shifts::pre32;
			pos += 8;
		}
		m_decimator2.myDecimate(xreal[0], yimag[0], &xreal[1], &yimag[1]);
		m_decimator2.myDecimate(xreal[2], yimag[2], &xreal[3], &yimag[3]);
		m_decimator2.myDecimate(xreal[4], yimag[4], &xreal[5], &yimag[5]);
		m_decimator2.myDecimate(xreal[6], yimag[6], &xreal[7], &yimag[7]);
		m_decimator4.myDecimate(xreal[1], yimag[1], &xreal[3], &yimag[3]);
		m_decimator4.myDecimate(xreal[5], yimag[5], &xreal[7], &yimag[7]);
		m_decimator8.myDecimate(xreal[3], yimag[3], &xreal[7], &yimag[7]);
		(**it).setReal(xreal[7] >> decimation_shifts::post32);
		(**it).setImag(yimag[7] >> decimation_shifts::post32);
		++(*it);
	}
}
template
void Decimators::decimate32_sup(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len)
{
    qint32 xreal[8], yimag[8];
    for (int pos = 0; pos < len - 31; )
    {
        for (int i = 0; i < 8; i++)
        {
            xreal[i] = (bufQ[pos+0] - bufI[pos+1] - bufQ[pos+2] + bufI[pos+3]) << decimation_shifts::pre32;
            yimag[i] = (bufI[pos+2] + bufQ[pos+3] - bufI[pos+0] - bufQ[pos+1]) << decimation_shifts::pre32;
            pos += 4;
        }
        m_decimator2.myDecimate(xreal[0], yimag[0], &xreal[1], &yimag[1]);
        m_decimator2.myDecimate(xreal[2], yimag[2], &xreal[3], &yimag[3]);
        m_decimator2.myDecimate(xreal[4], yimag[4], &xreal[5], &yimag[5]);
        m_decimator2.myDecimate(xreal[6], yimag[6], &xreal[7], &yimag[7]);
        m_decimator4.myDecimate(xreal[1], yimag[1], &xreal[3], &yimag[3]);
        m_decimator4.myDecimate(xreal[5], yimag[5], &xreal[7], &yimag[7]);
        m_decimator8.myDecimate(xreal[3], yimag[3], &xreal[7], &yimag[7]);
        (**it).setReal(xreal[7] >> decimation_shifts::post32);
        (**it).setImag(yimag[7] >> decimation_shifts::post32);
        ++(*it);
    }
}
template
void Decimators::decimate64_inf(SampleVector::iterator* it, const T* buf, qint32 len)
{
	qint32 xreal[16], yimag[16];
	for (int pos = 0; pos < len - 127; )
	{
		for (int i = 0; i < 16; i++)
		{
			xreal[i] = (buf[pos+0] - buf[pos+3] + buf[pos+7] - buf[pos+4]) << decimation_shifts::pre64;
			yimag[i] = (buf[pos+1] - buf[pos+5] + buf[pos+2] - buf[pos+6]) << decimation_shifts::pre64;
			pos += 8;
		}
		m_decimator2.myDecimate(xreal[0], yimag[0], &xreal[1], &yimag[1]);
		m_decimator2.myDecimate(xreal[2], yimag[2], &xreal[3], &yimag[3]);
		m_decimator2.myDecimate(xreal[4], yimag[4], &xreal[5], &yimag[5]);
		m_decimator2.myDecimate(xreal[6], yimag[6], &xreal[7], &yimag[7]);
		m_decimator2.myDecimate(xreal[8], yimag[8], &xreal[9], &yimag[9]);
		m_decimator2.myDecimate(xreal[10], yimag[10], &xreal[11], &yimag[11]);
		m_decimator2.myDecimate(xreal[12], yimag[12], &xreal[13], &yimag[13]);
		m_decimator2.myDecimate(xreal[14], yimag[14], &xreal[15], &yimag[15]);
		m_decimator4.myDecimate(xreal[1], yimag[1], &xreal[3], &yimag[3]);
		m_decimator4.myDecimate(xreal[5], yimag[5], &xreal[7], &yimag[7]);
		m_decimator4.myDecimate(xreal[9], yimag[9], &xreal[11], &yimag[11]);
		m_decimator4.myDecimate(xreal[13], yimag[13], &xreal[15], &yimag[15]);
		m_decimator8.myDecimate(xreal[3], yimag[3], &xreal[7], &yimag[7]);
		m_decimator8.myDecimate(xreal[11], yimag[11], &xreal[15], &yimag[15]);
		m_decimator16.myDecimate(xreal[7], yimag[7], &xreal[15], &yimag[15]);
		(**it).setReal(xreal[15] >> decimation_shifts::post64);
		(**it).setImag(yimag[15] >> decimation_shifts::post64);
		++(*it);
	}
}
template
void Decimators::decimate64_inf(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len)
{
    qint32 xreal[16], yimag[16];
    for (int pos = 0; pos < len - 63; )
    {
        for (int i = 0; i < 16; i++)
        {
            xreal[i] = (bufI[pos+0] - bufQ[pos+1] + bufQ[pos+3] - bufI[pos+2]) << decimation_shifts::pre64;
            yimag[i] = (bufQ[pos+0] - bufQ[pos+2] + bufI[pos+1] - bufI[pos+3]) << decimation_shifts::pre64;
            pos += 4;
        }
        m_decimator2.myDecimate(xreal[0], yimag[0], &xreal[1], &yimag[1]);
        m_decimator2.myDecimate(xreal[2], yimag[2], &xreal[3], &yimag[3]);
        m_decimator2.myDecimate(xreal[4], yimag[4], &xreal[5], &yimag[5]);
        m_decimator2.myDecimate(xreal[6], yimag[6], &xreal[7], &yimag[7]);
        m_decimator2.myDecimate(xreal[8], yimag[8], &xreal[9], &yimag[9]);
        m_decimator2.myDecimate(xreal[10], yimag[10], &xreal[11], &yimag[11]);
        m_decimator2.myDecimate(xreal[12], yimag[12], &xreal[13], &yimag[13]);
        m_decimator2.myDecimate(xreal[14], yimag[14], &xreal[15], &yimag[15]);
        m_decimator4.myDecimate(xreal[1], yimag[1], &xreal[3], &yimag[3]);
        m_decimator4.myDecimate(xreal[5], yimag[5], &xreal[7], &yimag[7]);
        m_decimator4.myDecimate(xreal[9], yimag[9], &xreal[11], &yimag[11]);
        m_decimator4.myDecimate(xreal[13], yimag[13], &xreal[15], &yimag[15]);
        m_decimator8.myDecimate(xreal[3], yimag[3], &xreal[7], &yimag[7]);
        m_decimator8.myDecimate(xreal[11], yimag[11], &xreal[15], &yimag[15]);
        m_decimator16.myDecimate(xreal[7], yimag[7], &xreal[15], &yimag[15]);
        (**it).setReal(xreal[15] >> decimation_shifts::post64);
        (**it).setImag(yimag[15] >> decimation_shifts::post64);
        ++(*it);
    }
}
template
void Decimators::decimate64_sup(SampleVector::iterator* it, const T* buf, qint32 len)
{
	qint32 xreal[16], yimag[16];
	for (int pos = 0; pos < len - 127; )
	{
		for (int i = 0; i < 16; i++)
		{
			xreal[i] = (buf[pos+1] - buf[pos+2] - buf[pos+5] + buf[pos+6]) << decimation_shifts::pre32;
			yimag[i] = (buf[pos+4] + buf[pos+7] - buf[pos+0] - buf[pos+3]) << decimation_shifts::pre32;
			pos += 8;
		}
		m_decimator2.myDecimate(xreal[0], yimag[0], &xreal[1], &yimag[1]);
		m_decimator2.myDecimate(xreal[2], yimag[2], &xreal[3], &yimag[3]);
		m_decimator2.myDecimate(xreal[4], yimag[4], &xreal[5], &yimag[5]);
		m_decimator2.myDecimate(xreal[6], yimag[6], &xreal[7], &yimag[7]);
		m_decimator2.myDecimate(xreal[8], yimag[8], &xreal[9], &yimag[9]);
		m_decimator2.myDecimate(xreal[10], yimag[10], &xreal[11], &yimag[11]);
		m_decimator2.myDecimate(xreal[12], yimag[12], &xreal[13], &yimag[13]);
		m_decimator2.myDecimate(xreal[14], yimag[14], &xreal[15], &yimag[15]);
		m_decimator4.myDecimate(xreal[1], yimag[1], &xreal[3], &yimag[3]);
		m_decimator4.myDecimate(xreal[5], yimag[5], &xreal[7], &yimag[7]);
		m_decimator4.myDecimate(xreal[9], yimag[9], &xreal[11], &yimag[11]);
		m_decimator4.myDecimate(xreal[13], yimag[13], &xreal[15], &yimag[15]);
		m_decimator8.myDecimate(xreal[3], yimag[3], &xreal[7], &yimag[7]);
		m_decimator8.myDecimate(xreal[11], yimag[11], &xreal[15], &yimag[15]);
		m_decimator16.myDecimate(xreal[7], yimag[7], &xreal[15], &yimag[15]);
		(**it).setReal(xreal[15] >> decimation_shifts::post64);
		(**it).setImag(yimag[15] >> decimation_shifts::post64);
		++(*it);
	}
}
template
void Decimators::decimate64_sup(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len)
{
    qint32 xreal[16], yimag[16];
    for (int pos = 0; pos < len - 63; )
    {
        for (int i = 0; i < 16; i++)
        {
            xreal[i] = (bufQ[pos+0] - bufI[pos+1] - bufQ[pos+2] + bufI[pos+3]) << decimation_shifts::pre32;
            yimag[i] = (bufI[pos+2] + bufQ[pos+3] - bufI[pos+0] - bufQ[pos+1]) << decimation_shifts::pre32;
            pos += 4;
        }
        m_decimator2.myDecimate(xreal[0], yimag[0], &xreal[1], &yimag[1]);
        m_decimator2.myDecimate(xreal[2], yimag[2], &xreal[3], &yimag[3]);
        m_decimator2.myDecimate(xreal[4], yimag[4], &xreal[5], &yimag[5]);
        m_decimator2.myDecimate(xreal[6], yimag[6], &xreal[7], &yimag[7]);
        m_decimator2.myDecimate(xreal[8], yimag[8], &xreal[9], &yimag[9]);
        m_decimator2.myDecimate(xreal[10], yimag[10], &xreal[11], &yimag[11]);
        m_decimator2.myDecimate(xreal[12], yimag[12], &xreal[13], &yimag[13]);
        m_decimator2.myDecimate(xreal[14], yimag[14], &xreal[15], &yimag[15]);
        m_decimator4.myDecimate(xreal[1], yimag[1], &xreal[3], &yimag[3]);
        m_decimator4.myDecimate(xreal[5], yimag[5], &xreal[7], &yimag[7]);
        m_decimator4.myDecimate(xreal[9], yimag[9], &xreal[11], &yimag[11]);
        m_decimator4.myDecimate(xreal[13], yimag[13], &xreal[15], &yimag[15]);
        m_decimator8.myDecimate(xreal[3], yimag[3], &xreal[7], &yimag[7]);
        m_decimator8.myDecimate(xreal[11], yimag[11], &xreal[15], &yimag[15]);
        m_decimator16.myDecimate(xreal[7], yimag[7], &xreal[15], &yimag[15]);
        (**it).setReal(xreal[15] >> decimation_shifts::post64);
        (**it).setImag(yimag[15] >> decimation_shifts::post64);
        ++(*it);
    }
}
template
void Decimators::decimate2_cen(SampleVector::iterator* it, const T* buf, qint32 len)
{
	qint32 intbuf[2];
	for (int pos = 0; pos < len - 3; pos += 4)
	{
		intbuf[0]  = buf[pos+2] << decimation_shifts::pre2;
		intbuf[1]  = buf[pos+3] << decimation_shifts::pre2;
		m_decimator2.myDecimate(
				buf[pos+0] << decimation_shifts::pre2,
				buf[pos+1] << decimation_shifts::pre2,
				&intbuf[0],
				&intbuf[1]);
		(**it).setReal(intbuf[0] >> decimation_shifts::post2);
		(**it).setImag(intbuf[1] >> decimation_shifts::post2);
		++(*it);
	}
}
template
void Decimators::decimate2_cen(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len)
{
    qint32 intbuf[2];
    for (int pos = 0; pos < len - 1; pos += 2)
    {
        intbuf[0]  = bufI[pos+1] << decimation_shifts::pre2;
        intbuf[1]  = bufQ[pos+1] << decimation_shifts::pre2;
        m_decimator2.myDecimate(
                bufI[pos+0] << decimation_shifts::pre2,
                bufQ[pos+0] << decimation_shifts::pre2,
                &intbuf[0],
                &intbuf[1]);
        (**it).setReal(intbuf[0] >> decimation_shifts::post2);
        (**it).setImag(intbuf[1] >> decimation_shifts::post2);
        ++(*it);
    }
}
template
void Decimators::decimate4_cen(SampleVector::iterator* it, const T* buf, qint32 len)
{
	qint32 intbuf[4];
	for (int pos = 0; pos < len - 7; pos += 8)
	{
		intbuf[0]  = buf[pos+2] << decimation_shifts::pre4;
		intbuf[1]  = buf[pos+3] << decimation_shifts::pre4;
		intbuf[2]  = buf[pos+6] << decimation_shifts::pre4;
		intbuf[3]  = buf[pos+7] << decimation_shifts::pre4;
		m_decimator2.myDecimate(
				buf[pos+0] << decimation_shifts::pre4,
				buf[pos+1] << decimation_shifts::pre4,
				&intbuf[0],
				&intbuf[1]);
		m_decimator2.myDecimate(
				buf[pos+4] << decimation_shifts::pre4,
				buf[pos+5] << decimation_shifts::pre4,
				&intbuf[2],
				&intbuf[3]);
		m_decimator4.myDecimate(
				intbuf[0],
				intbuf[1],
				&intbuf[2],
				&intbuf[3]);
		(**it).setReal(intbuf[2] >> decimation_shifts::post4);
		(**it).setImag(intbuf[3] >> decimation_shifts::post4);
		++(*it);
	}
}
template
void Decimators::decimate4_cen(SampleVector::iterator* it, const T* bufI, const T* bufQ, qint32 len)
{
    qint32 intbuf[4];
    for (int pos = 0; pos < len - 3; pos += 4)
    {
        intbuf[0]  = bufI[pos+1] << decimation_shifts::pre4;
        intbuf[1]  = bufQ[pos+1] << decimation_shifts::pre4;
        intbuf[2]  = bufI[pos+3] << decimation_shifts::pre4;
        intbuf[3]  = bufQ[pos+3] << decimation_shifts::pre4;
        m_decimator2.myDecimate(
                bufI[pos+0] << decimation_shifts::pre4,
                bufQ[pos+0] << decimation_shifts::pre4,
                &intbuf[0],
                &intbuf[1]);
        m_decimator2.myDecimate(
                bufI[pos+2] << decimation_shifts::pre4,
                bufQ[pos+2] << decimation_shifts::pre4,
                &intbuf[2],
                &intbuf[3]);
        m_decimator4.myDecimate(
                intbuf[0],
                intbuf[1],
                &intbuf[2],
                &intbuf[3]);
        (**it).setReal(intbuf[2] >> decimation_shifts::post4);
        (**it).setImag(intbuf[3] >> decimation_shifts::post4);
        ++(*it);
    }
}
template
void Decimators::decimate8_cen(SampleVector::iterator* it, const T* buf, qint32 len)
{
	qint32 intbuf[8];
	for (int pos = 0; pos < len - 15; pos += 16)
	{
		intbuf[0]  = buf[pos+2] << decimation_shifts::pre8;
		intbuf[1]  = buf[pos+3] << decimation_shifts::pre8;
		intbuf[2]  = buf[pos+6] << decimation_shifts