#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "esp_err.h"
#include "stdlib.h"
#include "config.h"
#include "utils.h"
#include "ads1220.h"
#include "fram.h"
#include "ModbusM.h"
#include "ModbusS.h"

#include "rotary_encoder.h"

#include "esp_log.h"
#include "esp_check.h"
#include "soc/rtc.h"
#include "driver/mcpwm.h"
#include "config.h"
#include "esp_gatts_api.h"
#include "bt_server.h"

#include "esp_system.h"
#include "driver/uart.h"
#include "string.h"
#include "driver/gpio.h"

#include <string.h>
#include <stdio.h>

static const char *TAG = "depth";

typedef struct
{
	uint32_t capture_signal;
	mcpwm_capture_signal_t sel_cap_signal;
} capture;

typedef struct capture_event
{
	int ch;
	uint32_t val;
} capture_event_t;

// #define DEPTH_PIN_CLK GPIO_NUM_36 // 捕获GPIO端口
// #define GPIO_PCNT_PIN_1 36		  // Set GPIO 18 as phaseA/C1
// #define GPIO_PCNT_PIN_2 35		  // Set GPIO 19 as phaseB/C2
// #define DEPTH_PIN_PULSE    	36       //深度脉冲GPIO端口
// #define DEPTH_PIN_CTRL    	35       //深度控制GPIO端口

#define FLOW_SYNC_PIN 7 // 捕获GPIO端口 SPI2_nIRQ

#define DEPTH_PIN_PULSE 39 // 深度脉冲GPIO端口  TIM15_CH1
#define DEPTH_PIN_CTRL 38  // 深度控制GPIO端口	TIM15_CH2
#define DEPTH_PIN_ENC_A 42 // 深度脉冲GPIO端口	TIM3_CH1
#define DEPTH_PIN_ENC_B 41 // 深度控制GPIO端口	TIM3_CH2
#define SEND_DATA_TEST 0

// #define DEPTH_PIN_ENC_A 36 // 深度脉冲GPIO端口
// #define DEPTH_PIN_ENC_B 35 // 深度控制GPIO端口

void add_recod_item(void);

extern int get_total_flow_by_time(int ch, uint32_t time);
extern void zero_totalflow(int ch);

extern void reset_depth(void);
// extern esp_err_t ec11_pcnt_clear(pcnt_unit_t unit);

void capture_depth_init();
extern flow_t *pflow;
extern uint16_t last_pile_id;
uint32_t enc1_update_time;
uint32_t _enc1_update_time;
int32_t enc1_value;

uint32_t enc2_update_time;
uint32_t _enc2_update_time;
int32_t enc2_value;

extern rotary_encoder_t *encoder;

typedef struct
{
	int16_t max_depth;
	uint16_t pile_id;
	uint16_t count;
	// uint16_t work_time;
	struct _item
	{
		int16_t speed;
		int16_t depth;
		int16_t flow[2];
		uint32_t total_flow[2];
		int16_t tilt_x;
		int16_t tilt_y;
		uint16_t current1;
		uint16_t current2;
	} item[10];
} record_t;

// 18字节
#pragma pack(1)
typedef struct
{
	uint8_t TAG;
	int16_t speed;
	int16_t depth;
	int16_t flow[2];
	uint32_t total_flow[2];
	uint16_t current1;
	uint16_t current2;
} send_to_bt_t1;

// 16字节
typedef struct
{
	uint8_t TAG;
	uint8_t status;
	uint8_t LAT[5];
	uint8_t LOG[5];
	int16_t ALT; // 海拔高度
	int16_t speed;
	uint32_t UTC;
	// uint16_t dir;
	// uint16_t speed;
	// int16_t tilt_x;
	// int16_t tilt_y;
} position_t;

typedef struct
{
	uint8_t TAG;
	int16_t distence;
	int16_t pitch;
	int16_t azimuth;
	int16_t tilt_x;
	int16_t tilt_y;
}direction_t;

typedef struct
{
	uint8_t TAG;
	uint8_t status;
	uint8_t LAT[5];
	uint8_t LOG[5];
	int16_t ALT; // 海拔高度
	int16_t speed;
	uint32_t UTC;
	// uint16_t dir;
	// uint16_t speed;
	// int16_t tilt_x;
	// int16_t tilt_y;
} send_to_bt_t2;

#pragma pack()

typedef struct
{
	int16_t up_down;			 // 12
	int16_t speed;				 // 13
	int16_t depth;				 // 14
	uint16_t sample_count;		 // 15
	uint16_t depth_flow[2];		 // 16~17
	uint32_t last_total_flow[2]; // 18~21
	int16_t depth_offset;		 // 22
								 // uint32_t update_time;
								 //  int16_t tilt_x;
								 //  int16_t tilt_y;
								 //  uint16_t current1;
								 //  uint16_t current2;
								 //  uint16_t current3;
} depth_t;

record_t *record = (record_t *)&gWordVar[RECORD_REG_ADDR];
depth_t *depth_data = (depth_t *)&gWordVar[DEPTH_REG_ADDR];
move_t *pMoveCtx = (move_t *)&gWordVar[MOVE_REG_ADDR];
depth_config_t *depth_config = (depth_config_t *)&gWordVar[DEPTH_CONFIG_ADDR];

rotary_encoder_t *encoder = NULL; // 编码器测量深度参数

// static uint32_t cap_val_begin_of_depth = 0;
// static uint32_t cap_val_end_of_depth = 0;
// extern uint32_t rtc_clk_apb_freq;
/**
 * @brief this is an ISR callback, we take action according to the captured edge
 */
static bool depth_isr_handler(mcpwm_unit_t mcpwm, mcpwm_capture_channel_id_t cap_sig, const cap_event_data_t *edata,
							  void *arg)
{

	// 双边沿触发中断，只有一个边缘会更新值,所以只有在值发生变化是才更新时间
	int value = encoder->get_counter_value(encoder);
	if (value != enc1_value)
	{
		enc1_value = value;
		enc1_update_time = edata->cap_value;
	}
	return false;
}

void capture_depth_init()
{
	int pulse_pin;
	if (depth_config->port)
	{
		pulse_pin = DEPTH_PIN_ENC_A;
	}
	else
	{
		pulse_pin = DEPTH_PIN_PULSE;
	}
	ESP_ERROR_CHECK(mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM_CAP_0, pulse_pin));
	// enable pull down CAP0, to reduce noise
	ESP_ERROR_CHECK(gpio_pullup_en(pulse_pin));
	// enable both edge capture on CAP0
	mcpwm_capture_config_t conf = {
		.cap_edge = MCPWM_NEG_EDGE,
		.cap_prescale = 1,
		.capture_cb = depth_isr_handler, // 绑定深度中断处理函数
		.user_data = NULL};
	if (depth_config->input_type > 1)
	{
		conf.cap_edge = MCPWM_BOTH_EDGE;
	}
	ESP_ERROR_CHECK(mcpwm_capture_enable_channel(MCPWM_UNIT_0, MCPWM_SELECT_CAP0, &conf));
	ESP_LOGI(TAG, "capture_depth_init");
}

void pcnt_rotary_encoder_init(void)
{
	// Rotary encoder underlying device is represented by a PCNT unit in this example
	uint32_t pcnt_unit = 0;
	int pulse_pin;
	int ctrl_pin;
	if (depth_config->port)
	{
		pulse_pin = DEPTH_PIN_ENC_A;
		ctrl_pin = DEPTH_PIN_ENC_B;
	}
	else
	{
		pulse_pin = DEPTH_PIN_PULSE;
		ctrl_pin = DEPTH_PIN_CTRL;
	}
	// Create rotary encoder instance
	rotary_encoder_config_t config = ROTARY_ENCODER_DEFAULT_CONFIG((rotary_encoder_dev_t)pcnt_unit, pulse_pin, ctrl_pin);
	config.flags = depth_config->input_type;
	ESP_ERROR_CHECK(rotary_encoder_new_ec11(&config, &encoder));
	// Filter out glitch (1us)
	ESP_ERROR_CHECK(encoder->set_glitch_filter(encoder, 10));
	// Start encoder
	ESP_ERROR_CHECK(encoder->start(encoder));
}

void depth_init(void)
{
	// if (depth_config->input_type == 1)
	// { // 电压型 方向+脉冲信号 使用比较器通道,IO口中断方式采集

	// 	// GPIO_InitTypeDef GPIO_InitStruct = {0};
	// 	// /*Configure GPIO pin : PtPin */
	// 	// GPIO_InitStruct.Pin = ROLLER_DIR_Pin;
	// 	// GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
	// 	// GPIO_InitStruct.Pull = GPIO_PULLUP;
	// 	// HAL_GPIO_Init(ROLLER_GPIO_Port, &GPIO_InitStruct);

	// 	// /*Configure GPIO pin : PtPin */
	// 	// GPIO_InitStruct.Pin = ROLLER_CLK_Pin;
	// 	// GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
	// 	// GPIO_InitStruct.Pull = GPIO_PULLUP;
	// 	// HAL_GPIO_Init(ROLLER_GPIO_Port, &GPIO_InitStruct);
	// }
	// else if (depth_config->input_type == 2)
	// { // 电压型 正交编码器 使用比较器通道,TIM1编码器通道采集
	// MX_TIM1_Init();
	// pcnt_rotary_encoder_init(); // 编码器初始化
	// capture_depth_init();
	// HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL);
	// }
	// else if (depth_config->input_type == 3)
	// { // 开漏输出 正交编码器 使用光耦输入通道,TIM3编码器通道采集
	//   //  MX_TIM3_Init();
	//   //  HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
	// }
	depth_data->depth_offset = -depth_config->depth_offset;
	depth_data->up_down = 1; // 默认工作
}

enum
{
	REST = -1,
	STOP = 0,
	DOWN = 1,
	UP = 2,
};

int last_enc_value = 0;
uint32_t _enc_update_time;
uint32_t enc_update_time;
int last_sample_depth = 0;
int last_flow[2] = {0};
int16_t prev_depth = 0;
uint32_t prev_update_time = 0;
int16_t target_sample_depth = 0;

//   正交编码器的深度 ： encoder->get_counter_value(encoder)

int abs_sub(uint32_t enc_update_time, uint32_t _enc_update_time)
{
	if (enc_update_time >= _enc_update_time)
	{
		return enc_update_time - _enc_update_time;
	}
	else
	{
		return (0xffffffff - _enc_update_time + enc_update_time + 1);
	}
}

send_to_bt_t1 send_to_bt1;
send_to_bt_t2 send_to_bt2;
extern bool is_connected;
extern uint16_t spp_handle_table[SPP_IDX_NB];
extern uint16_t spp_conn_id;
extern esp_gatt_if_t spp_gatts_if;
extern flow_t *pflow;
/*把两个结构体中的数据通过蓝牙发送*/
void send_data_to_bt(void)
{

// #if SEND_DATA_TEST
// 	send_to_bt1.TAG = 1;
// 	send_to_bt1.speed = 2;
// 	send_to_bt1.depth = 3;
// 	send_to_bt1.tilt_x = 4;
// 	send_to_bt1.tilt_y = 5;
// 	send_to_bt1.current1 = 6;
// 	send_to_bt1.current2 = 7;

// 	send_to_bt2.TAG = 2;
// 	send_to_bt2.flow[0] = 3;
// 	send_to_bt2.flow[1] = 4;
// 	send_to_bt2.total_flow[0] = 5;
// 	send_to_bt2.total_flow[1] = 6;
// 	ESP_LOGI(TAG, "set text data that send to bt\n");
// #else
// 	ESP_LOGI(TAG, "use real time data that send to bt\n");
// 	send_to_bt1.TAG = 1;
// 	send_to_bt1.tilt_x = (short)gWordVar[TILT_SENSER_ADDR];
// 	send_to_bt1.tilt_y = (short)gWordVar[TILT_SENSER_ADDR + 1];
// 	send_to_bt1.speed = depth_data->speed;
// 	send_to_bt1.depth = depth_data->depth;
// 	if (depth_config->move_current_channel == 2)
// 	{
// 		send_to_bt1.current1 = gWordVar[AC_CURRENT_REG_ADDR];
// 		send_to_bt1.current2 = gWordVar[AC_CURRENT_REG_ADDR + 1];
// 	}
// 	else if (depth_config->move_current_channel == 1)
// 	{
// 		send_to_bt1.current1 = gWordVar[AC_CURRENT_REG_ADDR];
// 		send_to_bt1.current2 = gWordVar[AC_CURRENT_REG_ADDR + 2];
// 	}
// 	else
// 	{
// 		send_to_bt1.current1 = gWordVar[AC_CURRENT_REG_ADDR + 1];
// 		send_to_bt1.current2 = gWordVar[AC_CURRENT_REG_ADDR + 2];
// 	}
// 	send_to_bt2.TAG = 2;
// 	send_to_bt2.flow[0] = pflow[0].flow;
// 	send_to_bt2.flow[1] = pflow[1].flow;
// 	send_to_bt2.total_flow[0] = pflow[0].total_flow;
// 	send_to_bt2.total_flow[1] = pflow[1].total_flow;
// #endif
// 	ESP_LOGI(TAG, "flow 1 :%d\n", send_to_bt2.flow[0]);
// 	ESP_LOGI(TAG, "flow 2 :%d\n", send_to_bt2.flow[1]);
// 	ESP_LOGI(TAG, "total_flow 1 :%d\n", send_to_bt2.total_flow[0]);
// 	ESP_LOGI(TAG, "total_flow 2 :%d\n", send_to_bt2.total_flow[1]);
// 	if (is_connected == true)
// 	{
// 		ESP_LOGI(TAG, "is connected\n");
// 		esp_err_t err = esp_ble_gatts_send_indicate(spp_gatts_if,
// 													spp_conn_id,
// 													spp_handle_table[4],
// 													sizeof(send_to_bt1),
// 													(uint8 *)&send_to_bt1,
// 													false);
// 		if (err == ESP_OK)
// 			ESP_LOGI(TAG, "send data to bt 1 sucessfully\n");
// 		else
// 			ESP_LOGI(TAG, "send data to bt 1 failed\n");

// 		err = esp_ble_gatts_send_indicate(spp_gatts_if,
// 										  spp_conn_id,
// 										  spp_handle_table[4],
// 										  sizeof(send_to_bt2),
// 										  (uint8 *)&send_to_bt2,
// 										  false);
// 		if (err == ESP_OK)
// 			ESP_LOGI(TAG, "send data to bt 2 sucessfully\n");
// 		else
// 			ESP_LOGI(TAG, "send data to bt 2 failed\n");
// 	}
}

void depth_task(void *arg)
{
	ESP_LOGI(TAG, "go to depth_task sucessfully\n");
	// int enc_value;
	int time_diff = 0;
	int speed_timeout = 0;
	int last_speed_enc_value = 0;  // 上次速度计算的编码器值
	int speed_enc_update_time = 0; // 上次速度计算的编码器值
	int speed_calc_count = 0;
	int enc_value = 0;
	// int count = 0;
	int bt_time_count = 0;
	depth_data->depth_offset = -depth_config->depth_offset;
	depth_data->up_down = 1; // 默认工作

	record->pile_id = ++last_pile_id;
	// send_to_bt1.pile_id = ++last_pile_id;

	gWordVar[LAST_PILE_ID_ADDR] = last_pile_id;
	while (1)
	{

		if (_enc1_update_time != enc1_update_time)
		{
			ESP_LOGI(TAG, "_enc1_update_time != enc1_update_time\n");
			// int enc_update_time = enc1_update_time;
			enc_update_time = enc1_update_time;
			enc_value = enc1_value;
			time_diff = abs_sub(enc_update_time, _enc1_update_time);
			_enc1_update_time = enc_update_time;
		}

		if (time_diff != 0)
		{
			ESP_LOGI(TAG, "time_diff != 0\n");
			int16_t depth = enc_value * depth_config->N / depth_config->M; // 1mm
			depth_data->depth = depth - depth_data->depth_offset;
			if (depth_data->depth > depth_config->max_depth)
			{
				ESP_LOGI(TAG, "depth_data->depth > depth_config->max_depth\n");
				depth_data->depth_offset = depth - depth_config->max_depth;
				depth_data->depth = depth_config->max_depth;
			}
			else if (depth_data->depth < depth_config->min_depth)
			{
				ESP_LOGI(TAG, "depth_data->depth < depth_config->min_depth\n");
				depth_data->depth_offset = depth - depth_config->min_depth;
				depth_data->depth = depth_config->min_depth;
			}
			if (depth_data->depth > record->max_depth)
			{
				ESP_LOGI(TAG, "depth_data->depth > record->max_depth\n");
				record->max_depth = depth_data->depth;
				// send_to_bt1.max_depth = depth_data->depth;
			}
			if (speed_calc_count++ > 50) // 500ms计算一次速度
			{
				ESP_LOGI(TAG, "speed_calc_count++ > 50\n");
				speed_calc_count = 0;
				int speed_time_diff = abs_sub(enc_update_time, speed_enc_update_time);
				int time_diff_us = speed_time_diff / (APB_CLK_FREQ / 1000000);
				if (time_diff_us > 0 && time_diff_us < 5000000)
				{
					ESP_LOGI(TAG, "time_diff_us:%d,dist_diff=%d", time_diff_us, enc_value - last_speed_enc_value);
					// speed = dist_diff / speed_time_diff speed 单位mm/min dist_diff 单位mm speed_time_diff 单位us
					depth_data->speed = (enc_value - last_speed_enc_value) * depth_config->N / depth_config->M * 600000ll / time_diff_us;
					speed_timeout = 0;
					speed_enc_update_time = enc_update_time;
					last_speed_enc_value = enc_value;
				}
				else
				{
					if (++speed_timeout > 10)
					{
						depth_data->speed = 0;
						speed_timeout = 0;
						speed_enc_update_time = enc_update_time;
						last_speed_enc_value = enc_value;
					}
				}
			}
			if (depth_data->up_down > STOP)
			{
				ESP_LOGI(TAG, "depth_data->up_down > STOP\n");
				if (enc_value > last_enc_value)
				{ // 下钻,深度增加
					if (depth_data->up_down != 1)
					{ // 方向改变
						depth_data->up_down = 1;
						target_sample_depth = (depth_data->depth / depth_config->sample_depth) * depth_config->sample_depth;
						if (abs(depth_data->depth - target_sample_depth) < depth_config->sample_depth / 2)
						{ // 小于半个采样长度的合并到下一个采样点
							target_sample_depth += depth_config->sample_depth;
						}
					}
					if (depth_data->depth >= target_sample_depth)
					{ // 到达或超过目标采样点
						// 由于编码器精度问题不能确保数据在采样点上，需要通过插值计算采样点
						// 当使用10线编码器时每个脉冲深度达6.25cm，当采样间隔为10cm时抖动值太大
						if ((prev_depth - depth_data->depth) != 0)
						{
							int i;
							// uint32_t time = (target_sample_depth - prev_depth) * (enc_update_time - prev_update_time) / (depth_data->depth - prev_depth) + prev_update_time;
							for (i = 0; i < 2; i++)
							{
								// int total_flow = get_total_flow_by_time(i, time);
								int total_flow = get_total_flow_by_time(i, pflow[i].update_time);
								int flow = total_flow - depth_data->last_total_flow[i];
								if (flow < 0)
								{
									flow = 0;
								}
								depth_data->depth_flow[i] = flow;
								depth_data->last_total_flow[i] = total_flow;
							}
							depth_data->sample_count++;
							target_sample_depth += depth_config->sample_depth;
							add_recod_item();
						}
					}
				}
				else if (enc_value < last_enc_value)
				{
					if (depth_data->up_down != 2)
					{ // 方向改变
						depth_data->up_down = 2;
						// target_sample_depth -= depth_config->sample_depth;
						target_sample_depth = (depth_data->depth / depth_config->sample_depth - 1) * depth_config->sample_depth;
						if (abs(depth_data->depth - target_sample_depth) < depth_config->sample_depth / 2)
						{ // 小于半个采样长度的合并到下一个采样点
							target_sample_depth -= depth_config->sample_depth;
						}
					}
					if (depth_data->depth <= target_sample_depth)
					{
						// 由于编码器精度问题不能确保数据在采样点上，需要通过插值计算采样点
						// 当使用10线编码器时每个脉冲深度达7.6cm，当采样间隔为10cm时抖动值太大
						if ((prev_depth - depth_data->depth) != 0)
						{
							int i;
							// uint32_t time = (prev_depth - target_sample_depth) * (enc_update_time - prev_update_time) / (prev_depth - depth_data->depth) + prev_update_time;
							for (i = 0; i < 2; i++)
							{
								// int total_flow = get_total_flow_by_time(i, time);
								int total_flow = get_total_flow_by_time(i, pflow[i].update_time);
								int flow = total_flow - depth_data->last_total_flow[i];
								if (flow < 0)
								{
									flow = 0;
								}
								depth_data->depth_flow[i] = flow;
								depth_data->last_total_flow[i] = total_flow;
							}
							depth_data->sample_count++;
							add_recod_item();
							target_sample_depth -= depth_config->sample_depth;
						}
					}
				}
				else
				{
					// depth_data->up_down = 0;
				}

				if (depth_data->depth < depth_config->inc_pile_depth) // 小于指定深度时才允许行走清零
				{
					if (pMoveCtx->pile_inc_req == 1)
					{
						pMoveCtx->pile_inc_req = 0;
						reset_depth();
					}
				}
				else
				{
					if (pMoveCtx->pile_inc_req == 1)
					{
						pMoveCtx->pile_inc_req = 0;
					}
				}
			}
			prev_depth = depth_data->depth;
			prev_update_time = enc_update_time;
			last_enc_value = enc_value;
		}
		////////////////////////////////////////////////////////////////////
		// 每隔500ms向蓝牙发送一次数据
		if (bt_time_count++ > 50)
		{
			ESP_LOGI(TAG, "bt_time_count++ > 50\n");
			bt_time_count = 0;
			send_data_to_bt();
		}
		////////////////////////////////////////////////////////////////////
		// if (++count > 100)
		// {
		// 	count = 0;
		// 	ESP_LOGI(TAG, "encoder:%d", encoder->get_counter_value(encoder));
		// }
		vTaskDelay(10);
	}
}

void add_recod_item(void)
{
	// 每10cm计算一次
	record->count = depth_data->sample_count;
	memmove(&record->item[1], &record->item[0], sizeof(record->item[0]) * 9);
	record->item[0].flow[0] = depth_data->depth_flow[0];
	record->item[0].flow[1] = depth_data->depth_flow[1];
	record->item[0].total_flow[0] = depth_data->last_total_flow[0];
	record->item[0].total_flow[1] = depth_data->last_total_flow[1];

	if (depth_config->move_current_channel == 2) // 1
	{
		record->item[0].current1 = gWordVar[AC_CURRENT_REG_ADDR];
		record->item[0].current2 = gWordVar[AC_CURRENT_REG_ADDR + 1];
	}
	else if (depth_config->move_current_channel == 1)
	{
		record->item[0].current1 = gWordVar[AC_CURRENT_REG_ADDR];
		record->item[0].current2 = gWordVar[AC_CURRENT_REG_ADDR + 2];
	}
	else
	{
		record->item[0].current1 = gWordVar[AC_CURRENT_REG_ADDR + 1];
		record->item[0].current2 = gWordVar[AC_CURRENT_REG_ADDR + 2];
	}

	record->item[0].tilt_x = (short)gWordVar[TILT_SENSER_ADDR];
	record->item[0].tilt_y = (short)gWordVar[TILT_SENSER_ADDR + 1];

	record->item[0].speed = depth_data->speed; // 每500ms计算一次
	record->item[0].depth = depth_data->depth; // 每10ms计算一次
}
extern int zero_totalflow_req;
extern void save_pile_id(void);
void reset_depth(void)
{
	// uint16_t pile_id;
	last_sample_depth = 0;
	last_flow[0] = 0;
	last_flow[1] = 0;
	prev_depth = 0;
	prev_update_time = 0;
	target_sample_depth = 0;
	zero_totalflow_req = 1;
	if (record->count > 0)
	{
		record->count = 0;
		memset(record->item, 0, sizeof(record->item));
	}
	memset(depth_data, 0, sizeof(*depth_data));
	if (record->max_depth >= depth_config->min_valid_depth) // 上一个桩有一定深度数据
	{
		save_pile_id();
		last_pile_id++;
		gWordVar[LAST_PILE_ID_ADDR] = last_pile_id;
	}
	memset(record, 0, sizeof(*record));
	record->pile_id = last_pile_id;
	// ec11_pcnt_clear(0);
	depth_data->depth_offset = -depth_config->depth_offset;
	depth_data->depth = depth_config->depth_offset;
	enc1_value = 0;
	enc1_update_time = 0;
	depth_data->up_down = 1; // 默认工作
}

void DEPTH_init()
{

	// depth_init();

	pcnt_rotary_encoder_init();
	capture_depth_init();
	xTaskCreate(depth_task, "depth_task", 4096, NULL, 10, NULL);
}