#include <stdio.h>

typedef __signed__ char __s8;
typedef unsigned char __u8;
typedef __signed__ int __s32;
typedef unsigned int __u32;


typedef __u8  u8;
typedef __s8  s8;
typedef __u32 u32;
typedef __s32 s32;


#define U32_MAX		((u32)~0U)
#define S32_MAX		((s32)(U32_MAX>>1))
#define S32_MIN		((s32)(-S32_MAX - 1))

struct cordic_iq {
	s32 i;
	s32 q;
};

struct b43_c32 { s32 i, q; };

#define CORDIC_ANGLE_GEN	39797
#define CORDIC_PRECISION_SHIFT	16
#define CORDIC_NUM_ITER	(CORDIC_PRECISION_SHIFT + 2)
#define CORDIC_FIXED(X)	((s32)((X) << CORDIC_PRECISION_SHIFT))
#define CORDIC_FLOAT(X)	(((X) >= 0) \
		? ((((X) >> (CORDIC_PRECISION_SHIFT - 1)) + 1) >> 1) \
		: -((((-(X)) >> (CORDIC_PRECISION_SHIFT - 1)) + 1) >> 1))


static const s32 arctan_table[] = {
	2949120,
	1740967,
	919879,
	466945,
	234379,
	117304,
	58666,
	29335,
	14668,
	7334,
	3667,
	1833,
	917,
	458,
	229,
	115,
	57,
	29
};

struct b43_c32 b43_cordic(int theta)
{
	static const u32 arctg[] = {
		2949120, 1740967, 919879, 466945, 234379, 117304,
		  58666,   29335,  14668,   7334,   3667,   1833,
		    917,     458,    229,    115,     57,     29,
	};
	u8 i;
	s32 tmp;
	s8 signx = 1;
	u32 angle = 0;
	struct b43_c32 ret = { .i = 39797, .q = 0, };

	while (theta > (180 << 16))
		theta -= (360 << 16);
	while (theta < -(180 << 16))
		theta += (360 << 16);

	if (theta > (90 << 16)) {
		theta -= (180 << 16);
		signx = -1;
	} else if (theta < -(90 << 16)) {
		theta += (180 << 16);
		signx = -1;
	}
//	printf("b43: theta: %d", theta);

	for (i = 0; i <= 17; i++) {
		if (theta > angle) {
			tmp = ret.i - (ret.q >> i);
			ret.q += ret.i >> i;
			ret.i = tmp;
			angle += arctg[i];
		} else {
			tmp = ret.i + (ret.q >> i);
			ret.q -= ret.i >> i;
			ret.i = tmp;
			angle -= arctg[i];
		}
	}

	ret.i *= signx;
	ret.q *= signx;

	return ret;
}

struct cordic_iq cordic_calc_iq(s32 theta)
{
	struct cordic_iq coord;
	s32 angle, valtmp;
	unsigned iter;
	int signx = 1;
	int signtheta;

	coord.i = CORDIC_ANGLE_GEN;
	coord.q = 0;
	angle = 0;

	theta = CORDIC_FIXED(theta);
	signtheta = (theta < 0) ? -1 : 1;
	theta = ((theta + CORDIC_FIXED(180) * signtheta) % CORDIC_FIXED(360)) -
		CORDIC_FIXED(180) * signtheta;

	if (CORDIC_FLOAT(theta) > 90) {
		theta -= CORDIC_FIXED(180);
		signx = -1;
	} else if (CORDIC_FLOAT(theta) < -90) {
		theta += CORDIC_FIXED(180);
		signx = -1;
	}
//	printf("  lib theta: %d", theta);

	for (iter = 0; iter < CORDIC_NUM_ITER; iter++) {
		if (theta > angle) {
			valtmp = coord.i - (coord.q >> iter);
			coord.q += (coord.i >> iter);
			angle += arctan_table[iter];
		} else {
			valtmp = coord.i + (coord.q >> iter);
			coord.q -= (coord.i >> iter);
			angle -= arctan_table[iter];
		}
		coord.i = valtmp;
	}

	coord.i *= signx;
	coord.q *= signx;
	return coord;
}

int main() {
	struct cordic_iq lib_iq;
	struct b43_c32 b43_iq;

	s32 di = 0;
	s32 dq = 0;

	//for (s32 theta = S32_MIN; theta <= S32_MAX; theta++) {
	printf("theta, lib_i, b43_i, lib_q, b43_q\n");
	for (s32 theta = -360; theta <= 360; theta++) {
		b43_iq = b43_cordic(CORDIC_FIXED(theta));

		lib_iq = cordic_calc_iq(theta);

		di = lib_iq.i - b43_iq.i;
		dq = lib_iq.q - b43_iq.q;

#if 0
		if (di != 0 || dq != 0) {
			printf("th: %d i:%d - %d, q: %d - %d\n", theta, lib_iq.i, b43_iq.i, lib_iq.q, b43_iq.q);
		} else {
			printf("theta: %d - OK!\n", theta);
		}
#endif
		printf("%d, %d, %d, %d, %d\n", theta, lib_iq.i, b43_iq.i, lib_iq.q, b43_iq.q);

	}
}