/**
 * \file
 * \brief Implementation of Non-Volatile-Memory storage
 * \author Erich Styger
 * \license SPDX-License-Identifier: BSD-3-Clause
 * This provides an implementation to store and retrieve data from the on-chip memory.
 */

#include "platform.h"
#if PL_CONFIG_HAS_NVM_CONFIG
#include "McuLib.h"
#include "NVM_Config.h"
#include <stdlib.h>
#include "fsl_flash.h"

/*! @brief Flash driver Structure */
static flash_config_t s_flashDriver;

static bool isErased(uint8_t *ptr, int nofBytes) {
  while (nofBytes>0) {
    if (*ptr!=0xFF) {
      return false; /* byte not erased */
    }
    ptr++;
    nofBytes--;
  }
  return true;
}

uint8_t NVMC_SetBlockFlash(uint32_t addr, void *data, size_t dataSize) {
  static uint32_t buf[NVMC_FLASH_BLOCK_SIZE/4]; /* backup buffer */
  unsigned int i;
  status_t status;
  uint8_t *p, *q;

  /* make backup of current content */
  for(i=0;i<sizeof(buf)/4; i++) {
    buf[i] = ((uint32_t*)NVMC_FLASH_START_ADDR)[i];
  }
  /* erase */
  status = FLASH_Erase(&s_flashDriver, NVMC_FLASH_START_ADDR, s_flashDriver.PFlashSectorSize, kFLASH_ApiEraseKey);
  if (status!=kStatus_FLASH_Success) {
    return ERR_FAILED;
  }
  /* copy new data into backup */
  p = ((uint8_t*)buf)+(addr-NVMC_FLASH_START_ADDR);
  q = (uint8_t*)data;
  for(i=0;i<dataSize; i++) {
    *p = *q;
    p++; q++;
  }
  status = FLASH_Program(&s_flashDriver, NVMC_FLASH_START_ADDR, buf, sizeof(buf));
  if (status!=kStatus_FLASH_Success) {
    return ERR_FAILED;
  }
  return ERR_OK;
}


#if PL_HAS_RADIO
uint8_t NVMC_SaveDeviceAddress16(void *data, size_t dataSize) {
  uint8_t res;
  
  if (dataSize!=2) {
    return ERR_OVERFLOW;
  }
   res = IFsh1_SetBlockFlash((byte*)data, (IFsh1_TAddress)(NVMC_GetDeviceAddr16Addr()), dataSize);
  return res;
}
#endif

uint8_t NVMC_SaveReflectanceData(void *data, uint16_t dataSize) {
  if (dataSize>NVMC_REFLECTANCE_DATA_SIZE) {
    return ERR_OVERFLOW;
  }
  return NVMC_SetBlockFlash(NVMC_REFLECTANCE_DATA_START_ADDR, data, dataSize);
}

void *NVMC_GetReflectanceData(void) {
  if (isErased((uint8_t*)NVMC_REFLECTANCE_DATA_START_ADDR, NVMC_REFLECTANCE_DATA_SIZE)) {
    return NULL;
  }
  return (void*)NVMC_REFLECTANCE_DATA_START_ADDR;
}

#if PL_CONFIG_APP_SUMO
uint8_t NVMC_SaveSumoData(void *data, uint16_t dataSize) {
  if (dataSize>NVMC_SUMO_DATA_SIZE) {
    return ERR_OVERFLOW;
  }
  return NVMC_SetBlockFlash(NVMC_SUMO_DATA_START_ADDR, data, dataSize);
}

void *NVMC_GetSumoData(void) {
  if (isErased((uint8_t*)NVMC_SUMO_DATA_START_ADDR, NVMC_SUMO_DATA_SIZE)) {
    return NULL;
  }
  return (void*)NVMC_SUMO_DATA_START_ADDR;
}

#endif /* PL_CONFIG_APP_SUMO */

uint8_t NVMC_SaveRobotData(NVMC_RobotData *data) {
  return NVMC_SetBlockFlash(NVMC_ROBOT_DATA_START_ADDR, data, sizeof(NVMC_RobotData));
}

const NVMC_RobotData *NVMC_GetRobotData(void) {
  if (isErased((uint8_t*)NVMC_ROBOT_DATA_START_ADDR, sizeof(NVMC_RobotData))) {
    return NULL;
  }
  return (void*)NVMC_ROBOT_DATA_START_ADDR;
}

void NVMC_Deinit(void) {
}

void NVMC_Init(void) {
  status_t status;

  memset(&s_flashDriver, 0, sizeof(flash_config_t));
  status = FLASH_Init(&s_flashDriver);
  if (status!=kStatus_FLASH_Success) {
    for(;;) {} /* error */
  }
}

#endif /* PL_CONFIG_HAS_NVM_CONFIG */