#include "OB90A64M3.h"
#include "cordic.h"
#include "math.h"

float CORDIC_Sin(float fDegree)
{
    OB_CORDIC->CORDIC_Y   = 0x00;
    OB_CORDIC->CORDIC_Z   = CORDIC_FloatToQ20(fDegree);

    OB_CORDIC->CORDIC_CTR_b.MODE = 0;       // Rotation Mode
    OB_CORDIC->CORDIC_CTR_b.CSYS = 1;       // Circular coordinate system.
    OB_CORDIC->CORDIC_CTR_b.PARASEL = 1;    // Parameter select enable
    OB_CORDIC->CORDIC_CTR_b.CCPSEL = 0;     // Angle

    OB_CORDIC->CORDIC_CTR_b.TRIG = 1;           // Trigger
    while(OB_CORDIC->CORDIC_CTR_b.TRIG != 0);   // Wait until TRIG = 0

    return CORDIC_Q20ToFloat(OB_CORDIC->CORDIC_Y);
}

float CORDIC_Cos(float fDegree)
{
    OB_CORDIC->CORDIC_Y   = 0x00;
    OB_CORDIC->CORDIC_Z   = CORDIC_FloatToQ20(fDegree);

    OB_CORDIC->CORDIC_CTR_b.MODE = 0;       // Rotation Mode
    OB_CORDIC->CORDIC_CTR_b.CSYS = 1;       // Circular coordinate system.
    OB_CORDIC->CORDIC_CTR_b.PARASEL = 1;    // Parameter select enable
    OB_CORDIC->CORDIC_CTR_b.CCPSEL = 0;     // Angle

    OB_CORDIC->CORDIC_CTR_b.TRIG = 1;           // Trigger
    while(OB_CORDIC->CORDIC_CTR_b.TRIG != 0);   // Wait until TRIG = 0

    return CORDIC_Q20ToFloat(OB_CORDIC->CORDIC_X);
}


float CORDIC_ArcTan(float fValueX, float fValueY)
{
    OB_CORDIC->CORDIC_X   = CORDIC_FloatToQ20(fValueX);
    OB_CORDIC->CORDIC_Y   = CORDIC_FloatToQ20(fValueY);
    OB_CORDIC->CORDIC_Z   = CORDIC_FloatToQ20(0);

    OB_CORDIC->CORDIC_CTR_b.MODE = 1;       // Vector Mode
    OB_CORDIC->CORDIC_CTR_b.CSYS = 1;       // Circular coordinate system
    OB_CORDIC->CORDIC_CTR_b.PARASEL = 0;    // Parameter select Disable
    OB_CORDIC->CORDIC_CTR_b.CCPSEL = 0;     // Angle

    OB_CORDIC->CORDIC_CTR_b.TRIG = 1;           // Trigger
    while(OB_CORDIC->CORDIC_CTR_b.TRIG != 0);   // Wait until TRIG = 0

    return CORDIC_Q20ToFloat(OB_CORDIC->CORDIC_Z);
}


// (X^2+Y^2)^0.5
float CORDIC_Ghypot(float fValueX, float fValueY)
{
    OB_CORDIC->CORDIC_X   = CORDIC_FloatToQ20(fValueX);
    OB_CORDIC->CORDIC_Y   = CORDIC_FloatToQ20(fValueY);
    OB_CORDIC->CORDIC_Z   = CORDIC_FloatToQ20(0);

    OB_CORDIC->CORDIC_CTR_b.MODE = 1;       // Vector Mode
    OB_CORDIC->CORDIC_CTR_b.CSYS = 1;       // Circular coordinate system
    OB_CORDIC->CORDIC_CTR_b.PARASEL = 0;    // Parameter select Disable
    OB_CORDIC->CORDIC_CTR_b.CCPSEL = 0;     // Angle

    OB_CORDIC->CORDIC_CTR_b.TRIG = 1;           // Trigger
    while(OB_CORDIC->CORDIC_CTR_b.TRIG != 0);   // Wait until TRIG = 0

    return 0.607252935009250 * CORDIC_Q20ToFloat(OB_CORDIC->CORDIC_X);
}



// Q20-format To float
float CORDIC_Q20ToFloat(uint32_t nQValue)
{
    uint32_t nSign;
    float x, y;

    if (nQValue >= 0x80000000)
    {
        nSign  = 0x01;
        nQValue = (~nQValue) + 1;
    }
    else
    {
        nSign = 0x00;
    }

    y = (nQValue >> 0x14);
    nQValue = nQValue & 0x000FFFFF;

    x = pow(2, 20);
    x = (float)nQValue/x;
    x = x + y;

    if (nSign == 0x01)
       x = x * (-1);

    return x;
}

// float To Q20-format
uint32_t CORDIC_FloatToQ20(float fValue)
{
    uint32_t nBuff, nX, nSign;

    if (fValue >= 0x00)
        nSign = 0x00;
    else
        nSign = 0x01;

    fValue = fabs(fValue);
    nBuff  = (uint32_t)fValue;
    fValue = fValue - nBuff;
    nX = pow(2, 20);
    nX = nX * fValue;
    nX = nX | (nBuff << 0x14);

    if (nSign == 0x01)
        nX = (~nX ) + 1;

    return nX;
}

