#include "task.h"
#include "soft_can.h"
#include "string.h"
#include "math.h"
#include "warn.h"
#include "my_math.h"
#include "soft_led.h"

static void Error_Led(FORMAT_CAN2PMU *msg)
{
    if (msg->WorkStatus == GOWRONG)
    {
        SET_RED_LED();
    }
    else
    {
       //RESET_RED_LED();
    }
}

static bool Check_Err(FORMAT_CAN2PMU *msg)
{
    WEIGHING_DEVICE *device = Get_Device_Handle();

    for (uint8_t sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    {
        if (sensor_num_c == device->sensor_num_mask)
        {
            continue;
        }
        if (device->sensor[sensor_num_c]->err_flag)
        {
            msg->WorkStatus = GOWRONG;
            msg->AlarmStatus = ANOMALY;
            return true;
        }
    }

    msg->WorkStatus = NORMAL;
    msg->AlarmStatus = NO_ERR;

    return false;
}

static void Warn_Check(WEIGHING_DEVICE *device, FORMAT_CAN2PMU *msg)
{

    Check_Err(msg);

    device->Weight_current = device->_ops->get_weight();

    if (device->Weight_last > 1e-6f && msg->AlarmStatus != ANOMALY)
    {
        /*rate*/
        if (device->Weight_current > RUNNING_THRESHOLD_VALUE)
        {
            memcpy(&msg->WeightRate, &device->rate, sizeof(device->rate));
        }
        /*小于一定值不计算rate*/
        if (device->Weight_last < RATE_THRESHOLD_VALUE)
        {
            msg->WeightRate = 0;
        }
    }

    Error_Led(msg); // 出现异常亮灯

    // for(uint8_t sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    // {
    // 	if(sensor_num_c == device->sensor_num_mask)
    // 	{
    // 		continue;
    // 	}
    // 	if(device->sensor[sensor_num_c]->err_flag)
    // 	{
    // 		Weight_curt -= device->sensor[sensor_num_c]->Real_Weight;
    // 	}
    // }
}

void Get_Sensor_Status(void)
{
    WEIGHING_DEVICE *device = Get_Device_Handle();
    FORMAT_CAN2PMU *msg = Get_Tx_Msg();

    if (device->check_self_flag == false)
    {
        msg->WorkStatus = HAVENOTCHECKED;
    }

    if (msg->AlarmStatus >= ANOMALY)
    {
        msg->WorkStatus = GOWRONG;
    }

    Warn_Check(device, msg);
}

/*截止频率(hz),周期时间(s)*/
float apply(float sample, float sample_last, float cutoff_freq, float dt)
{
    float out_put = 0.0f;
    float rc = 0.0f;
    float alpha = 0.0f;

    if (cutoff_freq <= 0.0f || dt <= 0.0f)
    {
        out_put = sample;
        return out_put;
    }

    rc = 1.0f / (M_2PI_F * cutoff_freq); /*计算rc常数*/
    alpha = constrain_float(dt / (dt + rc), 0.0f, 1.0f);
    out_put = sample_last + (sample - sample_last) * alpha;
    return out_put;
}