#include "soft_hd_water_pump.h"
#include "soft_water_device.h"
#include "common.h"
#include "string.h"
#include "soft_p_2_c.h"
#include "soft_flow.h"
#include "soft_seed_device.h"
#include "soft_crc.h"
#include "soft_version.h"

// 惠达采用小端存储

HWTail PumpControlTail = {0};
HWTail NozzleControlTail = {0};
HDpump HD_pump = {0};
HDMsg *_HDMsg = NULL;

HDnozzle NozzleMsg[5] = {0};
_nozzle_id Nozzle_id[4] = {0};

// 设置喷头id标志位
bool SetNozzleId = false;
// 识别喷头标志位
bool IdentifyNozzle = false;
// 设置喷头id
uint8_t SetNozzleIdNum = 4;
// 设置喷头设备id
uint32_t SetDevId = 1399067365;
// uint16_t curNodeID = 0;
// 超时时间
uint32_t overtime = 0;

void HuiDaCanRecvHookFunction(uint32_t id, uint8_t *recv_buf, uint8_t len)
{
	uint16_t TypeID = 0;
	uint8_t SrcNodeID = (id & HD_CANID_SRCNODE_MASK) >> HD_CANID_SRCNODE_POS;

	if (SrcNodeID == HD_CANID_PUMP_ID)
	{
	}
	else if (SrcNodeID >= HD_CANID_NOZZLE_0_ID && SrcNodeID <= HD_CANID_NOZZLE_4_ID)
	{
	}
	else
	{
		return;
	}

	TypeID = recv_buf[0];

	switch (SrcNodeID)
	{
	case HD_CANID_PUMP_ID:
		_HDMsg = &HD_pump.MultiMsg;

		Dev.Pump_Link.connect_status = COMP_NORMAL;
		Dev.Pump_Link.recv_time = HAL_GetTick();
		Dev.Pump.facid = FAC_HD_PUMP;

		// 多帧消息处理
		// 多帧发送需要二次打包
		// 判断接收的消息类型
		switch (_HDMsg->Msg_id)
		{
		// 非多帧消息
		case none:
			break;

		case PSL:
			HD_frame_process(_HDMsg, recv_buf, len);
			if (_HDMsg->finish == true)
			{
				memcpy(&HD_pump.pump1_pwm, &_HDMsg->buffer[3], 2);
				little_to_big_16(&HD_pump.pump1_pwm);

				memcpy(&HD_pump.pump2_pwm, &_HDMsg->buffer[5], 2);
				little_to_big_16(&HD_pump.pump2_pwm);

				memcpy(&HD_pump.pump1_speed, &_HDMsg->buffer[7], 2);
				little_to_big_16(&HD_pump.pump1_speed);

				memcpy(&HD_pump.pump2_speed, &_HDMsg->buffer[9], 2);
				little_to_big_16(&HD_pump.pump2_speed);

				uint16_t fault_code = 0;
				memcpy(&fault_code, &_HDMsg->buffer[11], 2);
				little_to_big_16(&fault_code);
				memcpy(&HD_pump.pump1_fault_code, &fault_code, 2);

				fault_code = 0;
				memcpy(&fault_code, &_HDMsg->buffer[13], 2);
				little_to_big_16(&fault_code);
				memcpy(&HD_pump.pump2_fault_code, &fault_code, 2);

				memcpy(&HD_pump.pump1_vol, &_HDMsg->buffer[15], 2);
				little_to_big_16(&HD_pump.pump1_vol);

				memcpy(&HD_pump.pump2_vol, &_HDMsg->buffer[17], 2);
				little_to_big_16(&HD_pump.pump2_vol);

				HD_pump.pump1_current = _HDMsg->buffer[19];
				HD_pump.pump2_current = _HDMsg->buffer[20];
				HD_pump.pump1_temp = _HDMsg->buffer[21];
				HD_pump.pump2_temp = _HDMsg->buffer[22];

				memcpy(&HD_pump.MCU_status, &_HDMsg->buffer[23], 1);

				_HDMsg->finish = false;
				_HDMsg->Msg_id = none;
			}
			// 处理完毕跳过单帧消息处理，防止重复处理错误
			goto next;
			// break;
		}

		switch (TypeID)
		{
		case HD_HIGH_FREQ_REPORT_ID:
			memcpy(&HD_pump.flow1, &recv_buf[1], 2);
			little_to_big_16(&HD_pump.flow1);

			memcpy(&HD_pump.flow2, &recv_buf[3], 2);
			little_to_big_16(&HD_pump.flow2);

			memcpy(&HD_pump.pump1_status, &recv_buf[5], 1);
			memcpy(&HD_pump.pump2_status, &recv_buf[6], 1);

			break;
		case HD_LOW_FREQ_REPORT_ID:
			memcpy(&_HDMsg->length, &recv_buf[1], 2);

			little_to_big_16(&_HDMsg->length);
			// 如果接收的消息长度正确
			if (_HDMsg->length == PSL_LEN)
			{
				_HDMsg->rxindex = 0;
				_HDMsg->LastTail.HWTailByte = recv_buf[7];

				memcpy(&_HDMsg->buffer, recv_buf, 7);
				_HDMsg->rxindex += 7;
				_HDMsg->Msg_id = PSL;
			}
			else
			{
				_HDMsg->rxindex = 0;
				_HDMsg->Msg_id = none;
			}
			break;

		case HD_FLOWMETER_PULSE_ID:
			memcpy(&HD_pump.flowmeter1_pulse_interval, &recv_buf[1], 2);
			little_to_big_16(&HD_pump.flowmeter1_pulse_interval);

			memcpy(&HD_pump.flowmeter2_pulse_interval, &recv_buf[3], 2);
			little_to_big_16(&HD_pump.flowmeter2_pulse_interval);
			break;
		case HD_HEART_ID:
			memcpy(&HD_pump.devID, &recv_buf[1], 4);
			little_to_big_32(&HD_pump.devID);
			break;
		}
		break;

	case HD_CANID_NOZZLE_0_ID ... HD_CANID_NOZZLE_4_ID:

	{
		HDnozzle *HD_nozzle = NULL;
		HD_nozzle = &NozzleMsg[SrcNodeID - HD_CANID_NOZZLE_0_ID];
		_HDMsg = &HD_nozzle->MultiMsg;

		Dev.Nozzle_Link.connect_status = COMP_NORMAL;
		Dev.Nozzle_Link.recv_time = HAL_GetTick();
		Dev.Nozzle.facid = FAC_HD_NOZZLE;

		HD_nozzle->deviceLink = COMP_NORMAL;
		HD_nozzle->linkTime = HAL_GetTick();
		HD_nozzle->nodeId = SrcNodeID;

		// 多帧消息处理
		// 多帧发送需要二次打包
		// 判断接收的消息类型
		switch (_HDMsg->Msg_id)
		{
		// 非多帧消息
		case none:
		{
			break;
		}

		case SSL:
		{
			HD_frame_process(_HDMsg, recv_buf, len);
			if (_HDMsg->finish == true)
			{
				uint16_t _fault_code = 0;
				memcpy(&_fault_code, &_HDMsg->buffer[3], 2);
				little_to_big_16(&_fault_code);
				memcpy(&HD_nozzle->fault_code, &_fault_code, 2);

				memcpy(&HD_nozzle->nozzle_vol, &_HDMsg->buffer[5], 2);
				little_to_big_16(&HD_nozzle->nozzle_vol);

				memcpy(&HD_nozzle->nozzle_current, &_HDMsg->buffer[7], 1);
				memcpy(&HD_nozzle->solenoid_valve_current, &_HDMsg->buffer[8], 1);
				memcpy(&HD_nozzle->nozzle_temp, &_HDMsg->buffer[9], 1);

				memcpy(&HD_nozzle->motor_power, &_HDMsg->buffer[10], 2);
				little_to_big_16(&HD_nozzle->motor_power);

				memcpy(&HD_nozzle->over_current_num, &_HDMsg->buffer[12], 2);
				little_to_big_16(&HD_nozzle->over_current_num);

				memcpy(&HD_nozzle->hard_over_current_num, &_HDMsg->buffer[14], 2);
				little_to_big_16(&HD_nozzle->hard_over_current_num);

				memcpy(&HD_nozzle->blockage_frequency, &_HDMsg->buffer[15], 1);

				_HDMsg->finish = false;
				_HDMsg->Msg_id = none;
			}
			// 跳过单帧消息处理，防止重复处理错误
			goto next;
			// break;
		}
		}

		switch (TypeID)
		{
		case HD_NOZZLE_HIGH_FREQ_ID:
		{
			memcpy(&HD_nozzle->speed, &recv_buf[1], 2);
			little_to_big_16(&HD_nozzle->speed);

			memcpy(&HD_nozzle->solenoid_valve_status, &recv_buf[3], 1);

			memcpy(&HD_nozzle->speed_command, &recv_buf[4], 2);
			little_to_big_16(&HD_nozzle->speed_command);

			memcpy(&HD_nozzle->nozzle_status, &recv_buf[6], 1);
			break;
		}
		case HD_NOZZLE_LOW_FREQ_ID:
		{
			memcpy(&_HDMsg->length, &recv_buf[1], 2);

			little_to_big_16(&_HDMsg->length);
			// 如果接收的消息长度正确
			if (_HDMsg->length == SSL_LEN)
			{
				_HDMsg->rxindex = 0;
				_HDMsg->LastTail.HWTailByte = recv_buf[7];

				memcpy(&_HDMsg->buffer, recv_buf, 7);
				_HDMsg->rxindex += 7;
				_HDMsg->Msg_id = SSL;
			}
			else
			{
				_HDMsg->rxindex = 0;
				_HDMsg->Msg_id = none;
			}
			break;
		}
		case HD_HEART_ID:
		{
			memcpy(&HD_nozzle->devID, &recv_buf[1], 4);
			little_to_big_32(&HD_nozzle->devID);
			// 添加设备id
			add_devId(SrcNodeID, HD_nozzle->devID);

			if (SetNozzleId == true && HD_nozzle->devID == SetDevId)
			{
				if (SrcNodeID == SetNozzleIdNum + HD_CANID_NOZZLE_0_ID)
				{
					SetNozzleId = false;
				}
				else
					overtime = millis();
			}
			break;
		}
		case HD_IDENTIFY_ID:
		{
			if (recv_buf[1] == 0 && IdentifyNozzle == true)
			{
				uint32_t dev_id;
				memcpy(&dev_id, &recv_buf[2], 4);
				little_to_big_32(&dev_id);
				if (dev_id == SetDevId)
					IdentifyNozzle = false;
				else
					overtime = millis();
			}
			else
				overtime = millis();

			break;
		}
		}
	}
	break;
	}

next:;
}

void HD_pump_func(void)
{
	// test

	// static uint32_t time_1hz = 0;
	if (Dev.Pump_Link.connect_status == COMP_NORMAL && Dev.Pump.facid == FAC_HD_PUMP)
	{
		/*
		if(Check_Timer_Ready(&time_1hz,_1_HZ_))
		{
			HW_CanGetESCInfomation();
			HW_CanSetESCInfomation();
		}
*/

		HD_can_sendmsg(HD_WATER_PUMP_CONTROL_ID, NULL);
		// HD_can_sendmsg( 1300 , 1300);
	}

	if (Dev.Nozzle_Link.connect_status == COMP_NORMAL && Dev.Nozzle.facid == FAC_HD_NOZZLE)
	{
		/*
		if(Check_Timer_Ready(&time_1hz,_1_HZ_))
		{
			HW_CanGetESCInfomation();
			HW_CanSetESCInfomation();
		}
*/
		if (IdentifyNozzle == true)
		{
			/*
			if (millis() - overtime > 5000)
			{
				IdentifyNozzle = false;
			}*/

			for (uint8_t i = 0; i < 4; i++)
			{
				if (Nozzle_id[i].devID == SetDevId)
				{
					HD_can_sendmsg(HD_IDENTIFY_ID, &Nozzle_id[i]);
				}
			}
		}
		else if (SetNozzleId == true)
		{
			/*
			if (millis() - overtime > 5000)
			{
				IdentifyNozzle = false;
			}*/

			for (uint8_t i = 0; i < 4; i++)
			{
				if (Nozzle_id[i].devID == SetDevId)
				{
					HD_can_sendmsg(HD_SET_NOZZLE_ID, &Nozzle_id[i]);
				}
			}
		}
		else
			HD_can_sendmsg(HD_NOZZLE_CONTROL_ID, NULL);
		// HD_can_sendmsg( 1300 , 1300);
	}
}

void HD_can_sendmsg(uint8_t msg_id, _nozzle_id *Nozzle_id)
{
	uint32_t canID;
	uint32_t _dev_ID;
	uint8_t can_buf[8] = {0};
	switch (msg_id)
	{
	case HD_WATER_PUMP_CONTROL_ID:
	{
		canID = ((HD_PRI_HIGHEST << HD_CANID_PRI_POS) & HD_CANID_PRI_MASK) |
				((HD_TYPE_SER << HD_CANID_SNM_POS) & HD_CANID_SNM_MASK) |
				((HD_TYPE_REQ << HD_CANID_RNR_POS) & HD_CANID_RNR_MASK) |
				((HD_CANID_REV_23 << HD_CANID_REV_23_POS) & HD_CANID_REV_23_MASK) |
				((HD_CANID_SERID << HD_CANID_SERID_POS) & HD_CANID_SERID_MASK) |
				((HD_CANID_PUMP_ID << HD_CANID_DESNODE_POS) & HD_CANID_DESNODE_MASK) |
				((FMU_NODE_ID_HD << HD_CANID_SRCNODE_POS) & HD_CANID_SRCNODE_MASK);

		can_buf[0] = HD_WATER_PUMP_CONTROL_ID;

		uint16_t _pwm1 = (uint16_t)pmu_pin.pump1;
		uint16_t _pwm2 = (uint16_t)pmu_pin.pump2;
		if (_pwm1 > 1800)
			_pwm1 = 1800;
		if (_pwm2 > 1800)
			_pwm2 = 1800;
		little_to_big_16(&_pwm1);
		little_to_big_16(&_pwm2);
		memcpy(&can_buf[1], &_pwm1, 2);
		memcpy(&can_buf[3], &_pwm2, 2);

		PumpControlTail.HWTailBit.start = 1;
		PumpControlTail.HWTailBit.end = 1;
		PumpControlTail.HWTailBit.toggle = 1;
		can_buf[5] = PumpControlTail.HWTailByte;

		can_send_msg_normal(can_buf, 6, canID);

		PumpControlTail.HWTailBit.tranid++;
		break;
	}
	case HD_NOZZLE_CONTROL_ID:
	{
		uint16_t rpm = 0;
		uint8_t open;
		for (uint8_t i = 1; i <= 4; i++)
		{
			switch (i)
			{
			case 1:
				rpm = pmu_pin.nozz1_fm;
				break;
			case 2:
				rpm = pmu_pin.nozz2_zp;
				break;
			case 3:
				rpm = pmu_pin.nozz3;
				break;
			case 4:
				rpm = pmu_pin.nozz4;
				break;
			default:
				break;
			}
			canID = ((HD_PRI_HIGHEST << HD_CANID_PRI_POS) & HD_CANID_PRI_MASK) |
					((HD_TYPE_SER << HD_CANID_SNM_POS) & HD_CANID_SNM_MASK) |
					((HD_TYPE_REQ << HD_CANID_RNR_POS) & HD_CANID_RNR_MASK) |
					((HD_CANID_REV_23 << HD_CANID_REV_23_POS) & HD_CANID_REV_23_MASK) |
					((HD_CANID_SERID << HD_CANID_SERID_POS) & HD_CANID_SERID_MASK) |
					(((HD_CANID_NOZZLE_0_ID + i) << HD_CANID_DESNODE_POS) & HD_CANID_DESNODE_MASK) |
					((FMU_NODE_ID_HD << HD_CANID_SRCNODE_POS) & HD_CANID_SRCNODE_MASK);
			open = 1;
			if (rpm <= 1020)
			{
				rpm = 0;
				open = 0;
			}
			else if (rpm >= 2000)
				rpm = 14000;
			else
				rpm = (rpm - 1020) * ((14000 - 2000) / (2000 - 1000));
			can_buf[0] = HD_NOZZLE_CONTROL_ID;
			// rpm = 14000;
			little_to_big_16(&rpm);
			memcpy(&can_buf[1], &rpm, 2);
			memcpy(&can_buf[3], &open, 1);

			PumpControlTail.HWTailBit.start = 1;
			PumpControlTail.HWTailBit.end = 1;
			PumpControlTail.HWTailBit.toggle = 1;
			can_buf[4] = PumpControlTail.HWTailByte;

			can_send_msg_normal(can_buf, 5, canID);

			PumpControlTail.HWTailBit.tranid++;
		}

		break;
	}
	case HD_IDENTIFY_ID:
	{
		if (Nozzle_id == NULL)
			return;
		canID = ((HD_PRI_HIGHEST << HD_CANID_PRI_POS) & HD_CANID_PRI_MASK) |
				((HD_TYPE_SER << HD_CANID_SNM_POS) & HD_CANID_SNM_MASK) |
				((HD_TYPE_REQ << HD_CANID_RNR_POS) & HD_CANID_RNR_MASK) |
				((HD_CANID_REV_23 << HD_CANID_REV_23_POS) & HD_CANID_REV_23_MASK) |
				((HD_CANID_SERID << HD_CANID_SERID_POS) & HD_CANID_SERID_MASK) |
				((Nozzle_id->nodeId << HD_CANID_DESNODE_POS) & HD_CANID_DESNODE_MASK) |
				((FMU_NODE_ID_HD << HD_CANID_SRCNODE_POS) & HD_CANID_SRCNODE_MASK);

		can_buf[0] = HD_IDENTIFY_ID;
		// rpm = 14000;
		_dev_ID = SetDevId;
		little_to_big_32(&_dev_ID);
		memcpy(&can_buf[1], &_dev_ID, 4);
		can_buf[5] = 0x00;
		can_buf[6] = 0x00;
		PumpControlTail.HWTailBit.start = 1;
		PumpControlTail.HWTailBit.end = 1;
		PumpControlTail.HWTailBit.toggle = 1;
		can_buf[7] = PumpControlTail.HWTailByte;

		can_send_msg_normal(can_buf, 8, canID);

		PumpControlTail.HWTailBit.tranid++;
		break;
	}
	case HD_SET_NOZZLE_ID:
	{
		if (Nozzle_id == NULL)
			return;
		canID = ((HD_PRI_HIGHEST << HD_CANID_PRI_POS) & HD_CANID_PRI_MASK) |
				((HD_TYPE_SER << HD_CANID_SNM_POS) & HD_CANID_SNM_MASK) |
				((HD_TYPE_REQ << HD_CANID_RNR_POS) & HD_CANID_RNR_MASK) |
				((HD_CANID_REV_23 << HD_CANID_REV_23_POS) & HD_CANID_REV_23_MASK) |
				((HD_CANID_SERID << HD_CANID_SERID_POS) & HD_CANID_SERID_MASK) |
				((Nozzle_id->nodeId << HD_CANID_DESNODE_POS) & HD_CANID_DESNODE_MASK) |
				((FMU_NODE_ID_HD << HD_CANID_SRCNODE_POS) & HD_CANID_SRCNODE_MASK);

		can_buf[0] = HD_SET_NOZZLE_ID;

		_dev_ID = SetDevId;
		little_to_big_32(&_dev_ID);
		memcpy(&can_buf[1], &_dev_ID, 4);
		can_buf[5] = SetNozzleIdNum + HD_CANID_NOZZLE_0_ID;
		can_buf[6] = 0;

		PumpControlTail.HWTailBit.start = 1;
		PumpControlTail.HWTailBit.end = 1;
		PumpControlTail.HWTailBit.toggle = 1;
		can_buf[7] = PumpControlTail.HWTailByte;

		can_send_msg_normal(can_buf, 8, canID);

		PumpControlTail.HWTailBit.tranid++;
		break;
	}
	default:
		break;
	}
}

// 多帧消息处理
void HD_frame_process(HDMsg *_HDMsg, uint8_t *recv_buf, uint32_t len)
{
	_HDMsg->Tail.HWTailByte = recv_buf[len - 1];

	// 如果收帧连续，数据进入buffer
	if (_HDMsg->Tail.HWTailBit.toggle != _HDMsg->LastTail.HWTailBit.toggle)
	{
		memcpy(&_HDMsg->buffer[_HDMsg->rxindex], recv_buf, (len - 1));
		_HDMsg->LastTail.HWTailByte = _HDMsg->Tail.HWTailByte;
		_HDMsg->rxindex = _HDMsg->rxindex + len - 1;
	}
	// 如果是最后一帧
	if (_HDMsg->Tail.HWTailBit.end == 1 && _HDMsg->rxindex == _HDMsg->length + length_len + msg_id_len + crc_len)
	{
		memcpy(&_HDMsg->crc, &_HDMsg->buffer[_HDMsg->rxindex - 2], 2);
		little_to_big_16(&_HDMsg->crc);

		// 校验
		if (_HDMsg->crc == crcAdd(0xFFFF, _HDMsg->buffer, _HDMsg->rxindex - 2))
		{
			_HDMsg->finish = true;
		}
		else
			_HDMsg->crc_wrong++;

		// 无论是否校验通过，都重置
		_HDMsg->Msg_id = none;
		_HDMsg->rxindex = 0;
	}
	// 超出字节重置
	else if (_HDMsg->rxindex >= (_HDMsg->length + length_len + msg_id_len + crc_len))
	{
		_HDMsg->rxindex = 0;
		_HDMsg->Msg_id = none;
		_HDMsg->len_wrong++;
	}
}

// 添加设备id到Nozzle_id数组
void add_devId(uint8_t SrcId, uint32_t devId)
{
	uint8_t p = 0;
	for (uint8_t i = 0; i < 4; i++)
	{
		// 超时视为断开连接，清除设备id
		if (Nozzle_id[i].devID != 0 && millis() - Nozzle_id[i].linkTime >= 2000)
			Nozzle_id[i].devID = 0;
	}
	for (uint8_t i = 0; i < 4; i++)
	{
		// 如果设备id已经存在，更新时间
		if (Nozzle_id[i].devID == devId)
		{
			Nozzle_id[i].linkTime = millis();
			Nozzle_id[i].nodeId = SrcId;
			return;
		}
		// 空位，记录
		else if (Nozzle_id[i].devID == 0)
		{
			p = i;
		}
	}
	// 当前id与之前的id都不相同，添加
	// 如果数组已满，不添加
	if (p < 4)
	{
		Nozzle_id[p].devID = devId;
		Nozzle_id[p].nodeId = SrcId;
		Nozzle_id[p].linkTime = millis();
	}
}

void little_to_big_32(uint32_t *value)
{
	if (value == NULL)
	{
		return;
	}
	uint32_t _value = *value;
	_value = ((_value & 0xFF) << 24) |
			 ((_value & 0xFF00) << 8) |
			 ((_value >> 8) & 0xFF00) |
			 ((_value >> 24) & 0xFF);
	*value = _value;
}

void little_to_big_16(uint16_t *value)
{
	if (value == NULL)
	{
		return;
	}
	uint16_t _value = *value;
	_value = ((_value & 0xFF) << 8) |
			 ((_value >> 8) & 0xFF);
	*value = _value;
}

uint32_t millis()
{
	return HAL_GetTick();
}