/*
 * Copyright (c) 2021, Jeffrey Lee
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met: 
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * 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.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "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 COPYRIGHT HOLDER OR CONTRIBUTORS 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.
 */
#ifndef GENMATH_H
#define GENMATH_H

#include <math.h>

template<typename T>
static inline T mla(T a,T b,T c)
{
	return a + b*c;
}

template<typename T>
static inline T mls(T a,T b,T c)
{
	return a - b*c;
}

template<typename T>
static inline T select(bool a,T b,T c)
{
	return (a ? b : c);
}

/* generic min/max */
template<typename T>
static inline T min(T const a,T const b)
{
	return select(a < b, a, b);
}

template<typename T>
static inline T max(T const a,T const b)
{
	return select(a > b, a, b);
}

/* utility */
template<typename T,typename T2,typename T3>
static inline T clamp(T const x,T2 const a,T3 const b)
{
	return min(max(x,a),b);
}

template<typename T>
static inline T saturate(T const a)
{
	return clamp(a,0.0f,1.0f);
}

static inline float fract(float f)
{
	return f-floor(f);
}

static inline float recp(float f)
{
	return 1/f;
}

static inline float SQRT(float a)
{
	return sqrtf(a);
}

static inline float inversesqrt(float f)
{
	return 1/SQRT(f);
}

static inline float length(float f)
{
	return f;
}

static inline float abs(float f)
{
	return fabs(f);
}

static inline float dot(float a,float b)
{
	return a*b;
}

static inline float floor(float a)
{
	return floor((double)a);
}

static inline float floor_tozero(float a)
{
	/* Isn't this wrong? floor() rounds to -inf? */
	return floor((double)a);
}

// Fraction, but only correct for positive numbers
template<typename T>
static inline T fract_positive(T f)
{
	return f-floor_tozero(f);
}

// https://en.wikipedia.org/wiki/Bhaskara_I%27s_sine_approximation_formula
template<typename T>
static inline T bhaskara_sine(T a)
{
	/* Input is expected to be a positive number, scaled so that 1 = 180 degrees */
	/* Select sign (and result scale) based on whether 0..180 or 180..360 */
	T sign = select(fract_positive(a*0.5) > 0.5,T(4),-T(4));
	T ang = fract_positive(a);
	ang = mls(ang,ang,ang);
	return (ang*sign)/(ang-1.25f);
}

template<typename T>
static inline T bhaskara_prep(T a)
{
	/* Scale so that 1 = 180 degrees */
	a = mla(T(-0.5),a,(float)M_1_PI);
	/* Ensure positive */
	return abs(a); /* sin -> cos, abs */
}

template<typename T>
static inline T bhaskara_prep_cos(T a)
{
	/* Scale so that 1 = 180 degrees */
	a *= M_1_PI;
	/* Ensure positive */
	return abs(a);
}

template<typename T>
static inline T fast_sin(T a)
{
	return bhaskara_sine(bhaskara_prep(a) + 0.5 /* cos -> sin */);
}

template<typename T>
static inline T fast_cos(T a)
{
	return bhaskara_sine(bhaskara_prep_cos(a) + 0.5 /* cos -> sin */);
}

template<typename T>
static inline T fast_tan(T a)
{
	/* Share the prep work */
	a = bhaskara_prep(a);
	T sine = bhaskara_sine(a + 0.5 /* cos -> sin */);
	T cosine = bhaskara_sine(a);
	return sine/cosine;
}

// http://nghiaho.com/?p=997
template<typename T>
static inline T fast_atan(T x)
{
	/* Ensure in range -1...1 */
	auto cmp = abs(x) > 1;
	x = select(cmp,recp(x),x);
	/* Calculate */
	T abs_x = abs(x);
	T temp = mla(T(0.2447),abs_x,0.0663f);
	temp = mla(-temp, temp, abs_x);
	T atn = x*M_PI_4;
	atn = mls(atn,x,temp);
	/* Invert if it was out of range on input */
	return select(cmp,-atn+select(x>=0,T(M_PI_2),-T(M_PI_2)),atn);
}

template<typename T>
static inline T fast_atan(T y,T x)
{
	T atn = fast_atan(y/x);
	/* Add PI if x<0, but keep result in range +/-PI
	   i.e. add PI if result is negative, subtract if positive */
	atn = select(x < 0,atn + select(atn < 0,T(M_PI),-T(M_PI)),atn);
	return atn;
}

#endif
