/* Uses the sine and cosine addition formulae
to compute zOut = zIn + 2^(-i), cos(zOut) and
sin(zOut) from input values of zIn and sin(zIn) and
Cos(zIn).  The tables are precomputed values of the shifts 
and the cosines and sines of the shifts 2^(-i).
*/
#include "CORDIC.h"

void CORDICUpdate(float *c, float *s, float *z, int i){
	float cTemp,sTemp,zTemp;
	/* Table of values of 1/2^k k=1..30*/
	float Shifts[30]={
	0.5,
	0.25,
	0.125,
	0.0625,
	0.03125,
	0.015625,
	0.0078125,
	0.00390625,
	0.001953125,
	0.0009765625,
	0.00048828125,
	0.000244140625,
	0.0001220703125,
	0.00006103515625,
	0.000030517578125,
	0.0000152587890625,
	7.62939453125e-6,
	3.814697265625e-6,
	1.9073486328125e-6,
	9.5367431640625e-7,
	4.76837158203125e-7,
	2.384185791015625e-7,
	1.192092895507812e-7,
	5.960464477539062e-8,
	2.980232238769531e-8,
	1.490116119384765e-8,
	7.450580596923828e-9,
	3.725290298461914e-9,
	1.862645149230957e-9,
	9.313225746154785e-10
	};
	/* Table of cos(1/2^k) k=1..30*/
	float CosShifts[30]={
	0.8775825618903727,
	0.9689124217106447,
	0.992197667229329,
	0.9980475107000991,
	0.9995117584851364,
	0.9998779321710066,
	0.999969482577095,
	0.9999923706151699,
	0.9999980926519734,
	0.9999995231628796,
	0.9999998807907127,
	0.9999999701976777,
	0.9999999925494194,
	0.9999999981373548,
	0.9999999995343387,
	0.9999999998835846,
	0.9999999999708961,
	0.999999999992724,
	0.999999999998181,
	0.9999999999995452,
	0.9999999999998863,
	0.9999999999999715,
	0.9999999999999928,
	0.9999999999999982,
	0.9999999999999995,
	0.9999999999999998,
	1.,
	1.,
	1.,
	1.
	};
	/* Table of sin(1/2^k) k=1..30*/
	float SinShifts[30]={
	0.479425538604203,	
	0.2474039592545229,
	0.1246747333852276,
	0.0624593178423802,
	0.03124491398532608,
	0.01562436422488337,
	0.007812420527382831,
	0.003906240065900116,
	0.001953123758236804,
	0.0009765623447795783,
	0.0004882812305974466,
	0.0002441406225746808,
	0.0001220703121968351,
	0.00006103515621210439,
	0.00003051757812026305,
	0.00001525878906190788,
	7.629394531175985e-6,
	3.814697265615748e-6,
	1.907348632811343e-6,
	9.536743164061054e-7,
	4.768371582031069e-7,
	2.384185791015602e-7,
	1.192092895507809e-7,
	5.960464477539059e-8,
	2.98023223876953e-8,
	1.490116119384765e-8,
	7.450580596923828e-9,
	3.725290298461914e-9,
	1.862645149230957e-9,
	9.313225746154785e-10
	};
	/* Local copies of z, c,and s */
	zTemp=*z;
	cTemp=*c;
	sTemp=*s;
	/* Updating z -> z+Shifts[i] and updating sine (s) 
	and cosine (c) according to the trig addition formulae.
	*/
	*z=zTemp+Shifts[i];
	*c=cTemp*CosShifts[i]-sTemp*SinShifts[i];
	*s=cTemp*SinShifts[i]+sTemp*CosShifts[i];
};