/*
 *			GPAC - MPEG-4 Systems C Development Kit
 *
 *			Copyright (c) Jean Le Feuvre 2000-2005 
 *					All rights reserved
 *
 *  This file is part of GPAC / Scene Rendering sub-project
 *
 *  GPAC 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 2, or (at your option)
 *  any later version.
 *   
 *  GPAC 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 GNU Make; see the file COPYING.  If not, write to
 *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA. 
 *
 */

#include <gpac/m4_math.h>

/*
	2D MATRIX TOOLS
 */

void m4_mx2d_add_matrix(M4Matrix2D *_this, M4Matrix2D *from)
{
	M4Matrix2D bck;
	if (!_this || !from) return;

	if (m4_mx2d_is_identity(*from)) return;
	else if (m4_mx2d_is_identity(*_this)) {
		m4_mx2d_copy(*_this, *from);
		return;
	}
	m4_mx2d_copy(bck, *_this);
	_this->m[0] = m4_mulfix(from->m[0], bck.m[0]) + m4_mulfix(from->m[1], bck.m[3]);
	_this->m[1] = m4_mulfix(from->m[0], bck.m[1]) + m4_mulfix(from->m[1], bck.m[4]);
	_this->m[2] = m4_mulfix(from->m[0], bck.m[2]) + m4_mulfix(from->m[1], bck.m[5]) + from->m[2];
	_this->m[3] = m4_mulfix(from->m[3], bck.m[0]) + m4_mulfix(from->m[4], bck.m[3]);
	_this->m[4] = m4_mulfix(from->m[3], bck.m[1]) + m4_mulfix(from->m[4], bck.m[4]);
	_this->m[5] = m4_mulfix(from->m[3], bck.m[2]) + m4_mulfix(from->m[4], bck.m[5]) + from->m[5];
}

void m4_mx2d_add_translation(M4Matrix2D *_this, Fixed cx, Fixed cy)
{
	M4Matrix2D tmp;
	if (!_this || (!cx && !cy) ) return;
	m4_mx2d_init(tmp);
	tmp.m[2] = cx;
	tmp.m[5] = cy;
	m4_mx2d_add_matrix(_this, &tmp);
}


void m4_mx2d_add_rotation(M4Matrix2D *_this, Fixed cx, Fixed cy, Fixed angle)
{
	M4Matrix2D tmp;
	if (!_this) return;
	m4_mx2d_init(tmp);

	m4_mx2d_add_translation(_this, -cx, -cy);
	
	tmp.m[0] = m4_cos(angle);
	tmp.m[4] = tmp.m[0];
	tmp.m[3] = m4_sin(angle);
	tmp.m[1] = -1 * tmp.m[3];
	m4_mx2d_add_matrix(_this, &tmp);
	m4_mx2d_add_translation(_this, cx, cy);
}

void m4_mx2d_add_scale(M4Matrix2D *_this, Fixed scale_x, Fixed scale_y)
{
	M4Matrix2D tmp;
	if (!_this || ((scale_x==FIX_ONE) && (scale_y==FIX_ONE)) ) return;
	m4_mx2d_init(tmp);
	tmp.m[0] = scale_x;
	tmp.m[4] = scale_y;
	m4_mx2d_add_matrix(_this, &tmp);
}

void m4_mx2d_add_scale_at(M4Matrix2D *_this, Fixed scale_x, Fixed scale_y, Fixed cx, Fixed cy, Fixed angle)
{
	M4Matrix2D tmp;
	if (!_this) return;
	m4_mx2d_init(tmp);
	if (angle) {
		m4_mx2d_add_rotation(_this, cx, cy, -angle);
	}
	tmp.m[0] = scale_x;
	tmp.m[4] = scale_y;
	m4_mx2d_add_matrix(_this, &tmp);
	if (angle) m4_mx2d_add_rotation(_this, cx, cy, angle);
}

void m4_mx2d_add_skew(M4Matrix2D *_this, Fixed skew_x, Fixed skew_y)
{
	M4Matrix2D tmp;
	if (!_this || (!skew_x && !skew_y) ) return;
	m4_mx2d_init(tmp);
	tmp.m[1] = skew_x;
	tmp.m[3] = skew_y;
	m4_mx2d_add_matrix(_this, &tmp);
}

void m4_mx2d_add_skewX(M4Matrix2D *_this, Fixed angle)
{
	M4Matrix2D tmp;
	Fixed a_sin = m4_sin(angle);
	if (!_this || !a_sin) return;
	m4_mx2d_init(tmp);
	tmp.m[1] = m4_divfix(m4_cos(angle), a_sin);
	tmp.m[3] = 0;
	m4_mx2d_add_matrix(_this, &tmp);
}

void m4_mx2d_add_skewY(M4Matrix2D *_this, Fixed angle)
{
	M4Matrix2D tmp;
	Fixed a_sin = m4_sin(angle);
	if (!_this || !a_sin) return;
	m4_mx2d_init(tmp);
	tmp.m[1] = 0;
	tmp.m[3] = m4_divfix(m4_cos(angle), a_sin);
	m4_mx2d_add_matrix(_this, &tmp);
}

Fixed m4_mx2d_get_determinent(M4Matrix2D *_this)
{
	if (_this)
		return m4_mulfix(_this->m[0], _this->m[4]) - m4_mulfix(_this->m[1], _this->m[3]);
	return 0;
}

void m4_mx2d_inverse(M4Matrix2D *_this)
{
	Fixed det;
	M4Matrix2D tmp;
	if(!_this) return;
	if (m4_mx2d_is_identity(*_this)) return;
	det = m4_mx2d_get_determinent(_this);
	if (!det) {
		m4_mx2d_init(*_this);
		return;
	}
	tmp.m[0] = m4_divfix(_this->m[4], det);
	tmp.m[1] = -1 *  m4_divfix(_this->m[1], det);
	tmp.m[2] = m4_mulfix(_this->m[1], _this->m[5]) - m4_mulfix(_this->m[4], _this->m[2]);
	tmp.m[2] = m4_divfix(tmp.m[2], det);
	tmp.m[3] = -1 * m4_divfix(_this->m[3], det);
	tmp.m[4] = m4_divfix(_this->m[0], det);
	tmp.m[5] = m4_mulfix(_this->m[3], _this->m[2]) - m4_mulfix(_this->m[0],_this->m[5]);
	tmp.m[5] = m4_divfix(tmp.m[5], det);
	m4_mx2d_copy(*_this, tmp);
}

void m4_mx2d_apply_coords(M4Matrix2D *_this, Fixed *x, Fixed *y)
{
	Fixed _x, _y;
	if (!_this || !x || !y) return;
	_x = m4_mulfix(*x, _this->m[0]) + m4_mulfix(*y, _this->m[1]) + _this->m[2];
	_y = m4_mulfix(*x, _this->m[3]) + m4_mulfix(*y, _this->m[4]) + _this->m[5];
	*x = _x;
	*y = _y;
}

void m4_mx2d_apply_point(M4Matrix2D *_this, M4Point2D *pt)
{
	m4_mx2d_apply_coords(_this, &pt->x, &pt->y);
}

void m4_mx2d_apply_rect(M4Matrix2D *_this, M4Rect *rc)
{
	M4Point2D c1, c2, c3, c4;
	c1.x = c2.x = rc->x;
	c3.x = c4.x = rc->x + rc->width;
	c1.y = c3.y = rc->y;
	c2.y = c4.y = rc->y - rc->height;
	m4_mx2d_apply_point(_this, &c1);
	m4_mx2d_apply_point(_this, &c2);
	m4_mx2d_apply_point(_this, &c3);
	m4_mx2d_apply_point(_this, &c4);
	rc->x = MIN(c1.x, MIN(c2.x, MIN(c3.x, c4.x)));
	rc->width = MAX(c1.x, MAX(c2.x, MAX(c3.x, c4.x))) - rc->x;
	rc->height = MIN(c1.y, MIN(c2.y, MIN(c3.y, c4.y)));
	rc->y = MAX(c1.y, MAX(c2.y, MAX(c3.y, c4.y)));
	rc->height = rc->y - rc->height;
	assert(rc->height>=0);
	assert(rc->width>=0);
}


/*
	COLOR MATRIX TOOLS
 */

void m4_cmx_init(M4ColorMatrix *_this)
{
	if (!_this) return;
	memset(_this->m, 0, sizeof(Fixed)*20);
	_this->m[0] = _this->m[6] = _this->m[12] = _this->m[18] = FIX_ONE;
	_this->identity = 1;
}


static void m4_cmx_identity(M4ColorMatrix *_this)
{
	M4ColorMatrix mat;
	m4_cmx_init(&mat);
	_this->identity = memcmp(_this->m, mat.m, sizeof(Fixed)*20) ? 0 : 1;
}

void m4_cmx_set_all(M4ColorMatrix *_this, Fixed *coefs)
{
	if (!_this || !coefs) return;
}

void m4_cmx_set(M4ColorMatrix *_this, 
				 Fixed c1, Fixed c2, Fixed c3, Fixed c4, Fixed c5,
				 Fixed c6, Fixed c7, Fixed c8, Fixed c9, Fixed c10,
				 Fixed c11, Fixed c12, Fixed c13, Fixed c14, Fixed c15,
				 Fixed c16, Fixed c17, Fixed c18, Fixed c19, Fixed c20)
{
	if (!_this) return;
	_this->m[0] = c1; _this->m[1] = c2; _this->m[2] = c3; _this->m[3] = c4; _this->m[4] = c5;
	_this->m[5] = c6; _this->m[6] = c7; _this->m[7] = c8; _this->m[8] = c9; _this->m[9] = c10;
	_this->m[10] = c11; _this->m[11] = c12; _this->m[12] = c13; _this->m[13] = c14; _this->m[14] = c15;
	_this->m[15] = c16; _this->m[16] = c17; _this->m[17] = c18; _this->m[18] = c19; _this->m[19] = c20;
	m4_cmx_identity(_this);
}

void m4_cmx_copy(M4ColorMatrix *_this, M4ColorMatrix *from)
{
	if (!_this || !from) return;
	memcpy(_this->m, from->m, sizeof(Fixed)*20);
	m4_cmx_identity(_this);
}


void m4_cmx_multiply(M4ColorMatrix *_this, M4ColorMatrix *w)
{
	Fixed res[20];
	if (!_this || !w || w->identity) return;
	if (_this->identity) {
		m4_cmx_copy(_this, w);
		return;
	}

	res[0] = m4_mulfix(_this->m[0], w->m[0]) + m4_mulfix(_this->m[1], w->m[5]) + m4_mulfix(_this->m[2], w->m[10]) + m4_mulfix(_this->m[3], w->m[15]);
	res[1] = m4_mulfix(_this->m[0], w->m[1]) + m4_mulfix(_this->m[1], w->m[6]) + m4_mulfix(_this->m[2], w->m[11]) + m4_mulfix(_this->m[3], w->m[16]);
	res[2] = m4_mulfix(_this->m[0], w->m[2]) + m4_mulfix(_this->m[1], w->m[7]) + m4_mulfix(_this->m[2], w->m[12]) + m4_mulfix(_this->m[3], w->m[17]);
	res[3] = m4_mulfix(_this->m[0], w->m[3]) + m4_mulfix(_this->m[1], w->m[8]) + m4_mulfix(_this->m[2], w->m[13]) + m4_mulfix(_this->m[3], w->m[18]);
	res[4] = m4_mulfix(_this->m[0], w->m[4]) + m4_mulfix(_this->m[1], w->m[9]) + m4_mulfix(_this->m[2], w->m[14]) + m4_mulfix(_this->m[3], w->m[19]) + _this->m[4];
	
	res[5] = m4_mulfix(_this->m[5], w->m[0]) + m4_mulfix(_this->m[6], w->m[5]) + m4_mulfix(_this->m[7], w->m[10]) + m4_mulfix(_this->m[8], w->m[15]);
	res[6] = m4_mulfix(_this->m[5], w->m[1]) + m4_mulfix(_this->m[6], w->m[6]) + m4_mulfix(_this->m[7], w->m[11]) + m4_mulfix(_this->m[8], w->m[16]);
	res[7] = m4_mulfix(_this->m[5], w->m[2]) + m4_mulfix(_this->m[6], w->m[7]) + m4_mulfix(_this->m[7], w->m[12]) + m4_mulfix(_this->m[8], w->m[17]);
	res[8] = m4_mulfix(_this->m[5], w->m[3]) + m4_mulfix(_this->m[6], w->m[8]) + m4_mulfix(_this->m[7], w->m[13]) + m4_mulfix(_this->m[8], w->m[18]);
	res[9] = m4_mulfix(_this->m[5], w->m[4]) + m4_mulfix(_this->m[6], w->m[9]) + m4_mulfix(_this->m[7], w->m[14]) + m4_mulfix(_this->m[8], w->m[19]) + _this->m[9];
	
	res[10] = m4_mulfix(_this->m[10], w->m[0]) + m4_mulfix(_this->m[11], w->m[5]) + m4_mulfix(_this->m[12], w->m[10]) + m4_mulfix(_this->m[13], w->m[15]);
	res[11] = m4_mulfix(_this->m[10], w->m[1]) + m4_mulfix(_this->m[11], w->m[6]) + m4_mulfix(_this->m[12], w->m[11]) + m4_mulfix(_this->m[13], w->m[16]);
	res[12] = m4_mulfix(_this->m[10], w->m[2]) + m4_mulfix(_this->m[11], w->m[7]) + m4_mulfix(_this->m[12], w->m[12]) + m4_mulfix(_this->m[13], w->m[17]);
	res[13] = m4_mulfix(_this->m[10], w->m[3]) + m4_mulfix(_this->m[11], w->m[8]) + m4_mulfix(_this->m[12], w->m[13]) + m4_mulfix(_this->m[13], w->m[18]);
	res[14] = m4_mulfix(_this->m[10], w->m[4]) + m4_mulfix(_this->m[11], w->m[9]) + m4_mulfix(_this->m[12], w->m[14]) + m4_mulfix(_this->m[13], w->m[19]) + _this->m[14];
	
	res[15] = m4_mulfix(_this->m[15], w->m[0]) + m4_mulfix(_this->m[16], w->m[5]) + m4_mulfix(_this->m[17], w->m[10]) + m4_mulfix(_this->m[18], w->m[15]);
	res[16] = m4_mulfix(_this->m[15], w->m[1]) + m4_mulfix(_this->m[16], w->m[6]) + m4_mulfix(_this->m[17], w->m[11]) + m4_mulfix(_this->m[18], w->m[16]);
	res[17] = m4_mulfix(_this->m[15], w->m[2]) + m4_mulfix(_this->m[16], w->m[7]) + m4_mulfix(_this->m[17], w->m[12]) + m4_mulfix(_this->m[18], w->m[17]);
	res[18] = m4_mulfix(_this->m[15], w->m[3]) + m4_mulfix(_this->m[16], w->m[8]) + m4_mulfix(_this->m[17], w->m[13]) + m4_mulfix(_this->m[18], w->m[18]);
	res[19] = m4_mulfix(_this->m[15], w->m[4]) + m4_mulfix(_this->m[16], w->m[9]) + m4_mulfix(_this->m[17], w->m[14]) + m4_mulfix(_this->m[18], w->m[19]) + _this->m[19];
	memcpy(_this->m, res, sizeof(Fixed)*20);
	m4_cmx_identity(_this);
}

#define CLIP_COMP(val)	{ if (val<0) { val=0; } else if (val>FIX_ONE) { val=FIX_ONE;} }


M4Color m4_cmx_apply(M4ColorMatrix *_this, M4Color col)
{
	Fixed _a, _r, _g, _b, a, r, g, b;
	if (!_this || _this->identity) return col;

	a = INT2FIX(col>>24); a /= 255;
	r = INT2FIX((col>>16)&0xFF); r /= 255;
	g = INT2FIX((col>>8)&0xFF); g /= 255;
	b = INT2FIX((col)&0xFF); b /= 255;
	_r = m4_mulfix(r, _this->m[0]) + m4_mulfix(g, _this->m[1]) + m4_mulfix(b, _this->m[2]) + m4_mulfix(a, _this->m[3]) + _this->m[4];
	_g = m4_mulfix(r, _this->m[5]) + m4_mulfix(g, _this->m[6]) + m4_mulfix(b, _this->m[7]) + m4_mulfix(a, _this->m[8]) + _this->m[9];
	_b = m4_mulfix(r, _this->m[10]) + m4_mulfix(g, _this->m[11]) + m4_mulfix(b, _this->m[12]) + m4_mulfix(a, _this->m[13]) + _this->m[14];
	_a = m4_mulfix(r, _this->m[15]) + m4_mulfix(g, _this->m[16]) + m4_mulfix(b, _this->m[17]) + m4_mulfix(a, _this->m[18]) + _this->m[19];
	CLIP_COMP(_a);
	CLIP_COMP(_r);
	CLIP_COMP(_g);
	CLIP_COMP(_b);
	return M4C_ARGB(FIX2INT(_a*255),FIX2INT(_r*255),FIX2INT(_g*255),FIX2INT(_b*255));
}

void m4_cmx_apply_fixed(M4ColorMatrix *_this, Fixed *a, Fixed *r, Fixed *g, Fixed *b)
{
	u32 col = M4C_ARGB_FIXED(*a, *r, *g, *b);
	col = m4_cmx_apply(_this, col);
	*a = INT2FIX(M4C_A(col)) / 255;
	*r = INT2FIX(M4C_R(col)) / 255;
	*g = INT2FIX(M4C_G(col)) / 255;
	*b = INT2FIX(M4C_B(col)) / 255;
}

/*
	RECTANGLE TOOLS
 */

/*transform rect to smallest covering integer pixels rect - this is needed to make sure clearing
of screen is correctly handled, otherwise we have troubles with bitmap hardware blitting (always integer)*/
M4IRect m4_rect_pixelize(M4Rect *r)
{
	M4IRect rc;
	rc.x = FIX2INT(m4_floor(r->x));
	rc.y = FIX2INT(m4_ceil(r->y));
	rc.width = FIX2INT(m4_ceil(r->x + r->width)) - rc.x;
	rc.height = rc.y - FIX2INT(m4_floor(r->y - r->height));
	return rc;
}


/*adds @rc2 to @rc1 - the new @rc1 contains the old @rc1 and @rc2*/
void m4_rect_union(M4Rect *rc1, M4Rect *rc2) 
{
	if (!rc1->width || !rc1->height) {*rc1=*rc2; return;}
	if (rc2->x < rc1->x) { rc1->width += rc1->x - rc2->x; rc1->x = rc2->x; }
	if (rc2->x + rc2->width > rc1->x+rc1->width) rc1->width = rc2->x + rc2->width - rc1->x;
	if (rc2->y > rc1->y) { rc1->height += rc2->y - rc1->y; rc1->y = rc2->y; }
	if (rc2->y - rc2->height < rc1->y - rc1->height) rc1->height = rc1->y - rc2->y + rc2->height;
}

void m4_rect_center(M4Rect *rc, Fixed w, Fixed h)
{
	rc->x=-w/2; rc->y=h/2; rc->width=w; rc->height=h;
}

Bool m4_rect_overlaps(M4Rect rc1, M4Rect rc2)
{
	if (! rc2.height || !rc2.width || !rc1.height || !rc1.width) return 0;
	if (rc2.x+rc2.width<=rc1.x) return 0;
	if (rc2.x>=rc1.x+rc1.width) return 0;
	if (rc2.y-rc2.height>=rc1.y) return 0;
	if (rc2.y<=rc1.y-rc1.height) return 0;
	return 1;
}

Bool m4_rect_equal(M4Rect rc1, M4Rect rc2) 
{ 
	if ( (rc1.x == rc2.x)  && (rc1.y == rc2.y) && (rc1.width == rc2.width) && (rc1.height == rc2.height) )
		return 1;
	return 0;
}

