//
//  geometry.h
//
//  Created by döme on 26.07.2009.
//

/*
 * Copyright (c) 2009 Doemoetoer Gulyas.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#import <Foundation/Foundation.h>

#import <OpenGLES/ES2/gl.h>
#import <CoreGraphics/CoreGraphics.h>

typedef union v3
{
	float a[3];
	struct {float x,y,z;} v;
} v3;

typedef union m16
{
	float a[16];
	float m[4][4];
} m16;

static inline v3 vcreate(const float a, const float b, const float c)
{
	return (v3){{a,b,c}};
}

static inline v3 vcross(v3 a, v3 b)
{
	return (v3){{a.v.y*b.v.z-a.v.z*b.v.y, a.v.z*b.v.x - a.v.x*b.v.z, a.v.x*b.v.y - a.v.y*b.v.x}};
}

static inline float vdot(v3 a, v3 b)
{
	return a.v.x*b.v.x+a.v.y*b.v.y+a.v.z*b.v.z;
}

static inline float vlength(v3 a)
{
	return sqrtf(vdot(a,a));
}

static inline v3 vmul(v3 a, float b)
{
	return (v3){{a.v.x*b, a.v.y*b, a.v.z*b}};
}


static inline v3 vproject(v3 a, v3 b)
{
	return vmul(b, vdot(a,b)/vdot(b,b));
}

static inline v3 vnormalize(v3 a)
{
	float length = vlength(a);
	if (length < FLT_EPSILON)
		return a;
	
	a.v.x /= length;
	a.v.y /= length;
	a.v.z /= length;
	
	return a;
}

static inline v3 vnegate(v3 a)
{
	return (v3){{-a.v.x,-a.v.y,-a.v.z}};
}

static inline v3 vadd(const v3 a, const v3 b)
{
	return (v3){{a.v.x+b.v.x, a.v.y+b.v.y, a.v.z+b.v.z}};
}

static inline v3 vsub(const v3 a, const v3 b)
{
	return (v3){{a.v.x-b.v.x, a.v.y-b.v.y, a.v.z-b.v.z}};
}

static inline float vsum(const v3 a)
{
	return a.v.x+a.v.y+a.v.z;
}

static inline v3 vfloor(const v3 a)
{
	return (v3){{floorf(a.v.x), floorf(a.v.y), floorf(a.v.z)}};
}

static inline float fclampf(const float a, const float mn, const float mx)
{
	return fminf(fmaxf(a, mn), mx);
}

static inline double fclamp(const double a, const double mn, const double mx)
{
	return fmin(fmax(a, mn), mx);
}

static inline v3 vclamp(const v3 a, const float mn, const float mx)
{
	return (v3){{fclampf(a.v.x,mn,mx), fclampf(a.v.y,mn,mx), fclampf(a.v.z,mn,mx)}};
}


static inline float sign(float a)
{
	return a >= 0.0f ? 1.0f : -1.0f;
}

static inline m16 midentity(void)
{
	m16 m = {{1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f}};
	return m;
}

static inline m16 mzero(void)
{
	m16 m = {{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}};
	return m;
}


static inline m16 mtranspose(const m16 a)
{
	m16 m;
	for (int i = 0; i < 4; ++i)
		for (int j = 0; j < 4; ++j)
			m.m[i][j] = a.m[j][i];
	return m;
}

static inline m16 mmul(const m16 a, const m16 b)
{
	m16 mm;

	for (int i = 0; i < 4; i++)
	{
		mm.m[i][0] = b.m[i][0]*a.m[0][0] + b.m[i][1]*a.m[1][0] + b.m[i][2]*a.m[2][0] + b.m[i][3]*a.m[3][0];
		mm.m[i][1] = b.m[i][0]*a.m[0][1] + b.m[i][1]*a.m[1][1] + b.m[i][2]*a.m[2][1] + b.m[i][3]*a.m[3][1];
		mm.m[i][2] = b.m[i][0]*a.m[0][2] + b.m[i][1]*a.m[1][2] + b.m[i][2]*a.m[2][2] + b.m[i][3]*a.m[3][2];
		mm.m[i][3] = b.m[i][0]*a.m[0][3] + b.m[i][1]*a.m[1][3] + b.m[i][2]*a.m[2][3] + b.m[i][3]*a.m[3][3];
	}

	return mm;
}


static inline v3 mtransformdir3(const m16 a, const v3 b)
{
	v3 v;
	v.v.x = a.a[0]*b.v.x + a.a[4]*b.v.y + a.a[8]*b.v.z;
	v.v.y = a.a[1]*b.v.x + a.a[5]*b.v.y + a.a[9]*b.v.z;
	v.v.z = a.a[2]*b.v.x + a.a[6]*b.v.y + a.a[10]*b.v.z;
	
	return v;
}

static inline v3 mtransformpos3(const m16 a, const v3 b)
{
	v3 v;
	v.v.x	= a.a[0]*b.v.x + a.a[4]*b.v.y + a.a[8]*b.v.z + a.a[12];
	v.v.y	= a.a[1]*b.v.x + a.a[5]*b.v.y + a.a[9]*b.v.z + a.a[13];
	v.v.z	= a.a[2]*b.v.x + a.a[6]*b.v.y + a.a[10]*b.v.z + a.a[14];
	float w = a.a[3]*b.v.x + a.a[7]*b.v.y + a.a[11]*b.v.z + a.a[15];
	return vcreate(v.v.x/w, v.v.y/w, v.v.z/w);
}


static inline m16 mcreatefrombases(const v3 ex, const v3 ey, const v3 ez)
{	
	return (m16){{ex.v.x, ex.v.y, ex.v.z, 0.0f,
				ey.v.x, ey.v.y, ey.v.z, 0.0f, 
				ez.v.x, ez.v.y, ez.v.z, 0.0f,
				0.0f, 0.0f, 0.0f, 1.0f}};
}

static inline m16 madjoint(m16 m)
{
	float A = m.m[2][2]*m.m[3][3] - m.m[3][2]*m.m[2][3];
	float B = m.m[1][2]*m.m[3][3] - m.m[3][2]*m.m[1][3];
	float C = m.m[1][2]*m.m[2][3] - m.m[2][2]*m.m[1][3];
	float D = m.m[0][2]*m.m[3][3] - m.m[3][2]*m.m[0][3];
	float E = m.m[0][2]*m.m[2][3] - m.m[2][2]*m.m[0][3];
	float F = m.m[0][2]*m.m[1][3] - m.m[1][2]*m.m[0][3];

	float A3 = m.m[2][1]*m.m[3][3] - m.m[3][1]*m.m[2][3];
	float B3 = m.m[1][1]*m.m[3][3] - m.m[3][1]*m.m[1][3];
	float C3 = m.m[1][1]*m.m[2][3] - m.m[2][1]*m.m[1][3];
	float D3 = m.m[0][1]*m.m[3][3] - m.m[3][1]*m.m[0][3];
	float E3 = m.m[0][1]*m.m[2][3] - m.m[2][1]*m.m[0][3];
	float F3 = m.m[0][1]*m.m[1][3] - m.m[1][1]*m.m[0][3];

	float A4 = m.m[2][1]*m.m[3][2] - m.m[3][1]*m.m[2][2];
	float B4 = m.m[1][1]*m.m[3][2] - m.m[3][1]*m.m[1][2];
	float C4 = m.m[1][1]*m.m[2][2] - m.m[2][1]*m.m[1][2];
	float D4 = m.m[0][1]*m.m[3][2] - m.m[3][1]*m.m[0][2];
	float E4 = m.m[0][1]*m.m[2][2] - m.m[2][1]*m.m[0][2];
	float F4 = m.m[0][1]*m.m[1][2] - m.m[1][1]*m.m[0][2];

	float AA = m.m[1][1]*A - m.m[2][1]*B + m.m[3][1]*C;
	float BB = m.m[0][1]*A - m.m[2][1]*D + m.m[3][1]*E;
	float CC = m.m[0][1]*B - m.m[1][1]*D + m.m[3][1]*F;
	float DD = m.m[0][1]*C - m.m[1][1]*E + m.m[2][1]*F;

	float EE = m.m[1][0]*A - m.m[2][0]*B + m.m[3][0]*C;
	float FF = m.m[0][0]*A - m.m[2][0]*D + m.m[3][0]*E;
	float GG = m.m[0][0]*B - m.m[1][0]*D + m.m[3][0]*F;
	float HH = m.m[0][0]*C - m.m[1][0]*E + m.m[2][0]*F;

	float II = m.m[1][0]*A3 - m.m[2][0]*B3 + m.m[3][0]*C3;
	float JJ = m.m[0][0]*A3 - m.m[2][0]*D3 + m.m[3][0]*E3;
	float KK = m.m[0][0]*B3 - m.m[1][0]*D3 + m.m[3][0]*F3;
	float LL = m.m[0][0]*C3 - m.m[1][0]*E3 + m.m[2][0]*F3;

	float MM = m.m[1][0]*A4 - m.m[2][0]*B4 + m.m[3][0]*C4;
	float NN = m.m[0][0]*A4 - m.m[2][0]*D4 + m.m[3][0]*E4;
	float OO = m.m[0][0]*B4 - m.m[1][0]*D4 + m.m[3][0]*F4;
	float PP = m.m[0][0]*C4 - m.m[1][0]*E4 + m.m[2][0]*F4;


	return (m16){{AA, -BB, CC, -DD, -EE, FF, -GG, HH, II, -JJ, KK, -LL, -MM, NN, -OO, PP}};

};

static inline float mdeterminant(m16 m)
{
	float A = m.m[2][2]*m.m[3][3] - m.m[3][2]*m.m[2][3];
	float B = m.m[1][2]*m.m[3][3] - m.m[3][2]*m.m[1][3];
	float C = m.m[1][2]*m.m[2][3] - m.m[2][2]*m.m[1][3];
	float D = m.m[0][2]*m.m[3][3] - m.m[3][2]*m.m[0][3];
	float E = m.m[0][2]*m.m[2][3] - m.m[2][2]*m.m[0][3];
	float F = m.m[0][2]*m.m[1][3] - m.m[1][2]*m.m[0][3];

	float AA = m.m[1][1]*A - m.m[2][1]*B + m.m[3][1]*C;
	float BB = m.m[0][1]*A - m.m[2][1]*D + m.m[3][1]*E;
	float CC = m.m[0][1]*B - m.m[1][1]*D + m.m[3][1]*F;
	float DD = m.m[0][1]*C - m.m[1][1]*E + m.m[2][1]*F;

	float AAA = m.m[0][0]*AA - m.m[1][0]*BB + m.m[2][0]*CC - m.m[3][0]*DD;

	return AAA;

};

static inline m16 mtranslate(v3 v)
{

	m16 m = midentity();
	m.m[3][0] = v.a[0];
	m.m[3][1] = v.a[1];
	m.m[3][2] = v.a[2];

	return m;
};

static inline m16 mscale(v3 v)
{

	m16 m = midentity();
	m.m[0][0] = v.a[0];
	m.m[1][1] = v.a[1];
	m.m[2][2] = v.a[2];

	return m;
};

static inline m16 mperspective(float fovy, float aspect, float zNear, float zFar)
{
	m16 m = mzero();
	float f = 1.0f/tanf(fovy*0.5f);
	
	m.m[0][0] = f/aspect;
	m.m[1][1] = f;
	m.m[2][2] = (zFar+zNear)/(zNear-zFar);
	m.m[2][3] = -1;
	m.m[3][2] = (2.0f*zFar*zNear)/(zNear-zFar);

	return m;
}

static inline m16 mortho(float left, float right, float bottom, float top, float zNear, float zFar)
{
	m16 m = midentity();
	
	m.m[0][0] = 2.0f/(right-left);
	m.m[1][1] = 2.0f/(top-bottom);
	m.m[2][2] = -2.0f/(zFar-zNear);
	m.m[3][0] = -(right+left)/(right - left);
	m.m[3][1] = -(top + bottom)/(top - bottom);
	m.m[3][2] = -(zFar + zNear)/(zFar - zNear);

	return m;
}


static inline m16 mmuls(m16 m, float s)
{
	for (int i = 0; i < 16; ++i)
		m.a[i] = m.a[i]*s;
	return m;
}

static inline m16 minverse(m16 m)
{
	m16		mm	= madjoint(m);
	float	d	= mdeterminant(m);
    return mmuls(mm, 1.0f/d);
};


m16 computeAlignedBasesFromDownVector(v3 acc);
m16 computeAlignedBasesFromDownVector2(v3 acc, v3 pf);

#pragma mark CG extensions

static inline CGSize CGSizeFitIntoSize(CGSize a, CGSize b)
{
	CGSize c = a;
	float heightR = a.height/b.height;
	float widthR = a.width/b.width;
	
	if (heightR > 1.0f)
	{
		c.height = b.height;
		c.width *= 1.0f/heightR;
		widthR = c.width/b.width;
	}

	if (widthR > 1.0f)
	{
		c.width = b.width;
		c.height *= 1.0f/widthR;
	}
	return c;
}


static inline CGPoint CGPointMin(CGPoint a, CGPoint b)
{
	return CGPointMake(fminf(a.x,b.x), fminf(a.y,b.y));
}

static inline CGPoint CGPointMax(CGPoint a, CGPoint b)
{
	return CGPointMake(fmaxf(a.x,b.x), fmaxf(a.y,b.y));
}

static inline CGPoint CGPointAdd(CGPoint a, CGPoint b)
{
	return CGPointMake(a.x+b.x, a.y+b.y);
}

static inline CGPoint CGPointSub(CGPoint a, CGPoint b)
{
	return CGPointMake(a.x-b.x, a.y-b.y);
}

static inline CGPoint CGPointScale(CGPoint a, float b)
{
	return CGPointMake(a.x*b, a.y*b);
}

static inline CGSize CGSizeScale(CGSize a, float b)
{
	return CGSizeMake(a.width*b, a.height*b);
}

static inline float CGPointDoubleDot(CGPoint a)
{
	return a.x*a.x+a.y*a.y;
}

static inline float CGPointDot(CGPoint a, CGPoint b)
{
	return (a.x*b.x + a.y*b.y);
}

static inline float CGPointCross(CGPoint a, CGPoint b)
{
	return (a.x*b.y - a.y*b.x);
}

static inline CGPoint CGPointNormalize(CGPoint a)
{
	float l1 = 1.0f/sqrtf(a.x*a.x+a.y*a.y);
	return CGPointMake(a.x*l1, a.y*l1);
}

static inline CGPoint CGPointNegate(CGPoint a)
{
	return CGPointMake(-a.x, -a.y);
}

static inline CGPoint CGPointNormal(CGPoint a)
{
	return CGPointMake(-a.y, a.x);
}

static inline float CGPointLength(CGPoint a)
{
	return sqrtf(a.x*a.x+a.y*a.y);
}

static inline CGPoint CGPointProject(CGPoint a, CGPoint b)
{
	return CGPointScale(b, CGPointDot(a,b)/CGPointDot(b,b));
}

static inline CGPoint CGPointReverseProject(CGPoint a, CGPoint b)
{
	return CGPointScale(b, CGPointDot(a,a)/CGPointDot(a,b));
}

static inline CGPoint CGPointLerp(CGPoint a, CGPoint b, float t)
{
	return CGPointAdd(a, CGPointScale(CGPointSub(b, a), t));
}


static inline CGPoint CGPointRotate(CGPoint a, float w)
{
	float cosa = cosf(w), sina = sin(w);
	
	CGPoint r;

	r.x = a.x*cosa - a.y*sina;
	r.y = a.y*cosa + a.x*sina;

	return r;
	
}


static inline long CGLineSegmentsIntersect2(CGPoint p0, CGPoint p1, CGPoint p2, CGPoint p3)
{
	float d = (p3.y - p2.y)*(p1.x - p0.x) - (p3.x - p2.x)*(p1.y - p0.y);
	
	if (d == 0.0)
		return 0;
	
	float a = (p3.x - p2.x)*(p0.y - p2.y) + (p3.y - p2.y)*(p0.x - p2.x);
	float b = (p1.x - p0.x)*(p0.y - p2.y) + (p1.y - p0.y)*(p0.x - p2.x);
	
	float ta = a/d;
	float tb = b/d;

	return ((ta > 0.0) && (ta < 1.0) && (tb > 0.0) && (tb < 1.0));
}

static inline long CGLineSegmentsIntersect(CGPoint p0, CGPoint p1, CGPoint p2, CGPoint p3)
{
	float d = CGPointCross(CGPointSub(p1,p0), CGPointSub(p3,p2));
	
	if (d == 0.0)
		return 0;
	
	float a = CGPointCross(CGPointSub(p2,p0), CGPointSub(p3,p2));
	float b = CGPointCross(CGPointSub(p2,p0), CGPointSub(p1,p0));
	
	float ta = a/d;
	float tb = b/d;

	return ((ta > 0.0) && (ta < 1.0) && (tb > 0.0) && (tb < 1.0));
}

static inline CGPoint CGLineIntersectionPoint(CGPoint p0, CGPoint p1, CGPoint p2, CGPoint p3)
{
	float d = CGPointCross(CGPointSub(p1,p0), CGPointSub(p3,p2));
	
	if (d == 0.0)
		return CGPointZero;
	
//	float a = CGPointCross(CGPointSub(p2,p0), CGPointSub(p3,p2));
	float b = CGPointCross(CGPointSub(p2,p0), CGPointSub(p1,p0));
	
//	float ta = a/d;
	float tb = b/d;

	return CGPointAdd(p2, CGPointScale(CGPointSub(p3,p2), tb));
}


static inline long CGPointInTriangle(CGPoint p, CGPoint a0, CGPoint a1, CGPoint a2)
{
	CGPoint v0 = CGPointSub(a1, a0);
	CGPoint v1 = CGPointSub(a2, a0);
	CGPoint v2 = CGPointSub(p, a0);

	float dot00 = CGPointDot(v0, v0);
	float dot01 = CGPointDot(v0, v1);
	float dot02 = CGPointDot(v0, v2);
	float dot11 = CGPointDot(v1, v1);
	float dot12 = CGPointDot(v1, v2);

	float invDenom = 1.0f / (dot00 * dot11 - dot01 * dot01);
	float u = (dot11 * dot02 - dot01 * dot12) * invDenom;
	float v = (dot00 * dot12 - dot01 * dot02) * invDenom;

	return (u > 0) && (v > 0) && (u + v < 1.0);

}

static long CGPointInCircumCircule(CGPoint d, CGPoint a, CGPoint b, CGPoint c)
{
	float m[3][3] = {
		{ a.x-d.x, b.x-d.x, c.x-d.x },
		{ a.y-d.y, b.y-d.y, c.y-d.y },
		{ CGPointDoubleDot(a) - CGPointDoubleDot(d), CGPointDoubleDot(b) - CGPointDoubleDot(d), CGPointDoubleDot(a) - CGPointDoubleDot(c), }
	};
	
	float cof[3] = {
		m[1][1]*m[2][2] -  m[2][1]*m[1][2],
		m[0][1]*m[2][2] -  m[2][1]*m[0][2],
		m[0][1]*m[1][2] -  m[1][1]*m[0][2]
	};
	float det = m[0][0]*cof[0] - m[1][0]*cof[1] + m[2][0]*cof[2];
	
	return det > 0;
}



static inline CGPoint cubicPointAtT(CGPoint p0, CGPoint p1, CGPoint p2, CGPoint p3, float t)
{
	float t1 = 1.0f-t;
	CGPoint p01 = CGPointAdd(CGPointScale(p0, t1), CGPointScale(p1, t));
	CGPoint p12 = CGPointAdd(CGPointScale(p1, t1), CGPointScale(p2, t));
	CGPoint p23 = CGPointAdd(CGPointScale(p2, t1), CGPointScale(p3, t));
	CGPoint p02 = CGPointAdd(CGPointScale(p01, t1), CGPointScale(p12, t));
	CGPoint p13 = CGPointAdd(CGPointScale(p12, t1), CGPointScale(p23, t));
	CGPoint p03 = CGPointAdd(CGPointScale(p02, t1), CGPointScale(p13, t));
	return p03;
}

static inline CGPoint quadraticPointAtT(CGPoint p0, CGPoint p1, CGPoint p2, float t)
{
	float t1 = 1.0f-t;
	CGPoint p01 = CGPointAdd(CGPointScale(p0, t1), CGPointScale(p1, t));
	CGPoint p12 = CGPointAdd(CGPointScale(p1, t1), CGPointScale(p2, t));
	CGPoint p02 = CGPointAdd(CGPointScale(p01, t1), CGPointScale(p12, t));
	return p02;
}

static inline CGPoint linearPointAtT(CGPoint p0, CGPoint p1, float t)
{
	float t1 = 1.0f-t;
	CGPoint p01 = CGPointAdd(CGPointScale(p0, t1), CGPointScale(p1, t));
	return p01;
}

CGPathRef CreateTransformedPathFromCGPath(CGPathRef cpath, CGAffineTransform m);

CGPoint CGPointTransform(CGPoint a, CGAffineTransform m);