/*****************************************************************************
 *
 * grail - Gesture Recognition And Instantiation Library
 *
 * Copyright (C) 2010-2011 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/>.
 *
 ****************************************************************************/

#include "grail-impl.h"
#include <stdlib.h>
#include <string.h>
#include <math.h>

static void set_center_velocity_and_radius(struct grail_impl *impl,
					   struct grail_element *slot)
{
	const struct utouch_contact **tc = slot->touches;
	double x, y, vx, vy, r2, dx, dy;
	int i;

	switch (slot->num_touches) {
	case 1:
		x = tc[0]->x;
		y = tc[0]->y;
		vx = tc[0]->vx;
		vy = tc[0]->vy;
		r2 = 0;
		break;
	case 2:
		dx = 0.5 * (tc[1]->x - tc[0]->x);
		dy = 0.5 * (tc[1]->y - tc[0]->y);
		x = tc[0]->x + dx;
		y = tc[0]->y + dy;
		vx = 0.5 * (tc[0]->vx + tc[1]->vx);
		vy = 0.5 * (tc[0]->vy + tc[1]->vy);
		r2 = dx * dx + dy * dy;
		break;
	default:
		x = y = vx = vy = r2 = 0;
		for (i = 0; i < slot->num_touches; i++) {
			x += tc[i]->x;
			y += tc[i]->y;
			vx += tc[i]->vx;
			vy += tc[i]->vy;
		}
		x /= slot->num_touches;
		y /= slot->num_touches;
		vx /= slot->num_touches;
		vy /= slot->num_touches;
		for (i = 0; i < slot->num_touches; i++) {
			dx = tc[i]->x - x;
			dy = tc[i]->y - y;
			r2 += dx * dx + dy * dy;
		}
		r2 /= slot->num_touches;
		break;
	}

	slot->center.x = x;
	slot->center.y = y;
	slot->velocity.x = 1000 * vx;
	slot->velocity.y = 1000 * vy;
	slot->radius2 = r2;
}

static void set_moveness_pivot_and_drag(struct grail_impl *impl,
					struct grail_element *slot,
					double ds, double dc)
{
	const struct grail_control *ctl = impl->ctl;
	const struct grail_element *pslot = slot->prev;
	double mx = slot->center.x - pslot->center.x;
	double my = slot->center.y - pslot->center.y;
	float *T = slot->transform;

	slot->moveness = 1;
	slot->pivot = pslot->center;

	if (slot->num_touches > 1) {
		double wx = (1 - dc) * mx + ds * my;
		double wy = (1 - dc) * my - ds * mx;
		double w2 = wx * wx + wy * wy;
		if (w2 > 0) {
			double q = (mx * mx + my * my) / w2;
			double s = ctl->pivot_unbound ?
				q : sqrt(pslot->radius2 / w2);
			if (s < q) {
				slot->moveness = 1 - s / q;
				slot->pivot.x += s * wx;
				slot->pivot.y += s * wy;
			} else {
				slot->moveness = 0;
				slot->pivot.x += q * wx;
				slot->pivot.y += q * wy;
			}
		}
	}

	mx *= slot->moveness;
	my *= slot->moveness;

	T[0] = dc;
	T[1] = ds;
	T[2] = (1 - dc) * slot->pivot.x - ds * slot->pivot.y + mx;
	T[3] = -ds;
	T[4] = dc;
	T[5] = (1 - dc) * slot->pivot.y + ds * slot->pivot.x + my;

	slot->drag.x = pslot->drag.x + mx;
	slot->drag.y = pslot->drag.y + my;
}

static void start_slot(struct grail_impl *impl,
		       struct grail_element *slot,
		       const struct utouch_frame *touch)
{
	float *T = slot->transform;

	slot->id = impl->seqid++ & GRAIL_ID_MAX;
	slot->expect_mask = GRAIL_EXPECT_MASK;
	slot->active_mask = 0;
	slot->start_time = touch->time;
	set_center_velocity_and_radius(impl, slot);
	slot->start_center = slot->center;
	T[0] = T[4] = 1;
	T[1] = T[2] = T[3] = T[5] = 0;
	slot->pivot = slot->center;
	slot->drag.x = 0;
	slot->drag.y = 0;
	slot->scale2 = 1;
	slot->angle = 0;
}

static void update_slot(struct grail_impl *impl,
			struct grail_element *slot,
			double ds, double dc)
{
	const struct grail_element *pslot = slot->prev;

	slot->id = pslot->id;
	slot->start_time = pslot->start_time;
	slot->start_center = pslot->start_center;
	slot->expect_mask = pslot->expect_mask;
	slot->active_mask = pslot->active_mask;

	set_center_velocity_and_radius(impl, slot);
	set_moveness_pivot_and_drag(impl, slot, ds, dc);

	slot->scale2 = pslot->scale2 * (ds * ds + dc * dc);
	slot->angle = pslot->angle + ds / dc; /* atan2(ds, dc) */
}

static void stop_slot(struct grail_impl *impl,
		      struct grail_element *slot)
{
	const struct grail_element *pslot = slot->prev;
	float *T = slot->transform;

	slot->id = -1;
	slot->num_touches = 0;
	slot->start_time = pslot->start_time;
	slot->start_center = pslot->start_center;
	slot->expect_mask = 0;
	slot->active_mask = pslot->active_mask;
	slot->center = pslot->center;
	slot->velocity = pslot->velocity;
	slot->radius2 = pslot->radius2;
	T[0] = T[4] = 1;
	T[1] = T[2] = T[3] = T[5] = 0;
	slot->moveness = 1;
	slot->pivot = pslot->pivot;
	slot->drag = pslot->drag;
	slot->scale2 = pslot->scale2;
	slot->angle = pslot->angle;
}

static void set_slot_one(struct grail_impl *impl,
			 struct grail_element *slot,
			 const struct utouch_frame *touch,
			 const struct utouch_contact *t1)
{
	const struct grail_element *pslot = slot->prev;
	const struct utouch_contact *p1 = pslot->touches[0];

	if (!t1->active) {
		stop_slot(impl, slot);
		return;
	}

	slot->touches[0] = t1;
	slot->num_touches = 1;

	if (pslot->num_touches != slot->num_touches || t1->id != p1->id) {
		start_slot(impl, slot, touch);
		return;
	}

	update_slot(impl, slot, 0, 1);
}

static void set_slot_two(struct grail_impl *impl,
			 struct grail_element *slot,
			 const struct utouch_frame *touch,
			 const struct utouch_contact *t1,
			 const struct utouch_contact *t2)
{
	const struct grail_element *pslot = slot->prev;
	const struct utouch_contact *p1 = pslot->touches[0];
	const struct utouch_contact *p2 = pslot->touches[1];
	double tx, ty, px, py, d2;

	if (!t1->active || !t2->active) {
		stop_slot(impl, slot);
		return;
	}

	slot->touches[0] = t1;
	slot->touches[1] = t2;
	slot->num_touches = 2;

	if (pslot->num_touches != slot->num_touches ||
	    t1->id != p1->id || t2->id != p2->id) {
		start_slot(impl, slot, touch);
		return;
	}

	tx = t2->x - t1->x;
	ty = t2->y - t1->y;
	px = p2->x - p1->x;
	py = p2->y - p1->y;

	d2 = px * px + py * py;
	if (d2 > 0) {
		px /= d2;
		py /= d2;
	}

	update_slot(impl, slot, tx * py - ty * px, tx * px + ty * py);
}

static void set_slot_multi(struct grail_impl *impl,
			   struct grail_element *slot,
			   struct grail_frame *frame,
			   const struct utouch_frame *touch)
{
	const struct grail_element *pslot = slot->prev;
	struct grail_element **slots = frame->slots;
	int i, j, n = impl->num_touches;
	struct grail_element *best = 0;

	if (touch->num_active < 3) {
		stop_slot(impl, slot);
		return;
	}

	memcpy(slot->touches, touch->active,
	       touch->num_active * sizeof(slot->touches[0]));
	slot->num_touches = touch->num_active;

	if (pslot->num_touches != slot->num_touches) {
		start_slot(impl, slot, touch);
		return;
	}

	for (i = 0; i < slot->num_touches; i++) {
		if (slot->touches[i]->id != pslot->touches[i]->id) {
			start_slot(impl, slot, touch);
			return;
		}
	}

	for (i = 0; i < impl->num_touches; i++) {
		for (j = i + 1; j < impl->num_touches; j++) {
			struct grail_element *s = slots[n++];
			if (!s->num_touches)
				continue;
			if (!best || s->radius2 > best->radius2)
				best = s;
		}
	}

	update_slot(impl, slot, best->transform[1], best->transform[0]);
}

static void set_slots(struct grail_impl *impl,
		      struct grail_frame *frame,
		      const struct utouch_frame *touch)
{
	struct grail_element **slots = frame->slots;
	struct utouch_contact *const *tc = touch->slots;
	int i, j, n = 0;

	for (i = 0; i < impl->num_touches; i++)
		set_slot_one(impl, slots[n++], touch, tc[i]);

	for (i = 0; i < impl->num_touches; i++)
		for (j = i + 1; j < impl->num_touches; j++)
			set_slot_two(impl, slots[n++], touch, tc[i], tc[j]);

	set_slot_multi(impl, slots[n++], frame, touch);
}

static void collect_transforms(struct grail_impl *impl,
			       struct grail_frame *frame,
			       const struct utouch_frame *touch)
{
	const struct utouch_surface *s = utouch_frame_get_surface(impl->fh);
	const struct grail_control *ctl = impl->ctl;
	float c_x = ctl->bar_center_x * (s->mapped_max_x - s->mapped_min_x);
	float c_y = ctl->bar_center_y * (s->mapped_max_y - s->mapped_min_y);
	float d_x = ctl->bar_drag_x * (s->mapped_max_x - s->mapped_min_x);
	float d_y = ctl->bar_drag_y * (s->mapped_max_y - s->mapped_min_y);
	float ds2 = ctl->bar_scale * ctl->bar_scale;
	float dt;
	int i;

	for (i = 0; i < impl->num_slots; i++) {
		struct grail_element *s = frame->slots[i];

		if (!s->num_touches)
			continue;

		dt = touch->time - s->start_time;
		if (dt > ctl->glue_ms) {
			unsigned int mask = s->active_mask;

			if (fabs(s->center.x - s->start_center.x) > c_x)
				mask |= GRAIL_EXPECT_CENTER_X;
			if (fabs(s->center.y - s->start_center.y) > c_y)
				mask |= GRAIL_EXPECT_CENTER_Y;
			if (fabs(s->drag.x) > d_x)
				mask |= GRAIL_EXPECT_DRAG_X;
			if (fabs(s->drag.y) > d_y)
				mask |= GRAIL_EXPECT_DRAG_Y;
			if (fabs(s->scale2 - 1) > ds2)
				mask |= GRAIL_EXPECT_SCALE;
			if (fabs(s->angle) > ctl->bar_angle)
				mask |= GRAIL_EXPECT_ANGLE;

			s->active_mask = mask;

			if (dt < ctl->drop_x_ms)
				mask |= GRAIL_EXPECT_CENTER_X;
			if (dt < ctl->drop_y_ms)
				mask |= GRAIL_EXPECT_CENTER_Y;
			if (dt < ctl->drop_x_ms)
				mask |= GRAIL_EXPECT_DRAG_X;
			if (dt < ctl->drop_y_ms)
				mask |= GRAIL_EXPECT_DRAG_Y;
			if (dt < ctl->drop_scale_ms)
				mask |= GRAIL_EXPECT_SCALE;
			if (dt < ctl->drop_angle_ms)
				mask |= GRAIL_EXPECT_ANGLE;

			s->expect_mask &= mask;
		}

		frame->ongoing[frame->num_ongoing++] = s;
	}
}

const struct grail_frame GRAIL_PUBLIC *
grail_pump_frame(grail_handle ge, const struct utouch_frame *touch)
{
	struct grail_impl *impl = ge->impl;
	struct grail_frame *frame = impl->frames[impl->nextframe];
	const struct grail_frame *prev = frame->prev;
	int i;

	if (touch->slot_revision == touch->prev->slot_revision &&
	    !prev->num_ongoing)
		return 0;

	frame->touch = touch;
	frame->num_ongoing = 0;
	for (i = 0; i < impl->num_slots; i++)
		frame->slots[i]->prev = prev->slots[i];

	set_slots(impl, frame, touch);
	collect_transforms(impl, frame, touch);

	impl->nextframe = (impl->nextframe + 1) % impl->num_frames;

	return frame;
}