#include "app.h"

void CORDIC_CosSin_Float(float angle_f, float* cos_f, float* sin_f)
{
	int angle_q31,cos_q31,sin_q31;

	if(angle_f == -1)
	{
		*cos_f = -1.000000;
		*sin_f =  0.000000;
	}
	else if(angle_f == -0.5)
	{
		*cos_f =  0.000000;
		*sin_f = -1.000000;		
	}
	else if(angle_f == 0.000000)
	{
		*cos_f =  1.000000;
		*sin_f =  0.000000;		
	}
	else if(angle_f == 0.5)
	{
		*cos_f =  0.000000;
		*sin_f =  1.000000;		
	}	
	else if(angle_f == 1)
	{
		*cos_f =  -1.000000;
		*sin_f =   0.000000;		
	}
	else
	{
		angle_q31 = (int)(angle_f * RANGE);
		CORDIC_CosSin_8(angle_q31, &cos_q31, &sin_q31);
		*cos_f = cos_q31 / 2147483648.0f;
		*sin_f = sin_q31 / 2147483648.0f;
	}
}

void CORDIC_CosSin_Test(void)
{
    float test_angle[5] = {-1.0, -0.56, 0 , 0.82, 0.95};//angle data is in radius and should be devided by PI
    float sin_f, cos_f;
    uint8_t i;

    for(i=0; i<5; i++)
    {
        CORDIC_CosSin_Float(test_angle[i], &cos_f, &sin_f);
        printf("sin(%f) = %.06f, cos(%f) = %.06f\n", test_angle[i]*PI, sin_f, test_angle[i]*PI, cos_f);
    }
    printf("CosSin demo end\n\n");
}

//atan_value must be multiplied by ¦Ð to obtain the angle in radians
void CORDIC_AtanSqrt_Float(float x_f, float y_f, float* sqrt_f, float* atan_f)
{
    int x_q31, y_q31;
    int sqrt_q31, atan_q31;
    
    x_q31 = (int)(x_f * RANGE);
    y_q31 = (int)(y_f * RANGE);
    
    CORDIC_AtanSqrt_8(x_q31, y_q31, &sqrt_q31, &atan_q31);
			
	*sqrt_f = (float)sqrt_q31*1.0/(CORDIC_F_31>>4);
    *atan_f = atan_q31 / 2147483648.0f;  
}

void CORDIC_AtanSqrt_Test(void)
{
    float test_x[5] = {1, -0.23, 0, 0.5, 1};  //x data,[-1,1]
    float test_y[5] = {1, -0.8, 0, 0.2, -0.5};//y data,[-1,1]
    float sqrt_f, atan_f;
    uint8_t i;
    
    for(i=0; i<5; i++)
    {
        CORDIC_AtanSqrt_Float(test_x[i], test_y[i], &sqrt_f, &atan_f);
        printf("x = %.06f, y = %.06f, sqrt = %.06f, atan = %.06f,\n", test_x[i], test_y[i], sqrt_f, (atan_f*PI));
    }
    printf("AtanSqrt demo end\n\n");
}


void CORDIC_CoshSinh_Float(float x_f, float* cosh_f, float* sinh_f)
{
	int x_q31,cosh_q31,sinh_q31;

    x_q31 = (int)(x_f / 2 * RANGE);
    CORDIC_CoshSinh_8(x_q31, &cosh_q31, &sinh_q31);
    
    *cosh_f = cosh_q31 / 2147483648.0f * 2;
    *sinh_f = sinh_q31 / 2147483648.0f * 2;
}

void CORDIC_CoshSinh_Test(void)
{
    float test_x[5] = {-1.118, -0.56, 0 , 0.86, 1.118};
    float sinh_f,cosh_f;
    uint8_t i;
    
    for(i=0; i<5; i++)
    {    
        CORDIC_CoshSinh_Float(test_x[i], &cosh_f, &sinh_f);
        printf("sinh(%f) = %.06f, cosh(%f) = %.06f\n", test_x[i], sinh_f, test_x[i], cosh_f);
    }
    printf("CoshSinh demo end\n\n");
}

void CORDIC_Atanh_Float(float x_f, float* atanh_f)
{
	int x_q31,atanh_q31,sinh_q31;

    x_q31 = (int)(x_f / 2 * RANGE);
    CORDIC_Atanh_8(x_q31, &atanh_q31);
    
    *atanh_f = atanh_q31 / 2147483648.0f * 2;
}

void CORDIC_Atanh_Test(void)
{
    float test_x[5] = {-0.806, -0.45, 0 , 0.75, 0.806};
    float atanh_f;
    uint8_t i;
    
    for(i=0; i<5; i++)
    {    
        CORDIC_Atanh_Float(test_x[i], &atanh_f);
        printf("atanh_f(%f) = %.06f\n", test_x[i], atanh_f);
    }
    printf("Atanh demo end\n\n");    
}

//ORIGIN RANGE         SCALE       ARG RANGE
//0.107 ¡Ü x < 1          1         0.0535 ¡Ü ARG1 < 0.5
//1 ¡Ü x < 3              2         0.25 ¡Ü ARG1 < 0.75
//3 ¡Ü x < 7              3         0.375 ¡Ü ARG1 < 0.875
//7 ¡Ü x ¡Ü 9.35           4         0.4375 ¡Ü ARG1 < 0.584
void CORDIC_Ln_Float(float x_f, float* ln_f)
{
	int x_q31,ln_q31;
    uint8_t scale_n;
    
    if((x_f >= 0.107) && (x_f < 1))
    {
        x_q31 = (int)(x_f / 2 * RANGE);
        scale_n = 1;
    }
    else if((x_f >= 1) && (x_f < 3))
    {
        x_q31 = (int)(x_f / 4 * RANGE);
        scale_n = 2;
    }
    else if((x_f >= 3) && (x_f < 7))
    {
        x_q31 = (int)(x_f / 8 * RANGE);
        scale_n = 3;
    }
    else if((x_f >= 7) && (x_f < 9.351))
    {
        x_q31 = (int)(x_f / 16 * RANGE);
        scale_n = 4;
    }
    
    CORDIC_Ln_8(x_q31, scale_n, &ln_q31);    
    *ln_f = ln_q31 / 2147483648.0f * 4;    
}

void CORDIC_Ln_Test(void)
{
    float test_x[9] = {0.107, 0.5, 1, 2, 3, 5, 7, 8, 9.35};
    float ln_f;
    uint8_t i,scale;
    
    for(i=0; i<9; i++)
    {    
        CORDIC_Ln_Float(test_x[i], &ln_f);
        printf("ln_f(%f) = %.06f\n", test_x[i], ln_f);
    } 
    printf("Ln demo end\n\n");        
}

//ORIGIN RANGE         SCALE       ARG RANGE
//0.027 ¡Ü x < 0.75       0         0.027 ¡Ü ARG1 < 0.75
//0.75 ¡Ü x < 1.75        1         0.375 ¡Ü ARG1 < 0.875
//1.75 ¡Ü x ¡Ü 2.341       2         0.4375 ¡Ü ARG1 ¡Ü 0.585
void CORDIC_Sqrt_Float(float x_f, float* sqrt_f)
{
	int x_q31,sqrt_q31;

    uint8_t scale_n;
    
    if( (x_f >= 0.027) && (x_f < 0.75))
    {
        x_q31 = (int)(x_f*RANGE);
        scale_n = 0;
    }
    else if( (x_f >= 0.75) && (x_f < 1.75))
    {
        x_q31 = (int)(x_f / 2 *RANGE);
        scale_n = 1;
    }
    else if( (x_f >= 1.75) && (x_f < 2.342))
    {
        x_q31 = (int)(x_f /4 *RANGE);
        scale_n = 2;
    }		
    
    CORDIC_Sqrt_8(x_q31, scale_n, &sqrt_q31);

    if(scale_n==0)
    {
        *sqrt_f=(float)sqrt_q31 * 1.0 / W_INV_Q31;
    }
    else if(scale_n==1)
    {
        *sqrt_f=(float)sqrt_q31 * 2.0 / W_INV_Q31 ; 
    }
    else if(scale_n==2)
    {
        *sqrt_f=(float)sqrt_q31 * 4.0 / W_INV_Q31 ; 
    }    
       
}

void CORDIC_Sqrt_Test(void)
{
    float test_x[7] = {0.027, 0.5, 0.75 , 1, 1.75, 2, 2.341};
    float sqrt_f;
    uint8_t i,scale;
    
    for(i=0; i<7; i++)
    {    
        CORDIC_Sqrt_Float(test_x[i], &sqrt_f);
        printf("sqrt_f(%f) = %.06f\n", test_x[i], sqrt_f);
    } 
    printf("Sqrt demo end\n\n");           
}
    
void APP_CORDIC_Test(void)
{
    CORDIC_CosSin_Test();
    CORDIC_AtanSqrt_Test();	
    CORDIC_CoshSinh_Test();    
    CORDIC_Atanh_Test();
    CORDIC_Ln_Test();
    CORDIC_Sqrt_Test();        
}




