/*****************************************************************************
 *
 * grail - Gesture Recognition And Instantiation Library
 *
 * Copyright (C) 2010 Canonical Ltd.
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or (at your
 * option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Authors:
 *      Henrik Rydberg <rydberg@bitmath.org>
 *
 ****************************************************************************/

#include "grail-recognizer.h"
#include <math.h>

static const float SN_MOVE = 200;
static const float SN_ZOOM = 200;
static const float SN_ROTATE = 200;
static const float BAR_MOVE = 4;
static const float BAR_ZOOM = 12;
static const float BAR_ROTATE = 12;

static void compute_position(float *x, float *y,
			     const struct touch_frame *frame)
{
	int i, n = frame->nactive;
	*x = 0;
	*y = 0;
	if (n < 1)
		return;
	for (i = 0; i < n; i++) {
		const struct touch *t = frame->active[i];
		*x += t->prop[TP_POS_X];
		*y += t->prop[TP_POS_Y];
	}
	*x /= n;
	*y /= n;
}

static float compute_radius(float x, float y,
			    const struct touch_frame *frame)
{
	int i, n = frame->nactive;
	float r = 0, r2 = 0;
	if (n < 2)
		return r;
	for (i = 0; i < n; i++) {
		const struct touch *t = frame->active[i];
		float dx = t->prop[TP_POS_X] - x;
		float dy = t->prop[TP_POS_Y] - y;
		r2 += dx * dx + dy * dy;
	}
	r2 /= n;
	r = sqrt(r2);
	return r;
}

static float compute_rotation(float x, float y, float r,
			      const struct touch_frame *prev,
			      const struct touch_frame *frame)
{
	int i, n = frame->nactive;
	float da = 0, da2 = 0;
	if (n < 2)
		return da;
	for (i = 0; i < n; i++) {
		const struct touch *t = frame->active[i];
		const struct touch *ot = &prev->touch[t->slot];
		float dx = t->prop[TP_POS_X] - x;
		float dy = t->prop[TP_POS_Y] - y;
		float mx = t->prop[TP_POS_X] - ot->prop[TP_POS_X];
		float my = t->prop[TP_POS_Y] - ot->prop[TP_POS_Y];
		da2 += dx * my - dy * mx;
	}
	da2 /= n;
	da = da2 / r;
	return da;
}

static float move_filter(const struct filter_model *m, float val)
{
	if (val > m->val - m->fuzz / 2 && val < m->val + m->fuzz / 2)
		return m->val;
	if (val > m->val - m->fuzz && val < m->val + m->fuzz)
		return (m->val * 3 + val) / 4;
	if (val > m->val - m->fuzz * 2 && val < m->val + m->fuzz * 2)
		return (m->val + val) / 2;
	return val;
}

static int move_reset(struct filter_model *m, float val)
{
	m->delta = 0;
	m->tickle = 0;
	m->active = 0;
	m->val = val;
	m->orig = val;
}

static int move_update(struct filter_model *m, float val)
{
	m->delta = val - m->val;
	m->tickle = fabs(m->delta) > 0;
	m->val = val;
	if (!m->active && fabs(val - m->orig) > m->bar)
		m->active = 1;
	return m->active;
}

void gru_init_motion(struct grail *ge, const struct touch_dev_caps *caps)
{
	struct gesture_recognizer *gru = ge->gru;
	struct move_model *state = &gru->move;
	float x = caps->info[TP_POS_X].maximum - caps->info[TP_POS_X].minimum;
	float y = caps->info[TP_POS_Y].maximum - caps->info[TP_POS_Y].minimum;
	float r = sqrt(x * x + y * y);
	state->x.fuzz = x / SN_MOVE;
	state->y.fuzz = y / SN_MOVE;
	state->r.fuzz = r / SN_ZOOM;
	state->a.fuzz = r / SN_ROTATE;
	state->x.bar = BAR_MOVE * state->x.fuzz;
	state->y.bar = BAR_MOVE * state->y.fuzz;
	state->r.bar = BAR_ZOOM * state->r.fuzz;
	state->a.bar = BAR_ROTATE * state->a.fuzz;
	state->multi = 0;
}

void gru_motion(struct grail *ge,
		const struct touch_frame *frame)
{
	struct gesture_recognizer *gru = ge->gru;
	struct move_model *state = &gru->move;
	float x, y, r, a;
	compute_position(&x, &y, frame);
	if (frame->nactive < 2 || frame->ncreate || frame->ndestroy) {
		r = compute_radius(x, y, frame);
		a = 0;
		move_reset(&state->x, x);
		move_reset(&state->y, y);
		move_reset(&state->r, r);
		move_reset(&state->a, a);
		state->multi = 0;
	} else {
		x = move_filter(&state->x, x);
		y = move_filter(&state->y, y);
		r = compute_radius(x, y, frame);
		r = move_filter(&state->r, r);
		a = state->a.val + compute_rotation(x, y, r, &gru->frame, frame);
		//a = move_filter(&state->a, a);
		move_update(&state->x, x);
		move_update(&state->y, y);
		move_update(&state->r, r);
		move_update(&state->a, a);
		state->multi = 1;
	}
	state->ntouch = frame->nactive;
	state->time = frame->time;
}

void gru_event(struct grail *ge, int gid,
	       const struct move_model *move,
	       const grail_prop_t *prop, int nprop)
{
	gin_gid_event(ge, gid,
		      move->x.val, move->y.val, move->ntouch,
		      prop, nprop,
		      GRAIL_STATUS_UPDATE);
}

void gru_end(struct grail *ge, int gid, const struct move_model *move)
{
	gin_gid_end(ge, gid, move->x.val, move->y.val, move->ntouch);
}