#include <math.h>

#include "jump_rope_count_device.h"
#include "schmidt_trigger.h"

#if !defined(__STDC_VERSION__) || __STDC_VERSION__ < 199901L

/* Before C99, float variants of math functions are not avaliable.  */
float _sqrtf(float x)
{
	return (float)sqrt(x);
}
#define sqrtf _sqrtf

float _fmodf(float x, float y)
{
	return (float)fmod(x, y);
}
#define fmodf _fmodf

/* Before C99, INFINITY may be unavaliable.  */
#ifndef INFINITY

#if defined __GNUC__ && (__GNUC__ > 3 || \
			 (__GNUC__ == 3 && __GNUC_MINOR >= 3))
#define INFINITY (__builtin_inff())
#else
#define INFINITY 1e10000f
#endif

#endif /* INFINITY */

#endif /* __STDC_VERSION__ < 199901L */

static int less(float a, float b)
{
	return a < b;
}

static int greater(float a, float b)
{
	return a > b;
}

int jump_rope_count_device_init(struct jump_rope_count_device *dev,
				struct jump_rope_count_config *cfg)
{
	int ret;

	ret = schmidt_init(&dev->trig_g, cfg->lg, cfg->hg);
	if (ret != 0)
		return ret;

	ret = schmidt_init(&dev->trig_a, cfg->la, cfg->ha);
	if (ret != 0)
		return ret;

	ret = schmidt_init(&dev->trig_gz, cfg->lgz, cfg->hgz);
	if (ret != 0)
		return ret;

	ret = ringbuf_init(&dev->rbuf_a, cfg->a_g_window);
	if (ret != 0)
		return ret;

	ret = ringbuf_init(&dev->rbuf_g, cfg->a_g_window);
	if (ret != 0)
		return ret;

	monotonic_queue_init(&dev->mq_min_cy, less);
	monotonic_queue_init(&dev->mq_max_cy, greater);

	dev->cy_window = cfg->cy_window;
	dev->cy_crit = cfg->cy_crit;
	dev->cy_suppress_time = cfg->cy_suppress_time;
	dev->wait_time = cfg->wait_time;

	dev->cy_suppress = 0;
	dev->last_cy = dev->last_cy_fixed = 0;

	return 0;
}

static float hypot3f(float x, float y, float z)
{
	return sqrtf(x*x + y*y + z*z);
}

static float angle_change(float old, float new)
{
	float ret = fmodf(new - old, 360.0);
	if (ret < -180.0)
		ret += 360.0;
	if (ret > 180.0)
		ret -= 360.0;
	return ret;
}

int process_packet(struct jump_rope_count_device *dev,
		   struct sensor_packet *packet,
		   enum jump_rope_count_result *result)
{
	int ret = 0;
	float min, max;

	float g = hypot3f(packet->gx, packet->gy, packet->gz);
	float a = hypot3f(packet->ax, packet->ay, packet->az);
	float gz = packet->gz;
	float cy = dev->last_cy_fixed + angle_change(dev->last_cy,
						     packet->cy);
	dev->last_cy_fixed = cy;
	dev->last_cy = packet->cy;

	ringbuf_push(&dev->rbuf_g, g);
	ringbuf_push(&dev->rbuf_a, a);

	schmidt_trig(&dev->trig_g, ringbuf_stdev(&dev->rbuf_g));
	schmidt_trig(&dev->trig_a, ringbuf_stdev(&dev->rbuf_a));

	ret = monotonic_queue_push(&dev->mq_min_cy, cy);
	if (ret != 0)
		return ret;

	if (monotonic_queue_get_len(&dev->mq_min_cy) > dev->cy_window)
		ret = monotonic_queue_pop(&dev->mq_min_cy);
	if (ret != 0)
		return ret;

	ret = monotonic_queue_push(&dev->mq_max_cy, cy);
	if (ret != 0)
		return ret;

	if (monotonic_queue_get_len(&dev->mq_max_cy) > dev->cy_window)
		ret = monotonic_queue_pop(&dev->mq_max_cy);
	if (ret != 0)
		return ret;

	ret = monotonic_queue_get_min(&dev->mq_min_cy, &min);
	if (ret != 0)
		return ret;

	ret = monotonic_queue_get_min(&dev->mq_max_cy, &max);
	if (ret != 0)
		return ret;

	if (max - min > dev->cy_crit)
		dev->cy_suppress = dev->cy_suppress_time;

	if (schmidt_get(&dev->trig_g) == 0 ||
	    schmidt_get(&dev->trig_a) == 0 ||
	    dev->cy_suppress > 0)
	{
		schmidt_trig(&dev->trig_gz, -INFINITY);
		*result = RESULT_INACTIVE;
		goto out;
	}

	if (schmidt_trig(&dev->trig_gz, gz) == 1 &&
	    schmidt_get(&dev->trig_gz) == 0 &&
	    dev->cy_suppress == 0)
	{
		dev->remain_time = dev->wait_time;
		*result = RESULT_TRIGGERED;
		goto out;
	}

	if (dev->remain_time)
		dev->remain_time--;

	*result = (dev->remain_time > 0 ? RESULT_NONE : RESULT_INACTIVE);
out:
	if (dev->cy_suppress > 0)
		dev->cy_suppress--;
	return 0;
}
/* vim: set ts=8 sw=8 sts=8 noet: */