#include "weight_read.h"
#include "math.h"
#include "string.h"
#include "soft_flow.h"
#include "main.h"

typedef struct MeanFilter_s
{
    bool init;
    
    //input
    float input[SAMPLE_NUM];
    
    uint8_t index;
    
    //sum
    float sum;
    //output
    float output;
} MeanFilter_t;

static MeanFilter_t _mean_filter[SENSOR_NUM] = {0};
// 满额输出688.128mv（128倍增益）
//  weight =  AD/50KG量程 *128增益 * 2^24 位数 * 5.376mV(3.36V * 1.6mV/V)  AD/weight     1g对应AD值136.88
// static uint32_t databuf[SENSOR_NUM][SAMPLE_NUM] = {0};
/*输出范围：0x800000 - 0x7FFFFF(补码) 宽度为 -8388608 ~ 8388607 */

static void Get_All_GrossWeight(void);
static float Get_Weight(void);
static void Processing_Data(void);
static void Filter_Init(void);
// static void Calib_Creep(WEIGHING_PARAM* param, float* buff ,float total_weight);
// static float Get_Compare(WEIGHING_PARAM* param, float total_weight);
// static uint8_t Check_No_Impact(WEIGHING_PARAM* param);
// static void Analog_Value(WEIGHING_PARAM* param, uint8_t num, float* buf);

/*参数列表*/
static struct SENSOR sensor1 =
    {
        .SCK_GPIO_Port = PD_SCK1_GPIO_Port,
        .SCK_Pin = PD_SCK1_Pin,
        .DOUT_GPIO_Port = DOUT1_GPIO_Port,
        .DOUT_Pin = DOUT1_Pin,
        .K.f = 112.000f,
        .base_k = 112.0f,
};
static struct SENSOR sensor2 =
    {
        .SCK_GPIO_Port = PD_SCK2_GPIO_Port,
        .SCK_Pin = PD_SCK2_Pin,
        .DOUT_GPIO_Port = DOUT2_GPIO_Port,
        .DOUT_Pin = DOUT2_Pin,
        .K.f = 112.000f,
        .base_k = 112.0f,
        .Num = 1,
};
static struct SENSOR sensor3 =
    {
        .SCK_GPIO_Port = PD_SCK3_GPIO_Port,
        .SCK_Pin = PD_SCK3_Pin,
        .DOUT_GPIO_Port = DOUT3_GPIO_Port,
        .DOUT_Pin = DOUT3_Pin,
        .K.f = 112.000f,
        .base_k = 112.0f,
        .Num = 2,
};


static WEIGHING_OPS __ops = {
    .get_allgrossweight = Get_All_GrossWeight,
    .get_weight = Get_Weight,
    .processing_data = Processing_Data,
    .filter_init = Filter_Init,
};

static WEIGHING_DEVICE _device = {
    .sensor = {&sensor1, &sensor2, &sensor3},
    .sensor_num_mask = 4,
    .correct_k = 1,
    ._ops = &__ops,
};

uint32_t (*read)(struct SENSOR *sensor) = Read_Value;

WEIGHING_DEVICE *Get_Device_Handle(void)
{
    return &_device;
}

uint32_t Read_Value(struct SENSOR *sensor)
{
    uint32_t Value;
    uint8_t CycleIndex;
    Value = 0;
    // sensor->DOUT_GPIO_Port->BSRR = sensor->DOUT_Pin;
    // sensor->SCK_GPIO_Port->BSRR = (uint32_t)sensor->SCK_Pin << 16;
    // delay(20);
    HAL_GPIO_WritePin(sensor->SCK_GPIO_Port, sensor->SCK_Pin, 0);
    uint32_t time = HAL_GetTick();
    /*等待HX711准备好数据*/
    while (HAL_GPIO_ReadPin(sensor->DOUT_GPIO_Port, sensor->DOUT_Pin))
    {
        /*大量采集时做了延迟保护*/
        if (HAL_GetTick() - time > 100)
        {
            return 0;
        }
    }
    for (CycleIndex = 0; CycleIndex < 24; ++CycleIndex)
    {
        // sensor->SCK_GPIO_Port->BSRR = (uint32_t)sensor->SCK_Pin;
        HAL_GPIO_WritePin(sensor->SCK_GPIO_Port, sensor->SCK_Pin, 1);
        delay_us(5);
        Value = Value << 1;
        // sensor->SCK_GPIO_Port->BSRR = (uint32_t)sensor->SCK_Pin << 16;
        HAL_GPIO_WritePin(sensor->SCK_GPIO_Port, sensor->SCK_Pin, 0);
        delay_us(5);
        if (HAL_GPIO_ReadPin(sensor->DOUT_GPIO_Port, sensor->DOUT_Pin))
        {
            Value++;
        }
    }
    /*第25个时钟脉冲选择128倍增益通道*/
    // sensor->SCK_GPIO_Port->BSRR = (uint32_t)sensor->SCK_Pin ;
    HAL_GPIO_WritePin(sensor->SCK_GPIO_Port, sensor->SCK_Pin, 1);
    Value = Value ^ BASE_VALUE; // 异或取原码防止过零
    Value = Value & 0xFFFFC0;   // 后7位太抖，根据手册取无噪声有效位17位，且0x7F只有1g大小，放弃使用
    // sensor->SCK_GPIO_Port->BSRR = (uint32_t)sensor->SCK_Pin << 16;
    HAL_GPIO_WritePin(sensor->SCK_GPIO_Port, sensor->SCK_Pin, 0);

    return (Value);
}

//============ 均值滤波器 ==================
static void MeanFilterInit( MeanFilter_t *filter, uint8_t coef_size, float def )
{
    for(uint8_t i = 0; i < coef_size; i++)
    {
        filter->input[i] = def;
    }
    filter->index = 0;
    
    filter->sum = def * coef_size;
    filter->output = def;
    
    filter->init = true;
}

static bool wait_front_data(void)
{
    static uint8_t wait_count = 0;
    wait_count++;
    if(wait_count >= SAMPLE_NUM * SENSOR_NUM)
    {
        wait_count = SAMPLE_NUM * SENSOR_NUM;
        return false;
    }
    return true;
}

/* Computes a MeanFilter_t filter on a sample  */
static float meanApply( MeanFilter_t *filter, float input, uint8_t size)
{
    filter->sum -= filter->input[filter->index];
    filter->sum += input;
    filter->output = filter->sum / size;
    
    filter->input[filter->index] = input;
    filter->index++;
    filter->index = filter->index % size;

    return filter->output;
}

static void Filter_Init(void)
{
    for(uint8_t i = 0; i < SENSOR_NUM; i++)
    {
        MeanFilterInit(&_mean_filter[i], SAMPLE_NUM, 0);
    }
}

static void old_Filter_Value(uint32_t data_tmp[][SAMPLE_NUM])
{
    WEIGHING_DEVICE *device = &_device;
    uint8_t sensor_num_c;
    uint32_t tmp1 = 0;

    for (sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    {
        if (sensor_num_c == device->sensor_num_mask)
        {
            continue;
        }
        for (uint8_t j = 0; j < SAMPLE_NUM - 1; j++)
        {
            for (uint8_t k = j + 1; k < SAMPLE_NUM; k++)
            {
                if (data_tmp[sensor_num_c][j] > data_tmp[sensor_num_c][k])
                {
                    tmp1 = data_tmp[sensor_num_c][j];
                    data_tmp[sensor_num_c][j] = data_tmp[sensor_num_c][k];
                    data_tmp[sensor_num_c][k] = tmp1;
                }
            }
        }
    }

    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];
        sensor->Raw_Value = 0;
        for (uint8_t i = SAMPLE_NUM - 18; i < SAMPLE_NUM - 2; i++)
        {
            // sensor->Raw_Value += *((uint32_t*)tmp + sensor_num_c * SAMPLE_NUM + i);
            sensor->Raw_Value += data_tmp[sensor_num_c][i];
        }
        // sensor->Raw_Value = *((uint32_t*)tmp + sensor_num_c * SAMPLE_NUM + VALUE_MID);

        sensor->Raw_Value = sensor->Raw_Value / (SAMPLE_NUM - 4);

        if ((sensor->GrossWeight) < sensor->Raw_Value)
        {
            sensor->Real_Variation = -(float)(sensor->Raw_Value - sensor->GrossWeight);
        }
        else if ((sensor->GrossWeight) >= sensor->Raw_Value)
        {
            sensor->Real_Variation = (float)(sensor->GrossWeight - sensor->Raw_Value);
        }
    }
}

void Filter_Value(uint32_t* data)
{
    WEIGHING_DEVICE *device = &_device;
    uint8_t sensor_num_c;

    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];
        sensor->Raw_Value = 
        meanApply(&_mean_filter[sensor_num_c] , data[sensor_num_c], SAMPLE_NUM);

        if(wait_front_data())
        {
            sensor->Raw_Value = sensor->GrossWeight;
        }

        if ((sensor->GrossWeight) < sensor->Raw_Value)
        {
            sensor->Real_Variation = -(float)(sensor->Raw_Value - sensor->GrossWeight);
        }
        else if ((sensor->GrossWeight) >= sensor->Raw_Value)
        {
            sensor->Real_Variation = (float)(sensor->GrossWeight - sensor->Raw_Value);
        }
    }
}

/*转重量*/
void Convert_Into_Weight(void)
{
    WEIGHING_DEVICE *device = &_device;
    uint8_t sensor_num_c;
    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];

        if (sensor->Real_Variation < 0)
        {
            sensor->Real_Variation = fabsf(sensor->Real_Variation);
        }
        
        sensor->Real_Weight = sensor->Real_Variation / sensor->base_k;

        // if(sensor->Real_Variation < 0)
        // {
        // 	sensor->Real_Weight = 0;
        // }
    }
}

static void Processing_Data(void)
{
    uint8_t sensor_num_c;
    uint8_t err_count = 0;
    WEIGHING_DEVICE *device = &_device;
    // static uint8_t index = 0;
    uint32_t data[SENSOR_NUM] = {0};
    // // static uint32_t** pdata = NULL;

    // if(pdata == NULL)
    // {
    // 	pdata = (uint32_t**)malloc(sizeof(uint32_t*) * device->sensor_num);
    // 	for(sensor_num_c = 0; sensor_num_c < device->sensor_num; sensor_num_c++)
    // 	{
    // 		if(sensor_num_c == device->sensor_num_mask)
    // 		{
    // 			continue;
    // 		}
    // 		pdata[sensor_num_c] = (uint32_t*)malloc(sizeof(uint32_t) * SAMPLE_NUM);
    // 	}
    // }

    for (sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    {
        if (sensor_num_c == device->sensor_num_mask)
        {
            continue;
        }
        data[sensor_num_c] = read(device->sensor[sensor_num_c]);

        while (data[sensor_num_c] == 0)
        {
            data[sensor_num_c] = read(device->sensor[sensor_num_c]);
            err_count++;
            if (err_count > 10 && device->sensor_num_mask == 4)
            {
                device->sensor[sensor_num_c]->err_flag = true;
                device->sensor_num_mask = sensor_num_c;
                err_count = 0;
                return;
            }
        }
    }
    // index++;
    // if (index == SAMPLE_NUM)
    {
        Filter_Value(data);

        Convert_Into_Weight();

        // for(sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
        // {
        // 	// if(sensor_num_c == device->sensor_num_mask)
        // 	// {
        // 	// 	continue;
        // 	// }
        //     free(pdata[sensor_num_c]);
        // }
        // free(pdata);
        // pdata = NULL;
        // memset(databuf, 0, sizeof(databuf));

        // index = 0;
    }
}

static float Deal_With_Jitter(WEIGHING_DEVICE *_device, float *buff)
{
    WEIGHING_DEVICE *device = _device;
    static uint32_t deal_drift = 0;
    static float last_total = 0;
    static float buff_snapshot[SENSOR_NUM] = {0.0f};

    float total = 0;
    uint8_t sensor_num_c;
    last_total = 0;
    /*赋值*/
    for (sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    {
        if (sensor_num_c == device->sensor_num_mask)
        {
            continue;
        }
        total += buff[sensor_num_c];
        last_total += buff_snapshot[sensor_num_c];
    }

    if (device->Weight_last != 0)
    {
        for (sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
        {
            if (sensor_num_c == device->sensor_num_mask)
            {
                continue;
            }
            /*限幅消抖*/
            if (fabsf(last_total - total) < ALLOW_VALUE)
            {
                deal_drift++;
                if (deal_drift > N)
                {
                    buff[sensor_num_c] = buff_snapshot[sensor_num_c];
                }
            }
            else
            {
                deal_drift = 0;
            }
        }
    }
    // /*清零*/
    total = 0;
    /*重新赋值*/
    for (sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    {
        if (sensor_num_c == device->sensor_num_mask)
        {
            continue;
        }
        if ((buff[sensor_num_c] < MAX_ZERO && buff[sensor_num_c] > MIN_ZERO))
        {
            buff[sensor_num_c] = 0.f;
        }
        total += buff[sensor_num_c];
    }

    memcpy(buff_snapshot, buff, sizeof(buff_snapshot));

    return total;
}

static float Get_Weight(void)
{
    WEIGHING_DEVICE *device = &_device;
    uint8_t sensor_num_c;
    // float Total = 0.f;
    float allot = 0.f;

    float buff[SENSOR_NUM] = {0.f};

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

        allot += buff[sensor_num_c];
    }

    allot = allot / device->correct_k;
    /*用于校准时分配偏移量*/
    // for (sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    // {
    //     if (sensor_num_c == device->sensor_num_mask)
    //     {
    //         continue;
    //     }
    //     device->sensor[sensor_num_c]->Scale = buff[sensor_num_c] / allot;
    //     // if(device->sensor[sensor_num_c]->Scale <= 0.1f)
    //     // {
    //     // 	device->sensor[sensor_num_c]->Scale = NAN;
    //     // }
    // }

    // Total = Deal_With_Jitter(device ,buff);

    return allot;
}

// static void Analog_Value(WEIGHING_DEVICE* _device, uint8_t num, float* buf)
// {
// 	float tmp = 0.f;

// 	if(num)
// 	{
// 		WEIGHING_DEVICE* device = _device;
// 		for(uint8_t sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
// 		{
// 			tmp += buf[sensor_num_c];
// 		}
// 		buf[num - 1] = tmp / (SENSOR_NUM - 1);
// 		tmp = 0;
// 		for(uint8_t sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
// 		{
// 			tmp += device->sensor[sensor_num_c]->Scale;
// 		}
// 		device->sensor[num - 1]->Scale = tmp / SENSOR_NUM;
// 		Err_Check_And_Set(GOWRONG);
// 	}
// 	else if(Check_No_Impact(device) == 0)
// 	{
// 		Err_Check_And_Set(0);
// 	}
// }

static uint32_t Get_GrossWeight(struct SENSOR *sensor)
{
    uint32_t GrossWeight = 0;
    int i, j, tmp1;
    uint32_t sum = 0;
    uint32_t tmp[5] = {0};
    uint8_t err_count = 0;
    /*采集零点*/
    for (i = 0; i < 5; ++i)
    {
        tmp[i] = read(sensor);
        if (tmp[i] <= 10)
        {
            WEIGHING_DEVICE *device = Get_Device_Handle();

            err_count++;
            if (err_count > 10)
            {
                if (device->sensor_num_mask != 4)
                {
                    sensor->err_flag = true;
                    err_count = 0;
                    return 0.f;
                }

                device->sensor_num_mask = sensor->Num;
                return 0.f;
            }
            i--;
        }
    }

    for (i = 0; i < 4; ++i)
    {
        for (j = i + 1; j < 5; ++j)
        {
            if (tmp[i] > tmp[j])
            {
                tmp1 = tmp[i];
                tmp[i] = tmp[j];
                tmp[j] = tmp1;
            }
        }
    }

    for (i = 1; i < 4; i++)
    {
        sum += tmp[i];
    }

    GrossWeight = sum / 3;
    return GrossWeight;
}

static void Get_All_GrossWeight(void)
{
    WEIGHING_DEVICE *device = &_device;
    uint8_t sensor_num_c;
    /*上电后读取数据作为零点*/
    for (sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    {
        if (sensor_num_c == device->sensor_num_mask)
        {
            continue;
        }
        read(device->sensor[sensor_num_c]);
    }
    for (sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
    {
        if (sensor_num_c == device->sensor_num_mask)
        {
            continue;
        }
        device->sensor[sensor_num_c]->GrossWeight = Get_GrossWeight(device->sensor[sensor_num_c]);
    }
    device->check_self_flag = true; // 自检标志位
}

#if 0
static void Calib_Creep(WEIGHING_DEVICE* _device, float* buf, float total_weight)
{
	WEIGHING_DEVICE* device = _device;
	uint8_t sensor_num_c;
	float creep_value = 0.f;
	if(HAL_GetTick() - device->creep_time >= HALF_HOUR)
	{
		device->creep_count++;
		device->creep_time = HAL_GetTick();
		for(sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
		{
			device->creep_snap += device->sensor[sensor_num_c]->Real_Weight;
		}
		creep_value = Get_Compare(device, total_weight);
	}

	if(device->creep_count)
	{
		for(sensor_num_c = 0; sensor_num_c < SENSOR_NUM; sensor_num_c++)
		{
			buf[sensor_num_c] += creep_value * device->creep_count;
		}
		// total_weight += creep_value * SENSOR_NUM * device->creep_count;
	}
}

static float Get_Compare(WEIGHING_DEVICE* _device, float total_weight)
{
	return _device->creep_snap >= total_weight ? CREEP_VALUE : -CREEP_VALUE;
}
#endif