add anyloop:clamp

fixes #7

devices/clamp.c

@@ -0,0 +1,206 @@
+#include <gsl/gsl_matrix.h>
+#include "anyloop.h"
+#include "logging.h"
+#include "clamp.h"
+#include "xalloc.h"
+
+
+int clamp_init(struct aylp_device *self)
+{
+	self->device_data = xcalloc(1, sizeof(struct aylp_clamp_data));
+	struct aylp_clamp_data *data = self->device_data;
+
+	self->close = &clamp_close;
+
+	// default to ±1
+	data->min = -1.0;
+	data->max = 1.0;
+
+	if (!self->params) {
+		log_error("No params object found.");
+		return -1;
+	}
+	json_object_object_foreach(self->params, key, val) {
+		// parse parameters
+		if (key[0] == '_') {
+			// keys starting with _ are comments
+		} else if (!strcmp(key, "type")) {
+			const char *s = json_object_get_string(val);
+			self->type_in = aylp_type_from_string(s);
+			log_trace("type = %s (0x%hhX)", s, self->type_in);
+		} else if (!strcmp(key, "min")) {
+			data->min = json_object_get_double(val);
+			log_trace("min = %d", data->min);
+		} else if (!strcmp(key, "max")) {
+			data->max = json_object_get_double(val);
+			log_trace("max = %d", data->max);
+		} else {
+			log_warn("Unknown parameter \"%s\"", key);
+		}
+	}
+	if (!self->type_in) {
+		log_error("You must provide the type parameter.");
+		return -1;
+	}
+
+	switch (self->type_in) {
+	// type-specific setup
+	#define INIT_CASE_TYPE(TYPE, type) \
+	case AYLP_T_##TYPE: \
+		self->process = &clamp_process_##type; \
+		break;
+	FOR_AYLP_CLAMP_TYPES(INIT_CASE_TYPE)
+	default:
+		log_error("Invalid input type %hhx", self->type_in);
+		return -1;
+	}
+
+	self->units_in = AYLP_U_ANY;
+	self->type_out = 0;
+	self->units_out = 0;
+	return 0;
+}
+
+
+int clamp_process_block(struct aylp_device *self, struct aylp_state *state)
+{
+	struct aylp_clamp_data *data = self->device_data;
+	if (UNLIKELY(!data->block)) {
+		// we have nowhere to put the result; let's allocate it
+		data->block = gsl_block_alloc(state->block->size);
+	} else if (UNLIKELY(data->block->size != state->block->size)) {
+		// somehow the state block changed size >:(
+		xfree_type(gsl_block, data->block);
+		data->block = gsl_block_alloc(state->block->size);
+	}
+
+	for (size_t i = 0; i < data->block->size; i++) {
+		double x = state->block->data[i];
+		if (x < data->min) x = data->min;
+		else if (data->max < x) x = data->max;
+		data->block->data[i] = x;
+	}
+
+	state->block = data->block;
+	return 0;
+}
+
+int clamp_process_vector(struct aylp_device *self, struct aylp_state *state)
+{
+	struct aylp_clamp_data *data = self->device_data;
+	if (UNLIKELY(!data->vector)) {
+		data->vector = gsl_vector_alloc(state->vector->size);
+	} else if (UNLIKELY(data->vector->size != state->vector->size)) {
+		xfree_type(gsl_vector, data->vector);
+		data->vector = gsl_vector_alloc(state->vector->size);
+	}
+
+	for (size_t i = 0; i < data->vector->size; i++) {
+		double x = state->vector->data[i * state->vector->stride];
+		if (x < data->min) x = data->min;
+		else if (data->max < x) x = data->max;
+		data->vector->data[i * data->vector->stride] = x;
+	}
+
+	state->vector = data->vector;
+	return 0;
+}
+
+int clamp_process_matrix(struct aylp_device *self, struct aylp_state *state)
+{
+	struct aylp_clamp_data *data = self->device_data;
+	gsl_matrix *src = state->matrix;
+	if (UNLIKELY(!data->matrix)) {
+		data->matrix = gsl_matrix_alloc(src->size1, src->size2);
+	} else if (UNLIKELY(data->matrix->size1 != src->size1
+	|| data->matrix->size2 != src->size2)) {
+		xfree_type(gsl_matrix, data->matrix);
+		data->matrix = gsl_matrix_alloc(src->size1, src->size2);
+	}
+	gsl_matrix *dst = data->matrix;
+
+	for (size_t i = 0; i < src->size1; i++) {
+		for (size_t j = 0; j < src->size2; j++) {
+			double x = src->data[i * src->tda + j];
+			if (x < data->min) x = data->min;
+			else if (data->max < x) x = data->max;
+			dst->data[i * dst->tda + j] = x;
+		}
+	}
+
+	state->matrix = data->matrix;
+	return 0;
+}
+
+int clamp_process_block_uchar(
+	struct aylp_device *self, struct aylp_state *state
+){
+	struct aylp_clamp_data *data = self->device_data;
+	gsl_block_uchar *src = data->block_uchar;
+	if (UNLIKELY(!data->block_uchar)) {
+		// we have nowhere to put the result; let's allocate it
+		data->block_uchar = gsl_block_uchar_alloc(src->size);
+	} else if (UNLIKELY(data->block_uchar->size != src->size)) {
+		// somehow the state block_uchar changed size >:(
+		xfree_type(gsl_block_uchar, data->block_uchar);
+		data->block_uchar = gsl_block_uchar_alloc(
+			state->block_uchar->size
+		);
+	}
+
+	unsigned char min = data->min;
+	unsigned char max = data->max;
+
+	for (size_t i = 0; i < data->block_uchar->size; i++) {
+		unsigned char x = src->data[i];
+		if (x < min) x = min;
+		else if (max < x) x = max;
+		data->block_uchar->data[i] = x;
+	}
+
+	state->block_uchar = data->block_uchar;
+	return 0;
+}
+
+int clamp_process_matrix_uchar(
+	struct aylp_device *self, struct aylp_state *state
+){
+	struct aylp_clamp_data *data = self->device_data;
+	gsl_matrix_uchar *src = state->matrix_uchar;
+	if (UNLIKELY(!data->matrix_uchar)) {
+		data->matrix_uchar = gsl_matrix_uchar_alloc(
+			src->size1, src->size2
+		);
+	} else if (UNLIKELY(data->matrix_uchar->size1 != src->size1
+	|| data->matrix_uchar->size2 != src->size2)) {
+		xfree_type(gsl_matrix_uchar, data->matrix_uchar);
+		data->matrix_uchar = gsl_matrix_uchar_alloc(
+			src->size1, src->size2
+		);
+	}
+	gsl_matrix_uchar *dst = data->matrix_uchar;
+
+	for (size_t i = 0; i < src->size1; i++) {
+		for (size_t j = 0; j < src->size2; j++) {
+			unsigned char x = src->data[i * src->tda + j];
+			if (x < data->min) x = data->min;
+			else if (data->max < x) x = data->max;
+			dst->data[i * dst->tda + j] = x;
+		}
+	}
+
+	state->matrix_uchar = data->matrix_uchar;
+	return 0;
+}
+
+
+int clamp_close(struct aylp_device *self)
+{
+	struct aylp_clamp_data *data = self->device_data;
+	#define FREE_RESULT(_, type) \
+		if (data->type) xfree_type(gsl_##type, data->type);
+	FOR_AYLP_CLAMP_TYPES(FREE_RESULT)
+	xfree(data);
+	return 0;
+}
+

devices/clamp.h

@@ -0,0 +1,39 @@
+#ifndef AYLP_DEVICES_CLAMP_H_
+#define AYLP_DEVICES_CLAMP_H_
+
+#include <gsl/gsl_matrix.h>
+#include "anyloop.h"
+
+// https://en.wikipedia.org/wiki/X_macro
+#define FOR_AYLP_CLAMP_TYPES(DO) \
+	DO(BLOCK, block) \
+	DO(VECTOR, vector) \
+	DO(MATRIX, matrix) \
+	DO(BLOCK_UCHAR, block_uchar) \
+	DO(MATRIX_UCHAR, matrix_uchar)
+
+struct aylp_clamp_data {
+	// min and max values to clamp to
+	double min;
+	double max;
+	// the place to put our result
+	#define DECLARE_RESULT(_, type) gsl_##type *type;
+	FOR_AYLP_CLAMP_TYPES(DECLARE_RESULT)
+	#undef DECLARE_RESULT
+};
+
+// initialize clamp device
+int clamp_init(struct aylp_device *self);
+
+// different process() function for each type
+#define DECLARE_PROCESS(_, type) int clamp_process_##type( \
+	struct aylp_device *self, struct aylp_state *state \
+);
+FOR_AYLP_CLAMP_TYPES(DECLARE_PROCESS)
+#undef DECLARE_PROCESS
+
+// close clamp device when loop exits
+int clamp_close(struct aylp_device *self);
+
+#endif
+

devices/device.h

@@ -2,6 +2,7 @@
 #define AYLP_DEVICES_DEVICE_H_
 
 #include "devices/center_of_mass.h"
+#include "devices/clamp.h"
 #include "devices/delay.h"
 #include "devices/file_sink.h"
 #include "devices/logger.h"
@@ -19,6 +20,7 @@ static const struct {
 	int (*init_fun)(struct aylp_device *);
 } init_map [] = {
 	{ "anyloop:center_of_mass", center_of_mass_init },
+	{ "anyloop:clamp", clamp_init },
 	{ "anyloop:delay", delay_init },
 	{ "anyloop:file_sink", file_sink_init },
 	{ "anyloop:logger", logger_init },

libaylp/anyloop.c

@@ -49,7 +49,41 @@ static void cleanup(void)
 }
 
 
-void handle_signal(int sig, siginfo_t *info, void *context)
+aylp_type aylp_type_from_string(const char *type_name)
+{
+	if (!strcasecmp(type_name, "none")) return AYLP_T_NONE;
+	if (!strcasecmp(type_name, "block")) return AYLP_T_BLOCK;
+	if (!strcasecmp(type_name, "vector")) return AYLP_T_VECTOR;
+	if (!strcasecmp(type_name, "matrix")) return AYLP_T_MATRIX;
+	if (!strcasecmp(type_name, "block_uchar")) return  AYLP_T_BLOCK_UCHAR;
+	if (!strcasecmp(type_name, "matrix_uchar")) return AYLP_T_MATRIX_UCHAR;
+
+	log_error("Couldn't parse type: %s", type_name);
+	return AYLP_T_NONE;
+}
+
+const char *aylp_type_to_string(aylp_type type)
+{
+	switch (type) {
+	case AYLP_T_BLOCK:
+		return "block";
+	case AYLP_T_VECTOR:
+		return "vector";
+	case AYLP_T_MATRIX:
+		return "matrix";
+	case AYLP_T_BLOCK_UCHAR:
+		return "block_uchar";
+	case AYLP_T_MATRIX_UCHAR:
+		return "matrix_uchar";
+	default:
+		log_error("Unknown type 0x%hhX", type);
+		return "NONE";
+	}
+}
+
+
+
+static void handle_signal(int sig, siginfo_t *info, void *context)
 {
 	UNUSED(info);
 	UNUSED(context);
@@ -188,24 +222,24 @@ int main(int argc, char **argv)
 	units_cur |= conf.devices[conf.n_devices-1].units_out;
 	for (size_t idx=0; idx<conf.n_devices; idx++) {
 		struct aylp_device d = conf.devices[idx];	// brevity
-		log_trace("type check: prev=0x%hX, in=0x%hX, out=0x%hX",
+		log_trace("type check: prev=0x%hhX, in=0x%hhX, out=0x%hhX",
 			type_cur, d.type_in, d.type_out
 		);
 		// typecheck fails if any of the bits set in type_cur are not
 		// set in type_in
 		if (type_cur & ~d.type_in) {
-			log_fatal("Device %s with input type 0x%hX "
-				"is incompatible with previous type 0x%hX",
+			log_fatal("Device %s with input type 0x%hhX "
+				"is incompatible with previous type 0x%hhX",
 				d.uri, d.type_in, type_cur
 			);
 			return EXIT_FAILURE;
 		}
-		log_trace("unit check: prev=0x%hX, in=0x%hX, out=0x%hX",
+		log_trace("unit check: prev=0x%hhX, in=0x%hhX, out=0x%hhX",
 			units_cur, d.units_in, d.units_out
 		);
 		if (units_cur & ~d.units_in) {
-			log_fatal("Device %s with input units 0x%hX "
-				"is incompatible with previous units 0x%hX",
+			log_fatal("Device %s with input units 0x%hhX "
+				"is incompatible with previous units 0x%hhX",
 				d.uri, d.units_in, units_cur
 			);
 			return EXIT_FAILURE;

libaylp/anyloop.h

@@ -226,5 +226,17 @@ struct aylp_conf {
 };
 
 
+/** Convert a string to a type.
+ * @param type_name: the string holding the type name (e.g. "vector")
+ */
+aylp_type aylp_type_from_string(const char *type_name);
+
+
+/** Convert a type to its name as a string.
+ * @param type: the string holding the type name (e.g. "vector")
+ */
+const char *aylp_type_to_string(aylp_type type);
+
+
 #endif	// include guard
 

meson.build

@@ -24,6 +24,7 @@ project_source_files = [
 	'libaylp/xalloc.c',
 	'libaylp/profile.c',
 	'devices/center_of_mass.c',
+	'devices/clamp.c',
 	'devices/device.c',
 	'devices/delay.c',
 	'devices/file_sink.c',