#include "unittest/TestSuite.h"
#include "gamemath/Scalar.h"

static void testClamp(void) {
	float result;
	
	result = clampf(1.0f, 0.0f, 2.0f);
	TestCase_assertFloatEqual(result, 1.0f);
	result = clampf(0.0f, 0.0f, 2.0f);
	TestCase_assertFloatEqual(result, 0.0f);
	result = clampf(2.0f, 0.0f, 2.0f);
	TestCase_assertFloatEqual(result, 2.0f);
	result = clampf(3.0f, 0.0f, 2.0f);
	TestCase_assertFloatEqual(result, 2.0f);
	result = clampf(-5.0f, 0.0f, 2.0f);
	TestCase_assertFloatEqual(result, 0.0f);
	
	result = clampf(0.0f, -1.0f, 1.0f);
	TestCase_assertFloatEqual(result, 0.0f);
	result = clampf(-3.0f, -1.0f, 1.0f);
	TestCase_assertFloatEqual(result, -1.0f);
	result = clampf(5000.0f, -1.0f, 1.0f);
	TestCase_assertFloatEqual(result, 1.0f);
}

static void testInterpolate(void) {
	float result;
	
	result = interpolatef(0.0f, 2.0f, 0.0f);
	TestCase_assertFloatEqual(result, 0.0f);
	result = interpolatef(0.0f, 2.0f, 0.5f);
	TestCase_assertFloatEqual(result, 1.0f);
	result = interpolatef(0.0f, 2.0f, 1.0f);
	TestCase_assertFloatEqual(result, 2.0f);
	result = interpolatef(0.0f, 2.0f, 2.0f);
	TestCase_assertFloatEqual(result, 4.0f);
	result = interpolatef(0.0f, 2.0f, -1.0f);
	TestCase_assertFloatEqual(result, -2.0f);
	
	result = interpolatef(3.0f, -3.0f, 0.0f);
	TestCase_assertFloatEqual(result, 3.0f);
	result = interpolatef(3.0f, -3.0f, 1.0f);
	TestCase_assertFloatEqual(result, -3.0f);
}

static void testRoundpositive(void) {
	float result;
	
	result = roundpositivef(0.0f);
	TestCase_assertFloatEqual(result, 0.0f);
	result = roundpositivef(0.49999f);
	TestCase_assertFloatEqual(result, 0.0f);
	result = roundpositivef(0.5f);
	TestCase_assertFloatEqual(result, 1.0f);
	result = roundpositivef(0.50001f);
	TestCase_assertFloatEqual(result, 1.0f);
	result = roundpositivef(1.5f);
	TestCase_assertFloatEqual(result, 2.0f);
	result = roundpositivef(-0.49999f);
	TestCase_assertFloatEqual(result, 0.0f);
	result = roundpositivef(-0.5f);
	TestCase_assertFloatEqual(result, 0.0f);
	result = roundpositivef(-0.50001f);
	TestCase_assertFloatEqual(result, -1.0f);
	result = roundpositivef(-1.5f);
	TestCase_assertFloatEqual(result, -1.0f);
}

static void testMinMax(void) {
	float result;
	
	result = minf(0.0f, 0.0f);
	TestCase_assertFloatEqual(result, 0.0f);
	result = minf(1.0f, 1.0f);
	TestCase_assertFloatEqual(result, 1.0f);
	result = minf(0.0f, 1.0f);
	TestCase_assertFloatEqual(result, 0.0f);
	result = minf(1.0f, 0.0f);
	TestCase_assertFloatEqual(result, 0.0f);
	result = minf(-1.0f, -2.0f);
	TestCase_assertFloatEqual(result, -2.0f);
	result = minf(20.0f, 50.0f);
	TestCase_assertFloatEqual(result, 20.0f);
	
	result = maxf(0.0f, 0.0f);
	TestCase_assertFloatEqual(result, 0.0f);
	result = maxf(1.0f, 1.0f);
	TestCase_assertFloatEqual(result, 1.0f);
	result = maxf(0.0f, 1.0f);
	TestCase_assertFloatEqual(result, 1.0f);
	result = maxf(1.0f, 0.0f);
	TestCase_assertFloatEqual(result, 1.0f);
	result = maxf(-1.0f, -2.0f);
	TestCase_assertFloatEqual(result, -1.0f);
	result = maxf(20.0f, 50.0f);
	TestCase_assertFloatEqual(result, 50.0f);
}

#define TOLERANCE 0.00001f

static void testAccelerate(void) {
	float velocity, position;
	
	// Accelerate exactly to 1 in 1 second
	velocity = position = 0.0f;
	acceleratef(&velocity, &position, 1.0f, 1.0f, 1.0f);
	TestCase_assertFloatApproximate(velocity, 1.0f, TOLERANCE);
	TestCase_assertFloatApproximate(position, 0.5f, TOLERANCE);
	
	// Accelerate exactly to -1 in 1 second
	velocity = position = 0.0f;
	acceleratef(&velocity, &position, 1.0f, -1.0f, 1.0f);
	TestCase_assertFloatApproximate(velocity, -1.0f, TOLERANCE);
	TestCase_assertFloatApproximate(position, -0.5f, TOLERANCE);
	
	// Accelerate exactly to 0.5 in 1 second
	velocity = position = 0.0f;
	acceleratef(&velocity, &position, 0.5f, 1.0f, 1.0f);
	TestCase_assertFloatApproximate(velocity, 0.5f, TOLERANCE);
	TestCase_assertFloatApproximate(position, 0.25f, TOLERANCE);
	
	// Accelerate to 1 in three discrete timesteps of different sizes
	velocity = position = 0.0f;
	acceleratef(&velocity, &position, 1.0f, 1.0f, 0.5f);
	TestCase_assertFloatApproximate(velocity, 0.5f, TOLERANCE);
	TestCase_assertFloatApproximate(position, 0.125f, TOLERANCE);
	
	acceleratef(&velocity, &position, 1.0f, 1.0f, 0.3f);
	TestCase_assertFloatApproximate(velocity, 0.8f, TOLERANCE);
	TestCase_assertFloatApproximate(position, 0.32f, TOLERANCE);
	
	acceleratef(&velocity, &position, 1.0f, 1.0f, 0.2f);
	TestCase_assertFloatApproximate(velocity, 1.0f, TOLERANCE);
	TestCase_assertFloatApproximate(position, 0.5f, TOLERANCE);
	
	// Accelerate from 2 toward 1 in 1 second
	velocity = 2.0f;
	position = 0.0f;
	acceleratef(&velocity, &position, 1.0f, 1.0f, 1.0f);
	TestCase_assertFloatApproximate(velocity, 1.0f, TOLERANCE);
	TestCase_assertFloatApproximate(position, 1.5f, TOLERANCE);
	
	// Accelerate to 0.5 in 0.5 seconds, then hold at 0.5 for the other half of the timestep
	velocity = position = 0.0f;
	acceleratef(&velocity, &position, 1.0f, 0.5f, 1.0f);
	TestCase_assertFloatApproximate(velocity, 0.5f, TOLERANCE);
	TestCase_assertFloatApproximate(position, 0.375f, TOLERANCE);
	
	// Accelerate to -0.5 in 0.5 seconds, then hold at -0.5 for the other half of the timestep
	velocity = position = 0.0f;
	acceleratef(&velocity, &position, 1.0f, -0.5f, 1.0f);
	TestCase_assertFloatApproximate(velocity, -0.5f, TOLERANCE);
	TestCase_assertFloatApproximate(position, -0.375f, TOLERANCE);
	
	// Don't accelerate, but still apply velocity when already at targetVelocity
	velocity = 1.0f;
	position = 0.0f;
	acceleratef(&velocity, &position, 1.0f, 1.0f, 1.0f);
	TestCase_assertFloatApproximate(velocity, 1.0f, TOLERANCE);
	TestCase_assertFloatApproximate(position, 1.0f, TOLERANCE);
}

static void testMatchDiagonalFraction(void) {
	float x, y;
	
	x = 3.25f;
	y = 4.5f;
	matchDiagonalFractionf(&x, &y, 1.0f, 1.0f, 1.0f);
	TestCase_assertFloatApproximate(x, 3.5f, TOLERANCE);
	TestCase_assertFloatApproximate(y, 4.5f, TOLERANCE);
	x = 3.75f;
	y = 4.5f;
	matchDiagonalFractionf(&x, &y, 1.0f, 1.0f, 1.0f);
	TestCase_assertFloatApproximate(x, 3.75f, TOLERANCE);
	TestCase_assertFloatApproximate(y, 4.75f, TOLERANCE);
	
	x = 3.25f;
	y = 4.5f;
	matchDiagonalFractionf(&x, &y, 1.0f, -1.0f, 1.0f);
	TestCase_assertFloatApproximate(x, 3.5f, TOLERANCE);
	TestCase_assertFloatApproximate(y, 4.5f, TOLERANCE);
	x = 3.75f;
	y = 4.5f;
	matchDiagonalFractionf(&x, &y, 1.0f, -1.0f, 1.0f);
	TestCase_assertFloatApproximate(x, 3.75f, TOLERANCE);
	TestCase_assertFloatApproximate(y, 4.25f, TOLERANCE);
	
	x = 3.25f;
	y = 4.5f;
	matchDiagonalFractionf(&x, &y, -1.0f, -1.0f, 1.0f);
	TestCase_assertFloatApproximate(x, 3.25f, TOLERANCE);
	TestCase_assertFloatApproximate(y, 4.25f, TOLERANCE);
	x = 3.75f;
	y = 4.5f;
	matchDiagonalFractionf(&x, &y, -1.0f, -1.0f, 1.0f);
	TestCase_assertFloatApproximate(x, 3.5f, TOLERANCE);
	TestCase_assertFloatApproximate(y, 4.5f, TOLERANCE);
	
	x = 3.25f;
	y = 4.5f;
	matchDiagonalFractionf(&x, &y, -1.0f, 1.0f, 1.0f);
	TestCase_assertFloatApproximate(x, 3.25f, TOLERANCE);
	TestCase_assertFloatApproximate(y, 4.75f, TOLERANCE);
	x = 3.75f;
	y = 4.5f;
	matchDiagonalFractionf(&x, &y, -1.0f, 1.0f, 1.0f);
	TestCase_assertFloatApproximate(x, 3.5f, TOLERANCE);
	TestCase_assertFloatApproximate(y, 4.5f, TOLERANCE);
	
	x = -10.625f;
	y = 6.125f;
	matchDiagonalFractionf(&x, &y, 5.0f, 5.0f, 1.0f);
	TestCase_assertFloatApproximate(x, -10.625f, TOLERANCE);
	TestCase_assertFloatApproximate(y, 6.625f, TOLERANCE);
	
	x = 1.125f;
	y = 1.75f;
	matchDiagonalFractionf(&x, &y, 10.0f, 10.0f, 0.5f);
	TestCase_assertFloatApproximate(x, 1.25f, TOLERANCE);
	TestCase_assertFloatApproximate(y, 1.75f, TOLERANCE);
	
	x = 1.875f;
	y = 1.75f;
	matchDiagonalFractionf(&x, &y, 0.25f, -0.25f, 0.5f);
	TestCase_assertFloatApproximate(x, 1.875f, TOLERANCE);
	TestCase_assertFloatApproximate(y, 1.625f, TOLERANCE);
	
	x = 3.25f;
	y = 4.5f;
	matchDiagonalFractionf(&x, &y, 0.0f, 1.0f, 1.0f);
	TestCase_assertFloatApproximate(x, 3.25f, TOLERANCE);
	TestCase_assertFloatApproximate(y, 4.5f, TOLERANCE);
	
	x = 3.25f;
	y = 4.5f;
	matchDiagonalFractionf(&x, &y, 1.0f, 0.0f, 1.0f);
	TestCase_assertFloatApproximate(x, 3.25f, TOLERANCE);
	TestCase_assertFloatApproximate(y, 4.5f, TOLERANCE);
	
	x = 3.25f;
	y = 4.5f;
	matchDiagonalFractionf(&x, &y, 1.0f, 1.0f, 0.0f);
	TestCase_assertFloatApproximate(x, 3.25f, TOLERANCE);
	TestCase_assertFloatApproximate(y, 4.5f, TOLERANCE);
}

static void testRampToward(void) {
	float result = rampTowardf(0.0f, 2.0f, 1.0f, 0.5f);
	TestCase_assertFloatApproximate(result, 0.5f, TOLERANCE);
	result = rampTowardf(0.0f, 0.25f, 1.0f, 0.5f);
	TestCase_assertFloatEqual(result, 0.25f);
	result = rampTowardf(0.0f, -2.0f, 1.0f, 0.5f);
	TestCase_assertFloatApproximate(result, -0.5f, TOLERANCE);
	result = rampTowardf(0.0f, -0.25f, 1.0f, 0.5f);
	TestCase_assertFloatEqual(result, -0.25f);
	
	result = rampTowardf(1.0f, 2.0f, 0.25f, 0.5f);
	TestCase_assertFloatApproximate(result, 1.125f, TOLERANCE);
	result = rampTowardf(1.0f, 0.0f, 0.25f, 0.5f);
	TestCase_assertFloatApproximate(result, 0.875f, TOLERANCE);
	result = rampTowardf(1.0f, 1.0f, 0.25f, 0.5f);
	TestCase_assertFloatEqual(result, 1.0f);
}

TEST_SUITE(ScalarTest,
           testClamp,
           testInterpolate,
           testRoundpositive,
           testMinMax,
           testAccelerate,
           testMatchDiagonalFraction,
           testRampToward)