/* implementation for class Dformat */
// ACCURACY and TABLE_SIZE are defined in ROM.h

#include "Cordic.h"
#include "ROM.h"
#include "Dformat.h"
#include <iostream>
#include <iomanip>
#include <fstream>
#include <cmath>

using namespace std;

// default constructor------------------------------------------------
Cordic::Cordic( const Dformat& CDf,int m,char dv_n,
			    double x0,double y0,double z0 )
{
	set( CDf,m,dv_n,x0,y0,z0 );
}

// public interfaces----------------------------------------------------

//setting functions
void Cordic::set( const Dformat& CDf, const int m,const char dv_n,
				  const double x0,const double y0,const double z0 )
{
	CORDIC_Dfmt=CDf;

	set( m );
	set( dv_n );
	set( x0,y0,z0 );
}

void Cordic::set(const unsigned int M, const unsigned int N, 
				 const int m, const char dv_n, 
				 const double x0, const double y0, const double z0)
{
	const Dformat CDf( M,N );// create an constant Dformat object
	set( CDf,m,dv_n,x0,y0,z0 );
}

void Cordic::set(const unsigned int M, const unsigned int N)
{
	CORDIC_Dfmt.set( M,N );
}

void Cordic::set(const int m)
{
	if( m<-1 || m>1 )
	{
		cout << "??Insupportable mode initialized.\n"
			<< " System will work in circular mode.\n"
			<< "-----------------------------------------\n\n";
		mode=1;// default mode: circular
	}
	else mode=m;
}

void Cordic::set(const char dv_n)
{
	if( dv_n=='y' || dv_n=='Y' ||
		dv_n=='z' || dv_n=='Z' )
		dv_name=dv_n;
	else 
	{
		cout << "??Insupportable decision variable.\n"
			<< "z will be chosen as decision variable.\n"
			<< "-----------------------------------------\n\n";
		dv_name='z';// default decision variable: z
	}
}

void Cordic::set( const double x0,const double y0,const double z0 )
{
	if( validator( x0,y0,z0 ) )
	{
		x=x0;y=y0;z=z0;
	}
	else 
		cout << "initial state is not valid.\n";
}

//
//============================================================================
// core functions
double Cordic::Sin()
{
	cordic( 1,'z' );
	return y;
}

double Cordic::Cos()
{
	cordic( 1,'z' );
	return x;
}

double Cordic::Abs()
{
	cordic( 1,'y' );
	return x;
}

double Cordic::Atan()
{
	cordic( 1,'y' );
	return z;
}

double Cordic::Mult()
{
	cordic( 0,'z' );
	return y;
}

double Cordic::Dev()
{
	if( x==0 )
		return 0;
	cordic( 0,'y' );
	return z;
}

double Cordic::Sinh()
{
	cordic( -1,'z' );
	return y;
}

double Cordic::Cosh()
{
	cordic( -1,'z' );
	return x;
}

double Cordic::Sqrt()
{
	cordic( -1,'y' );
	return x;
}

double Cordic::Atanh( )
{
	cordic( -1,'y' );
	return z;
}

//============================================================================
//
// getting functions
const double& Cordic::getx() const
{
	return x;
}

const double& Cordic::gety() const
{
	return y;
}

const double& Cordic::getz() const
{
	return z;
}

void Cordic::print( ostream& output )const
{
	output << "\n%% -----------SYSTEM PARAMETERS------------"
		   << "-----------------------\n";
	output << left << setw(20) << "%  DATA FORMAT";
	CORDIC_Dfmt.print( output );
	output << left << setw(30) << "%  MODE";
	switch ( mode )
	{
		case -1:output << "hyperbolic\n";break;
		case 0: output << "linear\n";break;
		case 1: output << "circular\n";break;
	}
	output << left << setw(30) << "%  DECISION VARIABLE NAME" 
		 << dv_name  << endl;
	output << left <<setw(30) << "%  CURRENT STATE"
		 << "[ " << x << ',' << y << ',' << z << " ]" << endl
		 << "%  -----------------------------------------"
		 << "-----------------------*/\n\n";
}

void Cordic::save(std::ostream & output) const
{
	output << left << setw(15) << x 
		   << left << setw(15) << y
		   << left << setw(15) << z << endl;
}

// default destructor---------------------------------------------------
Cordic::~Cordic(void)
{
}

// utility functions----------------------------------------------------
bool Cordic::validator(const double x0,
					   const double y0,
					   const double z0 ) const
{
	// initial state check
	return true;
}

bool Cordic::terminator( const long dv ) const
{
	long temp = ( dv>0 ? dv:-dv );
	return ( temp<= ACCURACY );
}

void Cordic::cordic( const int m,const char dv_n )
{
	ofstream RECORDER( "recorder.dat",ios::app );
	if( !RECORDER )
	{
		cerr << "Fail to create Recoder." << endl;
		exit(0);
	}
	// initial state
	set( m );// set mode
	set( dv_n );// set decision variable
	long xi = CORDIC_Dfmt.trans2Q( x );
	long yi = CORDIC_Dfmt.trans2Q( y );
	long zi = CORDIC_Dfmt.trans2Q( z );
	long dv = zi;// choose decision variable
	if( dv_n=='y' || dv_n=='y' ) dv=yi;
	else dv=zi;
	bool disable = terminator( dv );
	if( !disable )// iteration begins
	{
		int di;// variables declaration
		int Fi;
		long ei,xi_old;
		double An = 1.0;// gain recorder
		unsigned i=0;
		unsigned wordwidth =  CORDIC_Dfmt.getm()
							+ CORDIC_Dfmt.getn();
		// recording
		RECORDER << "\n%% ----------------------------------------------"
				 << "--------------------------------------\n"
				 << left << setw(20) << "%  DATA FORMAT";
					CORDIC_Dfmt.print( RECORDER );
		RECORDER << left << setw(30) << "%  MODE";
					switch ( mode )
					{
						case -1:RECORDER << "hyperbolic\n";break;
						case 0: RECORDER << "linear\n";break;
						case 1: RECORDER << "circular\n";break;
					}
		RECORDER << left << setw(30) << "%  DECISION VARIABLE NAME" 
				 << left << setw(15)  << dv_name  
				 << left << setw(10) << " current " 
				 << CORDIC_Dfmt.trans2f( dv ) << endl;
		RECORDER << left << setw(8) << 'i'
				 << left << setw(12) << "xi"
				 << left << setw(12)	<< "yi" 
				 << left << setw(16) << "zi" 
				 << left << setw(6) << "di" 
				 << left << setw(6) << "Fi" 
			 	 << left << setw(16) << "ei" 
				 << left << setw(12) << "An" << endl
			 	 << "%-----------------------------------------------"
			 	 << "-------------------------------------*/\n";
		// recording initial state
		RECORDER << left << setw(8) << i
				 << left << setw(12) << xi 
				 << left << setw(12) << yi 
				 << left << setw(16) << zi;
		for( i=0;i<wordwidth;++i )
		{
			if( dv_name=='z' || dv_name=='Z' )// direction to rotate
				di = zi<0 ? -1:1; 
			else
				di = yi<0 ? 1:-1;
			if( mode==1 )// angle to rotate
			{
				Fi = i; // places shift to right
				ei = CORDIC_Dfmt.trans2Q( ROMatan[ i ] ); 
			}
			else if( mode==-1 )
			{
				Fi = i;if( Fi==0 )continue;
				ei = CORDIC_Dfmt.trans2Q( ROMatanh[ i ] );
				// SPECIAL CASES-----------------------------
				if( (i==4) || (i== 13) || (i==40) || (i==121) )
				{
					xi_old = xi;
					if( di>0 )
					{
						xi += yi >> Fi;
						yi += xi_old >> Fi;
						zi -= ei;
					}
					else if( di<0 )
					{
						xi -= yi >> Fi;
						yi -= xi_old >> Fi;
						zi += ei;
					}
					An *= sqrt( 1-pow( (double)2,-( Fi<<1 ) ) );
					// recording special cases
					RECORDER << left << setw(6) << di 
						<< left << setw(6) << Fi 
						<< left << setw(16) << ei
						<< left << setw(12) << An << endl
						<< left << setw(8) << i+1
						<< left << setw(12) << xi 
						<< left << setw(12) << yi 
						<< left << setw(16) << zi;

					if( dv_n=='y' || dv_n=='y' ) dv=yi;
					else dv=zi;
					disable = terminator( dv );
					if( disable )break;
					// new choices
					if( dv_name=='z' || dv_name=='Z' )// direction to rotate
						di = zi<0 ? -1:1; 
					else
						di = yi<0 ? 1:-1;

				}
				// ------------------------------------------
			}
			else
			{
				Fi = i;
				ei = CORDIC_Dfmt.trans2Q( pow( (double)2,-(int)Fi ) );
			}
			xi_old = xi; // register the old value for xi			
			if( di>0 ) 
			{
				if( mode==1 )
					xi -= yi >> Fi;
				else if( mode==-1 )
					xi += yi >> Fi;
				yi += xi_old >> Fi;
				zi -= ei;
			}
			else if( di<0 )
			{
				if( mode==1 )
					xi += yi >> Fi;
				else if( mode==-1 )
					xi -= yi >> Fi;
				yi -= xi_old >> Fi;
				zi += ei;
			}
			// gain factor
			if( mode==1 )
				An *= sqrt( 1+pow( (double)2,-( Fi<<1 ) ) );
			else if( mode==-1 )
				An *= sqrt( 1-pow( (double)2,-( Fi<<1 ) ) );

			// recording
			RECORDER << left << setw(6) << di 
					 << left << setw(6) << Fi 
					 << left << setw(16) << ei
					 << left << setw(12) << An << endl
					 << left << setw(8) << i+1
					 << left << setw(12) << xi 
					 << left << setw(12) << yi 
					 << left << setw(16) << zi;

			if( dv_n=='y' || dv_n=='y' ) dv=yi;
			else dv=zi;
			disable = terminator( dv );
			if( disable )break;
		}
		x = CORDIC_Dfmt.trans2f( xi );
		y = CORDIC_Dfmt.trans2f( yi );
		z = CORDIC_Dfmt.trans2f( zi );
		// remove An
		if( mode==1 )
		{
			x /= An;
			if( dv_name=='z' || dv_name=='Z' )
				y /= An;
		}
		else if( mode==-1 )
		{
			x /= An;
			if( dv_name=='z' || dv_name=='Z' )
				y /= An;
		}
	}
}