#include "flash.h"
#include "DengFOC.h"
#include "ang_sensor.h"
#include "gd32c10x_crc.h"
#include "gd32c10x_fmc.h"
#include "math.h"
#include <Can.h>
#include <Filter.h>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <stdint.h>

FlashEEPROM flashEEP = FlashEEPROM();
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
extern FOC_Controlroller FOC; // FOC控制
constexpr u8 SIZE_U32 = sizeof(uint32_t);
constexpr u8 SIZE_FLOAT = sizeof(float);
constexpr u8 DEFAULT_VALUE = 0;
constexpr u8 NUM_SAVED = 6;
#define EEPDEBUG 0

// FlashEEPROM类实现
FlashEEPROM::FlashEEPROM() {
	// 构造函数可以为空，或者做一些初始化工作
}

/**
 * @brief  初始化Flash EEPROM模拟
 * @param  None
 * @retval None
 */
u8 FlashEEPROM::init() {
	rcu_periph_clock_enable(RCU_CRC);
	crc_deinit(); // 初始化CRC模块

	FlashEepromStatus status = FLASH_EEPROM_OK;
	// 读取系统配置
	read_saved_system_config();

	if (EEPROMStatus == FLASH_EEPROM_OK) {
		return FLASH_EEPROM_OK;
	} else {
		return FLASH_EEPROM_READ_ERROR;
	}
	return 1; // 正常不会返回这个，未预料的异常情况返回
}

/**
 * @brief  读取EEPROM数据
 * @param  virtualAddr: 虚拟地址
 * @param  data: 存储读取数据的指针
 * @retval EEPROMStatus: 是否成功
 */
u8 FlashEEPROM::read_saved_system_config(void) {
	// 读取保存的参数
	u32 CRC_NUM = 0x0000;
	FlashEepromStatus status_read[NUM_SAVED + 1] = {FLASH_EEPROM_OK};
	float data[NUM_SAVED] = {0.0f};
	// 尝试从Flash读取参数，如果不存在则使用默认值
	status_read[1] = readFloat(ADDR_CONTROL_MODE, &data[0]);
	status_read[2] = readFloat(ADDR_VOLTAGE_LIMIT, &data[1]);
	status_read[3] = readFloat(ADDR_VEL_LIMIT, &data[2]);
	status_read[4] = readFloat(ADDR_ANGLE, &data[3]);
	status_read[5] = readFloat(ADDR_TARGET_ANGLE, &data[4]);
	status_read[6] = readFloat(ADDR_TARGET_VEL, &data[5]);

	// 读取失败则使用默认值
	for (u8 i = 1; i < NUM_SAVED + 1; i++) {
		if (status_read[i] ==
			FLASH_EEPROM_DATA_NOT_FOUND) { // 首次读取，不保存报错
			data[i - 1] = DEFAULT_VALUE;
		} else if (status_read[i] != FLASH_EEPROM_OK) { // 其余错误，保存报错
			data[i - 1] = DEFAULT_VALUE;
			status_read[0] = status_read[i];
		}
	}
	// 读取到全部数据
	if (status_read[0] == FLASH_EEPROM_OK) {
		u32 next_address = EEPROM_END_ADDRESS;
		// 遍历Flash页面寻找下一个可写入位置
		for (u32 address = EEPROM_DATA_START_ADDRESS - SIZE_U32;
			 address > EEPROM_START_ADDRESS; address -= SIZE_U32) {
			// 安全读取Flash数据
			volatile u32 *flash_ptr = (volatile u32 *)address;
			u32 data_word = *flash_ptr;
			// 找到空位置
			if (data_word == 0xFFFFFFFF) {
				next_address = address + SIZE_U32; // 上一个位置为有效存储的数据
				break;
			}
			// 更新下一个可能的写入位置（如果需要格式化）
			next_address = address - SIZE_U32;
		}
		if (next_address == EEPROM_DATA_START_ADDRESS) {
			// 从来没有存储过，直接返回
			return FLASH_EEPROM_OK;
		}

		crc_data_register_reset(); // 确保CRC计算从初始状态开始
		CRC_NUM = crc_block_data_calculate((uint32_t *)data, NUM_SAVED);

#if EEPDEBUG
		printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address));
#endif
		if (CRC_NUM != *((u32 *)next_address)) { // CRC错误
			for (u8 i = 0; i < NUM_SAVED; i++) {
				data[i] = DEFAULT_VALUE;
			}
			EEPROMStatus = FLASH_EEPROM_CRC_ERROR;
		} else { //  CRC正确
			EEPROMStatus = FLASH_EEPROM_OK;
		}
	} else {
		// 读取失败，报错
		EEPROMStatus = status_read[0];
	}

	KTH7823.angle_set(0);

	FOC.control_mode = (u8)data[0];
	FOC.limit_voltage = data[1];
	FOC.limit_velocity = data[2];
	KTH7823.angle_set(data[3]);
	FOC.target_angle = data[4];
	FOC.target_velocity = data[5];
	return EEPROMStatus;
}

/**
 * @brief  从Flash EEPROM读取数据
 * @param  virtualAddr: 虚拟地址
 * @param  data: 指向存储读取数据的指针
 * @retval FlashEepromStatus: 操作状态
 */
FlashEepromStatus FlashEEPROM::read(u8 virtualAddr, u16 *data) const {
	// 参数检查
	if (data == nullptr) {
		return FLASH_EEPROM_INVALID_ADDR;
	}

	// 从Flash末尾开始向前搜索，找到最新的数据
	for (u32 address = EEPROM_END_ADDRESS - SIZE_U32;
		 address >= EEPROM_START_ADDRESS; address -= SIZE_U32) {
		u32 data_word = *((u32 *)address);
		// 检查是否是有效数据（非擦除状态）
		if (data_word != 0xFFFFFFFF) {
			u8 addr = (u8)(data_word & DATA_ADDR_MASK);
			// 找到匹配的虚拟地址
			if (addr == virtualAddr) {
				*data = (u16)(data_word & DATA_U16_MASK);
				return FLASH_EEPROM_OK;
			}
		}
	}
	return FLASH_EEPROM_DATA_NOT_FOUND;
}

/**
 * @brief  从Flash EEPROM读取float数据
 * @param  virtualAddr: 虚拟地址
 * @param  data: 指向存储读取数据的指针
 * @retval FlashEepromStatus: 操作状态
 */
FlashEepromStatus FlashEEPROM::readFloat(u8 virtualAddr, float *data) const {
	// 检查参数有效性
	if (data == nullptr) {
		return FLASH_EEPROM_INVALID_ADDR;
	}

	// 从Flash末尾开始向前搜索，找到最新的数据
	for (u32 address = EEPROM_END_ADDRESS - SIZE_U32;
		 address >= EEPROM_START_ADDRESS; address -= SIZE_U32) {
		// 使用volatile确保每次都从内存读取，防止编译器优化
		volatile u32 *flash_ptr = (volatile u32 *)address;
		u32 data_word = *flash_ptr;

		// 检查是否是有效数据（非擦除状态）
		if (data_word != 0xFFFFFFFF) {
			u8 addr = (u8)(data_word & DATA_ADDR_MASK);
			// 找到匹配的虚拟地址
			if (addr == virtualAddr) {
				// 清除最低的8位地址信息，保留高24位数据
				u32 float_data = data_word & DATA_FLOAT_MASK;
				// 使用内存拷贝将处理后的数据解释为float
				memcpy(data, &float_data, SIZE_FLOAT);
#if EEPDEBUG
				printf("read:0x%x=%f\r\n", address, *data);
#endif
				return FLASH_EEPROM_OK;
			}
		}
	}
	// 未找到指定虚拟地址的数据
	return FLASH_EEPROM_DATA_NOT_FOUND;
}

/**
 * @brief  向Flash EEPROM写入数据
 * @param  virtualAddr: 虚拟地址
 * @param  data: 要写入的数据
 * @retval FlashEepromStatus: 操作状态
 */
FlashEepromStatus FlashEEPROM::write(u8 virtualAddr, u16 data) {
	// 查找下一个可写入的位置
	u32 next_address = EEPROM_START_ADDRESS;
	u32 data_read;
	// 插入虚拟地址到低8位	格式: [data:16bit][reserved:8bit][addr:8bit]
	data = (data & DATA_U16_MASK) | (u32)virtualAddr;

	for (u32 address = EEPROM_END_ADDRESS - SIZE_U32;
		 address >= EEPROM_DATA_START_ADDRESS; address -= SIZE_U32) {
		// 安全读取Flash数据
		volatile u32 *flash_ptr = (volatile u32 *)address;
		data_read = *flash_ptr;

		if ((data_read & DATA_ADDR_MASK) == (u32)virtualAddr) { // 虚拟地址相同
			if ((data_read & DATA_U16_MASK) ==
				(data & DATA_U16_MASK)) { // 数据相同
#if EEPDEBUG
				data_read = data_read & DATA_U16_MASK;
				u16 float_data_read = *((u16 *)&data_read);
				printf("Found data in:0x%x=%u\r\n", address, data_read);
#endif
				return FLASH_EEPROM_OK;
			} else {
				break;
			}
		}
	}

	// 遍历Flash页面寻找下一个可写入位置
	for (u32 address = EEPROM_DATA_START_ADDRESS; address < EEPROM_END_ADDRESS;
		 address += SIZE_U32) {
		// 安全读取Flash数据
		volatile u32 *flash_ptr = (volatile u32 *)address;
		u32 data_word = *flash_ptr;
		// 找到空位置
		if (data_word == 0xFFFFFFFF) {
			next_address = address;
			break;
		}
		// 更新下一个可能的写入位置（如果需要格式化）
		next_address = address + SIZE_U32;
	}

	// 如果有足够空间，直接写入
	if (next_address < EEPROM_END_ADDRESS) {
		u32 data_word = ((u32)data << 16) | (u32)virtualAddr;

		// 使用GD32库函数进行安全编程
		fmc_unlock();
		fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR);
		fmc_state_enum fmc_status =
			fmc_word_program(next_address, data_word); // 写入数据
		fmc_lock();

		if (fmc_status != FMC_READY) {
			return FLASH_EEPROM_WRITE_ERROR;
		}
		return FLASH_EEPROM_OK;
	} else { // 空间不足，需要格式化页面
#if EEPDEBUG
		printf("no enough space\r\n");
#endif
		FlashEepromStatus status = format();
		if (status == FLASH_EEPROM_OK) {
			status = save_system_config(false);
			return status;
		} else {
			return FLASH_EEPROM_ERASE_ERROR;
		}
	}
}

/**
 * @brief  向Flash EEPROM写入float数据
 * @param  virtualAddr: 虚拟地址
 * @param  data: 要写入的数据
 * @retval FlashEepromStatus: 操作状态
 */
FlashEepromStatus FlashEEPROM::writeFloat(u8 virtualAddr, float data) {
	// 查找下一个可写入的位置
	u32 next_address = EEPROM_START_ADDRESS;
	u32 data_word;

	// 将完整的float数据复制到32位变量中
	memcpy(&data_word, &data, SIZE_FLOAT);
	// 插入虚拟地址到低8位	格式: [data:24bit][addr:8bit]
	data_word = (data_word & DATA_FLOAT_MASK) | (u32)virtualAddr;

	// 从Flash末尾开始向前搜索，找到最新的数据
	for (u32 address = EEPROM_END_ADDRESS - SIZE_U32;
		 address >= EEPROM_DATA_START_ADDRESS; address -= SIZE_U32) {
		// 安全读取Flash数据
		volatile u32 *flash_ptr = (volatile u32 *)address;
		u32 data_word_read = *flash_ptr;

		if ((data_word_read & DATA_ADDR_MASK) ==
			(u32)virtualAddr) { // 虚拟地址相同
			if ((data_word_read & DATA_FLOAT_MASK) ==
				(data_word & DATA_FLOAT_MASK)) { // 数据相同
#if EEPDEBUG
				data_word_read = data_word_read & DATA_FLOAT_MASK;
				float float_data_read = *((float *)&data_word_read);
				printf("Found data in:0x%x=%f\r\n", address, float_data_read);
#endif
				return FLASH_EEPROM_OK;
			} else {
				break; // 找到相同地址但数据不同，需要更新
			}
		}
	}

	// 遍历Flash页面寻找下一个可写入位置
	for (u32 address = EEPROM_DATA_START_ADDRESS; address < EEPROM_END_ADDRESS;
		 address += SIZE_U32) {
		// 安全读取Flash数据
		volatile u32 *flash_ptr = (volatile u32 *)address;
		u32 data_word_read = *flash_ptr;
		// 找到空位置
		if (data_word_read == 0xFFFFFFFF) {
			next_address = address;
			break;
		}
		// 更新下一个可能的写入位置（如果需要格式化）
		next_address = address + SIZE_U32;
	}

	// 如果有足够空间，直接写入
	if (next_address < EEPROM_END_ADDRESS) {
		// 使用GD32库函数进行安全编程
		fmc_unlock();
		fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR);

		// 确保使用对齐地址
		fmc_state_enum fmc_status = fmc_word_program(next_address, data_word);

		fmc_lock();
#if EEPDEBUG
		u32 data_word_c = *((u32 *)next_address);
		data_word_c = data_word_c & DATA_FLOAT_MASK;
		float float_data = *((float *)&data_word_c);
		printf("save:0x%x=%f\r\n", next_address, float_data);
#endif
		if (fmc_status != FMC_READY) {
			return FLASH_EEPROM_WRITE_ERROR;
		}
		return FLASH_EEPROM_OK;
	} else {
		// 空间不足，需要格式化页面
#if EEPDEBUG
		printf("no enough space\r\n");
#endif
		FlashEepromStatus status = format();
		if (status == FLASH_EEPROM_OK) {
			status = save_system_config(false);
			return status;
		} else {
			return FLASH_EEPROM_ERASE_ERROR;
		}
	}
}

/**
 * @brief  格式化Flash EEPROM页面
 * @param  None
 * @retval FlashEepromStatus: 操作状态
 */
FlashEepromStatus FlashEEPROM::format() {
	fmc_state_enum fmc_status;
#if EEPDEBUG
	printf("擦除页面\r\n");
#endif

	fmc_unlock();
	fmc_status = fmc_page_erase(EEPROM_START_ADDRESS); // 擦除第一页
	if (fmc_status != FMC_READY) {
		fmc_lock();
		return FLASH_EEPROM_ERASE_ERROR;
	}
	fmc_status = fmc_page_erase(EEPROM_DATA_PAGE2_ADDRESS); // 擦除第二页
	if (fmc_status != FMC_READY) {
		fmc_lock();
		return FLASH_EEPROM_ERASE_ERROR;
	}
	fmc_lock();
	return FLASH_EEPROM_OK;
}

/**
 * @brief 更新系统配置并保存到Flash
 * @param config 新的系统配置值
 */
FlashEepromStatus FlashEEPROM::save_system_config(bool saved) {
	u32 CRC_NUM = 0x0000;
	float data[NUM_SAVED] = {(float)FOC.control_mode, FOC.limit_voltage,
							 FOC.limit_velocity,	  FOC.rt_angle,
							 FOC.target_angle,		  FOC.target_velocity};

	// 如果系统配置标记已保存，则无需再次保存
	if (system_config_saved) {
		EEPROMStatus = FLASH_EEPROM_OK;
		return FLASH_EEPROM_OK;
	}

	// 写入参数并检查错误
	FlashEepromStatus status;

	status = writeFloat(ADDR_CONTROL_MODE, data[0]);
	if (status != FLASH_EEPROM_OK) {
		EEPROMStatus = status;
		return status;
	}

	status = writeFloat(ADDR_VOLTAGE_LIMIT, data[1]);
	if (status != FLASH_EEPROM_OK) {
		EEPROMStatus = status;
		return status;
	}

	status = writeFloat(ADDR_VEL_LIMIT, data[2]);
	if (status != FLASH_EEPROM_OK) {
		EEPROMStatus = status;
		return status;
	}

	status = writeFloat(ADDR_ANGLE, data[3]);
	if (status != FLASH_EEPROM_OK) {
		EEPROMStatus = status;
		return status;
	}

	status = writeFloat(ADDR_TARGET_ANGLE, data[4]);
	if (status != FLASH_EEPROM_OK) {
		EEPROMStatus = status;
		return status;
	}

	status = writeFloat(ADDR_TARGET_VEL, data[5]);
	if (status != FLASH_EEPROM_OK) {
		EEPROMStatus = status;
		return status;
	}

	// 创建用于CRC计算的模拟数据数组，模拟实际存储到Flash中的数据格式
	u32 crc_data_words[NUM_SAVED];

	// 按照writeFloat函数的方式处理数据
	for (u8 i = 0; i < NUM_SAVED; i++) {
		// 将完整的float数据复制到32位变量中
		memcpy(&crc_data_words[i], &data[i], SIZE_FLOAT);
		// 去除最后2位精度
		crc_data_words[i] = crc_data_words[i] & DATA_FLOAT_MASK;
	}

	crc_data_register_reset(); // 确保CRC计算从初始状态开始
	CRC_NUM = crc_block_data_calculate(crc_data_words, NUM_SAVED);
	writeCRC(CRC_NUM, saved);

	EEPROMStatus = FLASH_EEPROM_OK;
	return FLASH_EEPROM_OK;
}

/**
 * @brief 读取角度数据从Flash
 * @return float: 角度数据
 */
float FlashEEPROM::read_angle() {
	float angle = DEFAULT_VALUE;
	if (readFloat(ADDR_ANGLE, &angle) != FLASH_EEPROM_OK) {
		angle = DEFAULT_VALUE;
	}
	return angle;
}

/**
 * @brief 保存CRC数据到Flash
 * @param saved: 是否一次性保存
 * @return FlashEepromStatus: 状态
 */
FlashEepromStatus FlashEEPROM::writeCRC(u32 CRC_NUM, bool saved) {
	static u32 next_address;
	// 遍历Flash页面寻找下一个可写入crc的位置
	for (u32 address = EEPROM_DATA_START_ADDRESS - SIZE_U32;
		 address > EEPROM_START_ADDRESS; address -= SIZE_U32) {
		// 安全读取Flash数据
		volatile u32 *flash_ptr = (volatile u32 *)address;
		u32 data_word = *flash_ptr;
		// 找到空位置
		if (data_word == 0xFFFFFFFF) {
			next_address = address;
			break;
		}
		// 更新下一个可能的写入位置（如果需要格式化）
		next_address = address - SIZE_U32;
	}

	// 如果当前CRC与现在的crc值不同，则写入
	if (*((u32 *)next_address) != CRC_NUM) {
		// 如果有足够空间，直接写入crc值
		if (next_address > EEPROM_START_ADDRESS) {
			fmc_unlock();
			fmc_flag_clear(FMC_FLAG_PGERR | FMC_FLAG_PGAERR | FMC_FLAG_WPERR);
			fmc_state_enum fmc_status = fmc_word_program(next_address, CRC_NUM);
			fmc_lock();
#if EEPDEBUG
			printf("CRC cmp1,0x%X_0x%x \r\n", CRC_NUM, *((u32 *)next_address));
#endif

			// 检查写入是否成功
			if (fmc_status != FMC_READY) {
				EEPROMStatus = fmc_status;
				return FLASH_EEPROM_WRITE_ERROR;
			}
		} else {
			// 空间不足，需要格式化页面
#if EEPDEBUG
			printf("no enough space\r\n");
#endif
			FlashEepromStatus status = format();
			if (status == FLASH_EEPROM_OK) {
				status = save_system_config(false);
				return status;
			} else {
				EEPROMStatus = FLASH_EEPROM_ERASE_ERROR;
				return FLASH_EEPROM_ERASE_ERROR;
			}
		}
	}

	// 重新读取CRC校验
	if (CRC_NUM == *((u32 *)next_address)) {
		system_config_saved = saved;	// 标记已保存过
		EEPROMStatus = FLASH_EEPROM_OK; // 设置EEPROM状态
		return FLASH_EEPROM_OK;
	} else {
		EEPROMStatus = FLASH_EEPROM_CRC_ERROR; // 设置EEPROM错误状态
		return FLASH_EEPROM_CRC_ERROR;
	}
}