Add function to extract analysed harmonic from given pcm

Added dtmf analysation example

7 files changed, 169 insertions, 43 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7c89989..bd3c530 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -42,6 +42,15 @@ target_include_directories(
 )
 target_link_libraries(main gha m)
 
+add_executable(dtmf test/dtmf.c)
+target_include_directories(
+    dtmf
+    PRIVATE
+    .
+)
+target_link_libraries(dtmf gha m)
+
+
 enable_testing()
 add_test(gha_test_simple_1000_0_a main ../test/data/1000hz_0.85.pcm 0 1024 0.14247585 0.0000 0.850000)
 add_test(gha_test_simple_1000_0_b main ../test/data/1000hz_0.85.pcm 0 1000 0.14247585 0.0000 0.850000)
@@ -56,3 +65,5 @@ add_test(gha_test_simple_20000_0_a main ../test/data/20000hz_0.85.pcm 0 1024 2.8
 add_test(gha_test_simple_20000_0_b main ../test/data/20000hz_0.85.pcm 0 500 2.8495171 0.0000 0.850000)
 add_test(gha_test_simple_20000_0_c main ../test/data/20000hz_0.85.pcm 0 128 2.8495171 0.0000 0.850000)
 add_test(gha_test_simple_20000_0_d main ../test/data/20000hz_0.85.pcm 0 64 2.8495171 0.0000 0.850000)
+
+add_test(gha_test_dtmf_1 dtmf ../test/data/dtmf.pcm 32 256 0.547416 0.201057 0.949511 0.200154)
diff --git a/include/libgha.h b/include/libgha.h
index 3c5d123..6ebb33a 100644
--- a/include/libgha.h
+++ b/include/libgha.h
@@ -21,4 +21,7 @@ void gha_free_ctx(gha_ctx* ctx);
 // the result will be writen in to given gha_ingo structure
 void gha_analyze_one(const FLOAT* pcm, struct gha_info* info, gha_ctx* ctx);
 
+// Performs one GHA step and extract analysed harmonic from given PCM signal
+void gha_extract_one(FLOAT* pcm, struct gha_info* info, gha_ctx* ctx);
+
 #endif
diff --git a/src/gha.c b/src/gha.c
index e2c0686..b4baca8 100644
--- a/src/gha.c
+++ b/src/gha.c
@@ -202,3 +202,14 @@ void gha_analyze_one(const FLOAT* pcm, struct gha_info* info, gha_ctx* ctx)
 	gha_generate_sine(ctx->tmp_buf, ctx->size, info->freq, info->phase);
 	gha_estimate_magnitude(pcm, ctx->tmp_buf, ctx->size, info);
 }
+
+void gha_extract_one(FLOAT* pcm, struct gha_info* info, gha_ctx* ctx)
+{
+	int i;
+	FLOAT magnitude;
+	gha_analyze_one(pcm, info, ctx);
+	magnitude = info->magnitude;
+
+	for (i = 0; i < ctx->size; i++)
+		pcm[i] -= ctx->tmp_buf[i] * magnitude;
+}
diff --git a/test/common.h b/test/common.h
new file mode 100644
index 0000000..2823df3
--- /dev/null
+++ b/test/common.h
@@ -0,0 +1,64 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+static inline int compare_phase(float a, float b, float delta) {
+	if (abs(a - b) < delta)
+		return 0;
+	a = fmod(a + M_PI, 2 * M_PI);
+	b = fmod(b + M_PI, 2 * M_PI);
+//	fprintf(stderr, "%f %f  %f\n", a, b, delta);
+	if (abs(a - b) < delta)
+		return 0;
+	return -1;
+}
+
+static inline int load_file(const char* name, size_t len, size_t offset, size_t bits, float* buf)
+{
+	union {
+	    char b[4];
+	    int32_t i;
+	} sample;
+	int bytes_per_sample = 0;
+
+	size_t file_size, i;
+
+	FILE* file = fopen(name, "r");
+
+	if (!file)
+		return -1;
+
+	switch (bits) {
+		case 8:
+			bytes_per_sample = 1;
+			break;
+		case 24:
+			bytes_per_sample = 3;
+			break;
+		default:
+			fprintf(stderr, "Unsupported sample format\n");
+			return -1;
+	}
+
+	fseek(file, 0, SEEK_END);
+	file_size = ftell(file);
+	rewind(file);
+
+	if (file_size < (offset + len) * bytes_per_sample)
+		return -1;
+
+	if (fseek(file, offset * bytes_per_sample, SEEK_SET)) {
+		return -1;
+	}
+
+	memset(sample.b, 0, sizeof(sample.b)/sizeof(sample.b[0]));
+
+	i = 0;
+	char* p = sample.b + 4 - bytes_per_sample;
+	while ((fread(p, 1, bytes_per_sample, file) == bytes_per_sample) && i < len) {
+		buf[i] = (double)(sample.i) / (double)(1u<<31);
+                i++;
+	}
+	return 0;
+}
diff --git a/test/data/dtmf.pcm b/test/data/dtmf.pcm
new file mode 100644
index 0000000..68af8b7
--- /dev/null
+++ b/test/data/dtmf.pcm
diff --git a/test/dtmf.c b/test/dtmf.c
new file mode 100644
index 0000000..c98164b
--- /dev/null
+++ b/test/dtmf.c
@@ -0,0 +1,74 @@
+#include <include/libgha.h>
+
+#include "common.h"
+
+void usage(const char* selfname) {
+	fprintf(stderr, "GHA dtmf example utility, usage: %s <FILE> <OFFSET> <LEN> [<EXPECTED_FREQ_LOW> <EXPECTED_MAGNITUDE_LOW>, <EXPECTED_FREQ_HIGH>, <EXPECTED_MAGNITUDE_HIGH>]\n", selfname);
+	fprintf(stderr, "FILE - input pcm file (raw pcm, mono, 8 bit, 8000 kHz\n");
+	fprintf(stderr, "OFFSET - offset in input file, samples\n");
+	fprintf(stderr, "LEN - len to process, samples\n");
+	fprintf(stderr, "Following options are optional used by test framework:\n");
+	fprintf(stderr, "EXPECTED_FREQ_(LOW|HIGH) - expected angular frequency\n");
+	fprintf(stderr, "EXPECTED_MAGNITUDE_(LOW|HIGH) - expected magnitude (0 - 1)\n");
+}
+
+int main(int argc, char** argv) {
+	if (argc != 4 && argc != 8) {
+		usage(argv[0]);
+		return 1;
+	}
+
+	long long len = atoll(argv[3]);
+
+	gha_ctx* ctx;
+
+	float* buf = malloc(len * sizeof(float));
+	if (!buf)
+		abort();
+
+	if (load_file(argv[1], len, atoi(argv[2]), 8, buf)) {
+		fprintf(stderr, "Unable to load input file\n");
+		free(buf);
+		return 1;
+	}
+
+
+	ctx = gha_create_ctx(len);
+	if (!ctx) {
+		fprintf(stderr, "Unable to create gha ctx\n");
+		free(buf);
+		return 1;
+	}
+
+	struct gha_info res[2];
+	gha_extract_one(buf, &res[0], ctx);
+	gha_extract_one(buf, &res[1], ctx);
+
+	if (res[0].freq > res[1].freq) {
+		struct gha_info tmp;
+		memcpy(&tmp, &res[0], sizeof(struct gha_info));
+		memcpy(&res[0], &res[1], sizeof(struct gha_info));
+		memcpy(&res[1], &tmp, sizeof(struct gha_info));
+	}
+
+	gha_free_ctx(ctx);
+	free(buf);
+
+	if (argc == 8) {
+		double freq[2];
+		double magn[2];
+		freq[0] = atof(argv[4]);
+		magn[0] = atof(argv[5]);
+		freq[1] = atof(argv[6]);
+		magn[1] = atof(argv[7]);
+		if (fabs(freq[0] - res[0].freq) > 0.001 || fabs(magn[0] - res[0].magnitude) > 0.001 ||
+			fabs(freq[1] - res[1].freq) > 0.001 || fabs(magn[1] - res[1].magnitude) > 0.001)
+			return 1;
+		return 0;
+
+
+	} else {
+	    fprintf(stderr, "dtmf result: low freq: %f, magn: %f, high freq: %f, magn: %f\n",
+			res[0].freq, res[0].magnitude, res[1].freq, res[1].magnitude);
+	}
+}
diff --git a/test/main.c b/test/main.c
index 46475d7..703cbf5 100644
--- a/test/main.c
+++ b/test/main.c
@@ -1,12 +1,9 @@
 #include <include/libgha.h>
 
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <math.h>
+#include "common.h"
 
 void usage(const char* selfname) {
-	fprintf(stderr, "GHA utility, usage: %s <FILE> <OFFSET> <LEN> [<EXPECTED_FREQ> <EXPECTED_PHASE> <EXPECTED_MAGNITUDE>]\n", selfname);
+	fprintf(stderr, "GHA simple utility, usage: %s <FILE> <OFFSET> <LEN> [<EXPECTED_FREQ> <EXPECTED_PHASE> <EXPECTED_MAGNITUDE>]\n", selfname);
 	fprintf(stderr, "FILE - input pcm file (raw pcm, mono, 24 bit, 44100 kHz\n");
 	fprintf(stderr, "OFFSET - offset in input file, samples\n");
 	fprintf(stderr, "LEN - len to process, samples\n");
@@ -16,61 +13,27 @@ void usage(const char* selfname) {
 	fprintf(stderr, "EXPECTED_MAGNITUDE - expected magnitude (0 - 1)\n");
 }
 
-static int compare_phase(float a, float b, float delta) {
-	if (abs(a - b) < delta)
-		return 0;
-	a = fmod(a + M_PI, 2 * M_PI);
-	b = fmod(b + M_PI, 2 * M_PI);
-//	fprintf(stderr, "%f %f  %f\n", a, b, delta);
-	if (abs(a - b) < delta)
-		return 0;
-	return 1;
-}
 
 int main(int argc, char** argv) {
 	if (argc != 4 && argc != 7) {
 		usage(argv[0]);
 		return 1;
 	}
-	union {
-	    char b[4];
-	    int32_t i;
-	} sample;
-
-	long long i = 0;
 
-	const char* input_file = argv[1];
-	long long offset = atoll(argv[2]);
 	long long len = atoll(argv[3]);
-	FILE* file = fopen(input_file, "r");
-	long long file_size;
 
 	gha_ctx* ctx;
 
-	fseek(file, 0 , SEEK_END);
-	file_size = ftell(file);
-	rewind(file);
-
-	if (file_size < (offset + len) * 3) {
-		return 1;
-	}
-
-	if (fseek(file, offset * 3, SEEK_SET)) {
-		return 1;
-	}
-
 	float* buf = malloc(len * sizeof(float));
 	if (!buf)
 		abort();
 
-
-	memset(sample.b, 0, sizeof(sample.b)/sizeof(sample.b[0]));
-	// sample.b + 1 - sign bit at right place
-	while ((fread(sample.b + 1, 1, 3, file) == 3) && i < len) {
-		buf[i] = (double)(sample.i) / (double)(1u<<31);
-                i++;
+	if (load_file(argv[1], len, atoi(argv[2]), 24, buf)) {
+		free(buf);
+		return 1;
 	}
 
+
 	ctx = gha_create_ctx(len);
 	if (!ctx) {
 		fprintf(stderr, "Unable to create gha ctx\n");