/*
CORDIC atan2 implementation

Copyright 2019 Ahmet Inan <inan@aicodix.de>
*/

#pragma once

/*
for ((i = 0; i < 16; ++i)) ; do
	echo -n "int(0.5+FAC*"
	echo "scale=100; a((1/2)^$i)/(4*a(1))" | bc -l | head -n1 | sed 's/\\/L),/'
done
*/

namespace DSP {

template <typename TYPE>
class CORDIC
{
	static const int BYTES = sizeof(TYPE);
	static const int BITS = 8 * BYTES;
	static_assert(BITS <= 16, "LUT not big enough");
	static const int FAC = BYTES << BITS;
	static constexpr int LUT[16] = {
		int(0.5+FAC*.2500000000000000000000000000000000000000000000000000000000000000000L),
		int(0.5+FAC*.1475836176504332741754010762247405259511345238869178945999223128627L),
		int(0.5+FAC*.0779791303773693254605128897731301351165246187810070349907614355625L),
		int(0.5+FAC*.0395834241605655420108516713400380263836230512014484657091255843474L),
		int(0.5+FAC*.0198685243055408390593598828581045592395809198311117360197658161834L),
		int(0.5+FAC*.0099439478235892739286124987559949706675355099351305855673205197301L),
		int(0.5+FAC*.0049731872789504128535623156837751800756022858309425918885230220925L),
		int(0.5+FAC*.0024867453936697392949686323634656804594951905170750175789691381407L),
		int(0.5+FAC*.0012433916687141004173529641246144055092826557912518235730426698546L),
		int(0.5+FAC*.0006216982059233715959748655390818054931061630817558971376245115402L),
		int(0.5+FAC*.0003108493994100203787468007466901774427133407538770647518959302312L),
		int(0.5+FAC*.0001554247367611315255509951068712763232641191255978216125229683284L),
		int(0.5+FAC*.0000777123730125834146038201984686638046103667177724187710747868218L),
		int(0.5+FAC*.0000388561870852939914306938515989779351910341528053916759630446151L),
		int(0.5+FAC*.0000194280936150222836580154085598088977275755448309525831225639103L),
		int(0.5+FAC*.0000097140468165580528976715963565423783044063210250363625335628380L),
	};
public:
	static TYPE atan2(TYPE y, TYPE x)
	{
		int angle = 0;
		int real = x << 1;
		int imag = y << 1;
		if (x < 0)
			real = -real;
		for (int i = 0; i < BITS; ++i) {
			int re = real;
			int im = imag;
			if (imag < 0) {
				angle -= LUT[i];
				re -= imag >> i;
				im += real >> i;
			} else {
				angle += LUT[i];
				re += imag >> i;
				im -= real >> i;
			}
			real = re; imag = im;
		}
		if (x < 0)
			angle = FAC - angle;
		return angle >> BYTES;
	}
};

}