#include "EEPROM.h"
#include "pca9685.h"
#include "usbd_cdc_if.h"
#include "Print.h"
#include "RobotFunctions.h"

InfoBlock infoBlock;
extern IWDG_HandleTypeDef hiwdg;

//*********************** EEPROM_INIT ********************************//
MemoryStatus EEPROM_INIT() {
	memset(&infoBlock, 0x00, sizeof(infoBlock));
	MemoryStatus status = getInfoBlock(&infoBlock);
	if (status != EEPROM_OK) {
		char errorMsg[] = "Error reading InfoBlock from EEPROM\n";
		CDC_Transmit_FS((uint8_t*) errorMsg, strlen(errorMsg));
		return status;
	}
	char uid_ref[11] = "Stack_Sport";
	if (memcmp(&(infoBlock.init_uid), &uid_ref, 11) != 0) {
		char errorMsg[] = "uid non equal\n\n";
		CDC_Transmit_FS((uint8_t*) errorMsg, strlen(errorMsg));

		EEPROM_EARSE();

		memset(&infoBlock, 0x00, sizeof(InfoBlock));
		char uid[] = "Stack_Sport";
		memcpy(infoBlock.init_uid, uid, 11);

		infoBlock.hwInfo.timings.preRun = 3500;

		infoBlock.hwInfo.motors.speed_Rollers_min = 0;
		infoBlock.hwInfo.motors.speed_Screw_min = 0;

		infoBlock.hwInfo.servos[SERVO_AXIAL].invert = 0;
		infoBlock.hwInfo.servos[SERVO_AXIAL].min = 0;
		infoBlock.hwInfo.servos[SERVO_AXIAL].def = 90;
		infoBlock.hwInfo.servos[SERVO_AXIAL].max = 180;

		infoBlock.hwInfo.servos[SERVO_HORIZONTAL].invert = 0;
		infoBlock.hwInfo.servos[SERVO_HORIZONTAL].min = 0;
		infoBlock.hwInfo.servos[SERVO_HORIZONTAL].def = 90;
		infoBlock.hwInfo.servos[SERVO_HORIZONTAL].max = 180;

		infoBlock.hwInfo.servos[SERVO_VERTICAL].invert = 0;
		infoBlock.hwInfo.servos[SERVO_VERTICAL].min = 0;
		infoBlock.hwInfo.servos[SERVO_VERTICAL].def = 90;
		infoBlock.hwInfo.servos[SERVO_VERTICAL].max = 180;

		infoBlock.statInfo.shotsInShot = 0;
		infoBlock.statInfo.shotsInProgram = 0;
		infoBlock.statInfo.totalMacros = 0;

		status = saveInfoBlock(&infoBlock);
		if (status != EEPROM_OK) {
			char errorMsg[] = "EEPROM FAIL\n";
			CDC_Transmit_FS((uint8_t*) errorMsg, strlen(errorMsg));
			return status;
		}
		status = getInfoBlock(&infoBlock);

		if (status != EEPROM_OK) {
			char errorMsg[] = "Error reading InfoBlock from EEPROM\n";
			CDC_Transmit_FS((uint8_t*) errorMsg, strlen(errorMsg));
			return status;
		}
	}

	// Буфер для строки вывода
	char buffer[128];

	// Вывод информации о HardwareInit_t
	snprintf(buffer, sizeof(buffer), "Hardware Initialization:\n");
	CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer));

	char char_tmp[12];
	memcpy(char_tmp, infoBlock.init_uid, 11);
	char_tmp[11] = '\0';
	snprintf(buffer, sizeof(buffer), "init_uid: %s\n", char_tmp);
	CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer));

	snprintf(buffer, sizeof(buffer),
			"Servo 1: Invert=%u, Min=%u, Default=%u, Max=%u\n",
			infoBlock.hwInfo.servos[0].invert, infoBlock.hwInfo.servos[0].min,
			infoBlock.hwInfo.servos[0].def, infoBlock.hwInfo.servos[0].max);
	CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer));

	snprintf(buffer, sizeof(buffer),
			"Servo 2: Invert=%u, Min=%u, Default=%u, Max=%u\n",
			infoBlock.hwInfo.servos[1].invert, infoBlock.hwInfo.servos[1].min,
			infoBlock.hwInfo.servos[1].def, infoBlock.hwInfo.servos[1].max);
	CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer));

	snprintf(buffer, sizeof(buffer),
			"Servo 3: Invert=%u, Min=%u, Default=%u, Max=%u\n",
			infoBlock.hwInfo.servos[2].invert, infoBlock.hwInfo.servos[2].min,
			infoBlock.hwInfo.servos[2].def, infoBlock.hwInfo.servos[2].max);
	CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer));

	snprintf(buffer, sizeof(buffer),
			"Motors: Speed_Rollers_Min=%u, Speed_Screw_Min=%u\n",
			infoBlock.hwInfo.motors.speed_Rollers_min,
			infoBlock.hwInfo.motors.speed_Screw_min);
	CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer));

	// Вывод информации о DelayTimes
	snprintf(buffer, sizeof(buffer), "Timings: PreRun=%u\n",
			infoBlock.hwInfo.timings.preRun);
	CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer));

	// Вывод информации о Statistics
	snprintf(buffer, sizeof(buffer), "Statistics:\n");
	CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer));

	snprintf(buffer, sizeof(buffer), "Total Shots: %lu\n",
			infoBlock.statInfo.shotsInShot);
	CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer));

	snprintf(buffer, sizeof(buffer), "Total Programs: %lu\n",
			infoBlock.statInfo.shotsInProgram);
	CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer));

	snprintf(buffer, sizeof(buffer), "Total Macros: %lu\n\n",
			infoBlock.statInfo.totalMacros);
	CDC_Transmit_FS((uint8_t*) buffer, strlen(buffer));

	return status;
}
//************************************************************//

MemoryStatus saveInfoBlock() {
	return FLASH_WriteBlock(START_ADR_STAT, 0, (uint8_t*) &infoBlock,
			sizeof(InfoBlock), sizeof(InfoBlock));
}

MemoryStatus getInfoBlock() {
	MemoryStatus status = FLASH_ReadBlock(START_ADR_STAT, 0,
			(uint8_t*) &infoBlock, sizeof(InfoBlock));

	if (status != EEPROM_OK) {
		return EEPROM_FAIL;
	}

	return EEPROM_OK;
}

MemoryStatus saveShot(unsigned char number, Shot *shot) {
	if (FLASH_WriteBlock(START_ADR_SHOT, number, (uint8_t*) shot,
	SHOT_BLOCKSIZE, SHOT_BLOCKSIZE) == EEPROM_OK) {
		return EEPROM_OK;
	}
	return EEPROM_FAIL;
}

MemoryStatus getShot(unsigned char number, Shot *shot) {
	HAL_IWDG_Refresh(&hiwdg);
	if (FLASH_ReadBlock(START_ADR_SHOT, number, (uint8_t*) shot, SHOT_BLOCKSIZE)
			!= EEPROM_OK) {
		return EEPROM_FAIL;
	}
	if (!shot->isExist) {
		return EEPROM_MISSING_ELEMENT;
	}
	return EEPROM_OK;
}

MemoryStatus delShot(unsigned char number) {
	if (FLASH_DelBlock(START_ADR_SHOT, number, SHOT_BLOCKSIZE) == EEPROM_OK) {
		return EEPROM_OK;
	}
	return EEPROM_FAIL;
}

MemoryStatus saveProg(unsigned char number, Program *prog) {
	MemoryStatus result = EEPROM_OK;
	for (uint16_t i = 0; i < prog->header.shotCount; ++i) {
		HAL_IWDG_Refresh(&hiwdg);
		Shot shot;
		MemoryStatus stat = getShot(prog->shots[i].id, &shot);
		if (!(stat == EEPROM_OK || stat == EEPROM_MISSING_ELEMENT)) {
			return EEPROM_FAIL;
		}
		if (!shot.isExist) {
			// todo: добавить в запросы для загрузки снимков
			result = EEPROM_MISSING_ELEMENT;
		}
	}

	uint16_t totalSize = sizeof(ProgramHeader)
			+ sizeof(ProgramShot) * prog->header.shotCount;

	if (FLASH_WriteBlock(START_ADR_PROGRAM, number, (uint8_t*) prog, totalSize,
	PROGRAM_BLOCKSIZE) != EEPROM_OK) {
		return EEPROM_FAIL;
	}
	return result;
}

MemoryStatus getProg(unsigned char number, Program *prog) {
	if (FLASH_ReadBlock(START_ADR_PROGRAM, number, (uint8_t*) prog,
	PROGRAM_BLOCKSIZE) != EEPROM_OK) {
		return EEPROM_FAIL;
	}

	if (!prog->header.shotCount) {
		return EEPROM_MISSING_ELEMENT;
	}

	return EEPROM_OK;
}

MemoryStatus delProg(unsigned char number) {
	if (FLASH_DelBlock(START_ADR_PROGRAM, number, PROGRAM_BLOCKSIZE)
			== EEPROM_OK) {
		return EEPROM_OK;
	}
	return EEPROM_FAIL;
}

MemoryStatus saveMacro(unsigned char number, Macro *macro) {
	MemoryStatus result = EEPROM_OK;
	for (uint16_t i = 0; i < macro->header.programmCount; ++i) {
		HAL_IWDG_Refresh(&hiwdg);
		Program prog;
		MemoryStatus stat = getProg(macro->programs[i].id, &prog);
		if (!(stat == EEPROM_OK || stat == EEPROM_MISSING_ELEMENT)) {
			return EEPROM_FAIL;
		}
		if (!prog.header.shotCount) {
			result = EEPROM_MISSING_ELEMENT;
			// todo: добавить в запросы для загрузки программ
		}
	}

	uint16_t totalSize = sizeof(MacroHeader)
			+ sizeof(MacroProgram) * macro->header.programmCount;

	if (FLASH_WriteBlock(START_ADR_MACRO, number, (uint8_t*) macro, totalSize,
	MACRO_BLOCKSIZE) != EEPROM_OK) {
		return EEPROM_FAIL;
	}

	return result;
}

MemoryStatus getMacro(unsigned char number, Macro *macro) {
	if (FLASH_ReadBlock(START_ADR_MACRO, number, (uint8_t*) macro,
	MACRO_BLOCKSIZE) != EEPROM_OK) {
		return EEPROM_FAIL;
	}

	if (!macro->header.programmCount) {
		return EEPROM_MISSING_ELEMENT;
	}
	return EEPROM_OK;
}

MemoryStatus delMacro(unsigned char number) {
	if (FLASH_DelBlock(START_ADR_MACRO, number, MACRO_BLOCKSIZE) == EEPROM_OK) {
		return EEPROM_OK;
	}
	return EEPROM_FAIL;
}

MemoryStatus EEPROM_EARSE() {
    uint16_t addr = 0;
    const uint16_t EEPROM_MAX_ADDR = 1024*32;
    do {
        uint8_t Buf[64];
        HAL_IWDG_Refresh(&hiwdg);
        memset(Buf, 0x00, sizeof(Buf));
        HAL_IWDG_Refresh(&hiwdg);

        FLASH_WriteBlock(addr, 0, Buf, sizeof(Buf), sizeof(Buf));

        addr += sizeof(Buf);
    } while (addr < EEPROM_MAX_ADDR);

    return EEPROM_OK;
}


MemoryStatus FLASH_WriteBlock(uint16_t startAddr, uint8_t number,
		uint8_t *writeData, uint16_t dataSize, uint16_t blockSize) {
	HAL_StatusTypeDef result;

	// Проверка на корректность входных данных
	if ((startAddr == START_ADR_SHOT && number >= MAX_SHOT_COUNT)
			|| (startAddr == START_ADR_PROGRAM && number >= MAX_PROGRAM_COUNT)
			|| (startAddr == START_ADR_MACRO && number >= MAX_MACRO_COUNT)) {
		return EEPROM_OUT_OF_RANGE;
	}

	uint16_t blockAddr16 = (uint16_t) (startAddr
			+ (uint16_t) (number * blockSize));
	uint8_t blockAddr[2] = { HIBYTE(blockAddr16), LOBYTE(blockAddr16) };

	unsigned char Buf[dataSize + 2];
	memset(Buf, 0x00, sizeof(Buf));
	Buf[0] = blockAddr[0];
	Buf[1] = blockAddr[1];

	for (unsigned char i = 0; i < dataSize; i++)
		Buf[i + 2] = writeData[i];

	result = HAL_I2C_Master_Transmit(&hi2c1, (AT24C_ADRESS << 1), Buf,
			(dataSize + 2), 10);
//	print("Written ");
//	printNumber(dataSize);
//	print(" bytes\n");
	HAL_Delay(1);
	if (result != HAL_OK) {
		return EEPROM_FAIL;
	}
	return EEPROM_OK;
}

MemoryStatus FLASH_DelBlock(uint16_t startAddr, uint8_t number,
		uint16_t dataSize) {
	HAL_StatusTypeDef result;

	// Проверка на корректность входных данных
	if ((startAddr == START_ADR_SHOT && number >= MAX_SHOT_COUNT)
			|| (startAddr == START_ADR_PROGRAM && number >= MAX_PROGRAM_COUNT)
			|| (startAddr == START_ADR_MACRO && number >= MAX_MACRO_COUNT)) {
		return EEPROM_OUT_OF_RANGE;
	}

	uint16_t blockAddr16 = (uint16_t) (startAddr
			+ (uint16_t) (number * dataSize));
	uint8_t blockAddr[2] = { HIBYTE(blockAddr16), LOBYTE(blockAddr16) };

	unsigned char Buf[2 + 1];
	memset(Buf, 0x00, sizeof(Buf));
	Buf[0] = blockAddr[0];
	Buf[1] = blockAddr[1];
	Buf[2] = 0;

	result = HAL_I2C_Master_Transmit(&hi2c1, (AT24C_ADRESS << 1), Buf,
			sizeof(Buf), 10);
	HAL_Delay(1);
	if (result != HAL_OK) {
		return EEPROM_FAIL;
	}
	return EEPROM_OK;
}

MemoryStatus FLASH_ReadBlock(uint16_t startAddr, uint8_t number,
		uint8_t *readData, uint16_t dataSize) {
	HAL_StatusTypeDef result;

	// Проверка на корректность входных данных
	if ((startAddr == START_ADR_SHOT && number > MAX_SHOT_COUNT)
			|| (startAddr == START_ADR_PROGRAM && number > MAX_PROGRAM_COUNT)
			|| (startAddr == START_ADR_MACRO && number > MAX_MACRO_COUNT)) {
		return EEPROM_OUT_OF_RANGE;
	}

	memset(readData, 0x00, dataSize);

	uint16_t blockAddr16 = (uint16_t) (startAddr
			+ (uint16_t) (number * dataSize));
	uint8_t blockAddr[2] = { HIBYTE(blockAddr16), LOBYTE(blockAddr16) };

	result = HAL_I2C_Master_Transmit(&hi2c1, (AT24C_ADRESS << 1), blockAddr, 2,
			10);
	HAL_Delay(1);
	result = HAL_I2C_Master_Receive(&hi2c1, (AT24C_ADRESS << 1) | 1, readData,
			dataSize, 10);
	HAL_Delay(1);
	if (result != HAL_OK) {
		return EEPROM_FAIL;
	}
	return EEPROM_OK;
}