value_mapper.comp

component value_mapper "Diapason Mapper";

description """Diapason Mapper. Implements the following mapping function:

out(x) = ((x-x1)*(y2-y1)/(x2-x1)+y1)
Where x1 < x < x2

So, it maps one diapason of values to another
""";

pin in float in "Input value";
pin out float out "Mapped value";
pin in bit enable = 1 "Mapper enable. When false, the in is passed to out without any calculations.";

function _;

option extra_setup;

license "GPL";
;;
//#include <stdio.h>
//#include <map>

#define MAX 20
int commanded[MAX] = {0,};
RTAPI_MP_ARRAY_INT(commanded, MAX, "Commanded values array");
int measured[MAX] = {0,};
RTAPI_MP_ARRAY_INT(measured, MAX, "Measured values array");

int realCount = 0;
int get_count(void) {
    int i = 0;
    for(i=0; i<MAX && commanded[i]; i++) { /* Nothing */ }
    return i;
}

typedef struct {
	int commanded;
	int measured;
} data;

data raw_data[MAX];

EXTRA_SETUP() {
    int i;
    realCount = get_count();
    for(i = 0; i < realCount; ++i){
        raw_data[i].commanded = commanded[i];
        raw_data[i].measured =  measured[i];
    }
    return 0; 
}

int comparer(const void *_v1, const void *_v2)
{
	data *v1 = (data*)_v1;
	data *v2 = (data*)_v2;
	if(v1->measured == v2->measured)
		return 0;
	else if(v1->measured > v2->measured)
		return 1;
	return -1;
}

float calc(float x)
{
	int i, j = 0;
	int x1, x2, y1, y2;

	if(raw_data[0].measured > x) // X too low
		return raw_data[0].commanded;
	
	for(i = 0; i < realCount; ++i)
	{
		j = i + 1;
		if(j == realCount)	// x tool high
			return raw_data[i].commanded;

		if(raw_data[j].measured >= x)
			break;
	};

	y1 = raw_data[i].commanded;
	y2 = raw_data[j].commanded;
	x1 = raw_data[i].measured;
	x2 = raw_data[j].measured;

	if(x2 == x1)	// prevent divide by zero
	{
		return y1;
	}

	return ((float)(x-x1)*(y2-y1)/(x2-x1)+y1);
}

FUNCTION(_)
{
	float x = in;
	if(!enable || !realCount)
	{
		out = x;
		return;
	}
	out = calc(x);
}