#pragma once

/*
 * Copyright (c) 2020 STMicroelectronics.
 * All rights reserved.
 *
 * This software component is licensed by ST under BSD 3-Clause license,
 * the "License"; You may not use this file except in compliance with the
 * License. You may obtain a copy of the License at:
 *
 * https://opensource.org/licenses/BSD-3-Clause
 */

#define CORDIC_MAXITER 9
#define CORDIC_PI 0x10000000

static int CORDIC_ZTBL[] = { 0x04000000, 0x025C80A4, 0x013F670B, 0x00A2223B, 0x005161A8, 0x0028BAFC,
			     0x00145EC4, 0x000A2F8B, 0x000517CA, 0x00028BE6, 0x000145F3, 0x0000A2FA,
			     0x0000517D, 0x000028BE, 0x0000145F, 0x00000A30 };

inline static __attribute__((__unused__)) int fast_atan2(int y, int x)
{
	int k, tx, z = 0, fl = 0;

	if (x < 0) {
		fl = ((y > 0) ? +1 : -1);
		x = -x;
		y = -y;
	}

	for (k = 0; k < CORDIC_MAXITER; k++) {
		tx = x;

		if (y <= 0) {
			x -= (y >> k);
			y += (tx >> k);
			z -= CORDIC_ZTBL[k];
		} else {
			x += (y >> k);
			y -= (tx >> k);
			z += CORDIC_ZTBL[k];
		}
	}

	if (fl != 0) {
		z += fl * CORDIC_PI;
	}

	return z;
}