//
//  trig.cpp : Basic trigonometry routines.
//
//! \file trig.cpp
//! \author J Vijn
//! \date 20080130 - 20080210
//
/* === NOTES ===
	Multiple atan2's present. Select the one you want.
	Best performance tends to be with atan2Lerp or possibly atan2Tonc.
	If your division really sucks, use atan2Cordic.
	
	This module still needs a header for some system-specific basics 
	before you can use it. Specifically, ALIGN and QDIV will needs some 
	extra effort.
*/

/*
	dsgmLib adaptations:
	
	- Only atan2Lerp was needed, all other trigonometry functions are
	already present in libnds and were removed from this library
	
	- Upgraded the code from C++ to C
*/

#include "DSGM.h"

// --------------------------------------------------------------------
// MACROS / INLINES
// --------------------------------------------------------------------

// Get the octant a coordinate pair is in. 
#define OCTANTIFY(_x, _y, _o)	do {							\
	int _t;	_o= 0;												\
	if(_y<  0)	{			 _x= -_x;   _y= -_y; _o += 4; }		\
	if(_x<= 0)	{ _t= _x;    _x=  _y;   _y= -_t; _o += 2; }		\
	if(_x<=_y)	{ _t= _y-_x; _x= _x+_y; _y=  _t; _o += 1; }		\
} while(0);

// Special-case division because I need a little more control 
// than divf32 offers
static inline int QDIV(int num, int den, const int bits) {
	while(REG_DIVCNT & DIV_BUSY);
	REG_DIVCNT = DIV_64_32;

	REG_DIV_NUMER = ((int64)num)<<bits;
	REG_DIV_DENOM_L = den;

	while(REG_DIVCNT & DIV_BUSY);

	return (REG_DIV_RESULT_L);
}

// --------------------------------------------------------------------
// CONSTANTS
// --------------------------------------------------------------------

static const uint ATAN_ONE = 0x1000, ATAN_FP = 12, ATAN_PI = BRAD_PI;
#define ATANLUT_STRIDE (ATAN_ONE / 0x80)
static const uint ATANLUT_STRIDE_SHIFT = 5;

// --------------------------------------------------------------------
// LUTS
// --------------------------------------------------------------------

//{{ATANLUT
extern const unsigned short atanLUT[130];

// Arctangens LUT. Interval: [0, 1] (one=128); PI=0x20000
const unsigned short atanLUT[130] ALIGN(4)=
{
	0x0000,0x0146,0x028C,0x03D2,0x0517,0x065D,0x07A2,0x08E7,
	0x0A2C,0x0B71,0x0CB5,0x0DF9,0x0F3C,0x107F,0x11C1,0x1303,
	0x1444,0x1585,0x16C5,0x1804,0x1943,0x1A80,0x1BBD,0x1CFA,
	0x1E35,0x1F6F,0x20A9,0x21E1,0x2319,0x2450,0x2585,0x26BA,
	0x27ED,0x291F,0x2A50,0x2B80,0x2CAF,0x2DDC,0x2F08,0x3033,
	0x315D,0x3285,0x33AC,0x34D2,0x35F6,0x3719,0x383A,0x395A,
	0x3A78,0x3B95,0x3CB1,0x3DCB,0x3EE4,0x3FFB,0x4110,0x4224,
	0x4336,0x4447,0x4556,0x4664,0x4770,0x487A,0x4983,0x4A8B,
// 64
	0x4B90,0x4C94,0x4D96,0x4E97,0x4F96,0x5093,0x518F,0x5289,
	0x5382,0x5478,0x556E,0x5661,0x5753,0x5843,0x5932,0x5A1E,
	0x5B0A,0x5BF3,0x5CDB,0x5DC1,0x5EA6,0x5F89,0x606A,0x614A,
	0x6228,0x6305,0x63E0,0x64B9,0x6591,0x6667,0x673B,0x680E,
	0x68E0,0x69B0,0x6A7E,0x6B4B,0x6C16,0x6CDF,0x6DA8,0x6E6E,
	0x6F33,0x6FF7,0x70B9,0x717A,0x7239,0x72F6,0x73B3,0x746D,
	0x7527,0x75DF,0x7695,0x774A,0x77FE,0x78B0,0x7961,0x7A10,
	0x7ABF,0x7B6B,0x7C17,0x7CC1,0x7D6A,0x7E11,0x7EB7,0x7F5C,
// 128
	0x8000,0x80A2
};
//}}ATANLUT

// Basic lookup+linear interpolation for atan2. 
// Returns [0,2pi), where pi ~ 0x4000.
uint atan2Lerp(int x, int y) {
	if(y == 0) return (x >= 0 ? 0 : BRAD_PI);
	
	int phi, fa, fb, h;
	uint t;
	
	OCTANTIFY(x, y, phi);
	phi *= BRAD_PI/4;
	
	t = QDIV(y, x, ATAN_FP);
	h = t % ATANLUT_STRIDE;
	fa = atanLUT[t/ATANLUT_STRIDE  ];
	fb = atanLUT[t/ATANLUT_STRIDE+1];
	
	return phi + ((fa + ((fb-fa)*h >> ATANLUT_STRIDE_SHIFT))>>3);
}