#include "header.h"

double CORDIC(double theta_in)
{
	//Note we have been given assurance that 0<theta_in<1

/*
//Incantation of the double's modulo.
	//uncomment to invoke
	double divisor=0,quotient=0,remainder=0;
	divisor = pow(.5,i);
	quotient=(int)(in/divisor);
	remainder = in - (float) (quotient*divisor);
 	if (remainder > divisor/2)
 //warning: this is an unstable incantation.  Direct invokation may cause impish behavior.
 //But it may be needed, if we are to handle larger theta_in's...
 */

	
        //Quadrants: which functions are positive:
	//1   2   3   4
	//All Sin Tan Cos

	//Trigonometric Rules
        //sin(a+-b)=sin(a)cos(b)+-cos(a)sin(b)
	//cos(a+-b)=cos(a)cos(b)-+sin(a)sin(b)
	//tan(a+-b)=(tan(a)+-tan(b))/(1-+tan(a)tan(b))

	double scratch;	//For holding sintheta while preforming costheta update
	int i=0;
	unsigned int theta_binary=0,mask=1;
	double sintheta=0,costheta=1;//,tantheta=0;

	theta_binary=ftob(theta_in);	//Get the binary control code from theta

	for (i=(entries-1); i>=0; i--)//From the smallest to the largest
//	for (i=0; i<entries;i++)
	{
		if ( mask<<i & theta_binary)//If control says "go"
		{
		scratch=(sintheta*costable[i])+(costheta*sintable[i]);
		costheta=(costheta*costable[i])-(sintheta*sintable[i]);
		sintheta=scratch;
//		tantheta=(tantheta+tantable[i])/(1- (tantheta*tantable[i]));
		}
	}
	return sintheta;	//return whatever we've found.
};