package funkin.audiovis.dsp;

import funkin.audiovis.dsp.Complex;

using funkin.audiovis.dsp.OffsetArray;
using funkin.audiovis.dsp.Signal;

// these are only used for testing, down in FFT.main()

/**
	Fast/Finite Fourier Transforms.
**/
class FFT
{
	/**
		Computes the Discrete Fourier Transform (DFT) of a `Complex` sequence.

		If the input has N data points (N should be a power of 2 or padding will be added)
		from a signal sampled at intervals of 1/Fs, the result will be a sequence of N
		samples from the Discrete-Time Fourier Transform (DTFT) - which is Fs-periodic -
		with a spacing of Fs/N Hz between them and a scaling factor of Fs.
	**/
	public static function fft(input:Array<Complex>):Array<Complex>
		return do_fft(input, false);

	/**
		Like `fft`, but for a real (Float) sequence input.

		Since the input time signal is real, its frequency representation is
		Hermitian-symmetric so we only return the positive frequencies.
	**/
	public static function rfft(input:Array<Float>):Array<Complex>
	{
		final s = fft(input.map(Complex.fromReal));
		return s.slice(0, Std.int(s.length / 2) + 1);
	}

	/**
		Computes the Inverse DFT of a periodic input sequence.

		If the input contains N (a power of 2) DTFT samples, each spaced Fs/N Hz
		from each other, the result will consist of N data points as sampled
		from a time signal at intervals of 1/Fs with a scaling factor of 1/Fs.
	**/
	public static function ifft(input:Array<Complex>):Array<Complex>
		return do_fft(input, true);

	// Handles padding and scaling for forwards and inverse FFTs.
	private static function do_fft(input:Array<Complex>, inverse:Bool):Array<Complex>
	{
		final n = nextPow2(input.length);
		var ts = [for (i in 0...n) if (i < input.length) input[i] else Complex.zero];
		var fs = [for (_ in 0...n) Complex.zero];
		ditfft2(ts, 0, fs, 0, n, 1, inverse);
		return inverse ? fs.map(z -> z.scale(1 / n)) : fs;
		return fs;
	}

	// Radix-2 Decimation-In-Time variant of Cooley–Tukey's FFT, recursive.
	private static function ditfft2(time:Array<Complex>, t:Int, freq:Array<Complex>, f:Int, n:Int, step:Int, inverse:Bool):Void
	{
		if (n == 1)
		{
			freq[f] = time[t].copy();
		}
		else
		{
			final halfLen = Std.int(n / 2);
			ditfft2(time, t, freq, f, halfLen, step * 2, inverse);
			ditfft2(time, t + step, freq, f + halfLen, halfLen, step * 2, inverse);
			for (k in 0...halfLen)
			{
				final twiddle = Complex.exp((inverse ? 1 : -1) * 2 * Math.PI * k / n);
				final even = freq[f + k].copy();
				final odd = freq[f + k + halfLen].copy();
				freq[f + k] = even + twiddle * odd;
				freq[f + k + halfLen] = even - twiddle * odd;
			}
		}
	}

	// Naive O(n^2) DFT, used for testing purposes.
	private static function dft(ts:Array<Complex>, ?inverse:Bool):Array<Complex>
	{
		if (inverse == null)
			inverse = false;
		final n = ts.length;
		var fs = new Array<Complex>();
		fs.resize(n);
		for (f in 0...n)
		{
			var sum = Complex.zero;
			for (t in 0...n)
			{
				sum += ts[t] * Complex.exp((inverse ? 1 : -1) * 2 * Math.PI * f * t / n);
			}
			fs[f] = inverse ? sum.scale(1 / n) : sum;
		}
		return fs;
	}

	/**
		Finds the power of 2 that is equal to or greater than the given natural.
	**/
	static function nextPow2(x:Int):Int
	{
		if (x < 2)
			return 1;
		else if ((x & (x - 1)) == 0)
			return x;
		var pow = 2;
		x--;
		while ((x >>= 1) != 0)
			pow <<= 1;
		return pow;
	}

	// testing, but also acts like an example
	static function main()
	{
		// sampling and buffer parameters
		final Fs = 44100.0;
		final N = 512;
		final halfN = Std.int(N / 2);

		// build a time signal as a sum of sinusoids
		final freqs = [5919.911];
		final ts = [for (n in 0...N) freqs.map(f -> Math.sin(2 * Math.PI * f * n / Fs)).sum()];

		// get positive spectrum and use its symmetry to reconstruct negative domain
		final fs_pos = rfft(ts);
		final fs_fft = new OffsetArray([for (k in -(halfN - 1)...0) fs_pos[-k].conj()].concat(fs_pos), -(halfN - 1));

		// double-check with naive DFT
		final fs_dft = new OffsetArray(dft(ts.map(Complex.fromReal)).circShift(halfN - 1), -(halfN - 1));
		final fs_err = [for (k in -(halfN - 1)...halfN) fs_fft[k] - fs_dft[k]];
		final max_fs_err = fs_err.map(z -> z.magnitude).max();
		if (max_fs_err > 1e-6)
			haxe.Log.trace('FT Error: ${max_fs_err}', null);
		// else for (k => s in fs_fft) haxe.Log.trace('${k * Fs / N};${s.scale(1 / Fs).magnitude}', null);

		// find spectral peaks to detect signal frequencies
		final freqis = fs_fft.array.map(z -> z.magnitude)
			.findPeaks()
			.map(k -> (k - (halfN - 1)) * Fs / N)
			.filter(f -> f >= 0);
		if (freqis.length != freqs.length)
		{
			trace('Found frequencies: ${freqis}');
		}
		else
		{
			final freqs_err = [for (i in 0...freqs.length) freqis[i] - freqs[i]];
			final max_freqs_err = freqs_err.map(Math.abs).max();
			if (max_freqs_err > Fs / N)
				trace('Frequency Errors: ${freqs_err}');
		}

		// recover time signal from the frequency domain
		final ts_ifft = ifft(fs_fft.array.circShift(-(halfN - 1)).map(z -> z.scale(1 / Fs)));
		final ts_err = [for (n in 0...N) ts_ifft[n].scale(Fs).real - ts[n]];
		final max_ts_err = ts_err.map(Math.abs).max();
		if (max_ts_err > 1e-6)
			haxe.Log.trace('IFT Error: ${max_ts_err}', null);
		// else for (n in 0...ts_ifft.length) haxe.Log.trace('${n / Fs};${ts_ifft[n].scale(Fs).real}', null);
	}
}