#include "MGE_Hal.h"
#include "tva.h"
#include "math.h"

#include "math_utils.h"
// #include "Project_Config.h"

/*############## Test Vaule NEED TO MOVED ###########*/

static float rotation_offset = 0.0f;
static float angle_prev = 0;

static int8_t mge_direction = MGE_DIRECTION_UNKNOWN;

/*############## Test Vaule NEED TO MOVED ###########*/

TimeRec_s velTimeRec = {0};

#if TLE5012B_SPI_CONFIG_USE_IQ
static MGE_HAL_iq vel_preAngle = 0;
;
#else
static float vel_preAngle;
#endif

// static float staticAngleOffset = 2.8445f;
static float staticAngleOffset = 0.0f;

/*================================ Util Start ================================*/
int8_t mge_Get_direction(void)
{
    return mge_direction;
}

int mge_Set_direction(MGE_DIRECTION dir)
{
    switch (dir)
    {
    case MGE_DIRECTION_CCW:
        mge_direction = MGE_DIRECTION_CCW;
        break;
    case MGE_DIRECTION_CW:
        mge_direction = MGE_DIRECTION_CW;
        break;
    default:
        mge_direction = MGE_DIRECTION_UNKNOWN;
        break;
    }
    
}

/**
 * @brief Calculate the center of the smaller arc between two points
 * @param angle1 point1 In REG
 * @param angle2 point2 In REG
 * @return the angle of the center of the smaller arc between two points
 */
float mge_hal_calculateArcMidpointAngleBTP(float angle1, float angle2)
{
    float a, b, arc1, arc2, c;
    if (angle1 >= angle2)
    {
        a = angle1;
        b = angle2;
    }
    else
    {
        b = angle1;
        a = angle2;
    }
    arc1 = _2PI - a + b;
    arc2 = a - b;
    if (arc1 > arc2)
    {
        c = a + arc1 * 0.5f;
        if (c > _2PI)
        {
            return c - _2PI;
        }
        else
        {
            return c;
        }
    }
    else
    {
        return arc2 * 0.5f + b;
    }
}

static inline float angleCal_offset_direction(float RAWangle)
{
    // if (RAWangle >= _2PI)
    // {
    //     RAWangle -= _2PI;
    // }

    if (mge_direction == MGE_DIRECTION_CCW)
    {
        RAWangle = -RAWangle;
    }

    RAWangle -= staticAngleOffset;

   

    // if (RAWangle < 0 )
    // {
    //     RAWangle += _2PI;
    // }

    return RAWangle;
}

/*================================ Util END ================================*/

// 获得原始角度，无圈数累加
float mge_hal_GetRawAngle(void)
{
    return MGE_BSP_ANGEABS_GET();
}

mgeBsp_t mge_hal_GetBSPRawAngle(void)
{
    return MGE_BSP_ANGEBSPRAW_GET();
}

float mge_hal_GetAbsAngle(void)
{
    float mgeBSPangle = MGE_BSP_ANGEABS_GET();

    mgeBSPangle = angleCal_offset_direction(mgeBSPangle);

    // mgeBSPangle += staticAngleOffset;

    // if (mgeBSPangle >= _2PI)
    // {
    //     mgeBSPangle -= _2PI;
    // }

    return mgeBSPangle;
}

float mge_hal_GetAbsAngle_withFilter(void)
{
    float mgeBSPangle = MGE_BSP_ANGEABS_GET_WITH_FILTER(0);//范围0-6.18

    mgeBSPangle = angleCal_offset_direction(mgeBSPangle);


    float d_angle;
	// // printf("Angle:%d\n", angle_data);

	d_angle = mgeBSPangle - angle_prev;

    angle_prev = mgeBSPangle;

	if (fabs(d_angle) > (0.8 * _2PI))
		rotation_offset += d_angle > 0 ? -_2PI : _2PI;

    // rotation_offset += (rotation_offset > _2PI) ? -_2PI : rotation_offset < -_2PI ?_2PI:0;

	mgeBSPangle = (rotation_offset + mgeBSPangle);


    if (mgeBSPangle >= _PI)//此值大小根据限位来
    {
        rotation_offset-=_2PI;
        mgeBSPangle -= _2PI;
    }

    if (mgeBSPangle < -_PI)
    {
        rotation_offset+=_2PI;
        mgeBSPangle += _2PI;
    }

    return mgeBSPangle;
}



float mge_hal_GetAngle()
{
    return tle5012b_read_angle_f();
}

void mge_hal_Init()
{
    MGE_BSP_INIT();
    TimeFlash(&velTimeRec);
}

int mge_Set_abOffset(float offset)
{
    // if ((offset >= _2PI) || (offset <= -_2PI))
    // {
    //     return -1;
    // }
    // else
    // {
        staticAngleOffset = offset;
    //     return 0;
    // }

}

float mge_Get_abOffset(void)
{
    return staticAngleOffset;
}

float mge_hal_GetVel()
{
    float angle = 0;
    float vel = 0;
    float interTime = 0;

    interTime = TimeFlash(&velTimeRec); // get the velTime use IQ

    angle = mge_hal_GetAbsAngle_withFilter();

    vel = (angle - vel_preAngle) / interTime; // get Vel
    vel_preAngle = angle;

    return vel;
}

float mge_hal_GetVel_ExtAngle(float angle)
{
    // float angle = 0;
    float vel = 0;
    float interTime = 0;
    static float vel_preAngle = 0;

    interTime = TimeFlash(&velTimeRec); // get the velTime use IQ

    // angle = mge_hal_GetAbsAngle_withFilter();

    vel = (angle - vel_preAngle) / interTime; // get Vel
    vel_preAngle = angle;

    return vel;
}