#include "flash.h"
#include "ang_sensor.h"
#include "core_cmFunc.h"
#include "gd32c10x_fmc.h"
#include <Can.h>
#include <cstdio>
#include <stdint.h>

FlashEEPROM flashEEP = FlashEEPROM();
extern angle_sensor KTH7823;
extern CAN_transmiter CAN0_transmiter;
constexpr u8 SIZE_U32 = sizeof(uint32_t);

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

/**
 * @brief  初始化Flash EEPROM模拟
 * @param  None
 * @retval None
 */
u8 FlashEEPROM::init() {
	// 读取保存的参数
	u16 angle;
	// 尝试从Flash读取参数，如果不存在则使用默认值
	if (read(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) {
		angle = 0; // 默认值
		std::printf("角度初始化为: %d\r\n", angle);
	} else {
		std::printf("从Flash读取到角度数: %d\r\n", angle);
	}
	KTH7823.angle_set((float)angle);
	return 0;
}

/**
 * @brief  从Flash EEPROM读取数据
 * @param  virtualAddr: 虚拟地址
 * @param  data: 指向存储读取数据的指针
 * @retval FlashEepromStatus: 操作状态
 */
FlashEepromStatus FlashEEPROM::read(u16 virtualAddr, u16 *data) const {
	// 检查参数有效性
	if (data == nullptr || virtualAddr > 0xFFFE) {
		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) {
			u16 addr = (u16)(data_word & 0xFFFF);
			// 找到匹配的虚拟地址
			if (addr == virtualAddr) {
				*data = (u16)((data_word >> 16) & 0xFFFF);
				return FLASH_EEPROM_OK;
			}
		}
	}
	// 未找到指定虚拟地址的数据
	return FLASH_EEPROM_NOT_FOUND;
}

/**
 * @brief  向Flash EEPROM写入数据
 * @param  virtualAddr: 虚拟地址
 * @param  data: 要写入的数据
 * @retval FlashEepromStatus: 操作状态
 */
FlashEepromStatus FlashEEPROM::write(u16 virtualAddr, u16 data) {
	// 参数检查
	if (virtualAddr > 0xFFFE) {
		return FLASH_EEPROM_INVALID_ADDR;
	}
	// 查找下一个可写入的位置
	u32 next_address = EEPROM_START_ADDRESS;

	// 遍历Flash页面寻找下一个可写入位置
	for (u32 address = EEPROM_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 {
		// 空间不足，需要格式化页面
		FlashEepromStatus status = format();
		if (status != FLASH_EEPROM_OK) {
			return status;
		}

		// 格式化后在页面开头写入数据
		u32 new_data_word = ((u32)data << 16) | (virtualAddr & 0xFFFF);
		fmc_state_enum fmc_status =
			fmc_word_program(EEPROM_START_ADDRESS, new_data_word);

		if (fmc_status != FMC_READY) {
			return FLASH_EEPROM_WRITE_ERROR;
		}

		return FLASH_EEPROM_OK;
	}
}

/**
 * @brief  格式化Flash EEPROM页面
 * @param  None
 * @retval FlashEepromStatus: 操作状态
 */
FlashEepromStatus FlashEEPROM::format() {
	fmc_state_enum fmc_status;

	// 创建临时缓冲区存储唯一地址的最新数据
	u32 valid_entries[EEPROM_MAX_SIZE / SIZE_U32];
	u16 valid_count = 0;

	// 收集有效的数据项（去重，保留最新的）
	for (u32 address = EEPROM_START_ADDRESS; address < EEPROM_END_ADDRESS;
		 address += SIZE_U32) {
		u32 data_word = *((u32 *)address);

		// 检查是否是有效数据项
		if (data_word != 0xFFFFFFFF) {
			u16 addr = (u16)(data_word & 0xFFFF);
			u16 value = (u16)((data_word >> 16) & 0xFFFF);

			// 查找是否已经有相同虚拟地址的数据项
			uint8_t found = 0;
			for (u16 i = 0; i < valid_count; i++) {
				u16 existing_addr = (u16)(valid_entries[i] & 0xFFFF);
				if (existing_addr == addr) {
					// 更新为最新的数据
					valid_entries[i] = ((u32)value << 16) | addr;
					found = 1;
					break;
				}
			}

			// 如果是新的虚拟地址，添加到有效数据列表
			if (!found && valid_count < (EEPROM_MAX_SIZE / SIZE_U32)) {
				valid_entries[valid_count] = ((u32)value << 16) | addr;
				valid_count++;
			}
		}
	}

	// 擦除整个页面
	fmc_status = fmc_page_erase(EEPROM_START_ADDRESS);
	if (fmc_status != FMC_READY) {
		return FLASH_EEPROM_ERASE_ERROR;
	}

	// 重新写入有效的数据项
	for (u16 i = 0; i < valid_count; i++) {
		fmc_status = fmc_word_program(EEPROM_START_ADDRESS + i * SIZE_U32,
									  valid_entries[i]);
		if (fmc_status != FMC_READY) {
			return FLASH_EEPROM_WRITE_ERROR;
		}
	}

	return FLASH_EEPROM_OK;
}

/**
 * @brief 更新参数并保存到Flash
 * @param kp 新的KP值
 * @param ki 新的KI值
 * @param kd 新的KD值
 */
void FlashEEPROM::update_pid_parameters(u16 kp, u16 ki, u16 kd) {
	write(ADDR_MOTOR_KP, kp);
	write(ADDR_MOTOR_KI, ki);
	write(ADDR_MOTOR_KD, kd);
}

/**
 * @brief 更新系统配置并保存到Flash
 * @param config 新的系统配置值
 */
void FlashEEPROM::update_system_config(u16 config) {
	write(ADDR_SYSTEM_CONFIG, config);
}

/**
 * @brief 保存角度数据到Flash
 * @param angle 角度数据
 * @return FlashEepromStatus: 操作状态
 */
FlashEepromStatus FlashEEPROM::save_angle(u16 angle, bool saved) {
	u16 angle_read = 0;
	FlashEepromStatus status = FLASH_EEPROM_ERROR;

	// 读取保存的参数
	if (!angle_saved) {
		// 写入参数
		status = write(ADDR_ANGLE_DATA, angle);

		if (read(ADDR_ANGLE_DATA, &angle_read) != FLASH_EEPROM_OK)
			angle_read = 0; // 读不到角度值，存储失败

		if (angle_read != angle) {
			status = FLASH_EEPROM_WRITE_ERROR;
			CAN0_transmiter.EEPROMStatus = 1; // 设置EEPROM错误状态
		} else {
			angle_saved = saved;			  // 标记已保存过
			CAN0_transmiter.EEPROMStatus = 0; // 设置EEPROM状态
		}
	}
	return status;
}

/**
 * @brief 读取角度数据从Flash
 * @return float: 角度数据
 */
float FlashEEPROM::read_angle() {
	u16 angle = 0;
	if (read(ADDR_ANGLE_DATA, &angle) != FLASH_EEPROM_OK) {
		angle = 0;
	}
	return (float)angle; // 默认返回0度
}

/**
 * @brief 增加错误计数并保存到Flash
 */
void FlashEEPROM::increment_error_count(void) {
	u16 error_count = 0;

	// 读取当前错误计数
	FlashEepromStatus read_status = read(ADDR_ERROR_COUNT, &error_count);
	if (read_status == FLASH_EEPROM_OK ||
		read_status == FLASH_EEPROM_NOT_FOUND) {
		// 增加错误计数（如果是NOT_FOUND，则error_count已经是0）
		error_count++;

		// 保存更新后的错误计数
		write(ADDR_ERROR_COUNT, error_count);
	}
}

/**
 * @brief 示例主函数 - 展示Flash EEPROM的使用方法
 */
void flash_eeprom_example_main(void) {
	std::printf("\r\n=== Flash EEPROM 使用示例 ===\r\n");

	// 初始化Flash EEPROM
	flashEEP.init();

	// 模拟参数更新
	std::printf("\r\n--- 更新PID参数 ---\r\n");
	flashEEP.update_pid_parameters(120, 80, 30);

	// 模拟系统配置更新
	std::printf("\r\n--- 更新系统配置 ---\r\n");
	flashEEP.update_system_config(0x03);

	// 模拟错误发生
	std::printf("\r\n--- 模拟错误计数增加 ---\r\n");
	flashEEP.increment_error_count();
	flashEEP.increment_error_count();

	// 再次读取参数验证保存是否成功
	std::printf("\r\n--- 验证保存的数据 ---\r\n");
	u16 kp_value, ki_value, kd_value;
	u16 system_config;
	u16 error_count;

	// if (flashEEP.read(ADDR_MOTOR_KP, &kp_value) == FLASH_EEPROM_OK) {
	// 	std::printf("当前KP值: %d\r\n", kp_value);
	// }

	// if (flashEEP.read(ADDR_MOTOR_KI, &ki_value) == FLASH_EEPROM_OK) {
	// 	std::printf("当前KI值: %d\r\n", ki_value);
	// }

	// if (flashEEP.read(ADDR_MOTOR_KD, &kd_value) == FLASH_EEPROM_OK) {
	// 	std::printf("当前KD值: %d\r\n", kd_value);
	// }

	// if (flashEEP.read(ADDR_SYSTEM_CONFIG, &system_config) == FLASH_EEPROM_OK)
	// { 	std::printf("当前系统配置: 0x%02X\r\n", system_config);
	// }

	// if (flashEEP.read(ADDR_ERROR_COUNT, &error_count) == FLASH_EEPROM_OK) {
	// 	std::printf("当前错误计数: %d\r\n", error_count);
	// }

	std::printf("\r\n=== 示例结束 ===\r\n");
}