dft_demo.c

#include <complex.h>
#include <stdint.h>
#include <assert.h>
#include <stddef.h>
#include <stdlib.h>
#include <limits.h>
#include <tgmath.h>
#include <stdio.h>
#include <stdbool.h>

#define clz(x) \
    _Generic((x), \
		unsigned: __builtin_clz, \
		unsigned long: __builtin_clzl, \
		unsigned long long: __builtin_clzll \
	)(x)
#define ilog2(x) (sizeof((x))*CHAR_BIT - 1 - clz((x)))

#if !(defined(SIZE_WIDTH)) && defined(__SIZE_WIDTH__)
#define SIZE_WIDTH __SIZE_WIDTH__
#endif

#if SIZE_WIDTH == 64
#define bitreverse_size __builtin_bitreverse64
#elif SIZE_WIDTH == 32
#define br_size __builtin_bitreverse32
#elif SIZE_WIDTH == 16
#define br_size __builtin_bitreverse16
#elif SIZE_WIDTH == 8
#define br_size __builtin_bitreverse8
#else
#error "Unsupported SIZE_WIDTH"
#endif

typedef _Complex double cf64;
typedef int32_t i32;
typedef uint32_t u32;

typedef struct {
	float L, C;
} lc_lowpass;

static inline cf64 div2(cf64 x, i32 n) {
	double a = creal(x);
	double b = cimag(x);

	a = ldexp(a, n);
	b = ldexp(b, n);

	return a + b * I;
}

void fft_fast(cf64 const *input, cf64 *output, size_t n, bool ifft) {
	assert(__builtin_popcount(n) == 1);
	assert(output != NULL);
	assert(input != NULL);

	u32 depth = ilog2(n);

	for (size_t i = 0; i < n; i++) {
		size_t flipped = bitreverse_size(i) >> (SIZE_WIDTH - depth);
		assert(flipped < n);

		output[i] = input[flipped];
	}

	for (size_t block_sz = 2; block_sz <= n; block_sz <<= 1) {
		cf64 w = cexp(-2j * M_PI / block_sz * (ifft ? 1 : -1));

		cf64 twiddle = 1;
		for (size_t k = 0; k < block_sz / 2; k++) {
			for (size_t block = 0; block < n; block += block_sz) {
				cf64 *odd = output + block + block_sz / 2;
				odd[k] *= twiddle;
			}

			twiddle *= w;
		}

		for (size_t block = 0; block < n; block += block_sz) {
			cf64 *even = output + block;
			cf64 *odd = output + block + block_sz / 2;

			for (size_t k = 0; k < block_sz / 2; k++) {
				cf64 even_k = even[k];

				even[k] = even_k + odd[k];
				odd[k] = even_k - odd[k];
			}
		}
	}

	if (ifft) {
		for (size_t i = 0; i < n; i++) {
			output[i] = div2(output[i], -((i32) depth));
		}
	}
}

void estimate_transfer_function(lc_lowpass const *lc_config, cf64 const *voltage_in, cf64 const *current_out, double Ts, cf64 *transfer_function, size_t n) {
	cf64 *Vin = calloc(sizeof(cf64), n);
	cf64 *Iout = calloc(sizeof(cf64), n);

	assert(Vin != NULL);
	assert(Iout != NULL);

	fft_fast(voltage_in, Vin, n, false);
	fft_fast(current_out, Iout, n, false);

	cf64 L = lc_config->L;
	cf64 C = lc_config->C;

	for (size_t k = 0; k < n; k++) {
		cf64 w = 2 * M_PI * k / (n * Ts);
		cf64 jw = 1j * w;

		if (cabs(Iout[k]) < 1e-4 || cabs(Vin[k]) < 1e-4) { // scuffed but probably fine
			transfer_function[k] = 0;
			continue;
		}

		cf64 Z_L = jw * L;
		cf64 Z_C = 1 / (jw * C);

		if (w == 0) {
			Z_L = 0;
			Z_C = 1000000000;
		}

		transfer_function[k] = Z_C*(-Iout[k]*Z_L + Vin[k])/(Vin[k]*(Z_C + Z_L));
	}

	free(Vin);
	free(Iout);
}

void simulate_parallel_LC_load(
		double L,
		double C,
		double dt,
		size_t N,
		const cf64 *v_in,
		const cf64 *Z_load,
		cf64 *i_load,
		cf64 *v_out
) {
	cf64 iL = 0;
	cf64 vout = 0;

	if (N > 0) {
		i_load[0] = vout / Z_load[0];
		v_out[0] = vout;
	}

	for (size_t k = 0; k + 1 < N; k++) {
		i_load[k] = vout / Z_load[k];

		cf64 diL_dt = (v_in[k] - vout) / L;
		cf64 dvout_dt = (iL - i_load[k]) / C;

		iL += diL_dt * dt;
		vout += dvout_dt * dt;

		v_out[k + 1] = vout;
		i_load[k + 1] = vout / Z_load[k + 1];
	}
}

#define TEST_SIZE 256
#define T (1./256)

static void test_estimate_load_impedance() {
	double const w = 2;
	lc_lowpass lc_config = {
			.L = 0.1,
			.C = 0.1
	};

	cf64 v_in[TEST_SIZE];
	cf64 Z_load[TEST_SIZE];

	for (size_t i = 0; i < TEST_SIZE; i++) {
		v_in[i] = 24 * sin(w * i * T);
		Z_load[i] = 1000;
	}

	cf64 i_out[TEST_SIZE];
	cf64 v_out[TEST_SIZE];

	simulate_parallel_LC_load(lc_config.L, lc_config.C, T, TEST_SIZE, v_in, Z_load, i_out, v_out);

	cf64 Z_calculated[TEST_SIZE];
	estimate_transfer_function(&lc_config, v_in, i_out, T, Z_calculated, TEST_SIZE);

	cf64 V_in[TEST_SIZE];
	cf64 V_out[TEST_SIZE];
	fft_fast(v_in, V_in, TEST_SIZE, false);
	fft_fast(v_out, V_out, TEST_SIZE, false);

	cf64 tfn[TEST_SIZE];
	for (int i = 0; i < TEST_SIZE; i++) {
		if (cabs(V_in[i]) < 1e-4 || cabs(V_out[i]) < 1e-4)
			tfn[i] = 0;
		else tfn[i] = V_out[i] / V_in[i];
	}

	puts("H_measured,H_calculated");
	for (size_t i = 0; i < TEST_SIZE; i++) {
		printf("%f, %f\n", cabs(tfn[i]), cabs(Z_calculated[i]));
	}
}

int main() {
	test_estimate_load_impedance();
	return 0;
}