#include "eeprom.h"
#include "errors.h"
#include "stm32f3xx_hal.h"

#include <string.h>

#define EEPROM_I2C_ADDR          0xA0
// Don't use the beginning of the EEPROM, since the testbench writes there
#define EEPROM_CONFIG_BASE       0x0100
#define EEPROM_CONFIG_ECC_OFFSET 0x20

EEPROMConfig eeprom_config;

static I2C_HandleTypeDef* hi2c;

void eeprom_init(I2C_HandleTypeDef* handle) {
  hi2c = handle;
  uint8_t buf[sizeof(EEPROMConfig) * 3];
  // Read 3 EEPROM config buffers at 32 byte offsets
  for (size_t ecc_i = 0; ecc_i < 3; ecc_i++) {
    HAL_StatusTypeDef status = HAL_I2C_Mem_Read(
        hi2c, EEPROM_I2C_ADDR,
        EEPROM_CONFIG_BASE + EEPROM_CONFIG_ECC_OFFSET * ecc_i, 2,
        buf + sizeof(EEPROMConfig) * ecc_i, sizeof(EEPROMConfig), 100);
    if (status != HAL_OK) {
      set_error_source(ERROR_SOURCE_EEPROM);
    }
  }
  // ECC
  for (size_t i = 0; i < sizeof(EEPROMConfig); i++) {
    const uint8_t a = buf[i + sizeof(EEPROMConfig) * 0];
    const uint8_t b = buf[i + sizeof(EEPROMConfig) * 1];
    const uint8_t c = buf[i + sizeof(EEPROMConfig) * 2];
    if (a == b || a == c) {
      buf[i] = a;
    } else if (b == c) {
      buf[i] = b;
    } else {
      set_error_source(ERROR_SOURCE_EEPROM);
    }
  }

  memcpy(&eeprom_config, buf, sizeof(EEPROMConfig));
  // Write back config
  // eeprom_config.id = 3;
  eeprom_config_save();
}

void eeprom_config_save() {
  for (size_t ecc_i = 0; ecc_i < 3; ecc_i++) {
    HAL_StatusTypeDef status = HAL_I2C_Mem_Write(
        hi2c, EEPROM_I2C_ADDR,
        EEPROM_CONFIG_BASE + EEPROM_CONFIG_ECC_OFFSET * ecc_i, 2,
        (uint8_t*)&eeprom_config, sizeof(eeprom_config), 100);
    if (status != HAL_OK) {
      set_error_source(ERROR_SOURCE_EEPROM);
    }
    HAL_Delay(100);
  }
}