#include "task.h"
#include "soft_can.h"
#include "tim.h"
#include "string.h"
#include "math.h"
#include "warn.h"
#include "soft_flow.h"

static void Task_80_hz(WEIGHING_DEVICE *device);
static void Task_10_hz(void);
static void Task_5hz(void);
static void Task_1hz(WEIGHING_DEVICE *device);

void Task_Polling(void)
{
    WEIGHING_DEVICE *device = Get_Device_Handle();
	//解析称重消息
    Can_Rx_Decode();

    if (true == Start_80_hz) // tim4定时器0.2s进入一次中断，更新此FLAG
    {

        Start_80_hz = false;
    }

    if (true == Start_10_hz)
    {
        Task_80_hz(device);

        Task_10_hz();

        Start_10_hz = false;

    }

    if (true == Start_5_hz)
    {
        // Task_5hz();
        can2Pmu_3hz_info();
        Start_5_hz = false;
		//发送流量计数据
		Flow_Function();
        if(factory_calibration == true)
            Set_Ack_Status(ack_e1);
    }

    if (true == Start_1_hz)
    {
        Task_1hz(device);

        Start_1_hz = false;
    }
}

static void Get_Diff(WEIGHING_DEVICE *_device)
{
    WEIGHING_DEVICE *device = _device;
    volatile uint8_t licence_count[SENSOR_NUM] = {0};

    for (uint8_t sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    {
        for (uint8_t sensor_num_t = 0; sensor_num_t < SENSOR_NUM; sensor_num_t++)
        {
            if (sensor_num_c == device->sensor_num_mask)
            {
                continue;
            }
            int diff = fabsf(device->sensor[sensor_num_c]->Real_Variation - device->sensor[sensor_num_t]->Real_Variation);

            if (diff > (int)12000 && !device->sensor[sensor_num_t]->err_flag)
            {
                licence_count[sensor_num_c]++;
            }
            if ((device->sensor_num_mask == 4) && (licence_count[sensor_num_c] >= 3))
            {
                device->sensor[sensor_num_c]->licence_flag = true;
            }
            else if ((device->sensor_num_mask != 4) && (licence_count[sensor_num_c] >= 2))
            {
                device->sensor[sensor_num_c]->licence_flag = true;
            }
            else
            {
                device->sensor[sensor_num_c]->licence_flag = false;
            }
        }
    }
}

static bool Auxiliary_Judgment(struct SENSOR *sensor)
{
    struct SENSOR *Sensor = sensor;

    if (Sensor->raw_init_value <= Sensor->Raw_Value)
    {
        if (Sensor->Raw_Value - Sensor->raw_init_value >= 2500)
        {
            sensor->err_flag = true;
            return true;
        }
    }
    return false;
}

static void Judge_Higher(struct SENSOR *sensor, uint32_t *err_time)
{
    struct SENSOR *Sensor = sensor;
    uint32_t Error_Time = *err_time;

    if (Sensor->Raw_Value >= BASE_VALUE)
    {
        if (((Sensor->Raw_Value <= 0x803000) && (Sensor->licence_flag)) && (HAL_GetTick() - Error_Time > 2000))
        {
            Sensor->err_flag = true;
        }
        if (((Sensor->Raw_Value > 0x801000) && (Sensor->Raw_Value <= 0x803000)) && (Sensor->licence_flag == false))
        {
            Sensor->err_flag = false;
            *err_time = HAL_GetTick();
        }
        else if (Sensor->Raw_Value > 0x803000)
        {
            Sensor->err_flag = false;
            *err_time = HAL_GetTick();
        }
    }
    else
    {
        if ((((Sensor->Raw_Value > 0x7FB000) && (Sensor->licence_flag))) && (HAL_GetTick() - Error_Time > 2000))
        {
            Sensor->err_flag = true;
        }
        if ((Sensor->Raw_Value <= 0x7FF000 && Sensor->Raw_Value > 0x7FB000) && (Sensor->licence_flag == false))
        {
            Sensor->err_flag = false;
            *err_time = HAL_GetTick();
        }
        else if (Sensor->Raw_Value <= 0x7FB000)
        {
            Sensor->err_flag = false;
            *err_time = HAL_GetTick();
        }
    }
}

static void Judge_Lower(struct SENSOR *sensor, uint32_t *err_time)
{
    struct SENSOR *Sensor = sensor;
    uint32_t Error_Time = *err_time;

    if (Sensor->Raw_Value >= BASE_VALUE)
    {
        if (HAL_GetTick() - Error_Time > 2000)
        {
            Sensor->err_flag = true;
        }
    }
    else
    {
        if ((Sensor->Raw_Value > 0x7FB000) && (HAL_GetTick() - Error_Time > 2000))
        {
            Sensor->err_flag = true;
        }
        if (((Sensor->Raw_Value <= 0x7FF000) && (Sensor->Raw_Value > 0x7FB000)) && (Sensor->licence_flag == false))
        {
            Sensor->err_flag = false;
            *err_time = HAL_GetTick();
        }
        else if (Sensor->Raw_Value <= 0x7FB000)
        {
            Sensor->err_flag = false;
            *err_time = HAL_GetTick();
        }
    }
}

static void Judge_Equal(struct SENSOR *sensor, uint32_t *err_time)
{
    struct SENSOR *Sensor = sensor;
    uint32_t Error_Time = *err_time;

    if (Sensor->Raw_Value >= BASE_VALUE)
    {
        if (((Sensor->Raw_Value <= 0x803000 && Sensor->licence_flag)) && (HAL_GetTick() - Error_Time > 2000))
        {
            Sensor->err_flag = true;
        }
        if (((Sensor->Raw_Value > 0x801000) && (Sensor->Raw_Value <= 0x803000)) && (Sensor->licence_flag == false))
        {
            Sensor->err_flag = false;
            *err_time = HAL_GetTick();
        }
        else if (Sensor->Raw_Value > 0x803000)
        {
            Sensor->err_flag = true;
        }
    }
    else
    {
        if ((Sensor->Raw_Value >= 0x7FB000 && (Sensor->licence_flag)) && (HAL_GetTick() - Error_Time > 2000))
        {
            Sensor->err_flag = true;
        }
        if (((Sensor->Raw_Value < 0x7FF000) && (Sensor->Raw_Value >= 0x7FB000)) && (Sensor->licence_flag == false))
        {
            Sensor->err_flag = false;
            *err_time = HAL_GetTick();
        }
        else if (Sensor->Raw_Value < 0x7FB000)
        {
            Sensor->err_flag = false;
            *err_time = HAL_GetTick();
        }
    }
}
void Find_Err_Sensor(WEIGHING_DEVICE *_device)
{
    WEIGHING_DEVICE *device = _device;
    uint8_t sensor_num_c;
    static uint32_t error_time[SENSOR_NUM] = {0};

    Get_Diff(device);

    for (sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    {
        if (sensor_num_c == device->sensor_num_mask)
        {
            continue;
        }
        struct SENSOR *sensor = device->sensor[sensor_num_c];
        switch (sensor->init_flag)
        {
        case higher:
            if (!Auxiliary_Judgment(sensor))
            {
                Judge_Higher(sensor, &error_time[sensor_num_c]);
            }
            break;
        case lower:
            if (!Auxiliary_Judgment(sensor))
            {
                Judge_Lower(sensor, &error_time[sensor_num_c]);
            }
            break;
        case equal:
            if (!Auxiliary_Judgment(sensor))
            {
                Judge_Equal(sensor, &error_time[sensor_num_c]);
            }
            break;
        default:
            break;
        }
    }
}

#define RATE_DT 60
#define ONE_KG 1000

static void Compute_Rate(WEIGHING_DEVICE *device)
{
    static float Last_Rate = 0.f;
    static float Rate_buf = 0.f;

    Last_Rate = Rate_buf;
    Rate_buf = device->Weight_current;
    if (Last_Rate > 1e-6f)
    {
        device->rate = (uint16_t)((fabsf((Last_Rate - Rate_buf) / Last_Rate)) * RATE_DT);
    }
}

static void Task_80_hz(WEIGHING_DEVICE *device)
{
    device->_ops->processing_data();
}


static void Task_10_hz(void)
{
    Get_Sensor_Status();

    CAN2PMU_Send();
	
}

static void Task_5hz(void)
{
    // Find_Err_Sensor(hdevice);
}

static void Task_1hz(WEIGHING_DEVICE *device)
{
    Compute_Rate(device);
}