/*
	This file is part of Warzone 2100.
	Copyright (C) 1999-2004  Eidos Interactive
	Copyright (C) 2005-2020  Warzone 2100 Project

	Warzone 2100 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.

	Warzone 2100 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 Warzone 2100; if not, write to the Free Software
	Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
/*
 * Trig.c
 *
 * Trig lookup tables
 *
 */

#if defined( _MSC_VER )
	#pragma warning( disable : 4244 ) // warning C4244: conversion from 'type1' to 'type2', possible loss of data // FIXME?
#endif

/* Allow frame header files to be singly included */
#define FRAME_LIB_INCLUDE

#include "types.h"
#include "trig.h"
#include "crc.h"

#include <assert.h>
#include <stdlib.h>
// for M_PI
#if defined(WZ_CC_MSVC)
#define _USE_MATH_DEFINES
#endif

#include <math.h>
#include <algorithm>

static uint16_t trigSinTable[0x4001];
static uint16_t trigAtanTable[0x2001];

/* Initialise the Trig tables */
bool trigInitialise(void)
{
	uint32_t crc;
	uint32_t i;

	// Generate tables.
	STATIC_ASSERT(sizeof(trigSinTable) / sizeof(*trigSinTable) == 0x4001);
	for (i = 0; i != 0x4001; ++i)
	{
		trigSinTable[i] = (int)(0x10000 * sin(i * (M_PI / 0x8000)) + 0.5) - !!i; // -!!i = subtract 1, unless i == 0.
	}
	STATIC_ASSERT(sizeof(trigAtanTable) / sizeof(*trigAtanTable) == 0x2001);
	for (i = 0; i != 0x2001; ++i)
	{
		trigAtanTable[i] = (int)(0x8000 / M_PI * atan((double)i / 0x2000) + 0.5);
	}

	// Check tables are correct.
	crc = ~crcSumU16(0, trigSinTable, sizeof(trigSinTable) / sizeof(*trigSinTable));
	ASSERT(crc == 0x6D3C8DB5, "Bad trigSinTable CRC = 0x%08X, sin function is broken.", crc);
	crc = ~crcSumU16(0, trigAtanTable, sizeof(trigAtanTable) / sizeof(*trigAtanTable));
	ASSERT(crc == 0xD2797F85, "Bad trigAtanTable CRC = 0x%08X, atan function is broken.", crc);

	// Test large and small square roots.
	for (uint32_t test = 0x0000; test != 0x10000; ++test)
	{
		uint32_t lower = test * test;
		uint32_t upper = (test + 1) * (test + 1) - 1;
		ASSERT((uint32_t)iSqrt(lower) == test, "Sanity check failed, iSqrt(%" PRIu32") gave %" PRIu32" instead of %" PRIu32"!", lower, (uint32_t)iSqrt(lower), test);
		ASSERT((uint32_t)iSqrt(upper) == test, "Sanity check failed, iSqrt(%" PRIu32") gave %" PRIu32" instead of %" PRIu32"!", upper, (uint32_t)iSqrt(upper), test);
	}
	for (uint64_t test = 0xFFFE0000; test != 0x00020000; test = (test + 1) & 0xFFFFFFFF)
	{
		uint64_t lower = test * test;
		uint64_t upper = (test + 1) * (test + 1) - 1;
		ASSERT((uint32_t)i64Sqrt(lower) == test, "Sanity check failed, i64Sqrt(%" PRIu64") gave %" PRIu32" instead of %" PRIu64"!", lower, (uint32_t)i64Sqrt(lower), test);
		ASSERT((uint32_t)i64Sqrt(upper) == test, "Sanity check failed, i64Sqrt(%" PRIu64") gave %" PRIu32" instead of %" PRIu64"!", upper, (uint32_t)i64Sqrt(upper), test);
	}

	return true;
}

int32_t iSin(uint16_t a)
{
	int sign[4] = {1, 1, -1, -1};
	bool reverse[4] = {false, true, false, true};
	int      q = a >> 14;
	uint16_t r = a & 0x3FFF;
	uint16_t rvr = reverse[q] ? 0x4000 - r : r;
	return sign[q] * (trigSinTable[rvr] + !!rvr); // +!!rvr = add 1, unless rvr == 0.
}

int32_t iCos(uint16_t a)
{
	int sign[4] = {1, -1, -1, 1};
	bool reverse[4] = {true, false, true, false};
	int      q = a >> 14;
	uint16_t r = a & 0x3FFF;
	uint16_t rvr = reverse[q] ? 0x4000 - r : r;
	return sign[q] * (trigSinTable[rvr] + !!rvr); // +!!rvr = add 1, unless rvr == 0.
}

int32_t iSinR(uint16_t a, int32_t r)
{
	return static_cast<int32_t>(((int64_t)r * iSin(a)) / 65536);
}

int32_t iCosR(uint16_t a, int32_t r)
{
	return static_cast<int32_t>(((int64_t)r * iCos(a)) / 65536);
}

int32_t iSinSR(int32_t a, int32_t s, int32_t r)
{
	return static_cast<int32_t>(((int64_t)r * iSin(((int64_t)a << 16) / s)) / 65536);
}

int32_t iCosSR(int32_t a, int32_t s, int32_t r)
{
	return static_cast<int32_t>(((int64_t)r * iCos(((int64_t)a << 16) / s)) / 65536);
}

uint16_t iAtan2(int32_t s, int32_t c)
{
	uint16_t d = 0;         // Dummy initialisation.
	uint32_t j = 0, k = 0;  // Dummy initialisations.

	if (s == 0 && c == 0)
	{
		return 0;       // All return values equally valid, don't divide by 0.
	}

	switch (((s < 0) << 1) + (c < 0))
	{
	case 0: j =  s; k =  c; d = 0x0000; break;
	case 1: j = -c; k =  s; d = 0x4000; break;
	case 2: j =  c; k = -s; d = 0xC000; break;
	case 3: j = -s; k = -c; d = 0x8000; break;
	}
	return j < k ? d +          trigAtanTable[((int64_t)j * 0x2000 + k / 2) / k]
	       : d + 0x4000 - trigAtanTable[((int64_t)k * 0x2000 + j / 2) / j];
}

int32_t iSqrt(uint32_t n)
{
	uint32_t r = (uint32_t) sqrt((double)n);          // Calculate square root, rounded down. Excess precision does not change the result.

	// Check that we got the right result.
	ASSERT((int32_t)(r * r - n) <= 0 && (int32_t)((r + 1) * (r + 1) - n) > 0, "Too badly broken sqrt function, iSqrt(%u) = %u.", (unsigned)n, (unsigned)r);

	return r;
}

int32_t i64Sqrt(uint64_t n)
{
	uint64_t r;
	if (sizeof(void *) > 4)
	{
		r = (uint64_t) sqrt((double)n);          // Calculate square root, usually rounded down. Excess precision may result in rounding down instead of up, which is fine.
	}
	else
	{
		// Bad compiler workaround. On some compilers, sqrt() seems to have somehow been taking 64-bit doubles and returning 80-bit doubles, breaking expected rounding behaviour.
		r = (uint64_t) sqrtl(n);         // Calculate square root, usually rounded down. Excess precision may result in rounding down instead of up, which is fine.
	}

	// Correct for different rounding & precision
	// See: https://codereview.stackexchange.com/questions/69069/computing-the-square-root-of-a-64-bit-integer
	r = std::min(r, (uint64_t)UINT32_MAX);
	while (r * r > n)
		r--;
	while (n - r * r > r * 2)
		r++;

	// Check that we got the right result.
	ASSERT((int64_t)(r * r - n) <= 0 && (int64_t)((r + 1) * (r + 1) - n) > 0, "Too badly broken sqrt function, i64Sqrt(%" PRIu64") = %" PRIu64".", n, r);

	return static_cast<int32_t>(r);
}

int32_t iHypot(int32_t x, int32_t y)
{
	return i64Sqrt((uint64_t)x * x + (uint64_t)y * y); // Casting from int32_t to uint64_t does sign extend.
}

int32_t iHypot3(int32_t x, int32_t y, int32_t z)
{
	return i64Sqrt((uint64_t)x * x + (uint64_t)y * y + (uint64_t)z * z); // Casting from int32_t to uint64_t does sign extend.
}

// For testing above functions.
#if 0
int main()
{
	srand(time(NULL));
	for (unsigned i = 0; i != 1000000; ++i)
	{
		int32_t s = rand() % 2 ? rand() + rand() + rand() + rand() : rand() % 101 - 50;
		int32_t c = rand() % 2 ? rand() + rand() + rand() + rand() : rand() % 101 - 50;
		int a1 = uint16_t(round(32768 / M_PI * atan2(s, c))), a2 = iAtan2(s, c);
		if (a1 + 1 < a2 || a1 > a2 + 1)
		{
			std::printf("32768/π atan2(%d, %d) = %d, iAtan2() = %d\n", s, c, a1, a2);
		}
	}
	for (unsigned a = 0; a < 65536; ++a)
	{
		int s1 = round(65536 * sin(a * M_PI / 32768)), s2 = iSin(a);
		int c1 = round(65536 * cos(a * M_PI / 32768)), c2 = iCos(a);
		if (s1 != s2 || c1 != c2)
		{
			std::printf("a = %d, sin = %d, %d, cos = %d, %d\n", a, s1, s2, c1, c2);
		}
	}
}
#endif