/*
Copyright (C) 1996-2001 Id Software, Inc.
Copyright (C) 2002-2009 John Fitzgibbons and others
Copyright (C) 2007-2008 Kristian Duske
Copyright (C) 2010-2014 QuakeSpasm developers

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 2
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, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

*/

#ifndef spingle__mathlib_h
#define spingle__mathlib_h

#include <math.h>

#define PI 3.14159265358979323846
#define PI_DIV_180 (PI / 180.0) //johnfitz

#define Q_rint(x) _Generic((x), float: Q_rintf, double: Q_rintd)(x)

struct mplane_s;

extern vec3_t vec3_origin;

void TurnVector(vec3_t out, const vec3_t forward, const vec3_t side, float angle);  //johnfitz

static inline int32_t Q_rintf(float x)
{
	return x > 0 ? (int32_t)(x + 0.5) : (int32_t)(x - 0.5);
}

static inline int32_t Q_rintd(double x)
{
	return x > 0 ? (int32_t)(x + 0.5) : (int32_t)(x - 0.5);
}

static inline vec_t DotProduct(vec3_t const v1, vec3_t const v2)
{
	return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
}

static inline vec_t DoublePrecisionDotProduct(vec3_t const v1, vec3_t const v2)
{
	return (double)v1[0] * v2[0] + (double)v1[1] * v2[1] + (double)v1[2] * v2[2];
}
static inline void VectorSubtract(vec3_t const veca, vec3_t const vecb, vec3_t out)
{
	out[0] = veca[0] - vecb[0];
	out[1] = veca[1] - vecb[1];
	out[2] = veca[2] - vecb[2];
}

static inline void VectorAdd(vec3_t const veca, vec3_t const vecb, vec3_t out)
{
	out[0] = veca[0] + vecb[0];
	out[1] = veca[1] + vecb[1];
	out[2] = veca[2] + vecb[2];
}

static inline void VectorCopy(vec3_t const in, vec3_t out)
{
	out[0] = in[0];
	out[1] = in[1];
	out[2] = in[2];
}

static inline void VectorInverse(vec3_t v)
{
	v[0] = -v[0];
	v[1] = -v[1];
	v[2] = -v[2];
}

static inline void VectorScale(vec3_t const in, vec_t scale, vec3_t out)
{
	out[0] = in[0] * scale;
	out[1] = in[1] * scale;
	out[2] = in[2] * scale;
}

//johnfitz -- courtesy of lordhavoc
// QS: To avoid strict aliasing violations, use a float/int32_t union instead of type punning.
// [agw] NO YOU FOOL THAT'S UNDEFINED BEHAVIOUR
static inline void VectorNormalizeFast(vec3_t v)
{
	union { float f; uint32_t i; } y, number;
	number.f = DotProduct(v, v);
	if(number.f != 0.0)
	{
		y.i = 0x5f3759df - (number.i >> 1);
		y.f = y.f * (1.5f - (number.f * 0.5f * y.f * y.f));
		VectorScale(v, y.f, v);
	}
}

static inline int32_t VectorCompare(vec3_t v1, vec3_t v2)
{
	int32_t i;

	for(i = 0 ; i < 3 ; i++)
		if(v1[i] != v2[i])
			return 0;

	return 1;
}

static inline void VectorMA(vec3_t veca, float scale, vec3_t vecb, vec3_t vecc)
{
	vecc[0] = veca[0] + scale * vecb[0];
	vecc[1] = veca[1] + scale * vecb[1];
	vecc[2] = veca[2] + scale * vecb[2];
}

static inline void CrossProduct(vec3_t v1, vec3_t v2, vec3_t cross)
{
	cross[0] = v1[1] * v2[2] - v1[2] * v2[1];
	cross[1] = v1[2] * v2[0] - v1[0] * v2[2];
	cross[2] = v1[0] * v2[1] - v1[1] * v2[0];
}

static inline vec_t VectorLength(vec3_t v)
{
	return sqrt(DotProduct(v, v));
}

static inline float VectorNormalize(vec3_t v)
{
	float   length, ilength;

	length = sqrt(DotProduct(v, v));

	if(length)
	{
		ilength = 1 / length;
		v[0] *= ilength;
		v[1] *= ilength;
		v[2] *= ilength;
	}

	return length;

}

static inline int32_t Q_log2(int32_t val)
{
	int32_t answer = 0;
	while(val >>= 1)
		answer++;
	return answer;
}

//johnfitz -- the opposite of AngleVectors.  this takes forward and generates pitch yaw roll
//TODO: take right and up vectors to properly set yaw and roll
static inline void VectorAngles(const vec3_t forward, vec3_t angles)
{
	vec3_t temp;

	temp[0] = forward[0];
	temp[1] = forward[1];
	temp[2] = 0;
	angles[PITCH] = -atan2(forward[2], VectorLength(temp)) / PI_DIV_180;
	angles[YAW] = atan2(forward[1], forward[0]) / PI_DIV_180;
	angles[ROLL] = 0;
}

void R_ConcatRotations(float in1[3][3], float in2[3][3], float out[3][3]);
void R_ConcatTransforms(float in1[3][4], float in2[3][4], float out[3][4]);

void FloorDivMod(double numer, double denom, int32_t *quotient,
                 int32_t *rem);
int32_t GreatestCommonDivisor(int32_t i1, int32_t i2);

void AngleVectors(vec3_t angles, vec3_t forward, vec3_t right, vec3_t up);
int32_t BoxOnPlaneSide(vec3_t emins, vec3_t emaxs, struct mplane_s *plane);
float   anglemod(float a);


#define BOX_ON_PLANE_SIDE(emins, emaxs, p) \
	(((p)->type < 3) \
		? (((p)->dist <= (emins)[(p)->type]) \
			? 1 \
			: (((p)->dist >= (emaxs)[(p)->type]) ? 2 : 3)) \
		: BoxOnPlaneSide((emins), (emaxs), (p)))

#endif