#include "calc_lib.h"
#include <math.h>

//΢ÃëÑÓʱ
void user_delay_us(uint16_t us)
{
	 for(; us > 0; us--)
	 {
		 for(uint8_t i = 50; i > 0; i--)
		 {
				;
		 }
	 }
}

//ºÁÃëÑÓʱ
void user_delay_ms(uint16_t ms)
{
	 for(; ms > 0; ms--)
	 {
		user_delay_us(1000);
	 }
}

//¸¡µãÊý·¶Î§ÏÞÖÆ
void abs_limit_fp(fp32 *num, fp32 Limit)
{
    if (*num > Limit)
    {
        *num = Limit;
    }
    else if (*num < -Limit)
    {
        *num = -Limit;
    }
}

//ÕûÊý·¶Î§ÏÞÖÆ
void abs_limit_int(int64_t *num, int64_t Limit)
{
    if (*num > Limit)
    {
        *num = Limit;
    }
    else if (*num < -Limit)
    {
        *num = -Limit;
    }
}

//Ñ­»·ÏÞ·ù
fp32 loop_fp32_constrain(fp32 Input, fp32 minValue, fp32 maxValue)
{
    if (maxValue < minValue)
    {
        return Input;
    }

    if (Input > maxValue)
    {
        fp32 len = maxValue - minValue;
        while (Input > maxValue)
        {
            Input -= len;
        }
    }
    else if (Input < minValue)
    {
        fp32 len = maxValue - minValue;
        while (Input < minValue)
        {
            Input += len;
        }
    }
    return Input;
}

int32_t loop_int32_constrain(int32_t Input, int32_t minValue, int32_t maxValue)
{
    if (maxValue < minValue)
    {
        return Input;
    }
			 if (Input > maxValue)
			{
        int32_t len = maxValue - minValue;
        while (Input > maxValue)
        {
            Input -= len;
        }
			}
			else if (Input < minValue)
			{
        int32_t len = maxValue - minValue;
        while (Input < minValue)
        {
            Input += len;
        }
			}
		
   
    return Input;
}

int map(int x, int in_min, int in_max, int out_min, int out_max)		//Ó³É亯Êý
{
	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

fp32 map_fp32(fp32 x, fp32 in_min, fp32 in_max, fp32 out_min, fp32 out_max)		//Ó³É亯Êý
{
	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

// ??????????????????????
float expo_map(float value, float expo_factor) {
    return value * (1 - expo_factor) + value * fabs(value) * expo_factor;
}


bool is_reached_multiple(float current1, float current2, float current3, float target1, float target2, float target3, float mistake, int threshold) {
    static int count = 0;      // 满足条件的计数

    if (count >= threshold) {
			  count=0;//重置
        return true; // 如果已经满足条件，返回 1
			
    }

    // 判断三个值是否都满足条件
    bool all_reached = (fabs(current1 - target1) < mistake) &&
                       (fabs(current2 - target2) < mistake) &&
                       (fabs(current3 - target3) < mistake);

    if (all_reached) {
        count++; // 所有条件都满足，计数加 1
    } 
    return false; // 未满足条件达到阈值，返回 0
}