power_measure.c

//
// Created by alec on 11/25/24.
//

#include <stddef.h>
#include <assert.h>
#include <limits.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>

#include "helpers.h"
#include "signals.h"

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);
	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 analytic(cf64 const *input, cf64 *output, size_t n) {
	cf64 *tmp = malloc(n * sizeof(cf64));
	fft_fast(input, tmp, n, false);

	for (int i = 0; i < n; i++) {
		if (i == 0 || i == n/2)
			tmp[i] = tmp[i];
		else if (i >= 1 && i < (n / 2))
			tmp[i] *= 2;
		else
			tmp[i] = 0;
	}

	fft_fast(tmp, output, n, true);
	free(tmp);
}

static double anglewrap(double angle) {
	while (angle > M_PI) {
		angle -= 2 * M_PI;
	}

	while (angle < -M_PI) {
		angle += 2 * M_PI;
	}

	return angle;
}

void phase_difference(cf64 const *signal1, cf64 const *signal2, size_t n, double *output) {
	cf64 *analytic1 = malloc(n * sizeof(cf64));
	cf64 *analytic2 = malloc(n * sizeof(cf64));

	assert(analytic1 != NULL);
	assert(analytic2 != NULL);

	analytic(signal1, analytic1, n);
	analytic(signal2, analytic2, n);

	for (size_t i = 0; i < n; i++) {
		output[i] = carg(analytic1[i]) - carg(analytic2[i]);
		output[i] = anglewrap(output[i]);
	}

	free(analytic1);
	free(analytic2);
}

static cf64 dot(cf64 const *a, cf64 const *b, size_t n) {
	cf64 dot = 0;

	for (size_t i = 0; i < n; i++) {
		dot += a[i] * conj(b[i]);
	}

	return dot;
}

double thd(cf64 const *sig_t, cf64 const *fundamental_t, size_t n) {
	cf64 *sig = malloc(n * sizeof(cf64));
	cf64 *xref = malloc(n * sizeof(cf64));

	assert(sig != NULL);
	assert(xref != NULL);

	fft_fast(sig_t, sig, n, false);
	fft_fast(fundamental_t, xref, n, false);

	n = n/2 + 1;

	cf64 shift = cexp(-I * carg(dot(sig, xref, n)));

	for (int i = 0; i < n; i++) {
		sig[i] *= shift;
	}

	cf64 xspec = dot(sig, xref, n);
	cf64 efund = dot(xref, xref, n);

	cf64 comp = xspec / efund;
	double harmonics_power = 0;
	double fundamental_power = 0;
	for (size_t i = 0; i < n; i++) {
		cf64 proj = comp * xref[i];
		cf64 harm = sig[i] - proj;

		harmonics_power += cabs(harm) * cabs(harm);
		fundamental_power += cabs(proj) * cabs(proj);
	}

	free(sig);
	free(xref);

	return sqrt(harmonics_power) / sqrt(fundamental_power);
}