#include "density.hh"
namespace Heck {

    namespace Constants {
        constexpr Samplerate AUDIO_SAMPLERATE = Samplerate::SAI_48KHZ;
    }

    void Density::process(const float *inSourceP, float *inDestP, u32 size)
    {
        UInt32 nSampleFrames = size;
        const Float32 *sourceP = inSourceP;
        Float32 *destP = inDestP;
        Float64 inputSample;
        Float64 drySample;
        Float64 overallscale = 1.0;
        overallscale /= 44100.0;
        overallscale *= Constants::AUDIO_SAMPLERATE;
        Float64 density = params[0];
        Float64 iirAmount = pow(params[1], 3) / overallscale;
        Float64 output = params[2];
        Float64 wet = params[3];
        Float64 dry = 1.0 - wet;
        Float64 bridgerectifier;
        Float64 out = fabs(density);
        density = density * fabs(density);
        Float64 count;

        while (nSampleFrames-- > 0) {
            inputSample = *sourceP;

            if (fabs(inputSample) < 1.18e-23) {
                inputSample = fpd * 1.18e-17;
            }

            drySample = inputSample;

            if (fpFlip) {
                iirSampleA = (iirSampleA * (1 - iirAmount)) + (inputSample * iirAmount);
                inputSample -= iirSampleA;
            } else {
                iirSampleB = (iirSampleB * (1 - iirAmount)) + (inputSample * iirAmount);
                inputSample -= iirSampleB;
            }

            //highpass section
            count = density;
            while (count > 1.0) {
                bridgerectifier = fabs(inputSample) * 1.57079633;
                if (bridgerectifier > 1.57079633) {
                    bridgerectifier = 1.57079633;
                }
                //max value for sine function
                bridgerectifier = sin(bridgerectifier);
                if (inputSample > 0.0) {
                    inputSample = bridgerectifier;
                } else {
                    inputSample = -bridgerectifier;
                }
                count = count - 1.0;
            }

            //we have now accounted for any really high density settings.
            while (out > 1.0) {
                out = out - 1.0;
            }
            bridgerectifier = fabs(inputSample) * 1.57079633;
            if (bridgerectifier > 1.57079633) {
                bridgerectifier = 1.57079633;
            }
            //max value for sine function
            if (density > 0) {
                bridgerectifier = sin(bridgerectifier);
            } else {
                bridgerectifier = 1 - cos(bridgerectifier);
            }
            //produce either boosted or starved version
            if (inputSample > 0) {
                inputSample = (inputSample * (1 - out)) + (bridgerectifier * out);
            } else {
                inputSample = (inputSample * (1 - out)) - (bridgerectifier * out);
            }
            //blend according to density control

            if (output < 1.0) {
                inputSample *= output;
            }

            if (wet < 1.0) {
                inputSample = (drySample * dry) + (inputSample * wet);
            }

            //nice little output stage template: if we have another scale of floating point
            //number, we really don't want to meaninglessly multiply that by 1.0.
            fpFlip = !fpFlip;

            //begin 32 bit floating point dither

            if constexpr (DO_DITHER) {
                int expon;
                frexpf((float)inputSample, &expon);
                fpd ^= fpd << 13;
                fpd ^= fpd >> 17;
                fpd ^= fpd << 5;
                inputSample += ((double(fpd) - uint32_t(0x7fffffff)) * 5.5e-36l * pow(2, expon + 62));
                //end 32 bit floating point dither
            }
            *destP = inputSample;

            //that simple.
            sourceP++;
            destP++;
        }
    }
} // namespace Heck