#include "GrayscaleSensor.h"
#include "zf_driver_adc.h"
#include "zf_driver_gpio.h"
#include "zf_driver_delay.h"

#define AD2 B8
#define AD1 B9
#define AD0 B14

#define FILTER_ALPHA 0.5f //位置滤波系数，范围 [0, 1]，值越大响应越快但噪声越大

volatile static uint16 adc_white[8] = {4096,4096,4096,4096,4096,4096,4096,4096}; // 存储8路灰度传感器的白色参考值，初始值为满量程
volatile static uint16 adc_black[8] = {0,0,0,0,0,0,0,0}; // 存储8路灰度传感器的黑色参考值，初始值为0
volatile static uint16 adc_range[8] = {4096,4096,4096,4096,4096,4096,4096,4096}; // 存储8路灰度传感器的动态范围，初始值为满量程
volatile static uint16 adc_raw_value[8] = {0}; // 存储8路灰度传感器的原始 ADC 值
volatile float adc_normalized[8] = {0}; // 存储8路灰度传感器的归一化值，范围 [0, 1]
static float adc_normalized_sum = 0; // 归一化值的总和，用于计算位置
volatile float grayscale_sensor_position_prev = 0; // 存储小车相对于黑线的位置
volatile float grayscale_sensor_position = 0; // 存储小车相对于黑线的位置
static const int position[8] = {-4, -3, -2, -1, 1, 2, 3, 4}; // 每个传感器对应的位置

void grayscale_sensor_set_white(void)
{
    for(uint8 i=0;i<8;i++)
    {
        adc_white[i] = adc_raw_value[i];
        adc_range[i] = adc_white[i] - adc_black[i];
    }
}
void grayscale_sensor_set_black(void)
{
    for(uint8 i=0;i<8;i++)
    {
        adc_black[i] = adc_raw_value[i];
        adc_range[i] = adc_white[i] - adc_black[i];
    }
}

void grayscale_sensor_init(void)
{
    adc_init(ADC1_CH5_B18, ADC_12BIT);
    gpio_init(AD2, GPO, GPIO_LOW, GPO_PUSH_PULL);
    gpio_init(AD1, GPO, GPIO_LOW, GPO_PUSH_PULL);
    gpio_init(AD0, GPO, GPIO_LOW, GPO_PUSH_PULL);
    gpio_set_level(AD2, 1);
    gpio_set_level(AD1, 1);
    gpio_set_level(AD0, 1);
}
/*真值表
AD2 AD1 AD0
0   0   0   CH1
0   0   1   CH2
0   1   0   CH3
0   1   1   CH4
1   0   0   CH5
1   0   1   CH6
1   1   0   CH7
1   1   1   CH8
*/
void grayscale_sensor_scan(void)
{
    gpio_set_level(AD2, 0);
    gpio_set_level(AD1, 0);
    gpio_set_level(AD0, 0); // CH1
    system_delay_us(1);
    adc_raw_value[0]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
    gpio_set_level(AD2, 0);
    gpio_set_level(AD1, 0);
    gpio_set_level(AD0, 1); // CH2
    system_delay_us(1);
    adc_raw_value[1]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
    gpio_set_level(AD2, 0);
    gpio_set_level(AD1, 1);
    gpio_set_level(AD0, 0); // CH3
    system_delay_us(1);
    adc_raw_value[2]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
    gpio_set_level(AD2, 0);
    gpio_set_level(AD1, 1);
    gpio_set_level(AD0, 1); // CH4
    system_delay_us(1);
    adc_raw_value[3]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
    gpio_set_level(AD2, 1);
    gpio_set_level(AD1, 0);
    gpio_set_level(AD0, 0); // CH5
    system_delay_us(1);
    adc_raw_value[4]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
    gpio_set_level(AD2, 1);
    gpio_set_level(AD1, 0);
    gpio_set_level(AD0, 1); // CH6
    system_delay_us(1);
    adc_raw_value[5]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
    gpio_set_level(AD2, 1);
    gpio_set_level(AD1, 1);
    gpio_set_level(AD0, 0); // CH7
    system_delay_us(1);
    adc_raw_value[6]=adc_mean_filter_convert(ADC1_CH5_B18, 15);
    gpio_set_level(AD2, 1);
    gpio_set_level(AD1, 1);
    gpio_set_level(AD0, 1); // CH8
    system_delay_us(1);
    adc_raw_value[7]=adc_mean_filter_convert(ADC1_CH5_B18, 15);

    // 计算小车相对于黑线的位置
    grayscale_sensor_position = 0;
    adc_normalized_sum = 0;
    for(uint8 i=0;i<8;i++)
    {
        adc_normalized[i] = ((float)(adc_white[i] - adc_raw_value[i])) / adc_range[i];
        grayscale_sensor_position += adc_normalized[i] * position[i];
        adc_normalized_sum += adc_normalized[i];
    }
    if(adc_normalized_sum > 0)
    {
        grayscale_sensor_position /= adc_normalized_sum;
    }
    else
    {
        grayscale_sensor_position = 0; // 避免除以零，默认位置为0
    }
    grayscale_sensor_position =  grayscale_sensor_position_prev + FILTER_ALPHA * (grayscale_sensor_position - grayscale_sensor_position_prev);
    grayscale_sensor_position_prev = grayscale_sensor_position;
}