#ifndef __MATH__
#define __MATH__

#define FLT_EPSILON 1.192092896e-07

float ofMap(float value, float inputMin, float inputMax, float outputMin, float outputMax, int bClamp)
{
	if (abs(inputMin - inputMax) < FLT_EPSILON) {
		return outputMin;
	}
	else {
		float outVal = ((value - inputMin) / (inputMax - inputMin) * (outputMax - outputMin) + outputMin);
		if (bClamp > 0) {
			if (outputMax < outputMin) {
				if (outVal < outputMax) outVal = outputMax;
				else if (outVal > outputMin) outVal = outputMin;
			}
			else {
				if (outVal > outputMax) outVal = outputMax;
				else if (outVal < outputMin) outVal = outputMin;
			}
		}
		return outVal;
	}
}

float2 ofMap(float2 value, float2 inputMin, float2 inputMax, float2 outputMin, float2 outputMax, int bClamp)
{
	float x = ofMap(value.x, inputMin.x, inputMax.x, outputMin.x, outputMax.x, bClamp);
	float y = ofMap(value.y, inputMin.y, inputMax.y, outputMin.y, outputMax.y, bClamp);
	return float2(x, y);
}

float3 ofMap(float3 value, float3 inputMin, float3 inputMax, float3 outputMin, float3 outputMax, int bClamp)
{
	float x = ofMap(value.x, inputMin.x, inputMax.x, outputMin.x, outputMax.x, bClamp);
	float y = ofMap(value.y, inputMin.y, inputMax.y, outputMin.y, outputMax.y, bClamp);
	float z = ofMap(value.z, inputMin.z, inputMax.z, outputMin.z, outputMax.z, bClamp);
	return float3(x, y, z);
}

float2x2 RotationMatrix(float a)
{
	float2x2 rot = { cos(a), -sin(a), sin(a), cos(a) };
	return rot;
}

// implement from Rodrigues rotation formula
float3x3 RotationMatrix(float3 from, float3 to)
{
	float3 a = normalize(from);
	float3 b = normalize(to);
	float3 v = cross(a, b);
	float s = length(v);
	float c = dot(a, b);
	float3x3 W = { 0, -v.z, v.y, v.z, 0, -v.x, -v.y, v.x, 0 };
	float3x3 I = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
	float3x3 R = I + W * s + W * W * (1.0f - c);
	//float theta = acos( dot(a, b) );
	//float3x3 R = I + W * sin(theta) + W * W * (1.0f - cos(theta));
	return R;
}

float Rand(float2 co)
{
	return frac(sin(dot(co.xy, float2(12.9898, 78.233))) * 43758.5453);
}

float4 MakeQuaternion(float pitch, float roll, float yaw)
{
	float4 q;
	float t0 = cos(yaw * 0.5f);
	float t1 = sin(yaw * 0.5f);
	float t2 = cos(roll * 0.5f);
	float t3 = sin(roll * 0.5f);
	float t4 = cos(pitch * 0.5f);
	float t5 = sin(pitch * 0.5f);

	q.w = t0 * t2 * t4 + t1 * t3 * t5;
	q.x = t0 * t3 * t4 - t1 * t2 * t5;
	q.y = t0 * t2 * t5 + t1 * t3 * t4;
	q.z = t1 * t2 * t4 - t0 * t3 * t5;
	return q;
}

// Quaternion

float4 quat_from_axis_angle(float3 axis, float angle)
{
	float4 qr;
	float half_angle = (angle * 0.5);// *3.14159 / 180.0;
	qr.x = axis.x * sin(half_angle);
	qr.y = axis.y * sin(half_angle);
	qr.z = axis.z * sin(half_angle);
	qr.w = cos(half_angle);
	return qr;
}

float4 quat_conj(float4 q)
{
	return float4(-q.x, -q.y, -q.z, q.w);
}

float4 quat_mult(float4 q1, float4 q2)
{
	float4 qr;
	qr.x = (q1.w * q2.x) + (q1.x * q2.w) + (q1.y * q2.z) - (q1.z * q2.y);
	qr.y = (q1.w * q2.y) - (q1.x * q2.z) + (q1.y * q2.w) + (q1.z * q2.x);
	qr.z = (q1.w * q2.z) + (q1.x * q2.y) - (q1.y * q2.x) + (q1.z * q2.w);
	qr.w = (q1.w * q2.w) - (q1.x * q2.x) - (q1.y * q2.y) - (q1.z * q2.z);
	return qr;
}

float3 rotate_vertex_position(float3 position, float4 quat)
{
	return position + 2.0f * cross(quat.xyz, cross(quat.xyz, position) + quat.w * position);
}

float3 rotate_vertex_position(float3 position, float3 axis, float angle)
{
	float4 q = quat_from_axis_angle(axis, angle);
	return rotate_vertex_position(position, q);
	//float4 v = position.xyz;
	//return v + 2.0 * cross(q.xyz, cross(q.xyz, v) + q.w * v);
}

void calc_tangents(float3 vertices[4], float3 normals[4], float2 texcoords[4], out float4 tangents[4])
{
	//float4 tangents[3];
	float3 tan1[4];
	float3 tan2[4];
	int i;

	for (i = 0; i < 4; i++)
	{
		tan1[i] = tan2[i] = float3(0, 0, 0);
	}

	for (i = 0; i < 4 - 2; i++)
	{
		int i1 = 0;
		int i2 = i + 1;
		int i3 = i + 2;

		float3 v1 = vertices[i1];
		float3 v2 = vertices[i2];
		float3 v3 = vertices[i3];

		float2 w1 = texcoords[i1];
		float2 w2 = texcoords[i2];
		float2 w3 = texcoords[i3];

		float x1 = v2.x - v1.x;
		float x2 = v3.x - v1.x;
		float y1 = v2.y - v1.y;
		float y2 = v3.y - v1.y;
		float z1 = v2.z - v1.z;
		float z2 = v3.z - v1.z;

		float s1 = w2.x - w1.x;
		float s2 = w3.x - w1.x;
		float t1 = w2.y - w1.y;
		float t2 = w3.y - w1.y;

		float r = 1.0f / (s1 * t2 - s2 * t1);
		float3 sdir = float3((t2 * x1 - t1 * x2) * r, (t2 * y1 - t1 * y2) * r, (t2 * z1 - t1 * z2) * r);
		float3 tdir = float3((s1 * x2 - s2 * x1) * r, (s1 * y2 - s2 * y1) * r, (s1 * z2 - s2 * z1) * r);

		tan1[i1] += sdir;
		tan1[i2] += sdir;
		tan1[i3] += sdir;

		tan2[i1] += tdir;
		tan2[i2] += tdir;
		tan2[i3] += tdir;
	}	

	for (i = 0; i < 4; i++)
	{
		float3 n = normals[i];
		float3 t = tan1[i];

		tangents[i].xyz = tan1[i];
		tangents[i].w = (dot(cross(n, t), tan2[i]) < 0.0f) ? -1.0f : 1.0f;
	}
}

float4x4 quat_to_rotation_matrix(float4 q)
{
	return float4x4(
		1.0 - 2.0*q.y*q.y - 2.0*q.z*q.z, 2.0*q.x*q.y - 2.0*q.z*q.w,		  2.0*q.x*q.z + 2.0*q.y*q.w,	   0.0,
		2.0*q.x*q.y + 2.0*q.z*q.w,		 1.0 - 2.0*q.x*q.x - 2.0*q.z*q.z, 2.0*q.y*q.z - 2.0*q.x*q.w,	   0.0,
		2.0*q.x*q.z - 2.0*q.y*q.w,		 2.0*q.y*q.z + 2.0*q.x*q.w,		  1.0 - 2.0*q.x*q.x - 2.0*q.y*q.y, 0.0,
		0.0,							 0.0,							  0.0,							   1.0);
}

float4x4 make_translation_matrix(float3 trans)
{
	return float4x4(
		1, 0, 0, trans.x,
		0, 1, 0, trans.y,
		0, 0, 1, trans.z,
		0, 0, 0, 1
	);
}

float4x4 make_scaling_matrix(float s)
{
	return float4x4(
		s, 0, 0, 0,
		0, s, 0, 0,
		0, 0, s, 0,
		0, 0, 0, 1
	);
}

const float4x4 identity_matrix = {
	1, 0, 0, 0,
	0, 1, 0, 0,
	0, 0, 1, 0,
	0, 0, 0, 1
};

half3 cosine_gradient(half3 A, half3 B, half3 C, half3 D, float t)
{
	return saturate(A + B * cos(C * t + D));
}



#endif // __MATH__