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

#include "rotary_encoder.h"
#include "esp_log.h"
#include "esp_check.h"
#include "soc/rtc.h"
#include "driver/mcpwm.h"
// #include "driver/mcpwm_prelude.h"
#include "config.h"
#include "../../../../components/bt/host/bluedroid/api/include/api/esp_gatts_api.h"
// #include "bt_server.h"
// #include "../ble_gatts_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 PILE_DIR_NONE		0x01
#define PILE_DIR_UP			0x02
#define PILE_DIR_DOWN		0x03

// #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;
extern uint16_t *cur_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
// {
// 	uint16_t pile_work_state_and_direction;	 // 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]; //
// 	int16_t depth_offset;		 // 22
// 	uint16_t one_pile_work_time;//23 系统工作时间
// 								 // 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->pile_work_state_and_direction = (PILE_STATE_STOP | PILE_DIR_NONE); // 默认不工作
}

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[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 calculate_depth_speed(int speed_enc_value)
{
	static int speed_last_enc_value = -1;
	static int64_t speed_last_time = -1;
	int64_t speed_time = esp_timer_get_time();
	
	if (speed_last_enc_value == -1)	// 第一次，记录值，并不计算
	{
		goto exit;
	}

	depth_data->speed = (speed_enc_value - speed_last_enc_value) * depth_config->N / depth_config->M * 600000ll / (speed_time - speed_last_time);
	// ESP_LOGI(TAG, "speed:%d", depth_data->speed);
exit:
	speed_last_enc_value = speed_enc_value;
	speed_last_time = speed_time;
}


int last_depth_flag = 0;			// 0小于允许换桩深度，1大于允许换桩深度
int depth_over_inc_pile_flag = 0;	// 深度过允许换桩深度标志
/**
 * _enc1_update_time	编码器1上一次更新时间
 * enc1_update_time		编码器1当前更新时间
 * time_diff			两次编码器更新时间的绝对差值
 * depth_config			预设好的深度数据
 * enc1_value			编码器1值，在深度中断中获得
 * prev_depth			上一次的深度
*/
void depth_task(void *arg)
{
	ESP_LOGI(TAG, "depth_task start\n");

	int time_diff = 0;
	int speed_calc_count = 0;
	int enc_value = 0;
	int64_t work_start_time = 0;
	uint16_t last_work_state = 0;

	// depth_data->depth_offset = -depth_config->depth_offset;
	depth_data->pile_work_state_and_direction = PILE_STATE_STOP;
	last_work_state = PILE_STATE_STOP;
	// record->pile_id = ++last_pile_id;
	// record->pile_id = *cur_pile_id;
	// send_to_bt1.pile_id = ++last_pile_id;

	// gWordVar[LAST_PILE_ID_ADDR] = last_pile_id;

	while (1)
	{
		depth_data->depth_offset = -depth_config->depth_offset;
		record->pile_id = *cur_pile_id;

		if (_enc1_update_time != enc1_update_time)	/* 深度数据更新 */
		{
		//	ESP_LOGI(TAG, "_enc1_update_time != enc1_update_time\n");
			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 = %d\n",time_diff);
			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;
			}

			// ESP_LOGI(TAG, "depth_data->depth:0x%x", (unsigned int)depth_data->depth);

			uint16_t pile_work_state = (depth_data->pile_work_state_and_direction & 0xff00);
			if (pile_work_state == PILE_STATE_WORK)
			{
			//	ESP_LOGI(TAG, "pile_work_state == PILE_STATE_WORK\n");
				/*如果机器从停止状态->工作状态，则重新记录工作开始时间*/
				if(last_work_state == PILE_STATE_STOP){
					work_start_time = esp_timer_get_time();
					depth_data->one_pile_work_time = 0;
				}
				int64_t current_work_time = esp_timer_get_time();
				if(work_start_time != 0){
					depth_data->one_pile_work_time = (uint16_t)((current_work_time - work_start_time)/1000000); // s
					// ESP_LOGI(TAG, "time : %ud", (int)depth_data->one_pile_work_time);
				}

				/*下钻，深度增加 计算采样深度流量*/
				if (enc_value > last_enc_value)
				{
				//	ESP_LOGI(TAG, "enc_value > last_enc_value\n");
					uint8_t pile_work_dir = (depth_data->pile_work_state_and_direction & 0xff);
					if(pile_work_dir != PILE_DIR_DOWN)
					{
						// 方向改变，更新目标采样深度
						depth_data->pile_work_state_and_direction = ((depth_data->pile_work_state_and_direction & 0xff00) | PILE_DIR_DOWN);
						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)
				{
				//	ESP_LOGI(TAG, "enc_value < last_enc_value\n");
					uint8_t pile_work_dir = (depth_data->pile_work_state_and_direction & 0xff);
					if (pile_work_dir != PILE_DIR_UP)
					{ 
						// 方向改变
						depth_data->pile_work_state_and_direction = ((depth_data->pile_work_state_and_direction & 0xff00) | PILE_DIR_UP);
						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->pile_work_state_and_direction = ((depth_data->pile_work_state_and_direction & 0xff00) | PILE_DIR_NONE);
				}

				if (last_depth_flag == 0 && depth_data->depth >= depth_config->inc_pile_depth)
				{
					depth_over_inc_pile_flag = 1;
				}

				if (depth_data->depth < depth_config->inc_pile_depth)
				{
					last_depth_flag = 0;
				}
				else 
				{
					last_depth_flag = 1;
				}


				// 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;
				// 	}
				// }
			}
			else if((depth_data->pile_work_state_and_direction & 0xff00) == PILE_STATE_PAUSE)
			{

			}
			else if((depth_data->pile_work_state_and_direction & 0xff00) == PILE_STATE_STOP)
			{
				work_start_time = 0;
			}
			last_work_state = pile_work_state;
			prev_depth = depth_data->depth;
			prev_update_time = enc_update_time;
			last_enc_value = enc_value;
		}

		vTaskDelay(pdMS_TO_TICKS(100));

		// 500ms计算一次速度
		if (++speed_calc_count >= 5)
		{
			calculate_depth_speed(enc1_value);
			speed_calc_count = 0;
		}
	}
}

#if 0
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;

	/*获取当前时间*/
	uint16_t work_start_time = 0;
	uint8_t is_work = 0;
	
	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 = %d\n",time_diff);
			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;
			}

			// 500ms计算一次速度
			if (speed_calc_count++ > 50) 
			{
				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)
			{
				is_work ++;
				if(is_work == 1){
					int64_t time = esp_timer_get_time();
					if(time < 0){
						work_start_time = 0;
					}
					work_start_time = (uint16_t)(time/1000000);
					depth_data->one_pile_work_time = 0;
				}
				else{
					int64_t time = esp_timer_get_time();
					if(time > 0){
						if((uint16_t)(time/1000000) > work_start_time){
							depth_data->one_pile_work_time = ((uint16_t)(time/1000000) - work_start_time);
						}
					}
				}

				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;
					}
				}
			}
			else{
				is_work = 0;
				depth_data->one_pile_work_time = 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);
	}
}
#endif
void add_recod_item(void)
{
	// 每10cm计算一次
	record->count = depth_data->sample_count;
	// ESP_LOGI(TAG, "add_recod_item:id:%d   count:%d", record->pile_id, record->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->pile_work_state_and_direction = PILE_STATE_STOP | PILE_DIR_NONE; // 默认不工作
// }

void zero_depth(void)
{
	prev_depth = 0;
	prev_update_time = 0;
	target_sample_depth = 0;

	// depth_data->depth = 0;
	depth_data->depth_flow[0] = 0;
	depth_data->depth_flow[1] = 0;
	depth_data->last_total_flow[0] = 0;
	depth_data->last_total_flow[1] = 0;
	depth_data->one_pile_work_time = 0;
	depth_data->sample_count = 0;
	depth_data->speed = 0;
	
	memset(record, 0, sizeof(*record));
	// enc1_value = 0;
	// enc1_update_time = 0;
}


void DEPTH_init()
{

	// depth_init();

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